前言

在之前的文章中（)，我們介紹了ICP的基本思想與詳細(xì)的推導(dǎo)。本文將介紹ICP方法的兩種改進，分別是：PLICP[1]與NICP[2]。本文將分別介紹兩種改進的基本思想，具體算法以及一些補充說明。若有理解不到位和錯誤之處，請以論文原文為準(zhǔn)。 第一部分 PLICP

一、基本思想

PLICP中的“PL”表示”Point to Line”，顧名思義，在匹配時是一個點與一個直線進行匹配，而不是傳統(tǒng)方法的點與點進行匹配。之所以有這種思想，是因為我們認(rèn)為每次掃描的數(shù)據(jù)是對真實物理世界的一個平面的采樣，所以我們在匹配時應(yīng)該盡可能與這個直線去匹配而不是具體的采樣點。

（左圖棕色曲線表示真實的物理面，藍(lán)色的為帶有噪聲的采樣點；中間表示傳統(tǒng)ICP的點點距離，右圖表示PLICP方法，匹配時是計算到平面的距離）

二、 算法描述

2.1 利用上一次迭代的變化參數(shù)（或初值），對當(dāng)前采樣（curr）的每個點進行變化； 2.2 變換后，尋找每個點在參考點云（ref）中的最近鄰的兩個點 2.3 使用論文[3]中提到的方法，去除離群點； 2.4 構(gòu)建目標(biāo)函數(shù)：

三、補充說明

3.1 論文[3]介紹了一種截斷剔除離群點的方法，具體而言，在完成兩組點云的匹配后，計算每組匹配點的歐氏距離，只保留距離最小的一定百分比的匹配，從而對噪聲魯棒； 3.2 對算法中的目標(biāo)函數(shù)進一步解釋：可以看出點最近直線的距離，采用了投影的思想，法向量點乘即為在法向量上的投影； 3.3 算法中對目標(biāo)函數(shù)求解最佳變換參數(shù)的方法有很多，論文給出了一種閉式解的方法，具體請參考論文[1]的附錄，這里不再展開。 3.4 PLICP相比于ICP而言，收斂速度更快（論文證明，ICP是一階收斂，而PLICP是二階收斂）。但更容易陷入局部極值，故一般使用時，多采用全局ICP方法（例如論文采用了GPM[4]）進行粗匹配，然后在使用PLICP進行精確計算。 3.5 作者給出了PLCIP方法的C語言實現(xiàn)：https://censi.science/software/csm/ 第二部分NICP

一、基本思想

NICP的基本思想是，curr和ref的兩個點在匹配時，不僅要距離接近，而且所在處的法向量方向也要相同。在匹配時，根據(jù)距離、曲率以及法向量進行篩選，并在優(yōu)化變換參數(shù)時優(yōu)化增加了法向量的參數(shù)。

（左側(cè)圖片表示采樣點的曲率，越大的區(qū)域為紅色；右圖表示匹配，綠色和藍(lán)色為兩次掃描，紫紅色線表示匹配的點，可以看出，右上角部分雖然藍(lán)色和綠色的點在距離上重合，但由于法向量不同，并不會建立匹配關(guān)系）

二、算法描述

2.1 法向量計算方法

2.2 匹配原則

在進行匹配時，不同于傳統(tǒng)ICP，距離大于一定閾值時剔除，NICP采用3個準(zhǔn)則剔除錯誤匹配，分別是：1. 點距離超過閾值；2. 曲率接近；3. 法向量方向接近。 2.3 目標(biāo)函數(shù)

2.4 優(yōu)化求解

優(yōu)化求解可以采用任何優(yōu)化求解方法。論文采用了LM算法。

三、補充說明

1）NICP采用法向量進行擴充，包含了一定的語義成分； 2）對于協(xié)方差矩陣意義的個人理解 協(xié)方差矩陣求逆獲得了信息矩陣，在對匹配點誤差進行加權(quán)時，對不同方向上的誤差進行了不同權(quán)重的約束。例如，如果某個點在一個平面上，那么對應(yīng)的協(xié)方差矩陣的特征值最小值代表了法向量上的“厚度”，最小值越小，表示越接近于平面，那么在信息矩陣中對應(yīng)的位置權(quán)重越大，放大了ICP時在法向量方向上的誤差，避免在法向量方向上產(chǎn)生嚴(yán)重的“錯位”。

參考文獻(xiàn)

[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.

編輯：黃飛