CN113850293B - 基于多源数据和方向先验联合优化的定位方法 - Google Patents

基于多源数据和方向先验联合优化的定位方法 Download PDF

Info

Publication number
CN113850293B
CN113850293B CN202110959819.0A CN202110959819A CN113850293B CN 113850293 B CN113850293 B CN 113850293B CN 202110959819 A CN202110959819 A CN 202110959819A CN 113850293 B CN113850293 B CN 113850293B
Authority
CN
China
Prior art keywords
map
point cloud
point
image
points
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
CN202110959819.0A
Other languages
English (en)
Other versions
CN113850293A (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.)
Peking University
Original Assignee
Peking University
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 Peking University filed Critical Peking University
Priority to CN202110959819.0A priority Critical patent/CN113850293B/zh
Publication of CN113850293A publication Critical patent/CN113850293A/zh
Application granted granted Critical
Publication of CN113850293B publication Critical patent/CN113850293B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/23—Clustering techniques
    • G06F18/232—Non-hierarchical techniques
    • G06F18/2321—Non-hierarchical techniques using statistics or function optimisation, e.g. modelling of probability density functions
    • G06F18/23213—Non-hierarchical techniques using statistics or function optimisation, e.g. modelling of probability density functions with fixed number of clusters, e.g. K-means clustering
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/25—Fusion techniques
    • 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
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/40—Analysis of texture
    • 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/521—Depth or shape recovery from laser ranging, e.g. using interferometry; from the projection of structured light
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Theoretical Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Data Mining & Analysis (AREA)
  • General Physics & Mathematics (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Evolutionary Biology (AREA)
  • Evolutionary Computation (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • General Engineering & Computer Science (AREA)
  • Artificial Intelligence (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Optics & Photonics (AREA)
  • Probability & Statistics with Applications (AREA)
  • Image Processing (AREA)
  • Image Analysis (AREA)

Abstract

本发明公布了一种基于多源数据和方向先验联合优化的定位方法，融合多源数据和地图方向先验信息，通过融合多种几何约束进行联合优化，利用多源数据和地图方向先验所蕴含的多重几何线索设计二维、三维和方向先验的能量函数，实现车载平台同步定位与地图构建，从而求解出准确的车载平台定位结果。本发明可应用于车载平台同步定位与地图构建，通过从灭点得到少量地图方向先验，并结合二维、三维、方向先验的能量函数并进行联合优化，从图像、激光点云序列和少量地图方向得到每个时刻的六自由度位姿，提高车载平台定位精度。

Description

基于多源数据和方向先验联合优化的定位方法
技术领域
本发明属于计算机视觉和机器人技术领域，涉及同步定位与地图构建技术，具体涉及一种基于多源数据和方向先验联合优化的定位方法，可应用于车载平台同步定位与地图构建，通过从灭点得到少量地图方向先验，并结合二维、三维、方向先验的能量函数并进行联合优化，用于从图像、激光点云序列和少量地图方向得到每个时刻的六自由度位姿，提高车载平台定位精度。
背景技术
同步定位与地图构建(SLAM)是当前计算机视觉和机器人领域中的热门技术。它是人类和移动智能体的基本需求，得到了相关产业界的关注和应用，成为各类机器人、无人驾驶、虚拟现实等新兴应用的核心基础算法。SLAM算法想要解决的问题是，在未知环境中利用传感器(比如相机、激光雷达、惯性测量单元等)运动时获取的观测数据，求解出自身的六自由度位姿，与此同时建立周围环境的三维地图。现有经典视觉SLAM系统仅从图像上提取颜色或几何信息作为视觉线索来求解位姿，例如文献[1](Mur-Artal,Raul,and Juan D.Tardós."Orb-slam2:An open-source slam system for monocular,stereo,and rgb-dcameras."IEEE transactions on robotics 33.5(2017):1255-1262.)，但仅依靠相机会面临单目相机的尺度漂移，剧烈光照变化，纹理缺失等挑战性场景下定位失败的问题。也有方法尝试将图像和激光雷达点云进行数据关联的融合，主要是把激光雷达点云的深度信息结合到视觉线索上，例如文献[2](Huang,Shi-Sheng,et al."Lidar-monocular visualodometry using point and line features."2020IEEE International Conference onRobotics and Automation(ICRA).IEEE,2020.)，但该方法可能受限于激光雷达的线束数量限制，图像中的点、线特征并非总能得到完全对应的质量好的深度值。上述方法都属于松耦合，没有能充分利用多源数据、多重线索的互补性，所以方法的定位精度不高，需要提升。
发明内容
为了克服上述现有技术存在的不足，本发明提出一种基于多源数据和方向先验联合优化的定位方法，更加紧密地融合多源数据和地图方向先验，从融合多种几何约束进行联合优化的角度，利用多源数据和地图方向先验所蕴含的多重几何线索设计二维、三维、方向先验的能量函数，来解决车载平台同步定位与地图构建的技术问题，能求解出更准确的车载平台定位结果。
图像能提供丰富的场景表观信息，而激光雷达点云能得到更准确的三维场景几何结构，地图的主要方向能进一步约束汽车行驶的朝向的一个分量。本发明包括以下几方面：1)对每帧激光雷达点云计算特征，基于隐式移动最小二乘(IMLS)，通过点云-地图的点云配准得到每帧点云的初始位姿，输入到视觉同步定位与地图构建(SLAM)系统ORB-SLAM2(Oriented FAST and rotated BRIEF-Simultaneous localization and mapping v2，即基于ORB的同步定位与地图构建中，建立关键帧3d点云间基于隐式移动最小二乘(IMLS)约束；2)通过ORB-SLAM2使用上述1)的点云的初始位姿来得到包含图像和点云的关键帧数据结构，并建立稀疏视觉ORB特征点地图，其中含有关键帧间共视图，与之对应的是2d特征-3d地图点之间的对应关系，即建立2d特征-3d地图点之间的重投影误差约束；3)检测图像灭点，并利用少量的地图三维方向先验信息和灭点建立三维方向的投影约束，即建立地图三维方向先验的投影约束；最终联合以上三类约束：2d特征-3d地图点之间的重投影误差约束、关键帧3d点云间基于隐式移动最小二乘(IMLS)约束、少量地图三维方向先验的投影约束一同构建能量函数并进行优化。
本发明方法输入的数据模态包括双目RGB图像和点云，均可采用车载平台的相机和激光雷达采集到的数据。本发明提供的基于多源数据和方向先验联合优化的定位方法详细技术方案包括如下步骤：
A.基于隐式移动最小二乘(IMLS)方法，进行激光点云配准，得到每帧点云的初始位姿；
A1.点云数据预处理，包括以下4个步骤：
1)使用线性运动模型对点云进行校正，解决激光雷达自运动的问题；
2)去除点云中的运动物体，这里分为两种情况：如果是将KITTI数据集(参考Geiger,Andreas,Philip Lenz,and Raquel Urtasun."Are we ready for autonomousdriving？the kitti vision benchmark suite."2012IEEE conference on computervision and pattern recognition.IEEE,2012.)中的点云作为输入，可以使用来自公开数据集SemanticKITTI(参考Behley,Jens,et al."Semantickitti:A dataset for semanticscene understanding of lidar sequences."Proceedings of the IEEE/CVFInternational Conference on Computer Vision.2019.)的点云语义标签，滤除掉点云语义标签被标记为运动物体类的点；对于其他点云数据，由于没有现有的点云语义标签，需对三维点云进行聚类得到若干点云团，去除体积较小的点云团，认为这些点云团是运动物体；
3)体素网格下采样；
4)邻域半径滤波；
A2.点云特征的计算。
对上一步预处理之后的点云，使用每点的邻域计算其特征向量，该特征向量的计算方式来自文献(Deschaud,Jean-Emmanuel."IMLS-SLAM:scan-to-model matching basedon 3D data."2018IEEE International Conference on Robotics and Automation(ICRA).IEEE,2018.)，最后对于点云中的每一个点，都得到了它的特征向量，向量长度为9；
A3.构建历史点云地图，基于隐式移动最小二乘求解点到面的ICP(IterativeClosest Point，迭代最近点)，得到当前帧点云的位姿；
B.基于ORB-SLAM2来构建包含2d特征-3d地图点之间的重投影误差约束、关键帧3d点云间基于隐式移动最小二乘(IMLS)约束的图优化(graph-based optimization)；
B1.对来自车载平台上的相机采集的双目RGB图像进行数据处理，包含转为双目灰度图像和ORB特征提取；
B2.使用步骤A中得到的每帧数据的位姿，在每帧双目灰度图像和稀疏视觉ORB特征点地图建立2d特征-3d地图点的对应关系，也就得到了图像帧之间ORB特征间的对应关系；
B3.把步骤A2处理后的含有特征的点云数据和同时刻的双目灰度图像封装为新的关键帧数据结构，其中，激光雷达点云转为相机参考系下，该关键帧数据结构还包含了B1中对双目灰度图提取的ORB特征和A2中得到的点云特征；
B4.ORB-SLAM2的局部建图(Local mapping)线程不再进行捆集调整(Bundleadjustment)，只对稀疏视觉ORB特征点地图插入新关键帧、更新共视关系、三角化建立新的地图点等；
B5.取出全部的关键帧，根据视觉地图中图像2d特征-3d地图点之间的对应关系建立地图点到关键帧上特征点的视觉重投影误差，对于任一关键帧，取出和其有共视关系且时间戳在其之前的关键帧，两两之间建立3d-3d间的对应，仍是基于隐式移动最小二乘的匹配；
C.结合少量地图三维方向先验的投影约束进行联合优化；包括：
C1.检测每帧图像的灭点(Vanishing point)并保存；
C2.对灭点根据投影几何得到三维方向，通过三维空间的mean-shift聚类，得到若干个聚类中心，这些聚类中心所代表的方向就是来自于地图的少量三维方向先验，而且只有一个自由度的信息；
C3.输入得到的少量三维方向先验到对应灭点所在图像的关键帧数据结构中，于是这些三维方向先验都对应于各自关键帧，建立三维方向到关键帧上灭点的投影误差约束；
C4.联合B5中得到的2d特征-3d地图点之间的重投影误差约束、关键帧3d点云间基于隐式移动最小二乘(IMLS)约束和C3中得到的少量地图三维方向先验的投影约束，构建出能量函数进行全局联合优化，求解出各关键帧位姿，即实现了定位。
通过上述步骤，即实现基于多源数据和方向先验联合优化的定位。
与现有技术相比，本发明的有益技术效果是：
本发明提出一种基于多源数据和方向先验联合优化的定位方法，更加紧密的融合多源数据和地图方向先验的方法，从融合多种几何约束进行联合优化的角度来求解车载平台同步定位与地图构建的问题。解决了仅使用图像时面临尺度模糊，弱纹理等困难场景下定位失败的问题，也避免了直接把激光雷达点云深度投影到图像时深度值不完全对应的情况。本发明设计的二维、三维、方向先验的能量函数是一种能比较充分的利用多源数据和地图方向先验所蕴含的多重几何线索的方法，能求解出更准确的车载平台定位结果，提升定位精度。
附图说明
图1是本发明提供的基于多源数据和方向先验联合优化的定位方法的整体流程图。
图2是本发明中灭点检测方法和方向先验方法的示意图。
图3本发明中建立联合优化的示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明中的技术方案进行清楚完整的描述，但不以任何方式限制本发明的范围。
本发明提供一种基于多源数据和方向先验联合优化的定位方法，如图1所示的本发明的整体流程图，主要包括如下步骤：
1)基于隐式移动最小二乘(IMLS)的激光点云配准；
2)基于ORB-SLAM2，接受每帧点云的位姿得到关键帧，构建图优化(graph-basedoptimization)，包含2d特征-3d地图点之间的重投影误差约束、关键帧3d点云间基于隐式移动最小二乘(IMLS)约束；
3)从图像中检测灭点，并在三维空间对方向聚类，得到少量的聚类中心方向，和图像灭点坐标建立投影约束，和2)中的2d-3d，3d-3d一同构建能量函数进行全局联合优化。
本发明具体实施时，基于隐式移动最小二乘(IMLS)的激光点云配准包括以下步骤：
a)如发明内容中A1所述进行点云的预处理。使用线性运动模型对点云进行校正、去除点云中的运动物体这两个步骤的技术细节来自参考文献(Deschaud,Jean-Emmanuel."IMLS-SLAM:scan-to-model matching based on 3D data."2018IEEE InternationalConference on Robotics and Automation(ICRA).IEEE,2018.)。使用pcl点云处理库进行保留重心的体素网格下采样，精度为0.7m；邻域半径滤波，1.5m半径邻域内点数少于3就被删掉。
b)经过预处理后的点云，进行接下来的2个步骤，如发明内容中A2～A3中所述，这些步骤的技术细节同样来自文献(Deschaud,Jean-Emmanuel."IMLS-SLAM:scan-to-modelmatching based on 3D data."2018IEEE International Conference on Robotics andAutomation(ICRA).IEEE,2018.)，故此处不再赘述。这里最重要的就是建立3d-3d对应关系时的隐式移动最小二乘距离的计算，该算法在后面联合优化时建立关键帧间3d-3d约束时还要用到。
接下来，利用ORB-SLAM2构建联合优化图结构，包含2d-3d，3d-3d的多源数据几何约束，下面具体介绍(具体地，还可进一步参考文献(Mur-Artal,Raul,and Juan D.Tardós."Orb-slam2:An open-source slam system for monocular,stereo,and rgb-dcameras."IEEE transactions on robotics 33.5(2017):1255-1262.))：
a)Tracking线程不再进行基于图像的姿态求解，而是直接输入同一时刻点云的位姿，注意要先通过激光雷达和相机之间的标定好的相对位姿把点云转换到相机参考系下，然后在图像的特征点和当前的稀疏特征点的三维地图建立2d-3d对应关系。该线程得到的图像关键帧和得到的计算过特征后的点云数据结合构建新的关键帧数据结构；
b)Local Mapping线程不再进行原本的视觉对应关系的捆集调整(Bundleadjustment)，只进行对稀疏视觉特征点地图插入新关键帧，更新存储关键帧图像间特征点对应关系的共视图，三角化建立新的地图点，剔除冗余的关键帧和地图点。
c)Loop Closing线程也不再进行回环检测和矫正，只是在最后所有帧数据处理完毕后，开启新线程进行全局的联合优化。联合优化前，就要去建立包含2d-3d，3d-3d对应关系的约束。2d图像特征和3d特征点间的对应关系建立和ORB-SLAM2中一致，关键帧间3d-3d的约束的建立步骤如下：
i.从当前地图中得到关键帧集合：每帧对应的点云/>激光雷达点云地图中的三维点集合：/>关键帧k的共视关键帧集合：Ck。
ii.对于任一关键帧k∈[1，N-1]，得到时间戳在它之前的共视关键帧{j|j∈Ck，j<k}。在(k，j)之间建立3d-3d对应关系的约束，其中关于点到隐式平面的距离的计算仍参考IMLS-SLAM。于是就得到了包含2d-3d，3d-3d的多源数据几何约束的优化图结构。
最后结合来自地图的方向先验，和图像灭点构造一个自由度的方向约束，具体过程如下：
a)从每个双目图像的左目图像检测灭点(Vanishing points)，方法可参考(Tardif J.-P.,Non-iterative Approach for Fast and Accurate Vanishing PointDetection,12th IEEE International Conference on Computer Vision,Kyoto,Japan,September 27-October 4,2009)，每张图像检测到至多3个灭点，保存像素坐标；
b)利用GPS或先验地图提供的含有灭点的图像相对于世界坐标系的旋转变换矩阵Rwc，已知相机内参矩阵K，上一步得到的灭点的像素坐标向量v，则该帧图像的灭点对应的在世界系下的三维方向向量：
D＝Rwc*K-1*v
对所有三维方向进行三维空间上的均值漂移(Mean shift)聚类，保留分布在地图上主要直线区域的少量三维方向簇，并保存聚类中心，和图像以及灭点相对应。
c)在联合优化图的基础上，对于那些含有灭点和三维方向的关键帧，建立该聚类中心的三维方向到图像灭点的投影误差，参照图2，图中顶部为灭点检测的3个步骤，底部为先验方向约束方法的展示，世界坐标系下的方向先验D通过当前关键帧k的旋转矩阵Rkw和相机内参K投影(沿着相机光心O和方向先验D间的虚线)到图像上，该投影位置和图像检测到的灭点v之间的欧氏距离即为三维方向到图像灭点的投影误差：
注意这里的方向约束只能提供一个自由度的约束，即沿垂直于地面的轴上的旋转，而另外两个周上的旋转不受该约束限制，所以这是一个比较弱的方向约束。
d)我们还建立了来自于图像和点云数据的两类约束。对于2d-3d对应关系，每个关键帧上特征点和视觉特征点地图之间匹配关系可用集合χk表示，(xi,Xi)表示一对图像特征点和三维点，于是可以建立视觉地图中的三维特征点和图像二维特征点间的重投影误差：
对于3d-3d对应关系，如上一步骤所述，对于一组有共视关系的关键帧对(k,j)，可以写出基于IMLS的损失函数：
其中，xk是关键帧k的点云中被选择用来参与匹配的点，yj是xk在j中隐式移动最小二乘平面的投影点，为yj处的法线。
最后，我们把以上三类损失函数加权组合在一起，即为本发明提出的融合多源数据和地图方向先验的能量函数：
其中，(α,β,γ)为三类约束的权重，实际上是根据三类约束的数量来设置，属于可调的超参数，最终联合优化的图结构见图3，图中黑色箭头左侧为捆集调整，它是ORB-SLAM2原始的优化结构，右侧联合优化图为本发明提出的能量函数Etotal代表的优化图结构，其中2d特征-3d地图点之间的重投影误差约束即为E(k,i)，关键帧3d点云间基于隐式移动最小二乘(IMLS)约束为E(k,j)，少量地图三维方向先验D的投影约束为ED(k)。
需要注意的是，公布实施例的目的在于帮助进一步理解本发明，但是本领域的技术人员可以理解：在不脱离本发明及所附权利要求的范围内，各种替换和修改都是可能的。因此，本发明不应局限于实施例所公开的内容，本发明要求保护的范围以权利要求书界定的范围为准。

