論文地址：在公眾號(hào)「3D視覺工坊」，后臺(tái)回復(fù)「LVI-SAM」，即可直接下載。

我相信很多人對(duì)激光視覺慣導(dǎo)融合的系統(tǒng)都是這樣設(shè)計(jì)的，但是最難的是把自己的想法保質(zhì)保量的實(shí)現(xiàn)出來。我們做不到但是大佬可以！

注1：文末附【VSLAM】交流群

注2：整理不易，請(qǐng)點(diǎn)贊支持！

作者：Liam | 來源：3D視覺工坊微信公眾號(hào)

摘要

本文提出了一個(gè)緊耦合的雷達(dá)視覺慣導(dǎo)SLAM系統(tǒng)，可以實(shí)時(shí)高精度魯棒的進(jìn)行狀態(tài)估計(jì)和建圖。LVI-SAM構(gòu)建在因子圖之上，并且由兩個(gè)子系統(tǒng)組成：一個(gè)視覺慣導(dǎo)系統(tǒng)和一個(gè)激光慣導(dǎo)系統(tǒng)。這兩個(gè)子系統(tǒng)利用緊耦合的方法，視覺慣導(dǎo)的系統(tǒng)利用激光慣導(dǎo)的估計(jì)來做初始化。視覺慣導(dǎo)系統(tǒng)利用激光雷達(dá)的測(cè)量給視覺特征提供深度來提升精度。同時(shí)激光慣導(dǎo)系統(tǒng)利用視覺慣導(dǎo)的估計(jì)值作為初值來做幀的匹配。利用視覺做閉環(huán)檢測(cè)，把檢測(cè)的結(jié)果給激光慣導(dǎo)系統(tǒng)來做優(yōu)化。當(dāng)激光慣導(dǎo)和視覺慣導(dǎo)子系統(tǒng)的一個(gè)失敗的時(shí)候LVI-SAM仍然可以工作，這極大的提升了系統(tǒng)在缺少紋理或者特征場(chǎng)景中的魯棒性。LVI-SAM系統(tǒng)在數(shù)據(jù)集上進(jìn)行了測(cè)試，取得了很好的效果。

介紹

基于雷達(dá)的SLAM系統(tǒng)可以獲取很長(zhǎng)范圍的環(huán)境的細(xì)節(jié)，但是在缺少結(jié)構(gòu)信息的場(chǎng)景中很容易失效。例如長(zhǎng)走廊或者是開闊的廣場(chǎng)?；谝曈X的方法在紋理信息豐富的場(chǎng)景中表現(xiàn)良好，而且很容易做場(chǎng)景重識(shí)別。但是他們對(duì)光照的變化，快速運(yùn)動(dòng)和初始化很敏感。因此，激光和視覺經(jīng)常和慣導(dǎo)融合來提升他們的精度和魯棒性。慣導(dǎo)可以幫助去除點(diǎn)云的運(yùn)動(dòng)畸變，而且在缺少特征的環(huán)境中撐一小段時(shí)間，同時(shí)可以幫助視覺系統(tǒng)恢復(fù)尺度信息。

本文提供了一個(gè)緊耦合雷達(dá)視覺慣導(dǎo)里程計(jì)，LIS系統(tǒng)和VIS系統(tǒng)可以獨(dú)立的運(yùn)行（當(dāng)一個(gè)系統(tǒng)失敗的時(shí)候），也可以耦合在一起（當(dāng)檢測(cè)到很多特征點(diǎn)的時(shí)候）。視覺慣導(dǎo)系統(tǒng)執(zhí)行利用雷達(dá)幀恢復(fù)深度的視覺特征的跟蹤，通過優(yōu)化視覺重投影誤差和imu測(cè)量誤差的視覺里程計(jì)為激光雷達(dá)的scan-matching提供初值，并且把約束加入到因子圖中。當(dāng)利用imu把點(diǎn)云的畸變?nèi)コ?，激光慣導(dǎo)系統(tǒng)檢測(cè)點(diǎn)云的邊緣和平面特征，并和保存在劃窗中的features map對(duì)齊。LIS系統(tǒng)估計(jì)得到的系統(tǒng)狀態(tài)可以傳到VIS系統(tǒng)中做初始化。對(duì)于閉環(huán)，候選的匹配幀通過視覺的詞袋模型得到，在LIS系統(tǒng)中做優(yōu)化。來自視覺里程計(jì)，激光里程計(jì)，imu預(yù)積分和閉環(huán)的約束都會(huì)放到因子圖中，最后，優(yōu)化得到的IMU的bias用來遞推給出IMU頻率的位姿。本文的主要貢獻(xiàn)有：

• 基于因子圖的緊耦合的LVIO系統(tǒng)，實(shí)現(xiàn)了多傳感器融合與基于場(chǎng)景重識(shí)別的全局優(yōu)化；

