CN111462207A - 一种融合直接法与特征法的rgb-d同时定位与地图创建方法 - Google Patents

一种融合直接法与特征法的rgb-d同时定位与地图创建方法 Download PDF

Info

Publication number
CN111462207A
CN111462207A CN202010255468.0A CN202010255468A CN111462207A CN 111462207 A CN111462207 A CN 111462207A CN 202010255468 A CN202010255468 A CN 202010255468A CN 111462207 A CN111462207 A CN 111462207A
Authority
CN
China
Prior art keywords
map
pose
frame
camera
local
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
Application number
CN202010255468.0A
Other languages
English (en)
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.)
Chongqing University of Post and Telecommunications
Original Assignee
Chongqing University of Post and Telecommunications
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 Chongqing University of Post and Telecommunications filed Critical Chongqing University of Post and Telecommunications
Priority to CN202010255468.0A priority Critical patent/CN111462207A/zh
Publication of CN111462207A publication Critical patent/CN111462207A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/50—Depth or shape recovery
    • G06T7/55—Depth or shape recovery from multiple images
    • 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
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
    • G06V10/74—Image or video pattern matching; Proximity measures in feature spaces
    • G06V10/75—Organisation of the matching processes, e.g. simultaneous or sequential comparisons of image or video features; Coarse-fine approaches, e.g. multi-scale approaches; using context analysis; Selection of dictionaries
    • G06V10/751—Comparing pixel values or logical combinations thereof, or feature values having positional relevance, e.g. template matching
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/10—Terrestrial scenes
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/40—Scenes; Scene-specific elements in video content
    • G06V20/46—Extracting features or characteristics from the video content, e.g. video fingerprints, representative shots or key frames
    • 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/10016—Video; Image sequence
    • 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/10024—Color 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

Abstract

本发明请求保护一种融合直接法与特征法的RGB‑D同时定位与地图创建方法。主要包含3个并行线程：跟踪线程、局部建图和闭环线程。在跟踪线程中对非关键帧进行跟踪，通过最小化光度图像误差来进行相机的初始位姿估计以及像素点的对应关系计算，利用最小化局部地图点重投影误差进一步优化相机位姿，实现快速准确的跟踪与定位；在局部建图线程中对关键帧提取并匹配ORB特征，并执行局部BA(光束平差法)，对局部关键帧位姿和局部地图点的位置进行优化，提高SLAM的局部一致性；在闭环线程中执行对关键帧的闭环检测和优化，从而保证SLAM全局一致性。另外，根据RGB‑D图像和相机位姿信息，通过基于Octomap的建图框架，构建完整准确的3D稠密环境地图。

Description

