文章目录
- 原始积分传递方程
- 预积分的原因
- 预积分做法
- 零偏建模的原因
- 零偏建模做法
- 离散时间的预计分更新
- 视觉VIO中预计分和视觉的建模图解
- LIO－SAM中预计分和lidar位姿的建模图解
原始积分传递方程
首先看 IMU 原始积分的公式 ，两个关键帧 b k , b k + 1 b_{k} ,b_{k+1} bk,bk+1之间的状态传递
实际的系统状态是离散的，则变成如下
预积分的原因
从上面可以看出，当 k 时刻的状态发送变化时，第 k+1 时刻的状态也要发送变化，此时则会产生重新积分，而在因子图优化过程中，对状态调整是必然的，每次调整都会重新积分则很耗时，问题就在于，这个重新积分的的积分项的结果是不变的。
（个人理解为，假设调整第 k 时刻的位姿，但是 k 到 k+1时刻的IMU积分项是不会变的，因为这就是个固定的测量值，但是根据最上面的公式可以看到，当调整 k 时刻位姿时 第 k+1 时刻就要重新积分，而在图优化中，关键帧的位姿在不停的调整是必然且正常的，每次调整都要使得第 k+1 帧的位姿重新积分很耗时间）
- 预积分的作用：使得IMU积分的结果和上一时刻系统的状态无关，这样当系统状态在优化中发送调整时就不需要对下一时刻的系统状态进行重新积分了
预积分做法
两边同乘 R w b k R_{w}^{b_{k}} Rwbk
乘了以后，所有状态量都和世界坐标系无关，都变成只和当前起始的 第 k 帧有关，即和前一个关键帧相关
这么理解，当IMU开始时 PVQ都是单位阵，随着IMU数据进来，就变成相对于起始时刻的积分，而不是相对于世界坐标系的状态进行积分，这样就使得预积分项与第 k 时刻与 第 k+1 时刻，三者间都无关系，这样当第 k 时刻的状态发生变化时直接加上之前算好的预积分项即可，不用重新积分一遍
零偏建模的原因
上面的操作使得状态改变时不用重新积分，但是预计分虽然和两帧的具体位姿和速度无关，但是和零偏相关，从上面公式中可以看到零偏在积分项中，所以当零偏改变就需要重新积分，这样前面预积分的操作就失去意义了
零偏建模做法
为了避免零偏的变化导致预计分重新积分，，考虑通常零偏的变化在短时间内（100ms）非常小，因此可以用泰勒展开式来近似
这样当零偏被优化时，只需要根据以上公式进行更新即可，这样就不用重新积分了，里面的关于零偏的雅客比在预计分计算的时候一并算出来的
离散时间的预计分更新
实际系统是离散的数据，每收到一帧新的IMU数据时更新一次预计分量，这里是求和不是连续积分的过程，公式如下
视觉VIO中预计分和视觉的建模图解
这⾥⻩⾊代表IMU预积分，可以看到其只对相邻帧发⽣约束，五⾓星代表视觉地图点，同⼀个地图点可以通过重投影误差对多个关键帧的位姿形成约束
LIO－SAM中预计分和lidar位姿的建模图解
如图，蓝⾊圆圈代表关键帧位姿，蓝⾊矩形代表关键帧速度和零偏，橙⾊矩形代表IMU预积分约束，可以看到，它可以约束相邻帧的位姿、速度和零偏，绿⾊矩形代表lidar⾥程记的帧间约束，其约束相邻两帧的位置和姿态。