• 通過故障檢測(cè)來繞過失敗的子系統(tǒng)，使其對(duì)傳感器性能下降具有魯棒性；

• 利用不同的數(shù)據(jù)集進(jìn)行了完善的驗(yàn)證。

從系統(tǒng)的角度來看，作者的工作是新穎的，代表了VIO和LIO中最新技術(shù)的集成，實(shí)現(xiàn)了高精度的LVI系統(tǒng)，作者希望大家可以以此為基礎(chǔ)，推動(dòng)激光慣導(dǎo)視覺融合系統(tǒng)的發(fā)展。

完整的激光視覺慣導(dǎo)SLAM系統(tǒng)

A. 系統(tǒng)概述

系統(tǒng)接受3D激光點(diǎn)云、單目圖像和IMU信息作為輸入。VIS系統(tǒng)接受圖像和IMU信息，雷達(dá)點(diǎn)云是可選的輸入。通過最小化IMU和視覺的測(cè)量殘差來得到視覺里程計(jì)。激光里程計(jì)通過最小化檢測(cè)到的線面特征到特征圖的距離得到。特征圖保存在一個(gè)滑窗中來實(shí)時(shí)的執(zhí)行。最后狀態(tài)估計(jì)問題，可以公式化為一個(gè)MAP問題。利用iSAM2來優(yōu)化因子圖中IMU預(yù)積分，視覺里程計(jì)，激光里程計(jì)和閉環(huán)的約束的殘差。需要注意的是，LIS中采用的多傳感器圖優(yōu)化旨在減少數(shù)據(jù)交換并提高系統(tǒng)效率。

B. 視覺慣導(dǎo)系統(tǒng)

視覺慣導(dǎo)的pipeline如圖2所示。

視覺特征利用角點(diǎn)檢測(cè)器，利用KLT算法進(jìn)行跟蹤。我們利用視覺里程計(jì)對(duì)齊激光幀的點(diǎn)云得到一個(gè)稀疏的深度圖來得到特征的深度完成視覺慣導(dǎo)初始化。系統(tǒng)在滑窗中執(zhí)行BA，其中狀態(tài)X可以寫成：

R是SO3表示旋轉(zhuǎn)矩陣，p表示位置，v表示速度，b=[b_a, b_w]是imu的偏置。變換矩陣T是SE3表示從機(jī)體坐標(biāo)系到世界坐標(biāo)系的變換 。下邊將詳細(xì)的介紹VIS的初始化和特征深度估計(jì)。讀者們可以讀VINS-Mono的原文來了解細(xì)節(jié)。

1）初始化：基于優(yōu)化的VIO系統(tǒng)由于很強(qiáng)的非線性導(dǎo)致初始化發(fā)散。初始化的質(zhì)量取決于兩個(gè)主要因素：初始化的傳感器運(yùn)動(dòng)和IMU參數(shù)的精度。在實(shí)際的實(shí)驗(yàn)中，我們發(fā)現(xiàn)VINS很容易在系統(tǒng)速度很小或者勻速的時(shí)候初始化失敗。這是由于加速度激勵(lì)不夠大導(dǎo)致尺度不可觀。IMU的參數(shù)包含漸變的bias和高斯白噪聲。他影響這加速度和角速度的測(cè)量，在初始化的時(shí)候好的初值可以幫助系統(tǒng)快速的優(yōu)化。

為了改善VIS初始化的魯棒性，我們利用LIS系統(tǒng)來估計(jì)系統(tǒng)的狀態(tài)X和IMU的偏置b。因?yàn)橹苯涌梢酝ㄟ^雷達(dá)的觀測(cè)得到深度信息。我們首先初始化LIS系統(tǒng)獲得x和b。然后我們通過插值來把激光幀和視覺幀通過時(shí)間戳對(duì)齊。假設(shè)IMU的偏置在兩個(gè)圖象關(guān)鍵幀之間是常數(shù)。最后把LIS系統(tǒng)初始化得到的x和b作為VIS初始值，這很有效的改善初始化的速度和魯棒性。

2）深度特征關(guān)聯(lián)：根據(jù)VIS初始化的結(jié)果，我們利用視覺里程計(jì)對(duì)齊視覺幀和雷達(dá)幀。由于當(dāng)前的3D lidar掃描的是稀疏的點(diǎn)，我們把多幀激光點(diǎn)云組合起來得到一個(gè)稠密的深度圖。為了把特征和深度值關(guān)聯(lián)起來，我們把視覺特征和雷達(dá)點(diǎn)投影到以相機(jī)為圓心的單位圓上。然后對(duì)深度點(diǎn)進(jìn)行降采樣并利用極坐標(biāo)進(jìn)行保存保證點(diǎn)的密度是常數(shù)的。我們利用二維的KD-tree（極坐標(biāo)）來找視覺特征點(diǎn)周圍最近的三個(gè)點(diǎn)，最后特征點(diǎn)的深度計(jì)算的是相機(jī)中心和特征點(diǎn)的連線的長(zhǎng)度，這條線和三個(gè)深度點(diǎn)得到的平面在笛卡爾坐標(biāo)系中有交點(diǎn)?？梢暬缦聢Da所示，特征深度是虛線的長(zhǎng)度。

