兩種ICP的改進算法：PLICP與NICP

作者：小L

編輯：3D視覺工坊

前言

在之前的文章中（ ICP方法詳細推導 )，我們介紹了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時在法向量方向上的誤差，避免在法向量方向上產生嚴重的“錯位”。

備注：在公眾號「3D視覺工坊」后台，回復「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.

本文僅做學術分享，如有侵權，請聯系刪文。

下載1

在「3D視覺工坊」公眾號后台回復：3D視覺， 即可下載 3D視覺相關資料干貨，涉及相機標定、三維重建、立體視覺、SLAM、深度學習、點雲后處理、多視圖幾何等方向。

下載2

在「3D視覺工坊」公眾號后台回復： 3D視覺github資源匯總 ，即可下載包括結構光、標定源碼、缺陷檢測源碼、深度估計與深度補全源碼、點雲處理相關源碼、立體匹配源碼、單目、雙目3D檢測、基於點雲的3D檢測、6D姿態估計匯總等。

下載3

在「3D視覺工坊」公眾號后台回復：相機標定，即可下載獨家相機標定學習課件與視頻網址； 后台回復：立體匹配 ， 即可下載獨家立體匹配學習課件與視頻網址。

重磅！3DCVer-學術論文寫作投稿 交流群已成立

掃碼添加小助手微信，可申請加入3D視覺工坊-學術論文寫作與投稿 微信交流群，旨在交流頂會、頂刊、SCI、EI等寫作與投稿事宜。

同時也可申請加入我們的細分方向交流群，目前主要有3D視覺、CV&深度學習、SLAM、三維重建、點雲后處理、自動駕駛、CV入門、三維測量、VR/AR、3D人臉識別、醫療影像、缺陷檢測、行人重識別、目標跟蹤、視覺產品落地、視覺競賽、車牌識別、硬件選型、學術交流、求職交流等微信群。

一定要備注：研究方向+學校/公司+昵稱，例如：”3D視覺 + 上海交大 + 靜靜“。請按照格式備注，可快速被通過且邀請進群。原創投稿也請聯系。

▲長按加微信群或投稿

▲長按關注公眾號

3D視覺從入門到精通知識星球：針對3D視覺領域的知識點匯總、入門進階學習路線、最新paper分享、疑問解答四個方面進行深耕，更有各類大廠的算法工程人員進行技術指導。與此同時，星球將聯合知名企業發布3D視覺相關算法開發崗位以及項目對接信息，打造成集技術與就業為一體的鐵桿粉絲聚集區，近2000星球成員為創造更好的AI世界共同進步，知識星球入口：

學習3D視覺核心技術，掃描查看介紹，3天內無條件退款

圈里有高質量教程資料、可答疑解惑、助你高效解決問題