【SLAM】VINS-MONO解析——对vins-mono的提升建议

vins-mono刷了三遍，手写vio刷了两遍，SLAM十四讲刷了两三遍，从一开始完全看不懂是啥，不知道什么是SLAM，什么是VIO，什么是VINS，什么是SO3/SE3，甚至不知道什么是IMU…再到自己写总结写攻略，真的有很多感慨。学习开源代码的目的其实就是是为了自己能够去开发一个vio，一个slam系统。

slam最难的就是工程化了，公式很多人推导了一遍又一遍，道理大家都懂，但是转化为c++项目就是另一码事了。

vins系列真的是神作，对沈老师和对秦神真的是佩服。在这里我对vins-mono进行了一下小小的提升和修改。阅读代码的时候，有一些函数，它真的是太长了，包含了很多功能，所以我第一次阅读代码的时候，很多逻辑，功能，我想不清楚它为什么是这样的。

所以我做了一个工作，就是把那些包含很多独立功能的函数，统统进行再次套娃和封装出去。对于初始化和后端优化部分，我从estimator.cpp文件里拿出了，新创建了intial.cpp和backend.cpp两个文件，也都采用了面向对象的方式进行了封装。这么做，虽然提升了可读性和逻辑性，并且有利于模块化开发，把具有特定功能的模块从总流程中提取出来，但是可能会造成执行效率的下降。不过我仍然认为，不能因为效率的略微下降而放弃对模块化的努力和尝试。

此外，在esitmator.h中，我发现了一些没有用过的变量，也都删除了，github链接请点击这里。

4.流程

4.1 调整结构

修改目的：把主流程中的代码全部封装出去，从而提升逻辑的清晰性，代码的可读性和代码的模块化，另一方面方便对某一个模块或者函数进行整体性的修改或重写。
C++注释规范 https://www.cnblogs.com/aspiration2016/p/8433122.html
需要注意的是，有很多局部变量，构造新函数的时候，需要传参。
这一部分已经完成，放在catkin_ws1中。

4.1.1 每个package增加include文件夹，把hpp文件都放进去

(1)注意修改h文件开头声明：#ifndef，#define，#endif。
(2)注意修改#include路径。
刚开始，h和cpp文件在同一个目录下，目标是在每一个package下面新建一个include文件夹，然后把h文件放进去，就像很多别人家的工程一样。需要注意的是，每一个cpp文件的include路径都需要根据相对路径的方式进行修改。
(3)注意修改CMakeLists.txt里的路径。

4.1.2 把初始化，非线性优化等各个步骤封装成函数，从主流程中提出来

(1)feature_tracker.h/cpp

把主流程函数readImage()里的 直方图均匀化、光流追踪、新特征点提取 这三个流程封装成为三个独立的成员函数，已经在h和cpp中更改；

(2)estimator_node.cpp

主要修改process()主线程；
这个线程主要包含3步：测量量的提取和对齐；对测量量的操作(vio主流程)；状态量的更新；
测量量的提取和对齐：没有进一步封装；
vio主流程：封装为processMeasurement(),然后进一步封装了processIMU(), setReloFrame(), processVIO(), visualize()。
update()没有进一步封装。

(3)estimator.cpp——processImage()

这个是vio主函数，里面主要包含了5个功能：

a.判断marg_old还是marg_new并将来自feature_tracker_node的特征点放入f_manager中；
b.创建数据结构all_image_frame服务于初始化；
c.标定camera->IMU的外参(封装为CalibrationExRotation()函数)；
d.初始化(封装为initial()函数)；
e.后端非线性优化(封装为backend()函数)；

后续，我会把初始化和后端从estimator.h里面拿出来放在一个新的h文件里，创建新的类。

(3)estimator.cpp——processImage()——initial()——initialStructure()
initial()这部分的主流程代码已经很清晰了，不用再封装，对其中的initialStructure()进行封装；

initialStructure()可以分成6步：

