SO3和SE3的使用

SO3和SE3的使用

代碼：

///////////SO(3)的使用方法
Matrix3d R=AngleAxisd(M_PI/2,Vector3d(0,0,0)).toRotationMatrix();
Quaterniod q(R);
Sophus::SO3d SO3_R(R);
Sophus::SO3d SO3_q(q);
//使用對數映射獲得它的李代數
Vector3d so3=SO3_R.log()
//反對稱矩陣
Sophus::SO3d::hat(so3)
Sophus::SO3d::vee(Sophus::SO3d::hat(so3))
//增量擾動模型的更新  微小增量
Vector3d update_so3(1e-4,0,0);
Sophus::SO3d SO3_updated=Sophus::SO3d::exp(update_so3)*SO3_R;
SO3_updated.matrix();

//////////////SE(3)的使用方法
Matrix3d R=AngleAxisd(M_PI/2,Vector3d(0,0,0)).toRotationMatrix();
Quaterniod q(R);
Vector3d t(1,0,0);
Sophus::SE3d SE3_Rt(R,t);
Sophus::SE3d SE3_qt(q,t);
//log
typedef Eigen::Matrix<double,6,1> Vector6d;
Vector6d se3=SE_3Rt.log();
//反對稱矩陣
Sophus::SE3d::hat(se3)
Sophus::SE3d::vee(Sophus::SE3d::hat(se3))
//增量擾動模型的更新  微小增量
Vector6d update_se3;
update_se3.setZero();
update_se3(0,0)=1e-4d;

Sophus::SE3d SE3_updated=Sophus::SE3d::exp(update_se3)*SE3_Rt;
SE3_updated.matrix();