重用地圖的單目視覺慣導SLAM系統

2020-12-15 3D視覺工坊

點擊上方「3D視覺工坊」，選擇「星標」

乾貨第一時間送達

Visual-Inertial Monocular SLAM with Map Reuse

重用地圖的單目視覺慣導SLAM系統

摘要

近些年來有很多優秀的視覺慣導融合的裡程計系統，計算高精度和魯棒性的傳感器的增量運動。但是這些系統都是沒有閉環的，所以導致系統即使回到觀測過的地方還是會有累計誤差。本文作者提出了一個新穎的基於緊耦合的帶有閉環檢測的視覺慣導SLAM系統，他可以在已經建圖的地方重用地圖達到0漂移的定位精度。這個系統可以用在所有的相機上，這裡主要介紹存在尺度不確定性的單目相機。本文也提出了一個新穎的IMU初始化的方法可以在短時間內計算很高精度的尺度，重力方向，速度，加速度計和陀螺儀的偏置。在11個序列的飛行數據集上進行了測試，尺度誤差達到1%（釐米級）精度。

本文未開源，但是有大佬復現的代碼：https://github.com/jingpang/LearnVIORB

預備知識

系統的輸入是IMU的測量和單目相機幀，利用針孔模型利用投影模型可以把相機坐標系下的3D點投影到2D的圖像平面：

需要注意的是這個投影方程並沒有考慮相機的畸變，當我們檢測到了圖像中的特徵點先要對他們去畸變。

公式解釋：IMU的測量全部都在B系下，公式1IMU的角度測量減去偏置得到角速度真值，再和時間相乘得到時間間隔(K到K+1幀的)內的角度變化量，再加一個Exp相當於變換到了李群的空間，和上一幀的位姿相乘得到當前幀在W系下的位姿。公式2利用當前加速度的測量量減去當前幀的偏置和時間相乘得到速度變換量再轉換到世界坐標系下，再加上重力加速度引起的變化和上一幀的速度相加得到當前幀的速度。公式3當前幀的位置其實就是勻加速運動的位移方程。

視覺慣導的ORB-SLAM

這篇論文是基於ORB-SLAM做的，對於ORB-SLAM的介紹，可以參考解析ORB-SLAM3的文章。下邊主要介紹加上IMU後對ORB-SLAM框架中Tracking、Local Mapping和Loop Closing的影響。

Tracking

主要的變化是添加了相機頻率的IMU位置、速度和偏置的估計。這樣我們就可以得到比恆速模型準的多的機器人當前幀的初始狀態。一旦預測到了機器人的位置，局部地圖中的地圖點就投影到當前幀和當前幀的特徵點匹配。然後優化當前幀中特徵匹配的重投影誤差和IMU的誤差。這種優化是不同的，取決於地圖是否被局部建圖或閉環線程更新，如圖2所示。

當Tracking在地圖更新後執行的時候，IMU的誤差連接了當前幀j和上一幀i：

公式解釋：這裡就是重投影誤差的最小二乘的形式，利用IMU當前幀的位姿得到世界坐標系下地圖點在B系下的表示，然後利用外參可以得到地圖點在C系下的表示，最後利用針孔模型得到該地圖點在當前幀像素坐標系下的表示，得到和匹配點的誤差。

B.局部建圖

在一個新的關鍵幀插入的時候局部建圖線程進行Local BA，他優化最新的N幀(Local window)和所有被觀測到的地圖點，其他共視圖中維護的不在局部滑窗中的關鍵幀雖然參與優化但是不改變位姿。固定窗口中包含了N+1個和滑窗相連的最新的共視關鍵幀，他們可以約束IMU的狀態。

上圖表示出了ORB-SLAM中的BA和ORB-SLAM-VI中BA的區別，這裡的優化函數是IMU的誤差和重投影誤差的和，和純視覺的方法相比每個關鍵幀多了九個狀態量（速度和偏置）需要優化。所以要選擇一個合適的窗口的大小來實時的優化。

局部建圖線程還負責關鍵幀的管理。原始的ORB-SLAM策略中會丟棄冗餘的關鍵幀，這樣就不會在已經建圖很完善的地方增加很多的關鍵幀。但是這種方法與IMU約束連續關鍵幀的機制是不匹配的，兩個連續關鍵幀的時間相差越長，IMU提供的信息就越弱。因此如果局部窗口中的關鍵幀沒有差過0.5s的我們允許局部建圖線程丟棄冗餘關鍵幀。為了能在閉環檢測或者其他時間優化地圖，本文不允許兩個關鍵幀相差3s。如果關閉具有IMU約束的完整BA，只需要約束局部窗口中的時間偏移即可。

