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;