预积分学习总结
今天是5月20日,为了避免像大多数人“只是过了个普通的周三”,昨天就准备了预积分的论文,准备今天认真学习。之前多次翻阅预积分的内容,也都是草草看过,一直没有搞清楚预积分具体在干什么。今天学习了 C. Forster 的"On-Manifold Preintegration for Real-Time Visual-Inertial Odometry",并找到了邱博在泡泡SLAM上整理的《IMU预计分总结与公式推导》,大体上了解了IMU与积分的基础内容。看懂之后,觉得没有虚度周三,便写个总结。
0. 如果没有预积分?
在看推导的过程中,有困惑像邱博本人请教,邱博让我思考一个问题:如果不用预积分的话,IMU和视觉如何融合?这个问题在我理解了预积分的想法之后,认为:预积分构建了视觉两个关键帧之间的约束,结合视觉的预测,构成残差项,从而优化残差对R, v, p三个量进行优化。如果没有预积分,只能是IMU从头积分到尾,与全部的视觉关键帧,进行Full BA。
1. IMU测量与运动模型
测量模型即:获得角速度与加速度,角速度是以body坐标系计算的,而加速度需要从world系转到body系。二者都需要加上漂移bias和噪声。
在两个Key Frame之间,角标用 i 和 j 表示,之间的旋转、速度、平移,分别利用李代数增量、、,得到公式(32)。可以发现如果用(32)这个式子进行计算的话,等号右侧遇到了 三个变量,而这三个变量是依赖于状态 i 的,所以如果直接进行积分,每次计算下一个KF的状态时,需要从上一个KF的状态进行积分,造成了计算复杂度较高。
于是对(32)进行变形,定义了旋转、速度、平移的增量如下:
其中第一个等号上方有一个圆点,代表“定义为”,第二个等号后面为实际的计算式。可以看出,计算 时只需要IMU的数据进行积分,而不需要依赖之前状态。之后计算 v 和 p 时利用计算出来的也可以避免 v 和 p 的上一个状态。由此我们得到了预积分的式子。
这个式子还有一个问题,式中小括号里面的代表“真实变化量”,而我们拿到的是带有噪声的测量量( 上方都带有波浪号~)。如何去掉噪声和漂移的影响?
2. 噪声项与噪声的传递
我们对(33)式进行变形,具体推导过程参考论文或邱博的文档。首先假设漂移量 是不变的,分别得到旋转R、速度v、平移p,分离测量与噪声的表达式:三个式子都表示成了:“真值=测量-噪声”的形式。只不过噪声项的形式非常复杂而已。那么我们如何估计噪声?或者说噪声是如何传递的?论文给出了推导,证明旋转R、速度v和平移p的噪声项一阶近似后是符合高斯分布的,由此可以推导出:从上一个时刻的噪声,到这一个时刻的噪声的表达式,即:
上式来自论文的附录A,同时邱博的文档中也给出了详细的推导,即 的表达式。到这为止,我们得到了假设漂移不变时,噪声的传递形式。
3. 漂移量的更新
论文指出,如果我们每次重新去估计当前时刻的漂移值,计算复杂度太高,于是发现下一时刻的漂移可以用当前时刻的漂移,与当前状态的线性化(雅克比)增量进行表示,即:
(44)中左侧表示下一个时刻的漂移,右侧表示用当前时刻的漂移(上方带一个横道,bar)与雅克比(R、v、p对 的偏导)进行近似。具体的雅克比形式在论文附录邱博文档也给了出来,如下:
如此一来,我们可以推导 bias 的变化。
4. 残差项
当获得了预积分结果之后,我们得到的是两个关键帧之间的旋转、速度、平移约束,与视觉观测的结果做残差,得到残差项如下:使残差最小,进行优化,即可完成 IMU 与视觉的融合。
5. 心得体会
耐下心认真看,总是能看懂的。希望自己下一个520有人陪着过。
6. 参考资料
C. Forster. On-Manifold Preintegration for Real-Time Visual-Inertial Odometry.
邱笑晨:《IMU预积分总结与公式推导》
下载链接:https://pan.baidu.com/s/1APWzyhWiRJKDwCgWsLKBiA 提取码: q82u