點擊上方“視覺部落”，選擇“星標”公眾號

精選作品，第一時間送達

---

文章同步首發于知乎用戶yikang專欄

Part 1. 基本信息

本文提出了一種基于緊耦合滑動窗口非線性優化方法的單目視覺-慣性系統，來自港科大沈老師實驗室。這篇論文的亮點包括提出了效果最佳的IMU預積分理論、估計器初始化機制、故障檢測和復原機制、外參在線校訂、基于優化的緊耦合VIO、重定位機制以及全局位姿圖優化模塊等內容。論文開源地址：https://github.com/HKUST-Aerial-Robotics/VINS-Mono

Part 2. 整體框架

VINS的整體結構框圖如上所示。整體流程和各個模塊的作用可以理解概括為：

第一步是系統框圖上方中間的Sect.4，這一模塊中進行了相機和IMU測量數據的處理，包括相機圖像的特征提取和追蹤，兩幀圖像時刻之間IMU數據的預積分以及選取關鍵幀。可以看到圖中注明了相機和IMU數據采集頻率相差較大，因此下一步需要將相機和IMU數據進行對齊。

第二步是框圖上方右側的Sect.5，這一模塊進行了視覺慣性數據的對齊，通過對齊操作可以得到后面非線性優化部分需要的一些數值：相機位姿、速度、重力向量、陀螺儀偏差和路標點位置等。通過視覺慣性對齊，相機坐標系到世界坐標系(東北天坐標系)之間的關系就已知了，從而可以將相機坐標系中的軌跡(pose)對齊到世界坐標系，并且可以根據IMU的預積分值獲得單目視覺不可觀的尺度信息。因此可以這樣理解，在初始化部分IMU的作用包括：對齊世界坐標系、獲得尺度信息。

第三步是框圖中間的具有重定位功能的VIO模塊，該VIO系統基于非線性優化方法，并且引入了滑動窗口算法，在窗口移動的過程中，并不是直接丟掉舊的數據，而是將其進行邊緣化(marginalization)，被marg的數據將為下一時刻的窗口提供先驗信息，也就是把信息向前進行傳遞而非直接丟棄。而marg舊幀的策略也有專門的部分進行介紹。VIO模塊緊密融合了IMU預積分、圖像中的特征觀測以及閉合回路中的特征重檢測。

第四步也是最后一步是框圖下方的全局位姿圖優化模塊，接收來自上一步的重定位結果，并且執行4自由度(三維平移和偏航角yaw)的全局圖優化，目的是消除這四個自由度在長時間內存在的漂移。這一模塊還同時維護了一個關鍵幀集合，這與VO的backend部分策略較為相似。

值得總結的是上面提到的全局圖優化可視為backend，第三步的VIO/重定位可視為frontend，因此就像之前筆者介紹VO的時候講過的，可以使用多線程同時運行前端與后端，本文中也是這么做的。也就是使用多線程同時運行第三步和第四步。

Part 3. 各模塊細節

1.測量預處理 measurement preprocessing

總的來說這一部分主要針對視覺測量和IMU測量進行處理：對于視覺圖像，在最新的圖像幀中提取特征，在相鄰兩幀圖像之間進行特征追蹤；對于IMU數據，對兩幀圖像之間的時間內產生的數據進行預積分。并且IMU測量數據是同時受到bias和高斯白噪聲影響的，在進行預積分時會考慮bias的影響。

(1)視覺圖像處理

• 特征檢測采用角點特征，每幀圖像有最低的特征數量(100個，過少將會影響后續的特征追蹤)，相鄰特征之間設置最小像素間隔(避免特征點集中)，使用基于基礎矩陣F的RANSAC算法剔除異常點。

• 特征追蹤使用KLT稀疏光流算法。

跟模塊還負責選取關鍵幀，關鍵幀選擇標準有兩個：

• 平均視差標準：如果被跟蹤特征的平均視差介于當前幀和最新關鍵幀之間，并且超過某個閾值，則將當前幀設為新的關鍵幀。由于純旋轉情況可以引起視差但是不能進行三角化，因此使用陀螺儀的短期積分進行旋轉補償以解決這個問題(僅用于關鍵幀選擇)。

• 跟蹤質量：如果跟蹤的特征數低于某個閾值，將當前幀設為新的關鍵幀。這是為了避免特征軌跡由于特征點過少而丟失。