a.判断IMU是否有足够的运动激励，封装为checkIMUObservibility()函数；
b.创建数据结构sfm_f用于SfM过程，封装为buildSFMFeature()函数；
c.寻找滑窗里的第l帧，它与最新帧有足够的视差，求帧号l和最新帧到第l帧的 旋转平移；
e.SfM
f.对所有帧用PnP来求解他们在l帧上的位姿，封装为solvePnPForAllFrame()函数；
g.IMU和SfM粗耦合联合初始化visualInitialAlign()；
h. visualInitialAlign()包含两步，第一步就是IMU与视觉的粗耦合求各个待优化的状态量，第二步是把所有帧的位姿和landmarks的坐标转移到w系上，这部分封装为recoverStatusValuesFromInitial()函数中。

(4) estimator.cpp——processImage()——backend()——solveOdometry()——optimization()
optimization()是estimator.cpp中最长的单一函数了，我第一次看的时候总是陷入到单行代码中而没弄清楚总体流程。实际上这个函数主要干了2件事：

a.非线性优化，这部分封装为nonLinearOptimization()函数；
b.边缘化，其中marg_old封装为margOld()函数，marg_new封装为margNew()函数。

封装完成后，函数代码从原先的300行压缩到20行，能够一眼看出来函数逻辑和结构，也方便以后对它的模块化更改。

4.1.3 给hpp文件中各个成员变量和函数全部增加注释

4.2 把backend部分全部提取构造成类

这项工作是基于4.1的工作基础上的。在4.1，完成了对不同函数的进一步提取和所属功能的分类，现在，对vins_estimator文件夹下的estimator.h/cpp文件进行进一步操作，把属于后端优化的函数和变量全部提取出来，创建一个新的backend.h/cpp文件，并创建Backend类。不过，还有一种更简单的方式，就是新建h/cpp，但是不创建类，直接放函数。
这一部分已经完成，放在catkin_ws2中。

(1)构建backend类
由于初始化会用到优化部分的代码，所以先构建backend类。
如果新建一个h/cpp文件，会涉及到很多函数和变量的迁移和引用。考虑到后端会用到一些estimator.cpp的通用函数，所以需要传入estimator的this指针；
考虑到后端的作用是对状态量的更新，所以对backend类中需要只输入和输出状态量，而辅助的变量可以从estimator迁移到backend中去。

这里有一个非常麻烦的地方，就是estimator和backend这两个文件/类之间存在相互include/调用的情况。
另一方面，对于迁移的变量，需要把estimator中对这些变量的操作也迁移过来，对于没有迁移的变量，需要在这里进行指针操作。攻略见：
https://blog.csdn.net/xiqingnian/article/details/41214539?depth_1-utm_source=distribute.pc_relevant.none-task&utm_source=distribute.pc_relevant.none-task

(2)步骤

a.把后端相关函数和变量提取出来放到backend.h/cpp中；
b.把estimator.h和backend.h都要#include的头文件从estimator.h拿出放到backend.h里面；
c.backend.h中，不可以#include“estimator.h”；
d.backend.h中，在class Backend前面需要声明class Estimator，在class Backend内需要创建Estimator *estimator，注意，这个是指针；
e.backend.cpp中，需要同时include“estimator.h”和“backend.h”；
f. estimator.h中，需要include “backend.h”，在class Estimator前，需要声明class Backend,在class Estimator内，需要创建Backend backend，注意，这个是实例。
g.estimator.cpp中，需要include“estimator.h”，并且凡事在使用后端函数的时候，都需要传入this指针，这是由Backend的成员函数的定义确定的。
h.vins_estimator的CmakeLists.txt中的add_executable添加backend.cpp

(3)理由

class Estimator是主类，内部包含backend对象，创建backend成员变量时遇到一个问题，如何确定给backend这个数据成员分配多少内存呢？我们知道backend也需要使用estimator的方法，如果在backend内部使用的是estimator的指针，这样即能实现对estimator的调用，也能确定backend所需的内存，因为指针的大小是确定的，4个字节。此时，estimator创建的时候，知道给backend分配多少内容，那么就能实现正确创建。对于class Backend而言，因为一直使用的是estimator的指针，所以创建他的时候，也能确定自己所需要的内存。
所以，在Backend类中，一律创建和使用estimator的指针，而在Estimator类中，可以创建一个backend对象。

