CN116309813A - 一种固态激光雷达-相机紧耦合位姿估计方法 - Google Patents

一种固态激光雷达-相机紧耦合位姿估计方法 Download PDF

Info

Publication number
CN116309813A
CN116309813A CN202211499969.9A CN202211499969A CN116309813A CN 116309813 A CN116309813 A CN 116309813A CN 202211499969 A CN202211499969 A CN 202211499969A CN 116309813 A CN116309813 A CN 116309813A
Authority
CN
China
Prior art keywords
pose
camera
point
laser radar
solid
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
CN202211499969.9A
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.)
Beihang University
Original Assignee
Beihang 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 Beihang University filed Critical Beihang University
Priority to CN202211499969.9A priority Critical patent/CN116309813A/zh
Publication of CN116309813A publication Critical patent/CN116309813A/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/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
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T3/00—Geometric image transformations in the plane of the image
    • G06T3/06—Topological mapping of higher dimensional structures onto lower dimensional surfaces
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T5/00—Image enhancement or restoration
    • G06T5/20—Image enhancement or restoration using local operators
    • G06T5/30—Erosion or dilatation, e.g. thinning
    • 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/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/60—Analysis of geometric attributes
    • G06T7/64—Analysis of geometric attributes of convexity or concavity
    • 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]
    • 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
    • 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/10032—Satellite or aerial image; Remote sensing
    • G06T2207/10044—Radar 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Multimedia (AREA)
  • Geometry (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

本发明提出一种固态激光雷达‑相机紧耦合位姿估计方法，步骤一：对相机相邻两帧图像进行尺度不变特征变换SIFT特征点检测和描述子提取；步骤二：将相邻两帧固态激光雷达的非重复扫描点云投影到对应图像帧上，得到深度图，进而变成三维匹配点；步骤三：对三维匹配点进行基于刚体变换的RANSAC算法，得到相邻相机位姿，并计算相邻固态激光雷达位姿；步骤四：对固态激光雷达点云计算曲率，并根据曲率划分平面点和边缘点分别构建点到面的迭代最近点算法ICP和点到线的ICP激光雷达因子；步骤五：对图像提取2D配准点构建基于对极几何的相机因子；步骤六：融合激光雷达优化因子和相机优化因子，并进行因子图优化，最终得到精细的固态激光雷达位姿估计。

Description

一种固态激光雷达-相机紧耦合位姿估计方法
技术领域
本发明涉及固态激光雷达位姿估计，属于机器人状态估计领域。在三维重建、VR看房、SLAM、自动驾驶、机器人等领域具有广阔的应用前景。
背景技术
激光雷达分两类，传统机械式激光雷达(代表公司：Velodyne)和新兴的固态激光雷达(代表公司：Livox)。机械式激光雷达价格昂贵(32线激光雷达需要几十万元)，需要机械旋转结构旋转激光发射器，扫描出的点云呈现环状。而固态激光雷达相对便宜(LivoxHorizon仅需8000元)，通过内部光学棱镜旋转激光发射器获得非重复扫描的点云，在进行积分后可以获得覆盖视场的稠密点云。本发明聚焦固态激光雷达的位姿估计技术，它属于激光雷达位姿估计的一个子类。此外本发明为固态激光雷达-相机紧耦合位姿估计，即紧密地利用相机图像信息和激光雷达点云信息进行位姿估计，与分别估计相机位姿和激光雷达位姿再融合的松耦合形式有较大区别。
激光雷达位姿估计技术是一种对激光雷达的位置和姿态进行估计的技术，也可以理解成激光雷达点云配准技术或激光雷达建图技术，在各行各业有着许多重要的应用。比如自动驾驶车辆预先构建区域的高精地图(点云地图)，然后利用车载激光雷达的单帧点云数据与高精地图进行匹配，进而得到车辆的位姿(参见文献：Egger P,Borges P V K,CattG,et al.PoseMap:Lifelong,Multi-Environment 3D LiDAR Localization[C]//2018IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS).IEEE,2018.)。再比如对移动激光雷达进行时序点云数据帧间关联，从而对场景进行建图(参见文献：Zhang J,Singh S.LOAM:Lidar odometry and mapping in real-time[C]//Robotics:Science and Systems.2014,2(9):1-9.)，该技术在遥感测绘、三维重建、数字孪生等领域中均有重要作用。
激光雷达位姿估计有三种常用方法：1.基于ICP(Iterative Closest Point)的方法，ICP方法是一种交替进行求最近点对、剔除距离较大点对和根据匹配点对找最优刚体变换的算法，属于EM(Expectation Maximum)优化算法范畴。研究人员在经典的点到点的ICP(参见文献：Besl P J,McKay N D.Method for registration of 3-D shapes[C]//Sensorfusion IV:control paradigms and data structures.Spie,1992,1611:586-606.)算法基础上发展了点到面的ICP算法(参见文献：Low K L.Linear least-squaresoptimization for point-to-plane icp surface registration[J].Chapel Hill,University of North Carolina,2004,4(10):1-3.)，面到面的ICP(参见文献：Segal A,Haehnel D,Thrun S.Generalized-icp[C]//Robotics:science and systems.2009,2(4):435.)算法和基于体素的面到面ICP(参见文献：Koide K,Yokozuka M,Oishi S,etal.Voxelized gicp for fast and accurate3d point cloud registration[C]//2021IEEE International Conference on Robotics and Automation(ICRA).IEEE,2021:11054-11059.)算法。2.基于NDT(Normal Distributions Transform)的方法(参见文献：Biber P,Straβer W.The normal distributions transform:A new approach to laserscan matching[C]//Proceedings 2003IEEE/RSJ International Conference onIntelligent Robots and Systems(IROS 2003)(Cat.No.03CH37453).IEEE,2003,3:2743-2748.)，具体步骤为首先将参考点云划分为指定大小的体素，然后对每个体素内拟合一个高斯分布(计算高斯分布均值和协方差矩阵)。接着对目标点云位姿进行非线性最小二乘优化，使经过刚体变换后的目标点云落入参考点云分布的概率密度之和最大。3.基于LOAM(Lidar Odometry and Mapping)的方法，该方法是基于激光雷达的SLAM系统较常采用的方法，具体步骤是对扫描产生的点云线束计算拉普拉斯算子，通过计算结果大小来把点云分成平面点和边缘点两类。接着使用面到面的ICP和点到线的ICP进行位姿优化。后续LOAM系统进一步发展引入了回环检测，比如Lego-LOAM(参见文献：Shan T,Englot B.Lego-loam:Lightweight and ground-optimized lidar odometry and mapping on variableterrain[C]//2018IEEE/RSJ International Conference on Intelligent Robots andSystems(IROS).IEEE,2018:4758-4765.)；针对新型的固态激光雷达特点进行改进，比如Livox-LOAM(参见文献：Lin J,Zhang F.Loam livox:A fast,robust,high-precisionLiDAR odometry and mapping package for LiDARs of small FoV[C]//2020IEEEInternational Conference on Robotics and Automation(ICRA).IEEE,2020:3126-3131.)。
三种方法的详细对比如下表所示，其中ICP、NDT算法对点云的来源没有要求，而基于LOAM的算法要求点云必须是由激光雷达扫描得到的点云。在速度方面，由于ICP需要频繁的计算最近邻，因此速度慢；NDT方法则事先将点云分成若干体素并用高斯分布拟合，因此不需要计算最近邻，速度快；LOAM由于对点云进行了分类和降采样处理，可以大大降低计算量，速度较快。在内存占用方面，基于ICP和NDT的算法都需要较高的内存占用，而基于LOAM的算法由于对点云进行了预处理，因此优化所需内存较小。此外，在初值依赖性上，ICP算法对初值依赖性较高，不好的初值容易让ICP算法陷入局部极小值，算法鲁棒性较低；而NDT算法则可以在较差的初值下进行优化，具有较好的算法鲁棒性；基于LOAM的方法由于ICP的算法，位于ICP与NDT二者之间。
Figure BDA0003966995970000021
Figure BDA0003966995970000031
表1常用激光雷达位姿估计算法比较。
以上方法是只使用激光雷达点云模态的位姿估计方法，近年来有许多把激光雷达-相机融合进行位姿估计的方法，比如R2-Live将激光雷达、相机、惯性测量单元结合起来进行序列位姿估计(参见文献:Lin J,Zheng C,Xu W,et al.R2-LIVE:A Robust,Real-Time,LiDAR-Inertial-Visual Tightly-Coupled State Estimator and Mapping[J].IEEE Robotics and Automation Letters,2021,6(4):7469-7476.)。类似的工作还有LVI-SAM(参见文献：Shan T,Englot B,Ratti C,et al.Lvi-sam:Tightly-coupled lidar-visual-inertial odometry via smoothing and mapping[C]//2021IEEE internationalconference on robotics and automation(ICRA).IEEE,2021:5692-5698.)。
然而以上方法均基于非线性最小二乘优化算法，而优化目标函数是非凸的，在初始位姿不够好的时候容易收敛到局部极小值，因此以上方法均难以解决激光雷达位姿差距较大的情况；以上方法常用于解决序列数据(SLAM任务)，位姿差距较小的位姿估计问题。
发明内容
1、目的：针对上述问题，本发明目的在于提供一种可以同时解决大位姿和小位姿下的固态激光雷达位姿估计问题的方法。
2、技术方案：为实现这个目的，本发明的技术方案需要借助图像的信息，即传感器需要有相机和固态激光雷达两种模态。然后采用由粗到精的位姿估计策略，整体步骤分为粗略估计和精细估计两步。在粗略估计步骤，通过图像特征的匹配以及将点云投影获得深度图等步骤，得到三维匹配点，进而通过刚体变换随机一致性采样(Random SampleConsensus,RANSAC)得到粗糙的激光雷达位姿；然后在精细估计步骤结合激光雷达因子和相机因子进行因子图优化，进而获得一个更加精确的固态激光雷达位姿。本发明的技术思路主要体现在以下三个方面：
1)设计由粗到精的配准流程。对较大位姿的估计问题，可以先粗略估计一个位姿，再在它的基础上进行精细优化。
2)在粗略估计阶段结合图像信息。利用图像描述子得到二维匹配点，把固态激光雷达非重复扫描得到的稠密点云投影到图像上得到深度图，然后通过深度图映射成三维匹配点，最后通过刚体变换RANSAC得到粗略位姿。
3)在精细估计阶段结合图像信息。同时构造激光雷达因子和基于对极约束的相机因子，并将其融合进行激光雷达-相机紧耦合的因子图优化。
本发明涉及一种固态激光雷达-相机紧耦合位姿估计方法，该方法具体步骤如下：
步骤一：对相机相邻两帧图像进行尺度不变特征变换(Scale-invariant featuretransform)SIFT特征点检测和描述子提取，然后进行特征点匹配，获得图像匹配二维特征点。
步骤二：将相邻两帧固态激光雷达的非重复扫描点云投影到对应图像帧上，得到深度图。再通过深度图补全算法得到稠密深度图，进而图像的二维匹配点通过深度图可以变成三维匹配点。
步骤三：对三维匹配点进行基于刚体变换的随机抽样一致(Random SampleConsensus)RANSAC算法，得到相邻相机位姿。通过相机和固态激光雷达构成的位姿约束计算相邻固态激光雷达位姿，即粗略估计位姿。
步骤四：对固态激光雷达点云计算曲率，并根据曲率划分平面点和边缘点分别构建点到面的迭代最近点算法(Iterative Closest Point)ICP和点到线的ICP激光雷达因子。
步骤五：对图像提取2D配准点构建基于对极几何的相机因子。
步骤六：融合激光雷达优化因子和相机优化因子，并进行因子图优化，最终得到精细的固态激光雷达位姿估计。
输出：配准好的固态激光雷达点云，固态激光雷达位姿。
其中，所述步骤一具体如下：
1.1：对相邻图像进行SIFT特征点检测并提取描述子。
1.2：并使用蛮力匹配器进行粗匹配，并用基于单应性矩阵的模型剔除粗匹配中的离群点，进而得到最终图像相邻帧二维配准点。
其中，所述步骤二具体如下：
2.1：将点云投影至可见光图像上，将点云沿着相机z轴方向的深度信息赋值给最近邻像素，即可获得深度图像，此时深度图像还存在许多未定义点。
2.2：采用基于形态学运算的深度图补全(depth image completion)算法，对深度图进行补全，具体做法是交替的使用形态学闭操作、膨胀操作、中值滤波与高斯滤波操作。在深度图补全之后，整张深度图均有深度定义，进而可以通过反向投影把所有二维匹配点均可变成三维匹配点。具体步骤如下，假设图像匹配像素的齐次坐标为x＝[u，v，1],则它对应的在相机坐标系下的三维坐标求解方式见式(1)。
Figure BDA0003966995970000051
其中Krgb，Drgb分别为可见光相机内参和畸变系数，已经提前标定好，是已知值。
其中
Figure BDA0003966995970000052
代表在相机归一化平面上根据畸变系数Drgb对像素p进行去畸变操作，z是可见光图像像素[u，v]处的深度。由于之前已经获得了补全深度图Idepth，各个像素的深度在其上均有定义z＝Idepth(u，v)，因此根据式(1)所有二维匹配点均可变成三维匹配点。
其中，所述步骤三具体如下：
3.1：使用基于刚体变换的随机一致性采样算法(RANSAC)剔除3D匹配点对中的误匹配点，得到相邻相机的粗略估计位姿。步骤二输出N对三维匹配点集合SM＝{(sk，dk)}k＝1，2，...，N，其中(sk，dk)代表上一帧图像第k个对应点sk与当前帧图像第k个对应点dk构成的点对。从SM中随机从中采样T轮，每一轮使用均匀分布随机采样m个点对。然后用m个点对计算刚体变换[Ri|ti],其中Ri代表第i轮采样计算的旋转矩阵，ti代表第i轮采样计算的平移向量。利用该刚体变换对所有sk做变换，得到变换后的点s′k＝Ri×sk+ti，若s′k与dk的距离小于距离阈值∈d，则被判定为内点。统计每一轮采样得到刚体变换的内点数目，进而输出内点数目最多的刚体变换作为最优刚体变换，即相机帧间相对位姿粗略估计
Figure BDA0003966995970000053
3.2：基于刚体变换的RANSAC算法将输出相机帧间相对位姿粗略估计
Figure BDA0003966995970000054
根据图2激光雷达与相机位姿约束关系，激光雷达帧间相对位姿粗略估计
Figure BDA0003966995970000055
计算方法见式(2)。其中Tcam→Lidar指激光雷达相对相机的位姿，且提前进行标定，是已知值；/>
Figure BDA0003966995970000056
指对Tcam→Lidar取逆，代表相机相对激光雷达的位姿。
Figure BDA0003966995970000057
即该步利用固态激光雷达与相机构成的位姿约束把相机的粗略估计位姿变成激光雷达的粗略估计位姿。
其中，所述步骤四具体如下：
4.1：首先通过式(3)计算任意点Pi处的空间曲率Curvature(Pi)。其中Snn代表以激光雷达点Pi为中心前后3个点的指标集合，|Snn|代表集合Snn中元素的个数，||Pi||2代表点Pi的二范数，j取遍Snn的所有元素，Sj为对应的邻域点。
Figure BDA0003966995970000061
在通过式(3)计算每一点曲率后，通过设置阈值∈c把点云分为边缘点集合Ek与平面点集合Hk。
4.2：在提取激光雷达相邻帧边缘点集合Ek,Ek+1和平面点集合Hk，Hk+1后，可以构建如下激光雷达因子
Figure BDA0003966995970000062
记/>
Figure BDA0003966995970000063
优化目标函数如式(4)所示。
Figure BDA0003966995970000064
其中
Figure BDA0003966995970000065
Figure BDA0003966995970000066
代表第k帧激光雷达位姿，/>
Figure BDA0003966995970000067
代表第k+1帧激光雷达位姿，/>
Figure BDA0003966995970000068
代表激光雷达帧间相对位姿。de(e，Ek)代表边缘点e离Ek中最近边缘的距离，dh(h，Hk)代表平面点h距离Hk中最近平面的距离。该优化目标函数等价于点到线的ICP算法和点到面的ICP算法的结合。
其中，所述步骤五具体如下：
5.1：首先用SIFT特征点进行图像匹配，并用RANSAC算法消除错误匹配，得到满足对极几何约束的归一化平面对应点集合
Figure BDA0003966995970000069
其中/>
Figure BDA00039669959700000610
分别代表第k帧与第k+1帧的第m个对应点。
5.2：根据图2所示的固态激光雷达位姿约束关系，相机帧间位姿
Figure BDA00039669959700000611
可以由式(5)算出。
Figure BDA00039669959700000612
记Tc→l：＝TCam→Lidar,代表激光雷达相对相机外参，已提前标定好，利用以上外参关系构造如下相机因子
Figure BDA00039669959700000613
优化目标函数见式(6)。
Figure BDA00039669959700000614
其中trace代表矩阵的迹，essential(·)是将位姿
Figure BDA00039669959700000615
映射到本质矩阵(Essential Matrix)E：＝t^R的函数，t^代表关于向量t的反对称矩阵(skew matrix)。该损失函数的目的是约束第k帧与k+1图像的匹配点满足对极几何约束关系。如图3所示，Ok是第k帧图像Ik的相机光心，Ok+1是第k+1帧图像Ik+1的相机光心，Ok与Ok+1的连线与图像分别交于ek和ek+1。P是两帧相机拍到的一个共同的三维坐标点，其投影在Ik和Ik+1上的图像坐标分别为/>
Figure BDA00039669959700000616
和/>
Figure BDA00039669959700000617
lk为ek与/>
Figure BDA00039669959700000618
的交线，称为/>
Figure BDA00039669959700000619
对应的极线(epipolar line)，同理lk+1为/>
Figure BDA00039669959700000620
对应的极线。则公式(6)表达的意思是匹配点/>
Figure BDA00039669959700000621
必须在/>
Figure BDA00039669959700000622
对应的极线lk上。
其中，所述步骤六具体如下：
6.1：将激光雷达因子
Figure BDA00039669959700000623
与相机因子/>
Figure BDA00039669959700000624
融合得到的优化目标函数见式(7)。
Figure BDA00039669959700000625
可以同时优化多帧，也可只优化相邻两帧。此外该优化问题的初始位姿即步骤三输的激光雷达帧间相对位姿粗略估计
Figure BDA0003966995970000071
6.2：因子图如图4所示，使用LM(Levenberg-Marquardt)优化算法进行因子图优化，优化结果是最终激光雷达精细的帧间相对位姿估计
Figure BDA0003966995970000072
一种固态激光雷达-相机紧耦合位姿估计方法，该方法系统基本结构框架和工作流程如图1所示，其特征包括：
由粗到精的位姿估计策略。
粗略位姿估计步骤用于给予固态激光雷达位姿一个粗略的估计，作为精细估计步骤的初值。
精细位姿估计步骤用于对位姿进行精细优化，从而获得更好的位姿估计。
对于位姿差距较大的输入激光雷达点云和图像数据。在粗略位姿估计步骤，引入图像信息，进行图像特征点匹配。然后将点云投影至图像上获得深度图，并进行深度图补全。最后用刚体变换RANSAC估计相机位姿、激光雷达位姿。在精细位姿估计步骤，引入图像信息。构建了基于对极约束的相机因子，并于激光雷达因子融合一起紧耦合优化。优化初值即粗略位姿估计步骤输出的粗略位姿估计，最后获得精细的位姿估计和配准点云。
3、优点及功效：本发明提出的一种固态激光雷达-相机紧耦合位姿估计方法，算法由粗略位姿估计步骤与精细位姿估计步骤组成。其中，粗略位姿估计步骤引入图像信息和深度图信息可以获得一个粗略的位姿估计值。然后在精细位姿估计步骤进一步进行位姿优化获得更好的位姿估计。与其它位姿估计算法相比，该算法可以解决大视角、大位姿下的激光雷达位姿估计问题，在三维重建、VR看房、自动驾驶、机器人等领域有广泛的使用价值和应用前景。
附图说明
图1是本发明提出的一种固态激光雷达-相机紧耦合位姿估计方法的算法流程图。
图2是激光雷达-相机位姿变换模型及位姿约束示意图。
图3是图像对极几何约束示意图。
图4是激光雷达-相机紧耦合非线性优化因子图。
图5(a)是前一帧激光雷达点云。
图5(b)是当前帧激光雷达点云。
图5(c)是前一帧可见光图像。
图5(d)是当前帧可见光图像。
图6(a)是初始点云叠加结果的侧视图，即直接把两帧点云叠加效果。
图6(b)是初始点云叠加结果的俯视图，即直接把两帧点云叠加效果。
图7是基于SIFT的图像特征点匹配结果示意图。
图8(a)是前一帧原始深度图。
图8(b)是当前帧原始深度图。
图8(c)是前一帧深度图补全效果。
图8(d)是当前帧深度图补全效果。
图9(a)是固态激光雷达位姿粗略估计效果的主视图；图中圆圈代表匹配不好的地方示意。
图9(b)是固态激光雷达位姿粗略估计效果的俯视图；图中圆圈代表匹配不好的地方示意。
图10(a)是固态激光雷达位姿精细估计效果的主视图。
图10(b)是固态激光雷达位姿精细估计效果的俯视图。
图11(a)是对比方法迭代最近邻点(ICP)的运行效果主视图。
图11(b)是对比方法迭代最近邻点(ICP)的运行效果俯视图。
图11(c)是对比方法通用迭代最近邻点(GICP)的运行效果主视图。
图11(d)是对比方法通用迭代最近邻点(GICP)的运行效果俯视图。
图11(e)是对比方法正态分布变换(NDT)的运行效果主视图。
图11(f)是对比方法正态分布变换(NDT)的运行效果俯视图。
图11(g)是对比方法激光雷达里程计与建图(LOAM)的运行效果主视图。
图11(h)是对比方法激光雷达里程计与建图(LOAM)的运行效果俯视图。
图11(i)是本文提出的固态激光雷达-相机紧耦合位姿估计方法的运行效果主视图。
图11(j)是本文提出的固态激光雷达-相机紧耦合位姿估计方法的运行效果俯视图。
具体实施方式
为了更好地理解本发明的技术方案，以下结合附图对本发明的实施方式作进一步描述。
本发明是一种固态激光雷达-相机紧耦合位姿估计方法，其算法流程图如图1所示。算法的输入为不同帧对应的固态激光雷达点云和相机图像。输入示例如图5(a)-(d)所示，这里仅针对VR看房应用，仅以两帧室内数据举例，图5(a)、图5(b)是前一帧与当前帧的点云，为了显示方便，均把激光雷达点云投影到图像上获得着色点云；图5(c)、图5(d)是对应前一帧与当前帧的图像。两帧点云若直接叠加效果如图6(a)、图6(b)所示，可以看到此时两帧点云未配准，且位姿差距较大。首先提前标定好相机内参Krgb和相机畸变系数Drgb，在本例中
Figure BDA0003966995970000091
相机畸变系数Drgb＝[-0.227，0.244，4.52×10-5，8.65×10-4，0]。然后标定激光雷达相对相机位姿Tcam→lidar，在本例中/>
Figure BDA0003966995970000092
Figure BDA0003966995970000093
此时Krgb，Drgb，Tcam→lidar均作为已知量输入算法。
各部分具体实施步骤如下：
步骤一：对输入示例中前一帧与当前帧图像进行SIFT特征点检测和描述子提取，然后进行特征点匹配，并用基于单应性变换的RANSAC算法剔除离群点，获得图像匹配二维特征点，匹配二维特征点效果如图7所示。
步骤二：将相邻两帧固态激光雷达的非重复扫描点云投影到对应图像帧上，得到深度图。图8(a)是前一帧深度图效果，图8(b)是当前帧深度图效果。再通过深度图补全算法得到稠密深度图，前一帧补全效果与当前帧补全效果分别如图8(c)和图8(d)所示。进而图像的二维匹配点通过深度图可以变成三维匹配点，具体计算方法如公式1所示。
步骤三：对三维匹配点进行基于刚体变换的RANSAC算法，得到相邻相机位姿，其中刚体变换RANSAC算法采样轮数T＝30每一轮采样点对数M＝5，内点判定阈值∈d＝0.1m。通过相机和固态激光雷达构成的位姿约束计算相邻固态激光雷达位姿，即粗略估计位姿
Figure BDA0003966995970000094
粗略估计位姿的点云配准效果如图9(a)、图9(b)所示，可以看到与图6(a)、图6(b)的初始位姿相比，图9(a)、图9(b)获得了一个更好的点云配准效果。然而图9(a)、图9(b)仍不能实现完全匹配，图9中的圆圈代表匹配不好的区域示意。
步骤四：通过公式(3)对固态激光雷达点云计算曲率，设置曲率阈值∈c＝0.5，并根据曲率划分平面点和边缘点分别构建点到面的ICP和点到线的ICP激光雷达因子。
步骤五：对图像提取2D配准点，然后使用公式(5)所示的位姿约束构建基于对极几何的相机因子。
步骤六：融合激光雷达优化因子和相机优化因子，并进行因子图优化，优化初值为
Figure BDA0003966995970000095
最终得到精细的固态激光雷达位姿估计/>
Figure BDA0003966995970000096
精细位姿估计效果如图10(a)、图10(b)所示。
输出：配准好的固态激光雷达点云，固态激光雷达位姿。
与其他激光雷达位姿估计算法比较的结果如图11(a)-(j)所示。其中图11(a)、图11(b)分别是对比方法迭代最近邻点(ICP)的运行效果主视图、俯视图；图11(c)、图11(d)分别是对比方法通用迭代最近邻点(GICP)的运行效果主视图、俯视图；图11(e)、图11(f)分别是对比方法正交分布变换(NDT)的运行效果主视图、俯视图；图11(g)、图11(h)分别是对比方法激光雷达里程计与建图(LOAM)的运行效果主视图、俯视图；可以看到ICP、GICP、NDT、LOAM这类经典激光雷达位姿估计方法均无法解决大位姿下的激光雷达位姿估计问题，均收敛到了一个错位的误差，导致最终点云融合效果不理想。图11(i)、图11(j)分别是本发明一种固态激光雷达-相机紧耦合位姿估计方法的运行效果主视图、俯视图，可以看到，本发明却能很好的解决大位姿下的激光雷达位姿估计问题，从而证明了本发明效果上的优越性。
本发明提出的一种固态激光雷达-相机紧耦合位姿估计方法可以有效完成大位姿下的固态激光雷达帧间位姿估计问题，固态激光雷达位姿估计具有多方面的应用，在位姿估计准确后可以把帧间点云进行配准融合，进而应用到VR看房、三维重建、自动驾驶等应用。

Claims (7)

1.一种固态激光雷达-相机紧耦合位姿估计方法，其特征在于，具体步骤如下：
步骤一：对相机相邻两帧图像进行尺度不变特征变换SIFT特征点检测和描述子提取，然后进行特征点匹配，获得图像匹配二维特征点；
步骤二：将相邻两帧固态激光雷达的非重复扫描点云投影到对应图像帧上，得到深度图；再通过深度图补全算法得到稠密深度图，进而图像的二维匹配点通过深度图变成三维匹配点；
步骤三：对三维匹配点进行基于刚体变换的随机抽样一致RANSAC算法，得到相邻相机位姿；通过相机和固态激光雷达构成的位姿约束计算相邻固态激光雷达位姿，即粗略估计位姿；
步骤四：对固态激光雷达点云计算曲率，并根据曲率划分平面点和边缘点分别构建点到面的迭代最近点算法ICP和点到线的ICP激光雷达因子；
步骤五：对图像提取2D配准点构建基于对极几何的相机因子；
步骤六：融合激光雷达优化因子和相机优化因子，并进行因子图优化，最终得到精细的固态激光雷达位姿估计；
输出：配准好的固态激光雷达点云，固态激光雷达位姿。
2.根据权利要求1所述的一种固态激光雷达-相机紧耦合位姿估计方法，其特征在于：所述步骤一具体如下：
1.1：对相邻图像进行SIFT特征点检测并提取描述子；
1.2：使用蛮力匹配器进行粗匹配，用基于单应性矩阵的模型剔除粗匹配中的离群点，进而得到最终图像相邻帧二维配准点。
3.根据权利要求1所述的一种固态激光雷达-相机紧耦合位姿估计方法，其特征在于：所述步骤二具体如下：
2.1：将点云投影至可见光图像上，将点云沿着相机z轴方向的深度信息赋值给最近邻像素，即获得深度图像，此时深度图像还存在许多未定义点；
2.2：采用基于形态学运算的深度图补全算法，对深度图进行补全，交替使用形态学闭操作、膨胀操作、中值滤波与高斯滤波操作；在深度图补全之后，整张深度图均有深度定义，进而通过反向投影把所有二维匹配点均可变成三维匹配点；具体步骤为：设图像匹配像素的齐次坐标为x＝[u，v，1],则它对应的在相机坐标系下的三维坐标求解方式见式(1)；
Figure FDA0003966995960000011
其中Krgb，Drgb分别为可见光相机内参和畸变系数，已经提前标定好，是已知值；其中
Figure FDA0003966995960000021
代表在相机归一化平面上根据畸变系数Drgb对像素p进行去畸变操作，z是可见光图像像素[u，v]处的深度；由于之前已经获得了补全深度图Idepth，各个像素的深度在其上均有定义z＝Idepth(u，v)，因此根据式(1)所有二维匹配点均可变成三维匹配点。
4.根据权利要求1所述的一种固态激光雷达-相机紧耦合位姿估计方法，其特征在于：所述步骤三具体如下：
3.1：使用基于刚体变换的随机一致性采样算法RANSAC剔除3D匹配点对中的误匹配点，得到相邻相机的粗略估计位姿；步骤二输出N对三维匹配点集合SM＝{(sk，dk)}k＝1，2，..，N，其中(sk，dk)代表上一帧图像第k个对应点sk与当前帧图像第k个对应点dk构成的点对；从SM中随机从中采样T轮，每一轮使用均匀分布随机采样m个点对；然后用m个点对计算刚体变换[Ri|ti]；其中Ri代表第i轮采样计算的旋转矩阵，ti代表第i轮采样计算的平移向量；利用该刚体变换对所有sk做变换，得到变换后的点s′k＝Ri×sk+ti，若s′k与dk的距离小于距离阈值∈d，则被判定为内点；统计每一轮采样得到刚体变换的内点数目，进而输出内点数目最多的刚体变换作为最优刚体变换，即相机帧间相对位姿粗略估计
Figure FDA0003966995960000022
3.2：基于刚体变换的RANSAC算法将输出相机帧间相对位姿粗略估计
Figure FDA0003966995960000023
根据激光雷达与相机位姿约束关系，激光雷达帧间相对位姿粗略估计/>
Figure FDA0003966995960000024
计算方法见式(2)；其中Tcam→Lidar指激光雷达相对相机的位姿，且提前进行标定，是已知值；
Figure FDA0003966995960000025
指对Tcam→Lidar取逆，代表相机相对激光雷达的位姿；
Figure FDA0003966995960000026
步利用固态激光雷达与相机构成的位姿约束把相机的粗略估计位姿变成激光雷达的粗略估计位姿。
5.根据权利要求1所述的一种固态激光雷达-相机紧耦合位姿估计方法，其特征在于：所述步骤四具体如下：
4.1：首先通过式(3)计算任意点Pi处的空间曲率Curvature(Pi)；其中Snn代表以激光雷达点Pi为中心前后3个点的指标集合，|Snn|代表集合Snn中元素的个数，||Pi||2代表点Pi的二范数，j取遍Snn的所有元素，Sj为对应的邻域点；
Figure FDA0003966995960000027
在通过式(3)计算每一点曲率后，通过设置阈值∈c把点云分为边缘点集合Ek与平面点集合Hk；
4.2：在提取激光雷达相邻帧边缘点集合Ek，Ek+1和平面点集合Hk，Hk+1后，构建如下激光雷达因子
Figure FDA0003966995960000028
记/>
Figure FDA0003966995960000029
优化目标函数如式(4)所示；
Figure FDA0003966995960000031
其中
Figure FDA0003966995960000032
代表第k帧激光雷达位姿，/>
Figure FDA0003966995960000033
代表第k+1帧激光雷达位姿，/>
Figure FDA0003966995960000034
代表激光雷达帧间相对位姿；de(e，Ek)代表边缘点e离Ek中最近边缘的距离，dh(h，Hk)代表平面点h距离Hk中最近平面的距离；该优化目标函数等价于点到线的ICP算法和点到面的ICP算法的结合。
6.根据权利要求1所述的一种固态激光雷达-相机紧耦合位姿估计方法，其特征在于：所述步骤五具体如下：
5.1：首先用SIFT特征点进行图像匹配，并用RANSAC算法消除错误匹配，得到满足对极几何约束的归一化平面对应点集合
Figure FDA0003966995960000035
其中/>
Figure FDA0003966995960000036
分别代表第k帧与第k+1帧的第m个对应点；
5.2：根据固态激光雷达位姿约束关系，相机帧间位姿
Figure FDA0003966995960000037
由式(5)算出；
Figure FDA0003966995960000038
记Tc→l：＝TCam→Lidar，代表激光雷达相对相机外参，已提前标定好，外参关系构造如下相机因子
Figure FDA0003966995960000039
优化目标函数见式(6)；
Figure FDA00039669959600000310
其中trace代表矩阵的迹，essential(·)是将位姿
Figure FDA00039669959600000311
映射到本质矩阵E：＝t^R的函数，t^代表关于向量t的反对称矩阵；该损失函数是约束第k帧与k+1图像的匹配点满足对极几何约束关系；Ok是第k帧图像Ik的相机光心，Ok+1是第k+1帧图像Ik+1的相机光心，Ok与Ok+1的连线与图像分别交于ek和ek+1；P是两帧相机拍到的一个共同的三维坐标点，其投影在Ik和Ik+1上的图像坐标分别为/>
Figure FDA00039669959600000312
和/>
Figure FDA00039669959600000313
lk为ek与/>
Figure FDA00039669959600000324
的交线，称为/>
Figure FDA00039669959600000315
对应的极线，同理lk+1为/>
Figure FDA00039669959600000316
对应的极线；则公式(6)表达的意思是匹配点/>
Figure FDA00039669959600000317
必须在/>
Figure FDA00039669959600000318
对应的极线lk上。
7.根据权利要求1所述的一种固态激光雷达-相机紧耦合位姿估计方法，其特征在于：所述步骤六具体如下：
6.1：将激光雷达因子
Figure FDA00039669959600000319
与相机因子/>
Figure FDA00039669959600000320
融合得到的优化目标函数见式(7)；
Figure FDA00039669959600000321
同时优化多帧，或者只优化相邻两帧；此外该优化问题的初始位姿即步骤三输的激光雷达帧间相对位姿粗略估计
Figure FDA00039669959600000322
6.2：使用LM优化算法进行因子图优化，优化结果是最终激光雷达精细的帧间相对位姿估计
Figure FDA00039669959600000323
CN202211499969.9A 2022-11-28 2022-11-28 一种固态激光雷达-相机紧耦合位姿估计方法 Pending CN116309813A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211499969.9A CN116309813A (zh) 2022-11-28 2022-11-28 一种固态激光雷达-相机紧耦合位姿估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211499969.9A CN116309813A (zh) 2022-11-28 2022-11-28 一种固态激光雷达-相机紧耦合位姿估计方法

Publications (1)

Publication Number Publication Date
CN116309813A true CN116309813A (zh) 2023-06-23

Family

ID=86824542

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211499969.9A Pending CN116309813A (zh) 2022-11-28 2022-11-28 一种固态激光雷达-相机紧耦合位姿估计方法

Country Status (1)

Country Link
CN (1) CN116309813A (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116594080A (zh) * 2023-07-17 2023-08-15 中国海洋大学 一种水下目标探测系统及探测方法
CN117031443A (zh) * 2023-10-09 2023-11-10 天津云圣智能科技有限责任公司 点云数据构建方法、系统及电子设备
CN117351140A (zh) * 2023-09-15 2024-01-05 中国科学院自动化研究所 融合全景相机和激光雷达的三维重建方法、装置及设备
CN117710469A (zh) * 2024-02-06 2024-03-15 四川大学 一种基于rgb-d传感器的在线稠密重建方法及系统

Cited By (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116594080A (zh) * 2023-07-17 2023-08-15 中国海洋大学 一种水下目标探测系统及探测方法
CN116594080B (zh) * 2023-07-17 2023-12-01 中国海洋大学 一种水下目标探测系统及探测方法
CN117351140A (zh) * 2023-09-15 2024-01-05 中国科学院自动化研究所 融合全景相机和激光雷达的三维重建方法、装置及设备
CN117351140B (zh) * 2023-09-15 2024-04-05 中国科学院自动化研究所 融合全景相机和激光雷达的三维重建方法、装置及设备
CN117031443A (zh) * 2023-10-09 2023-11-10 天津云圣智能科技有限责任公司 点云数据构建方法、系统及电子设备
CN117031443B (zh) * 2023-10-09 2024-01-19 天津云圣智能科技有限责任公司 点云数据构建方法、系统及电子设备
CN117710469A (zh) * 2024-02-06 2024-03-15 四川大学 一种基于rgb-d传感器的在线稠密重建方法及系统
CN117710469B (zh) * 2024-02-06 2024-04-12 四川大学 一种基于rgb-d传感器的在线稠密重建方法及系统

Similar Documents

Publication Publication Date Title
CN110569704B (zh) 一种基于立体视觉的多策略自适应车道线检测方法
CN111563415B (zh) 一种基于双目视觉的三维目标检测系统及方法
CN116309813A (zh) 一种固态激光雷达-相机紧耦合位姿估计方法
Choi et al. RGB-D edge detection and edge-based registration
Scharstein View synthesis using stereo vision
Koide et al. General, single-shot, target-less, and automatic lidar-camera extrinsic calibration toolbox
Muñoz-Bañón et al. Targetless camera-LiDAR calibration in unstructured environments
CN109470149B (zh) 一种管路位姿的测量方法及装置
CN110021039A (zh) 序列图像约束的多视角实物表面点云数据初始配准方法
CN109900274B (zh) 一种图像匹配方法及系统
Li et al. Dense surface reconstruction from monocular vision and LiDAR
Alizadeh Object distance measurement using a single camera for robotic applications
CN110349249B (zh) 基于rgb-d数据的实时稠密重建方法及系统
CN112017248B (zh) 一种基于点线特征的2d激光雷达相机多帧单步标定方法
CN112419497A (zh) 基于单目视觉的特征法与直接法相融合的slam方法
CN112630469B (zh) 一种基于结构光和多光场相机的三维检测方法
Bileschi Fully automatic calibration of lidar and video streams from a vehicle
Ann et al. Study on 3D scene reconstruction in robot navigation using stereo vision
CN114812558A (zh) 一种结合激光测距的单目视觉无人机自主定位方法
CN112017259B (zh) 一种基于深度相机与热像仪的室内定位与建图方法
Sen et al. SceneCalib: Automatic targetless calibration of cameras and LiDARs in autonomous driving
Cigla et al. Gaussian mixture models for temporal depth fusion
CN116402904A (zh) 一种基于激光雷达间和单目相机的联合标定方法
CN113850293B (zh) 基于多源数据和方向先验联合优化的定位方法
Delmas et al. Stereo camera visual odometry for moving urban environments

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