CN106570913B - 基于特征的单目slam快速初始化方法 - Google Patents
基于特征的单目slam快速初始化方法 Download PDFInfo
- Publication number
- CN106570913B CN106570913B CN201610961795.1A CN201610961795A CN106570913B CN 106570913 B CN106570913 B CN 106570913B CN 201610961795 A CN201610961795 A CN 201610961795A CN 106570913 B CN106570913 B CN 106570913B
- Authority
- CN
- China
- Prior art keywords
- slam
- frame
- feature
- map
- distortion
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T11/00—2D [Two Dimensional] image generation
Abstract
一种基于特征的单目SLAM快速初始化方法,包括以下步骤:1)开始SLAM并获取第一帧图片,对图片提取ORB特征点P(x,y);2)对P(x,y)进行图像去畸;3)对步骤2)中的去畸特征点的图像坐标进行归一化;4)为步骤3)中的每个特征点构建随机深度;5)将步骤3)、步骤4)的结果组合,构建每个特征点对应的地图点,获取初始地图;6)将后续相邻帧的匹配结果与现有地图进行优化,为接下去新增的每一帧执行正常的特征SLAM的流程,不断调整扩充地图,实现SLAM的连续跟踪。本发明提供一种速度较快、计算量较小、不受景深限制的基于特征的单目SLAM快速初始化方法。
Description
技术领域
本发明涉及一种SLAM初始化方法。
背景技术
SLAM(simultaneous localization and mapping,即时定位与地图构建)指的是机器人在自身位置不确定的条件下,在完全未知环境中创建地图,同时利用地图进行自主定位和导航。基于视觉传感器定位方式,是近些年国内外研究的一个热点,它又分为单目、双目和多目定位。由于单目SLAM一次观测不能获得相对于环境特征的全部信息,只能获得方向信息,距离信息无法提取,是一种仅有方向信息的方法,所以建立特征地图存在特征初始化方法,特征初始化就是对特征的初始位置进行估计,获得估计的深度信息。目前的单目特征SLAM初始化方法有PTAM(Parallel Tracking and Mapping,同步追踪与构图)、ORB-SLAM(a real-time accurate monocular SLAM system based on ORB features,基于ORB特征点的SLAM系统)等。
PTAM标准代码中,地图初始化是手动的。操作方法是手持摄像头运动一定的角度之后按一个键,得到两个关键帧,一个是初始帧,即最初的关键帧;另一个是结束帧,即运动一定角度后的关键帧。从初始帧到结束帧之间,会使用ESM(Efficient Second-orderMinimization,高效的二阶最小化)预测,SSD(Sum of Squared Difference,差的平方和相关法)模板跟踪的方式在帧与帧之间进行关键点的匹配,最后PTAM使用这两个关键帧进行地图的初始化。
ORB-SLAM地图初始化常见的方法有三种:1、追踪一个已知物体。单帧图像的每一个点都对应于空间的一条射线。通过不同角度不同位置扫描同一个物体,期望能够将三维点的不确定性缩小到可接受的范围;2、基于假设空间存在一个平面物体,选取两帧不同位置的图像,通过计算Homography来估计位姿。这类方法在视差较小或者平面上的点靠近某个主点时效果不好;3、根据两帧之间的特征点匹配计算Fundamental matrix,进一步估计位姿。这种方法要求存在不共面的特征点。
然而上述方法都需要人为或算法来选取两帧进行初始化,估计摄像机姿态并重建出初始的三维点云。初始化的计算量非常大,导致初始化过程很慢,并且因为要重建三维点云,由于距离的问题,会受到景深的限制。
发明内容
为了克服现有PTAM、ORB-SLAM等单目特征SLAM方法的初始化速度慢、计算量大、受景深限制的不足,本发明提供一种速度较快、计算量较小、不受景深限制的基于特征的单目SLAM快速初始化方法,通过提取第一帧图片的ORB特征点,来进行初始化,使得单目SLAM初始化速度大大提高,远景特征提取更为精确。
本发明解决其技术问题所采用的技术方案是:
一种基于特征的单目SLAM快速初始化方法,所述方法包括以下步骤:
1)开始SLAM并获取第一帧图片,对图片提取ORB特征点P(x,y);
2)对P(x,y)进行图像去畸;
3)对步骤2)中的去畸特征点的图像坐标进行归一化,得到(Pw_x,Pw_y);
4)为步骤3)中的每个特征点构建随机深度Pw_z;
5)将步骤3)、步骤4)的结果组合,构建每个特征点对应的地图点(Pw_x,Pw_y,Pw_z),获取初始地图;
6)将后续相邻帧的匹配结果与现有地图进行优化,为接下去新增的每一帧执行正常的特征SLAM的流程,不断调整扩充地图,实现SLAM的连续跟踪。
进一步,所述步骤6)中,SLAM的流程如下:
6.1)初始化关键帧序列;
6.2)对于新来的一帧I,计算关键帧序列中最后一帧与I的运动,并估计该运动的大小e,有以下几种可能性:
6.2.1)若e>Eerror,说明运动太大,可能是计算错误,丢弃该帧;
6.2.2)若没有匹配上,说明该帧图像质量不高,丢弃;
6.2.3)若e<Ekey,说明离前一个关键帧很近,同样丢弃;
6.2.4)剩下的情况,只有是特征匹配成功,运动估计正确,同时又离上一个关键帧有一定距离,则把I作为新的关键帧,进入回环检测程序;
6.3)近距离回环:匹配I与关键帧序列末尾m个关键帧,匹配成功时,在图里增加一条边;
6.4)随机回环:随机在关键帧序列里取n个帧,与I进行匹配。若匹配上,在图里增加一条边;
6.5)将I放入关键帧序列末尾,若有新的数据,则返回6.2);若无,则进行优化与地图拼接。
再进一步,所述步骤2)中,去畸过程为:
2.1)径向畸变校正建模如下:
其中,(x,y)为畸变点在成像仪上的原始位置,(x',y')为矫正后的新位置,标定中,k1,k2,k3为畸变内参;
2.2)切向畸变校正模型如下:
标定中,k1,k2为畸变内参。
更进一步,所述步骤3)中,归一化过程如下:
首先利用图像中对仿射变换具有不变性的矩来确定变换函数的参数,然后利用此参数确定的变换函数把原始图像变换为一个标准形式的图像;坐标归一化公式如下:
其中,(Cx,Cy)为主点,(fx,fy)为焦距。
所述步骤4)中,所述随机深度满足0.5-1.5的正态分布Normal(1,0.125),正态分布公式如下:
其中,μ为平均数,σ为标准差。
本发明的有益效果主要表现在:只需选取单帧进行初始化,使得整个初始化过程不必重建三维点云,即不再需要几何计算,故而大大减少了计算量,加快了初始化的速度,且不再受到景深的影响。
具体实施方式
下面对本发明作进一步描述。
一种基于特征的单目SLAM快速初始化方法,包括以下步骤:
1)、开始SLAM并获取第一帧图片,对图片提取ORB特征点P(x,y)。
ORB(Oriented FAST and Rotated BRIEF,面向快速旋转不变性算法)是一种快速特征点提取和描述的算法,具有旋转不变性的同时具有较好的抗噪能力,特征点的主方向是通过矩(moment)计算得来,也就是说通过矩来计算特征点以r为半径范围内的质心,特征点坐标到质心形成一个向量作为该特征点的方向。
2)对P(x,y)进行图像去畸
摄像机成像的过程实质上是几个坐标系的转换,首先空间中的一点由世界坐标系转换到摄像机坐标系,然后再将其投影到成像平面(图像物理坐标系),最后再将成像平面上的数据转换到图像平面(图像像素坐标系)。
图像像素坐标系(xOy坐标系)下的无畸变坐标(x,y),经过径向畸变和切向畸变后落在了xOy坐标系的(x",y")上。即就是说,真实图像imgP与畸变图像imgR之间的关系为:imgP(x,y)=imgR(x",y")。
2.1)径向畸变校正建模如下:
(x,y)为畸变点在成像仪上的原始位置,(x',y')为矫正后的新位置,标定中,k1,k2,k3为畸变内参。
2.2)切向畸变校正模型如下:
标定中,k1,k2为畸变内参。
3)对步骤2)中的去畸特征点的图像坐标进行归一化
归一化就是通过一系列变换(即利用图像的不变矩寻找一组参数使其能够消除其他变换函数对图像变换的影响),将待处理的原始图像转换成相应的唯一标准形式(该标准形式图像对平移、旋转、缩放等仿射变换具有不变特性)。
基于矩的图像归一化技术基本工作原理为:首先利用图像中对仿射变换具有不变性的矩来确定变换函数的参数,然后利用此参数确定的变换函数把原始图像变换为一个标准形式的图像。其坐标归一化公式如下:
其中(Cx,Cy)为主点,(fx,fy)为焦距;
4)对步骤3)中的每个特征点构建随机深度Pw_z,该随机深度满足0.5-1.5的正态分布Normal(1,0.125)。正态分布公式如下:
其中μ为平均数,σ为标准差。
5)将步骤3)、步骤4)的结果组合,构建每个特征点对应的地图点(Pw_x,Pw_y,Pw_z),获取初始地图;
6)在初始地图的基础上,将后续相邻帧的匹配结果与现有地图进行优化,剔除错误点,增加新的点,不断调整扩充地图,实现SLAM的连续跟踪。
SLAM的流程如下:
6.1)初始化关键帧序列。
6.2)对于新来的一帧I,计算关键帧序列中最后一帧与I的运动,并估计该运动的大小e,有以下几种可能性:
6.2.1)若e>Eerror,说明运动太大,可能是计算错误,丢弃该帧;
6.2.2)若没有匹配上(match太少),说明该帧图像质量不高,丢弃;
6.2.3)若e<Ekey,说明离前一个关键帧很近,同样丢弃;
6.2.4)剩下的情况,只有是特征匹配成功,运动估计正确,同时又离上一个关键帧有一定距离,则把I作为新的关键帧,进入回环检测程序。
6.3)近距离回环:匹配I与关键帧序列末尾m个关键帧。匹配成功时,在图里增加一条边。
6.4)随机回环:随机在关键帧序列里取n个帧,与I进行匹配。若匹配上,在图里增加一条边。
6.5)将I放入关键帧序列末尾。若有新的数据,则返回6.2);若无,则进行优化与地图拼接。
Claims (4)
1.一种基于特征的单目SLAM快速初始化方法,其特征在于:所述方法包括以下步骤:
1)开始SLAM并获取第一帧图片,对图片提取ORB特征点P(x,y);
2)对P(x,y)进行图像去畸;
3)对步骤2)中的去畸特征点的图像坐标进行归一化,得到(Pw_x,Pw_y);
4)为步骤3)中的每个特征点构建随机深度Pw_z;
5)将步骤3)、步骤4)的结果组合,构建每个特征点对应的地图点(Pw_x,Pw_y,Pw_z),获取初始地图;
6)将后续相邻帧的匹配结果与现有地图进行优化,为接下去新增的每一帧执行特征SLAM的流程,不断调整扩充地图,实现SLAM的连续跟踪;
所述步骤6)中,SLAM的流程如下:
6.1)初始化关键帧序列;
6.2)对于新来的一帧I,计算关键帧序列中最后一帧与I的运动,并估计该运动的大小e,有以下几种可能性:
6.2.1)若e>Eerror,说明运动太大,可能是计算错误,丢弃该帧;
6.2.2)若没有匹配上,说明该帧图像质量不高,丢弃;
6.2.3)若e<Ekey,说明离前一个关键帧很近,同样丢弃;
6.2.4)剩下的情况,只有是特征匹配成功,运动估计正确,同时又离上一个关键帧有一定距离,则把I作为新的关键帧,进入回环检测程序;
6.3)近距离回环:匹配I与关键帧序列末尾m个关键帧,匹配成功时,在图里增加一条边;
6.4)随机回环:随机在关键帧序列里取n个帧,与I进行匹配,若匹配上,在图里增加一条边;
6.5)将I放入关键帧序列末尾,若有新的数据,则返回6.2);若无,则进行优化与地图拼接。
2.如权利要求1所述的基于特征的单目SLAM快速初始化方法,其特征在于:所述步骤2)中,去畸过程为:
2.1)径向畸变校正建模如下:
其中,(x,y)为畸变点在成像仪上的原始位置,(x',y')为矫正后的新位置,标定中,k1,k2,k3为畸变内参;
2.2)切向畸变校正模型如下:
标定中,k1,k2为畸变内参。
3.如权利要求1所述的基于特征的单目SLAM快速初始化方法,其特征在于:所述步骤3)中,归一化过程如下:
首先利用图像中对仿射变换具有不变性的矩来确定变换函数的参数,然后利用此参数确定的变换函数把原始图像变换为一个标准形式的图像;坐标归一化公式如下:
其中,(Cx,Cy)为主点,(fx,fy)为焦距。
4.如权利要求1所述的基于特征的单目SLAM快速初始化方法,其特征在于:所述步骤4)中,所述随机深度满足0.5-1.5的正态分布Normal(1,0.125),正态分布公式如下:
其中,μ为平均数,σ为标准差。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201610961795.1A CN106570913B (zh) | 2016-11-04 | 2016-11-04 | 基于特征的单目slam快速初始化方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201610961795.1A CN106570913B (zh) | 2016-11-04 | 2016-11-04 | 基于特征的单目slam快速初始化方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN106570913A CN106570913A (zh) | 2017-04-19 |
CN106570913B true CN106570913B (zh) | 2019-12-13 |
Family
ID=58535624
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201610961795.1A Active CN106570913B (zh) | 2016-11-04 | 2016-11-04 | 基于特征的单目slam快速初始化方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN106570913B (zh) |
Families Citing this family (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107689048B (zh) * | 2017-09-04 | 2022-05-31 | 联想(北京)有限公司 | 一种检测图像特征点的方法及一种服务器集群 |
EP3474230B1 (en) | 2017-10-18 | 2020-07-22 | Tata Consultancy Services Limited | Systems and methods for edge points based monocular visual slam |
CN110660098B (zh) * | 2018-06-28 | 2022-08-12 | 北京京东叁佰陆拾度电子商务有限公司 | 基于单目视觉的定位方法和装置 |
CN109544632B (zh) * | 2018-11-05 | 2021-08-03 | 浙江工业大学 | 一种基于层次主题模型的语义slam对象关联方法 |
CN109887053B (zh) * | 2019-02-01 | 2020-10-20 | 广州小鹏汽车科技有限公司 | 一种slam地图拼接方法及系统 |
CN109887087B (zh) * | 2019-02-22 | 2021-02-19 | 广州小鹏汽车科技有限公司 | 一种车辆的slam建图方法及系统 |
CN110060282B (zh) * | 2019-03-27 | 2021-06-08 | 东软睿驰汽车技术(沈阳)有限公司 | 一种点云配准的方法及装置 |
CN110060202B (zh) * | 2019-04-19 | 2021-06-08 | 湖北亿咖通科技有限公司 | 一种单目slam算法的初始化方法及系统 |
CN110610520B (zh) * | 2019-08-29 | 2022-03-29 | 中德(珠海)人工智能研究院有限公司 | 一种基于双球幕相机的视觉定位方法及系统 |
CN111311684B (zh) * | 2020-04-01 | 2021-02-05 | 亮风台(上海)信息科技有限公司 | 一种进行slam初始化的方法与设备 |
CN112037258B (zh) * | 2020-08-25 | 2024-03-08 | 广州视源电子科技股份有限公司 | 目标跟踪方法、装置、设备和存储介质 |
CN112258555A (zh) * | 2020-10-15 | 2021-01-22 | 佛山科学技术学院 | 实时姿态估计运动分析方法、系统、计算机设备及存储介质 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR20150082379A (ko) * | 2012-11-02 | 2015-07-15 | 퀄컴 인코포레이티드 | 단안 시각 slam 을 위한 고속 초기화 |
CN104778697A (zh) * | 2015-04-13 | 2015-07-15 | 清华大学 | 基于快速定位图像尺度和区域的三维跟踪方法及系统 |
CN104851094A (zh) * | 2015-05-14 | 2015-08-19 | 西安电子科技大学 | 一种基于rgb-d的slam算法的改进方法 |
CN105976391A (zh) * | 2016-05-27 | 2016-09-28 | 西北工业大学 | 基于orb-slam的多摄像机标定方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20150371440A1 (en) * | 2014-06-19 | 2015-12-24 | Qualcomm Incorporated | Zero-baseline 3d map initialization |
-
2016
- 2016-11-04 CN CN201610961795.1A patent/CN106570913B/zh active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR20150082379A (ko) * | 2012-11-02 | 2015-07-15 | 퀄컴 인코포레이티드 | 단안 시각 slam 을 위한 고속 초기화 |
CN104778697A (zh) * | 2015-04-13 | 2015-07-15 | 清华大学 | 基于快速定位图像尺度和区域的三维跟踪方法及系统 |
CN104851094A (zh) * | 2015-05-14 | 2015-08-19 | 西安电子科技大学 | 一种基于rgb-d的slam算法的改进方法 |
CN105976391A (zh) * | 2016-05-27 | 2016-09-28 | 西北工业大学 | 基于orb-slam的多摄像机标定方法 |
Non-Patent Citations (3)
Title |
---|
ORB-SLAM map initialization improvement using depth;Satoshi Fujimoto 等;《2016 IEEE International Conference on Image Processing (ICIP)》;20160928;第2381-8549页 * |
ORB-SLAM: a Versatile and Accurate Monocular SLAM System;Raúl Mur-Artal 等;《IEEE Transactions on Robotics 》;20150824;第1147 - 1163页 * |
基于ORB关键帧匹配算法的机器人SLAM实现;艾青林 等;《机电工程》;20160520;第513-520页 * |
Also Published As
Publication number | Publication date |
---|---|
CN106570913A (zh) | 2017-04-19 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN106570913B (zh) | 基于特征的单目slam快速初始化方法 | |
CN109544636B (zh) | 一种融合特征点法和直接法的快速单目视觉里程计导航定位方法 | |
CN109903313B (zh) | 一种基于目标三维模型的实时位姿跟踪方法 | |
CN109166149B (zh) | 一种融合双目相机与imu的定位与三维线框结构重建方法与系统 | |
JP6430064B2 (ja) | データを位置合わせする方法及びシステム | |
Urban et al. | Mlpnp-a real-time maximum likelihood solution to the perspective-n-point problem | |
CN103325112B (zh) | 动态场景中运动目标快速检测方法 | |
KR20180087947A (ko) | 3차원의 포인트 클라우드를 이용한 모델링 방법 및 모델링 장치 | |
CN111897349B (zh) | 一种基于双目视觉的水下机器人自主避障方法 | |
CN111862296B (zh) | 三维重建方法及装置、系统、模型训练方法、存储介质 | |
US20170019655A1 (en) | Three-dimensional dense structure from motion with stereo vision | |
Lourakis et al. | Model-based pose estimation for rigid objects | |
CN108225319B (zh) | 基于目标特征的单目视觉快速相对位姿估计系统及方法 | |
JP2009525543A (ja) | 二次元画像からの三次元の顔の復元 | |
KR20120048370A (ko) | 물체 자세 인식장치 및 이를 이용한 물체 자세 인식방법 | |
CN110827321B (zh) | 一种基于三维信息的多相机协同的主动目标跟踪方法 | |
CN111768449A (zh) | 一种双目视觉结合深度学习的物体抓取方法 | |
CN110543817A (zh) | 基于姿势指导特征学习的行人再识别方法 | |
Koide et al. | General, single-shot, target-less, and automatic lidar-camera extrinsic calibration toolbox | |
Liu et al. | Adaptive ground plane estimation for moving camera-based 3D object tracking | |
CN116468786B (zh) | 一种面向动态环境的基于点线联合的语义slam方法 | |
Zheng et al. | TP-AE: Temporally primed 6D object pose tracking with auto-encoders | |
CN115953471A (zh) | 室内场景多尺度矢量图像检索与定位方法、系统及介质 | |
CN116433760A (zh) | 一种水下导航定位系统和方法 | |
Hachiuma et al. | Recognition and pose estimation of primitive shapes from depth images for spatial augmented reality |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |