ORB-SLAM2的特征提取算法

ORB-SLAM2跟踪线程对相机输入的每一帧图像进行跟踪处理,如下图所示,主要包括4步,提取ORB特征、从上一帧或者重定位来估计初始位姿、局部地图跟踪和关键帧处理。

以下结合相关理论知识,阅读ORB-SLAM2源代码,从而理解ORB-SLAM2算法中ORB特征提取过程。

ORB(Oriented FAST and Rotated BRIEF)

基于特征点的方法是SLAM的前端VO的主流方法,因为其运行稳定,对光照、运动物体不敏感。特征点由关键点(Key-point)和描述子(Descriptor)两部分组成。比如,当说到SIFT特征时,是指“提取SIFT关键点,并计算SIFT描述子”。关键点是指该特征点在图像里的位置,有些特征点还具有朝向、大小等信息。描述子通常是一个向量,按照某种人为设计的方式,描述了该关键点周围像素的信息。描述子是按照“外观相似的特征应该有相似的描述子”的原则设计的。因此,只要两个特征点的描述子在向量空间上的距离相近,就可以人为它们是同样的特征点。

常见的特征有SIFT特征,SURF特征等。那么为什么ORB-SLAM2选择ORB特征呢?

这是因为虽然SIFT考虑了图像变换过程中出现的光照、尺寸、旋转等变化,但需要较大计算量,在没有GPU加速的情况下,很难在SLAM这种系统中进行实时计算,另一方面,SIFT特征和SURF特征是受到专利保护的,需要付费使用。

ORB特征由Ethan Rublee, Vincent Rabaud, Kurt Konolige和Gary R. Bradski在他们2011年的论文《ORB: An efficient alternative to SIFT or SURF》提出,如论文题目所述,ORB特征在计算速度、匹配性能,以及在专利要求上都可以替代SIFT和SURF。

ORB取名已经反映出其是一个结合了改良后的FAST角点提取和BRIEF描述子的算法,提取ORB特征分为两步:

  1. FAST关键点提取:找出图像中的FAST角点,相较于原版的FAST,ORB中计算了特征点的主方向,为后续的BRIEF描述子增加了旋转不变性;

  2. BRIEF描述子:对上一步提取出关键点的周围图像区域进行描述。

FAST关键点

FAST是一种角点,主要检测局部像素灰度变化明显的地方,以速度快著称。FAST只需要比较像素亮度大小,速度很快,它的检测过程如下:

  1. 在图像中选取像素p,假设它的亮度为Ip

  2. 设置一个阈值T(比如Ip的20%);

  3. 以像素p为中心,选取半径为3的圆上的16个像素点;

  4. 假如选取的圆上有连续的N个点的亮度大于Ip+T或者小于IpT,那么像素p可以被认为是特征点(N通常取12,即FAST-12)。

  5. 循环以上四步,对每一个像素执行相同的操作。

FAST角点检测虽然速度很快,但是它存在一些问题。首先是FAST角点数量很大且不确定,因此ORB对其进行改进。ORB指定最终要提取的角点数量N,对原始FAST角点分别计算Harris响应值,然后选取前N个具有最大值的角点作为最终的角点集合。

其次,FAST不具有尺寸,因此ORB构建图像金字塔,对图像进行不同层次的降采样,获得不同分辨率的图像,并在金字塔的每一层上检测角点,从而获得多尺寸特征。

FAST没有计算旋转,因此ORB通过计算以FAST角点O为中心的图像块的质心C,那么向量OC的方向就是特征点的方向,具体值通过图像块的矩得到。

通过各种改进,FAST特征具有了尺寸和旋转的描述,在ORB中,把这种改进后的FAST称为oFAST。

BRIEF描述子

BRIEF描述子是一种二进制字符描述子,其描述向量定义如下:

  

p(x) 是图像块p中点x的强度。τ的选择有很多种,常见的选择方式是围绕图像块中心的高斯分布。n选为256的话,fn(p)就是256维的向量。BRIEF由于使用了二进制表达,存储起来十分方便,适用于实时的图像匹配。原始的BRIEF描述子不具有旋转不变性,因此在图片发生旋转时,匹配性能会急速下降。ORB根据之前关键点的方向来旋转图像块,得到“steer BRIEF”。

BRIEF具有每个bit的方差很大,均值约为0.5的特性,但是“steer BRIEF”丧失了这种特性,其均值不再集中在0.5左右。可以理解为特定方向的角点关键点使得其产生发散。这样会导致使用“steer BRIEF”进行匹配时的错误率变高,因为“steer BRIEF”的方差发生了亏损,彼此之间区分度降低。同时我们希望每个τ彼此不相干,这样得到的BRIEF更加有区分度。

