CN117649536B - 一种点线和线结构特征融合的视觉同步定位与建图方法 - Google Patents
一种点线和线结构特征融合的视觉同步定位与建图方法 Download PDFInfo
- Publication number
- CN117649536B CN117649536B CN202410116907.8A CN202410116907A CN117649536B CN 117649536 B CN117649536 B CN 117649536B CN 202410116907 A CN202410116907 A CN 202410116907A CN 117649536 B CN117649536 B CN 117649536B
- Authority
- CN
- China
- Prior art keywords
- line
- line segment
- point
- ith
- projection
- 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 59
- 238000013507 mapping Methods 0.000 title claims abstract description 27
- 230000001360 synchronised effect Effects 0.000 title claims abstract description 18
- 230000000007 visual effect Effects 0.000 title claims abstract description 18
- 238000000605 extraction Methods 0.000 claims abstract description 22
- 230000008569 process Effects 0.000 claims abstract description 16
- 238000012545 processing Methods 0.000 claims abstract description 14
- 238000010276 construction Methods 0.000 claims abstract description 10
- 230000004927 fusion Effects 0.000 claims abstract description 6
- 238000003780 insertion Methods 0.000 claims abstract description 4
- 230000037431 insertion Effects 0.000 claims abstract description 4
- 239000011159 matrix material Substances 0.000 claims description 18
- 238000005457 optimization Methods 0.000 claims description 12
- 230000033001 locomotion Effects 0.000 claims description 11
- 238000004364 calculation method Methods 0.000 claims description 9
- 238000013519 translation Methods 0.000 claims description 5
- 238000012937 correction Methods 0.000 claims description 4
- 230000006870 function Effects 0.000 description 6
- 238000005516 engineering process Methods 0.000 description 5
- 238000010586 diagram Methods 0.000 description 3
- 238000002474 experimental method Methods 0.000 description 3
- 101100365794 Schizosaccharomyces pombe (strain 972 / ATCC 24843) sim3 gene Proteins 0.000 description 2
- 230000008859 change Effects 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- DMSMPAJRVJJAGA-UHFFFAOYSA-N benzo[d]isothiazol-3-one Chemical compound C1=CC=C2C(=O)NSC2=C1 DMSMPAJRVJJAGA-UHFFFAOYSA-N 0.000 description 1
- QBWCMBCROVPCKQ-UHFFFAOYSA-N chlorous acid Chemical compound OCl=O QBWCMBCROVPCKQ-UHFFFAOYSA-N 0.000 description 1
- 238000004140 cleaning Methods 0.000 description 1
- 230000000052 comparative effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 230000001788 irregular Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
Landscapes
- Image Analysis (AREA)
Abstract
本发明公开了一种点线和线结构特征融合的视觉同步定位与建图方法，包括三个线程：追踪线程、局部建图线程、回环线程；所述追踪线程包括提取特征点、提取特征线、融合线段等；所述局部建图线程包括关键帧插入、特征点和特征线处理、提取线结构、局部光束平差法及局部关键帧处理；线结构是空间两条相交线的线段组。本发明以空间两条相交的线段组为线结构，线结构与传统点、特征线信息相融合，提高了SLAM在复杂场景的精度和鲁棒性。
Description
技术领域
本发明属于同步定位与建图技术领域，具体涉及一种点线和线结构特征融合的视觉同步定位与建图方法。
背景技术
近年来，随着机器人技术的迅猛发展，同步定位与建图（SLAM）技术受到了研究人员的广泛关注。然而，传统基于点特征线的SLAM方法对场景环境要求较高，如果相机快速移动、场景纹理较少或图像模糊时，SLAM算法的精度会会受到较大影响。
视觉SLAM（VSLAM）技术由于传感器成本低廉，技术成熟，采集的信息丰富，是SLAM技术的重要分支。VSLAM本质上与基于几何环境的理解，一般先提取视频前后图像帧的特征，然后通过特征匹配，同时结合相机载体的几何约束获得移动机器人的位姿变化信息。由于点特征的获取与计算简单便捷，因此常见的VSLAM常采用点特征信息。对于这类方法，特征点的获取和进准匹配至关重要，这直接影响到算法的有效性。然而，特征点所含信息量少，相似度高，容易出现误匹配。如果处在弱纹理环境中，或者摄像机因运动或抖动而模糊，会严重干扰特征点的提取，从而影响到定位精度，甚至导致定位失败。除了点特征外，室内结构化环境提供了大量几何特征，使用更高级的几何特征并将其集成到VSLAM系统中，重建出的地图将具有更加丰富的语义特征，相比于基于点的方法来说系统更加鲁棒，因此近年来越来越多的研究人员开始考虑将图像中的几何信息，如直线、平面、物体等，作为特征用于计算机器人的位姿变化信息。特征线是一种比点特征更高级的几何特征，特征更加明显和稳定，能够更稳定的反映出图像信息，因而出现误匹配的概率更小，近年来出现不少基于点特征线融合的SLAM算法。平面特征也是一种常见的几何特征。平面模型仅需少量参数即可以密集方式重建复杂场景，因此也有些学者使用平面特征来构造和求解用于姿态估计的优化函数。近年来基于语义的VSLAM也开始受到人们关注。语义VSLAM不仅可以在映射过程中获取环境中的几何结构信息，而且可以识别环境中的对象并获取语义信息，以适应复杂的环境并执行更智能的任务。
平面、物体等更高级的语义特征因为特征明显而独特，不易混淆，匹配过程不易出错，因而算法鲁棒性较高。不过其特征抽取过程比较复杂，计算量大。此外，对于曲面和不规则物体，也难以用简单的解析式加以表达，给位姿优化及定位带来了较大的困难，因而在SLAM技术中尚难广泛应用。特征线提取相对容易，如今已经有较多的基于特征线的VSLAM算法。不过对于纹理较为丰富的场景，由于特征线众多，信息冗杂，既容易造成计算量激增，也影响算法效率，造成SLAM实时性能的瓶颈。事实上许多物体的轮廓线也表现为直线结构，这些线结构既保留了直特征线计算简单，易于表达的优点，同时也包含了一些物体结构方面的高级语义信息，将这些结构信息引入SLAM算法，有助于提高算法效率。
发明内容
为了提高在快速且运动模糊的环境下的精度及鲁棒性，本发明提供一种点线和线结构特征融合的视觉同步定位与建图方法，以空间两条相交的线段组为线结构，线结构与传统点、特征线信息相融合，提高了SLAM在复杂场景的精度和鲁棒性。
本发明通过下述技术方案来实现。一种点线和线结构特征融合的视觉同步定位与建图方法，主要包括三个线程：追踪线程、局部建图线程、回环线程；
所述追踪线程包括提取特征点、提取特征线、融合线段、初始化位姿估计，重定位或地图构建、局部地图追踪和新关键帧决策；
所述局部建图线程包括关键帧插入、特征点和特征线处理、提取线结构、局部光束平差法及局部关键帧处理；线结构是空间两条相交线的线段组；
所述提取线结构的过程为：
设局部建图线程中观测到的两条线段分别为第i条线段Li和第j条线段Lj，第i条线段Li的两个端点为Pi1，Pi2，第j条线段Lj的两个端点为Qj1，Qj2；如果两条线段相交于交点H，则进一步判断交点H与第i条线段Li和第j条线段Lj两个端点间的距离，保留与点H距离更远的端点即Pi2与Qj1这两个端点与交点H构成的线段组，即为所提取的线结构；
预先设定一个误差阈值，如果两条线段间的最短距离小于该误差阈值，则认为两条线段相交，取两条线段最短距离所在连线的中点作为两条线段的近似交点，两条线段构成的线段组，即为所提取的线结构。
进一步优选，所述回环线程包括数据库查询、计算Sim3或SE3变换、闭环校正、本质图优化和全局捆绑调整(Global Bundle Adjustment,GBA)。
进一步优选，采用ORB特征提取方法提取双目图像帧的特征点。
进一步优选，采用LSD算法提取双目图像帧的特征线，所有特征线构成线段集。
进一步优选，所述融合线段是指：需要对线段集中同属一条直线的线段进行融合，并提取其中长度大于一定阈值的线段作为判断是否构成线结构的有效线段集，用于后续处理，小于阈值的线段作为无效线段予以剔除，以保证线结构特征的稳定性。
进一步优选，融合线段的过程如下：
对于LSD算法提取的第i条线段Li与第j条线段Lj，第i条线段Li两端点坐标为 Pi1（x1，y1）与Pi2（x2，y2），第i条线段Li与水平方向的角度为θ1，第i条线段Li的长度为di。第j条线段Lj两端点坐标为Qj1（x3，y3）与Qj2（x4，y4），第j条线段Lj与水平方向的角度为θ2，第j条线段Lj的长度为dj。设定的3个阈值分别为距离阈值δd，角度阈值δθ以及第i条线段Li与第j条线段Lj之间的最近端点的距离阈值为δep。若两条线段满足以下条件：
第i条线段Li中点到第j条线段Lj的距离d1与第j条线段Lj中点到第i条线段Li的距离d2均小于距离阈值δd；
第i条线段Li与第j条线段Lj之间的角度之差小于角度阈值δθ；
第i条线段Li与第j条线段Lj不相交而且两个最近端点的距离ep小于最近端点的距离阈值δep；
则两条线段融合为两端点坐标为Z1（x5，y5）和Z2（x6，y6）的长线段，融合后的长线段与水平方向的角度为θ3。
进一步优选，所述初始化位姿估计，重定位或地图构建是：通过最小化当前图像帧与前一图像帧匹配的特征点与有效特征线的重投影误差，利用全局重定位或构建地图的方法来初始化位姿。
进一步优选，所述提取线结构的过程为：
设局部建图线程中观测到的两条线段分别为第i条线段Li(i=1,2,…,m)和第j条线段Lj(j=1,2,…,n)，第i条线段Li的两个端点为Pi1，Pi2，第j条线段Lj的两个端点为Qj1，Qj2；如果两条线段相交于交点H，则进一步判断交点H与第i条线段Li和第j条线段Lj两个端点间的距离，保留与点H距离较远的端点即Pi2与Qj1这两个端点与交点H构成的线段组，即为所提取的线结构；
预先设定一个误差阈值，如果两条线段间的最短距离小于该误差阈值，则认为两条线段相交，两条线段最短距离所在连线的中点作为两条线段的近似交点。
进一步优选，两条线段最短距离的计算方法如下：
第i条线段Li和第j条线段Lj的方程表示为：
，；
，；
其中，为沿第i条线段Li的方向向量移动倍的距离，为沿第j条线段
Lj的方向向量移动倍的距离，为第i条线段Li的方向向量，为第j条线段Lj的方向向
量，为第i条线段Li的移动倍率，为第j条线段Lj的移动倍率；
线段间的最短距离求解转化为有约束的最优化函数，如下式所示：
；
根据极小值条件：可得：
；
若，则最短距离，其中分别代表与的
最优解，表示第i条线段Li与第j条线段Lj的公垂线；否则分别求端点到第j条线段Lj的
最短距离d4、端点到第j条线段Lj的最短距离d5、端点Qj1到第i条线段Li的最短距离d6、端
点Qj2到第i条线段Li的最短距离d7，两线段最短距离dmin=min{d4,d5,d6,d7}。
进一步优选，采用局部光束平差法最小化关键帧的重投影误差；第o图像帧所观测
到的第s个线结构的第一个端点为Aws，对应的图像观测点为aos，在图像上的投影点为；线
结构的第二个端点为Bws，对应的图像观测点为bos，在图像上的投影点为；线结构的第三
个端点为Cws，对应的图像观测点为cos，在图像上的投影点为；w表示世界坐标系，第o图像
帧所观测到的第s个线结构的重投影误差eos为：
；
其中，为投影点到图像观测的线段的距离，为投影点到线段
的距离，为投影点的坐标与图像观测点bos的坐标的距离，T表示矩阵或
者向量的转置，为投影线段在相机坐标系的直线系数，为投影线段在相机坐
标系的直线系数；
；
；
其中为投影线段在三维空间的法向量，为投影线段在三维
空间的法向量，其中为投影线段斜率的分子，为投影线段斜率的分母,为
投影线段的截距。为投影线段斜率的分子, 为投影线段斜率的分母,为投影线段的截距，K为投影矩阵；
第o图像帧所观测到的第s个线结构的重投影误差eos关于位姿增量的雅可比矩
阵见下式：
；
其中：
；
；
；
其中，，表示线结构，为相机坐标系下的普吕克表达，c表示相机坐
标系，表示线段AwsBws的普吕克坐标，表示线段BwsCws的普吕克坐标，表示3行6列
矩阵；Rcw,tcw为从世界坐标系到相机坐标系的旋转矩阵和平移向量，vw为世界坐标系下线段
的方向向量，nw为线段的法向量；[]×表示反对称矩阵；
最小化各关键帧重投影误差的公式为：
；
其中P、L、S分别代表点、线段、线结构的集合；eog为第o图像帧第g个点的重投影误
差，eok为第o图像帧第k条线段的重投影误差，eos为第o图像帧所观测到的第s个线结构的重
投影误差；ρ为核函数，β为加权因子，表示相机的位姿与点、线段和线结构的三维位置组
成的向量，表示相机的位姿与点、线段和线结构的三维位置组成的向量的最优值，表
示关键帧。
本发明的有益效果：在传统点特征线的基础上增加了结构特征，通过提取物体轮廓中的线结构信息，用于位姿的预测与优化，以增加算法对复杂环境下信息提取的稳定性。以空间两条相交的线段组为线结构，并给出了线结构的重投影误差模型，并推导了误差雅可比矩阵。将本发明所提出的线结构加入ORB_LINE_SLAM中，提高了SLAM在复杂场景的精度和鲁棒性。
附图说明
图1为本发明的方法流程图。
图2为线段融合示意图。
图3为线结构特征提取示意图。
图4为两条线段距离示意图。
图5为线结构重投影误差示意图。
具体实施方式
下面结合附图和实施例进一步详细阐明本发明。
如图1所示，一种点线和线结构特征融合的视觉同步定位与建图方法，主要包括三个线程：追踪线程、局部建图线程、回环线程。
所述追踪线程包括提取特征点、提取特征线、融合线段、初始化位姿估计，重定位或地图构建、局部地图追踪和新关键帧决策。
所述局部建图线程包括关键帧插入、特征点和特征线处理、提取线结构、局部光束平差法及局部关键帧处理。
所述回环线程包括数据库查询、计算Sim3或SE3变换、闭环校正、本质图优化和全局捆绑调整(Global Bundle Adjustment,GBA)。
本实施例采用ORB特征提取方法提取双目图像帧的特征点。
本实施例采用LSD算法提取双目图像帧的特征线，所有特征线构成线段集。
LSD算法在提取特征线时可能会将长直线分割成多个短线段。同时，大量的细碎纹理产生的短线段也会影响视觉同步定位与建图方法的计算效率。实际上，长线段相比短线段更显著，更具有重复性，受到噪声的影响相对更较小。为了得到显著而且稳定的线结构特征，需要对线段集中同属一条直线的线段进行融合，并提取其中长度大于一定阈值的线段作为判断是否构成线结构的有效线段集，用于后续处理，小于阈值的线段作为无效线段予以剔除，以保证线结构特征的稳定性。
参照图2，本实施例融合线段的过程如下：
对于LSD算法提取的第i条线段Li与第j条线段Lj，第i条线段Li两端点坐标为 Pi1（x1，y1）与Pi2（x2，y2），第i条线段Li与水平方向的角度为θ1，第i条线段Li的长度为di。第j条线段Lj两端点坐标为Qj1（x3，y3）与Qj2（x4，y4），第j条线段Lj与水平方向的角度为θ2，第j条线段Lj的长度为dj。设定的3个阈值分别为距离阈值δd，角度阈值δθ以及第i条线段Li与第j条线段Lj之间的最近端点的距离阈值为δep。若两条线段满足以下条件：
（1）第i条线段Li中点到第j条线段Lj的距离d1与第j条线段Lj中点到第i条线段Li的距离d2均小于距离阈值δd；
（2）第i条线段Li与第j条线段Lj之间的角度之差小于角度阈值δθ；
（3）第i条线段Li与第j条线段Lj不相交而且两个最近端点的距离ep小于最近端点的距离阈值δep；
则两条线段融合两端点坐标为Z1（x5，y5）和Z2（x6，y6）的长线段，融合后的长线段与水平方向的角度为θ3。
本实施例所述初始化位姿估计，重定位或地图构建是：通过最小化当前图像帧与前一图像帧匹配的特征点与有效特征线的重投影误差，利用全局重定位或构建地图的方法来初始化位姿。
本实施例所述局部地图追踪和新关键帧决策，当图像帧的跟踪特征数量低于140，并且该图像帧创建了超过100个新的近距离立体特征时，则生成新的关键帧。
所述局部建图线程中的特征点和特征线处理，是指当插入关键帧之后，进一步剔除那些关键帧当中的观测次数太少，质量不高的特征点与特征线，以确保线结构的提取及后续过程的稳定。线结构的提取基于特征线，通过对所提取的有效特征线进行进一步处理得到。
线结构指的是图像帧中由多条显著的线段组成的固定组合。这些线段可能是物体轮廓线，也可能是显著的纹理线等，线段可能是直线，也可能是曲线，组合方式可以是平行线、或者垂直线等。为了处理方便，线结构是空间两条相交线的线段组。
本实施例所述提取线结构的过程为：首先初始化线结构集合，通过获取已观测的线段集中的相邻的两条线段，判断相邻的两条线段，是否为相交，若相交，则将两条线段放入线结构集合中，并清理线结构集合中的重复线段组合。
设局部建图线程中观测到的两条线段分别为第i条线段Li(i=1,2,…,m)和第j条线段Lj(j=1,2,…,n)，第i条线段Li的两个端点为Pi1，Pi2，第j条线段Lj的两个端点为Qj1，Qj2。如果两条线段相交于交点H，如图3所示。则进一步判断交点H与第i条线段Li和第j条线段Lj两个端点间的距离，保留与点H距离较远的端点即Pi2与Qj1这两个端点与交点H构成的线段组，即为所提取的线结构。
考虑到空间直线参数在感知过程中存在误差，原本实际相交的两条线段找不到交点。为此，预先设定一个误差阈值，如果两条线段间的最短距离小于该误差阈值，则认为两条线段相交，两条线段最短距离所在连线的中点作为两条线段的近似交点，如图4所示。
其中两条线段最短距离的计算方法如下：
第i条线段Li和第j条线段Lj的方程可以表示为：
，；
，；
其中，为沿第i条线段Li的方向向量移动倍的距离，为沿第j条线段
Lj的方向向量移动倍的距离，为第i条线段Li的方向向量，为第j条线段Lj的方向向
量，为第i条线段Li的移动倍率，为第j条线段Lj的移动倍率。
线段间的最短距离求解转化为有约束的最优化函数，如下式所示：
；
根据极小值条件：可得：
；
若，则最短距离，其中分别代表与的
最优解，表示第i条线段Li与第j条线段Lj的公垂线。否则分别求端点到第j条线段Lj的
最短距离d4、端点到第j条线段Lj的最短距离d5、端点Qj1到第i条线段Li的最短距离d6、端
点Qj2到第i条线段Li的最短距离d7，两线段最短距离dmin=min{d4,d5,d6,d7}。
本实施例所述局部光束平差法是：采用局部光束平差法最小化关键帧的重投影误
差。如图5所示，第o图像帧所观测到的第s个线结构的第一个端点为Aws，对应的图像观测点
为aos，在图像上的投影点为。线结构的第二个端点为Bws，对应的图像观测点为bos，在图
像上的投影点为。线结构的第三个端点为Cws，对应的图像观测点为cos，在图像上的投影
点为。w表示世界坐标系，第o图像帧所观测到的第s个线结构的重投影误差eos为：
；
其中，为投影点到图像观测的线段的距离，为投影点到线段
的距离，为投影点的坐标与图像观测点bos的坐标的距离，T表示矩阵或
者向量的转置，为投影线段在相机坐标系的直线系数，为投影线段在相机坐
标系的直线系数；
；
；
其中为投影线段在三维空间的法向量，为投影线段在三维
空间的法向量，其中为投影线段斜率的分子，为投影线段斜率的分母,为
投影线段的截距。为投影线段斜率的分子, 为投影线段斜率的分母,为投影线段的截距，K为投影矩阵。
；
其中fx，fy分别代表相机在x轴和y轴的焦距，cx，cy分别为表示相机光圈中心。投影
点由下计算：
；
第o图像帧所观测到的第s个线结构的重投影误差eos关于位姿增量的雅可比矩
阵见下式：
；
其中：
；
；
；
其中，，表示线结构，为相机坐标系下的普吕克表达，c表示相机坐
标系，表示线段AwsBws的普吕克坐标，表示线段BwsCws的普吕克坐标，表示3行6列
矩阵。Rcw,tcw为从世界坐标系到相机坐标系的旋转矩阵和平移向量，vw为世界坐标系下线段
的方向向量，nw为线段的法向量。[]×表示反对称矩阵。
最小化各关键帧重投影误差的公式为：
；
其中P、L、S分别代表点、线段、线结构的集合。eog为第o图像帧第g个点的重投影误
差，eok为第o图像帧第k条线段的重投影误差，eos为第o图像帧所观测到的第s个线结构的重
投影误差。ρ为核函数，β为加权因子，表示相机的位姿与点、线段和线结构的三维位置组
成的向量，表示相机的位姿与点、线段和线结构的三维位置组成的向量的最优值，表
示关键帧。β用来衡量线段与线结构参与优化的权重。
；
其中，表示权重计算函数，div表示取整运算。
本实施例所述局部关键帧处理过程为：为了避免地图中包含由于三角测量导致的不可追踪特征点以及无意义的重建场景特征，对进入局部优化的线段进行了限制，要求符合以下条件：
1）该线段必须在局部地图中超过1%的关键帧中检测到；
2）该线段创建后经过了多个关键帧，必须至少被5个关键帧所观测到。
某些局部关键帧可能会被剔除，特别是那些没有为地图提供足够信息或被认为是多余的帧。关键帧剔除有助于减少计算量和内存使用，优化SLAM系统的整体性能。
本实施例的回环线程中，数据库查询是指：每当新的关键帧被添加时，检测新的关键帧在活跃地图和整个地图集合中的公共区域；以找出当前观察到的场景是否与之前记录的场景匹配；数据库中保存了先前所有观察到的关键帧的特征，用于检测环路闭合。计算Sim3或SE3变换是指在确定了可能的回环之后，计算相似性变换，包括缩放、旋转和平移（Sim3）或刚体变换，仅包括旋转和平移（SE3）变换。这个Sim3或SE3变换描述了当前帧和回环帧之间的几何关系。Sim3或SE3变换计算完成，进行闭环校正，以纠正地图中的累积误差。本质图（Essential Graph）指的是一个精简的图，只包括关键帧和其连接。通过本质图优化，进一步精确关键帧之间的相对姿态，以及关键帧与全局地图的对齐。全局捆绑调整(Global Bundle Adjustment,GBA)，是一种全局优化过程，通过最小化重投影误差来同时精调所有关键帧和地图点的姿态和位置，提升精度。
选取Euroc数据集进行算法性能实验。Euroc数据集提供了两种主要的录制类型。其中Machine hall录制了5个数据集，着重展示现实工业场景； Vicon room提供了另外两种场景，分别包含了不同配置的障碍物。每个场景都由3个数据集组成，总计11个数据集，囊括了从良好视觉条件下的慢速飞行到较差照明下的动态飞行的场景。Euroc各数据集场景特点见表1所示。
表1 Euroc数据集特点
比较实验主要在本发明的方法、ORB_LINE_SLAM和经典的ORB_SLAM3之间进行。其中ORB_SLAM3，ORB_LINE_SLAM和本发明方法是双目不加IMU的视觉SLAM。实验环境配置为Intel(R) Core(TM) i5-11400H @ 2.70GHz 2.69 GHz。
三种算法的绝对轨迹差的均方根误差（RMSE）见表2。表2中的数值是取五次实验后均方根误差（RMSE）的中位数。
表2 各算法绝对轨迹差数据比较
从表2数据可以看出：本发明的方法的所有测试的数据集序列的均方根误差（RMSE）平均值为0.062，而ORB_SLAM3的均方根误差（RMSE）平均值为0.112，ORB_LINE_SLAM的均方根误差（RMSE）平均值为0.069，本发明的方法比ORB_LINE_SLAM下降了10%，比ORB_SLAM3下降了45%。其中由于MH-04-dif与MH-05-dif提供的特征线及线结构特征较少，所以RMSE没有得到较大减少。本发明的方法在具有挑战性的场景如MH-03-med，V1-02-med，V1-03-dif，V2-02-med，V2-03-dif相对与ORB_LINE_SLAM和ORB_SLAM3具有更低的RMSE，尤其是其中的V1-03-dif和V2-03-dif，V1-03-dif的RMSE比ORB_LINE_SLAM提升了43%，V2-03-dif比ORB_SLAM3提升了67%。综上所示，本发明提出的线结构显著提升了SLAM系统的准确性和鲁棒性。
以上公开的本发明优选实施例只是用于帮助阐述本发明。优选实施例并没有详尽叙述所有的细节，也不限制该本发明仅为所述的具体实施方式。显然，根据本说明书的内容，可作很多的修改和变化。本说明书选取并具体描述这些实施例，是为了更好地解释本发明的原理和实际应用，从而使所属技术领域技术人员能很好地理解和利用本发明。本发明仅受权利要求书及其全部范围和等效物的限制。
Claims (8)
1.一种点线和线结构特征融合的视觉同步定位与建图方法，包括三个线程：追踪线程、局部建图线程、回环线程；其特征在于：
所述追踪线程包括提取特征点、提取特征线、融合线段、初始化位姿估计，重定位或地图构建、局部地图追踪和新关键帧决策；
所述局部建图线程包括关键帧插入、特征点和特征线处理、提取线结构、局部光束平差法及局部关键帧处理；线结构是空间两条相交线的线段组；
所述提取线结构的过程为：
设局部建图线程中观测到的两条线段分别为第i条线段Li和第j条线段Lj，第i条线段Li的两个端点为Pi1，Pi2，第j条线段Lj的两个端点为Qj1，Qj2；如果两条线段相交于交点H，则进一步判断交点H与第i条线段Li和第j条线段Lj两个端点间的距离，保留与点H距离更远的端点即Pi2与Qj1这两个端点与交点H构成的线段组，即为所提取的线结构；
预先设定一个误差阈值，如果两条线段间的最短距离小于该误差阈值，则认为两条线段相交，取两条线段最短距离所在连线的中点作为两条线段的近似交点，两条线段构成的线段组，即为所提取的线结构；
采用局部光束平差法最小化关键帧的重投影误差；第o图像帧所观测到的第s个线结构的第一个端点为Aws，对应的图像观测点为aos，在图像上的投影点为；线结构的第二个端点为Bws，对应的图像观测点为bos，在图像上的投影点为/>；线结构的第三个端点为Cws，对应的图像观测点为cos，在图像上的投影点为/>；w表示世界坐标系，第o图像帧所观测到的第s个线结构的重投影误差eos为：
；
其中，为投影点/>到图像观测的线段/>的距离，/>为投影点/>到线段/>的距离，/>为投影点/>的坐标/>与图像观测点bos的坐标/>的距离，T表示矩阵或者向量的转置，/>为投影线段/>在相机坐标系的直线系数，/>为投影线段/>在相机坐标系的直线系数；
；
；
其中为投影线段/>在三维空间的法向量，/>为投影线段/>在三维空间的法向量，其中/>为投影线段/>斜率的分子，/>为投影线段/>斜率的分母,/>为投影线段/>的截距；/>为投影线段/>斜率的分子, />为投影线段/>斜率的分母,/>为投影线段/>的截距，K为投影矩阵；
第o图像帧所观测到的第s个线结构的重投影误差eos关于位姿增量的雅可比矩阵/>见下式：
；
其中：
；
；
；
其中，，/>表示线结构，/>为相机坐标系下的普吕克表达，c表示相机坐标系，表示线段AwsBws的普吕克坐标，/>表示线段BwsCws的普吕克坐标，/>表示3行6列矩阵；Rcw,tcw为从世界坐标系到相机坐标系的旋转矩阵和平移向量，vw为世界坐标系下线段的方向向量，nw为线段的法向量；[]×表示反对称矩阵；
最小化各关键帧重投影误差的公式为：
；
其中P、L、S分别代表点、线段、线结构的集合；eog为第o图像帧第g个点的重投影误差，eok为第o图像帧第k条线段的重投影误差，eos为第o图像帧所观测到的第s个线结构的重投影误差；ρ为核函数，β为加权因子，表示相机的位姿与点、线段和线结构的三维位置组成的向量，/>表示相机的位姿与点、线段和线结构的三维位置组成的向量的最优值，/>表示关键帧。
2.根据权利要求1所述的一种点线和线结构特征融合的视觉同步定位与建图方法，其特征在于：所述回环线程包括数据库查询、计算Sim3或SE3变换、闭环校正、本质图优化和全局捆绑调整。
3.根据权利要求1所述的一种点线和线结构特征融合的视觉同步定位与建图方法，其特征在于：采用ORB特征提取方法提取双目图像帧的特征点。
4.根据权利要求1所述的一种点线和线结构特征融合的视觉同步定位与建图方法，其特征在于：采用LSD算法提取双目图像帧的特征线，所有特征线构成线段集。
5.根据权利要求4所述的一种点线和线结构特征融合的视觉同步定位与建图方法，其特征在于：所述融合线段是指：需要对线段集中同属一条直线的线段进行融合，并提取其中长度大于一定阈值的线段作为判断是否构成线结构的有效线段集，用于后续处理，小于阈值的线段作为无效线段予以剔除。
6.根据权利要求5所述的一种点线和线结构特征融合的视觉同步定位与建图方法，其特征在于：融合线段的过程如下：
对于LSD算法提取的第i条线段Li与第j条线段Lj，第i条线段Li两端点坐标为 Pi1（x1，y1）与Pi2（x2，y2），第i条线段Li与水平方向的角度为θ1，第i条线段Li的长度为di；第j条线段Lj两端点坐标为Qj1（x3，y3）与Qj2（x4，y4），第j条线段Lj与水平方向的角度为θ2，第j条线段Lj的长度为dj；设定的3个阈值分别为距离阈值δd，角度阈值δθ以及第i条线段Li与第j条线段Lj之间的最近端点的距离阈值为δep；若两条线段满足以下条件：
第i条线段Li中点到第j条线段Lj的距离d1与第j条线段Lj中点到第i条线段Li的距离d2均小于距离阈值δd；
第i条线段Li与第j条线段Lj之间的角度之差小于角度阈值δθ；
第i条线段Li与第j条线段Lj不相交而且两个最近端点的距离ep小于最近端点的距离阈值δep；
则两条线段融合为两端点坐标为Z1（x5，y5）和Z2（x6，y6）的长线段，融合后的长线段与水平方向的角度为θ3。
7.根据权利要求1所述的一种点线和线结构特征融合的视觉同步定位与建图方法，其特征在于：所述初始化位姿估计，重定位或地图构建是：通过最小化当前图像帧与前一图像帧匹配的特征点与有效特征线的重投影误差，利用全局重定位或构建地图的方法来初始化位姿。
8.根据权利要求1所述的一种点线和线结构特征融合的视觉同步定位与建图方法，其特征在于：两条线段最短距离的计算方法如下：
第i条线段Li和第j条线段Lj的方程表示为：
，/>；
，/>；
其中，为沿第i条线段Li的方向向量移动/>倍的距离，/>为沿第j条线段Lj的方向向量移动/>倍的距离，/>为第i条线段Li的方向向量，/>为第j条线段Lj的方向向量，/>为第i条线段Li的移动倍率，/>为第j条线段Lj的移动倍率；
线段间的最短距离求解转化为有约束的最优化函数，如下式所示：
；
根据极小值条件：可得：
；
若，则最短距离/>，其中/>分别代表/>与/>的最优解，/>表示第i条线段Li与第j条线段Lj的公垂线；否则分别求端点/>到第j条线段Lj的最短距离d4、端点/>到第j条线段Lj的最短距离d5、端点Qj1到第i条线段Li的最短距离d6、端点Qj2到第i条线段Li的最短距离d7，两线段最短距离dmin=min{d4,d5,d6,d7}。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410116907.8A CN117649536B (zh) | 2024-01-29 | 2024-01-29 | 一种点线和线结构特征融合的视觉同步定位与建图方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410116907.8A CN117649536B (zh) | 2024-01-29 | 2024-01-29 | 一种点线和线结构特征融合的视觉同步定位与建图方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117649536A CN117649536A (zh) | 2024-03-05 |
| CN117649536B true CN117649536B (zh) | 2024-04-16 |
Family
ID=90048085
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410116907.8A Active CN117649536B (zh) | 2024-01-29 | 2024-01-29 | 一种点线和线结构特征融合的视觉同步定位与建图方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117649536B (zh) |
Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108682027A (zh) * | 2018-05-11 | 2018-10-19 | 北京华捷艾米科技有限公司 | 基于点、线特征融合的vSLAM实现方法及系统 |
| CN110782494A (zh) * | 2019-10-16 | 2020-02-11 | 北京工业大学 | 一种基于点线融合的视觉slam方法 |
| CN111337947A (zh) * | 2020-05-18 | 2020-06-26 | 深圳市智绘科技有限公司 | 即时建图与定位方法、装置、系统及存储介质 |
| CN111542860A (zh) * | 2016-12-30 | 2020-08-14 | 迪普迈普有限公司 | 用于自主车辆的高清地图的标志和车道创建 |
| CN112509044A (zh) * | 2020-12-02 | 2021-03-16 | 重庆邮电大学 | 一种基于点线特征融合的双目视觉slam方法 |
| CN113192140A (zh) * | 2021-05-25 | 2021-07-30 | 华中科技大学 | 一种基于点线特征的双目视觉惯性定位方法和系统 |
| CN115855018A (zh) * | 2022-11-21 | 2023-03-28 | 山东科技大学 | 一种基于点线综合特征的改进同步定位与建图方法 |
| WO2023184968A1 (zh) * | 2022-04-02 | 2023-10-05 | 华南理工大学 | 一种基于点线面特征的结构化场景视觉slam方法 |
| CN117011660A (zh) * | 2023-08-23 | 2023-11-07 | 成都理工大学 | 一种低纹理场景下融合深度信息的点线特征slam方法 |
| CN117218195A (zh) * | 2023-09-11 | 2023-12-12 | 福州大学 | 基于点、线特征约束的同时定位与建图方法 |
| CN117253003A (zh) * | 2023-09-20 | 2023-12-19 | 盐城工学院 | 一种融合直接法与点面特征法的室内rgb-d slam方法 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2018049581A1 (zh) * | 2016-09-14 | 2018-03-22 | 浙江大学 | 一种同时定位与地图构建方法 |
-
2024
- 2024-01-29 CN CN202410116907.8A patent/CN117649536B/zh active Active
Patent Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111542860A (zh) * | 2016-12-30 | 2020-08-14 | 迪普迈普有限公司 | 用于自主车辆的高清地图的标志和车道创建 |
| CN108682027A (zh) * | 2018-05-11 | 2018-10-19 | 北京华捷艾米科技有限公司 | 基于点、线特征融合的vSLAM实现方法及系统 |
| CN110782494A (zh) * | 2019-10-16 | 2020-02-11 | 北京工业大学 | 一种基于点线融合的视觉slam方法 |
| CN111337947A (zh) * | 2020-05-18 | 2020-06-26 | 深圳市智绘科技有限公司 | 即时建图与定位方法、装置、系统及存储介质 |
| CN112509044A (zh) * | 2020-12-02 | 2021-03-16 | 重庆邮电大学 | 一种基于点线特征融合的双目视觉slam方法 |
| CN113192140A (zh) * | 2021-05-25 | 2021-07-30 | 华中科技大学 | 一种基于点线特征的双目视觉惯性定位方法和系统 |
| WO2023184968A1 (zh) * | 2022-04-02 | 2023-10-05 | 华南理工大学 | 一种基于点线面特征的结构化场景视觉slam方法 |
| CN115855018A (zh) * | 2022-11-21 | 2023-03-28 | 山东科技大学 | 一种基于点线综合特征的改进同步定位与建图方法 |
| CN117011660A (zh) * | 2023-08-23 | 2023-11-07 | 成都理工大学 | 一种低纹理场景下融合深度信息的点线特征slam方法 |
| CN117218195A (zh) * | 2023-09-11 | 2023-12-12 | 福州大学 | 基于点、线特征约束的同时定位与建图方法 |
| CN117253003A (zh) * | 2023-09-20 | 2023-12-19 | 盐城工学院 | 一种融合直接法与点面特征法的室内rgb-d slam方法 |
Non-Patent Citations (2)
| Title |
|---|
| Straight line detection algorithm of truck point cloud based on statistical line chart;Ming Wei;《2022 34th Chinese Control and Decision Conference (CCDC)》;20230214;第1206-1211页 * |
| 一种点线特征融合的双目同时定位与地图构建方法;蒋林;夏旭洪;韩璐;邱存勇;张泰;宋杰;;科学技术与工程;20200428(第12期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117649536A (zh) | 2024-03-05 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109166149B (zh) | 一种融合双目相机与imu的定位与三维线框结构重建方法与系统 | |
| CN110223348B (zh) | 基于rgb-d相机的机器人场景自适应位姿估计方法 | |
| CN112634451B (zh) | 一种融合多传感器的室外大场景三维建图方法 | |
| CN111739063A (zh) | 一种基于多传感器融合的电力巡检机器人定位方法 | |
| CN112258600A (zh) | 一种基于视觉与激光雷达的同时定位与地图构建方法 | |
| CN113432600B (zh) | 基于多信息源的机器人即时定位与地图构建方法及系统 | |
| CN111583369A (zh) | 一种基于面线角点特征提取的激光slam方法 | |
| CN108564616A (zh) | 快速鲁棒的rgb-d室内三维场景重建方法 | |
| CN113902860A (zh) | 一种基于多线激光雷达点云的多尺度静态地图构建方法 | |
| CN112419497A (zh) | 基于单目视觉的特征法与直接法相融合的slam方法 | |
| CN116468786B (zh) | 一种面向动态环境的基于点线联合的语义slam方法 | |
| CN110851978B (zh) | 一种基于可见性的摄像机位置优化方法 | |
| CN116878501A (zh) | 一种基于多传感器融合的高精度定位与建图系统及方法 | |
| CN114648584B (zh) | 一种用于多源融合定位的鲁棒性控制方法和系统 | |
| Zhang et al. | A visual-inertial dynamic object tracking SLAM tightly coupled system | |
| Sheng et al. | Mobile robot localization and map building based on laser ranging and PTAM | |
| CN117649536B (zh) | 一种点线和线结构特征融合的视觉同步定位与建图方法 | |
| Zhang et al. | Lidar odometry and mapping based on two-stage feature extraction | |
| Shang et al. | Model-based tracking by classification in a tiny discrete pose space | |
| CN115855018A (zh) | 一种基于点线综合特征的改进同步定位与建图方法 | |
| Svedman et al. | Structure from stereo vision using unsynchronized cameras for simultaneous localization and mapping | |
| Li | Research on RGB-D SLAM Dynamic Environment Algorithm Based on YOLOv8 | |
| CN114821113A (zh) | 基于自适应鲁棒核的单目视觉惯性slam方法与系统 | |
| WO2022073172A1 (zh) | 一种基于点线特征的机器人全局最优视觉定位方法及装置 | |
| CN113850293A (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 |