Sophus + ceres用于SE3优化可能出现的问题

利用Sophus和ceres做位姿优化可能掉进的坑

这一问题与数据存储方式及在更新操作中调用SE3::exp函数的操作有关

Eigen内部Quaterniond元素存储位置与初始化构造函数中的参数顺序存在差异

1、Eigen初始化Quaterniond(w,x,y,z),其中w是实部，x,y,z对应三个轴的虚部

然而，在数据存储顺序上与Eigen内部的Quarterniond存在差异；具体来说是按照x、y、z、w的顺序。

在Sophus内部实现中使用了Eigen库中的四元数类来表示SO3部分，在该框架中平移部分则由Eigen库提供的三维向量类Vector3d来表示。为了访问SE3对象的实际数据区域可以通过调用SE3::data()方法获得一个指向其实部存储空间的双精度浮点数指针

data()[0]对应Quaterniond x

data()[1]对应Quaterniond y

data()[2]对应Quaterniond z

data()[3]对应Quaterniond w

data()[4]对应SE3的tx

data()[5]对应SE3的ty

data()[6]对应SE3的tz

然而，在 Sophus::SE3d::exp(const Eigen::Vector6d &a) 函数中该参数 a 的前三个维度分别对应于 SE3 群体中的