(2)IMU預積分

首先給出IMU測量值的數學模型：

其中加速度計和陀螺儀的噪聲項都設為高斯白噪聲，bias誤差模型為隨機游走，其導數為零均值高斯分布。

根據積分關系，對IMU的測量值在世界坐標系內進行積分可以得到位置、速度和旋轉四元數在任意時間間隔內的值。未了避免IMU積分的重傳遞，采用了預積分協方差傳播的策略，關于預積分部分的公式推導要去看原文中的對應部分，值得學習。注意預積分得到的結果都是以本體系/相機系為參考坐標系的。

當對bias的估計發生變化時，本文提出使用兩時刻間的協方差傳遞矩陣對預積分結果進行修正，而不是重新計算預積分的值，k時刻和k+1時刻的協方差矩陣為：

2.估計器初始化 estimator initialization

初始化通常是單目VINS系統中最脆弱的一步。實驗條件下當然可以在靜止狀態下進行初始化但是在實際使用中顯然運動條件下的初始化才是更經常遇到的。因此本部分解決的是如何在未知運動的條件下就對VINS系統進行初始化。

首先具體一點來說明通過坐標系對齊來獲得初始值的思路：在之前分享的文章中我們已經知道視覺SLAM有著良好的初始化特性，我們可以根據相鄰幀間的相對運動獲得相機位姿等參數，因此在 SfM 的基礎上通過對齊IMU預積分結果和視覺SfM結果，可以大致獲得尺度、重力、速度甚至bias偏差。這種數據融合方式屬于松耦合。該對齊過程的示意圖如下所示：

本文在初始化階段忽略了加速度計的bias偏差，這是因為和重力的量綱比起來bias偏差實在是太小了以至于可以融合在重力向量中。

(1)視覺 SfM 的滑動窗口算法 Sliding Window Vision-Only SfM

首先通過視覺SfM來獲得去尺度化的位姿和路標點組成的圖。過程大概如下：

檢查最新幀和之前所有幀之間的特征對應關系：如果能在滑動窗口中找到穩定的特征跟蹤(超過30個被跟蹤特征)和足夠的視差(超過20個旋轉補償像素)，就使用五點法恢復這兩幀之間的相對運動；如果沒有穩定的特征跟蹤和足夠的視差，就把當前幀留在滑動窗口中并等待下一幀進來。

若運動恢復成功，就對兩幀中共同觀測到的所有特征進行三角化，尺度任意。

有了第二步的三角化結果，就可以對窗口中的其他幀進行PnP求解(3D-2D)。

最后使用全局BA對所有的觀測到的特征的重投影誤差，優化變量是相機位姿和路標點位置。(其實BA問題應該是我們非常熟悉的內容了)

在上面的步驟中，我們已第一幀中的相機系為參考坐標系。這是為什么呢？因為我們還沒有引入IMU來進行相機系和是世界系的對齊唄，這里也就體現了如果沒有IMU來測量重力向量，那我們就沒有辦法把相機的軌跡對齊到世界坐標系(一般就是東北天坐標系)中。

在引出下一部分的視覺慣性對齊內容前，先提一嘴尺度因子 s，假設說我們獲得了IMU和相機之間的外參?和?(也就是相機系到本體系的平移和旋轉)，那么我們就可以把相機系下的參數變換到本體系中(非常顯然)：

而其中的尺度因子 s 就是下一部分對齊時獲得的，解決這個參數可是能否成功初始化的關鍵(畢竟啊，引入IMU的一個出發點就是向解決尺度不可觀的問題，而這個問題的解決就在初始化部分了啊(嚴格來說后面的優化中其實還有，但是初始化部分的尺度因子是非常關鍵的))

(2)視覺慣性對齊 Visual-Inertial Alignment

這一部分又分成了四個部分，分別概括一下進行的工作：

校準陀螺儀bias偏差?：通過對IMU預積分關于陀螺儀bias偏差的雅可比進行線性化，以及最小化損失函數，可以獲得??的初始校準，并用此??來更新預積分中的平移、速度和旋轉項。

對速度、重力向量和尺度因子進行初始化：根據相機和IMU在物理上是剛體連接的約束，我們可以得到速度、旋轉等幾何約束，根據這些約束以及視覺SfM中恢復的相機運動，可以 得到窗口中每個幀的速度，視覺參考幀中的重力矢量，以及尺度因子。