我們通過檢查和特征點(diǎn)最近的三個(gè)深度點(diǎn)之間的距離來進(jìn)一步驗(yàn)證關(guān)聯(lián)的特征深度。這是因?yàn)槎询B來自不同時(shí)間戳的激光雷達(dá)幀可能會(huì)導(dǎo)致來自不同對(duì)象的深度模糊。這種情況的圖示在圖3（b）中。在時(shí)間t_i處觀察到的深度點(diǎn)以綠色表示。相機(jī)在t_j移至新位置并觀測(cè)到了灰色的深度點(diǎn)。但是在t_i觀測(cè)到的被灰色線框起來的點(diǎn)，由于點(diǎn)云堆疊在t_j時(shí)刻可能還是能看到。所以使用來自不同對(duì)象的深度點(diǎn)來關(guān)聯(lián)特征深度會(huì)導(dǎo)致估算不準(zhǔn)確。與類似Laser-Visual-Inertial Odometry and Mapping with High Robustness and Low Drift論文中的做法一樣，我們通過檢查特征周圍深度點(diǎn)之間的最大距離，如果最大距離大于2m，就不對(duì)該特征點(diǎn)進(jìn)行數(shù)據(jù)關(guān)聯(lián)。

圖4顯示了對(duì)齊的深度圖和視覺特征。圖4的a和c，利用視覺里程計(jì)對(duì)齊點(diǎn)云得到深度圖投影到了圖像上。在圖像b和d上，通過深度圖恢復(fù)視覺特征的被標(biāo)記成了綠色。需要注意的是，雖然圖4a中深度點(diǎn)覆蓋了圖像的大部分區(qū)域，但是由于很多特征落在窗戶的角上導(dǎo)致深度關(guān)聯(lián)檢驗(yàn)失敗。

3）失敗檢測(cè)：如果運(yùn)動(dòng)變化劇烈，光照變化或者環(huán)境缺少紋理就會(huì)導(dǎo)致VIS系統(tǒng)失敗。這是由于在這種場(chǎng)景中跟蹤的特征點(diǎn)會(huì)顯著減少，而特征太少會(huì)導(dǎo)致優(yōu)化失敗。當(dāng)VIS系統(tǒng)失敗的時(shí)候會(huì)導(dǎo)致IMU的bias很大，所以當(dāng)VIS系統(tǒng)跟蹤的特征點(diǎn)少于一個(gè)閾值或者IMU估計(jì)出來的bias大于一個(gè)閾值的時(shí)候我們認(rèn)為VIS系統(tǒng)失敗。主動(dòng)失效檢測(cè)對(duì)于本文的系統(tǒng)是必需的，以便其故障不會(huì)影響LIS系統(tǒng)。一旦檢測(cè)到故障，VIS將重新初始化并通知LIS。

4）閉環(huán)檢測(cè)：本文利用DBoW2來做閉環(huán)檢測(cè)，對(duì)于每一個(gè)新來的圖像關(guān)鍵幀，檢測(cè)出BRIEF描述子并把他和原來檢測(cè)的描述子匹配。通過DBoW2檢測(cè)到的閉環(huán)候選幀的時(shí)間戳?xí)o到LIS系統(tǒng)來做進(jìn)一步的驗(yàn)證。

C.雷達(dá)慣導(dǎo)系統(tǒng)

如圖5所示，提出的雷達(dá)慣導(dǎo)系統(tǒng)是基于LIO-SAM的，主要是利用因子圖來做全局的位姿優(yōu)化。