C.閉環檢測

閉環的主要作用是機器人又回到原來到過的地方的時候降低裡程計帶來的累計誤差。場景充實別模塊匹配最近的關鍵幀和原來的一個關鍵幀。利用兩幀的詞袋匹配可以得到剛體變換，然後執行優化的過程來降低裡程計帶來的累計誤差。這種優化在大場景中很耗時，所以執行位姿圖優化，==這樣就可以忽略結構==，也可以很好的收斂。本文可以執行六自由度的位姿圖優化，因為尺度是可觀的。這個系統忽略IMU信息不優化速度和偏置。==速度利用把關聯的關鍵幀旋轉到正確的位姿上來修正。==同時並行的執行一個全局的BA來優化所有的狀態，包括速度和偏置。

IMU初始化

根據由運行一段時間的單目SLAM算法得到的若干關鍵幀來計算視覺-慣導的全局BA的尺度、重力方向、速度和IMU偏置的初始估計。運行一段時間（幾秒鐘）的單目SLAM算法，假設傳感器運動後導致所有的狀態都可觀。

初始化問題可以分成小的子問題

陀螺儀偏置尺度和重力估值（假設沒加速度偏置）加速度偏置估計，尺度和重力方向細化速度估計陀螺儀偏置估計

可以利用兩個連續關鍵幀的orientation估計陀螺儀的偏置，假設偏差的變化忽略不計（bias是緩慢變化的量），可以直接對所有的連續關鍵幀優化常數的bg（最小化陀螺儀的積分和ORB-SLAM計算出來的orientation）：

尺度和重力估計（沒有加速度的偏置）

得到陀螺儀的偏置以後，可以利用預積分得到速度，位置，正確的旋轉加速度的測量來補償陀螺儀的bias。ORB-SLAM計算的相機軌跡具有尺度不確定性，所以在從相機坐標繫到IMU的機體坐標系轉的時候需要加一個尺度因子：(imu的所有的狀態都是在B系下測量得到的)

C.加速度偏置估計，尺度和重力細化

同樣可以整理為公式12的形式：

D.速度估計

在公式12和19中都是考慮三個連續的關鍵幀，所以線性的系統中就沒有3N個額外的未知的速度，現在所有的速度都可以用公式18在重力、尺度和偏置已知的情況下來計算。為了計算最近幾幀的速度，這裡用公式3。

E.重定位後的bias重新初始化

當系統重新初始化的時候，利用公式9重新初始化陀螺儀偏置，利用公式19重新初始化加速度計偏置，尺度和重力是已知的。利用二十個關鍵幀來初始化這兩個偏置。

備註：作者也是我們「3D視覺從入門到精通」特邀嘉賓：一個超乾貨的3D視覺學習社區

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

下載1

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

下載2

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

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

