C++ Isometry3d::matrix方法代码示例

void parseInertial(shared_ptr<RigidBody> body, XMLElement* node, RigidBodyTree * model)
{
  Isometry3d T = Isometry3d::Identity();

  XMLElement* origin = node->FirstChildElement("origin");
  if (origin)
    poseAttributesToTransform(origin, T.matrix());

  XMLElement* mass = node->FirstChildElement("mass");
  if (mass)
    parseScalarAttribute(mass, "value", body->mass);

  body->com << T(0, 3), T(1, 3), T(2, 3);

  Matrix<double, TWIST_SIZE, TWIST_SIZE> I = Matrix<double, TWIST_SIZE, TWIST_SIZE>::Zero();
  I.block(3, 3, 3, 3) << body->mass * Matrix3d::Identity();

  XMLElement* inertia = node->FirstChildElement("inertia");
  if (inertia) {
    parseScalarAttribute(inertia, "ixx", I(0, 0));
    parseScalarAttribute(inertia, "ixy", I(0, 1));
    I(1, 0) = I(0, 1);
    parseScalarAttribute(inertia, "ixz", I(0, 2));
    I(2, 0) = I(0, 2);
    parseScalarAttribute(inertia, "iyy", I(1, 1));
    parseScalarAttribute(inertia, "iyz", I(1, 2));
    I(2, 1) = I(1, 2);
    parseScalarAttribute(inertia, "izz", I(2, 2));
  }

  auto bodyI = transformSpatialInertia(T, static_cast<Gradient<Isometry3d::MatrixType, Eigen::Dynamic>::type*>(NULL), I);
  body->I = bodyI.value();
}

Transform3 eigen2Xform(const Isometry3d& B) {
  Eigen::Matrix4d me = B.matrix();
  mat4_t<real> mz;
  for (int i=0; i<4; ++i) {
    for (int j=0; j<4; ++j) {
      mz(i,j) = me(i,j);
    }
  }
  Transform3 rval;
  rval.setFromMatrix(mz);
  return rval;
}

Pointcloud::Ptr joint(camera &cam, Pointcloud::Ptr points, Isometry3d &T, frame &newframe)
{
	Pointcloud::Ptr cloud = map2point(cam, newframe.rgb, newframe.depth);
  
  cout << "combining clouds together" << endl;
  Pointcloud::Ptr output(new Pointcloud());
  //transform cloud points into the same coordinate system
  transformPointCloud(*points, *output, T.matrix());
  //put the points together
  *output += *cloud;
  
  return output;
}

void test()
{
	Isometry3d T = Eigen::Isometry3d::Identity();
	T(0,0)=1;
	T(0,1)=0;
	T(0,2)=0;
	T(1,0)=0;
	T(1,1)=0.707;
	T(1,2)=-0.707;
	T(2,0)=0;
	T(2,1)=0.707;
	T(2,2)=0.707;
	T(0,3)=0;
	T(1,3)=0;
	T(2,3)=0.5;

	cout<<T.matrix()<<endl;
	
	for (int i = 2; i <=8; i++)
	{
		g2o::EdgeSE3* edge = new g2o::EdgeSE3();
		g2o::VertexSE3 *v = new g2o::VertexSE3();
		
		v->setId(i);
		v->setEstimate( Eigen::Isometry3d::Identity() );
	        m_globalOptimizer.addVertex(v);
	        
	        edge->vertices() [0] = m_globalOptimizer.vertex( i-1 );
	        edge->vertices() [1] = m_globalOptimizer.vertex( i );
	        
	        Eigen::Matrix<double, 6, 6> information = Eigen::Matrix< double, 6,6 >::Identity();
	        information(0,0) = information(1,1) = information(2,2) = 100;
	        information(3,3) = information(4,4) = information(5,5) = 100;
	        edge->setInformation( information );
	        edge->setMeasurement( T );
	        m_globalOptimizer.addEdge(edge);

		
		
	}
	
#if 0
	T = Eigen::Isometry3d::Identity();
	//T(2,3)=1;
	//cout<<T.matrix()<<endl;
	
	g2o::EdgeSE3* edge = new g2o::EdgeSE3();
		g2o::VertexSE3 *v = new g2o::VertexSE3();
		
		v->setId(8);
		v->setEstimate( Eigen::Isometry3d::Identity() );
	        m_globalOptimizer.addVertex(v);
	        
	        edge->vertices() [0] = m_globalOptimizer.vertex( 1 );
	        edge->vertices() [1] = m_globalOptimizer.vertex( 8 );
	        
	        Eigen::Matrix<double, 6, 6> information = Eigen::Matrix< double, 6,6 >::Identity();
	        information(0,0) = information(1,1) = information(2,2) = 100;
	        information(3,3) = information(4,4) = information(5,5) = 100;
	        edge->setInformation( information );
	        edge->setMeasurement( T );
	        m_globalOptimizer.addEdge(edge);
	

#endif
	
	m_globalOptimizer.save("pa.g2o");
	m_globalOptimizer.initializeOptimization();
	//m_globalOptimizer.optimize(50);
	m_globalOptimizer.save("pa2.g2o");
	cout<<"fuck"<<endl;
	VertexSE3* vertex = dynamic_cast<VertexSE3*>(m_globalOptimizer.vertex(2));
	Isometry3d pose = vertex -> estimate();
	//cout << "matrix:" << endl;
	cout << pose.matrix() << endl;
	
}

 SE3Quat toSE3Quat(const Isometry3d& t)
 {
   SE3Quat result(t.matrix().topLeftCorner<3,3>(), t.translation());
   return result;
 }

