ORB-SLAM（五）优化
ORB-SLAM作为单目SLAM，其精度很大程度上决定于帧与帧之间的位姿优化的是否准确。因此优化（optimization）在ORB-SLAM里面扮演了很重要的角色。这一小节探讨一下ORB-SLAM里用到的优化。
ORB-SLAM选用g2o作为图优化的方法，关于g2o可以参考http://www.cnblogs.com/gaoxiang12/p/5304272.html。
一、为什么要优化
因为摄像机标定（camera calibration）和追踪（tracking）的精度不够。摄像机标定的误差会体现在重建中（比如三角法重建时），而追踪的误差则会体现在不同关键帧之间的位姿中，和重建中（单目）。误差的不断累积会导致后面帧的位姿离实际位姿越来越远，最终会限制系统整体的精度。
1.1 摄像机标定
单目SLAM文献中一般假设摄像机标定的结果是准确的，并不考虑这个因素带来的误差（大概因为很多时候跑标准的数据集，认为摄像机标定的误差是相似的）。然而对于一个产品，不同类型的传感器对应的标定误差并不相同，甚至有可能差异很大。因此，如果要评估整个系统的精度，这方面的误差必须要考虑进去。
1.2 追踪
无论在单目、双目还是RGBD中，追踪得到的位姿都是有误差的。单目SLAM中，如果两帧之间有足够的对应点，那么既可以直接得到两帧之间的位姿（像初始化中那样），也可以通过求解一个优化问题得到（如solvePnP）。由于单目中尺度的不确定性，还会引入尺度的误差。由于tracking得到的总是相对位姿，前面某一帧的误差会一直传递到后面去，导致tracking到最后位姿误差有可能非常大。为了提高tracking的精度，可以1. 在局部和全局优化位姿；2. 利用闭环检测（loop closure）来优化位姿。
二、如何优化
2.1 优化的目标函数在SLAM问题中，常见的几种约束条件为： 1. 三维点到二维特征的映射关系（通过投影矩阵）；2. 位姿和位姿之间的变换关系（通过三维刚体变换）；3. 二维特征到二维特征的匹配关系（通过F矩阵）；5. 其它关系（比如单目中有相似变换关系）。如果我们能够知道其中的某些关系是准确的，那么可以在g2o中定义这样的关系及其对应的残差，通过不断迭代优化位姿来逐步减小残差和，从而达到优化位姿的目标。
2.2 局部优化
当新的关键帧加入到convisibility graph时，作者在关键帧附近进行一次局部优化，如下图所示。Pos3是新加入的关键帧，其初始估计位姿已经得到。此时，Pos2是和Pos3相连的关键帧，X2是Pos3看到的三维点，X1是Pos2看到的三维点，这些都属于局部信息，共同参与Bundle Adjustment。同时，Pos1也可以看到X1，但它和Pos3没有直接的联系，属于Pos3关联的局部信息，参与Bundle Adjustment，但取值保持不变。Pos0和X0不参与Bundle Adjustment。
因此，参与优化的是下图中红色椭圆圈出的部分，其中红色代表取值会被优化，灰色代表取值保持不变。(u,v)是X在Pos下的二维投影点，即X在Pos下的测量（measurement）。优化的目标是让投影误差最小。
2.3 全局优化
在全局优化中，所有的关键帧（除了第一帧）和三维点都参与优化。
2.4 闭环处的Sim3位姿优化
当检测到闭环时，闭环连接的两个关键帧的位姿需要通过Sim3优化（以使得其尺度一致）。优化求解两帧之间的相似变换矩阵，使得二维对应点（feature）的投影误差最小。
如下图所示，Pos6和Pos2为一个可能的闭环。通过 (u4,2,v4,2) (u4,2,v4,2)和 (u4,6,v4,6) (u4,6,v4,6)之间的投影误差来优化 S6,2 S6,2。
2.5 Sim3上的位姿优化
单目SLAM一般都会发生尺度（scale）漂移，因此Sim3上的优化是必要的。相对于SE3，Sim3的自由度要多一个，而且优化的目标是矫正尺度因子，因此优化并没有加入更多的变量（如三维点）。
作者在检测到闭环时在Sim3上对所有的位姿进行一次优化。定义Sim3上的残差如下：
ei,j=logSim3(SijSjwS−1iw) ei,j=logSim3(SijSjwSiw−1)
其中 Siw Siw的初值是尺度为1的Pos i相对于世界坐标系的变换矩阵。 Si,j Si,j为Pos i和Pos j之间的（Sim3优化之前的）相对位姿矩阵，表示 Siw Siw和 Sjw Sjw之间的测量（measurement）。此处相当于认为局部的相对位姿是准确的，而全局位姿有累计误差，是不准确的。
三、小结
个人理解，单目SLAM中的优化需要更多技巧，要有明确的优化目标，要仔细权衡其中的参数选择、自由度、速度和稳定性。
上一篇提到，无论在单目、双目还是RGBD中，追踪得到的位姿都是有误差的。随着路径的不断延伸，前面帧的误差会一直传递到后面去，导致最后一帧的位姿在世界坐标系里的误差有可能非常大。除了利用优化方法在局部和全局调整位姿，也可以利用回环检测（loop closure）来优化位姿。
这件事情就好比一个人走在陌生的城市里，一开始还能分清东南西北，但随着在小街小巷转来转去，已经不知道自己在什么地方了。通过认真辨识周边环境，他可以建立起局部的地图信息（局部优化）。再回忆以前走过的路径，他可以纠正一些以前的地图信息（全局优化）。然而他还是不敢确定自己在城市的精确方位。直到他看到了一个之前路过的地方，就会恍然大悟，“噢！原来我回到了这个地方。”此时，将这个信息传递回整个地图，就可以得到相当准确的地图信息。这就是回环检测。
因此，回环检测在大尺度地图构建上是一个非常有用的方法。回环检测可以从二维图像出发，也可以从三维点云出发。目前大家更推荐基于二维图像的方法。
DBoW2
基于二维图像的方法本质上是一个场景识别的问题。我没有深入研究过，因此直接介绍一下ORB－SLAM中用到的DBoW2的方法。
BoW（bag of words，词袋模型），可以理解为一个以特征描述作为元素的词典。如果是ORB特征，那就是ORB词典；如果是SIFT特征，那就是SIFT词典。词典可以从图像数据集中训练出来。下面举一个简单的例子。假如我们有一个一万幅图像的数据集，并认为它基本上涵盖了我们所面临的场景。
从每幅图像中提取特征点和特征描述；特征描述一般是一个多维向量，因此可以计算两个特征描述之间的距离；
将这些特征描述进行聚类（比如k－means），类别的个数就是词典的单词数，比如1000；也可以用Beyes、SVM等；
DBoW2将这个词典组织成树的形式，方便搜索。
在实际应用中，每一幅图像都在词典中搜索其最近邻的单词，并在该单词下留下标记。如果A、B两幅图像定位到同一个单词时，说明这两幅图像有可能有相似的特征点。当A、B有一定量的相似点时，可以认为这两幅图像之间存在着一定的相似性。
ORB－SLAM的作者修改了DBoW2，输出一系列候选图像（candidate）而不是一幅最相似的图像。
基于BoW的方法有一些非常好的优势：
词典可以离线训练。在实时应用中能离线的东西越多越好。作者提供了通过大量数据训练出来的BRIEF和SIFT的词典。
搜索速度飞快。小尺寸的图像可以在毫秒级别完成。作者提供了正向（direct index）和反向（inverse index）两种辅助指标。反向指标在节点（单词）上储存到达这个节点的图像特征的权重信息和图像编号，因此可用于快速寻找相似图像。正向指标则储存每幅图像上的特征以及其对应的节点在词典树上的某一层父节点的位置，因此可用于快速特征点匹配（只需要匹配该父节点下面的单词）。
很多slam应用本身就需要计算特征点和描述，因此可以用特征来搜索。
ORB－SLAM的作者还用词典的特性做快速的特征筛选，减少特征匹配需要的时间（特别是在大尺度上搜索特征时）。
当然它也有自己的劣势：
如果应用的场景比较特殊，请训练自己的词典。一般的词典会不太好用。
BoW一般不太考虑特征之间的几何关系（有人在做，但不清楚效果和计算量如何）还有几个comments：
如果应用本身不需要计算特征，那要考虑额外的计算时间。
推荐像ORB－SLAM一样，BoW只用来快速筛选图像，后续需要通过其它方法一一验证、严格验证。如果回环选错了，那，就跪了。
如果场景特征很少，或者重复的特征太多，比较难办。
在稠密点云重建中，如果场景本身有丰富的几何纹理，那么可以利用两帧之间（包括相邻位置）的三维点云匹配去验证回环。如果匹配的误差足够小，那么回环是比较准确的。
回环验证和Sim3优化
对每个候选的回环帧，作者先匹配其和当前帧上的特征点，然后用特征点对应的三维点去求解一个相似变换矩阵（RANSAC框架下）。如果某个回环帧对应的矩阵有足够多的内点，那去做Sim3优化。利用优化结果再去寻找更多的特征匹配，再做一遍优化。如果内点足够多，那么接受这个回环。
Sim3优化详见上一篇。
回环融合（fusion）
这里有一个隐含假设，即误差随着时间不断累积，相对而言，我们更信任之前的信息而不是当前的信息。这部分主要是把回环帧的信息融合到当前帧里面，包括匹配的特征点对应的三维信息（深度、尺度等），世界坐标系下的位姿（通过Sim3的结果转化过去）等等。融合也包括回环帧的邻域和当前帧的邻域。
全局优化
全局优化时，固定回环帧及其邻域，当前帧及其邻域，优化剩余帧在世界坐标系的位姿。详见上一篇。
预告：
这个系列写到这里就暂时告一段落啦。回头来看，前面几篇写的并不满意，尤其追踪那部分，将来争取重写一下。
接下来，本来想写关于LSD的文章，估计会暂缓一下，先写一两篇关于网格生成的文章。
