CN113160130A - 一种回环检测方法、装置及计算机设备 - Google Patents
一种回环检测方法、装置及计算机设备 Download PDFInfo
- Publication number
- CN113160130A CN113160130A CN202110256945.XA CN202110256945A CN113160130A CN 113160130 A CN113160130 A CN 113160130A CN 202110256945 A CN202110256945 A CN 202110256945A CN 113160130 A CN113160130 A CN 113160130A
- Authority
- CN
- China
- Prior art keywords
- pose
- calculating
- camera
- image
- frames
- 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.)
- Pending
Links
- 238000001514 detection method Methods 0.000 title claims abstract description 55
- 238000004422 calculation algorithm Methods 0.000 claims description 51
- 238000000034 method Methods 0.000 claims description 32
- 230000009466 transformation Effects 0.000 claims description 32
- 238000004590 computer program Methods 0.000 claims description 18
- 230000004927 fusion Effects 0.000 claims description 11
- 238000012545 processing Methods 0.000 claims description 9
- 238000004364 calculation method Methods 0.000 claims description 8
- 238000009826 distribution Methods 0.000 claims description 8
- 239000013598 vector Substances 0.000 claims description 5
- 230000008859 change Effects 0.000 claims description 3
- 238000001914 filtration Methods 0.000 claims description 3
- 230000003993 interaction Effects 0.000 claims description 3
- 238000010586 diagram Methods 0.000 description 13
- 230000006870 function Effects 0.000 description 9
- 230000000694 effects Effects 0.000 description 8
- 238000013507 mapping Methods 0.000 description 7
- 238000005516 engineering process Methods 0.000 description 6
- 238000005457 optimization Methods 0.000 description 5
- 230000008569 process Effects 0.000 description 5
- 238000003860 storage Methods 0.000 description 5
- 238000010276 construction Methods 0.000 description 3
- 238000012986 modification Methods 0.000 description 3
- 230000004048 modification Effects 0.000 description 3
- 238000005070 sampling Methods 0.000 description 3
- 238000000605 extraction Methods 0.000 description 1
- 238000007499 fusion processing Methods 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000002093 peripheral effect Effects 0.000 description 1
- 238000012216 screening Methods 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/0002—Inspection of images, e.g. flaw detection
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/30—Determination of transform parameters for the alignment of images, i.e. image registration
- G06T7/33—Determination of transform parameters for the alignment of images, i.e. image registration using feature-based methods
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10004—Still image; Photographic image
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10028—Range image; Depth image; 3D point clouds
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/30—Subject of image; Context of image processing
- G06T2207/30244—Camera pose
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Quality & Reliability (AREA)
- Computer Graphics (AREA)
- Geometry (AREA)
- Software Systems (AREA)
- Image Analysis (AREA)
Abstract
本申请公开了一种回环检测方法、装置及计算机设备,用于提高回环检测的精度。本申请公开的回环检测方法包括:对***进行初始化;通过相机采集图像;提取所述图像中每一帧图像的特征点,并匹配连续两帧图像之间的特征点;构建三维点云地图;计算当前相机位姿点与历史相机位姿点之间的欧拉距离,若所述欧拉距离小于预设的第一阈值,则计算空间相邻帧之间的相似度,若所述相似度大于预设的第二阈值,则进行是否回环的判断。本申请还提供了一种回环检测装置及计算机设备。
Description
技术领域
本申请涉及定位技术领域,尤其涉及一种回环检测方法、装置和计算机设备。
背景技术
在自动驾驶、无人驾驶、自主移动机器人中,建图与定位技术是关键技术之一,而回环检测在建图与定位技术中是重要的一步。但是现有技术中,回环检测技术精准度不够,时常出现误检漏检,从而影响建图与定位效果。
发明内容
针对上述技术问题,本申请实施例提供了一种回环检测方法、装置及计算机设备,用以提高回环检测的精度。
第一方面,本申请实施例提供的一种回环检测方法,包括:
对***进行初始化;
通过相机采集图像;
提取所述图像中每一帧图像的特征点,并匹配连续两帧图像之间的特征点;
构建三维点云地图;
计算当前相机位姿点与历史相机位姿点之间的欧拉距离;
若所述欧拉距离小于预设的第一阈值,则计算空间相邻帧之间的相似度;
若所述相似度大于预设的第二阈值,则进行是否回环的判断。
优选的,所述对***进行初始化包括:
导入初始化参数和摄像头内部参数;
所述初始化参数包括以下之一或者组合:关键帧欧拉距离阈值、关键帧旋转角度差值阈值、位姿欧拉距离邻近阈值、词袋模型描述相似度阈值、正态分布变换算法NDT配准迭代次数和回环收敛阈值。
进一步的,所述提取所述图像中每一帧图像的特征点,并匹配连续两帧图像之间的特征点包括:
提取每帧图像的Oriented Fast and Rotated Brief,ORB,特征关键点;
计算所述每帧图像的ORB特征描述子;
通过快速近似最近邻算法对连续两帧图像进行特征点匹配。
优选的,所述构建三维点云地图包括:
用三角测量算法计算出ORB特征点的三维空间位置;
对特征点点云进行配准,得到相机位姿变换;
对极几何算法计算得到的相机位姿变换和通过NDT配准特征点点云得到的相机位姿进行融合,得到相机融合位姿;
计算当前相机融合位姿与上一关键帧之间的欧拉距离和旋转角度差值,当欧拉距离大于关键帧欧拉距离阈值,或者旋转角度差值大于关键帧旋转角度差值阈值时,将当前融合位姿的图像帧作为关键帧;
根据所述提取的关键帧进行三维点云地图构建,删除非关键帧图像及点云。
进一步的,所述计算出ORB特征点的三维空间位置包括:
根据连续两帧图像中的ORB特征点,计算所述ORB特征点描述子之间的汉明距离,确定所述ORB特征点对的相似程度;
根据快速近似最近邻算法匹配连续两帧图像中对应的ORB特征点,构成ORB特征点对,删除没有完成匹配的ORB特征点;
根据对极几何算法处理所述特征点对,得到相机的位姿变换;
根据相机位姿变化,通过三角测量算法计算得到ORB特征点的三维空间坐标。
优选的,所述对特征点点云进行配准包括:
通过正态分布变换算法NDT配准所述ORB特征点点云,得到相机位姿变换。
优选的,所述对所述相机位姿变换和所述三维空间位置进行融合,确定融合位姿包括:
通过卡尔曼滤波算法对所述通过对极几何算法得到的相机位姿变换和通过NDT点云配准算法得到的相机位姿变换进行融合,确定融合位姿。。
本发明中,所述计算当前相机位姿点与历史相机位姿点之间的欧拉距离包括:
排除时序上邻近的N个位姿点;
计算当前相机位姿与历史相机位姿点之间的欧拉距离;
其中,N为预设的数值,N是大于等于1的整数。
优选的,述计算空间相邻帧之间的相似度包括:
计算空间相邻帧之间的描述子Bag of Word,BoW,相似程度;
其中通过下列公式计算空间相邻帧之间的描述子BoW相似程度:
其中,a,b∈RW为W维度的向量,表示两帧图像的BoW描述;
s(a,b)为两帧图像间相似程度;
||a-b||1为取L1范数,表示各元素绝对值之和。
优选的,所述进行是否回环的判断包括:
通过NDT算法将两帧欧拉距离邻近点云进行配准,进行M次迭代计算,如果配准结果收敛到预设收敛阈值之下,则判定为检测到回环;
其中,M是预先设置的,M是大于等于1的整数。
进一步的,判定为检测到回环之后还包括:
对相机位姿进行重新计算,重新建图。
本发明提供的回环检测方法,应用NDT算法对视觉SLAM建图与定位的回环检测技术进行改进,从而提高了回环检测的精度。
第二方面,本申请实施例还提供一种回环检测装置,包括:
初始化模块,被配置用于对***进行初始化;
图像采集模块,被配置用于采集图像;
特征点匹配模块,被配置用于提取所述图像中每一帧图像的特征点,并匹配连续两帧图像之间的特征点;
云地图构建模块,被配置用于构建三维点云地图;
回环判断模块,被配置用于计算当前相机位姿点与历史相机位姿点之间的欧拉距离;若所述欧拉距离小于预设的第一阈值,则计算空间相邻帧之间的相似度;若所述相似度大于预设的第二阈值,则进行是否回环的判断。
第三方面,本申请实施例还提供一种计算机设备,包括:存储器、处理器和用户接口;
所述存储器,用于存储计算机程序;
所述用户接口,用于与用户实现交互;
所述处理器,用于读取所述存储器中的计算机程序,所述处理器执行所述计算机程序时,实现本发明提供的回环检测方法。
附图说明
为了更清楚地说明本申请实施例中的技术方案,下面将对实施例描述中所需要使用的附图作简要介绍,显而易见地,下面描述中的附图仅是本申请的一些实施例,对于本领域的普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他的附图。
图1为回环检测示意图一;
图2为回环检测示意图二;
图3为本申请实施例提供的一种回环检测方法流程示意图;
图4为本申请实施例提供的另一种回环检测方法流程示意图;
图5为本申请实施例提供的一种回环检测装置示意图;
图6为本申请实施例提供的一种计算机设备结构示意图。
具体实施方式
为了使本发明的目的、技术方案和优点更加清楚,下面将结合附图对本发明作进一步地详细描述,显然,所描述的实施例仅仅是本发明一部份实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例,都属于本发明保护的范围。
下面对文中出现的一些词语进行解释:
1、本发明实施例中术语“和/或”,描述关联对象的关联关系,表示可以存在三种关系,例如,A和/或B,可以表示:单独存在A,同时存在A和B,单独存在B这三种情况。字符“/”一般表示前后关联对象是一种“或”的关系。
2、本申请实施例中术语“多个”是指两个或两个以上,其它量词与之类似。
3、回环检测,又称闭环检测,是指机器人识别曾经到达的某个场景,使得地图闭环的过程。
4、NDT算法,是指正态分布变换(Normal Distribution Transform),是利用点云的空间正态分布特性来对齐配准两组点云的方法。
5、BoW,是指词袋模型(bag of word),是使用图像中拥有的预定义像素阵列特征数量来描述图像独特特征的一种方法。
6、时序邻近帧,是指在拍摄时间排序上相邻的图像帧。
7、ORB,是Oriented FAST and Rotated BRIEF的缩写,即特征点提取和特征点描述。其中,FAST是Features from Accelerated Segment Test的缩写,BRIEF是BinaryRobust Independent Elementary Features的缩写。
8、SLAM,是simultaneous localization and mapping的缩写,即同步定位与地图绘制。
9、FLANN,是Fast Library for Approximate Nearest Neighbors的缩写,即快速近似最近邻。
10、关键帧,是能代表小段空间内一部分图像帧的,且用于回环检测和建图的图像帧,每个关键帧之间距离为预设关键帧欧拉距离或关键帧旋转角度差值。
如图1所示为回环检测示意图1,机器人从A点出发,经过若干个采样点之后,回到A点,回环检测成功。其中,机器人在每个采样点均进行回环检测,只有当满足回环检测成功的条件时,才判决为回环检测成功,或者称为检测到回环。
如图2所示为回环检测示意图2,机器人从B点出发,经过若干个采样点之后,回到B点,回环检测失败。整个检测过程中,没有任何一个点满足回环检测成功的条件,或者称为没有检测到回环。
本发明针对现有技术中回环检测精度不高的问题,提出了一种回环检测方法。
下面将结合本申请实施例中的附图,对本申请实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本申请一部分实施例,并不是全部的实施例。基于本申请中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都属于本申请保护的范围。
需要说明的是,本申请实施例的展示顺序仅代表实施例的先后顺序,并不代表实施例所提供的技术方案的优劣。
实施例一
参见图3,本申请实施例提供的一种回环检测方法示意图,如图3所示,该方法包括步骤S301到S305:
S301、对***进行初始化;
S302、通过相机采集图像;
S303、提取所述图像中每一帧图像的特征点,并匹配连续两帧图像之间的特征点;
S304、构建三维点云地图;
S305、计算当前相机位姿点与历史相机位姿点之间的欧拉距离,若所述欧拉距离小于预设的第一阈值,则计算空间相邻关键帧之间的相似度,若所述相似度大于预设的第二阈值,则进行是否回环的判断。
作为一种优选示例,上述步骤S301中,对***进行初始化可以包括:导入初始化参数以及摄像头内部参数。其中,所述初始化参数包括以下之一或者组合:关键帧欧拉距离阈值、关键帧旋转角度差值阈值、位姿欧拉距离邻近阈值、BoW描述相似度阈值、NDT算法配准迭代次数以及收敛阈值等。
作为一种优选示例,上述步骤S303包括:
步骤1A、提取每帧图像ORB特征关键点;
步骤1B、计算每帧图像ORB特征描述子;
步骤1C、通过FLANN算法对连续两帧图像进行特征点匹配。
作为一种优选示例,上述步骤S304中,构建三维点云地图包括:
步骤2A、用三角测量算法计算出ORB特征点的三维空间位置;
步骤2B、对特征点点云进行配准,得到相机位姿变换;
步骤2C、对所述对极几何算法计算得到的相机位姿变换和所述通过NDT配准特征点点云得到的相机位姿进行融合,得到相机融合位姿;
本步骤中,对极几何计算出一个相机位姿,NDT配准三位特征点点云计算出一个相机位姿,这两个相机位姿都是客观真实值的估算值。进行融合是将这两个相机位姿进行融合,以此得到一个更加准确的相机位姿估算。
步骤2D、计算当前相机融合位姿与上一关键帧之间的欧拉距离和旋转角度差值,当欧拉距离大于关键帧欧拉距离阈值,或者旋转角度差值大于关键帧旋转角度差值阈值时,将当前融合位姿的图像帧作为关键帧。
步骤2E、根据所述提取的关键帧进行三维点云地图构建,删除非关键帧图像及点云。
本实施例中,作为一种优选示例,计算出ORB特征点的三维空间位置时,根据连续两帧图像中的ORB特征点,首先通过计算ORB特征点描述子之间的汉明距离得到ORB特征点对的相似程度,并通过快速近似最近邻(FLANN)算法加速匹配连续两帧图像中对应的ORB特征点,构成ORB特征点对,同时删除多余的没有完成匹配的ORB特征点。通过对极几何算法计算得到的ORB特征点对数据,得到相机的位姿变换,再通过三角测量算法计算得到ORB特征点的三维空间坐标。就是说,上述步骤2A可以包括:
步骤3A、根据连续两帧图像中的ORB特征点,计算所述ORB特征点描述子之间的汉明距离,确定所述ORB特征点对的相似程度;
步骤3B、根据快速近似最近邻算法匹配连续两帧图像中对应的ORB特征点,构成ORB特征点对,删除没有完成匹配的ORB特征点;
步骤3C、根据对极几何算法处理所述特征点对,得到相机的位姿变换。
步骤3D、根据相机位姿变化,通过三角测量算法计算得到ORB特征点的三维空间坐标。
作为一种优选示例,上述步骤2B中,对特征点点云进行配准时,为了提高配准的精确度,通过NDT算法配准得到的ORB特征点点云,得到相机位姿变换。
融合通过对极几何计算的相机位姿和通过DNT点云匹配计算得到的相机位姿,作为一种优选示例,该融合过程可以通过卡尔曼滤波算法进行。
计算出当前相机位姿与所有关键帧相机位姿点之间的欧拉距离后,判断所述欧拉距离是否小于预设的第一阈值,若小于第一阈值,则计算空间相邻帧之间的相似度。作为一个优选示例,当所述欧拉距离不小于第一阈值时,返回S302。需要说明的是,第一阈值可以是在S301的***初始化时导入的位姿欧拉距离邻近阈值;
作为一种优选示例,关键帧是指相机运动距离超过预设阈值的帧。例如摄像头每秒获取15-30帧图像,相机运动距离超过预设阈值的帧作为关键帧,关键帧用于回环检测和三位地图构建。
作为一种优选示例,计算空间相邻帧之间的相似度之前,先累计欧拉距离邻近的位姿连续点对数量,若连续的欧拉距离邻近位姿点对数量超过预设的数量阈值,则计算空间相邻帧之间的相似度,否则返回S302。
作为一种优选示例,计算空间相邻帧之间的相似度包括:
通过以下公式计算相似度:
其中:
a,b∈RW为W维度的向量,表示两帧图像的BoW描述;
s(a,b)为两帧图像间相似度;
||a-b||1为取L1范数,即各元素绝对值之和。
当两个向量完全一致时,s(a,b)得到1;完全相反时s(a,b)得到0。
作为一种优选示例,当上述空间相邻关键帧之间的相似度s(a,b)大于预设的第二阈值时,进行是否回环的判断。即当s(a,b)大于预设的第二阈值时,判断回环检测是否成功。
需要说明的是,第二阈值可以是S301中***初始化时导入的BoW描述相似度阈值。
作为另一种优选示例,当上述空间相邻帧之间的相似度s(a,b)不大于预设的第二阈值时,返回S301。
作为一种优选示例,上述S305中,进行是否回环的判断包括:通过NDT算法将两帧欧拉距离邻近点云进行配准,进行M次迭代计算,如果配准结果收敛到预设收敛阈值之下,则判定为检测到回环;其中,M是预先设置的,M是大于等于1的整数。
需要说明的是,M可以为S301中***初始化时导入的NDT算法配准迭代次数,预设收敛阈值可以为S301中***初始化时导入的收敛阈值。
作为另一种优选示例,判定为检测到回环时,对相机位姿进行非线性优化计算,优化建图;判定为未检测到回环时,返回S302。对相机位姿进行非线性优化计算,优化建图包括:构建回环约束,对相机位姿进行后端优化计算,优化建图。
作为一种优选示例,本实施例中的后端优化计算是指,检测到回环后,即增加了一个方程约束,将所有方程输入非线性优化器中进行求解,即可优化每个位姿点和特征点空间的位置的解。
下面结合图4,对本实施例的回环检测方法给出一个具体的示例:
S401、对***进行初始化。同S301,在此不再赘述;
S402、通过相机采集图像。同S302,在此不再赘述;
S403、计算ORB特征和描述子,并匹配连续两帧图像之间的特征点。
ORB特征点和描述子的计算,同S303;通过FLANN算法对连续两帧图像进行特征点匹配。
S404、计算ORB特征点的三维空间位置,通过NDT进行相机位姿估计,选取关键帧,构建三维点云地图。
本步骤中,根据连续两帧图像中的ORB特征点,首先通过计算ORB特征点描述子之间的汉明距离得到ORB特征点对的相似程度,并通过快速近似最近邻(FLANN)算法来匹配连续两帧图像中对应的ORB特征点,构成ORB特征点对,同时删除多余的没有完成匹配的ORB特征点。通过对极几何算法处理得到的ORB特征点对数据,得到相机的位姿变换,通过三角测量算法计算得到ORB特征点的三维空间坐标。通过NDT算法配准得到的ORB特征点点云,得到相机位姿变换。通过卡尔曼滤波算法融合相机的第一位姿变换和第二位姿变换,得到相机的第三位姿变换,若第三位姿变换符合关键帧选取要求,则利用该关键帧构建点云地图。
S405、判断欧拉距离是否小于预设的第一阈值,若是则执行S406,否则执行S402。在判断欧拉距离是否小于预设的第一阈值之前,先计算欧拉距离,计算欧拉距离的方法同S305,在此不再赘述;
S406、判断空间相邻帧之间的相似度是否大于预设的第二阈值,若是则执行S407,否则执行S402。在判断空间相邻帧之间的相似度是否大于预设的第二阈值之前,先计算空间相邻帧之间的相似度,计算方法同S305,在此不再赘述;
S407、判断当前帧中点云与地图点云配准是否收敛,若收敛则执行S408,否则执行S402。本步骤中,通过NDT算法将两帧欧拉距离邻近点云进行配准,进行M次迭代计算,如果配准结果收敛到预设收敛阈值之下,则判定为检测到回环。
S408、构建回环约束,优化建图。
通过本实施例的方法,通过摄像头进行图像采集,计算图像ORB特征,计算ORB特征点的空间位置形成三维空间点云,利用NDT配准算法进行相机位姿估计,在相机移动过程中,不断计算相机位姿与历史轨迹的距离,当空间位置小于阈值时,进一步计算当前帧图像中的BoW描述与之空间邻近的图像中BoW描述相似度。当BoW描述相似度大于预设阈值时,用当前帧中ORB特征点点云与点云地图进行配准,若配准收敛则判定为回环。本实施例的方法提高了回环检测的准确度,提高了建图定位的质量。
通过本实施例的方法,通过先验筛查,提供回环检测候选点,再次确认是否回环检测成功,以此提高检测精度。融合两个位姿得到一个更好的相机位姿,这样空间上预估的回环位置会更加准确。再用NDT对回环检测前两步进行最终认定。
实施例二
基于同一个发明构思,本发明实施例还提供了一种回环检测装置,如图5所示,该装置包括:
初始化模块501,被配置用于对***进行初始化;
图像采集模块502,被配置用于采集图像;
特征点匹配模块503,被配置用于提取所述图像中每一帧图像的特征点,并匹配连续两帧图像之间的特征点;
云地图构建模块504,被配置用于构建三维点云地图;
回环判断模块505,被配置用于计算当前相机位姿点与历史相机位姿点之间的欧拉距离;若所述欧拉距离小于预设的第一阈值,则计算空间相邻帧之间的相似度;若所述相似度大于预设的第二阈值,则进行是否回环的判断。
作为一个优选示例,本实施例提供的初始化模块501,对***进行初始化包括:
导入初始化参数和摄像头内部参数;
所述初始化参数包括以下之一或者组合:时序邻近帧数量、位姿欧拉距离邻近阈值、词袋模型描述相似度阈值、正态分布变换算法NDT配准迭代次数和回环收敛阈值。
作为一种优选示例,特征点匹配模块503,被配置用于提取所述图像中每一帧图像的特征点,并匹配连续两帧图像之间的特征点包括:
提取每帧图像的Oriented Fast and Rotated Brief,ORB,特征关键点;
计算所述每帧图像的ORB特征描述子;
通过快速近似最近邻算法对连续两帧图像进行特征点匹配。
作为一种优选示例,云地图构建模块504,还被配置于根据以下步骤构建三维点云地图:
计算出ORB特征点的三维空间位置;
对特征点点云进行配准,得到相机位姿变换;
对所述相机位姿变换和所述三维空间位置进行融合,确定融合位姿;
根据所述融合位姿进行三维点云地图构建。
作为一种优选示例,回环判断模块505,还被配置用于根据以下步骤计算空间相邻帧之间的相似度包括:
计算空间相邻帧之间的描述子BoW相似程度;
其中通过下列公式计算空间相邻帧之间的描述子BoW相似程度:
其中,a,b∈RW为W维度的向量,表示两帧图像的BoW描述;
s(a,b)为两帧图像间相似程度;
||a-b||1为取L1范数,表示各元素绝对值之和。
作为一种优选示例,回环判断模块505,还被配置用于根据以下步骤判断是否回环:
通过NDT算法将两帧欧拉距离邻近点云进行配准,进行M次迭代计算,如果配准结果收敛到预设收敛阈值之下,则判定为检测到回环;
其中,M是预先设置的,M是大于等于1的整数。
作为一种优选示例,本实施例提供的回环检测装置还包括重建模块506,被配置用于对相机位姿进行重新计算,重新建图。
需要说明的是,本实施例提供的初始化模块501,能实现实施例一中步骤S301包含的全部功能,解决相同技术问题,达到相同技术效果,在此不再赘述;
相应的,本实施例提供的图像采集模块502,能实现实施例一中步骤S302包含的全部功能,解决相同技术问题,达到相同技术效果,在此不再赘述;
相应的,本实施例提供的特征点匹配模块503,能实现实施例一中步骤S303包含的全部功能,解决相同技术问题,达到相同技术效果,在此不再赘述;
相应的,本实施例提供的云地图构建模块504,能实现实施例一中步骤S304包含的全部功能,解决相同技术问题,达到相同技术效果,在此不再赘述;
相应的,本实施例提供的回环判断模块505,能实现实施例一中步骤S305包含的全部功能,解决相同技术问题,达到相同技术效果,在此不再赘述;
需要说明的是,实施例二提供的装置与实施例一提供的方法属于同一个发明构思,解决相同的技术问题,达到相同的技术效果,实施例二提供的装置能实现实施例一的所有方法,相同之处不再赘述。
实施例三
基于同一个发明构思,本发明实施例还提供了一种计算机设备,如图6所示,该装置包括:
包括存储器602、处理器601和用户接口603;
所述存储器602,用于存储计算机程序;
所述用户接口603,用于与用户实现交互;
所述处理器601,用于读取所述存储器602中的计算机程序,所述处理器601执行所述计算机程序时,实现:
对***进行初始化;
通过相机采集图像;
提取所述图像中每一帧图像的特征点,并匹配连续两帧图像之间的特征点;
构建三维点云地图;
计算当前相机位姿点与历史相机位姿点之间的欧拉距离,若所述欧拉距离小于预设的第一阈值,则计算空间相邻帧之间的相似度,若所述相似度大于预设的第二阈值,则进行是否回环的判断。
其中,在图6中,总线架构可以包括任意数量的互联的总线和桥,具体由处理器601代表的一个或多个处理器和存储器602代表的存储器的各种电路链接在一起。总线架构还可以将诸如***设备、稳压器和功率管理电路等之类的各种其他电路链接在一起,这些都是本领域所公知的,因此,本文不再对其进行进一步描述。总线接口提供接口。处理器601负责管理总线架构和通常的处理,存储器602可以存储处理器601在执行操作时所使用的数据。
处理器601可以是CPU、ASIC、FPGA或CPLD,处理器601也可以采用多核架构。
处理器601执行存储器602存储的计算机程序时,实现实施例一中的任一回环检测方法。
需要说明的是,实施例三提供的装置与实施例一提供的方法属于同一个发明构思,解决相同的技术问题,达到相同的技术效果,实施例三提供的装置能实现实施例一的所有方法,相同之处不再赘述。
本申请还提出一种处理器可读存储介质。其中,该处理器可读存储介质存储有计算机程序,所述处理器执行所述计算机程序时实现实施例一中的任一回环检测方法。
需要说明的是,本申请实施例中对单元的划分是示意性的,仅仅为一种逻辑功能划分,实际实现时可以有另外的划分方式。另外,在本申请各个实施例中的各功能单元可以集成在一个处理单元中,也可以是各个单元单独物理存在,也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现,也可以采用软件功能单元的形式实现。
本领域内的技术人员应明白,本申请的实施例可提供为方法、***、或计算机程序产品。因此,本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且,本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器和光学存储器等)上实施的计算机程序产品的形式。
本申请是参照根据本申请实施例的方法、设备(***)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器,使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中,使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品,该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
显然,本领域的技术人员可以对本申请进行各种改动和变型而不脱离本申请的精神和范围。这样,倘若本申请的这些修改和变型属于本申请权利要求及其等同技术的范围之内,则本申请也意图包含这些改动和变型在内。
Claims (13)
1.一种回环检测方法,其特征在于,包括:
对***进行初始化;
通过相机采集图像;
提取所述图像中每一帧图像的特征点,并匹配连续两帧图像之间的特征点;
构建三维点云地图;
计算当前相机位姿点与历史相机位姿点之间的欧拉距离,若所述欧拉距离小于预设的第一阈值,则计算空间相邻帧之间的相似度,若所述相似度大于预设的第二阈值,则进行是否回环的判断。
2.根据权利要求1所述的方法,其特征在于,所述对***进行初始化包括:
导入初始化参数和摄像头内部参数;
所述初始化参数包括以下之一或者组合:关键帧欧拉距离阈值、关键帧旋转角度差值阈值、位姿欧拉距离邻近阈值、词袋模型描述相似度阈值、正态分布变换算法NDT配准迭代次数和回环收敛阈值。
3.根据权利要求1所述的方法,其特征在于,所述提取所述图像中每一帧图像的特征点,并匹配连续两帧图像之间的特征点包括:
提取每帧图像的Oriented Fast and Rotated Brief,ORB,特征关键点;
计算所述每帧图像的ORB特征描述子;
通过快速近似最近邻算法对连续两帧图像进行特征点匹配。
4.根据权利要求1所述的方法,其特征在于,所述构建三维点云地图包括:
用三角测量算法计算出ORB特征点的三维空间位置;
对特征点点云进行配准,得到相机位姿变换;
对极几何算法计算得到的相机位姿变换和通过NDT配准特征点点云得到的相机位姿进行融合,得到相机融合位姿;
计算当前相机融合位姿与上一关键帧之间的欧拉距离和旋转角度差值,当欧拉距离大于关键帧欧拉距离阈值,或者旋转角度差值大于关键帧旋转角度差值阈值时,将当前融合位姿的图像帧作为关键帧;
根据所述提取的关键帧进行三维点云地图构建,删除非关键帧图像及点云。
5.根据权利要求4所述的方法,其特征在于,所述计算出ORB特征点的三维空间位置包括:
根据连续两帧图像中的ORB特征点,计算所述ORB特征点描述子之间的汉明距离,确定所述ORB特征点对的相似程度;
根据快速近似最近邻算法匹配连续两帧图像中对应的ORB特征点,构成ORB特征点对,删除没有完成匹配的ORB特征点;
根据对极几何算法处理所述特征点对,得到相机的位姿变换;
根据相机位姿变化,通过三角测量算法计算得到ORB特征点的三维空间坐标。
6.根据权利要求4所述的方法,其特征在于,所述对特征点点云进行配准包括:
通过正态分布变换算法NDT配准所述ORB特征点点云,得到相机位姿变换。
7.根据权利要求4所述的方法,其特征在于,所述对所述相机位姿变换和所述三维空间位置进行融合,确定融合位姿包括:
通过卡尔曼滤波算法对所述通过对极几何算法得到的相机位姿变换和通过NDT点云配准算法得到的相机位姿变换进行融合,确定融合位姿。
8.根据权利要求1所述的方法,其特征在于,所述计算当前相机位姿点与历史相机位姿点之间的欧拉距离包括:
排除时序上邻近的N个位姿点;
计算当前相机位姿与历史相机位姿点之间的欧拉距离;
其中,N为预设的数值,N是大于等于1的整数。
10.根据权利要求1所述的方法,其特征在于,所述进行是否回环的判断包括:
通过NDT算法将两帧欧拉距离邻近点云进行配准,进行M次迭代计算,如果配准结果收敛到预设收敛阈值之下,则判定为检测到回环;
其中,M是预先设置的,M是大于等于1的整数。
11.根据权利要求10所述的方法,其特征在于,判定为检测到回环之后还包括:
对相机位姿进行重新计算,重新建图。
12.一种回环检测装置,其特征在于,包括:
初始化模块,被配置用于对***进行初始化;
图像采集模块,被配置用于采集图像;
特征点匹配模块,被配置用于提取所述图像中每一帧图像的特征点,并匹配连续两帧图像之间的特征点;
云地图构建模块,被配置用于构建三维点云地图;
回环判断模块,被配置用于计算当前相机位姿点与历史相机位姿点之间的欧拉距离;若所述欧拉距离小于预设的第一阈值,则计算空间相邻帧之间的相似度;若所述相似度大于预设的第二阈值,则进行是否回环的判断。
13.一种计算机设备,其特征在于,包括存储器、处理器和用户接口;
所述存储器,用于存储计算机程序;
所述用户接口,用于与用户实现交互;
所述处理器,用于读取所述存储器中的计算机程序,所述处理器执行所述计算机程序时,实现如权利要求1到11之一所述的回环检测方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110256945.XA CN113160130A (zh) | 2021-03-09 | 2021-03-09 | 一种回环检测方法、装置及计算机设备 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110256945.XA CN113160130A (zh) | 2021-03-09 | 2021-03-09 | 一种回环检测方法、装置及计算机设备 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN113160130A true CN113160130A (zh) | 2021-07-23 |
Family
ID=76886526
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110256945.XA Pending CN113160130A (zh) | 2021-03-09 | 2021-03-09 | 一种回环检测方法、装置及计算机设备 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113160130A (zh) |
Cited By (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113536024A (zh) * | 2021-08-11 | 2021-10-22 | 重庆大学 | 一种基于fpga的orb_slam重定位特征点检索加速方法 |
CN113625249A (zh) * | 2021-07-30 | 2021-11-09 | 深圳市优必选科技股份有限公司 | 反光板定位方法、机器人及计算机可读存储介质 |
CN113744236A (zh) * | 2021-08-30 | 2021-12-03 | 阿里巴巴达摩院(杭州)科技有限公司 | 回环检测方法、装置、存储介质及计算机程序产品 |
CN113808270A (zh) * | 2021-09-28 | 2021-12-17 | 中国科学技术大学先进技术研究院 | 基于网联的无人驾驶测试环境地图搭建方法及*** |
CN115601433A (zh) * | 2022-12-12 | 2023-01-13 | 安徽蔚来智驾科技有限公司(Cn) | 回环检测方法、计算机设备、计算机可读存储介质及车辆 |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104537709A (zh) * | 2014-12-15 | 2015-04-22 | 西北工业大学 | 一种基于位姿变化的实时三维重建关键帧确定方法 |
CN108682027A (zh) * | 2018-05-11 | 2018-10-19 | 北京华捷艾米科技有限公司 | 基于点、线特征融合的vSLAM实现方法及*** |
CN109816769A (zh) * | 2017-11-21 | 2019-05-28 | 深圳市优必选科技有限公司 | 基于深度相机的场景地图生成方法、装置及设备 |
CN109848996A (zh) * | 2019-03-19 | 2019-06-07 | 西安交通大学 | 基于图优化理论的大规模三维环境地图创建方法 |
CN110322511A (zh) * | 2019-06-28 | 2019-10-11 | 华中科技大学 | 一种基于物体和平面特征的语义slam方法和*** |
CN112258647A (zh) * | 2020-11-12 | 2021-01-22 | Oppo广东移动通信有限公司 | 地图重建方法及装置、计算机可读介质和电子设备 |
CN112254729A (zh) * | 2020-10-09 | 2021-01-22 | 北京理工大学 | 一种基于多传感器融合的移动机器人定位方法 |
-
2021
- 2021-03-09 CN CN202110256945.XA patent/CN113160130A/zh active Pending
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104537709A (zh) * | 2014-12-15 | 2015-04-22 | 西北工业大学 | 一种基于位姿变化的实时三维重建关键帧确定方法 |
CN109816769A (zh) * | 2017-11-21 | 2019-05-28 | 深圳市优必选科技有限公司 | 基于深度相机的场景地图生成方法、装置及设备 |
CN108682027A (zh) * | 2018-05-11 | 2018-10-19 | 北京华捷艾米科技有限公司 | 基于点、线特征融合的vSLAM实现方法及*** |
CN109848996A (zh) * | 2019-03-19 | 2019-06-07 | 西安交通大学 | 基于图优化理论的大规模三维环境地图创建方法 |
CN110322511A (zh) * | 2019-06-28 | 2019-10-11 | 华中科技大学 | 一种基于物体和平面特征的语义slam方法和*** |
CN112254729A (zh) * | 2020-10-09 | 2021-01-22 | 北京理工大学 | 一种基于多传感器融合的移动机器人定位方法 |
CN112258647A (zh) * | 2020-11-12 | 2021-01-22 | Oppo广东移动通信有限公司 | 地图重建方法及装置、计算机可读介质和电子设备 |
Non-Patent Citations (3)
Title |
---|
徐彬彬,刘鹏远,张峻宁: "《基于特征地图的改进回环检测算法》", 《兵工自动化》 * |
李同,张奇志: "《基于ORB词袋模型的SLAM回环检测研究", 《信息通信》 * |
柴梦娜,刘元盛,任丽军: "《基于激光点云NDT特征的两步回环检测》", 《激光与红外》 * |
Cited By (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113625249A (zh) * | 2021-07-30 | 2021-11-09 | 深圳市优必选科技股份有限公司 | 反光板定位方法、机器人及计算机可读存储介质 |
WO2023005020A1 (zh) * | 2021-07-30 | 2023-02-02 | 深圳市优必选科技股份有限公司 | 反光板定位方法、机器人及计算机可读存储介质 |
CN113536024A (zh) * | 2021-08-11 | 2021-10-22 | 重庆大学 | 一种基于fpga的orb_slam重定位特征点检索加速方法 |
CN113744236A (zh) * | 2021-08-30 | 2021-12-03 | 阿里巴巴达摩院(杭州)科技有限公司 | 回环检测方法、装置、存储介质及计算机程序产品 |
CN113744236B (zh) * | 2021-08-30 | 2024-05-24 | 阿里巴巴达摩院(杭州)科技有限公司 | 回环检测方法、装置、存储介质及计算机程序产品 |
CN113808270A (zh) * | 2021-09-28 | 2021-12-17 | 中国科学技术大学先进技术研究院 | 基于网联的无人驾驶测试环境地图搭建方法及*** |
CN113808270B (zh) * | 2021-09-28 | 2023-07-21 | 中国科学技术大学先进技术研究院 | 基于网联的无人驾驶测试环境地图搭建方法及*** |
CN115601433A (zh) * | 2022-12-12 | 2023-01-13 | 安徽蔚来智驾科技有限公司(Cn) | 回环检测方法、计算机设备、计算机可读存储介质及车辆 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110335316B (zh) | 基于深度信息的位姿确定方法、装置、介质与电子设备 | |
CN113160130A (zh) | 一种回环检测方法、装置及计算机设备 | |
KR101849373B1 (ko) | 인체의 관절구조를 추정하기 위한 장치 및 방법 | |
KR101532864B1 (ko) | 모바일 디바이스들에 대한 평면 맵핑 및 트래킹 | |
US8792726B2 (en) | Geometric feature extracting device, geometric feature extracting method, storage medium, three-dimensional measurement apparatus, and object recognition apparatus | |
CN110135455A (zh) | 影像匹配方法、装置及计算机可读存储介质 | |
KR102367361B1 (ko) | 위치 측정 및 동시 지도화 방법 및 장치 | |
JP2019075082A (ja) | 深度値推定を用いた映像処理方法及び装置 | |
WO2016210227A1 (en) | Aligning 3d point clouds using loop closures | |
CN109241844B (zh) | 三维物体的姿态估计方法、装置、设备及存储介质 | |
CN109974721A (zh) | 一种基于高精度地图的视觉回环检测方法和装置 | |
CN114332415B (zh) | 基于多视角技术的输电线路廊道的三维重建方法及装置 | |
CN111833447A (zh) | 三维地图构建方法、三维地图构建装置及终端设备 | |
CN111445526A (zh) | 一种图像帧之间位姿的估计方法、估计装置和存储介质 | |
Li et al. | Co-planar parametrization for stereo-SLAM and visual-inertial odometry | |
Schönberger et al. | Structure-from-motion for MAV image sequence analysis with photogrammetric applications | |
Shiratori et al. | Efficient large-scale point cloud registration using loop closures | |
Nousias et al. | Large-scale, metric structure from motion for unordered light fields | |
JP2006252275A (ja) | カメラ運動及び物体形状の復元システム | |
CN112270748B (zh) | 基于图像的三维重建方法及装置 | |
CN112258647B (zh) | 地图重建方法及装置、计算机可读介质和电子设备 | |
Liao | SLAMORE: SLAM with object recognition for 3D radio environment reconstruction | |
CN115239776A (zh) | 点云的配准方法、装置、设备和介质 | |
Djordjevic et al. | An accurate method for 3D object reconstruction from unordered sparse views | |
Romanoni et al. | Efficient moving point handling for incremental 3d manifold reconstruction |
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 | ||
TA01 | Transfer of patent application right | ||
TA01 | Transfer of patent application right |
Effective date of registration: 20220801 Address after: 100191 No. 37, Haidian District, Beijing, Xueyuan Road Applicant after: BEIHANG University Applicant after: Beijing Lobby Technology Co.,Ltd. Address before: 100191 No. 37, Haidian District, Beijing, Xueyuan Road Applicant before: BEIHANG University |
|
RJ01 | Rejection of invention patent application after publication | ||
RJ01 | Rejection of invention patent application after publication |
Application publication date: 20210723 |