本文深入分析了VINS-Mono中的边缘化理论，解释了如何通过删除旧状态并保留约束来转换优化问题。介绍了如何利用增量方程和雅克比矩阵进行边缘化迭代，并展示了在estimator.cpp的optimization()函数中边缘化最老帧和次新帧的代码实现。
1.边缘化理论
在阅读本篇博客前，建议读友先去看看崔神的文章《VINS论文推导及代码解析》的第6节和高神的《视觉SLAM十四讲》的10.2.3节的内容，边缘化是VINS-Mono系统中比较难理解的地方，本人理解的也不是很透彻，只是想把自己的一些浅见分享出来，希望各位大佬看到后能够指点一二。
边缘化是指将滑窗中旧的状态删除掉，但却保留下旧的状态所带来的约束关系的一个过程，相当于将约束信息转换为优化变量的先验信息。
我们都知道优化问题主要就是求解下式，
H Δ x = g H\Delta x=g HΔx=g
这里， H = J T J H=J^TJ H=JTJ， g = J T e g=J^Te g=JTe，所以
J T J Δ x = J T e J^TJ\Delta x=J^Te JTJΔx=JTe
如果， Δ x = [ Δ x 1 Δ x 2 ] \Delta x=\left[\begin{matrix}\Delta x_1\\\Delta x_2\end{matrix}\right] Δx=[Δx1Δx2]， H = [ H 11 H 12 H 21 H 22 ] H=\left[\begin{matrix}H_{11}&H_{12}\\H_{21}&H_{22}\end{matrix}\right] H=[H11H21H12H22]， g = [ g 1 g 2 ] g=\left[\begin{matrix}g_1\\g_2\end{matrix}\right] g=[g1g2]，并且 Δ x 2 \Delta x_2 Δx2要被边缘化掉，则根据下式，
[ H 11 H 12 H 21 H 22 ] [ Δ x 1 Δ x 2 ] = [ g 1 g 2 ] \left[\begin{matrix}H_{11}&H_{12}\\H_{21}&H_{22}\end{matrix}\right]\left[\begin{matrix}\Delta x_1\\\Delta x_2\end{matrix}\right]=\left[\begin{matrix}g_1\\g_2\end{matrix}\right] [H11H21H12H22][Δx1Δx2]=[g1g2]
可得，
( H 11 − H 12 H 22 − 1 H 21 ) Δ x 1 = g 1 − H 12 H 22 − 1 g 2 (H_{11}-H_{12}{H_{22}}^{-1}H_{21})\Delta x_1=g_1-H_{12}{H_{22}}^{-1}g_2 (H11−H12H22−1H21)Δx1=g1−H12H22−1g2
令 H ∗ = H 11 − H 12 H 22 − 1 H 21 = J α T J α H^*=H_{11}-H_{12}{H_{22}}^{-1}H_{21}=J^T_\alpha J_\alpha H∗=H11−H12H22−1H21=JαTJα， g ∗ = g 1 − H 12 H 22 − 1
最低0.47元/天 解锁文章
1313
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