一种融合直接法与特征法的RGB-D同时定位与地图创建方法
技术领域
本发明属于移动机器人视觉SLAM领域，特别是，具体涉及一种融合直接法与特征法的RGB-D同时定位与地图创建(SLAM)算法。
背景技术
同时定位与地图构建(SLAM)是机器人领域的一个关键挑战和主要研究重点。未知环境中的自主移动机器人在执行各种任务时需要对环境地图进行构建，例如路径规划，避障和探索等。移动机器人必须知道其位置才能创建地图。另一方面，移动机器人需要通过地图来获取位置信息。因此，移动机器人必须对环境地图进行创建，同时在地图中定位。
完整的视觉SLAM框架由跟踪前端、优化后端、闭环检测以及地图构建等四部分组成。当跟踪丢失时，闭环检测可以实现重定位，对于长时间导航的正确性起着至关重要的作用。视觉SLAM中的VO(visual odometry，视觉里程计)可分为两类：直接法与特征法。特征法通过提取图像中的特征点和描述子，之后进行特征点匹配，通过最小化重投影误差来计算获得机器人的位姿。匹配的特征点可用于闭环检测和重定位，这使得VO成为完整的SLAM。然而，特征提取与匹配需要耗费大量的计算时间，使得特征法比直接法耗时更多。直接法通过最小化光度误差来获得机器人的位姿信息，无需进行特征提取。然而基于直接法的SLAM的闭环检测仍然是一个待解决的问题。长时间导航出现的漂移问题仍然是直接法遇到的主要问题之一。
发明内容
本发明的目的在于提供一种融合特征法与直接法的RGB-D稠密视觉SLAM方法，以结合直接法的快速性能与直接法的高精度和闭环能力。对于关键帧提取与匹配ORB(Oriented FAST and Rotated BRIEF)特征，并通过最小化光度误差对非关键帧进行跟踪，而不需要特征提取与匹配。关键帧用于局部建图线程与闭环线程，而非关键帧用于跟踪与定位，这使得其耗时更少。此外，本发明还可根据RGB-D图像和相机位姿信息，通过在基于八叉树的建图框架Octomap上构建3D占用网格地图，为移动机器人完成导航和避障等任务提供更加准确的环境地图。
提出了一种融合直接法与特征法的RGB-D同时定位与地图创建方法。本发明的技术方案如下：
一种融合直接法与特征法的RGB-D同时定位与地图创建方法，其包括以下步骤：
步骤S1，初始化，并利用RGB-D相机采集室内环境的RGB彩色数据与Depth深度数据，将第一帧作为关键帧，初始化相机位姿之后，在关键帧中提取关键点，利用其在深度图像中的深度值构造一系列3D点，从所有3D点中创建一个初始化地图；
步骤S2，对步骤S1中的RGB彩色数据，通过最小化光度误差来进行相机的初始位姿估计以及像素点的对应关系计算；
步骤S3，利用最小化局部地图点的重投影误差再次优化相机位姿，对局部地图进行跟踪，并判断是否生成新的关键帧，若跟踪失败，则触发新的跟踪周期；
步骤S4，在局部建图线程中，对当前关键帧图像中的ORB(Oriented FAST andRotated BRIEF)特征进行提取与匹配，筛选出当前关键帧中冗余的地图点，由特征提取与匹配的结果来创建新的地图点；
步骤S5，当处理完所有关键帧之后，执行局部BA(光束平差法)优化，对周围的关键帧位姿以及地图点位置进行优化；
步骤S6，筛选候选关键帧，对冗余的关键帧进行剔除；
步骤S7，在闭环检测线程中，通过检索图像识别数据库执行闭环检测，当检测到闭环时，对闭环周期进行判断，若当前关键帧与闭环关键帧不属于同一个周期，则将闭环关键帧的周期与当前关键帧的周期合并为同一周期；
步骤S8，采用位姿图优化的方法对相机运动的累积误差进行校正；
步骤S9，将步骤S7中周期内的所有关键帧位姿与所有地图点位置作为优化变量执行全局BA优化，从而得到全局一致的环境地图和相机运动轨迹；
步骤S10，根据RGB-D图像和相机位姿信息，在创建点云地图的过程中，通过基于八叉树的建图框架Octomap来转化为3D占用网格地图。
进一步的，所述步骤S2相机的初始位姿估计包括以下几个步骤：
步骤S2.1，跟踪线程通过对每一帧图像处理来对当前帧的位姿进行估计，如果上一帧跟踪成功，利用一个恒速模型(即假定摄像头处于匀速运动状态)来对当前相机的位姿进行预测。
步骤S2.2，根据步骤S2.1中估计的位姿，将参考帧中跟踪的3D地图点投影到当前帧中，当前帧与参考帧的相对位姿变换Tcr∈SE(3)则可以通过最小化光度误差计算得到，
Figure BDA0002437147190000031
该误差主要建立在同一个3D点对应的图像块之间，即
Figure BDA0002437147190000032
其中：
Figure BDA0002437147190000033
u是参考帧的像素位置，ρ表示二范数，T表示变换矩阵。
Figure BDA0002437147190000034
其中xri是参考帧坐标系中第i个被跟踪的3D地图点的位置。lr表示参考帧的像素灰度值。
进一步的，所述步骤S2求取像素点对应关系的过程为：通过初始位姿估计，得到经过优化且与上一帧像素点相关联的地图点集，从该地图点集中，得到当前帧与局部关键帧可观测到的Covisibility地图点，将局部地图点在当前帧中进行投影，从而通过最小化光度误差的方法获取更多图像像素点和局部地图点的对应关系，最小化光度误差的方法包括以下步骤：为寻找与像素点p1对应着的同一像素点p2，首先根据当前相机的位姿估计值，来寻找p2的位置。但若相机的位姿不够好，p2的外观与p1会有明显差别。从而假设一个空间点在各个视角下，成像的灰度是不变的。基于灰度值不变的假设，通过非线性优化的方法最小化p的两个像素点的亮度误差来优化相机位姿，寻找与p1点更相似的p2点，其中优化变量为相机位姿。
进一步的，所述将局部地图点在当前帧中进行投影，具体包括：由初始的投影像素坐标栅格化投影到当前帧中的地图点，采用10×10栅格，并根据深度信息对栅格中的地图点进行排列，在计算像素点匹配时，对每个栅格至多选取6个不同深度的地图点，通过考虑曝光时间引起的灰度变化，引入光度响应函数对其进行优化
Figure BDA0002437147190000041
式中：u′i表示两个像素点之间的对应关系，Ik表示局部地图点所在关键帧对应的灰度数据，Ir表示局部地图点所在参考帧对应的灰度数据，Ai表示仿射变换矩阵，Δtr和Δtk分别是Ir和Ik的曝光时间，f(·)光度响应函数，f-1(·)为逆光度响应函数。最后利用G-N(Gauss Newton)算法求解上式。G-N(Gauss Newton)算法步骤如下：
(1)给定初始值；
(2)对于第次迭代，求出当前的雅可比矩阵J和误差f；
(3)求解增量方程：HΔxk；
若Δxk足够小，则停止。否则，令Δxk+1＝xk+Δxk，返回(2)。
进一步的，所述步骤S3跟踪局部地图的过程为：在完成局部地图点与当前帧像素点匹配后，利用g2o通过最小化重投影误差再次优化相机的相对位姿Tcw为
Figure BDA0002437147190000042
其中pwi是世界坐标系中第i个Covisibility地图点，g2o是一个基于图优化的库。此过程的优化问题仅对相机位姿执行BA优化，由于单个位姿节点的BA问题对于地图点的约束太过单一，因此将地图点的优化放在局部BA线程中执行，利用最小化局部地图点的重投影误差再次优化相机位姿，对局部地图进行跟踪，若跟踪失败，则触发新的跟踪周期。
进一步的，所述步骤S4在局部建图线程中，对当前关键帧图像中的ORB:特征进行提取与匹配，筛选出当前关键帧中冗余的地图点，由特征提取与匹配的结果来创建新的地图点，具体包括步骤：
步骤S7.1，如果在步骤S3中成功跟踪地图点，那么将成功跟踪的地图点进行缓存，以便在下一帧中对其进行优先处理；
步骤S7.2，确定当前帧是否应作为关键帧生成，选取关键帧的规则如下：
(1)当前帧至少跟踪到30个地图点；
(2)当前帧跟踪到的地图点少于参考关键帧的90％；
(3)距上一个关键帧插入之后已跟踪超过20帧图像。
(4)上一个关键帧与当前帧的位置差值或姿态角差值不小于阈值Tt。
进一步的，所述步骤S5当处理完所有关键帧之后，执行局部BA(光束平差法)优化，对周围的关键帧位姿以及地图点位置进行优化，具体为：
步骤S8.1，如果一个图像帧作为关键帧生成，则提取并匹配新的ORB特征，同时将成功跟踪的特征保存在此关键帧中；
步骤S8.2，通过最小化重投影误差进行局部BA来进一步优化关键帧和局部地图点的位姿，最小化重投影误差，即将像素坐标(观测到的投影位置)与3D点按照当前估计的位姿进行投影得到的位置相比较得到的误差进行求和，构建最小二乘问题，然后寻找最好的相机位姿，利用非线性优化的方法使其最小化。
进一步的，所述步骤S7在闭环检测线程中，利用ORB特征在DBoW2上构建的词袋模型来检测闭环。
进一步的，所述步骤S8，采用位姿图优化的方法对相机运动的累积误差进行校正，即利用位姿图中的节点表示相机位姿，边表示两个位姿节点之间的相对运动估计，所有的位姿顶点和位姿-位姿边构成了一个图优化，本质上是一个最小二乘法问题，优化变量为各个顶点的位姿，边来自于为位姿观测约束，最后利用g2o(General Graphic Optimization，G2O)进行求解，从而校正累计误差。
进一步的，所述步骤S10具体为：
在八叉树建图过程中根节点的占据概率由子节点的概率计算得出，具体为：计算根节点的方法有两种：求平均值法
Figure BDA0002437147190000061
和求最大值法
Figure BDA0002437147190000062
ni表示某个节点，L(ni)表示某节点的概率对数值。在点云地图创建完成后，把输出的pcd格式点云转换成octomap格式的点云，进而生成Octomap地图。
本发明的优点及有益效果如下：
经由上述的技术方案可知，目前视觉里程计的实现方法主要有两大类：直接法和特征法。与直接法相比，特征法需要进行特征提取与匹配过程，因此比较耗时，但直接法缺乏闭环检测，容易出现漂移问题，与现有技术相比，本发明公开提供了一种融合特征法与直接法的RGB-D稠密视觉SLAM算法。该方法在跟踪线程中，对于非关键帧采用直接法进行初始位姿估计和像素点匹配，无需进行特征提取与匹配，从而提高了跟踪线程的运行速率。而对于关键帧采用特征法进行特征提取与匹配，以保证SLAM算法局部优化、全局优化以及闭环检测功能的实现。此外，本方法基于RGB-D相机构建了压缩的且易于维护的八叉树稠密地图，满足了移动机器人导航和避障的需求。本方法保持了与ORB-SLAM2算法相当的定位精度，并且在跟踪线程中的运行帧率得到了显著提升，从而加快了系统运行效率。
附图说明
图1是本发明提供优选实施例一种融合特征法与直接法的RGB-D稠密视觉SLAM算法流程图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、详细地描述。所描述的实施例仅仅是本发明的一部分实施例。
本发明解决上述技术问题的技术方案是：
本发明是一种融合直接法与特征法的RGB-D同时定位与地图创建(SLAM)算法。为了保持直接法的快速性与基于特征法的高精度和闭环能力，该算法主要包含3个并行线程：跟踪线程、局部建图和闭环线程。在跟踪线程中对非关键帧进行跟踪，通过最小化光度图像误差来进行相机的初始位姿估计以及像素点的对应关系计算，利用最小化局部地图点重投影误差进一步优化相机位姿，实现快速准确的跟踪与定位；在局部建图线程中对关键帧提取并匹配ORB特征，并执行局部BA(光束平差法)，对局部关键帧位姿和局部地图点的位置进行优化，提高SLAM的局部一致性；在闭环线程中执行对关键帧的闭环检测和优化，从而保证SLAM全局一致性。另外，根据RGB-D图像和相机位姿信息，通过基于Octomap的建图框架，构建完整准确的3D稠密环境地图。下面结合附图，对本发明作进一步的详细说明。
如图1所示，本发明的实现流程图包括如下：
步骤1，在系统启动时，利用RGB-D相机采集室内环境的RGB彩色数据与Depth深度数据，将第一帧作为关键帧，初始化其位姿之后，从所有三维坐标点中创建一个初始化地图；
步骤2，对步骤1中的RGB数据，通过对每一帧图像处理来对当前帧的位姿进行估计，如果上一帧跟踪成功，利用一个恒速模型来对当前相机的位姿进行预测，根据估计的位姿，将参考帧中跟踪的3D地图点投影到当前帧中。当前帧与参考帧的相对位姿变换Tcr∈SE(3)则可以通过最小化光度误差计算得到，该误差主要建立在同一个3D点对应的图像块之间，即
Figure BDA0002437147190000071
其中：
Figure BDA0002437147190000072
u是参考帧的像素位置，
Figure BDA0002437147190000073
是图像块区域，光度误差δl如下所示
Figure BDA0002437147190000081
其中xri是参考帧坐标系中第i个被跟踪的3D地图点的位置；
步骤3，通过初始位姿估计，可以得到经过优化且与上一帧像素点相关联的地图点集。从该地图点集中，可以得到当前帧与局部关键帧可观测到的Covisibility地图点，将局部地图点在当前帧中进行投影，从而通过最小化光度误差的方法获取更多图像像素点和局部地图点的对应关系。由初始的投影像素坐标栅格化投影到当前帧中的地图点，采用10×10栅格，并根据深度信息对栅格中的地图点进行排列，在计算像素点匹配时，对每个栅格至多选取6个不同深度的地图点，从而提高算法的运行效率。由于普遍使用的相机都包含自动曝光功能，同时2个时刻相机的曝光时间也不相同，这会造成环境中同一点在不同图像中灰度误差较大。因此本发明通过考虑曝光时间引起的灰度变化，引入光度响应函数对其进行优化
Figure BDA0002437147190000082
式中：Δtr和Δtk分别是Ir和Ik的曝光时间，f(·)光度响应函数，f-1(·)为逆光度响应函数。最后利用G-N(Gauss Newton)算法求解上式。
步骤4，在完成局部地图点与当前帧像素点匹配后，利用g2o通过最小化重投影误差再次优化相机的相对位姿Tcw为
Figure BDA0002437147190000083
其中pwi是世界坐标系中第i个Covisibility地图点。此过程的优化问题仅对相机位姿执行BA优化，由于单个位姿节点的BA问题对于地图点的约束太过单一，因此将地图点的优化放在局部BA线程中执行。利用最小化局部地图点的重投影误差再次优化相机位姿，对局部地图进行跟踪，若跟踪失败，则触发新的跟踪周期；
步骤5，如果在步骤4中成功跟踪地图点，那么将成功跟踪的地图点进行缓存，以便在下一帧中对其进行优先处理。确定当前帧是否应作为关键帧生成，关键帧的质量决定了局部地图的好坏，本方法选取关键帧的规则如下：
(1)当前帧至少跟踪到30个地图点。
(2)当前帧跟踪到的地图点少于参考关键帧的90％。
(3)距上一个关键帧插入之后已跟踪超过20帧图像。
(4)上一个关键帧与当前帧的位置差值或姿态角差值不小于阈值Tt。
步骤6，在局部建图线程中，对当前关键帧图像中的ORB特征进行提取与匹配。如果一个图像帧作为关键帧生成，则提取并匹配新的ORB特征，同时将成功跟踪的特征保存在此关键帧中；
步骤7，筛选出当前关键帧中冗余的地图点，由特征提取与匹配的结果来创建新的地图点；
步骤8，当处理完所有关键帧之后，执行局部BA优化，对周围的关键帧位姿以及地图点位置进行优化，以获得更加准确的定位与建图精度；
步骤9，筛选候选关键帧，对冗余的关键帧进行剔除；
步骤10，在闭环检测线程中，利用ORB特征在DBoW2上构建的词袋模型来检测闭环，当检测到闭环时，对闭环周期进行判断，若当前关键帧与闭环关键帧不属于同一个周期，则将闭环关键帧的周期与当前关键帧的周期合并为同一周期；
步骤11，采用位姿图优化的方法对相机运动的累积误差进行校正。
步骤12，将周期内的所有关键帧位姿与所有地图点位置作为优化变量执行全局BA优化，从而得到全局一致的环境地图和相机运动轨迹；
步骤13，在创建点云地图的过程中，通过基于八叉树的建图框架Octomap来转化为3D占用网格地图。在八叉树建图过程中根节点的占据概率可由子节点的概率计算得出。计算根节点的方法有两种：求平均值法
Figure BDA0002437147190000091
和求最大值法
Figure BDA0002437147190000092
以上这些实施例应理解为仅用于说明本发明而不用于限制本发明的保护范围。在阅读了本发明的记载的内容之后，技术人员可以对本发明作各种改动或修改，这些等效变化和修饰同样落入本发明权利要求所限定的范围。

