淦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 。
在这里插入图片描述

  • 1
    点赞
  • 26
    收藏
    觉得还不错? 一键收藏
  • 1
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值