淦ORB-SLAM2源码 09--SIM(3)算法
SIM3
- SIM(3)
- 什么是SIM3?
- 3对点计算旋转可以吗?
- 计算SIM3的平移
- 计算SIM3的尺度因子
- 计算SIM3的旋转
- 计算 Sim3 的步骤总结
以下内容参考计算机视觉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 。
淦ORB-SLAM2源码 09--SIM(3)算法相关推荐
- ORB SLAM2源码解读(三):Frame类
文章目录 前言 构造函数 双目相机 RGBD相机 单目相机 ExtractORB:提取特征点 ComputeBoW:计算词袋数据 SetPose:设置相机外参 isInFrustum:判断一个MapP ...
- marlin源码gcode解析与算法插补执行框架
marlin源码gcode解析与算法插补执行框架
- kademlia java_死磕以太坊源码分析之Kademlia算法
死磕以太坊源码分析之Kademlia算法 KAD 算法概述 Kademlia是一种点对点分布式哈希表(DHT),它在容易出错的环境中也具有可证明的一致性和性能.使用一种基于异或指标的拓扑结构来路由查询 ...
- Linux源码中的mktime算法解析
Linux源码中的mktime算法解析 我们知道,从CMOS中读出来的系统时间并不是time_t类型,而是类似于struct tm那样,年月日时分秒是分开存储的. 那么,要把它转化为系统便于处理的ti ...
- Android系统源码学习-SIM卡(二)
随着手机的普及,大家从非智能机到智能机的转变,从没有操作系统的定制机到智能手机,但唯一没有变的是,手机中的SIM,今天我们就来谈谈手机中SIM卡相关的内容.在日常生活中,SIM卡就是一张很小的卡片,但 ...
- python算法实现源码_python 实现A_算法的示例代码
python 实现A_算法的示例代码 来源:中文源码网 浏览: 次 日期:2018年9月2日 [下载文档: python 实现A_算法的示例代码.txt ] (友情提示:右键点上行txt ...
- STL源码剖析 数值算法 copy 算法
copy复制操作,其操作通过使用assignment operator .针对使用trivial assignment operator的元素型别可以直接使用内存直接复制行为(使用C函数 memove ...
- [FFmpeg] 在 ffplay 源码中嵌入 YOLO 算法实现实时物体检测
源码安装 FFmpeg-4.3.1 源码安装 OpenCV-3.4.13 下载并研究 libtorch-yolov3 算法 尝试单独编译 ffplay 源码 将 yolov3 算法嵌入 ffplay ...
- armax函数 matlab 源码_【源码】Kmeans聚类算法(超快速、简洁的设计方法)
这是一种超快速MATLAB实现的kmeans聚类算法. This is a super duper fast implementation of the kmeans clustering algor ...
- C#语言实例源码系列-异或算法加密解密
专栏分享 点击跳转=>Unity3D特效百例 点击跳转=>案例项目实战源码 点击跳转=>游戏脚本-辅助自动化 点击跳转=>Android控件全解手册
最新文章
- Android 设计模式:(三)装饰者模式 —— 装饰对象
- 为什么基类的析构函数要声明成虚函数
- Linux 上与系统负载有关的命令
- 组数总和—leetcode39
- 集训队脱单大法:这是一道只能由学姐我自己出数据的水题(牛客竞赛)
- 用.NetCore 编译国产老牌PHP论坛DiscuzX ,世界上最好的语言从此属于.Net 的一员
- thinkphp mysql 更新_THINKPHP5修改数据库数据出现“缺少更新条件”的错误
- centos php日志分析,记录一下CentOS7安装GoAccess日志分析工具
- 测试需要的的linux命令,(面试必备)软件测试人员必备Linux命令操做(初级基础)...
- 如何让火狐浏览器兼容window.event
- 国际旅游管理专业跨专业考计算机,第二年跨校跨专业考旅游管理公费成功,一点经验给大家...
- 【Axure电商原型】电商app高保真原型、移动端通用版电商app模板、用户中心、会员体系、签到、高保真商城app、rp原型、直播、运营活动、订单管理、售后退款、电商系统、购物车、高保真移动端电商
- [转载] Python集合set
- vue控制台报错Duplicate keys detected: 'xxxxx'. This may cause an update error.解决方案
- 德标螺纹规格对照表_德标与国标对照表
- 诡异事件之自定义UINavigationController
- CF1095A Repeating Cipher
- 阿里云账号注销踩坑实践记录
- AcWing蓝桥杯AB组辅导课10、疑难杂题
- 对前端工程化、模块化、组件化开发的理解