CN111508008A - 一种点云配准的方法、电子设备和存储介质 - Google Patents
一种点云配准的方法、电子设备和存储介质 Download PDFInfo
- Publication number
- CN111508008A CN111508008A CN202010270686.1A CN202010270686A CN111508008A CN 111508008 A CN111508008 A CN 111508008A CN 202010270686 A CN202010270686 A CN 202010270686A CN 111508008 A CN111508008 A CN 111508008A
- Authority
- CN
- China
- Prior art keywords
- point cloud
- data
- cloud registration
- relationship
- relation
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 72
- 238000005259 measurement Methods 0.000 claims abstract description 6
- 238000013519 translation Methods 0.000 claims description 19
- 230000008569 process Effects 0.000 claims description 15
- 230000009466 transformation Effects 0.000 claims description 7
- 238000004590 computer program Methods 0.000 claims description 6
- 230000001131 transforming effect Effects 0.000 claims description 5
- 230000006870 function Effects 0.000 description 8
- 239000004576 sand Substances 0.000 description 5
- 239000011159 matrix material Substances 0.000 description 4
- 238000012545 processing Methods 0.000 description 3
- 238000013461 design Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000002093 peripheral effect Effects 0.000 description 2
- 240000007594 Oryza sativa Species 0.000 description 1
- 235000007164 Oryza sativa Nutrition 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 235000009566 rice Nutrition 0.000 description 1
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
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F18/00—Pattern recognition
- G06F18/20—Analysing
- G06F18/22—Matching criteria, e.g. proximity measures
-
- 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)
- General Physics & Mathematics (AREA)
- Data Mining & Analysis (AREA)
- Artificial Intelligence (AREA)
- Bioinformatics & Cheminformatics (AREA)
- Bioinformatics & Computational Biology (AREA)
- Evolutionary Biology (AREA)
- Evolutionary Computation (AREA)
- Life Sciences & Earth Sciences (AREA)
- General Engineering & Computer Science (AREA)
- Image Analysis (AREA)
- Image Processing (AREA)
Abstract
本发明实施例涉及三维重建技术领域,公开了一种点云配准的方法、电子设备和存储介质。本发明中点云配准的方法,包括:获取与第一点云数据匹配的第一惯性测量单元IMU数据以及与第二点云数据匹配的第二IMU数据;根据第一IMU数据和第二IMU数据,确定第一点云数据与第二点云数据的点云配准关系中n个维度的参数,点云配准关系中包括k个维度的参数,k>n,且k和n均为大于0的整数;根据第一点云数据、第二点云数据以及确定的n个维度的参数,确定点云配准关系中剩余的k‑n个维度的参数,得到点云配准关系。采用本实施方式,能够提高点云配准的准确度,提高点云配准的速度。
Description
技术领域
本发明实施例涉及三维重建技术领域,特别涉及一种点云配准的方法、电子设备和存储介质。
背景技术
在三维重建领域和即时定位与地图构建(simultaneous localization andmapping,简称“SLAM”)领域,点云配准技术是最关键、最基础的技术之一,通过点云配准可以计算出深度传感器的空间旋转和平移变化,进而恢复出更加丰富的三维结构。
发明人发现相关技术中至少存在如下问题:目前的点云配准通常采用迭代最近邻法(Iterative Closest Point,简称“ICP”)存在配准速度慢以及容易出现配准失败的问题。
发明内容
本发明实施方式的目的在于提供一种点云配准的方法、电子设备和存储介质,能够提高点云配准的准确度,提高点云配准的速度。
为解决上述技术问题,本发明的实施方式提供了一种点云配准的方法,包括:获取与第一点云数据匹配的第一惯性测量单元IMU数据以及与第二点云数据匹配的第二IMU数据;根据第一IMU数据和第二IMU数据,确定第一点云数据与第二点云数据的点云配准关系中n个维度的参数,点云配准关系中包括k个维度的参数,k>n,且k和n均为大于0的整数;根据第一点云数据、第二点云数据以及确定的n个维度的参数,确定点云配准关系中剩余的k-n个维度的参数,得到点云配准关系。
本发明的实施方式还提供了一种电子设备,包括:至少一个处理器,以及,与至少一个处理器通信连接的存储器;其中,存储器存储有可被至少一个处理器执行的指令,指令被至少一个处理器执行,以使至少一个处理器能够执行上述的点云配准的方法。
本发明的实施方式还提供了一种可读存储介质,存储有计算机程序,计算机程序被处理器执行时实现上述的点云配准的方法。
本发明实施方式相对于现有技术而言,第一IMU数据与第一点云数据匹配,第二IMU数据与第二点云数据相匹配,由于通过第一IMU数据和第二IMU数据可以确定出点云配准关系中的n个维度的参数后,根据第一点云数据、第二点云数据以及确定的n个维度的参数,只需确定点云配准关系中剩余的k-n个维度的参数,减少了在后续点云配准过程中的待求解的维度的个数,大大缩短了点云配准的时间,提高了点云配准的速度;由于IMU测量的IMU数据中仅有一个姿态角存在漂移,而其他姿态角的数据非常准确,使得通过第一IMU数据和第二IMU数据,确定的n个维度的参数也是准确的,大大提高了点云配准关系的准确性;此外,由于n个维度的参数的准确性,也提高了后续确定的剩余k-n个维度的参数的准确性,使得得到的点云配准关系非常准确。
另外,根据第一点云数据、第二点云数据以及确定的n个维度的参数,确定点云配准关系中剩余的k-n个维度的参数,得到点云配准关系,包括:初始化步骤和迭代步骤;初始化步骤包括,根据确定的n个维度的参数确定初始化的点云配准关系;迭代步骤包括:根据第一点云数据、第二点云数据、上一次预测的点云配准关系以及预设的解算方程,调整点云配准关系中剩余的k-n个维度的参数,得到本次预测的点云配准关系,解算方程用于确定对上一次预测的点云配准关系中剩余k-n个维度调整的空间增量;重复执行迭代步骤直至本次预测的点云配准关系收敛;其中,迭代步骤首次执行中使用的上一次预测的点云配准关系为初始化的点云配准关系。通过迭代的方式不断调整点云配准关系中的剩余的k-n个维度的参数,从而确定出满足收敛关系的点云配准关系,只调整点云配准关系中剩余的k-n个维度的参数,可以有效减下迭代次数,可以提高确定的点云配准关系的准确性。
另外,点云配准关系包括:第一点云数据与第二点云数据之间的旋转关系以及平移关系,空间增量包括旋转增量和平移增量;在根据第一点云数据、第二点云数据、上一次预测的点云配准关系以及预设的解算方程,调整点云配准关系中剩余的k-n个维度的参数得到本次预测的点云配准关系之前,方法还包括:根据预设的点云数据之间的残差关系以及残差最小原则,构建基于点云配准关系的姿态方程;根据姿态方程以及预设的扰动条件,确定基于空间增量的解算方程,扰动条件为:对旋转关系增加旋转增量后的值等于对旋转关系中剩余的3-n个维度的参数增加旋转增量后的值,其中,旋转关系包括3个维度的参数,且n=2。根据扰动条件,可以快速确定出解算方程。
另外,初始化的点云配准关系包括:初始化的旋转关系和初始化的平移关系;初始化的点云配准关系的确定过程,包括:将第一IMU数据和第二IMU数据之间的差异关系作为初始化的旋转关系;将初始化的平移关系中各维度的参数均置为0。将第一IMU数据和第二IMU数据之间的差异关系作为初始化的旋转关系,使得在迭代之前无需进行额外的粗定位,且由于该第一IMU数据和第二IMU数据可以确定出点云配准关系中n个维度的参数,使得该初始化的旋转关系与点云配准关系中的旋转关系之间的差值小,便于可以速得到最后的点云配准关系,减少迭代次数。
另外,根据第一点云数据、第二点云数据、上一次预测的点云配准关系以及预设的解算方程,调整点云配准关系中剩余的k-n个维度的参数,得到本次预测的点云配准关系,包括:根据第一点云数据、第二点云数据、上一次预测的点云配准关系以及预设的解算方程,确定对上一次预测的点云配准关系的空间增量;根据上一次预测的点云配准关系以及空间增量,确定本次预测的点云配准关系;判断空间增量是否满足预设条件,若未满足,则继续进行下一次迭代,否则,确定本次预设的点云配准关系收敛。
另外,预设条件为:确定的空间增量小于预设阈值。
另外,n为2;根据第一IMU数据和第二IMU数据,确定第一点云数据与第二点云数据的点云配准关系中n个维度的参数,包括:确定第一IMU数据和第二IMU数据之间的差异关系,将差异关系中的横滚姿态角和俯仰姿态角作为点云配准关系中的2个维度的参数。由于IMU数据的横滚姿态角和俯仰姿态角的数据准确,将差异关系中的横滚姿态角和俯仰姿态角作为点云配准关系中的2个维度的参数,使得确定的2个维度的参数速度快且准确。
另外,在根据第一点云数据、第二点云数据以及确定的n个维度的参数,确定点云配准关系中剩余的k-n个维度的参数之前,方法还包括:根据云坐标系与IMU坐标系之间的空间变换关系,将第一点云数据和第二点云数据变换至IMU坐标系下。将第一点云数据和第二点云数据转换到IMU坐标系下,使得可以将点云坐标系中X轴与Y轴分别对应于IMU坐标系下的横滚姿态角和俯仰姿态角。
另外,在根据第一点云数据、第二点云数据以及确定的n个维度的参数,确定点云配准关系中剩余的k-n个维度的参数之后,方法还包括:将确定的点云配准关系,从IMU坐标系转换为点云坐标系。将确定的点云配准关系转换回点云坐标系下,便于后续对第一点云数据和第二点云数据进行配准。
附图说明
一个或多个实施例通过与之对应的附图中的图片进行示例性说明,这些示例性说明并不构成对实施例的限定,附图中具有相同参考数字标号的元件表示为类似的元件,除非有特别申明,附图中的图不构成比例限制。
图1是根据本发明第一实施例中提供的一种点云配准的方法的具体流程图;
图2是根据本发明第二实施例中提供的一种点云配准的方法的具体流程图;
图3是根据本发明第三实施例中提供的一种电子设备的具体结构示意图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚,下面将结合附图对本发明的各实施方式进行详细的阐述。然而,本领域的普通技术人员可以理解,在本发明各实施方式中,为了使读者更好地理解本申请而提出了许多技术细节。但是,即使没有这些技术细节和基于以下各实施方式的种种变化和修改,也可以实现本申请所要求保护的技术方案。
发明人发现相关的点云配准方法通常是进行粗配准,根据粗配准的点云配准关系以及ICP算法,进行精细配准,但是在进行ICP精细配准过程中,若两个点云数据之间的差别较大,或者点云中三维特征不明显的情况下,ICP算法会陷入局部最优而导致配准失败的情况,且迭代次数多,耗费时间长。
本发明的第一实施方式涉及一种点云配准的方法。该点云配准的方法应用于具有数据处理功能的电子设备,例如,无人驾驶车、服务器、电脑、机器人等设备。该点云配准的方法的具体流程如图1所示。
步骤101:获取与第一点云数据匹配的第一惯性测量单元IMU数据以及与第二点云数据匹配的第二IMU数据。
具体的说,第一点云数据和第二点云数据可以是电子设备中的点云采集装置采集获得,点云采集装置可以是各种类型的深度传感器,例如,多线激光雷达、双目立体相机、结构光/TOF深度相机等。第一IMU数据与第一点云数据的采集时刻相同;第二IMU数据与第二点云数据的采集时刻相同。第一点云数据和第二点云数据为同一点云采集装置在不同时刻采集的点云数据,例如,该第一IMU数据为t时刻IMU采集的数据,第二IMU数据为在s时刻IMU采集的数据。下面以t时刻的采集的点云数据作为第一点云数据Plt,s时刻采集的点云数据作为第二点云数据Pls,t时刻采集的IMU姿态角作为与第一点云数据匹配的第一IMU数据Rt,s时刻采集的IMU姿态角作为与第二点云数据匹配的第二IMU数据Rs;第一点云数据、第二点云数据可以如公式(1)和公式(2)所示;
其中,第一点云数据Plt中包括j个点,第二点云数据Pls中包括g个点。
可以理解的是,该点云采集装置和IMU均设置在同一个设备上。IMU可以将采集的第一IMU数据和第二IMU数据上传电子设备,点云采集装置将采集的第一点云数据和第二点云数据上传电子设备。为了点云配准的准确性,可以滤除第一点云数据和第二点云数据中的无效点。
步骤102:根据第一IMU数据和第二IMU数据,确定第一点云数据与第二点云数据的点云配准关系中n个维度的参数,点云配准关系中包括k个维度的参数,k>n,且k和n均为大于0的整数。
具体的说,点云配准关系包括:第一点云数据与第二点云数据之间的旋转关系以及平移关系。本示例中,以k为6,n为2为例,介绍确定点云配准关系中n个维度的参数的过程。旋转关系中包括3个维度的参数,平移关系中包括3个维度的参数,且n=2。
在一个例子中,n为2;根据第一IMU数据和第二IMU数据,确定第一点云数据与第二点云数据的点云配准关系中n个维度的参数,包括:确定第一IMU数据和第二IMU数据之间的差异关系,将差异关系中的横滚姿态角和俯仰姿态角作为点云配准关系中的2个维度的参数。
具体的说,由于IMU测量的横滚姿态角和俯仰姿态角的值非常准确,而偏航姿态角的值随着移动存在漂移,导致偏航姿态角不准确,可以将第一IMU数据和第二IMU数据之间的差异关系中的横滚姿态角和俯仰姿态角作为点云配准关系中的2个维度的参数。设定旋转关系R为3*3的矩阵,那么该差异关系可以如公式(3)所示:
该R即为第一IMU数据和第二IMU数据之间的差异关系,该差异关系中的横滚姿态角和俯仰姿态角作为点云配准关系中的2个维度的参数。
值得一提的是,为了后续快速构建解算方程,设定点云坐标系中的X轴对应IMU坐标系中的横滚姿态角,点云坐标系中的Y轴对应IMU坐标系中的俯仰姿态角,点云坐标系中的Z轴对应IMU坐标系中的偏航姿态角。
步骤103:根据第一点云数据、第二点云数据以及确定的n个维度的参数,确定点云配准关系中剩余的k-n个维度的参数,得到点云配准关系。
在一个例子中,得到点云配准关系的过程包括:初始化步骤和迭代步骤。下面分别介绍初始化步骤和迭代步骤。
初始化步骤包括,根据确定的n个维度的参数确定初始化的点云配准关系。
具体的说,点云配准关系包括第一点云数据和第二点云数据之间的旋转关系和平移关系,该初始化的点云配准关系包括:初始化的旋转关系和初始化的平移关系。初始化的点云配准关系的确定过程为:将第一IMU数据和第二IMU数据之间的差异关系作为初始化的旋转关系;将初始化的平移关系中各维度的参数均置为0。例如,设定旋转关系R为3*3的矩阵,那么该差异关系可以公式(3)所示:
设定平移关系为3*1的向量,将初始化的平移关系中各维度的参数均设置为0,如公式(4)所示:
T={0,0,0} 公式(4);
此时公式(3)中的R作为初始化的旋转关系,将T={0,0,0}作为初始化的平移关系。
迭代步骤包括:根据第一点云数据、第二点云数据、上一次预测的点云配准关系以及预设的解算方程,调整点云配准关系中剩余的k-n个维度的参数,得到本次预测的点云配准关系,解算方程用于确定对上一次预测的点云配准关系中剩余k-n个维度调整的空间增量;重复执行迭代步骤直至本次预测的点云配准关系收敛;其中,迭代步骤首次执行中使用的上一次预测的点云配准关系为初始化的点云配准关系。
在一个例子中,进行迭代步骤之前,可以预先确定出解算方程,解算方程的确定过程包括:根据预设的点云数据之间的残差关系以及残差最小原则,构建基于点云配准关系的姿态方程;根据姿态方程以及预设的扰动条件,确定基于空间增量的解算方程,扰动条件为:对旋转关系增加旋转增量后的值等于对旋转关系中剩余的3-n个维度的参数增加旋转增量后的值,其中,旋转关系包括3个维度的参数。
具体的说,预设的点云数据包括N个匹配点的点云Ps和Pt,p0和p1是任意一组匹配点,其中,匹配点是指配准成功的两个点。那么,预设的点云数据之间的残差关系如公式(5)所示:
ε=Rp0+T-p1 公式(5);
该残差公式是基于旋转关系R和平移关系T的函数,根据残差最小原则,可以得到如公式(6)所示的姿态方程:
Rp0+T-p1≈0 公式(6);
公式(6)的左边是关于旋转关系R和平移关系T的函数,即关于点云配准关系的函数,用f(R,T)表示。
R和T加上很小的旋转增量Δθ和平移增量ΔT,根据多元函数的一阶泰勒展开公式得到公式(7):
根据扰动条件:对旋转关系增加旋转增量后的值等于对旋转关系中剩余的3-n个维度的参数增加旋转增量后的值,其中,旋转关系包括3个维度的参数,n=2,那么姿态方程公式(6)按照扰动条件进行变形,扰动条件的关系如公式(8)所示,可推导出公式(9);
根据公式(8),可得公式(9):
根据公式(7)和公式(9),即可得到关于空间增量的解算方程,该空间增量包括Δθ和ΔT,解算方程的公式可以如公式(10)和公式(11)所示:
或者;
解算方程得到之后,即可进行迭代步骤。
在一个例子中,每次迭代得到本次预测的点云配准关系的具体过程可以包括:根据第一点云数据、第二点云数据、上一次预测的点云配准关系以及预设的解算方程,确定对上一次预测的点云配准关系的空间增量;根据上一次预测的点云配准关系以及空间增量,确定本次预测的点云配准关系;判断空间增量是否满足预设条件,若未满足,则继续进行下一次迭代,否则,确定本次预设的点云配准关系收敛。
具体的说,根据上一次预测的点云配准关系和第一点云数据,得到配准后的第一点云数据;在第二点云数据中搜寻与配准后的第一点云数据中的点相匹配的点,得到多组点云匹配对;根据多组匹配点以及解算方程,确定点云配准关系中的空间增量。具体过程如下:
根据该初始化的点云配准关系和第一点云数据,得到配准后的第一点云数据,配准后的第一点云数据如公式(12)所示:
P=RPi s+T 公式(12);
其中,P表示配准后的第一点云数据,Pi s表示第一点云数据,R表示初始化的旋转关系,T表示初始化的平移关系。
在第二点云数据中搜寻与配准后的第一点云数据的点相匹配的点,得到多组匹配点,搜寻的方式可以采用欧氏距离最小的方式,例如,很对配准后的第一点云数据P中的每个点,可以进行如下处理;以该点P1 s为中心,预设半径的范围内从第二点云数据中搜寻与该点的欧氏距离最小的点P1 t,欧氏距离最小的点可以根据距离阈值判断是否为最小,若小于该距离阈值,则确定为欧氏距离最小,那么该点P1 s和点P1 t为以一组匹配点;得到m组匹配点,m组匹配点分别用Ps和Pt表示,如公式(13)所示:
将第i组匹配点代入解算方程,可得如公式(14)所示的方程:
每一组匹配点对可以建立一个方程组,m组匹配点可以建立m个方程组,联立方程组,求解方程组即可得到未知数Δθz和ΔT,求解方式可以采用奇异值分解(SingularValue Decomposition,简称“SVD”)算法,其中,ΔT为三维向量,Δθz即为待求解的旋转关系中的偏航姿态角的增量,根据Δθz即可确定出旋转增量Δθ,Δθ中的横滚姿态角和俯仰姿态角的增量均为0。
得到空间增量(即Δθ和ΔT)之后,根据上一次预测的点云配准关系以及该空间增量,确定本次预测的点云配准关系,即,本次预测的点云配准关系如公式(16)和公式(17)所示:
R=ΔR·R 公式(16);
T=T+ΔT 公式(17);
判断空间增量是否满足预设条件,若未满足,则继续进行下一次迭代,否则,确定本次预设的点云配准关系收敛,预设条件为:确定的空间增量小于预设阈值,预设阈值包括旋转增量的旋转阈值,以及平移增量的平移阈值。
本发明实施方式相对于现有技术而言,第一IMU数据与第一点云数据匹配,第二IMU数据与第二点云数据相匹配,由于通过第一IMU数据和第二IMU数据可以确定出点云配准关系中的n个维度的参数后,根据第一点云数据、第二点云数据以及确定的n个维度的参数,只需确定点云配准关系中剩余的k-n个维度的参数,减少了在后续点云配准过程中的待求解的维度的个数,大大缩短了点云配准的时间,提高了点云配准的速度;由于IMU测量的IMU数据中仅有一个姿态角存在漂移,而其他姿态角的数据非常准确,使得通过第一IMU数据和第二IMU数据,确定的n个维度的参数也是准确的,大大提高了点云配准关系的准确性;此外,由于n个维度的参数的准确性,也提高了后续确定的剩余k-n个维度的参数的准确性,使得得到的点云配准关系非常准确。
本发明的第二实施方式涉及一种点云配准的方法。第二实施方式与第一实施方式大致相同,主要区别之处在于:在本实施方式中,在根据第一点云数据、第二点云数据以及确定的n个维度的参数,确定点云配准关系中剩余的k-n个维度的参数之前,将第一点云数据和第二点云数据变换至IMU坐标系下,该点云配准的方法的具体流程如图2所示:
步骤201:获取与第一点云数据匹配的第一惯性测量单元IMU数据以及与第二点云数据匹配的第二IMU数据。
步骤202:根据点云坐标系与IMU坐标系之间的空间变换关系,将第一点云数据和第二点云数据变换至IMU坐标系下。
具体的说,IMU与点云采集装置之间的相对位置固定,可以获取预先标定的IMU和点云采集装置之间的空间变换关系,根据该空间变换关系,可以将第一点云数据和第二点云数据变换至IMU坐标系下。
例如,t时刻深度传感器采集的点云数据为第一点云数据,表示为Plt;s时刻深度传感器采集的点云数据为第二点云数据,表示为Pls,其中,Plt中包括j个点,Pls中包括g个点,t和s为不相等的两个时刻;与Plt匹配的第一IMU数据表示为Rt,与Pls匹配的第二IMU数据表示为Rs。点云坐标系与该IMU坐标系的空间变换关系可以包括:深度传感器到IMU的旋转关系以及深度传感器到IMU的平移关系那么将Plt和Pls变换到IMU坐标系下得到变换后的第一点云数据Pi t和变换后的第二点云数据Pi s,如公式(18)和公式(19);
值得一提的是,在转换坐标系之后,步骤203至步骤204均在IMU坐标系下进行,由于将点云坐标系转换至IMU坐标系下,使得可以将点云坐标系中X轴与Y轴分别对应于IMU坐标系下的横滚姿态角和俯仰姿态角对应,以便可以通过第一IMU数据和第二IMU数据快速确定点云配准关系中的横滚姿态角和俯仰姿态角。
步骤203:根据第一IMU数据和第二IMU数据,确定第一点云数据与第二点云数据的点云配准关系中n个维度的参数。
步骤204:根据第一点云数据、第二点云数据以及确定的n个维度的参数,确定点云配准关系中剩余的k-n个维度的参数,得到点云配准关系。
步骤205:将确定的点云配准关系,从IMU坐标系转换为点云坐标系。
将确定的点云配准关系从IMU坐标系转换为点云坐标系,便于第一点云数据和第二点云数据根据点云配准关系进行配准。
下面以一个具体的例子介绍整个点云配准的过程:
将100HZ的IMU安装在10HZ的16线激光雷达顶部,激光雷达可以安装在机器人顶部,然后把机器人开到室外。在t时刻激光雷达采集第一点云数据Plt,以及与第一点云数据匹配的第一IMU数据Rt;之后控制机器人移动2.5米,在转动30度,在s时刻激光雷达采集第二点云数据Pls以及与第二点云数据Pls匹配的第二IMU数据Rs;其中,Pls包含32000个点,滤除无效点后剩余22409个点,Plt包含32000个点,滤除无效点后剩余23521个点。
将过滤后的第一点云数据和第二点云数据变换至IMU坐标系下;获得从激光雷达到IMU的旋转关系和从激光雷达到IMU的旋转关系的平移关系将Plt和Pls变换到IMU坐标系下得到变换后的第一点云数据Pi t和变换后的第二点云数据Pi s,如公式(18)和公式(19);
设定初始化的旋转关系R为3*3的矩阵,由第一IMU数据和第二IMU数据得到:
设定初始化的平移关系T为3*1的向量,各个维度的参数均为0,得到T={0,0,0}。
设置预设条件为:Δθz的旋转阈值为0.1度,ΔT的平移阈值为0.05度;另外为了防止迭代次数过多,可以设置最大迭代次数为10,可以设定搜寻匹配点对过程中匹配点对两者之间的距离阈值为0.3米。
本示例中,经过3次迭代后,获得点云配准关系,配准成功,表1为迭代详情:
迭代次数 | 1 | 2 | 3 |
匹配点对数量(个) | 6399 | 11321 | 13356 |
Δθ<sub>z</sub>(度) | 2.27 | 0.38 | 0.04 |
ΔT(米) | 2.35,-0.11,0.05 | 0.07,0.05,-0.02 | 0.01,-0.02,-0.01 |
表1
每次迭代后得到当次迭代的点云配准关系。
将得到的点云配准关系的RL和TL转换至点云坐标系下。为了便于查看,还可以将旋转关系RL转为欧拉角θL,即:
θL={0.03,0.01,31.07};
TL={2.43,-0.08,0.02};
可见,该结果与实际移动和转动相符合。
本实施方式提供的点云配准的方法,将第一点云数据和第二点云数据转换到IMU坐标系下,使得可以将点云坐标系中X轴与Y轴分别对应于IMU坐标系下的横滚姿态角和俯仰姿态角。
上面各种方法的步骤划分,只是为了描述清楚,实现时可以合并为一个步骤或者对某些步骤进行拆分,分解为多个步骤,只要包括相同的逻辑关系,都在本专利的保护范围内;对算法中或者流程中添加无关紧要的修改或者引入无关紧要的设计,但不改变其算法和流程的核心设计都在该专利的保护范围内。
本发明第三实施方式涉及一种电子设备,该电子设备30的具体结构如图3所示,包括:至少一个处理器301,以及,与至少一个处理器301通信连接的存储器302;其中,存储器302存储有可被至少一个处理器301执行的指令,指令被至少一个处理器301执行,以使至少一个处理器301能够执行第一实施方式或第二实施方式中的点云配准的方法。
其中,存储器302和处理器301采用总线方式连接,总线可以包括任意数量的互联的总线和桥,总线将一个或多个处理器301和存储器302的各种电路链接在一起。总线还可以将诸如外围设备、稳压器和功率管理电路等之类的各种其他电路链接在一起,这些都是本领域所公知的,因此,本文不再对其进行进一步描述。总线接口在总线和收发机之间提供接口。收发机可以是一个元件,也可以是多个元件,比如多个接收器和发送器,提供用于在传输介质上与各种其他装置通信的单元。经处理器处理的数据通过天线在无线介质上进行传输,进一步,天线还接收数据并将数据传送给处理器301。
处理器301负责管理总线和通常的处理,还可以提供各种功能,包括定时,外围接口,电压调节、电源管理以及其他控制功能。而存储器可以被用于存储处理器在执行操作时所使用的数据。
本发明第四实施方式涉及一种可读存储介质,存储有计算机程序,计算机程序被处理器执行时实现第一实施方式或第二实施方式的点云配准的方法。
本领域技术人员可以理解实现上述实施例方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成,该程序存储在一个存储介质中,包括若干指令用以使得一个设备(可以是单片机,芯片等)或处理器(processor)执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括:U盘、移动硬盘、只读存储器(ROM,Read-OnlyMemory)、随机存取存储器(RAM,Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
本领域的普通技术人员可以理解,上述各实施方式是实现本发明的具体实施例,而在实际应用中,可以在形式上和细节上对其作各种改变,而不偏离本发明的精神和范围。
本申请实施例公开A1.一种点云配准的方法,包括:
获取与第一点云数据匹配的第一惯性测量单元IMU数据以及与第二点云数据匹配的第二IMU数据;
根据第一IMU数据和所述第二IMU数据,确定所述第一点云数据与所述第二点云数据的点云配准关系中n个维度的参数,所述点云配准关系中包括k个维度的参数,k>n,且k和n均为大于0的整数;
根据所述第一点云数据、所述第二点云数据以及确定的n个维度的参数,确定所述点云配准关系中剩余的k-n个维度的参数,得到所述点云配准关系。
A2.根据A1所述的点云配准的方法,所述根据所述第一点云数据、所述第二点云数据以及确定的n个维度的参数,确定所述点云配准关系中剩余的k-n个维度的参数,得到所述点云配准关系,包括:初始化步骤和迭代步骤;
所述初始化步骤包括,根据所述确定的n个维度的参数确定初始化的点云配准关系;
所述迭代步骤包括:根据所述第一点云数据、所述第二点云数据、上一次预测的点云配准关系以及预设的解算方程,调整所述点云配准关系中剩余的k-n个维度的参数,得到本次预测的点云配准关系,所述解算方程用于确定对上一次预测的点云配准关系中剩余k-n个维度调整的空间增量;重复执行所述迭代步骤直至所述本次预测的点云配准关系收敛;其中,所述迭代步骤首次执行中使用的所述上一次预测的点云配准关系为所述初始化的点云配准关系。
A3.根据A2所述的点云配准的方法,所述点云配准关系包括:所述第一点云数据与所述第二点云数据之间的旋转关系以及平移关系,所述空间增量包括旋转增量和平移增量;
在所述根据所述第一点云数据、所述第二点云数据、上一次预测的点云配准关系以及预设的解算方程,调整所述点云配准关系中剩余的k-n个维度的参数得到本次预测的点云配准关系之前,所述方法还包括:
根据预设的点云数据之间的残差关系以及残差最小原则,构建基于所述点云配准关系的姿态方程;
根据所述姿态方程以及预设的扰动条件,确定基于所述空间增量的解算方程,所述扰动条件为:对所述旋转关系增加所述旋转增量后的值等于对所述旋转关系中剩余的3-n个维度的参数增加所述旋转增量后的值,其中,旋转关系包括3个维度的参数,且n=2。
A4.根据A2所述的点云配准的方法,所述初始化的点云配准关系包括:初始化的旋转关系和初始化的平移关系;
所述初始化的点云配准关系的确定过程,包括:
将第一IMU数据和所述第二IMU数据之间的差异关系作为初始化的旋转关系;
将初始化的平移关系中各维度的参数均置为0。
A5.根据A2至A4中任一项所述的点云配准的方法,根据所述第一点云数据、所述第二点云数据、上一次预测的点云配准关系以及预设的解算方程,调整所述点云配准关系中剩余的k-n个维度的参数,得到本次预测的点云配准关系,包括:
所述根据第一点云数据、所述第二点云数据、上一次预测的点云配准关系以及预设的解算方程,确定对上一次预测的点云配准关系的空间增量;
根据所述上一次预测的点云配准关系以及所述空间增量,确定所述本次预测的点云配准关系;
判断所述空间增量是否满足预设条件,若未满足,则继续进行下一次迭代,否则,确定所述本次预设的点云配准关系收敛。
A6.根据A5所述的点云配准的方法,所述预设条件为:确定的所述空间增量小于预设阈值。
A7.根据A1或A2所述的点云配准的方法,所述n为2;
所述根据第一IMU数据和所述第二IMU数据,确定所述第一点云数据与所述第二点云数据的点云配准关系中n个维度的参数,包括:
确定所述第一IMU数据和所述第二IMU数据之间的差异关系,将所述差异关系中的横滚姿态角和俯仰姿态角作为所述点云配准关系中的2个维度的参数。
A8.根据A1至A7中任一项所述的点云配准的方法,在所述根据所述第一点云数据、所述第二点云数据以及确定的n个维度的参数,确定所述点云配准关系中剩余的k-n个维度的参数之前,所述方法还包括:
根据点云坐标系与IMU坐标系之间的空间变换关系,将所述第一点云数据和所述第二点云数据变换至所述IMU坐标系下。
A9.根据A7所述的点云配准的方法,在所述根据所述第一点云数据、所述第二点云数据以及确定的n个维度的参数,确定所述点云配准关系中剩余的k-n个维度的参数之后,所述方法还包括:
将确定的点云配准关系,从IMU坐标系转换为所述点云坐标系。
本申请实施例公开了B1.一种电子设备,包括:至少一个处理器,以及,与所述至少一个处理器通信连接的存储器;其中,所述存储器存储有可被所述至少一个处理器执行的指令,所述指令被所述至少一个处理器执行,以使所述至少一个处理器能够执行A1至A9任一所述的点云配准的方法。
本申请实施例公开了C1.一种计算机可读存储介质,存储有计算机程序,所述计算机程序被处理器执行时实现A1至A9中任一项所述的点云配准的方法。
Claims (10)
1.一种点云配准的方法,其特征在于,包括:
获取与第一点云数据匹配的第一惯性测量单元IMU数据以及与第二点云数据匹配的第二IMU数据;
根据第一IMU数据和所述第二IMU数据,确定所述第一点云数据与所述第二点云数据的点云配准关系中n个维度的参数,所述点云配准关系中包括k个维度的参数,k>n,且k和n均为大于0的整数;
根据所述第一点云数据、所述第二点云数据以及确定的n个维度的参数,确定所述点云配准关系中剩余的k-n个维度的参数,得到所述点云配准关系。
2.根据权利要求1所述的点云配准的方法,其特征在于,所述根据所述第一点云数据、所述第二点云数据以及确定的n个维度的参数,确定所述点云配准关系中剩余的k-n个维度的参数,得到所述点云配准关系,包括:初始化步骤和迭代步骤;
所述初始化步骤包括,根据所述确定的n个维度的参数确定初始化的点云配准关系;
所述迭代步骤包括:根据所述第一点云数据、所述第二点云数据、上一次预测的点云配准关系以及预设的解算方程,调整所述点云配准关系中剩余的k-n个维度的参数,得到本次预测的点云配准关系,所述解算方程用于确定对上一次预测的点云配准关系中剩余k-n个维度调整的空间增量;重复执行所述迭代步骤直至所述本次预测的点云配准关系收敛;其中,所述迭代步骤首次执行中使用的所述上一次预测的点云配准关系为所述初始化的点云配准关系。
3.根据权利要求2所述的点云配准的方法,其特征在于,所述点云配准关系包括:所述第一点云数据与所述第二点云数据之间的旋转关系以及平移关系,所述空间增量包括旋转增量和平移增量;
在所述根据所述第一点云数据、所述第二点云数据、上一次预测的点云配准关系以及预设的解算方程,调整所述点云配准关系中剩余的k-n个维度的参数得到本次预测的点云配准关系之前,所述方法还包括:
根据预设的点云数据之间的残差关系以及残差最小原则,构建基于所述点云配准关系的姿态方程;
根据所述姿态方程以及预设的扰动条件,确定基于所述空间增量的解算方程,所述扰动条件为:对所述旋转关系增加所述旋转增量后的值等于对所述旋转关系中剩余的3-n个维度的参数增加所述旋转增量后的值,其中,旋转关系包括3个维度的参数,且n=2。
4.根据权利要求2所述的点云配准的方法,其特征在于,所述初始化的点云配准关系包括:初始化的旋转关系和初始化的平移关系;
所述初始化的点云配准关系的确定过程,包括:
将第一IMU数据和所述第二IMU数据之间的差异关系作为初始化的旋转关系;
将初始化的平移关系中各维度的参数均置为0。
5.根据权利要求2至4中任一项所述的点云配准的方法,其特征在于,根据所述第一点云数据、所述第二点云数据、上一次预测的点云配准关系以及预设的解算方程,调整所述点云配准关系中剩余的k-n个维度的参数,得到本次预测的点云配准关系,包括:
所述根据第一点云数据、所述第二点云数据、上一次预测的点云配准关系以及预设的解算方程,确定对上一次预测的点云配准关系的空间增量;
根据所述上一次预测的点云配准关系以及所述空间增量,确定所述本次预测的点云配准关系;
判断所述空间增量是否满足预设条件,若未满足,则继续进行下一次迭代,否则,确定所述本次预设的点云配准关系收敛。
6.根据权利要求5所述的点云配准的方法,其特征在于,所述预设条件为:确定的所述空间增量小于预设阈值。
7.根据权利要求1或2所述的点云配准的方法,其特征在于,所述n为2;
所述根据第一IMU数据和所述第二IMU数据,确定所述第一点云数据与所述第二点云数据的点云配准关系中n个维度的参数,包括:
确定所述第一IMU数据和所述第二IMU数据之间的差异关系,将所述差异关系中的横滚姿态角和俯仰姿态角作为所述点云配准关系中的2个维度的参数。
8.根据权利要求1至7中任一项所述的点云配准的方法,其特征在于,在所述根据所述第一点云数据、所述第二点云数据以及确定的n个维度的参数,确定所述点云配准关系中剩余的k-n个维度的参数之前,所述方法还包括:
根据点云坐标系与IMU坐标系之间的空间变换关系,将所述第一点云数据和所述第二点云数据变换至所述IMU坐标系下。
将确定的点云配准关系,从IMU坐标系转换为所述点云坐标系。
9.一种电子设备,其特征在于,包括:
至少一个处理器,以及,
与所述至少一个处理器通信连接的存储器;其中,
所述存储器存储有可被所述至少一个处理器执行的指令,所述指令被所述至少一个处理器执行,以使所述至少一个处理器能够执行如权利要求1至8任一所述的点云配准的方法。
10.一种计算机可读存储介质,存储有计算机程序,其特征在于,所述计算机程序被处理器执行时实现权利要求1至8中任一项所述的点云配准的方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010270686.1A CN111508008B (zh) | 2020-04-08 | 2020-04-08 | 一种点云配准的方法、电子设备和存储介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010270686.1A CN111508008B (zh) | 2020-04-08 | 2020-04-08 | 一种点云配准的方法、电子设备和存储介质 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN111508008A true CN111508008A (zh) | 2020-08-07 |
CN111508008B CN111508008B (zh) | 2023-07-14 |
Family
ID=71875895
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010270686.1A Active CN111508008B (zh) | 2020-04-08 | 2020-04-08 | 一种点云配准的方法、电子设备和存储介质 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111508008B (zh) |
Cited By (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112070835A (zh) * | 2020-08-21 | 2020-12-11 | 达闼机器人有限公司 | 机械臂位姿预测方法、装置、存储介质及电子设备 |
CN112629415A (zh) * | 2020-12-29 | 2021-04-09 | 合肥达朴汇联科技有限公司 | 一种粮仓粮食体积测量系统 |
CN113436235A (zh) * | 2021-05-25 | 2021-09-24 | 北京理工大学 | 一种激光雷达与视觉点云的初始化自动配准方法 |
CN114274139A (zh) * | 2020-09-27 | 2022-04-05 | 西门子股份公司 | 自动喷涂方法、装置、系统和存储介质 |
CN115797573A (zh) * | 2023-02-09 | 2023-03-14 | 四川省公路规划勘察设计研究院有限公司 | 一种衡量点云孪生几何准确度的方法、装置和介质 |
Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102608620A (zh) * | 2012-03-12 | 2012-07-25 | 北京北科安地科技发展有限公司 | 一种依据反射强度与地形形态的激光扫描点云植被过滤方法 |
CN105027190A (zh) * | 2013-01-03 | 2015-11-04 | 美达公司 | 用于虚拟或增强介导视觉的射出空间成像数字眼镜 |
CN107886529A (zh) * | 2017-12-06 | 2018-04-06 | 重庆理工大学 | 一种用于三维重建的点云配准方法 |
US20180218537A1 (en) * | 2013-03-15 | 2018-08-02 | State Farm Mutual Automobile Insurance Company | Audio-based 3d point cloud generation and analysis |
CN109814126A (zh) * | 2018-12-29 | 2019-05-28 | 北京农业信息技术研究中心 | 一种移动式平面激光雷达植物形态测量装置 |
CN109919989A (zh) * | 2019-03-27 | 2019-06-21 | 广东工业大学 | 一种雷达电力巡检的点云数据配准方法、装置及设备 |
CN110070567A (zh) * | 2019-04-28 | 2019-07-30 | 中铁七局集团有限公司 | 一种地面激光点云配准方法 |
CN110276834A (zh) * | 2019-06-25 | 2019-09-24 | 达闼科技(北京)有限公司 | 一种激光点云地图的构建方法、终端和可读存储介质 |
CN110335297A (zh) * | 2019-06-21 | 2019-10-15 | 华中科技大学 | 一种基于特征提取的点云配准方法 |
-
2020
- 2020-04-08 CN CN202010270686.1A patent/CN111508008B/zh active Active
Patent Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102608620A (zh) * | 2012-03-12 | 2012-07-25 | 北京北科安地科技发展有限公司 | 一种依据反射强度与地形形态的激光扫描点云植被过滤方法 |
CN105027190A (zh) * | 2013-01-03 | 2015-11-04 | 美达公司 | 用于虚拟或增强介导视觉的射出空间成像数字眼镜 |
US20180218537A1 (en) * | 2013-03-15 | 2018-08-02 | State Farm Mutual Automobile Insurance Company | Audio-based 3d point cloud generation and analysis |
CN107886529A (zh) * | 2017-12-06 | 2018-04-06 | 重庆理工大学 | 一种用于三维重建的点云配准方法 |
CN109814126A (zh) * | 2018-12-29 | 2019-05-28 | 北京农业信息技术研究中心 | 一种移动式平面激光雷达植物形态测量装置 |
CN109919989A (zh) * | 2019-03-27 | 2019-06-21 | 广东工业大学 | 一种雷达电力巡检的点云数据配准方法、装置及设备 |
CN110070567A (zh) * | 2019-04-28 | 2019-07-30 | 中铁七局集团有限公司 | 一种地面激光点云配准方法 |
CN110335297A (zh) * | 2019-06-21 | 2019-10-15 | 华中科技大学 | 一种基于特征提取的点云配准方法 |
CN110276834A (zh) * | 2019-06-25 | 2019-09-24 | 达闼科技(北京)有限公司 | 一种激光点云地图的构建方法、终端和可读存储介质 |
Cited By (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112070835A (zh) * | 2020-08-21 | 2020-12-11 | 达闼机器人有限公司 | 机械臂位姿预测方法、装置、存储介质及电子设备 |
CN114274139A (zh) * | 2020-09-27 | 2022-04-05 | 西门子股份公司 | 自动喷涂方法、装置、系统和存储介质 |
CN114274139B (zh) * | 2020-09-27 | 2024-04-19 | 西门子股份公司 | 自动喷涂方法、装置、系统和存储介质 |
CN112629415A (zh) * | 2020-12-29 | 2021-04-09 | 合肥达朴汇联科技有限公司 | 一种粮仓粮食体积测量系统 |
CN113436235A (zh) * | 2021-05-25 | 2021-09-24 | 北京理工大学 | 一种激光雷达与视觉点云的初始化自动配准方法 |
CN113436235B (zh) * | 2021-05-25 | 2022-07-01 | 北京理工大学 | 一种激光雷达与视觉点云的初始化自动配准方法 |
CN115797573A (zh) * | 2023-02-09 | 2023-03-14 | 四川省公路规划勘察设计研究院有限公司 | 一种衡量点云孪生几何准确度的方法、装置和介质 |
Also Published As
Publication number | Publication date |
---|---|
CN111508008B (zh) | 2023-07-14 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111508008B (zh) | 一种点云配准的方法、电子设备和存储介质 | |
CN107862719B (zh) | 相机外参的标定方法、装置、计算机设备和存储介质 | |
CN113066105A (zh) | 激光雷达和惯性测量单元融合的定位与建图方法及系统 | |
Sweeney et al. | Solving for relative pose with a partially known rotation is a quadratic eigenvalue problem | |
CN108896047B (zh) | 分布式传感器网络协同融合与传感器位置修正方法 | |
CN114862932B (zh) | 基于bim全局定位的位姿修正方法及运动畸变矫正方法 | |
CN112907735B (zh) | 一种基于点云的柔性电缆识别与三维重建方法 | |
CN105004337B (zh) | 基于直线匹配的农用无人机自主导航方法 | |
CN104167001B (zh) | 基于正交补偿的大视场摄像机标定方法 | |
US20240094343A1 (en) | Method, device, system, and storage medium for tracking moving target | |
CN113295171B (zh) | 一种基于单目视觉的旋转刚体航天器姿态估计方法 | |
CN110688440A (zh) | 一种适用于子地图重叠部分较少的地图融合方法 | |
CN112446952B (zh) | 三维点云法向量的生成方法、装置、电子设备及存储介质 | |
Wang et al. | Multi-robot raster map fusion without initial relative position | |
Rigatos | Distributed particle filtering over sensor networks for autonomous navigation of UAVs | |
Zhang et al. | Self-calibration of multiple LiDARs for autonomous vehicles | |
CN108595373B (zh) | 一种无控制dem配准方法 | |
CN114742141B (zh) | 基于icp点云的多源信息数据融合研判方法 | |
CN115424046A (zh) | 无人机群的航迹追踪方法、装置、设备和介质 | |
CN111504276B (zh) | 一种基于视觉投影尺度因子集的联合目标函数多推进器姿态角获取方法 | |
CN109919998B (zh) | 卫星姿态确定方法、装置和终端设备 | |
CN108764161B (zh) | 基于极坐标系的破解稀疏阵引发的病态奇异性的遥感影像处理方法和装置 | |
Li et al. | Industrial Robot Hand–Eye Calibration Combining Data Augmentation and Actor-Critic Network | |
Yu et al. | Small UAV based multi-viewpoint image registration for extracting the information of cultivated land in the hills and mountains | |
CN113220023B (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 | ||
CB02 | Change of applicant information |
Address after: 200245 Building 8, No. 207, Zhongqing Road, Minhang District, Shanghai Applicant after: Dayu robot Co.,Ltd. Address before: 201100 2nd floor, building 2, No. 1508, Kunyang Road, Minhang District, Shanghai Applicant before: Dalu Robot Co.,Ltd. |
|
CB02 | Change of applicant information | ||
GR01 | Patent grant | ||
GR01 | Patent grant |