從零開始一起學習SLAM | ICP原理及應用

2021-01-13 計算機視覺life

 點「計算機視覺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算法流程

首先對於一幅點雲中的每個點，在另一幅點雲中計算匹配點（最近點）

極小化匹配點間的匹配誤差，計算位姿

然後將計算的位姿作用於點雲

再重新計算匹配點

如此迭代，直到迭代次數達到閾值，或者極小化的能量函數變化量小於設定閾值

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

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

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

小白：比如圖像金字塔！哈哈

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

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

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

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

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

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

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

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

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

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

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

下面是個示意圖

這就是ICP的原理啦！

下面是實踐環節

練習

1、推導ICP中的一個步驟

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

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

2、 精心設計的ICP編程實踐

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

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

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

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

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

本文參考：高翔《視覺SLAM十四講》

推薦閱讀

相關焦點

  • 學習SLAM需要哪些預備知識?
    他包含了上述在視覺slam需要的所有基礎知識，真是造福大眾啊。詳細研讀此書，以後讀各種論文就不會顯得那麼吃力了吧。最後列舉一些玩slam的一些必備工具和相關資源。1. ubuntu,  cmake, bash, vim, qt(optional).2.
  • SLAM軌跡全局誤差計算
    然後我們再進行方差的計算： 協方差： 將其進行SVD分解，記作，並且有： 相似變換的自由度可以表示為： 以上就是相似變換的umeyama算法原理evo_ape tum groundTruth.txt slam.txt -va --plot 使用evo工具進行對齊需要將所有數據按照tum數據集的格式進行txt文件寫入。
  • icp許可證的用途是什麼
    icp許可證的用途是什麼？簡單來說，就是從事網際網路經營服務的網站或者企業，就要辦理icp許可證。當然這是大範圍的說法，對於icp許可證來說，並不只是單單的經營性服務，旗下將涉及道許多的服務類型，都是同樣要辦理icp許可證的。
  • 從零開始的大冒險
    從零開始的大冒險 網路遊戲 大小: 502.96MB 版本: 1.0.1
  • 如何系統地從零開始學習PLC?
    PLC在現在的人懂的人看來，的確是比較簡單的東西，畢竟它屬於應用類型的二次編程開發。但是作為一個零基礎初學者，要學PLC，還是要付出一定的時間來摸索的。先要明白PLC本質是繼電器電路的軟體化，也就是在電腦上畫畫繼電器電路圖，這個是它的根本，所以在學習PLC之前，你一定要有點電工基礎，理解和實操過繼電器電路的使用，這個是非常重要的，畢竟PLC軟體的東西都是在模擬硬體電路，請關注：容濟點火器作為一個過來人，我當年也是從零開始學PLC的中國的大學教育，因為很多原因，往往比較重視理論教育，而在學校裡邊的動手機會是沒有那麼多的，特別是
  • icp備案是什麼意思?如何批量查詢icp備案信息
    相信一些做站群的用戶，在購買老域名的時候，都會買一批有備案域名去建站，那麼icp備案是什麼意思？如何批量查詢icp備案信息，下面聚查為您詳解一下以上問題。一：icp備案是什麼意思icp備案的意思是網絡內容服務商 ，即向廣大用戶綜合提供網際網路信息業務和增值業務的網絡運營商。ICP備案是信息產業部對網站的一種管理，為了防止非法網站。備案的目的就是為了防止在網上從事非法的網站經營活動，打擊不良網際網路信息的傳播，如果網站不備案的話，很有可能被查處以後關停。信息產業部對網站的一種管理，為了防止非法網站。
  • 四川成都icp網絡許可證,不一樣的經營icp,網際網路信息服務業務!
    官方稱為《中華人民共和國電信與信息服務經營許可證》或《中華人民共和國增值電信業務經營許可證》<信息服務業務（僅限網際網路信息服務）>，業內俗稱icp許可證、經營icp、網際網路信息服務業務icp許可證。
  • icp許可證每年要年檢嗎
    那麼，對於icp許可證的來說，是不是每年都要年檢呢？ icp許可證，就是網站經營許可證，經營性的網站就必須辦理icp許可證，網站是辦理icp許可證的基礎，不光是網站，對於符合要求的APP或者小程序都是需要辦理的。
  • 花垣國外有icp許可申請周期
    花垣國外有icp許可申請周期專業辦理網絡文化經營許可證、網絡表演文網文、直/播文網文、動漫文網文、音樂文網文、增值電信業務經營許可證、icp許可證辦理、edi許可證辦理、icp、IDC許可證、ISP許可證、SP許可證、許可證、網際網路藥-品信息服務許可證書、許可證、國內多方通信、營業性演出、出版物經營許可證、廣播電視節目製作許可證
  • 汾陽市ICP備案許可證需要哪些東西
    《互聯官信息服務經營許可證申請表》，營業執照副本，若企業尚未辦理營業執照可提供預登記證明，法人及股東身份證明，公司章程，公司主要管理人員及技術人員身份證複印件，5-8人社保證明，接入協議及託管商資質，註冊證書;申請ICP的時間一般為2-4個月， ICP經營許可證使用年限為5年，5年後需要辦理續期建立了網際網路通信業許可制度和非商業網際網路通信文件制度，經營或非經營icp
  • 從零開始學電路基礎- -從零開始學電路基礎,初學者們看這裡~~~
    導讀：從零開始學電路基礎，這篇文章志在讓親們快速掌握電路基礎知識，為以後學習更深入的知識打下基礎，從零開始學電路基礎是專門針對初學者的，大神們請自覺飄過~~~本文引用地址：http://www.eepw.com.cn/article/268472.htm　　電路的概念
  • 從零開始設計自動牙籤盒的原理圖
    下面嘗試從零開始設計這個自動牙籤盒的原理圖。02工作原理分析結合經驗思考了一下，自動牙籤盒的工作原理應該是這樣：有一個感應傳感器，應該是一套紅外對管，當手放到這個紅外對管的感應區域，紅外對管中的接收管就會導通，產生一個信號給單片機，單片機就開始控制電機轉動，推動結構件夾出一根牙籤，結構上需要用到兩個限位開關，當電機轉動時，結構件往上伸出來的時候，當觸發上限開關時，電機停轉，完成自動夾出牙籤的動作；當電機反轉時
  • 從零開始怎麼學計算機編程?
    於是很多人想投身其中，而且很大一部分都是零基礎非科班的狀態。在學習者中有大學生，也初高中生，他們都有一個共性，沒什麼基礎。從零基礎開始學習編程成為了一種常態，可是，也正是因為我們在編程方面是零基礎，很容易導致我們選錯專業和行業。從零開始學習電腦如何才能保證學好呢？以下為大家做一個簡單的總結，希望我們能在學習時有所參考。
  • 成都ICP許可證代辦,ICP許可證辦理材料是什麼?
    icp許可證又稱icp經營許可證。一般來說如果你的網站向用戶收費，你必須申請ICP許可證。這是第二種增值電信業務。ICP許可證只有企業才有資格申請。個人沒有資格申請。事實上從另一個方面來看，個人不能開設在線購物中心。涉及非法經營，相關部門將被罰款。如果數額較大，他們將違法。因此正規運營最好是註冊公司。
  • 陵縣外資企業icp備案申請條件
    陵縣外資企業icp備案申請條件專業辦理網絡文化經營許可證、網絡表演文網文、直/播文網文、動漫文網文、音樂文網文、增值電信業務經營許可證、icp許可證辦理、edi許可證辦理、icp、IDC許可證、ISP許可證、SP許可證、許可證、網際網路藥-品信息服務許可證書、許可證、國內多方通信、營業性演出、出版物經營許可證、廣播電視節目製作許可證
  • 如何從零開始逐步學習人工智慧
    它是研究、開發用於模擬、延伸和擴展人的智能的理論、方法、技術及應用系統的一門技術科學。人工智慧是計算機科學的一個分支，它企圖了解智能的實質，並生產出一種新的能以人類智能相似的方式做出反應的智能機器，該領域的研究包括機器人、語言識別、圖像識別、自然語言處理和專家系統等。人工智慧從誕生以來，理論和技術日益成熟，應用領域也不斷擴大，可以設想，未來人工智慧帶來的科技產品，將會是人類智慧的「容器」。
  • 《ICP經營許可證》的辦理流程、材料和條件
    ICP許可證經營類型分類，可分為以下幾種電商網站icp許可證；網際網路廣告類icp許可證；遊戲網站icp許可證；在線交易類網站icp許可證；導購類網站icp許可證。
  • 新版「阿爾法圍棋」從零開始40天稱王
    圖/視覺中國不依靠人類圍棋知識；「從零開始」自學圍棋，僅3天就擊敗前輩版本據新華社電 人工智慧「阿爾法圍棋」在幾次世界矚目的人機大戰後站在圍棋之巔，又以一種新的方式超越了自己：新版本「從零開始」學習圍棋，僅用3天就擊敗前輩版本，40天成為新的王者。
  • 企業icp備案接入商是什麼意思
    企業icp備案接入商是指為企業提供虛擬主機或伺服器專線接入的公司。直白的說，備案接入商就是企業的空間服務商，或者叫伺服器的提供者。舉例來說，你的企業是在西部數碼購買的伺服器，就在西部數碼的備案系統提交網站備案信息，西部數碼就是你企業的icp備案接入商。ICP可以理解為向廣大用戶提供網際網路信息業務和增值業務的電信運營商，是經國家主管部門批准的正式運營企業或部門。
  • 我英語基礎很差,怎麼從零開始學英語?
    很多同學已經準備開始複習工作了，在教學的過程中，我們收到很多問題，其中有一個問題出現的頻率最多，那就是——我英語基礎很差，如何從零開始學好英語？本月，我們的Hero老師，為英語基礎不好的同學們仔細解答這個問題，希望大家都可以輕鬆踏上自己的學習旅途。