VINS系列|VINS-Mono论文精读（二）

紧耦合单目VIO

在估计器初始化后，我们使用基于滑动窗口的紧耦合单目VIO进行高精度且鲁棒的状态估计。滑动窗口的图示如图5所示。

A.公式

滑动窗口中的完整状态向量定义为：

B.IMU测量残差

C.视觉测量残差

D.边缘化

如图7所示，当第二个最新的图像帧为关键帧时，它将保留在滑动窗口中，而最旧的帧与它对应的测量被边缘化。否则，如果第二个最新的帧为非关键帧时，我们丢弃视觉测量，并且保留连接该非关键帧的IMU测量。

为了维持系统的稀疏性，我们没有边缘化非关键帧的所有测量。我们的边缘化策略旨在保留滑动窗口中在空间上分隔开的关键帧。这确保具有足够的视差进行特征三角化，并且最大限度地提高了维持具有较大激励的加速度计测量的概率。

使用Schur补执行边缘化。我们基于与被移除状态相关的所有被边缘化测量来构建新的先验。这一新的先验被加到现有的先验上。

注意，边缘化导致需要在早期固定线性化点，这可能导致次优的估计结果。但是，由于小漂移对于VIO而言是可接受的，因此我们认为由边缘化引起的负面影响并不重要。

E.相机帧率状态估计的纯运动视觉-惯性优化

对于具有低算力的装置（例如移动电话），由于非线性优化对计算要求较高，因此紧耦合的单目VIO无法实现以相机帧率输出。为此，除了全局优化外，我们还采用一种轻量级的纯运动视觉-惯性优化，以将状态估计的频率提高到相机帧率（30Hz）。

纯运动视觉-惯性优化的代价函数与公式（14）中单目VIO的误差公式相同。然而，我们仅优化固定数量的最新IMU状态的位姿和速度，而不是优化滑动窗口中的所有状态。我们将不希望优化的特征深度、外参、偏置和旧的IMU状态作为常值。我们使用所有的视觉和惯性测量数据进行纯运动优化。这导致比单帧PnP方法具有更为平滑的状态估计结果。所提出策略的图示如图8所示。

与可能导致在最先进嵌入式计算机上运行时间超过50ms的完整紧耦合单目VIO不同，纯运动视觉-惯性优化仅耗费大约5ms来计算。这实现了低延迟相机帧率的位姿估计，它对无人机和AR应用特别有益。

F.IMU频率状态估计的IMU前向传播

IMU测量的频率远比视觉测量频率高得多。尽管本文VIO的频率受限于相机采集频率，但是我们仍然能够使用最新的IMU测量数据来直接地传递最新的VIO估计值，以实现以IMU频率输出估计值的性能。高频率的状态估计能够被用作回环的状态反馈。

重定位

本文的滑动窗口和边缘化策略限制了计算复杂度，但是它也给系统引入了累积漂移。为了消除漂移，提出一种紧耦合的重定位模块，它无缝地与单目VIO集成。重定位过程从回环检测模块开始，该模块识别已经访问过的位置。接着，建立回环候选与当前帧之间的特征级连接关系。这些特征对应被紧集成到单目VIO模块中，以最小计算代价实现无漂移状态估计。对多个特征的多次观测被直接用于重定位，从而实现更高的精度和更好的状态估计平滑性。重定位过程的图示如图9(a)所示。

A.回环检测

我们利用DBoW2（一种先进的词袋位置识别方法）进行回环检测。除了用于单目VIO的角点特征之外，另外检测500个角点，并且使用BRIEF描述子进行描述。额外的角点特征被用于在回环检测上实现更好的召回率。描述子被作为视觉单词来查询视觉数据库。在经过时间和几何一致性检查后，DBoW2返回回环候选。我们保留所有的BRIEF描述子用于特征检索，但是丢弃原始图像来降低内存消耗。

B.特征检索

当检测到回环时，通过检索特征对应来建立局部滑动窗口和回环候选之间的连接关系。特征对应通过BRIEF描述子匹配来找到。描述子匹配可能造成一些错误的匹配对。为此，我们使用两步的几何异常值剔除，如图10所示。

1）2D到2D：使用RANSAC的基础矩阵测试。我们使用当前图像和回环候选图像中被检索特征的两维观测来执行基础矩阵测试。

2）3D到2D：使用RANSAC的PnP测试。基于局部滑动窗口中已知的特征三维位置和回环候选中的两维观测，我们执行PnP测试。

在剔除异常值之后，我们将回环候选作为一个正确的回环检测结果，并且执行重定位。

C.紧耦合重定位

全局位姿图优化和地图复用

在重定位之后，执行额外的位姿图优化步骤来确保过去的位姿集合被配准到一个全局一致的配置中。

A.四个累积漂移的方向

得益于重力的惯性测量，横滚角和俯仰角在VINS中是完全可观的。如图11所示，随着目标移动，三维位置和旋转相对于参考坐标系相对变化。

B.位姿图中加入关键帧

在VIO处理之后，关键帧被加入到位姿图中。每个关键帧作为位姿图中的一个顶点，并且它通过两种类型的边与其它顶点连接，如图12所示。

2）回环边：如果关键帧具有回环连接，它通过位姿图中的回环边连接回环帧。类似地，回环边仅包含一个四自由度的相对位姿变换，它与公式（19）的定义相同。回环边的值使用来自重定位的结果来获取。

C.4自由度位姿图优化

位姿图优化和重定位在两个单独的线程中异步运行。这允许在最优化的位姿图可用时，立即使用它进行重定位。类似地，即使当前位姿图优化尚未完成，仍然能够使用现有的位姿图配置进行重定位。该过程如图9（b）所示。

D.位姿图合并

位姿图不仅能够优化当前地图，还能将当前地图与先前构建的地图合并。如果我们已经加载了先前构建的地图并且检测到两张地图之间的回环连接，我们就能将它们合并到一起。由于所有的边都是相对约束，因此位姿图优化通过回环连接自动地将两张地图合并到一起。如图13所示，当前地图通过回环边被拉入先前的地图。每个顶点和每条边都为相对变量，因此我们仅需要固定位姿图中的第一个顶点。

E.位姿图保存

文章转载自公众号：自动驾驶专栏