为了解决上述问题,BRIEF采用了贪婪搜索,对所有可能的τ进行搜索,找出既具有高方差,均值约为0.5,同时又不相干的τ,最终结果称为rBRIEF。

由于考虑了旋转和缩放,使得ORB在平移、旋转和缩放的变换下仍具有良好的表现。同时,FAST和BRIEF的计算非常高效,使得ORB特征在实时SLAM系统中得以应用。

以下阅读ORB-SLAM2的源代码,理清其跟踪线程中对ORB特征的提取过程。

函数入口

ORB-SLAM2跟踪运行在主线程,是整个SLAM系统的基础。主程序在初始化SLAM系统后,

// Examples/Monocular/mono_kitti.cc  line:53
// Create SLAM system. It initializes all system threads and gets ready to process frames.
ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true);

就可以将每一帧图像送往跟踪函数,如下是单目SLAM主函数调用跟踪函数的代码:

// Examples/Monocular/mono_kitti.cc  line:84
// Pass the image to the SLAM system
SLAM.TrackMonocular(im,tframe);

TrackMonocular()函数调用GrabImageMonocular()函数实现跟踪功能:

// System.cc  line:260
cv::Mat Tcw = mpTracker->GrabImageMonocular(im,timestamp);

 

双目和RGB-D调用方式类似,分别是SLAM.TrackStereo(imLeftRect,imRightRect,tframe);SLAM.TrackRGBD(imRGB,imD,tframe);

mpTrackerSystem类中的成员,是Tracking类的指针。mpTracker对输入的每一帧图像计算出对应的相机位姿,同时决定何时插入新的关键帧,创建新的地图点,并且在跟踪失效时进行重定位。mpTracker的初始化在System 类的对象SLAM初始化的构造函数中进行:

//System.cc  line:86~87
//Initialize the Tracking thread
//(it will live in the main thread of execution, the one that called this constructor)
mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer, mpMap, mpKeyFrameDatabase, strSettingsFile, mSensor);

 

那么mpTracker是如何实现上述功能的呢?我们来看Tracking类,其头文件为Tracking.h,其定义了接口如下:

// Tracking.h  line:61
// Preprocess the input and call Track(). Extract features and performs stereo matching.
cv::Mat GrabImageMonocular(const cv::Mat &im, const double &timestamp);

 

ORB特征提取

ORB-SLAM2是一个基于特征的方法,它对输入的图像提取出角点的特征,如下图所示: 

在提取出特征后,所有输入的图片都会删除,系统剩下的处理流程都是基于这些特征进行的,和相机类型无关。

单目的预处理流程实现过程在cv::Mat GrabImageMonocular(const cv::Mat &im, const double &timestamp)函数中体现为:首先将im转换为灰度图mImGray,然后预处理提取ORB特征:

// Tracking.cc  line:257~260
if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET)mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
elsemCurrentFrame = Frame(mImGray,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);

 

得到预处理的结果mCurrentFrame,从而系统剩余部分的处理流程都是基于mCurrentFrame,和单目相机无关。mCurrentFrameFrame类的对象,这里的预处理在Frame类的构造函数中进行。Frame类对单目相机输入的构造函数重载形式为:

// Frame.h
// Constructor for Monocular cameras.
Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth);

 

Frame.cc文件中查看其该重载函数定义,

// Frame.cc line:181~191
// Scale Level Info
mnScaleLevels = mpORBextractorLeft->GetLevels();
mfScaleFactor = mpORBextractorLeft->GetScaleFactor();
mfLogScaleFactor = log(mfScaleFactor);
mvScaleFactors = mpORBextractorLeft->GetScaleFactors();
mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors();
mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares();
mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares();// ORB extraction
ExtractORB(0,imGray);

 

其先提取ORB特征参数,然后调用Frame类成员函数ExtractORB()来提取ORB特征,ORB特征参数存储在配置文件中,在mpTracker的初始化中加载读入,并传入Frame的构造函数中。

ExtractORB()函数定义为:

// Frame.cc  line:247~253
// Extract ORB on the image. 0 for left image and 1 for right image.
void Frame::ExtractORB(int flag, const cv::Mat &im)
{if(flag==0)(*mpORBextractorLeft)(im,cv::Mat(),mvKeys,mDescriptors);else(*mpORBextractorRight)(im,cv::Mat(),mvKeysRight,mDescriptorsRight);
}

 

其调用了ORBextractor类的重载运算符来提取ORB特征:

