ICP方法的两种改进算法：PLICP与NICP

前言

在之前的文章中（)，我们介绍了ICP的基本思想与详细的推导。本文将介绍ICP方法的两种改进，分别是：PLICP[1]与NICP[2]。本文将分别介绍两种改进的基本思想，具体算法以及一些补充说明。若有理解不到位和错误之处，请以论文原文为准。 第一部分 PLICP

一、基本思想

PLICP中的“PL”表示”Point to Line”，顾名思义，在匹配时是一个点与一个直线进行匹配，而不是传统方法的点与点进行匹配。之所以有这种思想，是因为我们认为每次扫描的数据是对真实物理世界的一个平面的采样，所以我们在匹配时应该尽可能与这个直线去匹配而不是具体的采样点。

（左图棕色曲线表示真实的物理面，蓝色的为带有噪声的采样点；中间表示传统ICP的点点距离，右图表示PLICP方法，匹配时是计算到平面的距离）

二、 算法描述

2.1 利用上一次迭代的变化参数（或初值），对当前采样（curr）的每个点进行变化； 2.2 变换后，寻找每个点在参考点云（ref）中的最近邻的两个点 2.3 使用论文[3]中提到的方法，去除离群点； 2.4 构建目标函数：

三、补充说明

3.1 论文[3]介绍了一种截断剔除离群点的方法，具体而言，在完成两组点云的匹配后，计算每组匹配点的欧氏距离，只保留距离最小的一定百分比的匹配，从而对噪声鲁棒； 3.2 对算法中的目标函数进一步解释：可以看出点最近直线的距离，采用了投影的思想，法向量点乘即为在法向量上的投影； 3.3 算法中对目标函数求解最佳变换参数的方法有很多，论文给出了一种闭式解的方法，具体请参考论文[1]的附录，这里不再展开。 3.4 PLICP相比于ICP而言，收敛速度更快（论文证明，ICP是一阶收敛，而PLICP是二阶收敛）。但更容易陷入局部极值，故一般使用时，多采用全局ICP方法（例如论文采用了GPM[4]）进行粗匹配，然后在使用PLICP进行精确计算。 3.5 作者给出了PLCIP方法的C语言实现：https://censi.science/software/csm/ 第二部分NICP

一、基本思想

NICP的基本思想是，curr和ref的两个点在匹配时，不仅要距离接近，而且所在处的法向量方向也要相同。在匹配时，根据距离、曲率以及法向量进行筛选，并在优化变换参数时优化增加了法向量的参数。

（左侧图片表示采样点的曲率，越大的区域为红色；右图表示匹配，绿色和蓝色为两次扫描，紫红色线表示匹配的点，可以看出，右上角部分虽然蓝色和绿色的点在距离上重合，但由于法向量不同，并不会建立匹配关系）

二、算法描述

2.1 法向量计算方法

2.2 匹配原则

在进行匹配时，不同于传统ICP，距离大于一定阈值时剔除，NICP采用3个准则剔除错误匹配，分别是：1. 点距离超过阈值；2. 曲率接近；3. 法向量方向接近。 2.3 目标函数

2.4 优化求解

优化求解可以采用任何优化求解方法。论文采用了LM算法。

三、补充说明

1）NICP采用法向量进行扩充，包含了一定的语义成分； 2）对于协方差矩阵意义的个人理解 协方差矩阵求逆获得了信息矩阵，在对匹配点误差进行加权时，对不同方向上的误差进行了不同权重的约束。例如，如果某个点在一个平面上，那么对应的协方差矩阵的特征值最小值代表了法向量上的“厚度”，最小值越小，表示越接近于平面，那么在信息矩阵中对应的位置权重越大，放大了ICP时在法向量方向上的误差，避免在法向量方向上产生严重的“错位”。 

参考文献

[1]. A. Censi, "An ICP variant using a point-to-line metric," 2008 IEEE International Conference on Robotics and Automation, Pasadena, CA, 2008, pp. 19-25, doi: 10.1109/ROBOT.2008.4543181.

[2] J. Serafin and G. Grisetti, "NICP: Dense normal based point cloud registration," 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Hamburg, 2015, pp. 742-749, doi: 10.1109/IROS.2015.7353455.

[3]. D. Chetverikov, D. Svirko, D. Stepanov and P. Krsek, "The Trimmed Iterative Closest Point algorithm," Object recognition supported by user interaction for service robots, Quebec City, Quebec, Canada, 2002, pp. 545-548 vol.3, doi: 10.1109/ICPR.2002.1047997.

[4]. A. Censi, "Scan matching in a probabilistic framework," Proceedings 2006 IEEE International Conference on Robotics and Automation, 2006. ICRA 2006., Orlando, FL, 2006, pp. 2291-2296, doi: 10.1109/ROBOT.2006.1642044.

编辑：黄飞