主要有四種約束，IMU的預(yù)積分約束，視覺里程計(jì)的約束，雷達(dá)里程計(jì)的約束和閉環(huán)檢測(cè)的約束加入到因子圖中參與優(yōu)化。激光雷達(dá)里程計(jì)約束來自掃描匹配，在這里，我們將當(dāng)前的激光雷達(dá)關(guān)鍵幀與全局特征圖進(jìn)行匹配。閉環(huán)檢測(cè)的約束候選幀首先由VIS提供，然后通過掃描匹配進(jìn)一步優(yōu)化。我們?yōu)樘卣鲌D維護(hù)了一個(gè)激光雷達(dá)關(guān)鍵幀的滑動(dòng)窗口，這保證了有限的計(jì)算復(fù)雜性。當(dāng)機(jī)器人位姿變化超過閾值時(shí)，將選擇一個(gè)新的激光雷達(dá)關(guān)鍵幀。丟棄位于關(guān)鍵幀之間的普通激光雷達(dá)幀。選擇新的激光雷達(dá)關(guān)鍵幀后，新的機(jī)器人狀態(tài)x將作為節(jié)點(diǎn)添加到因子圖中。以這種方式添加關(guān)鍵幀，不僅可以在內(nèi)存消耗和地圖密度之間取得平衡，而且還有助于維護(hù)相對(duì)稀疏的因子圖以進(jìn)行實(shí)時(shí)優(yōu)化?？梢酝ㄟ^LIO-SAM的論文了解細(xì)節(jié)。在以下各節(jié)中，我們重點(diǎn)介紹提高系統(tǒng)魯棒性的新策略。

1）初始值估計(jì)：通過實(shí)驗(yàn)發(fā)現(xiàn)初始值對(duì)于連續(xù)的掃描匹配有著重要的作用，尤其是在劇烈運(yùn)動(dòng)的情況下。在LIS系統(tǒng)初始化之前和之后初值的來源是不同的。在LIS系統(tǒng)初始化之前，我們假設(shè)機(jī)器人在初始位置靜止，然后假設(shè)IMU的bias和噪聲都為0，對(duì)IMU原始值積分得到兩個(gè)雷達(dá)關(guān)鍵幀的位置和姿態(tài)信息作為scan-match的初值。經(jīng)過實(shí)驗(yàn)發(fā)現(xiàn)這種方法能在有挑戰(zhàn)的場(chǎng)景（初始速度小于10m/s，角速度小于180°/s）中初始化系統(tǒng)。一旦LIS系統(tǒng)初始化完成，我們估計(jì)因子圖中的IMU的bias，機(jī)器人的位姿，速度。然后把這些量傳到VIS系統(tǒng)中完成其初始化。

在LIS系統(tǒng)初始化完成之后，我們可以通過兩個(gè)途徑得到Scan-match的初值：IMU的積分和VIS系統(tǒng)。當(dāng)VIS系統(tǒng)的里程計(jì)可以輸出位姿時(shí)我們以他為初值，如果VIS系統(tǒng)報(bào)告失效的時(shí)候，我們利用IMU的積分作為初值。這個(gè)過程在紋理豐富和缺乏紋理的環(huán)境中都增加了初值的精度和魯棒性。

2）失敗檢測(cè)：盡管激光雷達(dá)可以得到場(chǎng)景中很遠(yuǎn)范圍內(nèi)的細(xì)節(jié)，但是也會(huì)在一些場(chǎng)景中導(dǎo)致掃描匹配失敗，如圖6所示。我們利用On Degeneracy of Optimization-based State Estimation Problems論文中介紹的方法來檢測(cè)LIS系統(tǒng)是不是失敗了。

scan-match的非線性優(yōu)化可以寫成一個(gè)線性最小二乘的形式：

其中A和b是從T處的線性化獲得的。當(dāng)$A^TA$的最小特征值小于第一次優(yōu)化迭代的閾值時(shí)，LIS報(bào)告失敗，此時(shí)不會(huì)將激光雷達(dá)里程計(jì)約束添加到因子圖中?？梢詮腛n Degeneracy of Optimization-based State Estimation Problems看到這些假設(shè)所基于的詳細(xì)分析。

實(shí)驗(yàn)

設(shè)備：十六線的Velodyne雷達(dá)、FLIR BFS-U3-04S2M-CS的單目相機(jī)、MicroStrain 3DM-GX5-25的IMU、Reach RS+ GPS、Intel i7-10710U in Ubuntu Linux

作者在三個(gè)自己錄制的數(shù)據(jù)集上進(jìn)行了測(cè)試都取得了很不錯(cuò)的效果。

備注：作者也是我們「3D視覺從入門到精通」特邀嘉賓：一個(gè)超干貨的3D視覺學(xué)習(xí)社區(qū)

本文僅做學(xué)術(shù)分享，如有侵權(quán)，請(qǐng)聯(lián)系刪文。

3D視覺工坊-VSLAM交流群

已建立3D視覺工坊-VSLAM微信交流群！想要進(jìn)VSLAM學(xué)習(xí)交流群的同學(xué)，可以直接加微信號(hào)：CV_LAB。加的時(shí)候備注一下：VSLAM+學(xué)校+昵稱，即可。然后就可以拉你進(jìn)群了。

強(qiáng)烈推薦大家關(guān)注3D視覺工坊知乎賬號(hào)和3D視覺工坊微信公眾號(hào)，可以快速了解到最新優(yōu)質(zhì)的3D視覺與SLAM論文。