用EVO工具评定SLAM系统参数(以ROVIO为例)
网上关于ROVIO的资料着实不多，今天我写一下最近跑ROVIO的笔记。多谢前人们的经验，帮我踩了很多坑。文章中有一些能查到的问题我不会说为什么，希望大家能自己思考，并参照论文找到答案。
其实一直想做一版ROVIO的注释代码，奈何自己也读不懂，更没时间哈哈哈。希望后两三个月有时间做吧。
文章会分以下几个模块来写：
一 ROVIO的安装及运行(附一些问题解决办法)
二 评测ROVIO在Euroc数据集上的结果
三 修改参数让ROVIO在Euroc的MH_05数据集表现更优
一 ROVIO安装与运行过程中的问题
安装过程：
首先，ROVIO的安装需要一些依赖项。
- ROS(感谢北邮的大神，分享了一版非ROS版的代码，其实ROS版本和非ROS版本的区别就在于在工程上，有很多时候PC上有，而开发板上没有ROS，所以需要去掉ROS，离线处理)
- kindr(很多网上的链接都会通过sudo apt-get install ros-indigo-kindr-*命令安装，但是由于我的版本是Ubuntu16.04，安装了ros-kinect，所以不能通过此步骤安装)
- lightweight_filtering
- kindr、(在rovio目录下操作)
安装过程请见：
下载数据集的时候，要下载.zip文件，因为下一步的evo评定工具要用到其中的ground_truth.csv文件。
最后一步，可以如此操作：
$ roslaunch rovio rovio_node.launch
$ rosbag play -r 20 /home/$YOUR_PATH/MH_01_easy.bag这样可以不用去操作rovio_rosbag_node.launch中的参数。
二 EVO工具评测ROVIO在Euroc数据集上的结果
首先是最重要的部分：如何在ROVIO跑数据集的时候取出想要的部分数据？
在Rovio_Node.hpp中，定义一个ofstream类，然后在发送IMU位姿上面，将这八个数据(时间戳，p的xyz，q的xyzw)保存到自己设定路径下的.txt文件中。代码如下：
o_pose << ros::Time(mpFilter_->safe_.t_) << " " << imuOutput_.WrWB()(0) << " " << imuOutput_.WrWB()(1) << " " << imuOutput_.WrWB()(2) << " "
<< imuOutput_.qBW().x() << " " << imuOutput_.qBW().y()<< " " << imuOutput_.qBW().z() << " " << -imuOutput_.qBW().w() << std::endl;其中，在MH系列数据集的mav0/state_groundtruth_estimate0文件夹中，都有一个data.csv文件，将其转换为.tum格式文件，才能用工具进行评定。
提示： 如果您不知道应该将哪种格式用于SLAM算法，请使用TUM格式。它很简单，但以后可以转换为任何其他格式，不需要ROS。
不同数据集下，转换格式的文章：
非ROS版本运行时，由于 Euroc 时间戳超过了 int64 的位数，所以需要修改 VIO 测试代码 Euroc 时间戳。详情参考：
下载并使用工具的文章：
三 修改参数让ROVIO在Euroc的MH_05数据集表现更优
首先，先说明我运行rovio_rosbag_node.launch，跑MH_05_difficult.bag和MH_03_medium.bag，生成的图是有一定问题的。即使优化完，也是不符合我的预期。由于有其他任务，暂时不能做下去了，所以贴一些图，对图中数据做一定分析，以及说明为什么要做优化。
从以上三张图中可以看出，曲线拟合效果并不理想，最大误差大约在 3.25m 左右，平均误差 1.17m 左右。这一误差太大了，虽然说MH_05是最难的一个数据集，但作为ROVIO来说，表现不应该这么差。所以通过初步优化，得到了0.2版本。
为什么要做这一步优化？
跑过MH_05数据集的小伙伴都知道，这个数据集是先把相机拿起来晃几下由放回原位，这个过程大概有十几秒，才拿起来去进行其他测试。看代码了解到，IMU初始化部分就是用第一帧去初始化的，以后的过程就会产生累计误差。所以相对来说，可以修改进行初始化那一帧的时间戳，让其在十几秒之后的那一帧再进行初始化，将数据集进行一定优化之后，再比对。具体方法是在imuCallback下添加一个if判断条件。其中的时间戳需要自己去/mav0/cam0/data/文件夹中查找，代码如下：
if(imu_msg->header.stamp.toSec() < $YOUR_TIMESTAMP){
return;
} 以下结果是优化后的：
可以明显看出，效果强了一些，最大误差大约在2.09m 左右，平均误差 1.05m 左右。误差虽然还是很大，但比前一版本强多了。相比之下，其在MH_03_medium的表现更靠谱一些。但并未满足论文中那么小的误差级别。所以肯定是有问题，之后再回来查找。
以上。
参考链接很多已经贴上了，以下是一些ROVIO推荐阅读：
桌边随笔：
这段时间雄心壮志想写艾伦方差标定的文章，还有因子图构造Hessian矩阵求解的文章。奈何精力有限，实力太菜，所以暂时保持周更吧。
希望大家多评论，有问题一起探讨，是我最开心的事。
