CN116592897B - 基于位姿不确定性的改进orb-slam2定位方法 - Google Patents
基于位姿不确定性的改进orb-slam2定位方法 Download PDFInfo
- Publication number
- CN116592897B CN116592897B CN202310873189.4A CN202310873189A CN116592897B CN 116592897 B CN116592897 B CN 116592897B CN 202310873189 A CN202310873189 A CN 202310873189A CN 116592897 B CN116592897 B CN 116592897B
- Authority
- CN
- China
- Prior art keywords
- current frame
- map
- representing
- pose
- uncertainty
- 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
- 238000000034 method Methods 0.000 title claims abstract description 30
- 238000001514 detection method Methods 0.000 claims abstract description 30
- 238000005457 optimization Methods 0.000 claims abstract description 23
- 230000004927 fusion Effects 0.000 claims abstract description 18
- 239000011159 matrix material Substances 0.000 claims description 10
- 239000013598 vector Substances 0.000 claims description 9
- 238000013519 translation Methods 0.000 claims description 2
- 238000012216 screening Methods 0.000 abstract description 2
- 230000000007 visual effect Effects 0.000 description 11
- 238000002474 experimental method Methods 0.000 description 4
- 238000004422 calculation algorithm Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 3
- 238000013507 mapping Methods 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 2
- 230000004807 localization Effects 0.000 description 2
- 230000007774 longterm Effects 0.000 description 2
- 240000005528 Arctium lappa Species 0.000 description 1
- 241000274965 Cyrestis thyodamas Species 0.000 description 1
- 238000012897 Levenberg–Marquardt algorithm Methods 0.000 description 1
- 102000008115 Signaling Lymphocytic Activation Molecule Family Member 1 Human genes 0.000 description 1
- 108010074687 Signaling Lymphocytic Activation Molecule Family Member 1 Proteins 0.000 description 1
- NMVPEQXCMGEDNH-TZVUEUGBSA-N ceftazidime pentahydrate Chemical compound O.O.O.O.O.S([C@@H]1[C@@H](C(N1C=1C([O-])=O)=O)NC(=O)\C(=N/OC(C)(C)C(O)=O)C=2N=C(N)SC=2)CC=1C[N+]1=CC=CC=C1 NMVPEQXCMGEDNH-TZVUEUGBSA-N 0.000 description 1
- 238000012790 confirmation Methods 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000013139 quantization Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
Classifications
-
- 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
-
- 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/20—Image preprocessing
- G06V10/24—Aligning, centring, orientation detection or correction of the image
- G06V10/245—Aligning, centring, orientation detection or correction of the image by locating a pattern; Special marks for positioning
-
- 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/40—Extraction of image or video features
- G06V10/46—Descriptors for shape, contour or point-related descriptors, e.g. scale invariant feature transform [SIFT] or bags of words [BoW]; Salient regional features
- G06V10/462—Salient features, e.g. scale invariant feature transforms [SIFT]
- G06V10/464—Salient features, e.g. scale invariant feature transforms [SIFT] using a plurality of salient features, e.g. bag-of-words [BoW] representations
-
- 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/757—Matching configurations of points or features
-
- 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/761—Proximity, similarity or dissimilarity measures
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V20/00—Scenes; Scene-specific elements
- G06V20/50—Context or environment of the image
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Abstract
本发明公开了一种基于位姿不确定性的改进ORB‑SLAM2定位方法，属于机器人导航定位技术领域。本发明首先将局部地图点投影到当前帧，并构造重投影误差函数；再通过最小化代价函数对当前帧的位姿进行优化，并计算当前帧的不确定性半径；然后重新计算待融合地图点的权重并对地图点进行加权融合；再对ORB‑SLAM2的闭环检测线程进行约束，对满足位姿约束的关键帧进行闭环检测；若当前帧满足位姿约束，则对当前帧进行图片相似度检测，若检测到闭环则执行全局优化，优化整个地图。本发明在不影响ORB‑SLAM2的实时运行的情况下，提高了定位精度，并且通过对闭环区域进行筛选大幅度增加了闭环效率。
Description
技术领域
本发明属于机器人导航定位技术领域，具体涉及基于位姿不确定性的改进ORB-SLAM2定位方法。
背景技术
想要机器人在未知环境中独立和安全地完成任务，就必须准确地知道其在环境中的位置。这就是SLAM（Simultaneous Localization And Mapping）要解决的问题。SLAM可以在未知环境建立环境地图，同时实现机器人的在环境中定位。激光SLAM技术目前已经十分成熟，但由于激光雷达的昂贵的价格，还无法在日常生活中普及。视觉SLAM由于其优越的感知和重新定位能力，以及较低的成本和易于安装的特点而越来越受欢迎。在过去的几年里，许多性能良好的视觉SLAM算法已经被提出。视觉SLAM框架通常由传感器数据、前端、后端、回路检测和建图等线程组成。在长期和大规模的导航场景中，累积误差是不可避免的，仅仅依靠视觉里程计来估计机器人的位置会出现严重的误差。闭环检测通过建立当前帧与之前某一帧间的位姿约束消除视觉里程计所产生的累积误差，进而实现地图优化。闭环检测在地图优化中起着关键作用，没有闭环检测的SLAM就等同于一个视觉里程计。因此，想要在长期和大规模的导航场景下实现精准定位，闭环检测是不可或缺的。
但目前的SLAM算法都专注于基于外观的闭环检测，而忽略了视觉里程计信息在闭环检测中的作用。在这些方法中，每输入一个图像帧闭环检测线程就需要检索整个闭环数据库进行图片相似度对比，这样做会浪费大量的计算资源。以ORB-SLAM2为例，每创建一个新的关键帧，就需要检索整个DBoW2的词汇字典进行图片相似度对比，虽然它限定了只对当前帧的非共视帧进行对比，但剩余的关键帧检索范围依然很大，尤其是随着地图规模的增长，检索的代价将越来越大。目前的研究很少有将视觉里程计和外观信息相结合在一起。
发明内容
为了解决单纯依靠外观信息，闭环检测计算量过大的问题，本发明的目的在于提供一种基于位姿不确定性的改进ORB-SLAM2定位方法，通过计算当前帧的不确定性半径，结合视觉里程计信息对ORB-SLAM2的闭环检测线程进行约束，有选择的进行闭环检测。同时考虑到不同关键帧对观测到的地图点的贡献不同，基于不确定性半径对ORB-SLAM2的地图点融合策略进行改进，利用重新计算的地图点权重，改进后的地图点融合策略提高了ORB-SLAM2的定位精度。
为达到上述目的，本发明的技术方案如下：
基于位姿不确定性的改进ORB-SLAM2定位方法，该方法包括如下步骤：
该方法包括如下步骤：
步骤1、根据当前帧的共视关系创建局部地图，将局部地图点投影到当前帧，并构造重投影误差函数；
步骤2、利用步骤1得到的重投影误差函数构建代价函数，通过最小化代价函数对当前帧的位姿进行优化，利用优化结果评估当前帧和前一帧的跟踪质量，并计算当前帧的不确定性半径；
步骤3、利用步骤2所得的不确定性半径，重新计算待融合地图点的权重并对地图点进行加权融合；
步骤4、利用里程计信息和步骤2所得的不确定性半径构建位姿约束，对ORB-SLAM2的闭环检测线程进行约束，对满足位姿约束的关键帧进行闭环检测；
步骤5、若当前帧满足位姿约束，则对当前帧进行图片相似度检测，若检测到闭环则执行全局优化，优化整个地图，完成优化后，重新计算地图内所有关键帧的不确定性半径。
进一步地，步骤1包括以下子步骤：
子步骤1.1、 利用当前帧在地图内的共视关系，创建局部地图；
子步骤1.2、 将局部地图的所有地图点投影到当前帧上，并建立重投影误差函数：
其中，e i 表示地图点到当前帧的重投影误差，T cw 表示当前帧的在世界坐标系下的位姿，表示地图点在世界坐标系下的三维坐标，/>表示三维实数向量，是/>在当前帧下对应的二维特征点的像素坐标，/>表示二维实数向量，/>是投影函数：
其中，和t cw 分别表示/>的旋转矩阵和平移向量部分，/>和/>分别表示相机的焦距和主点，x i 、y i 、z i 分别表示地图点/>在当前帧相机坐标系下的三维坐标，上标T表示矩阵的转置。
进一步地，步骤2具体包括以下子步骤：
子步骤2.1、 固定地图点，求解代价函数：
其中C表示代价函数的值，N表示局部地图的内点数量，表示Huber鲁棒核函数，/>是信息矩阵，和特征点被检测到时的尺度有关；
子步骤2.2、多次优化后取最后一次的优化结果，计算平均误差：
其中c表示当前帧优化后的平均误差；
子步骤2.3、计算当前帧的不确定性半径：
其中和/>分别表示当前帧和上一关键帧的不确定性半径，/>和表示当前帧和上一关键帧在世界坐标系下三维坐标，/>是可靠系数，表示当前帧跟踪到的地图点数量。
进一步地，步骤3包括以下子步骤：
子步骤3.1、根据不确定性半径，重新计算地图点权重：
其中，表示待计算的地图点权重，/>表示跟踪到该待计算的地图点的关键帧数量，/>表示第i个关键帧的不确定性半径；
子步骤3.2、根据地图点权重，对地图点进行加权融合，融合后的地图点位置为：
其中，p ab 表示地图点融合后在世界坐标系下的三维坐标，p a 和p b 分别表示待融合地图点a和b融合前在世界坐标系下的三维坐标，ω a 和ω b 分别表示待融合地图点a和b的权重；
子步骤3.3、完成融合后更新融合后地图点的权重和观测信息。
进一步地，步骤4包括以下子步骤：
子步骤4.1、计算当前帧与地图任意关键帧间的相对位姿：
其中，T ci 表示当前帧与地图内第i个关键帧间的相对位姿，T cw 和T iw 分别表示当前帧和地图中第i个关键帧在世界坐标系下的位姿,R iw 和t iw 分别表示T iw 的旋转和平移部分，R cw 和t cw 分别表示T cw 的旋转和平移部分，t ci 和R ci 分别表示T ci 的旋转和平移部分；
子步骤4.2、检测该关键帧是否满足位置约束：
其中，表示欧氏距离，r c 和r i 分别表示当前帧和地图内第i个关键帧的不确定性半径；若该关键帧满足位置约束，则进一步检测姿态约束；若不满足，继续检测下一关键帧，直到遍历完整个地图；
子步骤4.3、检测该关键帧是否满足姿态约束：
其中，表示矩阵的迹；如果该关键帧也能满足的姿态约束，就认为这是一个闭环区域，对当前帧进行进一步的图片相似度检测。
进一步地，步骤（5）包括以下内容：
利用DBoW2模型，计算当前帧和其共视关键帧间的图片相似度，如果当前帧可以通过图片相似度检测，则认为检测到了一次闭环，将对整个地图进行一次全局优化，优化完成后重新计算地图内所有关键帧的不确定性半径：
其中，表示当前帧到对应的闭环帧间的关键帧数量。
与现有技术相比，本发明具有如下优点：
本发明公开的一种基于位姿不确定性的改进ORB-SLAM2定位方法，通过建立局部地图对当前帧的位姿进行优化，利用多次优化后的投影误差估计当前帧的位姿不确定性；根据位姿不确定性重新计算地图点的权重，并对地图点进行加权融合；结合视觉里程计信息，构建位姿约束，只对满足约束的关键帧进行闭环检测；本发明在不影响ORB-SLAM2的实时运行的情况下，提高了定位精度，并且通过对闭环区域进行筛选大幅度增加了闭环效率。
附图说明
图1为本发明的基于位姿不确定性的改进ORB-SLAM2定位方法流程图；
图2为本发明的基于位姿不确定性的改进ORB-SLAM2定位方法框图。
具体实施方式
使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施方式，对本发明经行进一步的详细说明。显然，此处所描述的具体实施方式仅仅用于解释本发明，是本发明的一部分实施方式，而不是全部的实施方式。基于本发明中的实施方式，本领域技术普通人员在没有做出创造性劳动的前提下所获得的所有其他实施方式，都属于本发明保护的范围。
图1所示是本发明提供的一种位姿不确定性的改进ORB-SLAM2的方法流程图。本专利所提出的所有方法都在ORB-SLAM2的跟踪，局部建图和闭环三大线程下运行，具体的系统框图如图2所示。方法具体流程包括：
步骤1，根据当前帧的共视关系创建局部地图，将局部地图点投影到当前帧，并构造重投影误差函数：
其中，e i 表示地图点到当前帧的重投影误差，T cw 表示当前帧的在世界坐标系下的位姿，表示地图点在世界坐标系下的三维坐标，/>表示三维实数向量，是/>在当前帧下对应的二维特征点的像素坐标，/>表示二维实数向量，/>是投影函数。
为了表示关键帧间的相互联系，利用了共视图（Covisibility Graph）的概念，每个节点代表一个关键帧，如果两个关键帧间共同观测（至少15个地图点）则两个关键帧代表的节点间存在一条边。当前帧在被创建时，与当前帧帧共视关系最强的关键帧，被设为当前帧的父关键帧。
所建立的局部地图包括：
（a）当前帧的所有共视关键帧；
（b）步骤（a）中关键帧的所有父子关键帧；
（c）步骤（a）中所有关键帧的共视关系前十（观测到共同地图点数量最多）的关键帧；
（d）上述所有关键帧观测到的地图点；
步骤2，利用优化结果评估两帧间的跟踪质量，并计算当前帧的不确定性半径；
步骤（2.1）利用g2o下的列文伯格-马尔夸特算法求解代价函数：
其中C表示代价函数的值，N表示局部地图的内点数量，表示Huber鲁棒核函数，/>是信息矩阵，和特征点被检测到时的尺度有关。
步骤（2.2）多次优化后取最后一次的优化结果，计算平均误差：
其中c表示当前帧优化后的平均误差。
步骤（2.3）计算当前帧的不确定性半径：
其中和/>分别表示当前帧和上一关键帧的不确定性半径，/>和表示当前帧和上一关键帧在世界坐标系下三维坐标，/>是可靠系数，表示当前帧跟踪到的地图点数量。
步骤3，改进ORB-SLAM2地图点融合策略，利用步骤2所得的不确定性半径，重新计算待融合地图点权重并对地图点进行加权融合。具体地，地图点加权融合步骤如下：
步骤（3.1），根据不确定性半径，重新计算地图点权重：
其中，表示待计算的地图点权重，/>表示跟踪到该待计算的地图点的关键帧数量，/>表示第i个关键帧的不确定性半径。
步骤（3.2），根据地图点权重，对地图点进行加权融合，融合后的地图点位置为：
其中，p ab 表示地图点融合后在世界坐标系下的三维坐标，p a 和p b 分别表示待融合地图点a和b融合前在世界坐标系下的三维坐标，ω a 和ω b 分别表示待融合地图点a和b的权重。
步骤4，利用步骤2所得的位姿不确定性半径和里程计信息构建位姿约束，对ORB-SLAM2的闭环检测线程进行约束，对满足位姿约束的关键帧进行闭环检测；
位姿约束包括位置约束和姿态约束两个部分，如果当前帧和之前任意关键间的欧氏距离小于它们的不确定性半径之和则满足位置约束；如果当前帧和之前任意关键间相对旋转角度小于根据不确定性半径计算的旋转角度阈值则满足姿态约束。若当前地图存在关键帧与当前帧之间满足位姿约束，则进入图片相似度检测；若不存在将当前帧加入BoW字典数据库，继续对下一帧执行闭环检测，位姿约束的计算方法如下：
步骤（4.1），计算当前帧与地图任意关键帧间的相对位姿：
其中，T ci 表示当前帧与地图内第i个关键帧间的相对位姿，T cw 和T iw 分别表示当前帧和地图中第i个关键帧在世界坐标系下的位姿,R iw 和t iw 分别表示T iw 的旋转和平移部分，R cw 和t cw 分别表示T cw 的旋转和平移部分，t ci 和R ci 分别表示T ci 的旋转和平移部分。
步骤（4.2），检测该关键帧是否满足位置约束：
其中，表示欧氏距离，r c 和r i 分别表示当前帧和地图内第i个关键帧的不确定性半径；若该关键帧满足位置约束，则进一步检测姿态约束；若不满足，继续检测下一关键帧，直到遍历完整个地图。
步骤（4.3），检测该关键帧是否满足姿态约束：
其中，表示矩阵的迹；如果该关键帧也能满足的姿态约束，就认为这是一个闭环区域，对当前帧进行进一步的图片相似度检测。
步骤5，进行图片相似度检测。若检测到闭环则执行全局优化，优化整个地图；完成优化后对重新评估地图内所有关键帧的不确定性半径。
具体的，步骤5包括以下内容：
步骤（5.1），利用DBoW2模型，计算当前帧和其共视关键帧间的图片相似度：
其中，和/>分别表示当前帧和其共视关键帧的词袋向量；计算结束后，将最低得分记为S min。
步骤（5.2），检索所有与当前帧存在相同视觉单词，但不相邻的关键帧，将所有相似度大于阈值S min的关键帧列为候选闭环帧。
步骤（5.3），将每个候选闭环帧和他的共视关键帧设为一个候选关键帧组；继续对下一关键帧进行图片相似度检测。
步骤（5.4），若当前帧和上一关键帧的候选关键帧组间存在相同关键帧，则当前帧关键帧组的连续性加一。
步骤（5.5），如果当前帧存在候选关键帧组的连续性大于3，就将创建该组的候选关键帧设为闭环帧。
步骤（5.5），如果当前帧可以通过图片相似度检测，则认为检测到了一次闭环，将对整个地图进行一次全局优化；优化完成后重新计算地图内关键帧的不确定性半径：
其中，表示当前帧到对应的闭环帧间的关键帧数量。图2为本专利所用算法的系统框图。
为了进一步验证本发明的方法，本发明采用室外汽车数据KITTI进行了定位精度的实验，该数据集由一辆行驶在居民区的汽车所载相机采集，为了更清楚的表示本发明的方法在定位精度方面做出的改进，我们计算了每段轨迹的绝对轨迹误差，量化对比结果如下表1所示。
表1 KITTI室外数据集下ORB-SLAM2和本专利方法轨迹误差量化对比图
从表1可以看到，在这些序列中，本发明的方法提升了20-30%的定位精度。
接下来，对本发明的方法的闭环效率进行了验证，实验在室外数据集KITTI和室内数据集TUM下进行，TUM数据集是一个针对室内办公场景采集，由手持相机采集的数据集，本发明对KITTI和TUM数据集所有包含闭环的序列都进行了实验，并统计了过程中闭环检测所耗的总时间，实验结果如表2所示。
表2TUM和KITTI数据集下所有含有闭环区域的序列闭环检测时间对比
从表2可以看到本发明的方法节约了40%左右的闭环检测时间。随着地图规模的增加，遍历闭环数据库的时间会越来越长，位姿约束的加入对闭环检测有着重要意义。
本发明的方法已经在无人机上进行了实地实验，无人机搭载gopro运动相机和云台，实验结果证明本发明的方法可以在保证实时运行的情况下，有效提高ORB-SLAM2的定位精度和闭环效率。
本发明未尽事宜为公知技术。
上述实施方式只为说明本发明的技术构思及特点，其目的在于让熟悉此项技术的人士能够了解本发明的内容并据以实施，并不能以此限制本发明的保护范围。凡根据本发明精神实质所作的等效变化或修饰，都应涵盖在本发明的保护范围之内。
Claims (5)
1.一种基于位姿不确定性的改进ORB-SLAM2定位方法，其特征在于：该方法包括如下步骤：
步骤1、根据当前帧的共视关系创建局部地图，将局部地图点投影到当前帧，并构造重投影误差函数；
步骤2、利用步骤1得到的重投影误差函数构建代价函数，通过最小化代价函数对当前帧的位姿进行优化，利用优化结果评估当前帧和前一帧的跟踪质量，并计算当前帧的不确定性半径；
步骤3、利用步骤2所得的不确定性半径，重新计算待融合地图点的权重并对地图点进行加权融合；
步骤4、利用里程计信息和步骤2所得的不确定性半径构建位姿约束，对ORB-SLAM2的闭环检测线程进行约束，对满足位姿约束的关键帧进行闭环检测；
步骤4包括以下子步骤：
子步骤4.1、计算当前帧与地图任意关键帧间的相对位姿：
其中，Tci表示当前帧与地图内第i个关键帧间的相对位姿，Tcw和Tiw分别表示当前帧和地图中第i个关键帧在世界坐标系下的位姿,Riw和tiw分别表示Tiw的旋转和平移部分，Rcw和tcw分别表示Tcw的旋转和平移部分，tci和Rci分别表示Tci的旋转和平移部分；
子步骤4.2、检测该关键帧是否满足位置约束：
rc+ri＞||tci||
其中，||·||表示欧氏距离，rc和ri分别表示当前帧和地图内第i个关键帧的不确定性半径；若该关键帧满足位置约束，则进一步检测姿态约束，若不满足，继续检测下一关键帧，直到遍历完整个地图；
子步骤4.3、检测该关键帧是否满足姿态约束：
其中，tr(·)表示矩阵的迹；如果该关键帧也能满足的姿态约束，就认为这是一个闭环区域，对当前帧进行进一步的图片相似度检测；
步骤5、若当前帧满足位姿约束，则对当前帧进行图片相似度检测，若检测到闭环则执行全局优化，优化整个地图，完成优化后，重新计算地图内所有关键帧的不确定性半径。
2.根据权利要求1所述的基于位姿不确定性的改进ORB-SLAM2定位方法，其特征在于：步骤1包括以下子步骤：
子步骤1.1、利用当前帧在地图内的共视关系，创建局部地图；
子步骤1.2、将局部地图的所有地图点投影到当前帧上，并建立重投影误差函数：
其中，ei表示地图点到当前帧的重投影误差，Tcw表示当前帧的在世界坐标系下的位姿，表示地图点在世界坐标系下的三维坐标，/>表示三维实数向量，/>是/>在当前帧下对应的二维特征点的像素坐标，/>表示二维实数向量，h(·)是投影函数：
其中，Rcw和tcw分别表示Tcw的旋转矩阵和平移向量部分，(fx,fy)和(cx,cy)分别表示相机的焦距和主点，xi、yi、zi分别表示地图点在当前帧相机坐标系下的三维坐标，上标T表示矩阵的转置。
3.根据权利要求1所述的基于位姿不确定性的改进ORB-SLAM2定位方法，其特征在于：步骤2具体包括以下子步骤：
子步骤2.1、固定地图点，求解代价函数：
其中C表示代价函数的值，N表示局部地图的内点数量，ρh(·)表示Huber鲁棒核函数，Λi是信息矩阵，和特征点被检测到时的尺度有关；
子步骤2.2、多次优化后取最后一次的优化结果，计算平均误差：
其中c'表示当前帧优化后的平均误差；
子步骤2.3、计算当前帧的不确定性半径：
其中rc和rl分别表示当前帧和上一关键帧的不确定性半径，和/>表示当前帧和上一关键帧在世界坐标系下三维坐标，α是可靠系数，ntrack表示当前帧跟踪到的地图点数量。
4.按照权利要求1所述的基于位姿不确定性的改进ORB-SLAM2定位方法，其特征在于：步骤3包括以下子步骤：
子步骤3.1、根据不确定性半径，重新计算地图点权重：
其中，w表示地图点权重，nob表示跟踪到该地点的关键帧数量，ri表示第i个关键帧的不确定性半径；
子步骤3.2、根据地图点权重，对地图点进行加权融合，融合后的地图点位置为：
其中，pab表示地图点融合后在世界坐标系下的三维坐标，pa和pb分别表示待融合地图点a和b融合前在世界坐标系下的三维坐标，ωa和ωb分别表示待融合地图点a和b的权重；
子步骤3.3、完成融合后更新融合后地图点的权重和观测信息。
5.根据权利要求1所述的基于位姿不确定性的改进ORB-SLAM2定位方法，其特征在于，步骤(5)包括以下内容：
利用DBoW2模型，计算当前帧和其共视关键帧间的图片相似度，如果当前帧可以通过图片相似度检测，则认为检测到了一次闭环，将对整个地图进行一次全局优化，优化完成后重新计算地图内所有关键帧的不确定性半径rre：
其中，nloop表示当前帧到对应的闭环帧间的关键帧数量。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310873189.4A CN116592897B (zh) | 2023-07-17 | 2023-07-17 | 基于位姿不确定性的改进orb-slam2定位方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310873189.4A CN116592897B (zh) | 2023-07-17 | 2023-07-17 | 基于位姿不确定性的改进orb-slam2定位方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116592897A CN116592897A (zh) | 2023-08-15 |
| CN116592897B true CN116592897B (zh) | 2023-09-22 |
Family
ID=87599519
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310873189.4A Active CN116592897B (zh) | 2023-07-17 | 2023-07-17 | 基于位姿不确定性的改进orb-slam2定位方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116592897B (zh) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117893693A (zh) * | 2024-03-15 | 2024-04-16 | 南昌航空大学 | 一种密集slam三维场景重建方法及装置 |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109509230A (zh) * | 2018-11-13 | 2019-03-22 | 武汉大学 | 一种应用于多镜头组合式全景相机的slam方法 |
| CN110501017A (zh) * | 2019-08-12 | 2019-11-26 | 华南理工大学 | 一种基于orb_slam2的移动机器人导航地图生成方法 |
| CN111709893A (zh) * | 2020-06-16 | 2020-09-25 | 华南师范大学 | 一种基于信息熵和锐化调整的orb-slam2改进算法 |
| CN112240768A (zh) * | 2020-09-10 | 2021-01-19 | 西安电子科技大学 | 基于Runge-Kutta4改进预积分的视觉惯导融合SLAM方法 |
| CN112734841A (zh) * | 2020-12-31 | 2021-04-30 | 华南理工大学 | 一种用轮式里程计-imu和单目相机实现定位的方法 |
| CN113850293A (zh) * | 2021-08-20 | 2021-12-28 | 北京大学 | 基于多源数据和方向先验联合优化的定位方法 |
| CN114187447A (zh) * | 2021-12-09 | 2022-03-15 | 西安电子科技大学 | 一种基于实例分割的语义slam方法 |
| CN114255323A (zh) * | 2021-12-22 | 2022-03-29 | 深圳市普渡科技有限公司 | 机器人、地图构建方法、装置和可读存储介质 |
| CN115355904A (zh) * | 2022-07-25 | 2022-11-18 | 之江实验室 | 一种针对地面移动机器人的Lidar-IMU融合的slam方法 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR20230013883A (ko) * | 2021-07-20 | 2023-01-27 | 삼성전자주식회사 | Slam 기반의 전자 장치 및 그 동작 방법 |
-
2023
- 2023-07-17 CN CN202310873189.4A patent/CN116592897B/zh active Active
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109509230A (zh) * | 2018-11-13 | 2019-03-22 | 武汉大学 | 一种应用于多镜头组合式全景相机的slam方法 |
| CN110501017A (zh) * | 2019-08-12 | 2019-11-26 | 华南理工大学 | 一种基于orb_slam2的移动机器人导航地图生成方法 |
| CN111709893A (zh) * | 2020-06-16 | 2020-09-25 | 华南师范大学 | 一种基于信息熵和锐化调整的orb-slam2改进算法 |
| CN112240768A (zh) * | 2020-09-10 | 2021-01-19 | 西安电子科技大学 | 基于Runge-Kutta4改进预积分的视觉惯导融合SLAM方法 |
| CN112734841A (zh) * | 2020-12-31 | 2021-04-30 | 华南理工大学 | 一种用轮式里程计-imu和单目相机实现定位的方法 |
| CN113850293A (zh) * | 2021-08-20 | 2021-12-28 | 北京大学 | 基于多源数据和方向先验联合优化的定位方法 |
| CN114187447A (zh) * | 2021-12-09 | 2022-03-15 | 西安电子科技大学 | 一种基于实例分割的语义slam方法 |
| CN114255323A (zh) * | 2021-12-22 | 2022-03-29 | 深圳市普渡科技有限公司 | 机器人、地图构建方法、装置和可读存储介质 |
| CN115355904A (zh) * | 2022-07-25 | 2022-11-18 | 之江实验室 | 一种针对地面移动机器人的Lidar-IMU融合的slam方法 |
Non-Patent Citations (4)
| Title |
|---|
| A pose estimation method of space non-cooperative target based on ORBFPFH SLAM;Zhiqiang Yan;Optik -InternationalJournalforLightandElectronOptics286(2023)171025;全文 * |
| An Improved FastSLAM Using Resampling Based on Particle Fission Propagation;Haoqian Huang;IEEE;全文 * |
| RTM框架下基于点线特征的视觉SLAM算法;贾松敏;丁明超;张国梁;;机器人(03);全文 * |
| 基于改进ORB的视觉SLAM闭环检测;王远配;汤小红;;中国科技论文(14);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116592897A (zh) | 2023-08-15 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Li et al. | DXSLAM: A robust and efficient visual SLAM system with deep features | |
| Zhou et al. | To learn or not to learn: Visual localization from essential matrices | |
| CN110335319B (zh) | 一种语义驱动的相机定位与地图重建方法和系统 | |
| CN106885574B (zh) | 一种基于重跟踪策略的单目视觉机器人同步定位与地图构建方法 | |
| Fan et al. | Deep learning on monocular object pose detection and tracking: A comprehensive overview | |
| Fraundorfer et al. | Topological mapping, localization and navigation using image collections | |
| Liu et al. | Seqlpd: Sequence matching enhanced loop-closure detection based on large-scale point cloud description for self-driving vehicles | |
| CN110097584B (zh) | 结合目标检测和语义分割的图像配准方法 | |
| Cui et al. | Efficient large-scale structure from motion by fusing auxiliary imaging information | |
| Liu et al. | RGB-D inertial odometry for a resource-restricted robot in dynamic environments | |
| CN116592897B (zh) | 基于位姿不确定性的改进orb-slam2定位方法 | |
| CN111368759B (zh) | 基于单目视觉的移动机器人语义地图构建系统 | |
| Li et al. | Review of vision-based Simultaneous Localization and Mapping | |
| CN113674416A (zh) | 三维地图的构建方法、装置、电子设备及存储介质 | |
| CN110728751A (zh) | 一种室内3d点云语义地图的构建方法 | |
| CN109242899B (zh) | 一种基于在线视觉词典的实时定位与地图构建方法 | |
| Xu et al. | Probabilistic visual place recognition for hierarchical localization | |
| Xie et al. | A deep feature aggregation network for accurate indoor camera localization | |
| GB2599947A (en) | Visual-inertial localisation in an existing map | |
| GB2599948A (en) | Initialising keyframes for visual-inertial localisation and/or mapping | |
| Xie et al. | Hierarchical forest based fast online loop closure for low-latency consistent visual-inertial SLAM | |
| Vidanapathirana et al. | Spectral geometric verification: Re-ranking point cloud retrieval for metric localization | |
| Yang et al. | Probabilistic projective association and semantic guided relocalization for dense reconstruction | |
| CN116563341A (zh) | 一种处理复杂环境下动态物体的视觉定位与建图方法 | |
| Zhu et al. | LVIF: a lightweight tightly coupled stereo-inertial SLAM with fisheye camera |
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 |