點選上方“3D視覺工坊”，選擇“星标”
幹貨第一時間送達
作者：GeometryHub 連結：https://zhuanlan.zhihu.com/p/86042646 本文轉載自知乎，作者已授權，未經許可請勿二次轉載。
背景
兩個點雲要注冊在一塊，一般分兩個步驟：先做一個大緻的對齊，也就是所謂的初始注冊，一般可以通過一些可靠的點對來計算得到(如圖3所示)；然後在初始注冊的基礎上進行精細注冊，提升注冊的精度(如圖4所示)。精細注冊的方法，一般采用ICP算法，也就是最近點疊代的方法。
ICP算法總覽
下面先總的介紹一下ICP算法，之後再詳細介紹裡面的一些重要步驟。
算法輸入是兩片有部分重疊的點雲a和b，并且已經初始注冊好了，輸出是ICP注冊的剛體變換T：
1. 對b進行點采樣，得到采樣點集s
2. 在a中尋找采樣點集s的最近對應點，得到點對集合c
3. 對c中的點雲進行權重處理，并删掉一些不好的點對
4. 應用目标能量來優化點對距離，得到剛體變換Ti。Ti對采樣點集進行剛體變換
5. 疊代步驟2-4，直到目标能量優化停止。T=Tn * ...... T1 * T0
點采樣
由于計算速度的要求，一般是需要對點雲b進行采樣。然後用采樣點去找對應進行優化。除了計算上的要求，如果用全點雲進行比對的話，精度也不會更加的好。因為更好的采樣方法可以避免陷入能量局部極小的情況。常見的采樣方法有均勻采樣和幾何采樣。
- 均勻采樣：采樣點分布均勻，采樣速度快，适合幾何特征比較多的點雲。因為這樣的點雲，均勻采樣總能采樣到幾何特征。如果幾何特征少的話，如下左圖所示，有可能就采樣不到幾何特征。
- 幾何采樣：采樣點會在幾何特征明顯的地方被采樣到，如下右圖所示。它能夠抓住點雲的幾何特征，使得注冊精度更高，更穩定。計算速度可能會慢一些，并且不太适合噪音比較大的點雲，因為噪音其實就是幾何特征了。
一個比較理想的采樣方法，既能夠采樣到點雲的幾何特征，分布上也能做到局部均勻，然後就是計算速度要快。下面是一個采樣示例，左圖是原始點雲，中間圖是均勻采樣，右圖是幾何采樣。
點對應
從ICP的名字，就能看出點對應怎麼去找，也就是給每個采樣點找最近點。查找最近點是比較簡單的，一般用KD Tree來加速查找。這些點對，有些是無效的，需要剔除掉。常用的剔除政策有距離和法線：
- 距離：點雲a和b一般隻有部分重疊，b的采樣點集裡，有部分點在a裡是沒有對應的。記這部分點為c，那麼c的對應點對是需要剔除掉的。經過觀察，我們發現c的點對距離一般是比較大的，是以可以設定一個距離門檻值，來過濾掉這些點對。這個門檻值算是一個參數了，可以根據點雲的平均距離來設定，疊代開始的時候設定的大一些，如果找到的點對數目太少，可以适當的增大這個距離門檻值。ICP疊代過程中，點雲距離會逐漸減小，這個距離門檻值也可以随之動态減小。
- 法線：在ICP疊代初期，點雲位姿相差比較大，很多距離相近的點對也是錯誤的無效點對。我們可以根據點的一些屬性來過濾掉這些無效的點對。常用的屬性就是點雲法線。比如點法線夾角要小于一個角度門檻值。這個門檻值和距離門檻值一樣，也是動态變換的。
目标能量
常用的目标能量有兩種：點到點的能量和點到平面的能量。直覺上講，點到點的能量如左圖所示，優化的是有效點對之間的距離；點到平面的能量，如右圖所示，優化的是點到點雲局部平面的距離。
- 點到點的能量：∑ || a - T(s) ||：其中s是點雲b的有效采樣點，a是s對應的點，T是剛體變換
- 點到平面的能量：∑ || (a - T(s)) * n(s) ||：其中n(s)是采樣點s的法線
這兩個能量，各有優缺點：
- 速度：本質上兩個能量都是優化兩個點雲曲面的距離，點到點能量是線性收斂，點到平面能量的疊代等價于Gauss-Newton疊代，它的收斂速度是優于線性收斂的，情況好的時候，可以達到二階收斂速度。
- 點到點能量不需要法線資訊。有時候可靠的法線資訊不容易得到，比如曲面嚴重不光滑，噪音十分嚴重，或者點雲相對于物體采樣非常稀疏等。不可靠的法線會使得點到平面的能量優化不穩定。
求解目标能量
能量裡面隻有T的旋轉變換是非線性的，可以轉化成線性最小二乘求解。常用的兩種轉化方式：
- 一種是把T當作仿射變換，對求解得到的T做SVD分解，求得仿射變換在剛體變換空間中的投影
- 另一種是把旋轉矩陣用歐拉角的方式來表示，這樣T裡面的非線性部分就是sin和cos。我們假設每次疊代剛體變換的旋轉角度不大，則cos(theta) = 1, sin(theta) = theta。這樣就把T轉化為線性矩陣了。
疊代停止條件
疊代算法總要有個停止條件。這個看似平凡的步驟，卻是所有疊代算法的關鍵。它能影響算法最終的效果和性能。ICP常見的疊代停止條件：
- 最大疊代次數
- 疊代過程中，剛體變換近似恒等變換了
- 疊代過程中，點雲之間的距離小于一定的門檻值
- 疊代過程中，點雲之間的距離越來越大了，需要中止無效疊代。或者更新算法參數重新疊代。
推薦閱讀：
吐血整理|3D視覺系統化學習路線
那些精貴的3D視覺系統學習資源總結(附書籍、網址與視訊教程)
超全的3D視覺資料集彙總
大盤點|6D姿态估計算法彙總(上)
大盤點|6D姿态估計算法彙總(下)
機器人抓取彙總|涉及目标檢測、分割、姿态識别、抓取點檢測、路徑規劃
彙總|3D點雲目标檢測算法
彙總|3D人臉重建算法 那些年，我們一起刷過的計算機視覺比賽 總結|深度學習實作缺陷檢測 深度學習在3-D環境重建中的應用 彙總|醫學圖像分析領域論文 大盤點|OCR算法彙總
重磅！3DCVer-學術論文寫作投稿 交流群已成立
掃碼添加小助手微信，可申請加入3D視覺工坊-學術論文寫作與投稿 微信交流群，旨在交流頂會(ICRA/IROS/ROBIO/CVPR/ICCV/ECCV等)、頂刊(IJCV/TPAMI/TIP等)、SCI、EI等寫作與投稿事宜。
同時也可申請加入我們的細分方向交流群，目前主要有3D視覺、CV&深度學習、SLAM、三維重建、點雲後處理、自動駕駛、CV入門、三維測量、VR/AR、3D人臉識别、醫療影像、缺陷檢測、行人重識别、目标跟蹤、視覺産品落地、視覺競賽、車牌識别、硬體選型、學術交流、求職交流等微信群，請掃描下面微信号加群，備注：”研究方向+學校/公司+昵稱“，例如：”3D視覺 + 上海交大 + 靜靜“。請按照格式備注，否則不予通過。添加成功後會根據研究方向邀請進去相關微信群。原創投稿也請聯系。
▲長按加微信群或投稿
▲長按關注公衆号
3D視覺從入門到精通知識星球：針對3D視覺領域的知識點彙總、入門進階學習路線、最新paper分享、疑問解答四個方面進行深耕，更有各類大廠的算法工程人員進行技術指導。與此同時，星球将聯合知名企業釋出3D視覺相關算法開發崗位以及項目對接資訊，打造成集技術與就業為一體的鐵杆粉絲聚集區，近1000+星球成員為創造更好的AI世界共同進步，知識星球入口：
學習3D視覺核心技術，掃描檢視介紹，3天内無條件退款 icp算法的實作_點雲ICP注冊 圈裡有高品質教程資料、可答疑解惑、助你高效解決問題