Claims (10)

1.一种融合直接法与特征法的RGB-D同时定位与地图创建方法，其特征在于，包括以下步骤：
步骤S1，初始化，并利用RGB-D相机采集室内环境的RGB彩色数据与Depth深度数据，将第一帧作为关键帧，初始化相机位姿之后，在关键帧中提取关键点，利用其在深度图像中的深度值构造一系列3D点，从所有3D点中创建一个初始化地图；
步骤S2，对步骤S1中的RGB彩色数据，通过最小化光度误差来进行相机的初始位姿估计以及像素点的对应关系计算；
步骤S3，利用最小化局部地图点的重投影误差再次优化相机位姿，对局部地图进行跟踪，并判断是否生成新的关键帧，若跟踪失败，则触发新的跟踪周期；
步骤S4，在局部建图线程中，对当前关键帧图像中的ORB特征进行提取与匹配，筛选出当前关键帧中冗余的地图点，由特征提取与匹配的结果来创建新的地图点；
步骤S5，当处理完所有关键帧之后，执行局部BA光束平差法优化，对周围的关键帧位姿以及地图点位置进行优化；
步骤S6，筛选候选关键帧，对冗余的关键帧进行剔除；
步骤S7，在闭环检测线程中，通过检索图像识别数据库执行闭环检测，当检测到闭环时，对闭环周期进行判断，若当前关键帧与闭环关键帧不属于同一个周期，则将闭环关键帧的周期与当前关键帧的周期合并为同一周期；
步骤S8，采用位姿图优化的方法对相机运动的累积误差进行校正；
步骤S9，将步骤S7中周期内的所有关键帧位姿与所有地图点位置作为优化变量执行全局BA优化，从而得到全局一致的环境地图和相机运动轨迹；
步骤S10，根据RGB-D图像和相机位姿信息，在创建点云地图的过程中，通过基于八叉树的建图框架Octomap来转化为3D占用网格地图。
2.根据权利要求1所述的一种融合直接法与特征法的RGB-D同时定位与地图创建方法，其特征在于，所述步骤S2相机的初始位姿估计包括以下几个步骤：
步骤S2.1，跟踪线程通过对每一帧图像处理来对当前帧的位姿进行估计，如果上一帧跟踪成功，利用一个恒速模型即假定摄像头处于匀速运动状态来对当前相机的位姿进行预测；
步骤S2.2，根据步骤S2.1中估计的位姿，将参考帧中跟踪的3D地图点投影到当前帧中，当前帧与参考帧的相对位姿变换
Figure FDA0002437147180000029
则可以通过最小化光度误差计算得到，
Figure FDA0002437147180000021
该误差主要建立在同一个3D点对应的图像块之间，即
Figure FDA0002437147180000022
其中：
Figure FDA0002437147180000023
Figure FDA0002437147180000024
是参考帧的像素位置，ρ表示二范数，
Figure FDA0002437147180000025
是图像块区域，T表示变换矩阵；
Figure FDA0002437147180000026
其中
Figure FDA0002437147180000027
是参考帧坐标系中第i个被跟踪的3D地图点的位置，
Figure FDA0002437147180000028
表示参考帧的像素灰度值。
3.根据权利要求1或2所述的一种融合直接法与特征法的RGB-D同时定位与地图创建方法，其特征在于，所述步骤S2求取像素点对应关系的过程为：通过初始位姿估计，得到经过优化且与上一帧像素点相关联的地图点集，从该地图点集中，得到当前帧与局部关键帧可观测到的Covisibility地图点，将局部地图点在当前帧中进行投影，从而通过最小化光度误差的方法获取更多图像像素点和局部地图点的对应关系，最小化光度误差的方法包括以下步骤：为寻找与像素点p1对应着的同一像素点p2，首先根据当前相机的位姿估计值，来寻找p2的位置。但若相机的位姿不够好，p2的外观与p1会有明显差别，从而假设一个空间点在各个视角下，成像的灰度是不变的。基于灰度值不变的假设，通过非线性优化的方法最小化p的两个像素点的亮度误差来优化相机位姿，寻找与p1点更相似的p2点，其中优化变量为相机位姿。
4.根据权利要求3所述的一种融合直接法与特征法的RGB-D同时定位与地图创建方法，其特征在于，所述将局部地图点在当前帧中进行投影，具体包括：由初始的投影像素坐标栅格化投影到当前帧中的地图点，采用10×10栅格，并根据深度信息对栅格中的地图点进行排列，在计算像素点匹配时，对每个栅格至多选取6个不同深度的地图点，通过考虑曝光时间引起的灰度变化，引入光度响应函数对其进行优化
Figure FDA0002437147180000031
式中：u′i表示两个像素点之间的对应关系，Ik表示局部地图点所在关键帧对应的灰度数据，Ir表示局部地图点所在参考帧对应的灰度数据，Ai表示仿射变换矩阵，Δtr和Δtk分别是Ir和Ik的曝光时间，f(·)光度响应函数，f-1(·)为逆光度响应函数，最后利用G-N(Gauss Newton)算法求解上式，G-N(Gauss Newton)算法步骤如下：
(1)给定初始值x0；
(2)对于第k次迭代，求出当前的雅可比矩阵J(xk)和误差f(xk)；
(3)求解增量方程：HΔxk＝g；
(4)若Δxk足够小，则停止。否则，令Δxk+1＝xk+Δxk，返回(2)。
5.根据权利要求3所述的一种融合直接法与特征法的RGB-D同时定位与地图创建方法，其特征在于，所述步骤S3跟踪局部地图的过程为：在完成局部地图点与当前帧像素点匹配后，利用g2o(General Graphic Optimization，G2O)通过最小化重投影误差再次优化相机的相对位姿Tcw为
Figure FDA0002437147180000032
其中pwi是世界坐标系中第i个Covisibility地图点，g2o是一个基于图优化的库，此过程的优化问题仅对相机位姿执行BA优化，由于单个位姿节点的BA问题对于地图点的约束太过单一，因此将地图点的优化放在局部BA线程中执行，利用最小化局部地图点的重投影误差再次优化相机位姿，对局部地图进行跟踪，若跟踪失败，则触发新的跟踪周期。
6.根据权利要求5所述的一种融合直接法与特征法的RGB-D同时定位与地图创建方法，其特征在于，所述步骤S4在局部建图线程中，对当前关键帧图像中的ORB:特征进行提取与匹配，筛选出当前关键帧中冗余的地图点，由特征提取与匹配的结果来创建新的地图点，具体包括步骤：
步骤S7.1，如果在步骤S3中成功跟踪地图点，那么将成功跟踪的地图点进行缓存，以便在下一帧中对其进行优先处理；
步骤S7.2，确定当前帧是否应作为关键帧生成，选取关键帧的规则如下：
(1)当前帧至少跟踪到30个地图点；
(2)当前帧跟踪到的地图点少于参考关键帧的90％；
(3)距上一个关键帧插入之后已跟踪超过20帧图像。
(4)上一个关键帧与当前帧的位置差值或姿态角差值不小于阈值Tt。
7.根据权利要求6所述的一种融合直接法与特征法的RGB-D同时定位与地图创建方法，其特征在于，所述步骤S5当处理完所有关键帧之后，执行局部BA光束平差法优化，对周围的关键帧位姿以及地图点位置进行优化，具体为：
步骤S8.1，如果一个图像帧作为关键帧生成，则提取并匹配新的ORB特征，同时将成功跟踪的特征保存在此关键帧中；
步骤S8.2，通过最小化重投影误差进行局部BA来进一步优化关键帧和局部地图点的位姿，最小化重投影误差，即将像素坐标(观测到的投影位置)与3D点按照当前估计的位姿进行投影得到的位置相比较得到的误差进行求和，构建最小二乘问题，然后寻找最好的相机位姿，利用非线性优化的方法使其最小化。
8.根据权利要求7所述的一种融合直接法与特征法的RGB-D同时定位与地图创建方法，其特征在于，所述步骤S7在闭环检测线程中，利用ORB特征在DBoW2上构建的词袋模型来检测闭环。
9.根据权利要求8所述的一种融合直接法与特征法的RGB-D同时定位与地图创建方法，其特征在于，所述步骤S8，采用位姿图优化的方法对相机运动的累积误差进行校正，即利用位姿图中的节点表示相机位姿，边表示两个位姿节点之间的相对运动估计，所有的位姿顶点和位姿-位姿边构成了一个图优化，本质上是一个最小二乘法问题，优化变量为各个顶点的位姿，边来自于为位姿观测约束，最后利用g2o(General Graphic Optimization，G2O)进行求解，从而校正累计误差。
10.根据权利要求9所述的一种融合直接法与特征法的RGB-D同时定位与地图创建方法，其特征在于，所述步骤S10具体为：
在八叉树建图过程中根节点的占据概率由子节点的概率计算得出，具体为：计算根节点的方法有两种：求平均值法
Figure FDA0002437147180000051
和求最大值法
Figure FDA0002437147180000052
Figure FDA0002437147180000053
表示某个节点，
Figure FDA0002437147180000054
表示某节点的概率对数值。在点云地图创建完成后，把输出的pcd格式点云转换成octomap格式的点云，进而生成Octomap地图。
CN202010255468.0A 2020-03-30 2020-03-30 一种融合直接法与特征法的rgb-d同时定位与地图创建方法 Pending CN111462207A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010255468.0A CN111462207A (zh) 2020-03-30 2020-03-30 一种融合直接法与特征法的rgb-d同时定位与地图创建方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010255468.0A CN111462207A (zh) 2020-03-30 2020-03-30 一种融合直接法与特征法的rgb-d同时定位与地图创建方法

