https://zhuanlan.zhihu.com/p/47766990
==============
更新量转化
Eigen::Map<const Vector6> update(update_);
更新参数
se3_update=SE3Quat::exp(update)
setEstimate( se3_update* estimate());
==============
1旋转向量
（旋转）
绕旋转轴转的角度Θ 旋转轴n
2旋转矩阵（旋转）
角度 R 3*3
3-1旋转矩阵==>旋转向量
先求出角度
然后再求出旋转轴n=[ux,uy,uz]
3-2 旋转向量==>旋转矩阵
4变换矩阵 （旋转+位移）
5 目标求导
为了位姿优化，需要T对R，t求导
5-1 不能直接求导
T不能直接对R求导，求导需要加上微小量，t是相加性质, 但是R不是相加性质，且R具有正交约束限制，因此需要转换T
5-2 R需要转换
或者
过程
