計算機(jī)視覺方向簡介之視覺慣性里程計

VIO-SLAM

Visual-Inertial Odometry（VIO）即視覺慣性里程計，有時也叫視覺慣性系統(tǒng)（VINS，visual-inertial system），是融合相機(jī)和IMU數(shù)據(jù)實現(xiàn)SLAM的算法，根據(jù)融合框架的不同又分為松耦合和緊耦合。

其中VO（visual odometry）指僅視覺的里程計，T表示位置和姿態(tài)。松耦合中視覺運動估計和慣導(dǎo)運動估計系統(tǒng)是兩個獨立的模塊，將每個模塊的輸出結(jié)果進(jìn)行融合。

緊耦合則是使用兩個傳感器的原始數(shù)據(jù)共同估計一組變量，傳感器噪聲也是相互影響的。緊耦合算法比較復(fù)雜，但充分利用了傳感器數(shù)據(jù)，可以實現(xiàn)更好的效果，是目前研究的重點。

相機(jī)和IMU的缺點及互補(bǔ)性

相機(jī)和IMU融合有很好的互補(bǔ)性。首先通過將IMU 估計的位姿序列和相機(jī)估計的位姿序列對齊可以估計出相機(jī)軌跡的真實尺度，而且IMU 可以很好地預(yù)測出圖像幀的位姿以及上一時刻特征點在下幀圖像的位置，提高特征跟蹤算法匹配速度和應(yīng)對快速旋轉(zhuǎn)的算法魯棒性，最后IMU 中加速度計提供的重力向量可以將估計的位置轉(zhuǎn)為實際導(dǎo)航需要的世界坐標(biāo)系中。

隨著MEMS器件的快速發(fā)展，智能手機(jī)等移動終端可以便捷地獲取IMU數(shù)據(jù)和攝像頭拍攝數(shù)據(jù)，融合IMU 和視覺信息的VINS 算法可以很大程度地提高單目SLAM 算法性能，是一種低成本高性能的導(dǎo)航方案，在機(jī)器人、AR/VR 領(lǐng)域得到了很大的關(guān)注。

算法流程

整個流程圖可以分解為五部分：數(shù)據(jù)預(yù)處理、初始化、局部非線性優(yōu)化、回環(huán)檢測和全局優(yōu)化。

各個模塊的主要作用是：

圖像和IMU數(shù)據(jù)預(yù)處理：對于圖像，提取特征點，利用KLT金字塔進(jìn)行光流跟蹤，為后面僅視覺初始化求解相機(jī)位姿做準(zhǔn)備。對于IMU，將IMU數(shù)據(jù)進(jìn)行預(yù)積分，得到當(dāng)前時刻的位姿、速度、旋轉(zhuǎn)角，同時計算在后端優(yōu)化中將要用到的相鄰幀間的預(yù)積分增量，及預(yù)積分的協(xié)方差矩陣和雅可比矩陣。

初始化：初始化中，首先進(jìn)行僅視覺的初始化，解算出相機(jī)的相對位姿；然后再與IMU預(yù)積分進(jìn)行對齊求解初始化參數(shù)。

局部非線性優(yōu)化：對應(yīng)流程圖中滑動窗口的視覺慣導(dǎo)非線性優(yōu)化，即將視覺約束、IMU約束放在一個大目標(biāo)函數(shù)中進(jìn)行優(yōu)化，這里的局部優(yōu)化也就是只優(yōu)化當(dāng)前幀及之前的n幀的窗口中的變量，局部非線性優(yōu)化輸出較為精確的位姿。

回環(huán)檢測：回環(huán)檢測是將前面檢測的圖像關(guān)鍵幀保存起來，當(dāng)再回到原來經(jīng)過的同一個地方，通過特征點的匹配關(guān)系，判斷是否已經(jīng)來過這里。前面提到的關(guān)鍵幀就是篩選出來的能夠記下但又避免冗余的相機(jī)幀（關(guān)鍵幀的選擇標(biāo)準(zhǔn)是當(dāng)前幀和上一幀之間的位移超過一定閾值或匹配的特征點數(shù)小于一定閾值）。