Publications (1)

Publication Number Publication Date
CN111462207A true CN111462207A (zh) 2020-07-28

Family

ID=71680487

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010255468.0A Pending CN111462207A (zh) 2020-03-30 2020-03-30 一种融合直接法与特征法的rgb-d同时定位与地图创建方法

Country Status (1)

Country Link
CN (1) CN111462207A (zh)

Cited By (16)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112016612A (zh) * 2020-08-26 2020-12-01 四川阿泰因机器人智能装备有限公司 一种基于单目深度估计的多传感器融合slam方法
CN112025709A (zh) * 2020-08-31 2020-12-04 东南大学 一种基于车载摄像头视觉的移动机器人定位系统及方法
CN112115874A (zh) * 2020-09-21 2020-12-22 武汉大学 一种融合云端的视觉slam系统及方法
CN112129282A (zh) * 2020-09-30 2020-12-25 杭州海康机器人技术有限公司 一种不同导航方式之间定位结果的转换方法、转换装置
CN112308917A (zh) * 2020-10-15 2021-02-02 江苏大学 一种基于视觉的移动机器人定位方法
CN112419497A (zh) * 2020-11-13 2021-02-26 天津大学 基于单目视觉的特征法与直接法相融合的slam方法
CN112907654A (zh) * 2021-02-08 2021-06-04 上海汽车集团股份有限公司 多目相机外参数优化方法、装置、电子设备及存储介质
CN112950696A (zh) * 2021-02-03 2021-06-11 珠海格力智能装备有限公司 导航地图的生成方法及生成装置、电子设备
CN113108771A (zh) * 2021-03-05 2021-07-13 华南理工大学 一种基于闭环直接稀疏视觉里程计的移动位姿估计方法
CN113269876A (zh) * 2021-05-10 2021-08-17 Oppo广东移动通信有限公司 地图点坐标优化方法及装置、电子设备及存储介质
CN113432593A (zh) * 2021-06-25 2021-09-24 北京华捷艾米科技有限公司 集中式同步定位与地图构建方法、装置和系统
CN113447014A (zh) * 2021-08-30 2021-09-28 深圳市大道智创科技有限公司 室内移动机器人、建图方法、定位方法以及建图定位装置
CN114088081A (zh) * 2021-10-10 2022-02-25 北京工业大学 一种基于多段联合优化的用于精确定位的地图构建方法
CN114519472A (zh) * 2022-04-20 2022-05-20 南京信息工程大学 一种基于三维模型的应急管理监测方法
CN115115669A (zh) * 2022-06-28 2022-09-27 深圳市神州云海智能科技有限公司 基于边缘设备自监督学习的终端感知定位方法及系统
CN115965673A (zh) * 2022-11-23 2023-04-14 中国建筑一局(集团)有限公司 基于双目视觉的集中式多机器人定位方法

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109211241A (zh) * 2018-09-08 2019-01-15 天津大学 基于视觉slam的无人机自主定位方法
CN109509230A (zh) * 2018-11-13 2019-03-22 武汉大学 一种应用于多镜头组合式全景相机的slam方法
CN110363816A (zh) * 2019-06-25 2019-10-22 广东工业大学 一种基于深度学习的移动机器人环境语义建图方法
CN110501017A (zh) * 2019-08-12 2019-11-26 华南理工大学 一种基于orb_slam2的移动机器人导航地图生成方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109211241A (zh) * 2018-09-08 2019-01-15 天津大学 基于视觉slam的无人机自主定位方法
CN109509230A (zh) * 2018-11-13 2019-03-22 武汉大学 一种应用于多镜头组合式全景相机的slam方法
CN110363816A (zh) * 2019-06-25 2019-10-22 广东工业大学 一种基于深度学习的移动机器人环境语义建图方法
CN110501017A (zh) * 2019-08-12 2019-11-26 华南理工大学 一种基于orb_slam2的移动机器人导航地图生成方法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
D. G´ALVEZ-L´OPEZ ， J. D. TARD´OS: "Bags of binary words for fast place recognition in image sequences", 《IEEE TRANS. ROBOT.》 *
MUR-ARTAL，J. D. TARDÓS: "Fast relocalisation and loop closing in keyframe-based SLAM", 《2014 IEEE INTERNATIONAL CONFERENCE ON ROBOTICS AND AUTOMATION (ICRA)》 *
孙健: "基于RGB-D相机的室内移动机器人三维地图构建研究", 《中国优秀硕士学位论文全文数据库 信息科技辑》 *
张国良，姚二亮，林志林，徐慧: "融合直接法与特征法的快速双目SLAM算法", 《机器人》 *