Claims (7)

1.一种基于多源数据和方向先验联合优化的定位方法，其特征是，融合多源数据和地图方向先验信息，通过融合多种几何约束进行联合优化，利用多源数据和地图方向先验所蕴含的多重几何线索设计二维、三维和方向先验的能量函数，实现车载平台同步定位与地图构建，从而求解出准确的车载平台定位结果；包括以下步骤：
1）对每帧激光雷达点云计算特征，基于隐式移动最小二乘IMLS，通过点云-地图的点云配准得到每帧点云的初始位姿，输入到基于ORB的视觉同步定位与地图构建SLAM系统ORB-SLAM2中；
2）使用1）中得到的点云的初始位姿，通过ORB-SLAM2得到包含图像和点云的关键帧数据结构，并建立稀疏视觉ORB特征点地图，其中包括关键帧间共视图，表示图像2d特征-3d地图点之间的对应关系，即建立2d特征-3d地图点之间的重投影误差约束；
根据图像2d特征-3d地图点之间的对应关系，建立地图点到关键帧上特征点的视觉重投影误差，对于任一关键帧，取出与其有共视关系且时间戳在其之前的关键帧，两两之间建立关键帧3d-3d间的对应，即建立关键帧3d点云间基于隐式移动最小二乘IMLS约束；
3）检测图像灭点，并利用少量的地图三维方向先验信息和灭点建立三维方向的投影约束，即建立地图三维方向先验的投影约束；
再联合2d特征-3d地图点之间的重投影误差约束、关键帧3d点云间基于隐式移动最小二乘（IMLS）约束、地图三维方向先验的投影约束三类约束，构建能量函数并进行优化；
利用少量的地图三维方向先验信息和灭点建立三维方向的投影约束，具体过程如下：
从每个双目图像的左目图像检测灭点，每张图像检测到至多3个灭点，保存像素坐标；
利用GPS或先验地图提供的含有灭点的图像相对于世界坐标系的旋转变换矩阵，已知相机内参矩阵/>、灭点的像素坐标向量/>，则该帧图像的灭点对应的在世界系下的三维方向向量表示为：
对所有三维方向进行三维空间上的聚类，保留分布在地图上主要直线区域的少量三维方向簇，并保存聚类中心，和图像以及灭点相对应；
基于联合优化图，对于含有灭点和三维方向的关键帧，建立该聚类中心的三维方向到图像灭点的投影误差；世界坐标系下的方向先验通过当前关键帧k的旋转矩阵/>和相机内参/>投影到图像上，该投影位置和图像检测到的灭点/>之间的欧氏距离即为三维方向到图像灭点的投影误差，表示为：
其中，方向约束只能提供一个自由度的约束，即沿垂直于地面的轴上的旋转；另外两个周上的旋转不受该约束限制；
建立来自于图像和点云数据的两类约束，分别为：2d-3d对应关系约束和3d-3d对应关系约束；
对于2d-3d对应关系：每个关键帧上特征点和视觉特征点地图之间匹配关系采用集合表示，/>表示一对图像特征点和三维点，建立视觉地图中的三维特征点和图像二维特征点间的重投影误差，表示为：
对于3d-3d对应关系：对于一组有共视关系的关键帧对，基于IMLS的损失函数表示为：
其中，是关键帧k的点云中被选择用来参与匹配的点，/>是/>在j中隐式移动最小二乘平面的投影点，/>为/>处的法线；
将三类约束相应的三类损失函数加权，建立融合多源数据和地图方向先验的能量函数，表示为：
其中，为三类约束的权重，重投影误差约束为/>，IMLS帧间约束为/>，方向先验/>到关键帧的投影约束为/>；
结合地图三维方向先验的投影约束进行联合优化具体包括：
C1.检测每帧图像的灭点并保存；
C2.对灭点根据投影几何得到三维方向，通过三维空间聚类得到多个聚类中心；聚类中心所代表的方向是来自于地图的只有一个自由度的三维方向先验信息；三维方向先验信息对应于各自关键帧；
C3.将得到的三维方向先验信息输入到对应灭点所在图像的关键帧数据结构中，建立三维方向到关键帧上灭点的投影误差约束；
C4.联合2d-3d、3d-3d和方向约束，构建能量函数进行全局联合优化，求解出各关键帧位姿；
通过上述步骤，实现基于多源数据和方向先验联合优化的定位。
2.如权利要求1所述基于多源数据和方向先验联合优化的定位方法，其特征是，步骤1）中，基于隐式移动最小二乘IMLS方法，进行激光点云配准，得到每帧点云的初始位姿；具体包括：
A1.点云数据预处理，包括：使用3d点语义标签滤除掉被标记为运动物体类的点、体素网格下采样和邻域半径滤波；
A2.使用线性运动模型对点云进行校正；
A3.基于点云聚类去除点云中的运动物体；
A4.点云特征的计算：使用点云中每点的邻域计算得到点云特征向量；
A5.构建历史点云地图，基于隐式移动最小二乘求解点到面的迭代最近点ICP，得到当前帧点云的位姿。
3.如权利要求2所述基于多源数据和方向先验联合优化的定位方法，其特征是，A1中，具体是使用公开数据集SemanticKITTI中的3d点语义标签滤除掉被标记为运动物体类的点。
4.如权利要求2所述基于多源数据和方向先验联合优化的定位方法，其特征是，点云数据预处理具体是：首先基于SemanticKITTI中的到点云数据每点的语义类标，去掉非静态的物体；接着使用pcl点云处理库进行保留重心的体素网格下采样，精度为0.7m；进行邻域半径滤波，删掉半径为1.5m的邻域内点数少于3的数据。
5.如权利要求1所述基于多源数据和方向先验联合优化的定位方法，其特征是，步骤2）中，基于ORB-SLAM2来构建优化图结构；具体包括：
B1. 对双目RGB图像进行数据处理，包括转为双目灰度图像和提取ORB特征；
B2. 使用步骤1）中得到的每帧点云数据的位姿，在每帧双目灰度图像和稀疏视觉ORB特征点地图建立2d特征-3d地图点的对应关系，即得到图像帧之间ORB特征间的对应关系；
B3.把步骤A2处理后的含有特征的点云数据和同时刻的双目灰度图像封装为新的数据结构；其中，将激光雷达点云转为相机参考系下，并得到关键帧；关键帧中包含图像特征和点云特征；
B4.对ORB-SLAM2的局部建图线程不再进行捆集调整，只对稀疏视觉地图插入新关键帧、更新共视关系、三角化建立新的地图点；
B5.取出全部的关键帧，根据视觉地图中图像2d特征-3d地图点之间的对应关系，建立地图点到关键帧上特征点的视觉重投影误差；对于任一关键帧，取出和其有共视关系且时间戳在其之前的关键帧，两两之间建立3d-3d间的对应；该对应是基于隐式移动最小二乘的匹配。
6.如权利要求1所述基于多源数据和方向先验联合优化的定位方法，其特征是，步骤2）中，关键帧间3d-3d的约束的建立步骤如下：
从当前地图中得到关键帧集合：，每帧对应的点云/>，激光雷达点云地图中的三维点集合/>，关键帧/>的共视关键帧集合/>；
对于任一关键帧，得到时间戳在它之前的共视关键帧/>；在之间建立关键帧间3d-3d对应关系的约束。
7.如权利要求1所述基于多源数据和方向先验联合优化的定位方法，其特征是，步骤3）中，具体是通过三维空间的均值漂移mean-shift聚类方法得到若干个聚类中心。
CN202110959819.0A 2021-08-20 2021-08-20 基于多源数据和方向先验联合优化的定位方法 Active CN113850293B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110959819.0A CN113850293B (zh) 2021-08-20 2021-08-20 基于多源数据和方向先验联合优化的定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110959819.0A CN113850293B (zh) 2021-08-20 2021-08-20 基于多源数据和方向先验联合优化的定位方法