4.3 把inital部分全部提取构造成类

这项工作是基于4.2的工作基础上的。在4.2中，把所有的后端提取出来了。之所以先提取后端，是因为初始化步骤用到了后端的内容，在完成一次初始化后，紧接着跟着一次非线性优化和滑窗。所以，仅仅把初始化部分的initialStructure()提取出来放在initial.h/cpp中，而没有必要把后面的优化和滑窗操作也放进来，否则在initial.h/cpp中要传入backend和estimator两个大类的指针，这是没有必要。

(1)构建inital类
在上一步的基础上，这个操作就很容易了。这一部分已经完成，放在catkin_ws3中。完成后，estimator.cpp文件从1200行减少到220行。

4.2 对vins-mono的提升

在阅读代码的过程中，发现一些作用不大的，奇怪的数据结构和结构，在这里修改和测试。

(1)euroc.launch
路径：src/vins_estimator/launch/euroc.launch
里面有一个非常奇怪的路径，即：

  <arg name="vins_path" default = "$(find feature_tracker)/../config/../" />

修改为：

  <arg name="vins_path" default = "$(find feature_tracker)/../ " />

测试通过。

(2) estimator_node.cpp
路径：src/vins_estimator/estimator_node.cpp
定义了一个变量std::mutex i_buf;没有用上，删除。

(3) Estimator::clearState()
路径：src/vins_estimator/estimator.cpp
多写了一行solver_flag = INITIAL; ，删除。

(4) Estimator::processImage()
路径：src/vins_estimator/estimator.cpp
这里定义了一个数据结构all_image_frame，和相关连的数据结构比如说tmp_pre_integration，imageframe，它们在整个运行阶段都在不断的新建和维护，但是他们在代码中的作用仅用于初始化，所以我认为可以缩短以上数据结构的生命周期。在这些数据结构前面都加上if (solver_flag == INITIAL) 的判断。tmp_pre_integration在processIMU()中也出现过。
还有slideWindow()中对的marg_old操作也要加上判断。

(5) Estimator::initialStructure()
路径：src/vins_estimator/estimator.cpp
这个函数第一部分是用来判断IMU是否有足够的激励，但是作者又把return false注释掉了，所以我觉得整个代码都可以注释掉，减少计算量。但是这部分内容包含了ROS_INFO()，用于在log文件或者screen上显示机器人的运动激励。

当然了，考虑到这个函数仅仅在VINS生命周期的开头用上了，并且在这里有一个向log/screen的交互，不注释掉无所谓。

(6) estimator.h
路径：src/vins_estimator/estimator.h
头文件里出现了一些没有用上的数据结构，例如：

bool is_valid, is_key;

下面四个计数器，第一个和第四个就没有用过，中间两个虽然出现了，但是没有基于他们做进一步的操作：

int sum_of_outlier, sum_of_back, sum_of_front, sum_of_invalid;

还有，

vector<Vector3d> point_cloud;
vector<Vector3d> margin_cloud;

(7)FeatureTracker::readImage()
路径：src/feature_tracker/feature_tracker.cpp
作者在这个函数里面使用了3个时间，prev，cur和forw，向ROS发布的帧实际上是forw帧。但是我认为与prev相关的所有数据结构并没有什么作用，而且在feature_tracker中并没有看到对prev帧的相关数据有什么有意义的操作，所以我认为可以删掉，除了prev_un_pts_map保留下来用来计算像素速度。实际上在vins-fusion中，只保留了prev和cur这两个时刻的image，对应着vins-mono中的cur时刻和forw时刻。

删除以下内容：

prev_img = cur_img;
prev_pts = cur_pts;
prev_un_pts = cur_un_pts;

reduceVector(prev_pts, status);
reduceVector(cur_un_pts, status);