點“計算機視覺life”關注，星標更快接收乾貨！ ##

小白：師兄，最近忙什麼呢，都見不到你人影，我們的課也好久沒更新了呢

師兄：抱歉，抱歉，最近忙於俗事。我後面一起補上，學習勁頭得向你們年輕人學習啊！話說，你最近在研究什麼呢？

小白：最近在看ICP，十四講上簡單提了下，還有點懵懵噠

師兄：好，那今天就說說ICP演算法吧。搬個小板凳做好哈：

ICP全稱Iterative Closest Point，翻譯過來就是迭代最近點。ICP在點雲配準領域應用的非常廣泛，因此基於深度相機、鐳射雷達的演算法使用ICP的頻率比較高。

小白：那我是學視覺SLAM的，是不是不用關注了啊（內心OS：畢竟SLAM要學的太多了）

師兄：哈哈，這個懶不能偷的。ICP現在在視覺SLAM中用的也非常多了。我總結了一下下ICP的應用場景：

ICP到底有什麼用呢？

假設我們在三維空間中有兩組點集，我們分別稱之為P1, P2好了，P1, P2中都有幾千個點，那麼可以分為兩種情況：

1、我們完全不知道P1, P2中每個點是如何對應的。這樣的情況很常見，比如我用只有depth的深度相機或者鐳射雷達在兩個不同的位置對著一隻兔兒分別拍攝一張深度圖並轉化為點雲圖，我特喵的也不知道哪個點和哪個點對應，但是我想把這兩個點雲“融合”（專業詞彙叫做registration，有些地方翻譯為註冊，有點讓人不知所云，我建議翻譯為配準）在一起，變成一個更完整的兔兒。

2、我們已經知道P1, P2中哪個點對應的哪個點。比如我們使用RGB-D相機分別在兩個不同位置拍攝一張 RGB彩色圖 + 深度圖，而且彩色圖和深度圖是對齊好的。因為有彩色圖我們就可以做特徵點匹配了，因為每個特徵點都對應一個深度圖上的深度值，所以我們能夠得到兩組對應好的三維點。

小白：原來如此，看起來ICP還挺強大的，對應不對應都能搞定啊，ICP為何這麼牛叉？

ICP為何這麼牛叉？

師兄：來看看ICP的過程就知道了。為了方便，我們用一個二維的例子來說明吧，初始是兩個不同角度下的笑臉（深紅色和綠色），下面是紅色笑臉如何通過ICP過程和綠色笑臉重合的：

小白：好神奇啊

師兄：這個只是一個示例，通常情況下我們在SLAM中不會遇到這種情況，都是三維空間的點。我再以點雲配準為例進行說明。假設現在有兩幅待配準的點雲（比如上面的小兔子），ICP演算法是這樣配準兩幅點雲的：

ICP演算法流程

1. 首先對於一幅點雲中的每個點，在另一幅點雲中計算匹配點（最近點）
2. 極小化匹配點間的匹配誤差，計算位姿
3. 然後將計算的位姿作用於點雲
4. 再重新計算匹配點
5. 如此迭代，直到迭代次數達到閾值，或者極小化的能量函式變化量小於設定閾值

下面是用三維點雲進行ICP的一個效果

小白：第一次計算的誤差估計很大，後面會慢慢變小，直到收斂？

師兄：嗯，這是一個由粗放到精細的過程，在視覺演算法中由粗到精的方法很常用。。

小白：比如影象金字塔！哈哈

師兄：嗯，熟悉的配方，熟悉的味道（/笑臉）

ICP的精髓其實就是迭代，一次次的修正錯誤，最後獲得一個還不錯的結果。

不過話是這樣說，但是一般情況下資料都有不少噪音，如果噪音較大，第一種情況不一定能完全配準正確。

小白：聽起來合情合理，也不麻煩，書上為啥推導那麼複雜？

師兄：嗯，這就是感性到理性的過程。視覺SLAM十四講書上從數學上推導了具體如何實現。我這裡引用一下求解ICP的三個步驟：

小白：原理推導大概是能看明白了

師兄：嗯嗯，推導部分給你留了一個小小的課後作業了

小白：師兄想的周到，這樣才能真正掌握呢（內心OS：真狠啊。。）

