本篇内容涉及VINS-Mono第五至第气节内容，在原论文中为（p4-p9）包括：
第五节：VINS-Mono的鲁棒性初始化过程
第六节：紧耦合、自校准、基于非线性优化的单目VIO
第七节：紧耦合的重定位
单目惯性系统是高度非线性系统，系统的正常运行高度依赖准确的初始化过程。
初始化过程由松耦合下的imu预积分辅助纯视觉结构还原过程。
使用滑动窗口中纯视觉的sfm方法。
初始化过程从一个sfm过程开始，还原一个缺乏尺度信息的，由相机姿态和特征点位置构成的图模型。
1）获取视觉计算所需的大位移帧
因为图像是源源不断地传进来的，开始是不断对最新帧进行特征追踪，计算最新帧与之前所有帧特征的关系。如果最新帧与之前某一帧存在如下关系：对应追踪的特征点不少于30个，特征点间对应位移大于20像素（这里在代码里是平均位移），那么就算找到合适的大位移帧。
利用五点法，计算这两帧之间的位移和旋转（缺乏尺度信息的）。然后任意设置一个尺度，用三角测量方法计算出这两帧中特征点的三维坐标。
基于已三角化的特征点，使用pnp法计算整个滑动窗口中所有帧（一般为10帧）的姿态。
最后，使用完整BA(既包含姿态，又包含特征点)来最小化特征点的重投影误差。
因为还没有关于世界坐标系的信息，这里把相机的第一帧作为sfm过程的参考帧，也就是参考坐标系。
所有帧的姿态和特征点坐标结果都是相对于相机的第一帧（第一帧也就是相机坐标系）。借助于相机和imu的外参
p
c
b
p^b_c
pcb和
q
c
b
q^b_c
qcb，可以把相机坐标系下位姿转换到imu坐标系下：
使用imu预积分值，使纯视觉中的尺度信息得以和imu测量相匹配。
A)陀螺仪偏置校准
从纯视觉sfm中可以获得滑动窗口中所有帧得姿态，而从imu预积分中也可也获得连续两帧的相对旋转。最小化如下代价函数：
相对旋转的更新使用第二个函数，可以得到一个初始化的偏置的校正值，使用新的陀螺仪偏置值，对滑动窗口内imu预积分量进行重新积分。
B)速度、重力向量、尺度的初始化
陀螺仪偏置初始化后是对导航必要信息的速度、重力向量、尺度的初始化。
对imu预积分量有如下两个公式
该公式和尺度，重力向量、尺度信息相关。
通过最小化上面这个代价函数，可以获得滑动窗口中每一帧的速度。相对于相机坐标系的重力向量和尺度值。
C)重力向量优化
重力向量的更新策略是将
g
g
g向量使用
g
(
g
^
+
δ
g
)
g(\hat{g}+\delta g)
g(g^+δg)来代替，其中
g
^
\hat{g}
g^是测量出的平均重力向量单位方向，
δ
g
\delta g
δg是在正切球空间里的微小扰动。不断迭代这个值，直到
g
g
g向量收敛。
D)初始化的完成
在对重力向量完成优化后，就可以通过把重力向量旋转到z轴来获得世界坐标系和相机坐标系的旋转关系。然后把所有相机坐标系的量旋转到世界坐标系下，此时imu坐标系的量同样也旋转到世界坐标系下。在sfm中的位移关系会缩放到米单位下。到这里，初始化完成，所有米制数据会提供给接下来的紧耦合单目VIO系统。
完成初始化后，就进入基于滑动窗口的紧耦合单目VIO系统。
第一个是所有状态的集合。第二个是imu的状态向量，第三个是相机与imu的外参。
这里我就不解释每个字母含义了，论文里都介绍的很详细。对视觉特征的参数化使用的是逆深度形式。
这里使用的是视觉惯性BA优化方式，使得先验误差和所有测量值残差的马氏距离最小，从而获得最大后验估计。
第一项为先验误差，第二项为imu测量残差，第三项为摄像机测量残差。对视觉测量使用Huber函数以减小特征误匹配带来的影响。
在滑动窗口中连续两帧imu测量残差为：
基于状态量计算得到的相对位移，速度，旋转和imu预积分量的差值。
与传统方法中将特征点投影到单位平面上计算残差不一样的是，VINS-Mono把像素误差投影到一个单位圆上。即相机残差定义在如下的单位圆：
其原因是这种残差定义方式几乎适用于所有类型的相机，包括广角、鱼眼或全向相机。
将特征点第一次观测的坐标，通过先转到imu局部坐标，然后imu局部坐标转换到世界坐标，imu世界坐标再转换到局部坐标，然后投影到当前相机坐标系，当前坐标系下投影到单位圆，就形成了路标的单位向量。
观测点单位向量与路标单位向量相减，向量差在单位圆上一切平面分解，就得到了两个残差值，即视觉的几何误差残差。
为了限制VIO非线性优化的复杂度，使用了边缘化。对imu状态量和视觉特征选择性的进行边缘化，同时把边缘化的状态量转为先验。
如果第二新的帧是关键帧，则把滑动窗口中最老的关键帧边缘化掉（对应于第一种情况），如果第二新的帧不是关键帧，则把该帧对应的特征点舍弃掉，但保留其imu测量值。
为保证稀疏性，非关键帧的测量值不是所有的都会被边缘化掉。（这句话我认为是对第二新的帧不是关键帧情况的解释）。
边缘化方案只是对窗口中的关键帧。它确保三角化特征点过程中计算特征点具有足够的视差，并且加速度计能得到充分的激励。
边缘化使用舒尔补。
基于所有与移除状态的测量值，VINS-Mono建立了一个新的先验方法。新的先验被添加到现有的先验中。（没懂啥意思）
我们注意到边缘化导致线性化点的早期固定，这可能导致次优估计结果。然而，由于VIO的小漂移是可以接受的，我们认为由边缘化造成的负面影响并不重要。
对于计算能力有限的平台，比如手机，紧耦合单目VIO由于非线性优化沉重的计算量，在这些平台是难以达到相机实时帧率更新的。为此，除了完全优化之外，VINS-Mono还采用了轻量级运动视觉惯性优化，将状态估计频率提高到相机率(30Hz)。
纯运动视觉惯性优化的代价函数与单眼视觉优化的代价函数相同。但不同的是，纯运动状态估计并没有优化滑动窗口中的所有状态，而只是优化了固定数量的最新IMU状态的姿态和速度。将特征深度、外部参数、偏差和不想优化的旧IMU状态视为常数值。
但这确实使用所有的视觉和惯性测量来进行仅限运动的优化。（只不过有些值只是约束，不是优化量）
这种方法比单帧的pnp方法结果更为平滑。
这种方法把原来在嵌入式设备上需要50毫秒的全优化计算降低为5毫秒，使得在低算力设备上能得到相机帧率的姿态估计，特别适合在需要AR或者VR的手机上进行运行。
IMU测量的速度比视觉测量的速度要高得多。虽然我们的VIO频率受图像捕获频率的限制，但我们仍然可以直接传播最近的IMU测量的VIO估计，以实现IMU速率性能。高频状态估计可以作为回环状态反馈。第九-c节介绍了一个利用这种imu速率状态估计的自主飞行实验。
滑动窗口和边缘化可以控制计算复杂度，但同时也给系统带来了累计漂移。
VINS-Mono使用了一种紧耦合的重定位方式来消除累计误差。
重定位从一个回环检测模块开始，以分辨什么地方之前来过。
当前帧和回环帧在特征程度上的约束得以建立。
这些对应特征的约束被集成到了单目VIO系统中，优化至最小以得到无漂移的状态估计。
多特征的多次观测约束直接在重定位中使用，可以得到更高精度和平滑度的状态估计。
使用词袋方法DBoW2进行循环检测。
为了进行回环检测，从图像中抽取了500个特征角点并计算其BRIEF描述子。
额外的角点抽取是为了提高召回率。
描述子作为视觉数据库的检索词。
DBoW2在时间和几何一致性检查后返回候选闭环。数据库中保留所有简短的描述符以进行特征检索，但丢弃原始图像以减少内存消耗。
当检测到一个回环时，需要通过特征对应关系来求解回环帧间关系。特征对应关系通过描述符进行匹配。但特征匹配可能会导致误匹配的问题，对该问题通过如下方式来解决。
A)2D-2D：使用RANSAC的基本矩阵检验。在当前图像和环闭合候选图像中检索到的特征的二维坐标来进行基本矩阵测试，去除离群点。
B)3D-2D:使用RANSAC进行的PnP测试。基于已知的局部滑动窗口中特征在局部滑动窗口中的三维位置，以及在回环帧图像中的二维特征点坐标，可以进行PnP测试。
在除去异常值后，将该候选对象视为正确的循环检测，并执行重新定位。
重定位高效地将滑动窗口的帧与过去的姿态关联到了一起。
在重定位时，把回环的帧（滑动窗口中的帧）设为固定帧。
回环帧观察到的特征的视觉测量模型，与VIO中的视觉测量模型相同，其残差的计算方法类似。
唯一的区别是，回环帧的姿态取自姿态图（见第八节，即姿态图合并的情况)，或直接来自过去的VIO里程计的输出(如果这是第一次重新定位），并被视为一个常数。
于是，可以很轻易把回环帧的残差添加进优化中：
其中，回环残差的变量是在回环帧中检索到的所有特征的集合。
请注意，尽管待求解函数与（14）略有不同，但要求解的状态的维数保持相同，因为回环帧的姿态被认为是常数。
当滑动窗口中发生了多个回环时，同时使用所有回环帧的所检索的所有对应特征进行优化。
这为重新定位提供了多视图约束，从而获得了更高的精度和更好的平滑度。重新定位后保持一致性的全局优化将在第八节中进行讨论。
