淦ORB-SLAM2源码 09--SIM(3)算法

以下内容参考计算机视觉life公众号

SIM(3)

什么是SIM3?

Sim3 (Similarity Transformation)的提出就是为了解决两个坐标系之间的相似变换问题，只要我们能得到3对匹配好的不共线的三维点在两个坐标系下的坐标，我们就能解出Sim3相似变换。这个也是Sim3中数字3的来源。计算Sim3 实际就是计算这三个参数：旋转 、平移 、尺度因子 。

为什么三对不共线点就可以求解？

我们来感性的理解一下，我们有三对匹配的不共线三维点可以构成两个三角形。我们根据三角形各自的法向量可以得到他们之间的旋转，通过相似三角形面积能够得到尺度，用前面得到的旋转和尺度可以把两个三角形平行放置，通过计算距离可以得到平移.

ORB-SLAM2系统在LoopClosing线程中，当检测到闭环候选帧的时候，就需要对当前关键帧和对应的闭环候选帧之间计算其变换关系。这时需要用当前关键帧和其对应的闭环候选帧进行sim3求解，这里的sim3求解是对当前关键帧和闭环候选帧之间匹配的MapPoint进行sim3求解。通过sim3变换解出当前关键帧和闭环候选帧的匹配MapPoint之间的旋转矩阵R、平移向量t、尺度变换s，也就得到了当前关键帧到闭环关键帧之间的sim3变换gScm。使用这个sim3变换gScm乘上闭环关键帧的sim3位姿gSmw，mg2oScw=gScm*gSmw的乘积mg2oScw就是当前关键帧的sim3位姿，之后在闭环校正中就可以使用这个sim3位姿转换为SE3位姿后对当前关键帧进行位姿校正（当然也要对关键帧对应的MapPoints以及其共视的关键帧进行校正）.

3对点计算旋转可以吗?

假设坐标系1下有三个不共线三维点 P1,P2 ,P3 ，他们分别和坐标系2下的三个不共线三维点Q1 ,Q2 ,Q3一一匹配。

首先，我们根据坐标系1下的三个不共线三维点来构造一个新的坐标系。
沿着 x轴上的单位向量

沿着 y轴的单位向量

沿着 z轴的单位向量

同理，我们对于坐标系2下的Q1 ,Q2 ,Q3 也可以得到沿着3个坐标轴的单位向量,我们现在要 计算 坐标系1 到坐标系2的旋转，记坐标系单位向量构成的基底矩阵为

假设坐标系1下有一个向量V1 ，它在坐标系2下记为 V2，因为向量本身没有变化，根据坐标系定义有

那么从坐标系1到坐标系2的旋转就是

看起来好像没什么问题，但是实际上我们不会这样使用，因为存在如下问题：
1、这个旋转的结果和选择点的顺序关系密切，我们分别让不同的点做坐标系原点，得到的结果不同。
2、这种情况不适用于匹配点大于3个的情况。
因此实际上我们不会使用以上方法。我们通常能够拿到远大于3个的三维匹配点对，我们会使用最小二乘法来得到更稳定、更精确的结果。

计算SIM3的平移

假设我们得到了 n>3组匹配的三维点，分别记为Pi和Qi，我们的目的是对于
每对匹配点，找到如下的变换关系：

其中 s是尺度因子， R是旋转， t是平移。

如果数据是没有任何噪音的理想数据，理论上我们可以找到满足上述关系的尺度因子、旋转和平移。但
实际上数据是不可避免会有噪音和误差，所以我们转换思路，定义一个误差ei ，我们的目的就是寻找合适的尺度因子、旋转和平移，使得它在所有数据上的误差最小

在开始求解之前，我们先定义两个三维点集合中所有三维点的均值（或者称为质心、重心）

我们对每个三维点 Pi，Qi分别减去均值，得到去中心化后的坐标 ，则有：

下面开始推导我们的误差方程：

为了推导不显得那样臃肿，其中我们简记

根据前面的推导可得等式右边中间项

这样我们前面的误差方程可以化简为：

等式右边的两项都是大于等于0的平方项，并且只有第二项里的t0 和我们要求的t平移 有关，所以当t0=0时，我们可以得到平移的最优解

也就是说我们知道了旋转 R和尺度s 就能根据三维点均值做差得到平移 t了。注意这里平移的方向是Pi->Qi

计算SIM3的尺度因子

我们的误差函数也可以进一步简化为：

由于向量的模长不受旋转的影响，所以
为了后续更加清晰的表示，我们用简单的符号代替上述式子里的部分内容，所以有

由于 R是已知的，我们很容易看出来上面是一个以s 为自变量的一元二次方程，要使得该方程误差最小，我们可以得到此时尺度 s的取值：

但是，到这里还存在一个问题，我们对P,Q 做个调换后得到：

我们看到尺度并不具备对称性，也就是从Pi->Qi 得到的尺度并不等于从 Qi->Pi得到的尺度的倒数。 这也说明我们前面方法得到的尺度并不稳定。所以需要重新构造误差函数，使得我们得到的尺度是对称的、稳定的.
论文里的构造方式是：

上面等式右边第一项只和尺度s 有关的平方项，第二项和s 无关，但和旋转R 有关，因此令第一项为0，我们就能得到最佳的尺度。

同时，第二项里的Sp和SQ 都是平方项， 所以令第二项里的 D最大，可以使得剩下的误差函数最小。

计算SIM3的旋转

下面我们考虑用四元数来代替矩阵来表达旋转。
假设空间三维点 P=[x,y,z]，用一个虚四元数来表示为p=[0,x,y,z]T 。
我们现在的的代价函数可以做如下变换：

其中：

定义

引入M 是为了方便用其元素来表示N ，我们将上面的结果代入整理，则有：

然后我们对 N进行特征值分解，求得最大特征值对应的特征向量就是待求的用四元数表示的旋转，注意这里旋转的方向是Pi->Qi 。
至此，我们就得到Sim3 的三个参数：旋转 R、平移t，尺度因子s 。

计算 Sim3 的步骤总结

1、先计算旋转R 。
具体来说，先构建 M 矩阵。

然后得到矩阵N，我们再对 N进行特征值分解，求得最大特征值对应的特征向量就是待求的用四元数表示的旋转，注意这里旋转的方向是Pi->Qi。
2、根据上面计算的旋转R 来计算尺度s 。
具体来说，可以使用以下两种方法来计算，第一种是具有对称性的尺度（推荐）

第二种是不具有对称性的尺度（ORBSLAM使用）

3、根据旋转R 和尺度s 计算平移t 。