重力矢量細化：一般來說某個區域的重力大小是已知的，因此重力矢量的模長是已知的，這就構成了一個約束，利用這個約束可以是重力矢量的自由度變為2，由此就可以對重力矢量進行參數化(這里其實是很巧妙的，這個參數化在絕對重力矢量的基礎上加入了兩個正交的分量，然后通過迭代計算兩個分量進而對重力矢量進行細化)。

完成初始化：重點來了！利用上一步得到的精細化之后的重力向量和世界坐標系中的絕對重力矢量進行旋轉重合，由此得到相機系到世界系的變換關系，這就完成了對齊。按照此對齊關系，把相機坐標系中的速度也變換到世界坐標系，同時把軌跡按照尺度因子向公制單位進行縮放。至此就完成了初始化，對齊后的量就可以喂給緊耦合的VIO部分了。

我覺得還是稍微總結一下上面這幾個步驟的思路：首先在校準陀螺儀bias偏差?后對預積分量進行更新(使之更精確了)，然后利用IMU和相機的幾何約束獲得重力向量、速度和尺度因子的初始值，然后在對其中的重力向量進行精細化(因為重力向量是連接本體系和世界系的橋梁，被自己的這句總結文藝到了哈哈)，然后再根據重力向量重合這樣的條件使得相機系到世界系旋轉對齊，把速度和尺度因子也用上，這就完成了初始化部分的工作。

3.基于優化的緊耦合單目VIO模塊 tightly-couple monocular VIO

總的來看這一模塊的核心是滑動窗口(含邊緣化策略)以及視覺慣性BA，前者用于控制計算規模，后者用于實際優化。實現的功能就是初步優化得到了窗口中幀的位姿和速度(其實就是BA的作用)。這一節中同樣分為了幾個小的部分，大致分開講一下。

(1)整體上使用視覺慣性BA公式(屬于局部BA)，引入了Huber核函數和測量殘差的先驗信息，通過對構造的誤差函數求最小化來獲得最大的后驗估計。其中測量殘差包括IMU的殘差和相機視覺的殘差(具體定義的方式在去看原文哈)。在這里提一下與傳統的相機圖像重投影誤差定義方式不同，本文將圖像測量殘差定義在了單位球面上，將其向球面切平面的兩個正交方向上進行分解。

(2)邊緣化策略：這個還是比較有代表性的亮點了，這個算法的作用主要是維護滑動窗口的計算規模，新的一幀進來就要Marg掉舊的一幀，但是至于Marg哪一幀，以及如何把被Marg的信息轉換為先驗信息，這些都歸屬于邊緣化的策略中了：

• 如果窗口中第二最新幀是關鍵幀，則將其保留在窗口中，并將最舊幀及其相應的視覺和慣性測量邊緣化，被邊緣化的測量將被轉換為先驗測量；

• 如果第二個最新的幀不是關鍵幀，只需刪除該幀及其所有相應的視覺測量。但是對于非關鍵幀保留預積分慣性測量，并且預積分過程將繼續進行到下一幀。

• 具體的邊緣化操作通過舒爾補來進行，具體的實現建議去看一下原文，作者還提到邊緣化會導致線性化點的更早確定，這可能會導致次優的估計結果，但是又由于VIO可以接受小的偏移因此認為邊緣化帶來的負面影響可以忽略不計。

(3)輕量級的視覺慣性BA算法。一般的視覺慣性(VI)-BA優化難以實現相機幀率同步的運行，作者提出了一種輕量級的VI-BA優化算法：不去優化滑動窗口中的所有狀態，而是只優化固定數量的最新IMU狀態的姿態和速度。將特征深度、外部參數、偏差和不希望優化的舊IMU狀態視為常量。使用所有的視覺和慣性測量來進行僅運動的BA優化。與完全緊耦合的單目VIO(在最先進的嵌入式計算機上可能會超過50ms)相比，僅運動的VI-BA只需要大約5ms的計算時間。示意圖如下所示：

(4)故障檢測及復原策略。再魯棒的算法也難免遇到故障或失效的狀態，本文的故障檢測標準可以概括如下：

• 最新幀中被跟蹤的特征數小于某一閾值。這一點還是比較容易理解的，太少的特征點當然會讓讓運動跟蹤以及優化出現問題(跟丟了)。

