Vins-fusion用到的kitti数据集轨迹对不齐，使用evo -a转换

kitti数据集基准问题

下面两个图一个是转换前，一个是evo -a 转换后的

---

问题描述：

现在遇到的问题是groundtruth和估计的位姿没有在一个坐标系中，生成的轨迹对不齐，需要首先根据位姿的时间戳将真实值和估计值进行对齐。对于双目SLAM和RGB-D SLAM，我们需要通过最小二乘法计算一个从估计位姿到真实位姿的转换矩阵SE3。
这个矩阵怎么求呢？

Vio通过WGPS_T_WVIO矩阵进行了平移和旋转之后，生成了vio-fusion。
WGPS_T_WVIO矩阵是视觉到gps坐标系下的转换矩阵，Groundtruth用WGPS_T_WVIO矩阵进行转换，得到的也没有用的，因为WGPS_T_WVIO矩阵是vio与gps融合得出的，以gps为基准。

疑问：evo –a 命令会得到一个旋转过后的groundtruth，这个旋转平移是怎么进行的？应该用groundtruth与vins-fusion反推旋转矩阵。
解答：通过最小二乘法计算一个从估计位姿到真实位姿的转换矩阵，怎么做？

解决方案

理论可求，实际却不简单，感谢啰锣同学帮忙解决，找到了下面的方法~
evo -a 是可以求得的，evo调用调用eigen库的umeyama算法，最后用得到的变换矩阵作为最终的变换矩阵，具体可以看evo的源码。
evo本身带有日志输出功能，调用命令启用该功能，然后就可以得到对齐的旋转矩阵和平移向量

evo_config set global_logfile_enabled true //输出日志
evo pkg --clear_log  //清空日志
evo pkg --open_log  //打开日志