淦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
    点赞
  • 27
    收藏
    觉得还不错? 一键收藏
  • 1
    评论
### 回答1: ORB-SLAM2是一款基于特征点的SLAM算法,可以在实时运行中实现稠密地图的构建和定位。ORB-SLAM2的源代码解析v1.2 pdf为ORB-SLAM2算法的源代码进行详细讲解的文档。 这个文档详细介绍了ORB-SLAM2算法的各个模块以及其组成部分,包括图像预处理、特征点提取、视觉里程计、回环检测、地图构建和定位等。对于每个模块,文档都进行了详细的讲解,并展示了一些代码实现和示例。 其中,ORB特征点的提取是ORB-SLAM2的一个重要特点。文档详细介绍了ORB特征点的提取与描述,并对其进行了性能优化。在视觉里程计中,文档详细介绍了基于ORB-SLAM2的相机位姿估计算法,并同时对其进行了实验验证。 此外,orb-slam2源码解析 v1.2 pdf还对ORB-SLAM2的一些扩展进行了介绍,如RGBD-SLAM、半稠密点云地图构建、直接法视觉里程计等等。 总之,ORB-SLAM2是一个非常强大的SLAM算法,通过对orb-slam2源码解析 v1.2 pdf的学习,可以更好地理解其原理和实现,也为进一步研究和应用提供了参考。 ### 回答2: ORB-SLAM2是一种基于单目相机的实时稠密SLAM系统,被广泛应用于机器人、自动驾驶、增强现实等领域。ORB-SLAM2源码解析v1.2 pdf是一份PDF文档,对ORB-SLAM2源代码进行了详细的解析和分析。 该文档分为多个章节,首先介绍了ORB-SLAM2的概述和背景,包括SLAM系统的基本原理和ORB特征点的提取与匹配算法。接着,文档对ORB-SLAM2的系统框架、流程和算法进行了详细介绍,主要包括定位、建图、闭环检测和重定位等核心模块的实现细节。 文档还对ORB-SLAM2的实验结果和性能进行了评估和分析,包括系统的重定位精度、建图质量、算法复杂度和实时性等指标。同时,文档还针对ORB-SLAM2的应用场景进行了讨论和展望,包括基于ORB-SLAM2的三维重建、SLAM与深度学习的融合等前沿研究方向。 总之,ORB-SLAM2源码解析v1.2 pdf是一份非常有价值的文档,对想要深入了解和应用ORB-SLAM2的研究者和开发者有很大的帮助和启发作用。它不仅详细介绍了ORB-SLAM2的理论基础和实现细节,还从实验和应用角度对其性能和前景进行了评估和展望,为相关领域的技术人员提供了重要的参考和指导。 ### 回答3: ORB-SLAM2是一种基于单目或双目相机的实时视觉SLAM系统,可以在无GPS信号的情况下,通过对相机的位置和姿态的估计,构建3D环境地图。 ORB-SLAM2源码解析 v1.2 PDF是一份解析ORB-SLAM2源码的文档,其中包含了ORB-SLAM2的基本架构、算法实现以及关键代码的详细解释。通过学习该文档,可以深入了解ORB-SLAM2的原理和实现方法,从而更好地应用该系统进行SLAM操作。 该文档主要包括以下几个部分: 1.ORB-SLAM2的系统结构:介绍ORB-SLAM2的整体结构和各模块之间的关系。 2.特征提取与匹配:详细介绍ORB特征的提取和匹配算法,包括ORB算法原理、特征对齐和描述符生成等。 3.全局BA和回环检测:讲解ORB-SLAM2的全局优化和回环检测方法,其中包括BA算法流程、优化目标函数、回环检测的实现等。 4.实时定位:探讨如何实现ORB-SLAM2的实时运动估计和位姿估计,包括相机位姿估计、尺度恢复和点云重建等内容。 除此之外,该文档还对ORB-SLAM2的一些高级技术进行了讲解,如基于深度学习的特征提取、基于语义信息的地图构建等。 总之,该文档是一份非常有价值的ORB-SLAM2源码解析资料,对于深入了解ORB-SLAM2的实现原理、优化方法和应用场景,具有重要的参考意义。

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

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

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值