• 估計器最后兩個輸出之間的位置或旋轉的顯著不連續性。這一點也是容易理解的，位移或者角度不可能突然跳變，因此如果出現了較大的不連續性，那就說明出現了故障。

• bias偏差或者外參的估計值出現了較大的變化。理解同上一點，外參發生大的跳變是不合理的，bias隨機游走在短時間內是很小的(前面我們說了其導數是零均值的高斯分布)。

如果檢測到了故障怎么辦呢？其實也很簡單，那就是再把系統調到初始化階段(十分簡單直接)。

這個VIO模塊的一大作用是使得滾轉角和俯仰角變得完全可觀，整個系統只剩下三自由度平移和偏航角不可觀，于是后面步驟中會針對這四個自由度優化的誤差進行消除。(哦對還得補充一下VIO是如何讓roll和pitch變得可觀的呢？在初始化部分我們也介紹過，通過旋轉對齊重力矢量，我們將相機坐標系和世界坐標系進行了對齊。其實對齊重力矢量的過程就使得滾轉和俯仰可觀了，只剩下繞重力矢量方向的yaw角未知。)

4.重定位模塊 relocalization

作者在這一部分里提到了系統累計誤差來源于滑動窗口和邊緣化的操作，更具體一點就是四自由度的優化估計(三自由度平移以及繞重力方向的偏航角yaw)。為了消除這一誤差，作者提出了一種和VIO模塊緊密耦合的重定位模塊(就是這一部分的內容)。

概括的說這一模塊的過程為：閉合回環檢測、建立當前幀與回環的特征關聯、將這些特征關聯集成到前面的VIO模塊以消除漂移。

(1)回環檢測。這一部分使用的是DBoW2詞袋算法來識別位置， DBoW2在時間和幾何一致性檢查之后返回循環閉合候選項，哦對查詢時的單詞使用的是特征的BREIF描述子(這個是老朋友了應該很熟悉它的計算過程)。

(2)特征檢索。在上一步檢測到閉合回環之后，通過BREIF描述子去檢測當前滑動窗口和閉合回環之間的特征并建立關聯。但是呢直接對BRIEF描述子進行匹配會有很多異常值(誤匹配，這個之前還真是領教過，必須得進行異常值提出不然沒法看啊)，作者提出了兩步操作來剔除異常值：

• 2D-2D的基礎矩陣測試，測試對象是當前幀和回環候選幀中檢索到的特征的二維觀測。說的很繞口但其實就是把前面檢索到的特征的2D觀測拿出來進行基礎矩陣的測試，具體怎么測試呢，去看看代碼吧！

• 3D-2D的PnP測試，把檢索到的特征在當前滑動窗口中對應的3D位置與回環候選幀中特征的2D觀測進行測試。

經過上面兩步檢測，如果正常特征點的數量大于一定的閾值，就認為回環中的候選幀是正確的回環匹配，就進行下一步的緊耦合重定位。

(3)緊耦合重定位。重定位過程有效地將當前由單目VIO模塊維護的滑動窗口與過去的姿態圖對齊(品一品這個對齊的意思)。在這一步重定位中會使用所有IMU測量值、局部視覺測量值(也就是窗口內的視覺測量值)以及從回環檢測中檢索到的特征來聯合優化話滑動窗口。

5.全局位姿圖優化 global pose graph optimization

OK開頭還是有必要先說一下為什么要有這個全局圖優化的模塊。全局圖優化維護的是關鍵幀集合(也就是說被優化的是關鍵幀)。那就花點時間來看一下這些幀的來龍去脈，全局圖優化里面的關鍵幀是在滑動窗口中被marg掉的幀，這些幀變成了“舊的幀”，進而在回環檢測和重定位模塊被用來消除滑動窗口中幀的累計誤差，相當于是標尺的作用，所以這就意味著我們要確保這些“舊的幀”得是“準確的”、被優化過的、具有全局一致性的，這些幀從滑動窗口中被marg掉之前不是進行過重定位對齊了嗎？難道還不具備這些條件？確實是不具備的，我們回頭去看VIO模塊中盡在滑動窗口中優化了固定數量的狀態的位姿和速度，得到的結果其實是有誤差的(不具備全局一致性)，如果沒有這個全局圖優化模塊，即便有前面的回環檢測和重定位部分也不過是在有誤的地圖中進行了定位。

