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

VIO-SLAM

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

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

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

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

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

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

算法流程

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

各個(gè)模塊的主要作用是：

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

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

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

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

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

算法核心

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

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

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

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

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

實(shí)驗(yàn)結(jié)果與總結(jié)

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

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

實(shí)驗(yàn)結(jié)果如下：

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

實(shí)驗(yàn)二：車載數(shù)據(jù)上的實(shí)驗(yàn)

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

實(shí)驗(yàn)結(jié)果如下：

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

參考文獻(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