Conversion relationship between Lie algebra and rotation matrix and transformation matrix

Lie algebras can be transformed into Lie groups

Sophus :: SO3 SO3;
Eigen::Vector3d so3 = SO3.log;
Sophus::SO3 SO3_R = Sophus::SO3::exp( so3 );

But Sophus::SO3 SO3; cannot be used to represent a rotation matrix. In fact, it is the vector form of a rotation matrix, and it needs to be converted into a rotation matrix by

    // get its Lie algebra using logarithmic mapping 
    Eigen::Vector3d so3 = SO3_R.log();
    cout << " so3 = " <<so3.transpose()<< endl;
     // hat is vector to antisymmetric matrix 
    cout<< " so3 hat=\n " <<Sophus::SO3::hat(so3)< < endl;
     // relatively, vee is antisymmetric to the vector 
    cout<< " so3 hat vee= " <<Sophus::SO3::vee( Sophus::SO3::hat(so3) ).transpose()<< endl; // transpose is purely for output aesthetics

The same for SE3,

cout<<"SE3 updated = "<<endl<<SE3_updated.matrix()<<endl;
SE3_updated.matrix() is not a change matrix T, it needs to be obtained through hat
   // Similar to SE(3) 
    Eigen::Vector3d t( 1 , 0 , 0 );            // Translate 1 along the X axis 
    Sophus::SE3 SE3_Rt(R, t);            // Construct SE(3 from R, t ) 
    Sophus::SE3 SE3_qt(q,t);             // Construct SE(3) from q,t 
    cout<< " SE3 from R,t= " <<endl<<SE3_Rt<< endl;
    cout << " SE3 from q,t= " <<endl<<SE3_qt<< endl;
     // Lie algebra se(3) is a six-dimensional vector, for convenience, typedef first 
    typedef Eigen::Matrix< double , 6 , 1 > Vector6d;
    Vector6d se3 = SE3_Rt.log();
    cout << " se3 = " <<se3.transpose()<< endl;
     // Observe the output, you will find that in Sophus, the translation of se(3) is in the front and the rotation is in the back.
     // Similarly, there are hat and vee two operators 
    cout<< " se3 hat = " <<endl<<Sophus::SE3::hat(se3)<< endl;
    cout<<"se3 hat vee = "<<Sophus::SE3::vee( Sophus::SE3::hat(se3) ).transpose()<<endl;
    
    // Finally, demonstrate updating 
    Vector6d update_se3; // Update amount 
    update_se3.setZero();
    update_se3(0,0) = 1e-4d;
    Sophus::SE3 SE3_updated = Sophus::SE3::exp(update_se3)*SE3_Rt;
    cout<<"SE3 updated = "<<endl<<SE3_updated.matrix()<<endl;

 

Guess you like

Origin http://43.154.161.224:23101/article/api/json?id=325105877&siteId=291194637