C++ Vector3d::head方法代码示例

bool GridMap::getPosition3d(const std::string& type, const Eigen::Array2i& index, Eigen::Vector3d& position) const
{
  if (!isValid(index)) return false;
  Vector2d position2d;
  getPosition(index, position2d);
  position.head(2) = position2d;
  position.z() = at(type, index);
  return true;
}

void LinearSystem::_BuildSystem(
        LinearSystem*       pLS,
        const unsigned int& StartU,
        const unsigned int& EndU,
        const unsigned int& StartV,
        const unsigned int& EndV
        )
{
    // Jacobian
    Eigen::Matrix<double, 1, 6> BigJ;
    Eigen::Matrix<double, 1, 6> J;

    BigJ.setZero();
    J.setZero();

	// Errors
	double		 e;
	double		 SSD = 0;
    double       Error    = 0;
    unsigned int ErrorPts = 0;

    for( int ii = StartV; ii < EndV; ii++ ) {
        for( int jj = StartU; jj < EndU; jj++ ) {
            // variables
            Eigen::Vector2d Ur;        // pixel position
            Eigen::Vector3d Pr, Pv;    // 3d point
            Eigen::Vector4d Ph;        // homogenized point

            // check if pixel is contained in our model (i.e. has depth)
            if( pLS->m_vVirtDepth[ii * pLS->m_nImgWidth + jj] == 0 ) {
                continue;
            }

            // --------------------- first term 1x2
            // evaluate 'a' = L[ Trv * Linv( Uv ) ]
            // back project to virtual camera's reference frame
            // this already brings points to robotics reference frame
            Pv = pLS->_BackProject( jj, ii, pLS->m_vVirtDepth[ii * pLS->m_nImgWidth + jj] );

            // convert to homogeneous coordinate
            Ph << Pv, 1;

            // transform point to reference camera's frame
            // Pr = Trv * Pv
            Ph = pLS->m_dTrv * Ph;
            Pr = Ph.head( 3 );

            // project onto reference camera
            Eigen::Vector3d Lr;

            Lr = pLS->_Project( Pr );
            Ur = Lr.head( 2 );
            Ur = Ur / Lr( 2 );

            // check if point falls in camera's field of view
            if( (Ur( 0 ) <= 1) || (Ur( 0 ) >= pLS->m_nImgWidth - 2) || (Ur( 1 ) <= 1)
                    || (Ur( 1 ) >= pLS->m_nImgHeight - 2) ) {
                continue;
            }

            // finite differences
            float                       TopPix   = pLS->_Interpolate( Ur( 0 ), Ur( 1 ) - 1, pLS->m_vRefImg );
            float                       BotPix   = pLS->_Interpolate( Ur( 0 ), Ur( 1 ) + 1, pLS->m_vRefImg );
            float                       LeftPix  = pLS->_Interpolate( Ur( 0 ) - 1, Ur( 1 ), pLS->m_vRefImg );
            float                       RightPix = pLS->_Interpolate( Ur( 0 ) + 1, Ur( 1 ), pLS->m_vRefImg );
            Eigen::Matrix<double, 1, 2> Term1;

            Term1( 0 ) = (RightPix - LeftPix) / 2.0;
            Term1( 1 ) = (BotPix - TopPix) / 2.0;

            // --------------------- second term 2x3
            // evaluate 'b' = Trv * Linv( Uv )
            // this was already calculated for Term1
            // fill matrix
            // 1/c      0       -a/c^2
            // 0       1/c     -b/c^2
            Eigen::Matrix<double, 2, 3> Term2;
            double                      PowC = Lr( 2 ) * Lr( 2 );

            Term2( 0, 0 ) = 1.0 / Lr( 2 );
            Term2( 0, 1 ) = 0;
            Term2( 0, 2 ) = -(Lr( 0 )) / PowC;
            Term2( 1, 0 ) = 0;
            Term2( 1, 1 ) = 1.0 / Lr( 2 );
            Term2( 1, 2 ) = -(Lr( 1 )) / PowC;
            Term2         = Term2 * pLS->m_Kr;

            // --------------------- third term 3x1
            // we need Pv in homogenous coordinates
            Ph << Pv, 1;

            Eigen::Vector4d Term3i;

            // last row of Term3 is truncated since it is always 0
            Eigen::Vector3d Term3;

            // fill Jacobian with T generators
            Term3i    = pLS->m_dTrv * pLS->m_Gen[0] * Ph;
            Term3     = Term3i.head( 3 );
            J( 0, 0 ) = Term1 * Term2 * Term3;
//.........这里部分代码省略.........

void NuTo::StructureBase::ConstraintLinearEquationNodeToElementCreate(int rNode, int rElementGroup, NuTo::Node::eDof,
                                                                      const double rTolerance,
                                                                      Eigen::Vector3d rNodeCoordOffset)
{
    const int dim = GetDimension();

    Eigen::VectorXd queryNodeCoords = NodeGetNodePtr(rNode)->Get(Node::eDof::COORDINATES);
    queryNodeCoords = queryNodeCoords + rNodeCoordOffset.head(dim);


    std::vector<int> elementGroupIds = GroupGetMemberIds(rElementGroup);

    ElementBase* elementPtr = nullptr;
    Eigen::VectorXd elementNaturalNodeCoords;
    bool nodeInElement = false;
    for (auto const& eleId : elementGroupIds)
    {
        elementPtr = ElementGetElementPtr(eleId);

        // Coordinate interpolation must be linear so the shape function derivatives are constant!
        assert(elementPtr->GetInterpolationType().Get(Node::eDof::COORDINATES).GetTypeOrder() ==
               Interpolation::eTypeOrder::EQUIDISTANT1);
        const Eigen::MatrixXd& derivativeShapeFunctionsGeometryNatural =
                elementPtr->GetInterpolationType()
                        .Get(Node::eDof::COORDINATES)
                        .DerivativeShapeFunctionsNatural(
                                Eigen::VectorXd::Zero(dim)); // just as _some_ point, as said, constant

        // real coordinates of every node in rElement
        Eigen::VectorXd elementNodeCoords = elementPtr->ExtractNodeValues(NuTo::Node::eDof::COORDINATES);

        switch (mDimension)
        {
        case 2:
        {
            Eigen::Matrix2d invJacobian =
                    dynamic_cast<ContinuumElement<2>*>(elementPtr)
                            ->CalculateJacobian(derivativeShapeFunctionsGeometryNatural, elementNodeCoords)
                            .inverse();

            elementNaturalNodeCoords = invJacobian * (queryNodeCoords - elementNodeCoords.head(2));
        }
        break;
        case 3:
        {
            Eigen::Matrix3d invJacobian =
                    dynamic_cast<ContinuumElement<3>*>(elementPtr)
                            ->CalculateJacobian(derivativeShapeFunctionsGeometryNatural, elementNodeCoords)
                            .inverse();

            elementNaturalNodeCoords = invJacobian * (queryNodeCoords - elementNodeCoords.head(3));
        }
        break;

        default:
            throw NuTo::Exception(std::string(__PRETTY_FUNCTION__) + ": \t Only implemented for 2D and 3D");
        }


        if ((elementNaturalNodeCoords.array() > -rTolerance).all() and
            elementNaturalNodeCoords.sum() <= 1. + rTolerance)
        {
            nodeInElement = true;
            break;
        }
    }

    if (not nodeInElement)
    {
        GetLogger() << "Natural node coordinates: \n" << elementNaturalNodeCoords << "\n";
        throw Exception(__PRETTY_FUNCTION__, "Node is not inside any element.");
    }

    auto shapeFunctions =
            elementPtr->GetInterpolationType().Get(Node::eDof::DISPLACEMENTS).ShapeFunctions(elementNaturalNodeCoords);

    std::vector<Constraint::Equation> equations(dim); // default construction of Equation with rhs = Constant = 0
    for (int iDim = 0; iDim < dim; ++iDim)
    {
        equations[iDim].AddTerm(Constraint::Term(*NodeGetNodePtr(rNode), iDim, 1.));
    }


    for (int iNode = 0; iNode < shapeFunctions.rows(); ++iNode)
    {
        int localNodeId = elementPtr->GetInterpolationType().Get(Node::eDof::DISPLACEMENTS).GetNodeIndex(iNode);
        auto globalNode = elementPtr->GetNode(localNodeId, Node::eDof::DISPLACEMENTS);
        //        std::cout << "globalNodeId \t" << globalNodeId << std::endl;
        double coefficient = -shapeFunctions(iNode, 0);

        for (int iDim = 0; iDim < dim; ++iDim)
            equations[iDim].AddTerm(Constraint::Term(*globalNode, iDim, coefficient));
    }
    Constraints().Add(Node::eDof::DISPLACEMENTS, equations);
}