從零開始一起學(xué)習(xí)SLAM | ICP原理及應(yīng)用
點(diǎn)擊上方“小白學(xué)視覺”，選擇加"星標(biāo)"或“置頂”
重磅干貨，第一時(shí)間送達(dá)
小白：師兄，最近忙什么呢，都見不到你人影，我們的課也好久沒更新了呢
師兄：抱歉，抱歉，最近忙于俗事。我后面一起補(bǔ)上，學(xué)習(xí)勁頭得向你們年輕人學(xué)習(xí)?。≡捳f，你最近在研究什么呢？
小白：最近在看ICP，十四講上簡(jiǎn)單提了下，還有點(diǎn)懵懵噠
師兄：好，那今天就說說ICP算法吧。搬個(gè)小板凳做好哈：
ICP全稱Iterative Closest Point，翻譯過來就是迭代最近點(diǎn)。ICP在點(diǎn)云配準(zhǔn)領(lǐng)域應(yīng)用的非常廣泛，因此基于深度相機(jī)、激光雷達(dá)的算法使用ICP的頻率比較高。
小白：那我是學(xué)視覺SLAM的，是不是不用關(guān)注了?。▋?nèi)心OS：畢竟SLAM要學(xué)的太多了）
師兄：哈哈，這個(gè)懶不能偷的。ICP現(xiàn)在在視覺SLAM中用的也非常多了。我總結(jié)了一下下ICP的應(yīng)用場(chǎng)景：
ICP到底有什么用呢？
假設(shè)我們?cè)谌S空間中有兩組點(diǎn)集，我們分別稱之為P1, P2好了，P1, P2中都有幾千個(gè)點(diǎn)，那么可以分為兩種情況：
1、我們完全不知道P1, P2中每個(gè)點(diǎn)是如何對(duì)應(yīng)的。這樣的情況很常見，比如我用只有depth的深度相機(jī)或者激光雷達(dá)在兩個(gè)不同的位置對(duì)著一只兔兒分別拍攝一張深度圖并轉(zhuǎn)化為點(diǎn)云圖，我特喵的也不知道哪個(gè)點(diǎn)和哪個(gè)點(diǎn)對(duì)應(yīng)，但是我想把這兩個(gè)點(diǎn)云“融合”（專業(yè)詞匯叫做registration，有些地方翻譯為注冊(cè)，有點(diǎn)讓人不知所云，我建議翻譯為配準(zhǔn)）在一起，變成一個(gè)更完整的兔兒。
2、我們已經(jīng)知道P1, P2中哪個(gè)點(diǎn)對(duì)應(yīng)的哪個(gè)點(diǎn)。比如我們使用RGB-D相機(jī)分別在兩個(gè)不同位置拍攝一張 RGB彩色圖 + 深度圖，而且彩色圖和深度圖是對(duì)齊好的。因?yàn)橛胁噬珗D我們就可以做特征點(diǎn)匹配了，因?yàn)槊總€(gè)特征點(diǎn)都對(duì)應(yīng)一個(gè)深度圖上的深度值，所以我們能夠得到兩組對(duì)應(yīng)好的三維點(diǎn)。
小白：原來如此，看起來ICP還挺強(qiáng)大的，對(duì)應(yīng)不對(duì)應(yīng)都能搞定啊，ICP為何這么牛叉？
ICP為何這么牛叉？
師兄：來看看ICP的過程就知道了。為了方便，我們用一個(gè)二維的例子來說明吧，初始是兩個(gè)不同角度下的笑臉（深紅色和綠色），下面是紅色笑臉如何通過ICP過程和綠色笑臉重合的：
小白：好神奇啊
師兄：這個(gè)只是一個(gè)示例，通常情況下我們?cè)赟LAM中不會(huì)遇到這種情況，都是三維空間的點(diǎn)。我再以點(diǎn)云配準(zhǔn)為例進(jìn)行說明。假設(shè)現(xiàn)在有兩幅待配準(zhǔn)的點(diǎn)云（比如上面的小兔子），ICP算法是這樣配準(zhǔn)兩幅點(diǎn)云的：
ICP算法流程
首先對(duì)于一幅點(diǎn)云中的每個(gè)點(diǎn)，在另一幅點(diǎn)云中計(jì)算匹配點(diǎn)（最近點(diǎn)）
極小化匹配點(diǎn)間的匹配誤差，計(jì)算位姿
然后將計(jì)算的位姿作用于點(diǎn)云
再重新計(jì)算匹配點(diǎn)
如此迭代，直到迭代次數(shù)達(dá)到閾值，或者極小化的能量函數(shù)變化量小于設(shè)定閾值
下面是用三維點(diǎn)云進(jìn)行ICP的一個(gè)效果
小白：第一次計(jì)算的誤差估計(jì)很大，后面會(huì)慢慢變小，直到收斂？
師兄：嗯，這是一個(gè)由粗放到精細(xì)的過程，在視覺算法中由粗到精的方法很常用。。
小白：比如圖像金字塔！哈哈
師兄：嗯，熟悉的配方，熟悉的味道（/笑臉）
ICP的精髓其實(shí)就是迭代，一次次的修正錯(cuò)誤，最后獲得一個(gè)還不錯(cuò)的結(jié)果。
不過話是這樣說，但是一般情況下數(shù)據(jù)都有不少噪音，如果噪音較大，第一種情況不一定能完全配準(zhǔn)正確。
小白：聽起來合情合理，也不麻煩，書上為啥推導(dǎo)那么復(fù)雜？
師兄：嗯，這就是感性到理性的過程。視覺SLAM十四講書上從數(shù)學(xué)上推導(dǎo)了具體如何實(shí)現(xiàn)。我這里引用一下求解ICP的三個(gè)步驟：
小白：原理推導(dǎo)大概是能看明白了
師兄：嗯嗯，推導(dǎo)部分給你留了一個(gè)小小的課后作業(yè)了
小白：師兄想的周到，這樣才能真正掌握呢（內(nèi)心OS：真狠啊。。）
師兄：上面介紹是最簡(jiǎn)單的點(diǎn)和點(diǎn)匹配的ICP算法，實(shí)際應(yīng)用中為了使得算法魯棒，也就是在有不少噪聲的情況下仍然能夠得到正確的結(jié)果，很多研究者提出了不同的匹配思路：例如，極小化的誤差項(xiàng)包括對(duì)應(yīng)點(diǎn)的點(diǎn)到點(diǎn)的歐式距離，和對(duì)應(yīng)點(diǎn)的點(diǎn)到平面距離,以及極小化對(duì)應(yīng)點(diǎn)的顏色值誤差等
小白：這么多方法，用哪個(gè)好呢？
師兄：如果你應(yīng)用場(chǎng)景簡(jiǎn)單，數(shù)據(jù)干凈基本沒有噪聲，最原始的點(diǎn)點(diǎn)匹配就行。如果有噪聲，則最后考慮一下復(fù)雜點(diǎn)的方法。2003年的時(shí)候，pottman 和Hofer兩位大牛的論文中證明了當(dāng)兩幅點(diǎn)云比較接近時(shí)，極小化對(duì)應(yīng)點(diǎn)的點(diǎn)到平面距離比點(diǎn)到點(diǎn)距離更接近兩個(gè)平面之間的真實(shí)距離，也就是說計(jì)算點(diǎn)到平面的距離更靠譜！
下面是個(gè)示意圖
這就是ICP的原理啦！
下面是實(shí)踐環(huán)節(jié)
練習(xí)
1、推導(dǎo)ICP中的一個(gè)步驟
證明：《視覺SLAM十四講》第174頁公式7.55中的
各符號(hào)定義見書上，其中，tr表示矩陣的跡。
2、 精心設(shè)計(jì)的ICP編程實(shí)踐
給定一個(gè)軌跡１，數(shù)據(jù)格式：timestamp tx ty tz qx qy qz qw， 自定義一個(gè)任意的旋轉(zhuǎn)矩陣和平移向量（可以嘗試不同的值，甚至加一些噪聲看看結(jié)果有什么變化），對(duì)軌跡１進(jìn)行變換，得到一個(gè)新的軌跡２， 使用ICP算法（提示：取平移作為三維空間點(diǎn)）估計(jì)軌跡１，２之間的位姿，然后將該位姿作用在軌跡２。
驗(yàn)證：ICP算法估計(jì)的旋轉(zhuǎn)矩陣和平移向量是否準(zhǔn)確；軌跡１，２是否重合。
如下是我加了一個(gè)旋轉(zhuǎn)平移量后的兩個(gè)軌跡，經(jīng)過ICP計(jì)算好位姿后再反作用在變換后的軌跡，最終兩個(gè)軌跡是重合滴！
好消息！?
小白學(xué)視覺知識(shí)星球
開始面向外開放啦??????
下載1：OpenCV-Contrib擴(kuò)展模塊中文版教程 在「小白學(xué)視覺」公眾號(hào)后臺(tái)回復(fù)：擴(kuò)展模塊中文教程，即可下載全網(wǎng)第一份OpenCV擴(kuò)展模塊教程中文版，涵蓋擴(kuò)展模塊安裝、SFM算法、立體視覺、目標(biāo)跟蹤、生物視覺、超分辨率處理等二十多章內(nèi)容。 下載2：Python視覺實(shí)戰(zhàn)項(xiàng)目52講 在「小白學(xué)視覺」公眾號(hào)后臺(tái)回復(fù)：Python視覺實(shí)戰(zhàn)項(xiàng)目，即可下載包括圖像分割、口罩檢測(cè)、車道線檢測(cè)、車輛計(jì)數(shù)、添加眼線、車牌識(shí)別、字符識(shí)別、情緒檢測(cè)、文本內(nèi)容提取、面部識(shí)別等31個(gè)視覺實(shí)戰(zhàn)項(xiàng)目，助力快速學(xué)校計(jì)算機(jī)視覺。 下載3：OpenCV實(shí)戰(zhàn)項(xiàng)目20講 在「小白學(xué)視覺」公眾號(hào)后臺(tái)回復(fù)：OpenCV實(shí)戰(zhàn)項(xiàng)目20講，即可下載含有20個(gè)基于OpenCV實(shí)現(xiàn)20個(gè)實(shí)戰(zhàn)項(xiàng)目，實(shí)現(xiàn)OpenCV學(xué)習(xí)進(jìn)階。 交流群
歡迎加入公眾號(hào)讀者群一起和同行交流，目前有SLAM、三維視覺、傳感器、自動(dòng)駕駛、計(jì)算攝影、檢測(cè)、分割、識(shí)別、醫(yī)學(xué)影像、GAN、算法競(jìng)賽等微信群（以后會(huì)逐漸細(xì)分），請(qǐng)掃描下面微信號(hào)加群，備注：”昵稱+學(xué)校/公司+研究方向“，例如：”張三?+?上海交大?+?視覺SLAM“。請(qǐng)按照格式備注，否則不予通過。添加成功后會(huì)根據(jù)研究方向邀請(qǐng)進(jìn)入相關(guān)微信群。請(qǐng)勿在群內(nèi)發(fā)送廣告，否則會(huì)請(qǐng)出群，謝謝理解~