CN108133496B - 一种基于g2o与随机蕨类算法的稠密地图创建方法 - Google Patents

一种基于g2o与随机蕨类算法的稠密地图创建方法 Download PDF

Info

Publication number
CN108133496B
CN108133496B CN201711404798.6A CN201711404798A CN108133496B CN 108133496 B CN108133496 B CN 108133496B CN 201711404798 A CN201711404798 A CN 201711404798A CN 108133496 B CN108133496 B CN 108133496B
Authority
CN
China
Prior art keywords
model
loop
ferns
camera
random
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
Application number
CN201711404798.6A
Other languages
English (en)
Other versions
CN108133496A (zh
Inventor
贾松敏
李柏杨
张国梁
李秀智
张祥银
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing University of Technology
Original Assignee
Beijing University of Technology
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Beijing University of Technology filed Critical Beijing University of Technology
Priority to CN201711404798.6A priority Critical patent/CN108133496B/zh
Publication of CN108133496A publication Critical patent/CN108133496A/zh
Application granted granted Critical
Publication of CN108133496B publication Critical patent/CN108133496B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G06T7/75—Determining position or orientation of objects or cameras using feature-based methods involving models
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
    • 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
    • G06T7/344—Determination of transform parameters for the alignment of images, i.e. image registration using feature-based methods involving models

Abstract

一种基于g2o与随机蕨类算法的稠密地图创建方法属于机器人实时定位与地图创建领域。首先，构筑相机位姿模型和加权融合3D点截断信息的TSDF模型，用于准确表示创建物体的表面。其次，提出一种改进的回环检测方法，并将其与随机蕨类彩色图像编码化策略相结合，进而优化TSDF模型。最后，使用g2o图优化库解算约束函数，建立数据集间的优化边。实验结果表明，混合优化位姿模型可以较快建立全局SLAM地图，并有效识别曾到达区域。

Description

一种基于g2o与随机蕨类算法的稠密地图创建方法
技术领域
本发明属于机器人实时定位与地图创建领域。使用g2o优化局部及全局回环检测任务，利用随机蕨类算法进行闭环匹配，构建TSDF(Truncated Signed DistanceFunction，TSDF)模型。
背景技术
本发明主要解决室内环境下，采用视觉传感器的机器人V-SLAM(Vision-onlySimultaneous Localization and Mapping，V-SLAM)问题。SLAM的主要难点在于，精确的地图创建依赖于对机器人位置的准确估计，而这个估计又反之来自传感器对外界的感知，即地图中的标志物(Landmark)。因此，“定位”和“建图”作为两个相互耦合，相互依存的问题，容易受到彼此噪声的影响。此外，SLAM过程中出现的常见现象如数据关联(DataAssociation)，回环检测(Loop Closuredetection)，全局优化(Global Optimization)等，也是研究者们广泛关注的重要问题。另一方面，随着传感器及处理器技术的发展，求解定位、建图子问题的方法也越来越丰富，横跨计算机视觉、传感器设计、运筹学等多个领域，使得SLAM日渐成为一个极其综合、复杂的研究课题，也是智能机器人技术的一个重要代表性技术。闭环检测(Loop Closure Detection)是指机器人识别曾到达场景的能力。如果检测成功，可以显著地减小累积误差。基于EKF的SLAM高度依赖正确的数据关联，这可视为一种针对路标的闭环检测。而对于姿态图，由闭环检测带来的额外约束，也可以使优化算法得到一致性更强的结果。然而，回环检测的难点在于错误的检测结果将会对地图创建的效果产生较大影响。
基于上述研究现状，本发明提出了一种鲁棒性强、精度高，且能识别曾到达区域的V-SLAM方法。由于原始TSDF模型仅考虑相邻时间上的关联，误差将不可避免的累积到下一时刻，无法构建全局一致的地图。为了实时精确的建立大场景稠密3D地图，对TSDF模型进行了改进。首先，构筑相机位姿模型和加权融合3D点截断信息的TSDF模型，用于准确表示创建物体的表面。其次，提出一种改进的回环检测方法，并将其与随机蕨类彩色图像编码化策略相结合，进而优化TSDF模型。最后，使用g2o图优化库解算约束函数，建立数据集间的优化边。实验结果表明，混合优化位姿模型可以较快建立全局SLAM地图，并有效识别曾到达区域。
发明内容
本发明的目的是设计并实现一种鲁棒性强、精度高的V-SLAM方法。
本发明采用如下技术方案：
本发明提出了模型对模型配准的回环优化匹配，与基于随机蕨类的闭环检测相结合的算法。所提模型对模型配准算法既减少了系统运算量，又增加了机器人位姿的校正精度。通过g2o构造模型对模型的匹配策略，优化迭代效率。关键帧图像使用随机蕨类进行编码化处理，进行相似度打分，比较当前帧与历史帧之间的相似程度。最后，构造回环检测模型实现全局SLAM匹配，并在GPU上实时使用随机蕨类对采集图像进行检测，完成闭环检测。基于随机蕨类的闭环检测可以识别曾到达区域，消除点云模型的重影与不匹配现象，进而创建精度更高的V-SLAM系统，进一步优化用户体验，提高系统鲁棒性。本文算法在TUM数据集中的fr1/xyz、fr1/room、fr1/desk上均取得较好的效果。对比同样使用TSDF模型的KinectFusion，三个数据集的均方根误差分别减少0.59cm、3.14cm、0.94cm。特别在较大环境的fr1/room数据集下，均方根误差减少了31.2％，由此可见本文提出的混合位姿模型优化的TSDF系统在准确度和鲁棒性上均取得更好的效果。
本发明的技术特征如下：
(1)使用g2o构建模型对模型配准算法
传统EKF(Extended Kalman Filter，EKF)滤波虽然对系统观测方程和运动方程进行了泰勒展开，但EKF滤波有很大的局限性。只考虑了上一状态和当前状态的关系，每个状态之间的位姿估计存在一定的误差，当机器人运行轨迹很长时则会导致较大的累计误差。本方法使用g2o对机器人状态进行图优化，不仅使用前一状态，更会把历史数据加入状态估计，在同等计算量的情况下，非线性优化会取得更好的效果。若只考虑Kinect相机得到的观测数据，使用李代数对相机位姿和路标点进行转换，以便于进行无约束优化。通过g2o构造迭代函数求得误差最小的观测状态，使用GPU并行处理ICP匹配投影算法，构造模型对模型的匹配策略。使用g2o构建模型对模型配准算法具体包括以下4个步骤：
步骤一，使用g2o构造代价函数，使用李代数对相机位姿和路标点进行转换，以便于进行无约束优化。观测误差e为：
e＝z-h(ξ,p) (1)
其中z为相机观测数据，ξ，p为李代数表示的相机参数与位姿，函数h为观测方程。把关键状态的观测量加入，得到代价函数为：
Figure GDA0003311322650000031
其中m，n为对应矩阵的行坐标与列坐标，i，j分别为由1到m和n的数值，eij，zij，pij为观测误差，相机观测数据与位姿矩阵中对应坐标的变量。
步骤二，使用ICP进行投影算法匹配。使用g2o进行回环检测并优化时，对邻近的关键帧和历史数据集中的关键帧进行匹配优化校正全局地图。其中关键帧之间的匹配方式为ICP匹配，将图像上的点根据相机位姿投影在2D平面上的对应点，通过对应点的欧氏距离，法向量夹角计算和优化相机位姿矩阵。本发明使用李代数表示位姿矩阵，将该问题转变为无约束优化问题。
步骤三，根据步骤一的代价函数和步骤二的位姿矩阵获取方法，使用g2o构造回环模型。传统构造回环的方法，对关键帧的选取策略是随机的、无序的，建立帧与帧之间的数据约束时会发生帧与帧之间约束过密，造成运算量大，降低系统实时性；或者只有邻近帧之间建立约束，造成旋转平移矩阵校正效果不佳，出现累积误差。在多次实验测试后，选定4到7个帧为一个数据模型，建立模型与模型之间的约束关系，这样既减少了运算量又增加了校正精度。并在模型与模型匹配时加入随机的局部回环和大回环配准，保证全局SLAM的建立。
步骤四，由步骤三构造的模型，设计模型与模型之间的匹配迭代算法如下所示,f(xk)为模型与模型间的匹配函数，J(xk)为f(xk)的雅可比矩阵，xk为相机参数和相机坐标组成的向量，其中λ为拉格朗日乘子，“Δxk”表示相机参数和相机坐标组成的向量，在k时刻到k+1时刻中变化的向量。将算法转化为无约束优化问题，μ为Δx增量半径，D为非负数对角阵，可已将增量区域变更为更加合理的椭圆形区域，变量ρ的由公式4得到，MaxTHρ为变量ρ近似可行的阈值，本方法中设定为0.9。
Figure GDA0003311322650000041
Figure GDA0003311322650000042
在迭代过程中，如果ρ>3/4那么μ←2μ；如果ρ<1/4那么μ←0.5μ；如果ρ>MaxTHρ那么xk+1←xk+Δxk；不断迭代直到f(xk)收敛时，xk的最优值。
(2)使用随机蕨类进行识别曾到达区域
只使用g2o构造如上模型对模型的回环优化时，并不能有效的识别曾到达区域，完成最终的全局闭环。在反复建立某一局部区域或者回到曾经到达区域时，会出现重投影模型不匹配导致的重叠和错位的现象。为了解决上述问题，在回环优化模型中使用随机蕨类组成混合回环优化模型。
步骤一，使用如公式(5)所示的方式，编码化关键帧。每幅图像Q的编码由a个block组成，每个block由b个Ferns组成，k，l分别为由1到a和b的数值。
Figure GDA0003311322650000043
步骤二，每个Ferns的编码值，是由通道c上像素点u的灰度值θ来确定。如公式(6)所示，对关键帧进行编码化处理，其中对于一个新获取的关键帧Q，分别比较归一化的I(θ)与阈值τ(本方法中，阈值τ设定为0.45)来计算每一个block里的Ferns列表中的值。
Figure GDA0003311322650000051
步骤三，当新获取一个关键帧Q时，对Q进行步骤二同样的编码化处理，并比对临近关键帧和历史关键帧。通过对每一个Ferns的比对，计算两帧之间的相似度。
步骤四，对于图像上的像素点进行如公式(6)的判断，若相同则相似度加1，通过相似度判别，决定当前关键帧是否加入回环(与邻近关键帧相似度大于一定阈值，本方法设定为450)，或者存在回环(和历史关键帧相似度大于一定阈值，本方法设定为450)，通过和相似度大的图像配准，进行重定位。
附图说明
图1采用混合回环检测与闭环优化的TSDF地图创建流程图
图2回环模型配准试验示意图
(a)实验环境 (b)使用回环模型配准 (c)没有回环模型配准
图3随机蕨类识别曾到达区域示意图
(a)对实验桌建图 (b)向左增加建图区域
(c)向右增加建图区域 (d)回到曾到达区域
具体实施方式
下面结合附图和具体实施方式对本发明作进一步说明。
本发明基于Kinect传感器，通过地图创建算法对实验环境进行数据采集，得到相应的深度图和彩色图。对深度图进行优化并由采集得到的数据信息，计算旋转平移矩阵进而创建相机位姿模型。通过g2o构造迭代函数求得误差最小的观测状态，使用GPU并行处理ICP匹配投影算法，构造模型对模型的匹配策略，利用随机蕨类进行闭环匹配。建立TSDF模型表示三维物体表面并进行多帧融合。所涉及的方法整体流程如附图1所示，具体实施过程分为以下步骤：
步骤一，使用Kinect传感器对实验环境进行数据采集，得到深度图和彩色图。
步骤二，对深度图进行优化并由采集得到的数据信息建立相机位姿模型。
步骤三，使用TSDF模型融合深度数据，三维空间的深度数据被TSDF模型划分成三维像素，每个三维像素中存储了相应的数值。
步骤四，建立TSDF更新策略，使用GPU更新网格中的值，每个线程更新一串网格值。
步骤五，使用g2o构造代价函数，使用李代数对相机位姿和路标点进行转换，以便于进行无约束优化。
步骤六，使用ICP进行投影算法匹配，优化关键帧间的匹配误差。
步骤七，根据步骤五的代价函数和步骤六的位姿矩阵获取方法，选定4到7个帧为一个数据模型，建立模型与模型之间的约束关系，并在模型与模型匹配时加入随机的局部回环和大回环配准，保证全局SLAM的建立。
步骤八，结合步骤七，使用随即蕨类对关键帧进行编码化处理，快速计算两帧间的相似度，决定当前关键帧是否加入回环，识别曾到达区域，优化系统闭环误差。
步骤九，把优化后的系统数据不断进行TSDF模型融合，输出三维稠密地图并显示。
下面为本发明的在地图创建任务中的应用实例。
在实验室环境下，使用移动机器人携带Kinect传感器在实验室中运行并建立三维地图，其中机器人速度为0.15m/s，角速度为0.15rad/s，图像频率为30Hz。
说明书附图2(b)和(c)是由g2o_viewer建立的机器人姿态图，可看出是否使用基于g2o混合位姿模型方法的效果差异。从附图2中可以看出，使用混合位姿模型可以增加当前机器人位姿与历史机器人位姿之间的联系，增加局部回环和大回环约束。通过这种方式可以有效消除系统的累积误差，充分利用相机得到的数据，校正机器人姿态，提高系统的鲁棒性。
利用随机蕨类混合回环检测模型进行三维地图创建，基于Kinect摄像头验证局部回环的可行性，并且实现对重复到达区域的有效识别。如附图3(a)所示，对实验桌进行三维重建，然后分别向左和向右旋转摄像头，增加建图区域(如图3(b)和(c)所示)。最后，向后拉伸摄像头，可见反复创建的方框区域并没有出现明显的重影和误差，表明系统可以识别出曾到达区域，并且与右方区域建立了较完整的拼接，如图3(d)所示。

Claims (1)

1.一种基于g2o与随机蕨类算法的稠密地图创建方法，其特征在于，包括以下步骤：
(1)使用g2o构建模型对模型配准算法
具体包括以下4个步骤：
使用g2o构建模型对模型配准算法步骤一，使用g2o构造代价函数，使用李代数对相机位姿和路标点进行转换，观测误差e为：
e＝z-h(ξ,p) (1)
其中z为相机观测数据，ξ，p为李代数表示的相机参数与位姿，函数h为观测方程；把关键状态的观测量加入，得到代价函数为：
Figure FDA0003311322640000011
其中m，n为对应矩阵的行坐标与列坐标，i，j分别为由1到m和n的数值，eij，zij，pij为观测误差，相机观测数据与位姿矩阵中对应坐标的变量；
使用g2o构建模型对模型配准算法步骤二，使用g2o进行回环检测并优化时，对邻近的关键帧和历史数据集中的关键帧进行匹配优化校正全局地图；其中关键帧之间的匹配方式为ICP匹配，将图像上的点根据相机位姿投影在2D平面上的对应点，通过对应点的欧氏距离，法向量夹角计算和优化相机位姿矩阵；
使用g2o构建模型对模型配准算法步骤三，根据使用g2o构建模型对模型配准算法步骤一的代价函数和使用g2o构建模型对模型配准算法步骤二的位姿矩阵获取方法，使用g2o构造回环模型；选定4到7个帧为一个数据模型，建立模型与模型之间的约束关系，并在模型与模型匹配时加入随机的局部回环和大回环配准，保证全局SLAM的建立；
使用g2o构建模型对模型配准算法步骤四，由使用g2o构建模型对模型配准算法步骤三构造的模型，设计模型与模型之间的匹配迭代算法如下所示,f(xk)为模型与模型间的匹配函数，J(xk)为f(xk)的雅可比矩阵，xk为相机参数和相机坐标组成的向量，其中λ为拉格朗日乘子，“Δxk”表示相机参数和相机坐标组成的向量，在k时刻到k+1时刻中变化的向量；将算法转化为无约束优化问题，μ为Δx增量半径，D为非负数对角阵，变量ρ的由公式(4)得到，MaxTHρ为变量ρ近似可行的阈值，设定为0.9；
Figure FDA0003311322640000021
Figure FDA0003311322640000022
在迭代过程中，如果ρ>3/4那么μ←2μ；如果ρ<1/4那么μ←0.5μ；如果ρ>MaxTHρ那么xk+1←xk+Δxk；不断迭代直到f(xk)收敛时，xk的最优值；
(2)使用随机蕨类进行识别曾到达区域
使用随机蕨类进行识别曾到达区域的步骤一，使用如公式(5)所示的方式，编码化关键帧；每幅图像Q的编码由a个block组成，每个block由b个Ferns组成，k，l分别为由1到a和b的数值；
Figure FDA0003311322640000023
使用随机蕨类进行识别曾到达区域的步骤二，每个Ferns的编码值，是由通道c上像素点u的灰度值θ来确定；如公式(6)所示，对关键帧进行编码化处理，其中对于一个新获取的关键帧Q，分别比较归一化的I(θ)与阈值τ来计算每一个block里的Ferns列表中的值；阈值τ设定为0.45；
Figure FDA0003311322640000024
使用随机蕨类进行识别曾到达区域的步骤三，当新获取一个关键帧Q时，对Q进行使用随机蕨类进行识别曾到达区域的使用随机蕨类进行识别曾到达区域的步骤二同样的编码化处理，并比对临近关键帧和历史关键帧；通过对每一个Ferns的比对，计算两帧之间的相似度；
使用随机蕨类进行识别曾到达区域的步骤四，对于图像上的像素点进行如公式(6)的判断，若相同则相似度加1，通过相似度判别，决定当前关键帧是否加入回环，或者存在回环，如果是则认为当前关键帧相似度大，通过和相似度大的图像配准，进行重定位。
CN201711404798.6A 2017-12-22 2017-12-22 一种基于g2o与随机蕨类算法的稠密地图创建方法 Active CN108133496B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201711404798.6A CN108133496B (zh) 2017-12-22 2017-12-22 一种基于g2o与随机蕨类算法的稠密地图创建方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201711404798.6A CN108133496B (zh) 2017-12-22 2017-12-22 一种基于g2o与随机蕨类算法的稠密地图创建方法

Publications (2)

Publication Number Publication Date
CN108133496A CN108133496A (zh) 2018-06-08
CN108133496B true CN108133496B (zh) 2021-11-26

Family

ID=62392225

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201711404798.6A Active CN108133496B (zh) 2017-12-22 2017-12-22 一种基于g2o与随机蕨类算法的稠密地图创建方法

Country Status (1)

Country Link
CN (1) CN108133496B (zh)

Families Citing this family (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109325979B (zh) * 2018-07-20 2021-11-02 上海理工大学 基于深度学习的机器人回环检测方法
CN110322511B (zh) * 2019-06-28 2021-03-26 华中科技大学 一种基于物体和平面特征的语义slam方法和系统
CN112214629B (zh) * 2019-07-12 2024-01-26 珠海格力电器股份有限公司 基于图像识别的回环检测方法及可移动设备
CN110866952A (zh) * 2019-10-17 2020-03-06 杭州电子科技大学 一种基于深度图编码的重定位与回环检测方法
CN110807818A (zh) * 2019-10-29 2020-02-18 北京影谱科技股份有限公司 基于关键帧的rgb-dslam方法和装置
CN111862162B (zh) * 2020-07-31 2021-06-11 湖北亿咖通科技有限公司 回环检测方法及系统、可读存储介质、电子设备
CN113808268B (zh) * 2021-09-17 2023-09-26 浙江大学 一种低带宽全局一致的多机稠密建图方法

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9741140B2 (en) * 2014-05-19 2017-08-22 Microsoft Technology Licensing, Llc Fast solving for loop closure using a relative state space
CN104732518B (zh) * 2015-01-19 2017-09-01 北京工业大学 一种基于智能机器人地面特征的ptam改进方法
US9892552B2 (en) * 2015-12-15 2018-02-13 Samsung Electronics Co., Ltd. Method and apparatus for creating 3-dimensional model using volumetric closest point approach
CN106127739B (zh) * 2016-06-16 2021-04-27 华东交通大学 一种结合单目视觉的rgb-d slam方法
CN106296812B (zh) * 2016-08-18 2019-04-02 宁波傲视智绘光电科技有限公司 同步定位与建图方法
CN106910242B (zh) * 2017-01-23 2020-02-28 中国科学院自动化研究所 基于深度相机进行室内完整场景三维重建的方法及系统

Also Published As

Publication number Publication date
CN108133496A (zh) 2018-06-08

Similar Documents

Publication Publication Date Title
CN108133496B (zh) 一种基于g2o与随机蕨类算法的稠密地图创建方法
Johnson et al. Registration and integration of textured 3D data
Kaess Simultaneous localization and mapping with infinite planes
CN103400409B (zh) 一种基于摄像头姿态快速估计的覆盖范围3d可视化方法
Liu et al. Indoor localization and visualization using a human-operated backpack system
US20190026943A1 (en) Dense visual slam with probabilistic surfel map
CN103247075B (zh) 基于变分机制的室内环境三维重建方法
CN108682027A (zh) 基于点、线特征融合的vSLAM实现方法及系统
WO2019029099A1 (zh) 基于图像梯度联合优化的双目视觉里程计算方法
CN106296693A (zh) 基于3d点云fpfh特征实时三维空间定位方法
US20140105486A1 (en) Method for locating a camera and for 3d reconstruction in a partially known environment
CN103646391A (zh) 一种针对动态变化场景的实时摄像机跟踪方法
CN103106688A (zh) 基于双层配准方法的室内三维场景重建方法
Yang The study and improvement of Augmented reality based on feature matching
CN104616286A (zh) 快速的半自动多视图深度修复方法
Li et al. Leveraging planar regularities for point line visual-inertial odometry
CN110260866A (zh) 一种基于视觉传感器的机器人定位与避障方法
CN102768767B (zh) 刚体在线三维重建与定位的方法
Lee et al. Accurate continuous sweeping framework in indoor spaces with backpack sensor system for applications to 3-D mapping
Gao et al. Pose refinement with joint optimization of visual points and lines
Zhang et al. Structure-aware SLAM with planes and lines in man-made environment
CN104657985B (zh) 基于深度图像遮挡信息的静态视觉目标遮挡规避方法
Tykkälä et al. A dense structure model for image based stereo SLAM
CN111402429B (zh) 一种尺度还原、三维重建方法、系统、存储介质及设备
CN106408654B (zh) 一种三维地图的创建方法及系统

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