LIO-SAM代码简明分析

LIO-SAM代码简明分析

0.IMU预积分

目的

1.IMU通过加速度计和陀螺仪测出的是加速度和角速度，通过积分获得两帧之间的旋转和位移的变换；
2.在后端非线性优化的时候，需要优化位姿，每次调整位姿都需要在它们之间重新传递IMU测量值，需要重新积分，这将非常耗时，为了避免重新传递测量值，所以采取预积分策略。

一.ImageProjection：接受来自IMUPreintegration的高频激光里程计odom，发布经过运动畸变校正的点云。

1.点云去除运动畸变

(1).首帧imu姿态角赋予lidar帧做初始姿态角，之后令第一个imu帧旋转角为0，则之后每一个IMU时刻的旋转角都是基于当前IMU角速度和之前旋转角的累加。
(2).通过匀速模型计算之后每个imu数据帧的旋转角。
(3)获得首尾激光里程计之间的位姿变换
(4).寻找每个激光点云点最近的两个相邻imu帧，通过两个相邻imu帧之间的旋转，线性插值获得每个激光点云点的旋转。
(5).通过获得当前时刻与初始时刻之间的位姿变换，计算每个激光点云点与初始时刻的位姿变换，并且将每个激光点云点补偿到第一个点对应的时刻。

二.IMUPreintegration：接受imu原始数据与mapOptmization发出的里程计数据，发出经过imu预积分优化之后的里程计数据。

1.imu预积分以及更新步骤(imuHandler与odometryHandler结合观看的）

1. 第一次优化：

• 传入imu数据，传入mapOp发布的激光里程计数据；
• 系统初始化：获得先验odom位姿，重置imu积分器和opt积分器；
• 提取两帧imu数据，放入opt积分器进行预积分，构建imu因子；
• 然后把Xkey-1 到Xkey之间，加入这个imu因子以及 激光里程计提供的pose因子，整体做一个优化。优化的结果就是bias，以及“第二帧开始”这个时刻的系统位姿；
• 把优化的结果（主要是bias），重置opt积分器和imu积分器。 然后把当前帧（上面的“第二帧开始”）之前的数据给删掉，用imu积分器，从“第二帧开始”这里开始往后积分。（我们需要明确一点，在这个处理过程中，imu队列也在持续的进数据，（即1.1的imu