VINS-Mono+Fusion源码解析系列（二十）：边缘化约束

最新推荐文章于 2024-11-01 21:47:16 发布

原创

最新推荐文章于 2024-11-01 21:47:16 发布 · 788 阅读

标签

#自动驾驶 #计算机视觉 #人工智能

本文深入探讨VINS-Mono中的边缘化过程，重点解析舒尔补在信息矩阵变换中的应用。通过边缘化，保持系统优化的同时剔除旧帧，以减小计算负担。文章详细阐述了舒尔补的实现、信息矩阵H的变化，以及边缘化在VINS-Mono中的具体实现策略和关键类的作用。

1. 舒尔补

1.1 舒尔补的实现

当滑动窗口新增关键帧时，由于由于滑动窗口大小一定，新进来一帧关键帧意味着必然有老的一帧被剔除出去。直接暴力剔除被边缘化帧会损失掉与之相关的信息，因此需要通过舒尔补的方式，在保留它与剩余帧之间的联系前提下将其剔除。

对于滑动窗口中优化求解的增量方程，有：
$H\delta x = b$
根据待边缘化的状态量，对上述方程进行划分，如下所示：
$\left[ \begin{array}{cc} \Lambda_a & \Lambda_b \\ \Lambda_b^T &\Lambda_c\end{array} \right]\left[ \begin{array}{cc} \delta x_a \\ \delta x_b \end{array} \right] = \left[ \begin{array}{cc} g_a \\ g_b \end{array} \right]$
其中， $δxa\delta x_a$ 为待边缘化的状态量， $δxb\delta x_b$ 为保留下来的状态量， $H$ 和 $b$ 根据状态量的划分进行相应的分块处理。

舒尔补化简如下：
$\left[ \begin{array}{cc} I & 0 \\ -\Lambda_b^T\Lambda_a^{-1} & 1 \end{array} \right]\left[ \begin{array}{cc} \Lambda_a & \Lambda_b \\ \Lambda_b^T &\Lambda_c\end{array} \right]\left[ \begin{array}{cc} \delta x_a \\ \delta x_b \end{array} \right] = \left[ \begin{array}{cc} I & 0 \\ -\Lambda_b^T\Lambda_a^{-1} & 1 \end{array} \right]\left[ \begin{array}{cc} g_a \\ g_b \end{array} \right] \\ \left[ \begin{array}{cc} \Lambda_a & \Lambda_b \\ 0 & \Lambda_c - \Lambda_b^T\Lambda_a^{-1}\Lambda_b \end{array} \right]\left[ \begin{array}{cc} \delta x_a \\ \delta x_b \end{array} \right] = \left[ \begin{array}{cc} g_a \\ g_b - \Lambda_b^T\Lambda_a^{-1}g_a \end{array} \right] \\ (\Lambda_c - \Lambda_b^T\Lambda_a^{-1}\Lambda_b)\delta x_b = g_b - \Lambda_b^T\Lambda_a^{-1}g_a$
经过舒尔补边缘化后的增量方程仍然是一个 $Hδx=bH\delta x = b$ 形式， $H$ 可分解为 $J^TJ$ ， $b$ 可分解为 $J^Te$ ，分解后获得的 $J$ 和 $e$ 就是边缘化后的约束。

1.2 舒尔补后 $H$ 矩阵的变化

（1）图优化建模

假设有如下最小二乘系统，对应的图模型如下图所示：
$\xi=argmin_{\xi}\frac{1}{2}\sum_i{\vert| r_i \vert|^2}_{\sum_i}$
在这里插入图片描述

圆圈：表示顶点，需要优化估计的变量
边：表示顶点之间构建的残差。有一元边（只连一个顶点），二元边，多元边……

上述最小二乘问题，对应的高斯牛顿求解为：
$J^T\Sigma^{-1}J\delta{\xi} = -J^T\Sigma^{-1}r$
雅可比矩阵 $J$ 如下：

最低0.47元/天解锁文章