（算法分析）Relocalization, Global Optimization and Map Merging for Monocular Visual-Inertial SLAM

IV. Algorithm

A. Visual-inertial Odometry

在局部BA中优化的量有：
几个关键帧的位姿，速度，IMU的偏差，特征点的深度。

仅一些关键帧暂时被放在局部窗口中。之前的关键帧都被丢弃在窗之外，为了能够约束计算的复杂度。

按照文章所说，那么这幅图就代表了一个滑动窗的内容，其中 x₀ ,x₁ ,x₂ ,x₃ ,x₄ 是指关键帧的位置。f₀,f₁代表关键帧的特征点。

第k个IMU的状态包括位置变量P_bk^w,速度变量V_bk^w，方向变量q_bk^w,这些变量指的是世界坐标系下的变量。b_a,b_g是IMU的角速度和加速度的偏差。x_c^b指的是从相机坐标系到像素坐标系的转换（个人理解，可能有误）。估计就是一个最小二乘的问题：
这个最小二乘问题就是关于一个局部的BA过程，具体的可以参见视觉SLAM十四讲的具体讲解，这里不再细说。
最终的结论就是，在进行优化的时候，那么只是对当前窗口内的连续帧进行优化，而没有顾及到窗口之外的帧，那么只能达到局部的精确性，但是在长期过程中漂移的问题不可避免。

B. Loop Detection(回环检测)
重定位的问题实质就是，能够识别已经参观过的地方的问题。本次实验是使用了用于回环检测的词袋的方法。对于每一个关键帧，检测500个FAST关键点，并用BRIEF描述子进行描述（这里其实就是指的ORB特征）。把描述子转换成视觉向量用来查询视觉数据。从词袋中找到最佳的回环候选。描述子也用来特征检索。原始的图像将会被丢掉，这样减少存储的内容。
1）特征检索：
在回环检测之后，我们在当前帧和回环帧之间建立特征水平上的联系。特征匹配是由BRIEF描述子匹配完成的。通过寻找最小的汉明距离来选择匹配对。直接描述子匹配会造成很多的异常值，正如图4所示

为了去除异常值和验证回环检测，我们实行了两步的几何异常值重映射处理。
1）2D-2D: 在匹配对的2D观测中，使用随机采样一致性完成基础矩阵的测试
2）3D-2D：实行PnP的测试

（注：这块这两步属于直接翻译，感觉不是太懂，大家有什么认识多多发言，共同交流）

当我们找到最够多的正常值，我们就将该结果作为正确的回环检测.

C. Tightly-Coupled Relocalization(紧耦合重定位)

关于这块重定位中，有一些疑问，希望大家给出一些建议： 1.这里的回环帧到底是指的是什么？ 2.关于把它看成一个固定的量怎么解释？

在计算位姿的时候没有使用在两帧之间估计的方法，而是通过在局部窗口内联合优化的办法（其实说的就是BA优化）。回环检测帧被当做一个额外的帧固定在滑动窗口中，通过检索特征建立连接。 我们使用v代表回环帧。在重定位期间，我们讲之前关于V的姿态估计作为一个常量。回环帧通过检索特征建立与局部窗口的联系。我们使用所有的IMU测量，局部视觉测量来联合优化滑动窗口，并从回环中检索特征对应。通过回环帧v，我们能够很容易对检索的观察的特征写出相同的视觉测量模型。在（2）中的非线性代价函数只需要添加回环帧的视觉重投影误差项。 重定位的过程让局部得到优化，实现零漂移。

D.Global Pose Graph Optimization（全局位姿图优化）
要达到全局一致性，实行轻量级的4自由度的位姿优化。
1）4个累积漂移的方向：
我们可以通过重力确定水平面，那就意味着我们可以随时观测绝对的滚动角和俯仰角。因此，滚动角和俯仰角在世界坐标系中是绝对值，而x,y,z以及偏航角由于参考帧的存在得到的是相对的值。这样就意味着累积漂移存在四个自由度。为了充分利用有效消息，有效的校正漂移，我们对无漂移的滚动角和俯仰角进行了修正，只对四自由度的姿态图进行了优化。
2）添加关键帧到位姿图中：
每个关键帧被局部窗口边缘化出来之后就被添加到位姿图中。一个关键帧在位姿图中作为一个顶点。每个顶点连接其他的顶点通过两个边，一个边是顺序边，另一个是循环边。正如第7幅图所示。
sequential edge：
用于关键帧与之前一些关键帧的连接。这个边主要代表两个关键帧之间相对的转换。
Loop edge:
当回环发生的时候，关键帧将会与回环帧通过回环边进行连接。
3）4自由度的位姿图优化：
位姿图优化的主要想法是通过调整顶点的位姿来达到使配置尽可能的与边匹配。在我们的框架中，只调整3D位置和偏航角，把滚动角和俯仰角当做常量来处理。我们定义了边缘残差
4）map merging：
位姿图不仅能够优化当前地图，而且还能够将当前地图与之前建好的地图进行融合。我们在所有的测量都是相对测量，只需固定好第一个顶点即可。

文章中给出这样一幅图，黄色的达标之前建立好的地图，而蓝色代表的是当前建立好的地图，这个时候检测到一些回环信息，有一些关键帧和之前建立的图的关键帧有了匹配关系，那么这个时候两个图进行融合，就得到后边融合后的图。5）pose graph management
当运行距离越来越长的时候，位姿图的尺寸就会变得没有边界，限制系统的实时性能。因此，我们通过降采样的方式来将位姿图数据限制到一定的范围。具体的方法是：所有的回路关键帧约束将会被保留，而其他的关键帧离得近的或者说有非常相似的方向的将会被移除。

E. Map reuse