以下内容参考计算机视觉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 。