// ORBextractor.h  line:56~61
// Compute the ORB features and descriptors on an image.
// ORB are dispersed on the image using an octree.
// Mask is ignored in the current implementation.
void operator()( cv::InputArray image, cv::InputArray mask,std::vector<cv::KeyPoint>& keypoints,cv::OutputArray descriptors);

 

ORB-SLAM提取ORB特征时采用了8层金字塔,尺寸因子为1.2。对于像素为512*384到752*480的图片,提取1000个FAST角点,对于更高的分辨率,提取2000个FAST角点就可以了。

至此,得到当前帧ORB特征点mvKeys和描述子mDescriptors,均是Frame类对象mCurrentFrame的成员变量。提取出特征点后,需要对其去失真UndistortKeyPoints();。同时需要将图片分割为64*48大小的栅格,并将关键点按照位置分配到相应栅格中,从而降低匹配时的复杂度,实现函数为AssignFeaturesToGrid(); 。

参考资料

[1] 高翔,张涛.“视觉SLAM十四讲”

[2] OBR-SLAM2 github 主页

 

转载于:https://www.cnblogs.com/yuhui-snail/p/8819014.html

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.mzph.cn/news/367309.shtml

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

引导界面图标好大_游戏里那些图标和界面,原来是这么设计出来的?

UI设计最硬核的思维 就是功能微信现在在做一种全面连接的功能&#xff0c;而游戏需要实现的是人机互动的功能。实现并完善功能&#xff0c;是互联网、游戏、网站、渴望UI人才的根本原因。如果说有电脑的世界是一片很大的面&#xff0c;那么可视化的操作&#xff0c;都是UI设计师…

爬格子呀9.17(图论)

刘汝佳的紫书差不多就告一段落吧&#xff0c;我觉得可以了&#xff0c;怎么说呢&#xff0c;这书也陪着自己走了一年多了吧&#xff0c;也目睹了从一个啥也不会的萌新到一个稍微会一点的萌新的转变。 差不多开始下本书吧&#xff0c;自己也大三了&#xff0c;时间真的有点紧啊w…

一个vue加egg.js的博客

之前自己的博客是用hexo做的&#xff0c;后面想做一个有后台的博客就打算用vue加node来试试&#xff0c;于是就有了这个博客。 项目地址 W-Blog W-Blog是一个基于vue和node的小小小博客 前端用vue&#xff0c;后端用egg.js 快速入门 技术栈 前端&#xff1a; 用户端&#…

android音量图标不见了,电脑声音图标不见了如何解决?

最近有电脑用户反映&#xff0c;看视频时觉得声音太小了&#xff0c;要调大点声&#xff0c;却发现任务栏上的声音图标不见了&#xff0c;想调个声音都难。那么&#xff0c;电脑声音图标不见了如何解决呢?我们一起往下看看。方法步骤一、XP系统下找回任务栏上的声音图标1、重启…

认识iOS系统架构

关于本文&#xff1a; 文章主要介绍iOS系统架构中的四层结构的内容、常用的框架、大致的功能&#xff0c;然后对iOS开发人员的发展提出自己的一些拙见。 一、iOS系统是基于UNIX系统&#xff0c;所有从系统稳定性上来说的确比其他操作系统的产品要好。 iOS在系统架构上分为4层&a…

Java泛型教程–示例类,接口,方法,通配符等

泛型是Java编程的核心功能之一&#xff0c;它是Java 5中引入的。如果您使用的是Java Collections &#xff0c;并且版本5或更高版本&#xff0c;则可以肯定使用了它。 在集合类中使用泛型非常容易&#xff0c;但是它提供了比仅创建集合类型更多的功能&#xff0c;我们将在本文中…

html5中音乐播放器怎么写,打造属于自己的音乐播放器 HTML5之audio标签

我的音乐播放器HTML5中增加了Audio和Video标签&#xff0c;这两个标签的用法非常相似。功能却是相当强大&#xff0c;我们先来看一下Audio标签各个浏览器的支持情况。这里用的依然是Can I Use这个在线网站&#xff0c;相信学习前端的同学应该都不陌生。Can I Use我们可以看到&a…

初识react(四) react中异步解决方案之 redux-saga

回顾 初识react(一) 揭开jsx语法和虚拟DOM面纱初识react(二) 实现一个简版的html redux.js的demo初识react(三)在 react中使用redux来实现简版计数器初识react(四) react中异步解决方案之 redux-saga初识react(五) 数据流终极解决方案 dva(零配置) 今天demo是实现一个异步的计…

C# WinFrom 关于MDI

