知识点:
如何使用GTSAM,从而输出一个丝滑的位姿;
已知上一帧lidar里程计(频率低),当前帧lidar里程计(频率低),两个lidar帧之间的IMU数据,如何利用图优化的方式优化位姿状态量(PVQ)和IMU状态量(noise,bias);
如何结合lidar里程计(频率低)和IMU数据(频率低)输出IMU里程计(频率高)。
如何使用两个预积分器,一个服务于图优化,一个服务于imu里程计的计算。
从看代码的角度讲,LIO-SAM涉及到的数学好像不是很多,基本GTSAM都给你搞定了,所以知道相关的基本原理就能看懂代码。这部分内容对应IMUPreintegration类,这个类对外的作用是 根据低频率的lidar里程计和高频率的IMU数据,输出一个高频率的IMU里程计。为了保证输出的里程计的精度,作者采用GTSAM+图优化的方式来做这个事情。
LIO-SAM涉及到4个.cpp文件,有5个主要的类,同时有多个发布和订阅的数据,必须要搞清楚当前这个类这个函数订阅的数据是哪来的,频率是什么样的,发布的是什么数据,会被谁订阅,详情请见这个流程图。
我自己的思考:把低频的lidar里程计转化为高频的IMU里程计,直接简单的在当前帧上进行IMU积分可不可以,一定要用算力昂贵的GTSAM吗,一定要用复杂的传来传去的数据传输吗,一定要进行图优化吗?当业务的参考模型是LIO-SAM的时候,当我的机器人硬件资源有限的时候,我一定要按照作者相同的玩法来吗?
在这个博客讲解的过程中,是按照过程的顺序或者说代码的顺序来的,同时避免了大段大段的复制代码,反正我看别人的博客一上来大段大段代码粘上去,我是新手的时候还是挺难看进去的。
[En]
In the process of explaining this blog, it comes in the order of the process or the order of the code, and avoids copying large chunks of code. Anyway, as soon as I see large chunks of code glued to other people's blogs, it's hard to see it when I'm a novice.
这个类包含两个主要的函数:
odometryHandler():
获取低频lidarpose,配上缓存的IMU数据,进行图优化,维护预积分器。