CN113436235A - 一种激光雷达与视觉点云的初始化自动配准方法 - Google Patents

一种激光雷达与视觉点云的初始化自动配准方法 Download PDF

Info

Publication number
CN113436235A
CN113436235A CN202110573218.6A CN202110573218A CN113436235A CN 113436235 A CN113436235 A CN 113436235A CN 202110573218 A CN202110573218 A CN 202110573218A CN 113436235 A CN113436235 A CN 113436235A
Authority
CN
China
Prior art keywords
point cloud
point
transformation matrix
clouds
matrix
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.)
Granted
Application number
CN202110573218.6A
Other languages
English (en)
Other versions
CN113436235B (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.)
Beijing Institute of Technology BIT
Original Assignee
Beijing Institute of Technology BIT
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 Beijing Institute of Technology BIT filed Critical Beijing Institute of Technology BIT
Priority to CN202110573218.6A priority Critical patent/CN113436235B/zh
Publication of CN113436235A publication Critical patent/CN113436235A/zh
Application granted granted Critical
Publication of CN113436235B publication Critical patent/CN113436235B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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
    • 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/66—Analysis of geometric attributes of image moments or centre of gravity
    • 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/20—Special algorithmic details
    • G06T2207/20076—Probabilistic image processing

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Geometry (AREA)
  • Optical Radar Systems And Details Thereof (AREA)

Abstract

本发明公开了一种激光雷达与视觉点云的初始化自动配准方法，在现有的JRMPC(多点集联合注册)算法的基础上，加入了尺度估计，采用带有尺度估计的JRMPC算法进行激光雷达点云和视觉点云的精匹配；该方法采用将体素降采样和ESF(形状函数集成)描述符相结合，进行激光雷达点云和视觉点云的粗匹配，解决了激光雷达点云和视觉点云的规模不同问题；采用欧式距离计算，可以降低计算量，缩短计算时间。

Description

