控制科学与工程：随手笔记（6）--Sophus库使用

变量	功能
Sophus::SO3	SO3李群;初始化方法：①SO3 SO3_R®直接从选择矩阵构造②SO3 SO3_v(0,0,M_PI/2)从选择向量构造③SO3 SO3_q(q)从四元数构造
Sophus::SE3	SE3李群;初始化方法：①SE3 SE3_Rt(R,t)直接从R、t构造②SE3 SE3_qt(q,t)从四元数和t构造

函数	功能
log()	对数映射
Sophus::SO3::hat(so3)	从向量到反对称矩阵
Sophus::SO3::vee(SO3)	从反对称矩阵到向量
Sophus::SO3::exp(update_so3)*SO3_R	左乘扰动
Sophus::SE3::hat(se3)	从向量到反对称矩阵
Sophus::SE3::vee(SE3)	从反对称矩阵到向量
Sophus::SE3::exp(update_se3)*SE3_Rt	左乘扰动

补充：
typedef Eigen::Matrix<double,6,1>Vector6d;（由于se3是六维向量，所以自定义一个六维向量类型）
Vector6d update_se3;
update_se3.setZero();