Point Cloud Coarse Registration Algorithm Based on Two-Stage Coordinate Transformation
1 引言 近年来,随着三维扫描系统的飞速发展,三维重建已经广泛应用于医疗[1 ] 、工业[2 ] 、文物修复[3 -4 ] 、逆向工程[5 ] 等各个领域。由于受到扫描设备的限制和实际扫描环境的影响,得到的物体的点云数据一般都存在不完整、有噪声的情况,因此还需经历滤波[6 ] 、分割[7 ] 、配准等过程。而配准就是通过刚体变换对多个点云进行对齐,来完整扫描物体点云数据的过程。而如何快速、高效地寻找到最优的旋转平移矩阵,就成为了三维扫描系统的关键所在。
当今在国内外比较流行的点云配准算法是Besl等[8 ] 提出的迭代最近点(ICP)算法。该算法通过计算欧氏距离来求取两个点云之间的对应点对,并基于对应点构造旋转平移矩阵,估计变换后的源点云与目标点云的误差函数,反复迭代,直到误差小于所设阈值。ICP算法原理简单且拥有比较好的精度,但采用了迭代计算,导致算法计算速度较慢,而且极度依赖两个点云的初始位置,目标函数可能陷入局部最优。为了避免这种情况,一般将ICP搭配其他算法共同完成点云配准。实现点云粗配准的重要方法之一是采用特征进行匹配。Rusu等[9 -10 ] 提出了点特征直方图(PFH),通过计算点云和邻域点云之间的法向量等估计表面情况,并提出改进的快速点特征直方图(FPFH),进一步减少了计算特征点所耗费的时间。Tangelder等[11 ] 提出的三维形状上下文(3DSC)通过匹配向量的值来建立不同曲面点之间的对应关系,用于描述曲面上指定点及邻域的形状特征。江旭等[12 ] 提出了一种改进的基于T检验的迭代最近点(T-ICP)算法。李新春等[13 ] 提出一种基于邻域特征点提取和匹配的点云配准方法。张顺利等[14 ] 提出了一种基于自适应邻域匹配的点云配准方法。Ying等[15 ] 提出了基于ICP的尺度伸缩配准方法。汤慧等[16 ] 提出了一种基于扩展PFH特征的点云匹配算法。刘鸣等[17 ] 提出了一种利用独立成分分析(ICA)的点云配准方法。李宇翔等[18 ] 提出了一种基于内部形态描述子(ISS)及方向直方图描述子(SHOT)特征的点云配准算法。彭真等[19 ] 提出了一种基于关键点提取与最近点优化迭代的点云配准算法。詹旭等[20 ] 提出了一种基于余弦相似度的点云配准算法。王永波等[21 ] 提出了平面特征约束下基于四元数描述的LiDAR点云配准算法。
针对现有算法存在的精度或者时间问题,本文采用粗配准和精配准相结合的方式,分别解决时间和精度的问题。在粗配准上,提出了一种具有线性时间复杂度和常数空间复杂度的算法,大幅减少了粗配准阶段所需要的时间;再搭配ICP精配准算法完成配准,从而保证了配准的精度。
2 点云配准算法 点云配准的实质是找到一个从源点云P 到目标点云Q 的旋转平移矩阵,其中Q = R * P + t ,R 为旋转矩阵,t 为平移向量。从空间的竖直方向和水平方向入手,分两阶段寻找两点云间的变换关系,快速完成粗配准过程,再搭配ICP算法完成配准,从而在时间和精度上都取得较好的结果。
2.1 竖直方向对齐 首先定义源点云P = p 1 , p 2 , p 3 , . . . , p n ,其中p i = x i y i z i T , i = 1,2 , 3 , . . . , n ,目标点云Q = q 1 , q 2 , q 3 , . . . , q m ,其中q j = x j y j z j T , j = 1,2 , 3 , . . . , m 。竖直方向的配准过程如图1 所示,具体算法步骤如下。
图 1. 竖直方向配准过程Fig. 1. Vertical registration process 下载图片 查看所有图片
1)计算点云P 和点云Q 的质心,分别记为c P 和c Q ,表达式分别为
c P = 1 n ∑ i = 1 n p i ,
(1) c Q = 1 m ∑ j = 1 m q j 。
(2) 2)将点云P 和点云Q 去中心化:
p i = p i - c P ,
(3) q j = q j - c Q 。
(4) 3)在点云P 和点云Q 中分别寻找距离原点最远的点p d 、q d 。其中p d 、q d 两点距原点的欧氏距离分别为D P 、D Q :
D P = m a x x i 2 + y i 2 + z i 2 ,
(5) D Q = m a x x j 2 + y j 2 + z j 2 。
(6) 4)对于点云P ,在x O z 平面内寻找垂直于向量n O p d 的向量p ,对于点云Q ,在x O z 平面内寻找垂直于向量n O q d 的向量q 。设p = a p 0 c p T ,q = a q 0 c q T ,则有
n O p d ⋅ p = 0 ,
(7) n O q d ⋅ q = 0 。
(8) 5)计算向量n O p d 和y 轴的夹角α ,计算向量n O q d 和y 轴的夹角β 。设e = 0 1 0 T 为y 轴的一个单位向量,可得
c o s α = n O p d ⋅ e n O p d × e ,
(9) c o s β = n O q d ⋅ e n O q d × e 。
(10) 6)以向量p 为旋转轴,α 为旋转角度,求出旋转矩阵R P ,以向量q 为旋转轴,β 为旋转角度,求出旋转矩阵R Q ,
R P = c o s α + a p 2 ( 1 - c o s α ) - c p s i n α a p c p ( 1 - c o s α ) c p s i n α c o s α - a p s i n α a p c p ( 1 - c o s α ) a p s i n α c o s α + c p 2 ( 1 - c o s α ) ,
(11) R Q = c o s β + a q 2 ( 1 - c o s β ) - c q s i n β a q c q ( 1 - c o s β ) c q s i n β c o s β - a q s i n β a q c q ( 1 - c o s β ) a q s i n β c o s β + c q 2 ( 1 - c o s β ) 。
(12) 7)将点云P 、Q 按旋转矩阵R P 、R Q 旋转,把向量n O p d 、向量n O q d 与y 轴重合,完成竖直方向上的对齐,并形成新点云P y 、Q y 。
2.2 水平方向对齐 再按照图2 所示的过程完成水平方向配准。
图 2. 水平方向配准过程Fig. 2. Horizontal registration process 下载图片 查看所有图片
具体算法步骤如下。
1)将点云P y 、Q y 里的每个点的y 坐标置0,在x O z 平面上形成新点云P y 0 、Q y 0 。
2)在点云P y 0 、Q y 0 内分别寻找距离原点欧氏距离最远的点p d 0 、q d 0 。
3)计算向量n O p d 0 、n O q d 0 的夹角γ ,表达式为
c o s γ = n O p d 0 ⋅ n O q d 0 n O p d 0 × n O q d 0 。
(13) 4)以y 轴为旋转轴,夹角γ 为旋转角,计算旋转矩阵R x O z :
R x O z = c o s γ 0 - s i n γ 0 1 0 s i n γ 0 c o s γ 。
(14) 5)最后根据矩阵R P 、R Q 和R x O z ,可以得到最终旋转矩阵R :
R = R Q - 1 * R x O z * R P 。
(15)
2.3 ICP算法精配准 ICP算法是一种基于最小二乘法思想的算法,通过在约束条件下进行迭代,得到满足精度要求的结果。ICP算法对初始位置要求较高,否则容易陷入局部最优,因此将旋转矩阵R 作为初始旋转矩阵,赋值给R I C P 进行第一次计算,然后开始迭代。
ICP算法步骤如下。
1)在源点云P 中取点集p k ∈ P 。
2)在目标点云Q 中取点集q k ∈ Q ,使得满足q k - p k = l m i n 。
3)根据对应点集计算出旋转矩阵R I C P 和三维平移向量t 。
4)对源点集p k 使用步骤3)的R I C P 和t ,计算出新的源点集p k ' = R I C P * p k + t 。
5)使用误差函数
E R I C P , t = 1 L ∑ l = 1 L q l - R I C P * p l + t 。
(16) 6)如果误差小于设定阈值e 或者迭代次数大于设定N ,迭代结束,否则将源点集更新为p k ' ,重复步骤2)~6),直到满足收敛条件。
3 实验与分析 为了保证实验的可靠性,分别采用了斯坦福大学提供的Bunny点云和Horse点云等公共点云,以及如图3 所示的实验装置测量的Cochlea私有点云。Bunny点云、Horse点云和Cochlea点云的数量分别为33692、48485和29067。
图 3. 点云测量实验装置Fig. 3. Experimental setup for point cloud measurement 下载图片 查看所有图片
为了对比算法的耗时和精度,除了所提算法外,还采用了传统的PFH算法、FPFH算法和3DSC算法进行了对比。另外为了体现所提算法的适用性,加入了一组不同角度采集的单面点云进行验证。
3.1 Bunny兔子点云配准 首先采用4种算法对斯坦福大学提供的公共点云库中的Bunny兔子点云进行配准实验,配准结果如图4 所示。实验数据如表1 所示,对于Bunny点云,PFH算法的均方根误差(RMSE)为7.239×10-7 mm,用时34.796 s,比FPFH算法精度略高,但FPFH算法用时13.162 s,仅为PFH的1/3;3DSC算法配准精度最高,均方根误差为9.741×10-9 mm,但用时也最长;所提算法的均方根误差为1.897×10-8 mm,和3DSC算法相差不多,但用时4.226 s,仅为3DSC算法的1/12。
图 4. Bunny点云实验结果Fig. 4. Experiment results of Bunny point cloud 下载图片 查看所有图片
表 1. 不同算法的Bunny点云配准对比Table 1. Comparison of Bunny point cloud registration of different algorithmsPoint cloud Algorithm Time /s RMSE /mm Bunny Proposed algorithm 4.226 1.897×10-8 PFH algorithm 34.796 7.239×10-7 FPFH algorithm 13.162 1.443×10-6 3DSC algorithm 51.587 9.741×10-9
查看所有表
3.2 Horse点云配准 采用4种算法对斯坦福大学提供的公共点云库中的Horse点云进行配准实验,配准结果如图5 所示。实验数据如表2 所示,同Bunny点云相比,因为Horse点云模型本身较为平滑,因此4种算法的RMSE均略小于Bunny点云的结果;同时因为点云数量较多,比较耗时。4种算法对Horse点云的结果与对Bunny点云的结果相似,但随着点云数量的增多,3DSC算法耗时增加了51.113 s,PFH算法和FPFH算法也增加了10 s左右,而所提算法仅增加了0.03 s。
图 5. Horse点云实验结果Fig. 5. Experiment results of Horse point cloud 下载图片 查看所有图片
表 2. 不同算法的Horse点云配准对比Table 2. Comparison of Horse point cloud registration of different algorithmsPoint cloud Algorithm Time /s RMSE /mm Horse Proposed algorithm 4.256 9.296×10-9 PFH algorithm 44.976 1.239×10-7 FPFH algorithm 22.556 6.443×10-7 3DSC algorithm 102.7 2.392×10-10
查看所有表
3.3 Cochlea耳蜗点云配准 采用4种算法对实验室采集的Cochlea点云进行配准实验,配准结果如图6 所示。实验数据如表3 所示,同Bunny点云相比,因为实验室所测得的Cochlea点云和公共点云的尺度不同,因此所得到的RMSE有了较大的变化,所耗时间也均有不同程度的变化。PFH算法和FPFH算法的RMSE远大于所提算法和3DSC算法;而对于处理时间,所提算法虽有所增加,但依旧只有3DSC算法的1/10。
图 6. Cochlea点云实验结果Fig. 6. Experiment results of Cochlea point cloud 下载图片 查看所有图片
表 3. 不同算法的Cochlea点云配准对比Table 3. Comparison of Cochlea point cloud registration of different algorithmsPoint cloud Algorithm Time /s RMSE /mm Cochlea Proposed algorithm 5.38 2.457×10-9 PFH algorithm 23.812 1.744×10-2 FPFH algorithm 19.987 3.806×10-2 3DSC algorithm 52.189 4.744×10-9
查看所有表
3.4 实验结果分析 通过对三组点云的实验,得到了不同算法的配准时间和均方根误差,如图7 所示。
图 7. 不同算法的配准结果。(a)时间;(b)RMSEFig. 7. Registration results of different algorithms. (a) Time; (b) RMSE 下载图片 查看所有图片
Bunny点云的数量为33692,Horse点云的数量为48485,数量差别很大,如图7 (a)所示,可以清楚地看到:随着点的数量增多,PFH和FPFH所耗时间均增大;而3DSC算法的耗时增加更为显著;所提算法基于线性的时间复杂度和常数空间复杂度,耗时增加很少,因此对于数量越多的点云,所提算法优势越为明显。Bunny点云和Cochlea点云的数量相似,但由于采用不同设备采集点云数据,因此点云的尺度不同。如图7 (b)所示,当点云尺度有了较大变化后,PFH和FPFH的均方根误差变化很大,而所提算法和3DSC算法变化很小,依旧保持着较高的精度。但相对于所提算法,3DSC算法的配准速度又严重依赖点云的数量。因此,从配准耗时和RMSE的变化综合来看,所提算法具有良好的性能。
3.5 普适性验证实验 以上实验均采用了完整模型进行配准,为了验证所提算法的普适性和可行性,进行斯坦福大学公共点云库中Bunny点云的不同角度的单面点云实验。实验采用Bunny点云0°方向的点云S 和45°方向的点云T ,如图8 (a)所示。
图 8. 点云S 和点云T Fig. 8. Point cloud S and point cloud T 下载图片 查看所有图片
首先分别寻找点云S 和点云T 的特征点s d 和t d ,由于两个点云是物体在不同视角下采集的,因此点的数量和点云的质心都不同。由图8 (b)和图8 (c)也可以清楚地看到,特征点不是同一个点,但是两个特征点都在兔子的右耳朵上,相对位置相差不大。
首先按照所提算法步骤对点云S 和点云T 重设y 轴,完成竖直方向上的对齐,如图9 (a)所示,得到点云Sy 和点云Ty ;再将y 坐标置为0,如图9 (b)和图9 (c)所示,得到点云Sy 0 和Ty 0 。从图9 可以清晰地看出,对于水平方向上的两幅点云,形状不完全相同,但非常相似。这说明第一步在竖直方向上的对齐是有效的,没有出现投影在xOz 平面上后差异很大的情况。另外从点云Sy 0 和Ty 0 所得到的特征点s d0 和t d0 也不同,但都在兔子的尾巴上,相对位置也相差不大。
图 9. 粗配准过程Fig. 9. Coarse registration process 下载图片 查看所有图片
最后再经过水平方向的对齐后,综合两阶段得到最终的旋转矩阵。粗配准结果如图10 (a)所示,可以看到点云S 和点云T 已经在很大程度上完成了配准,而此粗配准过程仅花费0.636 s。最后再搭配ICP精配准算法完成最终的配准,避免了ICP算法陷入局部最优与迭代时间长的缺陷。通过实验,证明了所提算法对不同视角得到的单面点云数据的适用性。
图 10. 配准结果Fig. 10. Registration result 下载图片 查看所有图片
4 结论 采用粗配准和精配准相结合的方式,分别保证了配准的时间和精度。ICP精配准算法精度高,迭代计算慢,可能陷入局部最优。因此,从空间的竖直方向和水平方向入手,提出一种分两阶段寻找两点云间的变换关系的算法。该算法具有线性时间复杂度和常数空间复杂度,大幅减少了粗配准阶段所需要耗费的时间,从而快速完成粗配准过程。
通过采用三组有代表性的点云数据,实验证明了所提算法的强鲁棒性,无论改变点的数量还是改变点云的尺度,在时间和精度上均有所保证。通过补充实验,证明了所提算法对不同角度采集的部分点云也可较好地完成粗配准。但所提算法对配准对象有一定要求,即要求点云的特征点处在相邻位置,例如对于Bunny点云,耳朵是较为突出、细长的部分,当各视角的点云里都有耳朵,即可保证特征点都处在耳朵上,因此在获得点云模型时,需将物体突出、细长的部分作为公共区域进行点云获取;如果点云模型为球型、柱形等规则物体,没有较突出、细长部位,可以对物体加一个辅助杆,使其拥有一个细长、突出的公共部分,在完成配准后再通过将辅助杆删除的方法来解决。接下来则要对特征点的选择进行进一步深入研究,使所提算法可以适用于更多对象。
参考文献
[1] Jiang W, Xu K, Cheng Z Q, et al. Skeleton-based intrinsic symmetry detection on point clouds[J]. Graphical Models, 2013, 75(4): 177-188 .
[2] 朱宁宁. 三维激光扫描在地铁隧道形变监测中的应用[J]. 测绘工程, 2015, 24(5): 63-68 .
Zhu N N. Application of 3D laser scanning to the subway tunnel deformation monitoring[J]. Engineering of Surveying and Mapping, 2015, 24(5): 63-68 .
[3] 杨稳, 周明全, 郭宝, 等. 基于曲率图的颅骨点云配准方法[J]. 光学学报, 2020, 40(16): 1610002 .
Yang W, Zhou M Q, Guo B, et al. Skull point cloud registration method based on curvature maps[J]. Acta Optica Sinica, 2020, 40(16): 1610002 .
[4] 邱兆文, 张田文. 文物三维重建关键技术[J]. 电子学报, 2008, 36(12): 2423-2427 .
Qiu Z W, Zhang T W. Key techniques on cultural relic 3D reconstruction[J]. Acta Electronica Sinica, 2008, 36(12): 2423-2427 .
[5] 张德海, 崔国英, 白代萍, 等. 逆向工程中的三维光学检测点云采样技术研究[J]. 计算机应用研究, 2014, 31(3): 946-948 .
Zhang D H, Cui G Y, Bai D P, et al. Point cloud simplification technology of 3D optical measurement applied on reverse engineering[J]. Application Research of Computers, 2014, 31(3): 946-948 .
[6] 李绕波, 袁希平, 甘淑, 等. 一种基于对偶四元素描述的线面特征约束的点云配准方法[J]. 光学学报, 2022, 42(2): 0214003 .
Li R B, Yuan X P, Gan S, et al. Point cloud registration method based on dual quaternion description of line-planar feature constraints[J]. Acta Optica Sinica, 2022, 42(2): 0214003 .
[7] 赵梦娜, 花向红, 冯绍权, 等. 基于点云切片的建筑物门窗信息提取[J]. 中国激光, 2020, 47(6): 0604002 .
Zhao M N, Hua X H, Feng S Q, et al. Information extraction of buildings, doors, and windows based on point cloud slices[J]. Chinese Journal of Lasers, 2020, 47(6): 0604002 .
[8] Besl P J, McKay N D. A method for registration of 3-D shapes[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 1992, 14(2): 239-256 .
[9] RusuR B, BlodowN, MartonZ C, et al. Aligning point cloud views using persistent feature histograms[C]∥2008 IEEE/RSJ International Conference on Intelligent Robots and Systems, September 22-26, 2008, Nice, France. New York: IEEE Press, 2008: 3384-3391. 10.1109/iros.2008.4650967
[10] RusuR B, BlodowN, BeetzM. Fast point feature histograms (FPFH) for 3D registration[C]∥2009 IEEE International Conference on Robotics and Automation, May 12-17, 2009, Kobe, Japan. New York: IEEE Press, 2009: 3212-3217. 10.1109/robot.2009.5152473
[11] Tangelder J W H, Veltkamp R C. A survey of content based 3D shape retrieval methods[J]. Multimedia Tools and Applications, 2007, 39(3): 441-471 .
[12] 江旭, 耿楠, 张志毅, 等. 基于假设检验匹配约束的点云配准算法研究[J]. 计算机应用研究, 2021, 38(1): 305-310 .
Jiang X, Geng N, Zhang Z Y, et al. Research of point cloud registration algorithm based on hypothesis test matching constraints[J]. Application Research of Computers, 2021, 38(1): 305-310 .
[13] 李新春, 闫振宇, 林森, 等. 基于邻域特征点提取和匹配的点云配准[J]. 光子学报, 2020, 49(4): 0415001 .
Li X C, Yan Z Y, Lin S, et al. Point cloud registration based on neighborhood characteristic point extraction and matching[J]. Acta Photonica Sinica, 2020, 49(4): 0415001 .
[14] 张顺利, 徐艳芝, 周明全, 等. 基于自适应邻域匹配的点云配准方法[J]. 计算机学报, 2019, 42(9): 2114-2126 .
Zhang S L, Xu Y Z, Zhou M Q, et al. Registration of point clouds based on matching of general adaptive neighborhood[J]. Chinese Journal of Computers, 2019, 42(9): 2114-2126 .
[15] Ying S H, Peng J G, Du S Y, et al. A scale stretch method based on ICP for 3D data registration[J]. IEEE Transactions on Automation Science and Engineering, 2009, 6(3): 559-565 .
[16] 汤慧, 周明全, 耿国华. 一种基于扩展的PFH特征的点云匹配算法[J]. 激光与光电子学进展, 2019, 56(24): 241503 .
Tang H, Zhou M Q, Geng G H. A point cloud registration algorithm based on extended PFH feature[J]. Laser & Optoelectronics Progress, 2019, 56(24): 241503 .
[17] 刘鸣, 舒勤, 杨赟秀, 等. 基于独立成分分析的三维点云配准算法[J]. 激光与光电子学进展, 2019, 56(1): 011203 .
Liu M, Shu Q, Yang Y X, et al. Three-dimensional point cloud registration based on independent component analysis[J]. Laser & Optoelectronics Progress, 2019, 56(1): 011203 .
[18] 李宇翔, 郭际明, 潘尚毅, 等. 一种基于ISS-SHOT特征的点云配准算法[J]. 测绘通报, 2020(4): 21-26 .
Li Y X, Guo J M, Pan S Y, et al. A point cloud registration algorithm based on ISS-SHOT features[J]. Bulletin of Surveying and Mapping, 2020(4): 21-26 .
[19] 彭真, 吕远健, 渠超, 等. 基于关键点提取与优化迭代最近点的点云配准[J]. 激光与光电子学进展, 2020, 57(6): 061002 .
Peng Z, Lü Y J, Qu C, et al. Accurate registration of 3D point clouds based on keypoint extraction and improved iterative closest point algorithm[J]. Laser & Optoelectronics Progress, 2020, 57(6): 061002 .
[20] 詹旭, 蔡勇. 基于余弦相似度的点云配准算法[J]. 激光与光电子学进展, 2020, 57(12): 121503 .
Zhan X, Cai Y. Point cloud registration algorithm based on cosine similarity[J]. Laser & Optoelectronics Progress, 2020, 57(12): 121503 .
[21] 王永波, 郑南山, 卞正富. 平面特征约束下基于四元数描述的LiDAR点云配准算法[J]. 光学学报, 2020, 40(23): 2310001 .
Wang Y B, Zheng N S, Bian Z F. Planar feature-constrained, quaternion-based registration algorithm for LiDAR point clouds[J]. Acta Optica Sinica, 2020, 40(23): 2310001 .
李思远, 刘瑾, 杨海马, 刘海珊. 分两阶段变换坐标的点云粗配准算法[J]. 激光与光电子学进展, 2022, 59(16): 1610005. Siyuan Li, Jin Liu, Haima Yang, Haishan Liu. Point Cloud Coarse Registration Algorithm Based on Two-Stage Coordinate Transformation[J]. Laser & Optoelectronics Progress, 2022, 59(16): 1610005.