ICP方法的兩種改進(jìn)算法：PLICP與NICP

前言

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

一、基本思想

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

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

二、 算法描述

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

三、補(bǔ)充說明

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

一、基本思想

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

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

二、算法描述

2.1 法向量計(jì)算方法

2.2 匹配原則

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

2.4 優(yōu)化求解

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

三、補(bǔ)充說明

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

參考文獻(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.

編輯：黃飛