師兄：上面介紹是最簡單的點和點匹配的ICP演算法，實際應用中為了使得演算法魯棒，也就是在有不少噪聲的情況下仍然能夠得到正確的結果，很多研究者提出了不同的匹配思路：例如，極小化的誤差項包括對應點的點到點的歐式距離，和對應點的點到平面距離,以及極小化對應點的顏色值誤差等

小白：這麼多方法，用哪個好呢？

師兄：如果你應用場景簡單，資料乾淨基本沒有噪聲，最原始的點點匹配就行。如果有噪聲，則最後考慮一下複雜點的方法。2003年的時候，pottman 和Hofer兩位大牛的論文中證明了當兩幅點雲比較接近時，極小化對應點的點到平面距離比點到點距離更接近兩個平面之間的真實距離，也就是說計算點到平面的距離更靠譜！

下面是個示意圖

這就是ICP的原理啦！

下面是實踐環節

練習

1、證明：《視覺SLAM十四講》第174頁公式7.55中的

各符號定義見書上，其中，tr表示矩陣的跡。

2、 給定一個軌跡１，資料格式：timestamp tx ty tz qx qy qz qw， 自定義一個任意的旋轉矩陣和平移向量（可以嘗試不同的值，甚至加一些噪聲看看結果有什麼變化），對軌跡１進行變換，得到一個新的軌跡２， 使用ICP演算法（提示：取平移作為三維空間點）估計軌跡１，２之間的位姿，然後將該位姿作用在軌跡２

驗證：ICP演算法估計的旋轉矩陣和平移向量是否準確；軌跡１，２是否重合。

如下是我加了一個旋轉平移量後的兩個軌跡，經過ICP計算好位姿後再反作用在變換後的軌跡，最終兩個軌跡是重合滴！

程式碼框架、資料及預期結果已經為你準備好了，公眾號「計算機視覺life」後臺回覆：ICP，即可獲得。

歡迎留言討論，更多學習視訊、文件資料、參考答案等關注計算機視覺life公眾號，掃描二維碼檢視「從零開始學習SLAM」星球介紹，抓住三維視覺風口，快來和其他小夥伴一起學習交流~

本文參考：

高翔《視覺SLAM十四講》

推薦閱讀

如何從零開始系統化學習視覺SLAM？
從零開始一起學習SLAM | 為什麼要學SLAM？
從零開始一起學習SLAM | 學習SLAM到底需要學什麼？
從零開始一起學習SLAM | SLAM有什麼用？
從零開始一起學習SLAM | C++新特性要不要學？
從零開始一起學習SLAM | 為什麼要用齊次座標？
從零開始一起學習SLAM | 三維空間剛體的旋轉
從零開始一起學習SLAM | 為啥需要李群與李代數？
從零開始一起學習SLAM | 相機成像模型
從零開始一起學習SLAM | 不推公式，如何真正理解對極約束?
從零開始一起學習SLAM | 神奇的單應矩陣
從零開始一起學習SLAM | 你好，點雲
從零開始一起學習SLAM | 給點雲加個濾網
從零開始一起學習SLAM | 點雲平滑法線估計
從零開始一起學習SLAM | 點雲到網格的進化
從零開始一起學習SLAM | 理解圖優化，一步步帶你看懂g2o程式碼
從零開始一起學習SLAM | 掌握g2o頂點程式設計套路
從零開始一起學習SLAM | 掌握g2o邊的程式碼套路
零基礎小白，如何入門計算機視覺？
SLAM領域牛人、牛實驗室、牛研究成果梳理
我用MATLAB擼了一個2D LiDAR SLAM
視覺化理解四元數，願你不再掉頭髮
最近一年語義SLAM有哪些代表性工作？
視覺SLAM技術綜述
彙總 | VIO、鐳射SLAM相關論文分類集錦
研究SLAM，對程式設計的要求有多高？
2018年SLAM、三維視覺方向求職經驗分享
2018年SLAM、三維視覺方向求職經驗分享
深度學習遇到SLAM | 如何評價基於深度學習的DeepVO，VINet，VidLoc？
視覺SLAM關鍵方法總結
SLAM方向公眾號、知乎、部落格上有哪些大V可以關注？
SLAM實驗室
SLAM方向國內有哪些優秀公司？
SLAM面試常見問題
SLAM相關領域資料集調