Publications (2)

Publication Number Publication Date
CN113850293A CN113850293A (zh) 2021-12-28
CN113850293B true CN113850293B (zh) 2024-06-18

Family

ID=78975728

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110959819.0A Active CN113850293B (zh) 2021-08-20 2021-08-20 基于多源数据和方向先验联合优化的定位方法

Country Status (1)

Country Link
CN (1) CN113850293B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116592897B (zh) * 2023-07-17 2023-09-22 河海大学 基于位姿不确定性的改进orb-slam2定位方法

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110533587A (zh) * 2019-07-03 2019-12-03 浙江工业大学 一种基于视觉先验信息和地图恢复的slam方法
CN110782494A (zh) * 2019-10-16 2020-02-11 北京工业大学 一种基于点线融合的视觉slam方法

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110349213B (zh) * 2019-06-28 2023-12-12 Oppo广东移动通信有限公司 基于深度信息的位姿确定方法、装置、介质与电子设备
CN112219087A (zh) * 2019-08-30 2021-01-12 深圳市大疆创新科技有限公司 位姿预测方法、地图构建方法、可移动平台及存储介质
CN112258618B (zh) * 2020-11-04 2021-05-14 中国科学院空天信息创新研究院 基于先验激光点云与深度图融合的语义建图与定位方法
CN112902953B (zh) * 2021-01-26 2022-10-04 中国科学院国家空间科学中心 一种基于slam技术的自主位姿测量方法
CN112985416B (zh) * 2021-04-19 2021-07-30 湖南大学 激光与视觉信息融合的鲁棒定位和建图方法及系统

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110533587A (zh) * 2019-07-03 2019-12-03 浙江工业大学 一种基于视觉先验信息和地图恢复的slam方法
CN110782494A (zh) * 2019-10-16 2020-02-11 北京工业大学 一种基于点线融合的视觉slam方法