Cited By (20)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112016612A (zh) * 2020-08-26 2020-12-01 四川阿泰因机器人智能装备有限公司 一种基于单目深度估计的多传感器融合slam方法
CN112025709A (zh) * 2020-08-31 2020-12-04 东南大学 一种基于车载摄像头视觉的移动机器人定位系统及方法
CN112115874A (zh) * 2020-09-21 2020-12-22 武汉大学 一种融合云端的视觉slam系统及方法
CN112115874B (zh) * 2020-09-21 2022-07-15 武汉大学 一种融合云端的视觉slam系统及方法
CN112129282B (zh) * 2020-09-30 2021-06-18 杭州海康机器人技术有限公司 一种不同导航方式之间定位结果的转换方法、转换装置
CN112129282A (zh) * 2020-09-30 2020-12-25 杭州海康机器人技术有限公司 一种不同导航方式之间定位结果的转换方法、转换装置
CN112308917A (zh) * 2020-10-15 2021-02-02 江苏大学 一种基于视觉的移动机器人定位方法
CN112419497A (zh) * 2020-11-13 2021-02-26 天津大学 基于单目视觉的特征法与直接法相融合的slam方法
CN112950696A (zh) * 2021-02-03 2021-06-11 珠海格力智能装备有限公司 导航地图的生成方法及生成装置、电子设备
CN112907654A (zh) * 2021-02-08 2021-06-04 上海汽车集团股份有限公司 多目相机外参数优化方法、装置、电子设备及存储介质
CN112907654B (zh) * 2021-02-08 2024-03-26 上海汽车集团股份有限公司 多目相机外参数优化方法、装置、电子设备及存储介质
CN113108771A (zh) * 2021-03-05 2021-07-13 华南理工大学 一种基于闭环直接稀疏视觉里程计的移动位姿估计方法
CN113269876A (zh) * 2021-05-10 2021-08-17 Oppo广东移动通信有限公司 地图点坐标优化方法及装置、电子设备及存储介质
CN113432593A (zh) * 2021-06-25 2021-09-24 北京华捷艾米科技有限公司 集中式同步定位与地图构建方法、装置和系统
CN113447014A (zh) * 2021-08-30 2021-09-28 深圳市大道智创科技有限公司 室内移动机器人、建图方法、定位方法以及建图定位装置
CN114088081A (zh) * 2021-10-10 2022-02-25 北京工业大学 一种基于多段联合优化的用于精确定位的地图构建方法
CN114519472A (zh) * 2022-04-20 2022-05-20 南京信息工程大学 一种基于三维模型的应急管理监测方法
CN115115669A (zh) * 2022-06-28 2022-09-27 深圳市神州云海智能科技有限公司 基于边缘设备自监督学习的终端感知定位方法及系统
CN115965673A (zh) * 2022-11-23 2023-04-14 中国建筑一局(集团)有限公司 基于双目视觉的集中式多机器人定位方法
CN115965673B (zh) * 2022-11-23 2023-09-12 中国建筑一局(集团)有限公司 基于双目视觉的集中式多机器人定位方法