全局優(yōu)化：全局優(yōu)化是在發(fā)生回環(huán)檢測時，利用相機(jī)約束和IMU約束，再加上回環(huán)檢測的約束，進(jìn)行非線性優(yōu)化。全局優(yōu)化在局部優(yōu)化的基礎(chǔ)上進(jìn)行，輸出更為精確的位姿。

算法核心

局部優(yōu)化會用到邊緣化，僅用局部優(yōu)化精度低，全局一致性差，但是速度快，IMU利用率高；僅用全局優(yōu)化精度高，全局一致性好，但是速度慢，IMU利用率低；兩者側(cè)重點不同，所以將兩者結(jié)合，可以優(yōu)勢互補(bǔ)。

因此小編設(shè)計實驗采用局部優(yōu)化和全局優(yōu)化融合的方法。

局部優(yōu)化是滑動窗口內(nèi)相機(jī)幀的優(yōu)化，全局優(yōu)化是所有關(guān)鍵幀的優(yōu)化，兩者結(jié)合會產(chǎn)生邊緣幀沖突的問題，因為局部優(yōu)化會固定滑動窗口邊緣幀，而全局優(yōu)化發(fā)生回環(huán)檢測的時候則會固定回環(huán)起點的幀。這里的改進(jìn)就是采用相對的位姿邊緣化，即邊緣化以后的點是相對于它上一時刻關(guān)鍵幀的位姿而不是全局的位姿，這樣局部優(yōu)化邊緣化相對位姿（關(guān)鍵幀），扔給全局優(yōu)化整體優(yōu)化。局部邊緣化和全局邊緣化的結(jié)合部分是關(guān)鍵幀。

相對邊緣化可以具體解釋為，相對邊緣化的參考坐標(biāo)系不再是世界坐標(biāo)系，而是與當(dāng)前幀共視且距離最近的一個關(guān)鍵幀的相機(jī)系（設(shè)為第k0幀）。視覺約束可以表示為：

區(qū)別于絕對邊緣化的視覺約束

實驗結(jié)果與總結(jié)

實驗一：無人機(jī)數(shù)據(jù)集上的實驗

數(shù)據(jù)集采用了歐盟機(jī)器人挑戰(zhàn)數(shù)據(jù)集（EuRoC）。EuRoC 數(shù)據(jù)集使用 Asctec Firefly 六旋翼飛行器在倉庫和房間采集數(shù)據(jù)，數(shù)據(jù)集中包括以20Hz采集的相機(jī)圖像和200Hz的IMU數(shù)據(jù)，以及運動真值。

實驗結(jié)果如下：

實驗結(jié)果可見，融合優(yōu)化的軌跡和真實軌跡很接近，而僅使用局部優(yōu)化的定位結(jié)果誤差不斷累積。

實驗二：車載數(shù)據(jù)上的實驗

該車載數(shù)據(jù)是在北京市朝陽區(qū)某小區(qū)采集的，在數(shù)據(jù)采集階段，車輛以5km/h 到 30km/h 的速度行駛，一共行駛2271m。

實驗結(jié)果如下：

實驗結(jié)果可見，融合優(yōu)化后的定位結(jié)果明顯優(yōu)于僅使用局部優(yōu)化的定位結(jié)果，融合優(yōu)化中誤差得到及時修正。

參考文獻(xiàn)

J. Delmerico. A Benchmark Comparison of Monocular Visual-Inertial OdometryAlgorithms for Flying Robots. 2018. ICRA

T. Qin. VINS-Mono： A robust and versatile monocular visual-inertial state estimator. arXivpreprint arXiv： 1708.03852， 2017.

N. Trawny. Indirect KalmanFilter for 3D Attitude Estimation. 2005.

Sola. Quaternion kinematics for error-state kalmanfilter. 2017.

K. Eckenhoff. Decoupled， Consistent Node Removal and Edge sparsificationfor graph-based SLAM. 2016.

G. Sibley. Sliding window filter with application to planetary landing. 2010.

S. Leutenegger. Keyframe-Based Visual-Inertial SLAM Using Nonlinear Optimization. 2015.

H. Liu. ICE-BA： Incremental， consistenand efficient bundle adjustment for visual-inertial slam. 2018. CVPR.

H. Liu. Robust keyframe-based dense SLAM with an RGB-D camera. 2017.

編輯：jq