Also Published As

Publication number Publication date
CN113850293A (zh) 2021-12-28

Similar Documents

Publication Publication Date Title
CN112435325B (zh) 基于vi-slam和深度估计网络的无人机场景稠密重建方法
CN111815757B (zh) 基于图像序列的大型构件三维重建方法
CN111968129B (zh) 具有语义感知的即时定位与地图构建系统及方法
WO2021233029A1 (en) Simultaneous localization and mapping method, device, system and storage medium
CN110223348B (zh) 基于rgb-d相机的机器人场景自适应位姿估计方法
CN107564061B (zh) 一种基于图像梯度联合优化的双目视觉里程计算方法
CN110853075B (zh) 一种基于稠密点云与合成视图的视觉跟踪定位方法
CN106595659A (zh) 城市复杂环境下多无人机视觉slam的地图融合方法
CN108520554A (zh) 一种基于orb-slam2的双目三维稠密建图方法
CN108776989B (zh) 基于稀疏slam框架的低纹理平面场景重建方法
CN111612728B (zh) 一种基于双目rgb图像的3d点云稠密化方法和装置
CN104539928A (zh) 一种光栅立体印刷图像合成方法
CN111340922A (zh) 定位与地图构建的方法和电子设备
Tao et al. Stereo priori RCNN based car detection on point level for autonomous driving
CN112419497A (zh) 基于单目视觉的特征法与直接法相融合的slam方法
CN115719407A (zh) 一种面向大规模航拍图像的分布式多视图立体重建方法
CN112150518B (zh) 一种基于注意力机制的图像立体匹配方法及双目设备
Alcantarilla et al. Large-scale dense 3D reconstruction from stereo imagery
Stucker et al. ResDepth: Learned residual stereo reconstruction
CN117197333A (zh) 基于多目视觉的空间目标重构与位姿估计方法及系统
CN113850293B (zh) 基于多源数据和方向先验联合优化的定位方法
CN112270701B (zh) 基于分组距离网络的视差预测方法、系统及存储介质
Rothermel et al. A median-based depthmap fusion strategy for the generation of oriented points
CN117726747A (zh) 补全弱纹理场景的三维重建方法、装置、存储介质和设备
Chen et al. Densefusion: Large-scale online dense pointcloud and dsm mapping for uavs

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