Similar Documents

Publication Publication Date Title
CN111462207A (zh) 一种融合直接法与特征法的rgb-d同时定位与地图创建方法
CN111563442B (zh) 基于激光雷达的点云和相机图像数据融合的slam方法及系统
Park et al. Elastic lidar fusion: Dense map-centric continuous-time slam
US10553026B2 (en) Dense visual SLAM with probabilistic surfel map
US10546387B2 (en) Pose determination with semantic segmentation
CN111210463B (zh) 基于特征点辅助匹配的虚拟宽视角视觉里程计方法及系统
US11830216B2 (en) Information processing apparatus, information processing method, and storage medium
US10225473B2 (en) Threshold determination in a RANSAC algorithm
CN111445526B (zh) 一种图像帧之间位姿的估计方法、估计装置和存储介质
CN109974743B (zh) 一种基于gms特征匹配及滑动窗口位姿图优化的视觉里程计
CN111062966B (zh) 基于l-m算法和多项式插值对相机跟踪进行优化的方法
Belter et al. Improving accuracy of feature-based RGB-D SLAM by modeling spatial uncertainty of point features
JP6860620B2 (ja) 情報処理装置、情報処理方法、及びプログラム
KR101869605B1 (ko) 평면정보를 이용한 3차원 공간 모델링 및 데이터 경량화 방법
CN112652020B (zh) 一种基于AdaLAM算法的视觉SLAM方法
CN112085849A (zh) 基于航拍视频流的实时迭代三维建模方法、系统及可读介质
CN112967340A (zh) 同时定位和地图构建方法、装置、电子设备及存储介质
CN114494150A (zh) 一种基于半直接法的单目视觉里程计的设计方法
CN111161334A (zh) 一种基于深度学习的语义地图构建方法
CN112541423A (zh) 一种同步定位与地图构建方法和系统
Wientapper et al. Composing the feature map retrieval process for robust and ready-to-use monocular tracking
CN105339981B (zh) 用于使用一组基元配准数据的方法
CN113052907A (zh) 一种动态环境移动机器人的定位方法
KR101766823B1 (ko) 불규칙한 조도 변화에 강건한 영상 기반 주행거리 측정 시스템 및 방법
Buck et al. Capturing uncertainty in monocular depth estimation: Towards fuzzy voxel maps

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
RJ01 Rejection of invention patent application after publication
RJ01 Rejection of invention patent application after publication

Application publication date: 20200728