前言

在之前的文章中（)，我們介紹了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.

編輯：黃飛