SLAM：ORB-SLAM 位姿优化描述

只知道算法描述和代码，而不知道原理是比较扯的事情，还是把原理转载一下。

原文链接： http://www.cnblogs.com/luyb/p/5447497.html

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矩阵）；

4. 4呢？

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为一个可能的闭环。通过(u 4,2 ,v 4,2 ) (u4,2,v4,2)和(u 4,6 ,v 4,6 ) (u4,6,v4,6)之间的投影误差来优化S 6,2  S6,2。

2.5 Sim3上的位姿优化

单目SLAM一般都会发生尺度（scale）漂移，因此Sim3上的优化是必要的。相对于SE3，Sim3的自由度要多一个，而且优化的目标是矫正尺度因子，因此优化并没有加入更多的变量（如三维点）。

作者在检测到闭环时在Sim3上对所有的位姿进行一次优化。定义Sim3上的残差如下：

e i,j =log Sim3 (S ij S jw S −1 iw ) ei,j=logSim3(SijSjwSiw−1)

其中S iw  Siw的初值是尺度为1的Pos
i相对于世界坐标系的变换矩阵。S i,j  Si,j为Pos
i和Pos j之间的（Sim3优化之前的）相对位姿矩阵，表示S iw  Siw和S jw  Sjw之间的测量（measurement）。此处相当于认为局部的相对位姿是准确的，而全局位姿有累计误差，是不准确的。

三、小结

个人理解，单目SLAM中的优化需要更多技巧，要有明确的优化目标，要仔细权衡其中的参数选择、自由度、速度和稳定性。

该系列的其它文章：

ORB－SLAM（一）简介

ORB－SLAM（二）性能

ORB－SLAM（三）初始化

ORB－SLAM（四）追踪

ORB－SLAM（六）回环检测