CN104615880B - 一种三维激光雷达点云匹配的快速icp方法 - Google Patents
一种三维激光雷达点云匹配的快速icp方法 Download PDFInfo
- Publication number
- CN104615880B CN104615880B CN201510050356.0A CN201510050356A CN104615880B CN 104615880 B CN104615880 B CN 104615880B CN 201510050356 A CN201510050356 A CN 201510050356A CN 104615880 B CN104615880 B CN 104615880B
- Authority
- CN
- China
- Prior art keywords
- point
- point cloud
- laser radar
- row
- cloud data
- 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.)
- Expired - Fee Related
Links
- 238000000034 method Methods 0.000 title claims abstract description 35
- 238000009616 inductively coupled plasma Methods 0.000 title description 18
- 238000004364 calculation method Methods 0.000 claims description 3
- 238000000354 decomposition reaction Methods 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims description 3
- 239000011159 matrix material Substances 0.000 description 2
- 230000002159 abnormal effect Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000010191 image analysis Methods 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000007781 pre-processing Methods 0.000 description 1
Landscapes
- Image Processing (AREA)
- Optical Radar Systems And Details Thereof (AREA)
Abstract
本发明公开了一种三维激光雷达点云匹配的快速ICP方法,针对三维激光雷达原始点云数据的特点,在极坐标系下快速地计算两个点云之间的最近点方法。该方法提高了三维激光雷达点云配准的速度。使得三维激光雷达原始点云数据配准速度大幅度提高,即将时间复杂度从经典ICP方法的O(DNcNm)降为O(DNc),其中,Nc为当前点云数据个数,Nm为模型点集数据个数。
Description
技术领域
本发明涉及一种快速ICP方法,具体地说,涉及一种三维激光雷达点云匹配的快速ICP方法。
背景技术
三维激光雷达提供了环境(或物体)的三维深度信息,在机器人导航、物体重三维建、医学图像分析、地形测绘、文物保护等诸多应用中被广泛使用。
三维激光雷达提供的点云数据需要进行配准才能用于后续的环境建模、物体重建、地形构建等工作。目前,广为使用的三维激光雷达数据匹配方法是最近迭代算法(ICP,Iterative Close Point)及其改进方法。然而,ICP算法具有以下不足:(1)要求初始估计较为准确,(2)大量的点云数据使得ICP算法效率低,(3)ICP算法可能陷入局部最优解。
现有技术中提出了一种经典的迭代最近点(Iterative Closest Point,ICP)技术,可以对两个三维点集进行配准,迭代最近点(ICP)的主要缺点为:1)如果初始矩阵选择不当,可能导致陷入局部最优点;2)计算最近点的过程计算复杂度较大,为O(NcNm)(其中,Nc为当前点云数据个数,Nm为模型点集数据个数)。假设算法迭代次数为D,则算法复杂度为:O(DNcNm)。由于配准算法通常要被多次调用,而实际问题的数据点集规模也通常非常大,因此该方法难以获得实时结果。
现有技术中还提出了一种在极坐标系下配准二维激光雷达点云数据的方法。其基本思路是利用激光雷达原始扫描数据的结构信息,利用扫描投影的方法计算当前扫描在参考坐标系中的期望点云。该方法首先对原始点云数据进行适当的预处理以去除异常数据。此后,在初始配准估计的基础上,迭代地进行扫描投影(scan projection)、平移估计、旋转估计。该方法不能处理三维激光雷达点云数据。
发明内容
本发明的目的在于克服上述技术存在的缺陷,提供一种三维激光雷达点云匹配的快速ICP方法,使得三维激光雷达原始点云数据配准速度大幅度提高,即将时间复杂度从经典ICP方法的O(DNcNm)降为O(DNc)。该发明的核心是针对三维激光雷达原始点云数据的特点,在极坐标系下快速地计算两个点云之间的最近点方法,其中,Nc为当前点云数据个数,Nm为模型点集数据个数。
其具体技术方案为:
一种三维激光雷达点云匹配的快速ICP方法,包括以下步骤:
输入:激光雷达原始点云数据i∈[1,Km],j∈[1,Lm],表示模型点集;
激光雷达原始点云数据i∈[1,Kc],j∈[1,Lc],表示当前点集;
其中,表示Rm的第i行的第j列射线,表示Rc的第i行的第j列射线,Km为Rm的射线行数,Kc为Rc的射线行数,Lm为Rm的每行射线数,Lc为Rc的每行射线数,表示Rm的第i行第j列射线测量距离,表示Rc的第i行第j列射线测量距离,表示Rm的第i行射线的方向, 表示Rc的第i行射线的方向, 表示Rm的第j列射线的方向, 表示Rc的第j列射线的方向,分别表示行、列方向角度偏移,分别表示行、列方向角度分辨率;
输出:点云数据Rc相对于点云数据Rm的齐次变换T;
步骤1:计算模型点集Rm的平面直角坐标表示Pm
其中
步骤2:计算点集Rc的平面直角坐标表示Pc;
其中,
步骤3:初始化迭代次数k=0,T0=I4×4,误差阈值τ;
步骤4:计算中的点在Pm中的最近点及其距离i∈[1,Kc],j∈[1,Lc];其中表示中第(i,j)点在Pm中的最近点编号,其距离为
步骤5:利用奇异值分解(SVD,Singular Value Decomposition)方法计算配准及误差:
步骤6:应用配准,
步骤7:如果dk-1-dk<τ,则停止迭代,输出Tk,否则,k=k+1,转步骤4。
优选地,所述步骤4的具体过程如下:
(4.1)对于中的每一个点计算其方向
(4.2)计算方向在Pm中最近点编号及距离;
(4.3)根据距离阈值选择用于配准的点集
与现有技术相比,本发明的有益效果为:
本发明使得三维激光雷达原始点云数据配准速度大幅度提高,即将时间复杂度从经典ICP方法的O(DNcNm)降为O(DNc)。该发明的核心是针对三维激光雷达原始点云数据的特点,在极坐标系下快速地计算两个点云之间的最近点方法,其中,Nc为当前点云数据个数,Nm为模型点集数据个数。该方法提高了三维激光雷达点云配准的速度。
附图说明
图1是当前点集与模型点集的最近点匹配示意图;
图2是常规ICP方法的最近点与快速ICP方法的最近点比对结果图。
具体实施方式
为了使本发明实现的技术手段、创作特征、达成目的与功效易于明白了解,下面结合附图和具体实例进一步阐述本发明。
本发明应用于移动机器人三维位姿估计
Γt相对于Γt-1的位姿表示为xt=(xt,yt,zt,αt,βt,γt)T,其中xt,yt,zt分别表示Γt坐标系的原点在坐标系Γt-1中的坐标,γt,βt,αt分别表示Γt相对于Γt-1在X,Y,Z轴的旋转角度。
则平移向量为,
pt=(xt,yt,zt)T
旋转矩阵为,
从Γt-1到Γt的齐次变换T
输入:(1)t-1时刻激光雷达点云数据i∈[1,Kt-1],j∈[1,Lt-1],表示模型点集。
(2)t时刻激光雷达原始点云数据i∈[1,Kt],j∈[1,Lt],表示当前点集。
其中,表示Rt-1的第i行的第j列射线,表示Rt的第i行的第j列射线,Kt-1为Rt-1的射线行数,Kt为Rt的射线行数,Lt-1为Rt-1的每行射线数,Lt为Rt的每行射线数,表示Rt-1的第i行第j列射线测量距离,表示Rt的第i行第j列射线测量距离,表示Rt-1的第i行射线的方向, 表示Rt的第i行射线的方向,表示Rt-1的第j列射线的方向, 表示Rt的第j列射线的方向,分别表示行、列方向角度偏移,分别表示行、列方向角度分辨率。
输出:点云数据Rt相对于点云数据Rt-1的齐次变换T。
步骤1:计算模型点集Rt-1的平面直角坐标表示Pt-1
其中
步骤2:计算点集Rt的平面直角坐标表示Pt。
其中,
步骤3:初始化迭代次数k=0,T0=I4×4,误差阈值τ。
步骤4:计算中的点在Pt-1中的最近点及其距离i∈[1,Kt],j∈[1,Lt]。其中表示中第(i,j)点在Pt-1中的最近点编号,其距离为具体过程如下:
(4.1)对于中的每一个点计算其方向
(4.2)计算方向在Pt-1中最近点编号及距离。
(4.3)根据距离阈值选择用于配准的点集
步骤5:利用奇异值分解(SVD,Singular Value Decomposition)方法计算配准及误差:
步骤6:应用配准,
步骤7:如果dk-1-dk<τ,则停止迭代,输出Tk,否则,k=k+1,转第4步。
以上所述,仅为本发明最佳实施方式,任何熟悉本技术领域的技术人员在本发明披露的技术范围内,可显而易见地得到的技术方案的简单变化或等效替换均落入本发明的保护范围内。
Claims (1)
1.一种三维激光雷达点云匹配的快速ICP方法,其特征在于,包括以下步骤:
输入:激光雷达原始点云数据表示模型点集;
激光雷达原始点云数据表示当前点集;
其中,表示Rm的第i行的第j列射线,表示Rc的第i行的第j列射线,Km为Rm的射线行数,Kc为Rc的射线行数,Lm为Rm的每行射线数,Lc为Rc的每行射线数,表示Rm的第i行第j列射线测量距离,表示Rc的第i行第j列射线测量距离,表示Rm的第i行射线的方向, 表示Rc的第i行射线的方向, 表示Rm的第j列射线的方向, 表示Rc的第j列射线的方向, 分别表示行、列方向角度偏移,分别表示行、列方向角度分辨率;
输出:点云数据Rc相对于点云数据Rm的齐次变换T;
步骤1:计算模型点集Rm的平面直角坐标表示Pm
其中
步骤2:计算点集Rc的平面直角坐标表示Pc;
其中,
步骤3:初始化迭代次数k=0,T0=I4×4,误差阈值τ;
步骤4:计算中的点在Pm中的最近点及其距离其中表示中第(i,j)点在Pm中的最近点编号,其距离为具体过程如下:
(4.1)对于中的每一个点计算其方向
(4.2)计算方向在Pm中最近点编号及距离;
(4.3)根据距离阈值选择用于配准的点集
步骤5:利用奇异值分解方法计算配准及误差:
步骤6:应用配准,
步骤7:如果dk-1-dk<τ,则停止迭代,输出Tk,否则,k=k+1,转步骤4。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201510050356.0A CN104615880B (zh) | 2015-01-31 | 2015-01-31 | 一种三维激光雷达点云匹配的快速icp方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201510050356.0A CN104615880B (zh) | 2015-01-31 | 2015-01-31 | 一种三维激光雷达点云匹配的快速icp方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN104615880A CN104615880A (zh) | 2015-05-13 |
CN104615880B true CN104615880B (zh) | 2017-08-01 |
Family
ID=53150321
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201510050356.0A Expired - Fee Related CN104615880B (zh) | 2015-01-31 | 2015-01-31 | 一种三维激光雷达点云匹配的快速icp方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN104615880B (zh) |
Families Citing this family (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104914870B (zh) * | 2015-07-08 | 2017-06-16 | 中南大学 | 基于岭回归超限学习机的户外机器人局部路径规划方法 |
CN105787933B (zh) * | 2016-02-19 | 2018-11-30 | 武汉理工大学 | 基于多视角点云配准的岸线三维重建装置及方法 |
CN106600531B (zh) * | 2016-12-01 | 2020-04-14 | 深圳市维新登拓医疗科技有限公司 | 手持扫描仪、手持扫描仪点云拼接方法和装置 |
CN108225341B (zh) * | 2016-12-14 | 2021-06-18 | 法法汽车(中国)有限公司 | 车辆定位方法 |
CN107917710B (zh) * | 2017-11-08 | 2021-03-16 | 武汉大学 | 一种基于单线激光的室内实时定位与三维地图构建方法 |
CN108399643A (zh) * | 2018-03-15 | 2018-08-14 | 南京大学 | 一种激光雷达和相机间的外参标定***和方法 |
Citations (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102663237A (zh) * | 2012-03-21 | 2012-09-12 | 武汉大学 | 基于网格分块与移动最小二乘的点云数据全自动滤波方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP6286902B2 (ja) * | 2013-07-10 | 2018-03-07 | 株式会社Ihi | 解析装置及び流体解析手法 |
-
2015
- 2015-01-31 CN CN201510050356.0A patent/CN104615880B/zh not_active Expired - Fee Related
Patent Citations (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102663237A (zh) * | 2012-03-21 | 2012-09-12 | 武汉大学 | 基于网格分块与移动最小二乘的点云数据全自动滤波方法 |
Non-Patent Citations (2)
Title |
---|
A 3-D perceptual Method based on Laser Scanner for Mobile Robot;Zi-xing CAI等;《Robotics and Diomimetics,2005 IEEE International Conference on》;20050709;第658-663页 * |
基于激光雷达点云数据的配准方法研究;徐万鑫;《中国优秀硕士学位论文全文数据库》;20131215(第S2期);全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN104615880A (zh) | 2015-05-13 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN104615880B (zh) | 一种三维激光雷达点云匹配的快速icp方法 | |
CN105856230B (zh) | 一种可提高机器人位姿一致性的orb关键帧闭环检测slam方法 | |
Sharma | Comparative assessment of techniques for initial pose estimation using monocular vision | |
Garro et al. | Solving the pnp problem with anisotropic orthogonal procrustes analysis | |
CN105021124B (zh) | 一种基于深度图的平面零件三维位置和法向量计算方法 | |
EP3995988A1 (en) | Method and apparatus for establishing beacon map on basis of visual beacons | |
Li et al. | A 4-point algorithm for relative pose estimation of a calibrated camera with a known relative rotation angle | |
CN110570449B (zh) | 一种基于毫米波雷达与视觉slam的定位与建图方法 | |
WO2020211655A1 (zh) | 激光粗配准方法、装置、移动终端及存储介质 | |
CN101464134B (zh) | 一种空间目标三维位姿视觉测量方法 | |
CN107358629B (zh) | 一种基于目标识别的室内建图与定位方法 | |
JP6349418B2 (ja) | 高精度単眼移動によるオブジェクト位置特定 | |
CN104166989B (zh) | 一种用于二维激光雷达点云匹配的快速icp方法 | |
Yun et al. | Automated registration of multi-view point clouds using sphere targets | |
CN110021039A (zh) | 序列图像约束的多视角实物表面点云数据初始配准方法 | |
WO2022179094A1 (zh) | 车载激光雷达外参数联合标定方法、***、介质及设备 | |
CN105956074A (zh) | 邻近位姿融合引导的单幅图像场景六自由度定位方法 | |
CN103900504A (zh) | 纳米尺度下的实时三维视觉信息反馈方法 | |
Yin et al. | SLAM-based self-calibration of a binocular stereo vision rig in real-time | |
CN110930444B (zh) | 一种基于双边优化的点云匹配方法、介质、终端和装置 | |
CN110688440B (zh) | 一种适用于子地图重叠部分较少的地图融合方法 | |
Han et al. | DiLO: Direct light detection and ranging odometry based on spherical range images for autonomous driving | |
Dubbelman et al. | Efficient trajectory bending with applications to loop closure | |
Sibley et al. | A sliding window filter for incremental SLAM | |
Fang et al. | Propagation-based incremental triangulation for multiple views 3D reconstruction |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant | ||
CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20170801 Termination date: 20220131 |
|
CF01 | Termination of patent right due to non-payment of annual fee |