VI. 紧耦合单目VIO
估计器初始化后，我们采用基于滑动窗口的紧耦合单目VIO进行高精度和鲁棒状态估计。图3显示了滑动窗口的图示。
A. 公式化
滑动窗口中的完整状态向量定义为：
其中是捕获
图像时的IMU状态。它包含了IMU在世界帧中的位置、速度和方向，以及在IMU 本体坐标帧中的加速度计偏置和陀螺仪偏置，n是关键帧的总数，m是滑动窗口中的特征总数，
是第一次观测到的
特征的逆深度。
我们使用视觉惯性集束调整公式（BA）.我们最小化所有测量残差的先验和马氏范数之和，得到最大后验估计：
和
分别是IMU和视觉测量的残差。剩余残差项的详细定义将在第VI节的B和C中提出。
是所有IMU测量的集合，C是在当前滑动窗口中至少观察到两次的一组特征。{
}是来自边缘化的先验信息。Ceres求解器[38]来求解这个非线性问题。
B. IMU 测量残差
考虑滑动窗口中连续两个帧和
内的IMU测量，根据(13)中定义的IMU测量模型，预积分IMU测量的残差可以定义为：
其中，是提取四元数q的向量部分(既四元数虚部)，表示误差状态。
是四元数的三维误差状态表示。
是在两个连续图像帧间隔时间内，仅使用含噪声的加速度计和陀螺仪测量值的IMU预积分测量项。加速度计和陀螺仪偏置也包括在在线校正的残差项中。
C. 视觉测量残差
与传统的在广义图像平面上定义重投影误差的针孔相机模型相比，我们在单位球面上定义摄像机的测量残差。几乎所有类型相机的光学，包括广角、鱼眼或全向相机，都可以模拟为连接单位球体表面的单位射线。考虑第一次在第幅图像中观察到的第
个特征，第
幅图像中的特征观测的残差定义为：
其中是第
个特征在i图像中的第一次观测。
是相同特征点在j图像中的观察。
是利用摄像机内参将像素位置转换成单位向量的反投影函数。由于视觉残差的自由度是2，所以我们将残差向量投影到切平面上，如图6所示，
是两个任意选择的正交基,它们生成
的切平面。我们可以很容易地找到一组
，如算法1所示。在(22)中使用的
是正切空间中固定长度的标准协方差。
D. 边缘化
为了限制基于优化的VIO的计算复杂度，本文引入了边缘化.我们有选择地将IMU状态和特征点
从滑动窗口边缘化，同时将对应于边缘状态的测量转换为先验。
如图7所示，当次新的帧是关键帧时，它将停留在窗口中，而最老的帧与其相应的测量值被边缘化（边缘化的测量值作为先验信息）。否则，如果次新的帧是非关键帧，我们丢掉视觉测量值和保留连接到这个非关键帧IMU测量值。为了保持系统的稀疏性，我们不丢弃非关键帧的所有测量值（留下IMU预积分值）。我们的边缘化方案的目的是在窗口中保持空间分离的关键帧。这确保了特征三角化有足够的视差，并且最大限度地提高了在大激励下获得加速度计测量值的概率。
边缘化是利用舒而补[39]进行的。我们基于与移除状态相关的所有边缘化测量来构造一个新的先验。新的先验项被添加到已有的先验项中。
我们确实注意到，边缘化导致了线性化点的早期固定，这可能导致次优估计结果。然而，由于小型漂移对于VIO来说是可以接受的，我们认为边缘化所造成的负面影响并不重要。
E. 仅运动视觉惯性BA的相机帧率状态估计
对于计算能力较低的设备，如手机，由于对非线性优化的计算要求很高，紧耦合单目VIO无法以相机速率输出。为此，我们采用了一种轻量级的仅运动视觉惯性集束调整，以提高状态估计到相机的速率(30赫兹)。
仅运动视觉惯性集束调整（BA）的代价函数与(22)中的单目VIO的代价函数相同。然而，我们没有对滑动窗口中的所有状态进行优化，而是只对固定数量的最新IMU状态的姿态和速度进行了优化。我们将特征深度、外部参数、偏置和旧的IMU状态作为常量来处理，我们不希望优化这些状态。我们使用所有的视觉和惯性测量来进行仅运动的BA.这使得状态估计比单帧PNP方法更平滑。图8显示了提出方法的插图。与在最先进的嵌入式计算机上可能导致超过50ms的完全紧耦合的单目VIO不同，这种只需运动的视觉惯性集束调整只需大约5ms即可计算。这使得低延迟相机的姿态估计对无人机和AR应用特别有利.
F. IMU频率状态估计的IMU向前传递
IMU测量的频率比视觉测量高得多。虽然我们的VIO的频率受到图像捕获频率的限制，但是我们仍然可以通过最近的IMU测量来直接传播最新的VIO估计，以达到IMU速率的性能。高频状态估计可以作为回环检测的状态反馈。利用这种IMU速率状态估计进行的自主飞行实验在第九节的D中进行。
G. 故障检测和修复
虽然我们紧耦合的单目视觉对各种具有挑战性的环境和运动是鲁棒的。由于强烈的光照变化或剧烈的运动，失败仍然是不可避免的。主动故障检测和恢复策略可以提高系统的实用性。故障检测是一个独立的模块，它检测估计器的异常输出。我们目前使用以下标准进行故障检测：
*在最新帧中跟踪的特征数小于某一阈值；
*最后两个估计器输出的位置或旋转有较大的不连续性；
*偏置或外部参数估计有较大的变化；
一旦检测到故障，系统将切换回初始化阶段。一旦单目VIO被成功初始化，将新建一个独立的位姿图段。
VII. 重定位
我们的滑动窗口和边缘化方案限制了计算的复杂性，但也给系统带来了累积漂移。更确切地说，漂移发生在全局三维位置(x，y，z)和围绕重力方向的旋转(偏航)。为了消除漂移，提出了一种与单目VIO无缝集成的紧耦合重定位模块.重定位过程从一个循环检测模块开始，该模块标识已经访问过的地方。然后建立回环检测候选帧和当前帧之间的特征点级连接。这些特征对应，紧耦合到单目VIO模块中，从而得到无漂移状态估计，并且计算开销最小。多个特征的多重观测直接用于重定位，从而提高了定位的精度和状态估计的平滑性。图9(a)示出了重新定位过程的图形说明。
A. 闭环检测
我们利用一种最先进的词袋位置识别方法DBoW2，来进行闭环检测.除了用于单目VIO的角点特征，另外500个角点被检测并由BRIEF描述符描述。额外的角点特征用于在回路检测中实现更好的召回率。描述符被视为用于查询可视化数据库的可视单词。DBoW 2在时间和几何一致性检查后返回回环检测候选项。我们保留所有用于特征检索的BRIEF描述符，丢弃原始图像以减少内存消耗。
注意到，我们的单目VIO能够观测到滚动和俯仰角。因此，我们不需要依赖旋转不变的特性，例如ORB SLAM中使用的ORB特性。
B. 特征检索
当检测到回环时，通过检索特征对应性建立本地滑动窗口与回环候选点之间的连接。通过BRIEF描述符匹配找到对应关系。直接描述符匹配可能会导致大量异常值。为此，我们使用两步几何外点剔除，如图10所示。
*2D-2D：RANSAC的基础矩阵检验.我们利用当前图像中检索到的特征的二维观测和回环候选图像进行基础矩阵检验。
*3D-2D:RANSAC的PNP检验。基于已知特征点在局部滑动窗口中的三维位置，以及在闭环候选图像中的二维观测，进行PNP检验。
当内点数目超过一定阈值时，我们将该回环候选视为正确的循环检测并执行重新定位。
C. 紧耦合重定位
重定位过程有效地使单目VIO维护的当前滑动窗口与过去的位姿图保持一致。在重新定位过程中，我们将所有闭环帧的姿态作为常量，利用所有IMU测量值、局部视觉测量和从回环中提取特征对应值，共同优化滑动窗口。我们可以轻松地为回环帧v所观察到的检索特征，编写视觉测量模型，使其与VIO中的视觉测量相同，如(25)所示。唯一的区别是，从位姿图或直接从上一个里程计的输出(如果这是第一次重定位)获得的闭环帧的姿态被视为常数。为此，我们可以在(22)中稍微修改非线性代价函数，增加循环项：
其中L是闭环帧中检索特征的观测集。(l,v)是指在回环帧v中观察到的第l个特征。注意，虽然代价函数与(22)略有不同，但待解状态的维数保持不变，因为闭环帧的构成被视为常数。当用当前滑动窗口建立多个回环时，我们使用来自所有帧的所有闭环特征对应同时进行优化。这就为重新定位提供了多视角约束，从而提高了定位的精度和平滑性。请注意，过去的姿态和闭环帧的全局优化发生在重新定位之后，将在第VIII中讨论。
参考：
1.https://blog.csdn.nt/weixin_42905141/article/details/102782800