Skip to content

iwander-all/vins-mono-self-improved-

Repository files navigation

vins-mono(self-improved)

The official link is: https://github.com/HKUST-Aerial-Robotics/VINS-Mono

To run this project is exactly the same as the original one, take it easy. Put all the files above into src, and put src into catkin_ws.

Open terminal, type in "catkin_make" and drink a cup of coffee.

Then, open one terminal, type "roscore", of course you have to install ROS in advance.

And open 2 terminals and in the path of devel,type "source setup.bash", then fill in:

"roslaunch vins_estimator euroc.launch"

"roslaunch vins_estimator vins_rviz.launch"

Then another ternimal with:

"rosbag play YOUR_PATH_TO_DATASET/MH_01_easy.bag"

And my blog is https://blog.csdn.net/iwanderu/article/details/104926669.

修改日历: (1) 在每一个package下都创建了include文件夹,把.h文件都放在了include目录中,并修改了.cpp文件中#include的相对路径; (2) 把一些代码量比较长的,包含功能比较多的函数(例如process(), initialStructure(),Optimization()等)做了进一步的拆分; (3) 把initial和非线性优化部分从estimator.cpp文件中提取出来,放在了backend.h/cpp和initial.h/cpp文件中,并创建了class Initial和class Backend,estimator.cpp的代码减少到220行左右; (4) 在stimator::processImage()中,数据结构all_image_frame,temp_pre_integration等变量的生命周期被限定在了初始化的时间内; (5) 在文件中,删除了一些定义了但没有使用的变量: a. estimator_node.cpp中的std::mutex i_buf; b. Estimator::clearState()中多写了一行solver_flag = INITIAL; c. estimator.h中的bool is_valid, is_key和int sum_of_outlier, sum_of_back, sum_of_front, sum_of_invalid和vector point_cloud,margin_cloud。 (6) 在euroc.launch中,把修改为。 (7) 注意,回环检测的字典在原vins-mono的support-file里,因为超大了我没有上传,所以要想实现回环检测的正常功能,请把support-file复制到目录中。

以下为草稿:

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下面新建一个onclude文件夹,然后把h文件放进去,就像很多别人家的工程一样。需要注意的是,每一个cpp文件的include路径都需要根据相对路径的方式进行修改。

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是否有足够的运动激励,封装为checkOMUObservibility()函数; 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 里面有一个非常奇怪的路径,即: 修改为: 测试通过。

(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注释掉了,所以我觉得整个代码都可以注释掉,减少计算量。

(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 point_cloud; vector margin_cloud;

About

an improved version of vins-mono

Resources

License

Stars

Watchers

Forks

Releases

No releases published

Packages

No packages published

Languages