bool AlignmentAlgorithmPlaneLinear::operator()(AlignmentAlgorithmPlaneLinear::TransformType& transform, const CorrespondenceVector& correspondences, const IndexVector& indices)
{
    double err=0;
    //If my correspondaces indices are less then a minimum threshold, stop it please
    if ((int)indices.size()<minimalSetSize()) return false;

    //My initial guess for the transformation it's the identity matrix
    //maybe in the future i could have a less rough guess
    transform = Isometry3d::Identity();
    //Unroll the matrix to a vector
    Vector12d x=homogeneous2vector(transform.matrix());
    Matrix9x1d rx=x.block<9,1>(0,0);

    //Initialization of variables, melting fat, i've so much space
    Matrix9d H;
    H.setZero();
    Vector9d b;
    b.setZero();
    Matrix3x9d A;
    //iteration for each correspondace
    for (size_t i=0; i<indices.size(); i++)
    {

        A.setZero();
        const Correspondence& c = correspondences[indices[i]];
        const EdgePlane* edge = static_cast<const EdgePlane*>(c.edge());

        //estraggo i vertici dagli edge
        const VertexPlane* v1 = static_cast<const VertexPlane*>(edge->vertex(0));
        const VertexPlane* v2 = static_cast<const VertexPlane*>(edge->vertex(1));

        //recupero i dati dei piani
        const AlignmentAlgorithmPlaneLinear::PointEstimateType& pi= v1->estimate();
        const AlignmentAlgorithmPlaneLinear::PointEstimateType& pj= v2->estimate();

        //recupeo le normali, mi servono per condizionare la parte rotazionale
        Vector3d ni;
        Vector3d nj;
        Vector4d coeffs_i=pi.toVector();
        Vector4d coeffs_j=pj.toVector();

        ni=coeffs_i.head(3);
        nj=coeffs_j.head(3);

        //inizializzo lo jacobiano, solo la parte rotazionale (per ora)
        A.block<1,3>(0,0)=nj.transpose();
        A.block<1,3>(1,3)=nj.transpose();
        A.block<1,3>(2,6)=nj.transpose();

        if(DEBUG) {
            cerr << "[DEBUG] PI"<<endl;
            cerr << coeffs_i<<endl;
            cerr << "[DEBUG] PJ "<<endl;
            cerr << coeffs_j<<endl;
            cerr << "[ROTATION JACOBIAN][PLANE "<<i<<"]"<<endl;
            cerr << A<<endl;
        }
        //errore
        //inizializzo errore
        Vector3d ek;
        ek.setZero();
        ek=A*x.block<9,1>(0,0)-coeffs_i.head(3);
        H+=A.transpose()*A;

        b+=A.transpose()*ek;

        err+=abs(ek.dot(ek));

        if(DEBUG)
            cerr << "[ROTATIONAL Hessian for plane "<<i<<"]"<<endl<<H<<endl;
        if(DEBUG)
            cerr << "[ROTATIONAL B for plane "<<i<<"]"<<endl<<b<<endl;
    }
    LDLT<Matrix9d> ldlt(H);
    if (ldlt.isNegative()) return false;
    rx=ldlt.solve(-b); // using a LDLT factorizationldlt;

    x.block<3,1>(0,0)+=rx.block<3,1>(0,0);
    x.block<3,1>(3,0)+=rx.block<3,1>(3,0);
    x.block<3,1>(6,0)+=rx.block<3,1>(6,0);
    if(DEBUG) {
        cerr << "Solving the linear system"<<endl;
        cerr << "------------>H"<<endl;
        cerr << H<<endl;
        cerr << "------------>b"<<endl;
        cerr << b<<endl;
        cerr << "------------>rx"<<endl;
        cerr << rx<<endl;
        cerr << "------------>xTEMP"<<endl;
        cerr << vector2homogeneous(x)<<endl<<endl;

        cerr << "łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł"<<endl;
        cerr << "łłłłłłłłłłł Ringraziamo Cthulhu la parte rotazionale è finitałłłłłłłłłłł"<<endl;
        cerr << "łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł"<<endl;
    }
    Matrix4d Xtemp=vector2homogeneous(x);

    //now the problem si solved but i could have (and probably i have!)
    //a not orthogonal rotation matrix, so i've to recondition it

//.........这里部分代码省略.........