dev是一个牛B 到没边的控件 我们正常用winform做个原始mdi窗体 一点都不好看 但 用的dev只需要一个控件 就可让显示舒服多了 建一个项目 上边放一个 xtraTabbedMdiManager1 一个button1 button1.click如下&#xff1a; Form frm new Form(); frm.MdiParent this; frm.Text &…

Jfinal 文件上传

JFinal上传文件 uploadify 可以在http://www.uploadify.com/ 下载。 在原项目的基础上。 uploadify使用&#xff1a; <input id"file_upload_1" name"file_upload" type"file" multiple"true"> /** param uploader 文件上传方法…

轻量级的开源集成:Apache Camel还是Spring集成?

首先&#xff0c;为全面披露信息&#xff0c;在过去的1.5年中&#xff0c; 我一直担任 FuseSource&#xff08;现为Red Hat&#xff09; 的顾问&#xff0c;为零售&#xff0c;运输&#xff0c;银行/金融等不同行业的大型和小型公司提供SOA和集成项目支持。我的专长是使用该领域…

WePY:在质疑中前进 | 文末福利

WePY 作者介绍 Q: 先介绍一下自己吧~ Gcaufy: 我 2011 年大学毕业之后&#xff0c;阴错阳差的走上了 Web 开发的道路。15 年之前算是自由职业 SOHO 工作&#xff0c;主要给一些国外的大公司做外包系统&#xff0c;更多的是做后端开发。15 年之后以前端工程师的身份加入腾讯&a…

MySQL/MariaDB表表达式(3):视图

视图是表表达式的一种&#xff0c;所以它也是虚拟表。对视图操作的时候会通过语句动态的从表中临时获取数据。 1.创建、修改视图 CREATE [OR REPLACE][ALGORITHM {UNDEFINED | MERGE | TEMPTABLE}]VIEW [IF NOT EXISTS] view_name [(column_list)]AS select_statement[WITH [C…

Event Loop 其实也就这点事

前段时间在网上陆续看了很多关于 Event loop 的文章&#xff0c;看完也就混个眼熟&#xff0c;可能内心深处对这种偏原理的知识有一些抵触心情&#xff0c;看完后也都没有去深入理解。最近在看 Vue 的源码&#xff0c;在读到关于 nextTick 的实现时&#xff0c;总有一种似曾相识…

Kudu系列: Kudu主键选择策略

每个Kudu 表必须设置Pimary Key(unique), 另外Kudu表不能设置secondary index, 经过实际性能测试, 本文给出了选择Kudu主键的几个策略, 测试结果纠正了我之前的习惯认知. 简单介绍测试场景: 表中有一个unqiue字段Id, 另外还有一个日期维度字段histdate, 有三种设置kudu PK的方法…

OSS网页上传和断点续传(OSS配置篇)

OSS网页上传和断点续传主要根据BrowserJS-SDK和相关文档整理而得&#xff0c;快速构建OSS上传应用 一、Bucket设置 浏览器中直接访问OSS需要开通Bucket的CORS设置 将allowed origins设置成 *将allowed methods设置成 PUT, GET, POST, DELETE, HEAD将allowed headers设置成 *将e…

小程序各种姿势实现登录

喜闻乐见的背景时间--由于最近接触小程序比较多&#xff0c;又刚好经历过小程序的自动登录时代以及现在的点击登录时代。结合自己的实践以及观察到其他小程序的做法&#xff0c;就有了这篇小分享~ 本文可能涉及的内容-- 更新 首先感谢shaonialife同学的精彩评论~ 可能由于用词…

BBS-登录

from django.db import models# Create your models here. from django.contrib.auth.models import AbstractUser#用户 class UserInfo(AbstractUser):nidmodels.AutoField(primary_keyTrue)telephonemodels.CharField(max_length32)avatarmodels.FileField(upload_toavatar/,…

使用Mockito和BeanPostProcessors在Spring注入测试双打

我非常确定&#xff0c;如果您曾经使用过Spring并且熟悉单元测试&#xff0c;那么您会遇到与您不想修改的Spring应用程序上下文中注入模拟/间谍&#xff08;测试双打&#xff09;有关的问题。 本文介绍了一种使用Spring组件解决此问题的方法。 项目结构 让我们从项目结构开始&…

二叉搜索时与双向链表python_JZ26-二叉搜索树与双向链表

1、中序遍历&#xff0c;当前结点&#xff0c;以及左侧排好序的双向链表&#xff0c;再调整当前结点的指针指向最前结点/* struct TreeNode {int val;struct TreeNode *left;struct TreeNode *right;TreeNode(int x) :val(x), left(NULL), right(NULL) {} };*/ class Solution …