相關焦點

  • 無人機視覺slam給你答案
    目前，視覺SLAM可分為單目、雙目（多目）、RGBD這三類，另還有魚眼、全景等特殊相機，但目前在研究和產品中還屬於少數，此外，結合慣性測量器件（Inertial Measurement Unit，IMU）的視覺SLAM也是現在研究熱點之一。從實現難度上來說，大致將這三類方法排序為：單目視覺＞雙目視覺＞RGBD。
  • 視覺SLAM技術以及其應用詳解
    以傳感器而論，視覺SLAM研究主要分為三大類：單目、雙目（或多目）、RGBD。其餘還有魚眼、全景等特殊相機，但是在研究和產品中都屬於少數。此外，結合慣性測量器件（Inertial Measurement Unit，IMU）的視覺SLAM也是現在研究熱點之一。就實現難度而言，我們可以大致將這三類方法排序為：單目視覺>雙目視覺>RGBD。
  • 對比雷射SLAM與視覺SLAM:誰會成為未來主流趨勢?
    基於視覺的 SLAM 方案目前主要有兩種實現路徑，一種是基於 RGBD 的深度攝像機，比如 Kinect；還有一種就是基於單目、雙目或者魚眼攝像頭的。VSLAM 目前尚處於進一步研發和應用場景拓展、產品逐漸落地階段。雷射 SLAM：早在 2005 年的時候，雷射 SLAM 就已經被研究的比較透徹，框架也已初步確定。雷射 SLAM，是目前最穩定、最主流的定位導航方法。
  • 從單目視覺到三目視覺,三款目前最可靠裝備ADAS功能車型推薦!
    不過我們還是要言歸正傳，特斯拉的這套高級輔助駕駛系統全名autopilot網友們都更願意稱呼它AP。這套系統和我們今天要聊的其他兩款車型以及目前裝配該功能的車型比起來在軟硬體等很多方面都有著很大區別，大多數主流該技術解決方案慣用視覺攝像頭加毫米波雷達和超聲波雷達方案來實現車道居中保持，輔助變道，全速域跟車以及自動泊車等一系列功能，而特斯拉則重用視覺攝像頭輕雷達的方式。
  • 基於平面投影的單目視覺AGV 定位算法
    近年來，計算機視覺飛速發展，湧現出大量的技術與算法，如主動視覺、面向任務的視覺、基於知識的視覺、基於模型的視覺以及多傳感器和集成視覺等方法[1]。根據使用鏡頭數目分類，計算機視覺分為單目視覺定位、雙目視覺定位與多目視覺定位。雙目立體視覺是基於視差原理，由多幅圖像獲取物體三維幾何信息。
  • ORB-SLAM3 單目地圖初始化(終結篇)
    工坊致力於乾貨輸出，不做搬運工，為計算機視覺領域貢獻自己的力量！歡迎大家一起交流成長~一、前言請閱讀本文之前最好把ORB-SLAM3的單目初始化過程再過一遍（ORB-SLAM3 細讀單目初始化過程(上)、超詳細解讀ORB-SLAM3單目初始化（下篇）），以提高學習效率。
  • SLAM的動態地圖和語義問題
    當年計算機視覺唯一的工業落地場景「視覺監控」也在這個問題費了腦筋，比如我搬個凳子到新位置，然後就走了，系統是不是要自動更新背景呢？以前說過SFM和SLAM的稱呼，計算機視覺的同行多半說SFM，而機器人的行業流行說SLAM，到底區別在哪裡？有說SFM是假設背景不動，那麼outlier是什麼？
  • 大牛講堂|SLAM第一篇:基礎知識
    Simultaneous Localization and Mapping，即時定位與地圖構建技術。無論在室內、野外、空中還是水下，SLAM是機器人進入未知環境遇到的第一個問題。本期將給大家介紹SLAM的基礎知識：傳感器與視覺SLAM框架。傳感器與視覺SLAM框架
  • 大牛講堂|SLAM最終話:視覺裡程計
    Simultaneous Localization and Mapping，即時定位與地圖構建技術。無論在室內、野外、空中還是水下，SLAM是機器人進入未知環境遇到的第一個問題。本期將給大家介紹SLAM的視覺裡程計。
  • 一個超乾貨的3D視覺學習社區
    現在有沒有對於sfm或者slam之後得到的點雲進行補全處理的工作啊？你好，請問點雲表面重建有哪些方法呀？請問怎麼把點雲投影到一個以點雲平均點為圓心的球面上，然後再對球面進行三角網格化。請問一下，有沒有關於點雲增強方面的綜述性質的文章或者帖子呢？針對點雲增強這一點雲和深度圖怎麼轉換呢？
  • 面向環境與載體特性視覺SLAM方法簡介
    同步定位與地圖構建（Simultaneous Localization and Mapping，SLAM）是智慧機器人在未知環境中實現自主移動的核心技術之一。以攝像頭為主的SLAM技術又被稱為視覺SLAM，它通過處理視頻攝像頭捕獲的圖像信息，實時獲取攝像頭六自由度的位置朝向信息（定位），並同時構建環境的三維點雲地圖（地圖構建）視覺SLAM在多個領域具有極高的應用價值，如地面移動機器人、無人機自動駕駛車輛、穿戴式設備的定位導航，以及增強（AR）/虛擬實境（VR）中的相機姿態跟蹤，近年來受到業界高度關注。
  • No slam dunk?
    by the fire department is no slam dunk.Certainly, no slam dunk.According to Woodward, Tenet reassured the president that 「it’s a slam dunk case」 that Saddam had weapons of mass destruction.
  • 機器人的雙眸:視覺SLAM是如何實現的?
    只有學會了這些東西之後，你才能真正上手編一個SLAM系統。如果你要跑實際機器人，還要會ROS。當然，困難多意味著收穫也多，坎坷的道路才能鍛鍊人（比如說走著走著才發現Linux和C++才是我的真愛之類的。）鑑於目前網上關於視覺SLAM的資料極少，我於是想把自己這三年的經驗與大家分享一下。說的不對的地方請大家批評指正。
  • 視覺導航 vs 雷射導航,掃地機器人SLAM技術解析
    為了效率更高、更節能地實現清掃全屋的功能，繪製室內地圖以及確定自身位置是必不可少的功能，許多早期掃地機器人產品並沒有配備導航系統，只能按照設定的路線進行清掃，碰到牆壁或物體邊緣便轉向，導致清掃線路雜亂無章、效率低下。近年來不少中高端掃地機器人都配備了導航系統，可以為室內區域建圖或建模，以此來規劃清掃路線，既節能又可以提高效率。
  • 一文看懂SLAM導航技術分類、典型應用及國內發展現狀
    國內的科沃斯、塔米以及最新面世的嵐豹掃地機器人都可以通過用SLAM算法結合雷射雷達或者攝像頭的方法，讓掃地機高效繪製室內地圖，智能分析和規劃掃地環境，從而成功讓自己步入了智能導航的陣列。VR/AR方面：輔助增強視覺效果。SLAM技術能夠構建視覺效果更為真實的地圖，從而針對當前視角渲染虛擬物體的疊加效果，使之更真實沒有違和感。
  • 基於慣性導航、RFID 及圖像識別的 AGV 融合導航系統
    目前AGV 主要有電磁導引、光學導引、雷射導航、慣性導航、視覺導航和全球定位系統導航等方式[1]。多數AGV 採用雷射導航和磁導航方式，雖然這些導航技術已是成熟技術，但其成本高，難以維護和改造，因此，新的導航方式越來越受到人們的關注。其中，慣性導航技術作為一種不依賴外界輔助的獨立導航方式，在適用範圍和布局上具有得天獨厚的優勢。
  • 自主移動機器人的定位與地圖創建(SLAM)關鍵性問題
    移動機器人的定位和地圖創建是自主移動機器人領域的熱點研究問題。對於已知環境中的機器人自主定位和已知機器人位置的地圖創建已經有了一些實用的解決方法。然而在很多環境中機器人不能利用全局定位系統進行定位，而且事先獲取機器人工作環境的地圖很困難，甚至是不可能的。這時機器人需要在自身位置不確定的條件下，在完全未知環境中創建地圖,同時利用地圖進行自主定位和導航。
  • 農村公路數據普查法寶——立得空間可攜式勘測系統
    基於道路移動測量系統進行公路普查成為最佳方式。可攜式勘察箱是立得空間最新推出針對農村公路數據普查的可攜式勘測系統，該系統集雙目CCD相機、定位定姿系統、GPS接收機等傳感器於一體，具備小型化、輕量化、模塊化、軟體一體化的特點。
  • SLAM,讓機器人知行合一 | 雷鋒網公開課
    另外用在視覺SLAM這塊用的比較多是特徵點地圖，這種地圖記錄環境中特徵點（或稱為關鍵點）的幾何空間位置。這種地圖相比柵格地圖看起來就不那麼直觀了。當然這個問題並不是無解，一個好的商用化SLAM系統，迴環問題是否能很好的解決，就成了評判這個系統實力的指標了。這裡先講了第一個難點，是算法層面的。其實還有很多這種坑，比如什麼走廊問題。除了算法難點其實還有別的難點，SLAM要做好其實不光靠用的算法多好，尤其現在很多開源實現代表的是學術界。
  • 位姿參數辨識系統
    目前，參數辨識主要依靠視覺傳感器，視覺測量具有系統簡單、成本低、對目標無擾動、非接觸、測量參數豐富等特點[6]，能夠完成傳統接觸式測量方式無法完成的任務[3]。現有的基於視覺的目標運動參數測量技術，主要是對圖像中的目標進行特徵提取、圖像識別及運動學處理[2]。由於特徵點會發生漂移，得到的目標運動參數精度及實時性欠佳，難以滿足高精度、快響應的任務需求。