一种激光雷达与视觉点云的初始化自动配准方法
技术领域
本发明属于信息技术领域，具体涉及一种激光雷达与视觉点云的初始化自动配准方法。
背景技术
三维点云的配准一直以来是一个重要的研究方向，在自动驾驶、机器人导航、增强现实等各领域均有运用。但是异源3D点云的配准却十分困难，这是因为异源点云之间的尺度、密度、规模大小、噪声分布机制等各方面都有较大的差异性。目前3D点云的配准算法主要分为两类，一类是全局配准算法，另一类是非全局配准算法。全局配准算法是基于BnB(分支与定界)的方法，不断更新变换空间的几何边界，从而搜寻出最佳变换矩阵。这类算法的关键在于如何精准地数学建模出变换空间的几何边界，从而实现最佳变换矩阵的准确搜寻。该算法虽然配准精度较高，但是由于其需要进行大量的计算，比较耗时，不适合实际的运用。非全局配准方法主要分为基于点云特征的配准方法和基于混合高斯模型的配准方法。基于点云特征的配准算法，其算法的精确程度上取决于点云特征的描述是否准确。但是每一种描述子都有局限性，其鲁棒性在异源点云的描述中较低，这将导致无法实现点云对之间准确匹配，从而不能实现准确的注册。基于混合高斯模型的配准算法，是一种基于概率模型的配准算法，采用混合高斯模型模型来表示点云的分布，通过最小化相应混合高斯模型的统计差异，从而求取最佳的变换矩阵。该类算法在参数求解的时候，通常采用EM(期望最大化)或ECM(期望条件最大化)方法求解。由于该类算法鲁棒性高，对噪声、密度、视角等因素不敏感，在异源点云的配准中比较适用。
在复杂的跟踪定位系统中，通常会采集激光雷达构建离线地图，该地图规模较大，且是稠密点云，而在系统的在线阶段，采用视觉的方式进行在线的跟踪定位。为了实现二者之间的关联，需要将激光雷达点云和视觉重建的点云进行配准，以便实现精确的定位、导航等各种需求。由于二者构建的点云数据属于异源数据，在点云的尺度、密度、规模大小、噪声分布机制等各个方面存在较大差异性。因此，需要着重解决激光雷达与视觉点云的初始化配准。
发明内容
有鉴于此，本发明的目的是提供一种激光雷达与视觉点云的初始化自动配准方法，能够解决激光雷达和视觉点云异源数据的关联问题，具有快速实现异源点云初始化配准的效果。
一种激光雷达与视觉点云的初始化自动配准方法，其特征在于，包括如下步骤：
步骤一：获得整个真实场景的激光雷达扫描点云Pl；
步骤二：利用相机对真实场景的某一区域进行拍照，采用视觉重建的方式获得该区域的视觉点云Pv；
步骤三：对点云Pl和点云Pv进行预处理；
步骤四：从点云Pl中随机选取M个点作为球心，以每一个球心构建N个半径大小不同的球体，把球体内部的点作为构建候选区域点云的点，从而获得规模大小不同的候选区域点云
Figure BDA0003083337140000021
a＝1，...，N；b＝1，...，M；M＞1；N＞1；
步骤五：尺度统一化，以点云
Figure BDA0003083337140000022
的规模大小作为尺度基准，对点云Pv的尺度调整，得到点云
Figure BDA0003083337140000023
步骤六：分别计算点云
Figure BDA0003083337140000024
和点云
Figure BDA0003083337140000025
的ESF描述子；
步骤七：对点云
Figure BDA0003083337140000026
的ESF描述子和点云
Figure BDA0003083337140000027
的ESF描述子进行粗匹配；
步骤八：选出匹配程度最好的前NN个候选区域点云中的点云
Figure BDA0003083337140000028
n＝1，...，NN，和对应点云
Figure BDA0003083337140000029
NN取大于2整数；
步骤九：采用具有尺度估计的JRMPC算法计算变换矩阵，得到点云
Figure BDA00030833371400000210
与点云
Figure BDA00030833371400000211
间的变换矩阵Tn；
步骤十：计算点云
Figure BDA00030833371400000212
与点云
Figure BDA00030833371400000213
间的配准误差；然后返回步骤九，直到达到设定的执行次数，执行步骤十一；
步骤十一：选出配准误差最小的变换矩阵Tbest和其对应的点云Yv-best，完成点云Pl和Pv的配准。
较佳的，所述步骤九具体计算过程如下：
S90、将Xl n表示为X1，Yv n表示为X2；
S91、假设点云X1和X2之间存在一个虚拟场景中心点云W，用K个混合高斯模型表示该点云；其中μk，σk，k＝1，...，K是混合高斯模型的均值和方差，且W＝[μk]，W为3*K的矩阵，K为虚拟场景中心点云W的点云数量；
S92、基于此混合高斯模型的参数，构建点云X1和X2分别到虚拟场景中心点云W变换后点云T1(X1)、T2(X2)，点云T1(X1)、T2(X2)中各点属于混合高斯模型的概率如公式(2)所示：
Figure BDA0003083337140000031
其中p()表示一个混合高斯模型的概率；xji表示第j个点云中的第i个点，且j＝1，2，i＝1，...，Nj，Nj为第j个点云中点的数量；ω是内外点比率，h是点云Xj球体的体积；变换后点云中各点Tj(xji)＝sjRjxji+tj，sj、Rj和tj分别是第j个点云到虚拟场景中心点云W变换矩阵的尺度因子、变换矩阵和平移矢量；
S93、采用EM算法计算混合模型参数和变换矩阵参数，具体为：
S931、初始化混合高斯模型参数和变换矩阵参数；
S932、E步计算点云Xj的后验分布概率，其后验分布概率αjik表示第j个点云的第i个点属于第k个高斯模型的概率：
Figure BDA0003083337140000032
其中
Figure BDA0003083337140000033
S933、M步计算混合高斯模型参数和变换矩阵参数：先利用步骤a计算变换矩阵参数Tj，j＝1，2；然后利用步骤b计算混合高斯模型参数μk，σk：
步骤a：计算变换矩阵Tj的Rj、tj、sj参数：
Rj＝Uj*diag([11det(UjVj T)])*Vj T (4)
Figure BDA0003083337140000034
Figure BDA0003083337140000035
其中Aj＝[Ajk]，为3*K的矩阵，
Figure BDA0003083337140000041
Λj是K*K的对角矩阵，λjk是Λj的第k个对角元素的值，且
Figure BDA0003083337140000042
取
Figure BDA0003083337140000043
e为1*k的单位矢量；其中
Figure BDA0003083337140000044
将矩阵DjΛj 2Cj T进行SVD分解，得到矩阵Uj、Mj、Vj T。
步骤b：计算混合高斯模型参数μk，σk：
Figure BDA0003083337140000045
Figure BDA0003083337140000046
其中公式(8)中的ε2是调整因子；
S934、重复S932和S933直至迭代达到设定次数；
S935、基于上述步骤得到点云
Figure BDA0003083337140000047
和
Figure BDA0003083337140000048
分别到虚拟场景中心点云W的变换矩阵Tl、Tv；
S94、计算点云Xl n到Yv n的变换矩阵Tn；
较佳的，所述预处理包括降采样和滤波去噪。
较佳的，所述步骤六中，采用体素降采样和ESF描述符结合的方法，分别计算点云
Figure BDA0003083337140000049
和点云
Figure BDA00030833371400000410
的ESF描述子。
较佳的，所述步骤七中，计算计算点云
Figure BDA00030833371400000411
的ESF描述子和点云
Figure BDA00030833371400000412
的ESF描述子二者之间的欧式距离进行粗匹配。
较佳的，所述步骤八中，取欧式距离最小的前NN个候选区域点云中的点云
Figure BDA00030833371400000413
较佳的，所述步骤S94中，计算点云Xl n到Yv n的变换矩阵Tn为
Figure BDA00030833371400000414
其中：
Figure BDA00030833371400000415
s，R，t分别是点云
Figure BDA0003083337140000051
到
Figure BDA0003083337140000052
变换的尺度因子、旋转矩阵、平移矢量。具体计算公式如公式(9)所示。其中，sl、sv分别是
Figure BDA00030833371400000510
和
Figure BDA00030833371400000511
的尺度因子，Rl、Rv分别是
Figure BDA0003083337140000053
和
Figure BDA0003083337140000054
的旋转矩阵，tl和tv分别是
Figure BDA0003083337140000055
和
Figure BDA0003083337140000056
的平移矢量。
较佳的，所述步骤十中，点云
Figure BDA0003083337140000057
与点云
Figure BDA0003083337140000058
间的配准误差的具体计算如公式(10)所示：
Figure BDA0003083337140000059
其中F表示F范数。
本发明具有如下有益效果：
本发明针对激光雷达与视觉点云的初始化自动配准问题，提出了一种从粗到细的配准方法：
在现有的JRMPC(多点集联合注册)算法的基础上，加入了尺度估计，采用带有尺度估计的JRMPC算法进行激光雷达点云和视觉点云的精匹配；
该方法采用将体素降采样和ESF(形状函数集成)描述符相结合，进行激光雷达点云和视觉点云的粗匹配，解决了激光雷达点云和视觉点云的规模不同问题；
采用欧式距离计算，可以降低计算量，缩短计算时间。
附图说明
图1为本发明实施方式的流程图；
图2为Lidar(激光雷达)扫描构建的点云；
图3为视觉重建的点云(SFM(由运动产生的结构)重建的点云)；
图4为粗匹配选出第一个Lidar候选区域；
图5为精匹配第一个的注册结果；
图6为Lidar点云和SFM点云最终的注册结果。
具体实施方式
下面结合附图并举实施例，对本发明进行详细描述。
本发明的一种激光雷达与视觉点云的初始化自动配准方法，其基本实施过程如下：
步骤一：利用三维激光扫描仪在固定位置对整个真实场景扫描建模，获得整个真实场景的激光雷达扫描点云Pl。
步骤二：利用相机对真实场景的某一区域进行拍照，采用视觉重建的方式获得该区域的视觉点云Pv。
步骤三：对点云Pl和点云Pv进行降采样、滤波去噪等预处理。
步骤四：从点云Pl中随机选取M个点作为球心，以每一个球心构建N个半径大小不同的球体，把球体内部的点作为构建候选区域点云的点，从而获得规模大小不同的候选区域点云
Figure BDA0003083337140000061
步骤五：尺度统一化，以点云
Figure BDA0003083337140000062
的规模大小作为尺度基准，对点云Pv的尺度调整，得到点云
Figure BDA0003083337140000063
步骤六：采用体素降采样和ESF描述符结合的方法，分别计算点云
Figure BDA0003083337140000064
和点云
Figure BDA0003083337140000065
的ESF描述符。
步骤七：计算点云
Figure BDA0003083337140000066
的ESF描述子和点云
Figure BDA0003083337140000067
的ESF描述子二者之间的欧式距离，如公式(1)所示。
Figure BDA0003083337140000068
步骤八：根据欧式距离的大小，选出前NN个欧式距离最小的候选区域点云中的点云
Figure BDA0003083337140000069
和对应点云
Figure BDA00030833371400000610
此时的点云
Figure BDA00030833371400000611
可能来自点云
Figure BDA00030833371400000612
的同一个点云，也可能是不同的点云。其中，NN根据实际情况取，一般取2-5即可。
步骤九：采用具有尺度估计的JRMPC算法计算变换矩阵，重复执行该算法NN次，每执行一次迭代MM次，得到点云
Figure BDA00030833371400000613
与点云
Figure BDA00030833371400000614
间的变换矩阵Tn(n＝1，...，NN)。以点云
Figure BDA00030833371400000615
和
Figure BDA00030833371400000616
间的第n次变换矩阵Tn的计算为例，为了便于表示，设Xl n为X1、Yv n为X2，其具体计算过程如下：
S91、假设点云X1和X2之间存在一个虚拟场景中心点云W，用K个混合高斯模型表示该点云。其中μk，σk(k＝1，...，K)是混合高斯模型的均值和方差，且W＝[μk](k＝1，...，K)，W为3*K的矩阵，其中3为虚拟场景中心点云构建点云W的维度，K为其点云的数量。
S92、基于此混合高斯模型的参数，构建点云X1和X2分别到虚拟场景中心点云W变换后点云T1(X1)、T2(X2)，两者对应的变换矩阵分别设为T1和T2；点云T1(X1)、T2(X2)中各点属于混合高斯模型的概率如公式(2)所示，其中xji表示第j个点云的第i个点，且j＝(1，2)，i＝(1，...，Nj)，Nj为第j个点云的数量。ω是内外点比率，h是Xj(j＝1，2)是包围球体的体积。变换后点云中各点Tj(xji)＝sjRjxji+tj，sj、Rj、tj分别是第j个点云到虚拟场景中心点云W变换矩阵的尺度因子、变换矩阵、平移矢量。
点云中各点属于混合高斯模型的概率为：
Figure BDA0003083337140000071
S93、采用EM算法计算混合模型参数和变换矩阵参数，具体为：
S931、初始化混合高斯模型参数和变换矩阵参数。
S932、E步计算点云Xj(j＝1，2)的后验分布概率。其后验分布概率αjik表示第j个点云的第i个点属于第k个高斯模型的概率，其具体计算公式如公式(3)所示。其中
Figure BDA0003083337140000072
Figure BDA0003083337140000073
Figure BDA0003083337140000074
S933、M步计算混合高斯模型参数和变换矩阵参数。先计算变换矩阵参数Tj(j＝1，2)，其具体计算过程步骤a。然后计算混合高斯模型参数μk，σk(k＝1，...，K)，其具体计算过程见步骤b。
步骤a：计算变换矩阵Tj(j＝1，2)的Rj、tj、sj参数，其计算顺序是先计算Rj，然后计算sj，最后计算tj。Rj、tj、sj的具体计算公式分别见公式(4)、(6)、(5)。
Rj＝Uj*diag([11det(UjVj T)])*Vj T (4)
Figure BDA0003083337140000075
Figure BDA0003083337140000076
其中Aj＝[Ajk](k＝1，...，K)，为3*K的矩阵，
Figure BDA0003083337140000081
W＝[μk](1，...，K)，为3*K的矩阵，Λj是K*K的对角矩阵，λjk是Λj的第k个对角元素的值，且
Figure BDA0003083337140000082
取
Figure BDA0003083337140000083
e为1*k的单位矢量。其中
Figure BDA0003083337140000084
将矩阵DjΛj 2Cj T进行SVD分解，得到矩阵Uj、Mj、Vj T。
步骤b：计算混合高斯模型参数μk，σk(k＝1，...，K)。其μk，σk的具体计算公式如公式(7)、(8)所示，其中公式(8)的ε2是调整因子。
Figure BDA0003083337140000085
Figure BDA0003083337140000086
S934、重复S932和S933，直至迭代MM次，完成。
S935、基于上述步骤便得到X1、X2的变换矩阵T1、T2。即得到点云
Figure BDA0003083337140000087
和
Figure BDA0003083337140000088
分别到虚拟场景中心点云W的变换矩阵Tl、Tv。
S94、计算点云Xl n和Yv n的变换矩阵Tn，
Figure BDA0003083337140000089
其中s，R，t分别是点云
Figure BDA00030833371400000810
到
Figure BDA00030833371400000811
变换的尺度因子、旋转矩阵、平移矢量。具体计算公式如公式(9)所示。其中，sl、sv分别是
Figure BDA00030833371400000812
和
Figure BDA00030833371400000813
的尺度因子，Rl、Rv分别是
Figure BDA00030833371400000814
和
Figure BDA00030833371400000815
的旋转矩阵，tl和tv分别是
Figure BDA00030833371400000816
和
Figure BDA00030833371400000817
的平移矢量。
Figure BDA00030833371400000818
步骤十：计算点云
Figure BDA00030833371400000819
与点云
Figure BDA00030833371400000820
间的配准误差，具体计算如公式(10)所示，其中F表示F范数。
Figure BDA0003083337140000091
返回步骤九，直到达到设定的执行次数。
步骤十一：选出配准误差最小的变换矩阵Tbest和其对应的点云Yv-best，完成点云Pl和Pv的配准。由于Yv-best是Pv尺度统一化后点云，只需完成点云Pl和Yv-best的配准，即完成点云Pl和Pv的配准。至此，激光雷达与视觉点云的初始化自动配准便可实现。
以下给出带有尺度估计的JRMPC算法的推导过程：
给定点云Xj(j＝1，2)，其为3*Nj的矩阵，其中3为点云Xj的维度，Nj为第j个点云的数量。并假设Xj之间存在一个虚拟场景中心点云W，采用混合高斯模型μk，σk(k＝1，...，K)来表示虚拟场景中心点云W＝[μk](k＝1，...，K)，其为3*K的矩阵。基于此混合高斯模型，点云Xj在其变换矩阵Tj变换后的点云Tj(Xj)的概率表示如公式(1)所示，其中xji为第j(j＝(1，2))个点云的第i(i＝(1，...，Nj))个点，ω为内外点比率，h为点云Xj的包围球体的体积。Tj(xji)＝sjRjxji+tj，sj、Rj、tj分别是第j个点云到虚拟场景中心点云W变换矩阵的尺度因子、变换矩阵、平移矢量。
Figure BDA0003083337140000092
采用最大似然法对参数θ(μk，σk，(k＝1，...，K)；sj，Rj，tj，(j＝1，2))估计，由于无法采用求导的方法计算参数，这里是EM算法迭代的方式估计参数。通过推导，可得点云Xj其后验概率分布中，其中αjik表示第j个点云的第i个点属于第k个混合高斯模型的概率，具体如公式(2)所示。
Figure BDA0003083337140000093
其中
Figure BDA0003083337140000094
ω为内外点比率，根据实际情况设定。在这一步，完成了EM算法的E步的后验概率的估计。
在EM算法的M步需要完成对参数θ的估计，先对变换矩阵参数进行估计，后对混合高斯模型参数估计。在最大似然概率的约束下，推出尺度因子、旋转矩阵和平移矢量的最优估计约束条件如下公式(3)。令Aj＝[Ajk](k＝1，...，K)，其中
Figure BDA0003083337140000101
且Aj为3*K的矩阵，W为虚拟场景中心点云，也是3*K的矩阵。Λj是K*K的对角矩阵，
Figure BDA0003083337140000102
tj为Aj到W的变换矩阵的尺度因子、变换矩阵、平移矢量。e是1*K的单位矢量。
min|(sjRjAj+tje-W)Λj| (3)
通过公式(3)对tj求导，并导数等于0，得到tj的最优估计。
Figure BDA0003083337140000103
将公式(4)带入公式(3)中，取
Figure BDA0003083337140000104
重写公式(3)为公式(5)。
Figure BDA0003083337140000105
根据定理，可以得到Rj的SVD分解矩阵为DjΛj 2Cj T，其分解值为UjMjVj T。可得Rj的最优估计如公式(6)所示。
Rj＝Uj*diag([11det(UjVj T)])*VjT (6)
对于公式(5)关于sj求导，令其导数等于0，可得sj的最优估计，如公式(7)所示。
Figure BDA0003083337140000106
基于公式(5)、(6)、(7)，可以得出sj、Rj、tj的最优估计。计算流程先计算Rj，再计算sj，最后计算tj。
接下需要完成对对高斯模型参数μk，σk(k＝1，...，K)进行估计，通过对μk，σk求导并令其导数等于0，可得均值μk和方差σk的估计，具体估计见公式(8)、(9)。
Figure BDA0003083337140000111
Figure BDA0003083337140000112
其中，ε2是调整因子，有固定的取值。
整个带有尺度估计的JRMPC的算法流程如下，仅针对两个点云的注册。
输入：P1、P2点云。
输出：θ(si，Ri，ti，(j＝1，2)；μk，σk，(k＝1，...，K))
初始化参数θ。
(1)E步：计算后验概率αjik，其计算公式见公式(2)。
(2)M步：更新参数θ(sj，Rj，tj，μk，σk)
使用公式(7)、(6)、(4)计算sj，Rj，tj。
使用公式(8)、(9)更新μk，σk。
重复(1)、(2)直至迭代完成。
返回变换矩阵T，
Figure BDA0003083337140000113
实现P1到P2的注册P1＝sRP1+t
至此，已经完成带有尺度的JRMPC算法中的整个求解过程。
实施例：
一种激光雷达与视觉点云的初始化自动配准方法，其具体步骤包括：
步骤一、将Faro三维激光扫描仪固定室内某一位置，扫描整个真实场景并获得整个室内的真实场景点云PLidar。点云PLidar中每一个三维点包含位置信息(x，y，z)(空间位置坐标，单位为m)。
步骤二、以室内的某一区域为中心，采用相机从不同角度进行拍照。采用VisualSFM软件重建出该区域的点云PSFM，点云PSFM是室内的某一区域场景重建所得，而非整个室内场景。点云PSFM中每一个三维点包括位置信息(x，y，z)(空间位置坐标，单位未知)。
步骤三、基于PCL库完成对点云PLidar和点云PSFM的预处理。采用PCL库中体素降采样方法对点云PLidar和点云PSFM进行降采样，采用PCl库中直通滤波算法对点云PSFM进行降噪处理。
步骤四、基于PCL库完成点云PLidar的候选区域的构建，得到候选区域点云
Figure BDA0003083337140000121
从点云PLidar中随机选取1000个点作为球心，分别设置其半径为1.2、1.1、1.0、0.9、0.8。(单位为m)，将球体内部的点作为构建候选区域点云的点，总共构建5000个候选区域。
步骤五、以
Figure BDA0003083337140000122
的规模大小作为尺度为基准，其尺度取球体的半径
Figure BDA0003083337140000123
计算点云PSFM包围球体的半径rSFM，然后对其进行尺度统一化，具体如公式(11)所示，其中
Figure BDA0003083337140000124
是点云PSFM尺度统一化后的点云。
Figure BDA0003083337140000125
步骤六、基于PCL库将体素降采样与ESF描述符计算结合，先对候选区域点云
Figure BDA0003083337140000126
进行体素降采样，采样网格大小设置设为候选区域点云
Figure BDA0003083337140000127
最小包围盒的长、宽、高分别除以20。然后采用ESF算法计算候选区域点云
Figure BDA0003083337140000128
的ESF描述子。点云
Figure BDA0003083337140000129
也是先进行体素降采样，采样网格大小设置设为点云
Figure BDA00030833371400001210
最小包围盒的长、宽、高分别除以20，再采用ESF算法计算点云
Figure BDA00030833371400001211
的ESF描述子。
步骤七、计算候选区域点云
Figure BDA00030833371400001212
的ESF描述子与点云
Figure BDA00030833371400001213
的ESF描述子之间的欧式距离之差，计算由公式(12)可得。其中ESF描述符是640位的统计描述符，用F范数表示其欧式距离之差。
Figure BDA00030833371400001214
步骤八、根据描述子之间的欧式距离的大小，对候选区域点云
Figure BDA00030833371400001215
进行排序，挑选出欧式距离最小的前五个候选区域点云
Figure BDA00030833371400001216
和其对应
Figure BDA00030833371400001217
其中点云
Figure BDA00030833371400001218
也可能是来自点云
Figure BDA00030833371400001219
同一个点云，也可能是不同点云。
步骤九、在原有的JRMPC的基础上，加入了尺度估计。采用带有尺度估计的JRMPC算法计算变换矩阵，重复该算法5次，每次迭代100次，得到点云
Figure BDA0003083337140000131
和
Figure BDA0003083337140000132
的变换矩阵Tn(n＝1，...，5)。以点云
Figure BDA0003083337140000133
和
Figure BDA0003083337140000134
间的第n次的变换矩阵Tn计算为例，其具体计算过程如下：
①初始化混合高斯模型参数和变换矩阵参数、其他参数。混合高斯模型参数μk(k＝1，...，20(在单位球体内随机产生2000个三维离散点作为初始化值，并构建虚拟场景中心点云W＝[μk](k＝1，...，2000)，变换矩阵参数的尺度因子均设为1，旋转矩阵设为单位矩阵，平移矢量分别取各自点云的均值与虚拟场景中心点云W均值之差。混合高斯模型参数σk(k＝1，...，2000)取点云
Figure BDA0003083337140000135
和
Figure BDA0003083337140000136
在变换矩阵变换之后，分别与虚拟场景中心点云W作差后的最大值与最小值的欧式距离的倒数。设置ω＝0.01，
Figure BDA0003083337140000137
ε2＝1×10-6。
②EM算法E步计算点云
Figure BDA0003083337140000138
和
Figure BDA0003083337140000139
的后验分布概率，其具体计算公式见公式(3)
③EM算法M步更新混合高斯模型参数和变换矩阵参数。其中变换矩阵参数的更新见公式(4)、(5)、(6)；混和高斯模型参数的更新见公式(7)、(8)。
④重复②和③，迭代100次完成计算。
⑤基于上述步骤，得到点云
Figure BDA00030833371400001310
和
Figure BDA00030833371400001311
分别到虚拟场景中心点云W的变换矩阵。然后计算点云
Figure BDA00030833371400001312
到
Figure BDA00030833371400001313
的变换矩阵Tn，其计算过程如公式(9)所示。
步骤十、计算前5个候选区域点云
Figure BDA00030833371400001314
到其对应点云
Figure BDA00030833371400001315
的配准误差，具体计算公式见公式(10)。
步骤十一、根据配准误差排序，挑选出最佳变换矩阵Tbest和对应点云YSFM-best，其中完成YSFM-best是点云PSFM尺度统一化后点云，完成PLidar和YSFM-best的配准，即完成PLidar和PSFM的配准。其具体计算公式见公式(13)。
YSFM-best＝sbestRbestYSFM-best+tbest (13)
综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (8)

1.一种激光雷达与视觉点云的初始化自动配准方法，其特征在于，包括如下步骤：
步骤一：获得整个真实场景的激光雷达扫描点云Pl；
步骤二：利用相机对真实场景的某一区域进行拍照，采用视觉重建的方式获得该区域的视觉点云Pv；
步骤三：对点云Pl和点云Pv进行预处理；
步骤四：从点云Pl中随机选取M个点作为球心，以每一个球心构建N个半径大小不同的球体，把球体内部的点作为构建候选区域点云的点，从而获得规模大小不同的候选区域点云
Figure FDA0003083337130000011
步骤五：尺度统一化，以点云
Figure FDA0003083337130000012
的规模大小作为尺度基准，对点云Pv的尺度调整，得到点云
Figure FDA0003083337130000013
步骤六：分别计算点云
Figure FDA0003083337130000014
和点云
Figure FDA0003083337130000015
的ESF描述子；
步骤七：对点云
Figure FDA0003083337130000016
的ESF描述子和点云
Figure FDA0003083337130000017
的ESF描述子进行粗匹配；
步骤八：选出匹配程度最好的前NN个候选区域点云中的点云
Figure FDA0003083337130000018
和对应点云
Figure FDA0003083337130000019
NN取大于2整数；
步骤九：采用具有尺度估计的JRMPC算法计算变换矩阵，得到点云
Figure FDA00030833371300000110
与点云
Figure FDA00030833371300000111
间的变换矩阵Tn；
步骤十：计算点云
Figure FDA00030833371300000112
与点云
Figure FDA00030833371300000113
间的配准误差；然后返回步骤九，直到达到设定的执行次数，执行步骤十一；
步骤十一：选出配准误差最小的变换矩阵Tbest和其对应的点云Yv-best，完成点云Pl和Pv的配准。
2.如权利要求1所述的一种激光雷达与视觉点云的初始化自动配准方法，其特征在于，所述步骤九具体计算过程如下：
S90、将Xl n表示为X1，Yv n表示为X2；
S91、假设点云X1和X2之间存在一个虚拟场景中心点云W，用K个混合高斯模型表示该点云；其中μk,σk，k＝1,...,K是混合高斯模型的均值和方差，且W＝[μk]，W为3*K的矩阵，K为虚拟场景中心点云W的点云数量；
S92、基于此混合高斯模型的参数，构建点云X1和X2分别到虚拟场景中心点云W变换后点云T1(X1)、T2(X2)，点云T1(X1)、T2(X2)中各点属于混合高斯模型的概率如公式(2)所示:
Figure FDA0003083337130000021
其中p()表示一个混合高斯模型的概率；xji表示第j个点云中的第i个点，且j＝1,2,i＝1,...,Nj，Nj为第j个点云中点的数量；ω是内外点比率，h是点云Xj球体的体积；变换后点云中各点Tj(xji)＝sjRjxji+tj，sj、Rj和tj分别是第j个点云到虚拟场景中心点云W变换矩阵的尺度因子、变换矩阵和平移矢量；
S93、采用EM算法计算混合模型参数和变换矩阵参数，具体为：
S931、初始化混合高斯模型参数和变换矩阵参数；
S932、E步计算点云Xj的后验分布概率，其后验分布概率αjik表示第j个点云的第i个点属于第k个高斯模型的概率：
Figure FDA0003083337130000022
其中
Figure FDA0003083337130000023
S933、M步计算混合高斯模型参数和变换矩阵参数：先利用步骤a计算变换矩阵参数Tj，j＝1,2；然后利用步骤b计算混合高斯模型参数μk,σk：
步骤a:计算变换矩阵Tj的Rj、tj、sj参数：
Rj＝Uj*diag([11det(UjVj T)])*Vj T (4)
Figure FDA0003083337130000024
Figure FDA0003083337130000031
其中Aj＝[Ajk],为3*K的矩阵，
Figure FDA0003083337130000032
Λj是K*K的对角矩阵,λjk是Λj的第k个对角元素的值，且
Figure FDA0003083337130000033
取
Figure FDA0003083337130000034
e为1*k的单位矢量；其中
Figure FDA0003083337130000035
将矩阵DjΛj 2Cj T进行SVD分解，得到矩阵Uj、Mj、Vj T。
步骤b:计算混合高斯模型参数μk,σk：
Figure FDA0003083337130000036
Figure FDA0003083337130000037
其中公式(8)中的ε2是调整因子；
S934、重复S932和S933直至迭代达到设定次数；
S935、基于上述步骤得到点云
Figure FDA0003083337130000038
和
Figure FDA0003083337130000039
分别到虚拟场景中心点云W的变换矩阵Tl、Tv；
S94、计算点云Xl n到Yv n的变换矩阵Tn。
3.如权利要求1或2所述的一种激光雷达与视觉点云的初始化自动配准方法，其特征在于，所述预处理包括降采样和滤波去噪。
4.如权利要求1或2所述的一种激光雷达与视觉点云的初始化自动配准方法，其特征在于，所述步骤六中，采用体素降采样和ESF描述符结合的方法，分别计算点云
Figure FDA00030833371300000310
和点云
Figure FDA00030833371300000311
的ESF描述子。
5.如权利要求1或2所述的一种激光雷达与视觉点云的初始化自动配准方法，其特征在于，所述步骤七中，计算计算点云
Figure FDA00030833371300000312
的ESF描述子和点云
Figure FDA00030833371300000313
的ESF描述子二者之间的欧式距离进行粗匹配。
6.如权利要求4所述的一种激光雷达与视觉点云的初始化自动配准方法，其特征在于，所述步骤八中，取欧式距离最小的前NN个候选区域点云中的点云
Figure FDA0003083337130000041
7.如权利要求1或2所述的一种激光雷达与视觉点云的初始化自动配准方法，其特征在于，所述步骤S94中，计算点云Xl n到Yv n的变换矩阵Tn为
Figure FDA0003083337130000042
其中：
Figure FDA0003083337130000043
s,R,t分别是点云
Figure FDA0003083337130000044
到
Figure FDA0003083337130000045
变换的尺度因子、旋转矩阵、平移矢量。具体计算公式如公式(9)所示。其中，sl、sv分别是
Figure FDA0003083337130000046
和
Figure FDA0003083337130000047
的尺度因子，Rl、Rv分别是
Figure FDA0003083337130000048
和
Figure FDA0003083337130000049
的旋转矩阵，tl和tv分别是
Figure FDA00030833371300000410
和
Figure FDA00030833371300000411
的平移矢量。
8.如权利要求1或2所述的一种激光雷达与视觉点云的初始化自动配准方法，其特征在于，所述步骤十中，点云
Figure FDA00030833371300000412
与点云
Figure FDA00030833371300000413
间的配准误差的具体计算如公式(10)所示：
Figure FDA00030833371300000414
其中F表示F范数。
CN202110573218.6A 2021-05-25 2021-05-25 一种激光雷达与视觉点云的初始化自动配准方法 Active CN113436235B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110573218.6A CN113436235B (zh) 2021-05-25 2021-05-25 一种激光雷达与视觉点云的初始化自动配准方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110573218.6A CN113436235B (zh) 2021-05-25 2021-05-25 一种激光雷达与视觉点云的初始化自动配准方法

Publications (2)

Publication Number Publication Date
CN113436235A true CN113436235A (zh) 2021-09-24
CN113436235B CN113436235B (zh) 2022-07-01

Family

ID=77803380

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110573218.6A Active CN113436235B (zh) 2021-05-25 2021-05-25 一种激光雷达与视觉点云的初始化自动配准方法

Country Status (1)

Country Link
CN (1) CN113436235B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113917452A (zh) * 2021-09-30 2022-01-11 北京理工大学 一种视觉与雷达结合的盲道检测装置和方法

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105701820A (zh) * 2016-01-14 2016-06-22 上海大学 一种基于匹配区域的点云配准方法
CN107204009A (zh) * 2017-05-23 2017-09-26 哈尔滨工业大学 基于仿射变换模型cpd算法的三维点云配准方法
CN110658530A (zh) * 2019-08-01 2020-01-07 北京联合大学 一种基于双激光雷达数据融合的地图构建方法、系统及地图
CN111508008A (zh) * 2020-04-08 2020-08-07 达闼机器人有限公司 一种点云配准的方法、电子设备和存储介质

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105701820A (zh) * 2016-01-14 2016-06-22 上海大学 一种基于匹配区域的点云配准方法
CN107204009A (zh) * 2017-05-23 2017-09-26 哈尔滨工业大学 基于仿射变换模型cpd算法的三维点云配准方法
CN110658530A (zh) * 2019-08-01 2020-01-07 北京联合大学 一种基于双激光雷达数据融合的地图构建方法、系统及地图
CN111508008A (zh) * 2020-04-08 2020-08-07 达闼机器人有限公司 一种点云配准的方法、电子设备和存储介质

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
NINGSHENG HUANG 等: "Optimization for RGB-D SLAM Based on Plane Geometrical Constraint", 《2019 IEEE INTERNATIONAL SYMPOSIUM ON MIXED AND AUGMENTED REALITY ADJUNCT (ISMAR-ADJUNCT)》 *
ZHEN DONG 等: "Registration of large-scale terrestrial laser scanner point clouds: A review", 《ISPRS JOURNAL OF PHOTOGRAMMETRY AND REMOTE SENSING》 *
彭蹦等: "一种多模式融合的激光点云配准算法", 《激光与红外》 *
郭杭等: "基于体素格尺度不变特征变换的快速点云配准方法", 《中国惯性技术学报》 *
陈思颖等: "城区机载激光雷达点云数据与航空影像的多尺度配准方法", 《北京理工大学学报》 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113917452A (zh) * 2021-09-30 2022-01-11 北京理工大学 一种视觉与雷达结合的盲道检测装置和方法
CN113917452B (zh) * 2021-09-30 2022-05-24 北京理工大学 一种视觉与雷达结合的盲道检测装置和方法

Also Published As

Publication number Publication date
CN113436235B (zh) 2022-07-01

Similar Documents

Publication Publication Date Title
CN109544677B (zh) 基于深度图像关键帧的室内场景主结构重建方法及系统
CN110458939B (zh) 基于视角生成的室内场景建模方法
Gruen et al. Least squares 3D surface and curve matching
CN110853075B (zh) 一种基于稠密点云与合成视图的视觉跟踪定位方法
US8406509B2 (en) Three-dimensional surface generation method
CN108665491B (zh) 一种基于局部参考点的快速点云配准方法
Pons-Moll et al. Model-based pose estimation
CN112651944B (zh) 基于cad模型的3c部件高精度六维位姿估计方法及系统
CN114067075A (zh) 基于生成对抗网络的点云补全方法及装置
CN113436235B (zh) 一种激光雷达与视觉点云的初始化自动配准方法
He et al. Pose tracking of spacecraft based on point cloud DCA features
CN111612887B (zh) 一种人体测量方法和装置
JP6761388B2 (ja) 推定装置及びプログラム
Shang et al. Model-based tracking by classification in a tiny discrete pose space
CN115423854A (zh) 一种基于多尺度特征提取的多视角点云配准及点云融合方法
JP6853150B2 (ja) 選別装置、方法及びプログラム
Wang et al. Slow-Spinning Spacecraft Cross-Range Scaling and Attitude Estimation Based on Sequential ISAR Images
Sainz et al. Recovering 3D metric structure and motion from multiple uncalibrated cameras
CN117876447B (zh) 一种基于微表面融合与对齐的三维点云配准方法
CN117541614B (zh) 基于改进icp算法的空间非合作目标近距离相对位姿跟踪方法
Hall et al. Object Registration in Neural Fields
CN118135107A (zh) 三维场景快速建模以及姿态获取方法及系统
Srichumroenrattana et al. Stereoscopic face reconstruction from a single 2-dimensional face image using orthogonality of normal surface and y-ratio
Wu et al. Registration for cross-source point clouds based on Gaussian mixture model
Tokime et al. CrowdSLAM: Toward Large-Scale 3D Structure from Crowd-Captured Images

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