就像在前面兩個部分中提到的一樣，VIO模塊使得位姿中的滾轉和俯仰變得可觀，只剩下了3自由度平移和偏航角(繞重力向量的旋轉)不可觀，因此這里的全局圖優化只用來估計這四個自由度，所有又稱為4自由度全局位姿圖優化。

同樣的這一部分還是將分為建立位姿圖、執行優化、圖管理等幾個部分，分開看一下

(1)向位姿圖中加入關鍵幀。被加入圖優化的幀是從滑動窗口中被邊緣化的關鍵幀，并且進入到圖中之后會成為頂點，這些頂點有兩中連接的邊(請注意下面這兩種邊均只含有4自由度的位姿變換)：

• 連續運動邊：這樣的一條邊代表著滑動窗口中相鄰兩個關鍵幀之間的相對運動，邊的初始值就取自VIO中局部BA的求解結果。

• 回環匹配邊：當位姿圖中的一個關鍵幀成功檢測到回環之后，就與回環中的對應幀之間建立起一條這樣的邊。

(2)4自由度的圖優化。圖優化想必是大家已經非常熟悉的事情了，我們先定義邊的殘差，然后引入魯棒核函數(作者用的是Huber核函數，一般我們也這么用)，就得到了損失函數，對損失函數進行最小二乘優化求解。作者使用了多線程來完成圖優化。

(3)圖優化規模管理。這一機制主要是防止因為行駛路程太長使得全局圖優化的計算規模太大而影響實時性，于是作者限制了數據的規模：帶有回環約束的關鍵幀將被保留，而其他太接近或者與相鄰的關鍵幀非常相似的關鍵幀將被刪除。(非常不錯的想法，實現也值得一看)

Part 4. 實驗及總結

作者將VINS-Mono進行了多次實驗和測試，復述結果沒有什么意義，但是作者在分析結果時提到了幾點問題感覺還是有助于我們理解VINS的原理的。

VINS-Mono在滾轉roll和俯仰pitch的估計中效果不如OKVIS，原因可能是這兩個量是根據IMU的預積分得到的，預積分其實是為了節省計算量而采用的一種一階近似手段，因此可能造成了誤差存在。

關閉了回環檢測和重定位功能的VINS-Mono表現大打折扣，和OKVIS一樣在四自由度(三自由度平移和yaw角)上有漂移，但是打開回環檢測后效果提升就很顯著了。

移植到IOS移動端的程序與商用的Google tango比起來絲毫不落下風，尤其是在全局的誤差消除上，甚至比tango還要好，這得益于4自由度的全局位姿圖優化。

論文的最后作者說了幾點VINS系統接下來的發展方向以及其團隊的研究興趣，大概有這么幾點吧：

在線評價單目視覺系統可觀性的方法，以及在若可觀性下在線恢復。

將單目VINS系統大規模部署在移動設備(消費電子產品)上。

依托單目VINS系統進行稠密建圖。

最后是自己的一點感受吧，整個系統確實非常復雜精妙，這樣的成果肯定是作者積淀了很長很長時間所得來的，VIO或者VINS系統的理論可能剖析開來沒有多么復雜，但是在實際實現中，為了一個準確性、魯棒性、實時性要解決的細節和問題太多了。只能說作者“不愧是拿了華為兩百萬年薪的人”。(嗯甚至還覺得兩百萬給少了……)

---

推薦閱讀：論文學習——SVO半直接法單目視覺里程計

SLAM從0到1——圖優化g2o：從看懂代碼到動手編寫(長文)

SLAM從0到1——手寫視覺里程計中的直接法

SLAM從0到1——批量計算BA問題的圖優化解法

SLAM從0到1——推導思路：卡爾曼濾波與擴展卡爾曼濾波

SLAM從0到1——由雙目視覺圖像獲得三維點云(含Pangolin/OpenCV基本指令講解)

實戰技能——C++中的多線程編程

概述 | 53頁PPT為你深入淺出自動駕駛

---

覺得有用，給個在看吧！??

總結

以上是生活随笔為你收集整理的单目相机 svd 从图像恢复3维位置_论文学习——VINSMono：一种鲁棒且通用的单目视觉惯性系统...的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。