视觉lidar融合学习日记--LVI-SAM_lvi-sam后端优化

今天开始搞lvi-sam，先看一下数据效果。

1. 开始测试：

第一次测试：

节点成功启动

a. 问题： 运行没有成功结束,因为最后没有看见odometry的痕迹

这期间的报错

b. 警告： 四元数没有normalize

c. 警告； 图片接收出现问题，ros传输机制有问题？

d. 报错： imu偏差过大，vins重启

e. 报错： LM分解出现问题

f. Vins这样经常报错真的好吗？

第二次：

轨迹严重偏离，vins不断重启。不过也有可能是ros传输机制的问题。

在论文中也提到了，当特征值比较少，或者imu的bias过大时，也会有这种情况发生。

第三次，跑通了，没有其他报错。

小结：

第一天经过三次代码运行，对于论文强调的高鲁棒性表示了质疑，因为vins重启，失败的概率比较高。而在这种情况下，轨迹发生了严重的偏移，基本可以认为定位失败。如何避免这种情况，也是一个值得思考的问题8。

下一步，尝试将数据在其他针对驾驶的应用场景的数据集使用。

_____________

去楼下打了两个小时的球，带着焦虑的心情打球只有一种结果，屡战屡败。回来继续学习！

来看崔华坤男神讲vins。

坤男神的vins学习建议：

不要死记硬背，可以做如下尝试：

1. 把vins中的ros，闭环去掉，实现相对定位，如果你对绝对定位要求不高的话

2. vins在手机或者小车上跑通

3. 用特征点法替换掉LK光流，提高前端鲁棒性， lk光流速度快，但是不够鲁棒，要不要考虑用orb

4. 优化后端效率，vins后端效率比较低，看看是hx=b，舒尔补，哪部分比较耗时，提高帧率至20Hz。

在观察效果的时候，我们可以关注初始化时的问题，跟踪的问题。

 之后男神在做ros的科普教学（男神真是平易近人。。。）

euroc.launch 包括 feature_tracker, vins_estimator, pose_graph。

 

任何一个项目要先看cmakelists.txt, 查看可执行文件（add_executable），依赖库！（我一直以为一个项目要先看readme。。。，难道不对吗？）

 

男神说，先看后端！

主要关注标注的两个callback和一个后端线程。

再通过图流程图来看看，可以看到，callback后都是向放到buffer里面，然后通过getMeasurements再去找符合条件的数据。

TODO：我想看看他是怎么找的。

c++ 线程互斥锁

坤男神说每次了解一个新函数都会看看它的api，我也看了看

https://www.cplusplus.com/reference/mutex/mutex/

。。。赞一下给的例程，非常直接。

 接着，介绍了一下线程锁在程序里的应用。

 

 TODO；我也要过一遍后端的三个线程。

Comment: 在csdn写了关于这部分多线程操作的总结，值得注意的是，这里采用condition variable和wait的方式有效解决了问题。

再来看看前端的Node：feature_tracker

源文件只有3个。

再来看main函数。

从main函数可以看出来，主要就是调用了img_callback。

img callback最核心的函数就是readImage(),他做了5个事情。

1. 自适应直方图均衡化，目的是提高图像对比度。

2. 金字塔的光流跟踪，直接调用opencv的金字塔光流跟踪函数，跟踪两帧之间特征点，值得注意的是，如果采用imu作为预估计值的话，可以减少搜索范围。

3. 然后，通过计算F mat的方法，去除异常点，这是我在pcv里学的，但是有点忘记了，然后点数多的化，用ransac，否则用七点法，或者8点法的话，碰到异常点就gg了。

4. 设置mask

5. 提取托马斯角点。

TODO： 复习fundamental matrix的相关知识。

 rosmsg show XXXX 可以看msg的具体内容。

男神科普小课堂：

queue的back是最新的元素。

pop 可以去除front（最老的元素）

附上cpp链接 http://www.cplusplus.com/reference/queue/queue/。

我想总结一下，getMeasurements 干了啥？ ==》同步imu和同时刻的lidar信息，如果图像太老（在图上对应图像在前面），就去掉图像，如果imu太老，就选择等待新的imu。

 

接下来介绍视觉和imu联合优化的方式。 imu和视觉的重投影都会提供约束。 我们要考虑更相信谁，也就是imu的噪声。

然后开始介绍imu预计分（我曾经的噩梦）

在这里要注意的是，push_back所针对的是IntegrationBase，不是vector，而里头最重要的是函数propagate,可以用来更新midPointIntegration.

那么 midPointIntegration有啥用呢？ 

1. 它可以更新IMU约束的误差项

2. 它可以更新IMU约束的协方差，Jacobian。

协方差矩阵在imu更新的时候，从0开始，不断增大。

 

坤男神布置的小作业:

  

后端的优化也可以画出啦

 

 

观后感：

1. 崔男神能够不用公式把原理讲的这么直观是真的厉害。

2. 当然，也需要我进一步琢磨，关注细节是解决难题的关键。预计分的部分当然是难点，但是不应爱陷入进去，而失去了对整体的把握。

3. 重投影应该好好复习。