檢測器初始化失敗是什麼意思
我相信很多人對鐳射視覺慣導融合的系統都是這樣設計的，但是最難的是把自己的想法保質保量的實現出來。我們做不到但是大佬可以！
注1：文末附【VSLAM】交流群
注2：整理不易，請點贊支援！
作者：Liam | 來源：3D視覺工坊微信公眾號
摘要
本文提出了一個緊耦合的雷達視覺慣導SLAM系統，可以實時高精度魯棒的進行狀態估計和建圖。LVI-SAM構建在因子圖之上，並且由兩個子系統組成：一個視覺慣導系統和一個鐳射慣導系統。這兩個子系統利用緊耦合的方法，視覺慣導的系統利用鐳射慣導的估計來做初始化。視覺慣導系統利用鐳射雷達的測量給視覺特徵提供深度來提升精度。同時鐳射慣導系統利用視覺慣導的估計值作為初值來做幀的匹配。利用視覺做閉環檢測，把檢測的結果給鐳射慣導系統來做最佳化。當鐳射慣導和視覺慣導子系統的一個失敗的時候LVI-SAM仍然可以工作，這極大的提升了系統在缺少紋理或者特徵場景中的魯棒性。LVI-SAM系統在資料集上進行了測試，取得了很好的效果。
介紹
基於雷達的SLAM系統可以獲取很長範圍的環境的細節，但是在缺少結構資訊的場景中很容易失效。例如長走廊或者是開闊的廣場。基於視覺的方法在紋理資訊豐富的場景中表現良好，而且很容易做場景重識別。但是他們對光照的變化，快速運動和初始化很敏感。因此，鐳射和視覺經常和慣導融合來提升他們的精度和魯棒性。慣導可以幫助去除點雲的運動畸變，而且在缺少特徵的環境中撐一小段時間，同時可以幫助視覺系統恢復尺度資訊。
本文提供了一個緊耦合雷達視覺慣導里程計，LIS系統和VIS系統可以獨立的執行（當一個系統失敗的時候），也可以耦合在一起（當檢測到很多特徵點的時候）。視覺慣導系統執行利用雷達幀恢復深度的視覺特徵的跟蹤，透過最佳化視覺重投影誤差和imu測量誤差的視覺里程計為鐳射雷達的scan-matching提供初值，並且把約束加入到因子圖中。當利用imu把點雲的畸變去除後，鐳射慣導系統檢測點雲的邊緣和平面特徵，並和儲存在劃窗中的features map對齊。LIS系統估計得到的系統狀態可以傳到VIS系統中做初始化。對於閉環，候選的匹配幀透過視覺的詞袋模型得到，在LIS系統中做最佳化。來自視覺里程計，鐳射里程計，imu預積分和閉環的約束都會放到因子圖中，最後，最佳化得到的IMU的bias用來遞推給出IMU頻率的位姿。本文的主要貢獻有：
基於因子圖的緊耦合的LVIO系統，實現了多感測器融合與基於場景重識別的全域性最佳化；
透過故障檢測來繞過失敗的子系統，使其對感測器效能下降具有魯棒性；
利用不同的資料集進行了完善的驗證。
從系統的角度來看，作者的工作是新穎的，代表了VIO和LIO中最新技術的整合，實現了高精度的LVI系統，作者希望大家可以以此為基礎，推動鐳射慣導視覺融合系統的發展。
完整的鐳射視覺慣導SLAM系統
A。 系統概述
系統接受3D鐳射點雲、單目影象和IMU資訊作為輸入。VIS系統接受影象和IMU資訊，雷達點雲是可選的輸入。透過最小化IMU和視覺的測量殘差來得到視覺里程計。鐳射里程計透過最小化檢測到的線面特徵到特徵圖的距離得到。特徵圖儲存在一個滑窗中來實時的執行。最後狀態估計問題，可以公式化為一個MAP問題。利用iSAM2來最佳化因子圖中IMU預積分，視覺里程計，鐳射里程計和閉環的約束的殘差。需要注意的是，LIS中採用的多感測器圖最佳化旨在減少資料交換並提高系統效率。
B。 視覺慣導系統
視覺慣導的pipeline如圖2所示。
視覺特徵利用角點檢測器，利用KLT演算法進行跟蹤。我們利用視覺里程計對齊鐳射幀的點雲得到一個稀疏的深度圖來得到特徵的深度完成視覺慣導初始化。系統在滑窗中執行BA，其中狀態X可以寫成：
R是SO3表示旋轉矩陣，p表示位置，v表示速度，b=［b_a， b_w］是imu的偏置。變換矩陣T是SE3表示從機體座標系到世界座標系的變換 。下邊將詳細的介紹VIS的初始化和特徵深度估計。讀者們可以讀VINS-Mono的原文來了解細節。
1）初始化：基於最佳化的VIO系統由於很強的非線性導致初始化發散。初始化的質量取決於兩個主要因素：初始化的感測器運動和IMU引數的精度。在實際的實驗中，我們發現VINS很容易在系統速度很小或者勻速的時候初始化失敗。這是由於加速度激勵不夠大導致尺度不可觀。IMU的引數包含漸變的bias和高斯白噪聲。他影響這加速度和角速度的測量，在初始化的時候好的初值可以幫助系統快速的最佳化。
為了改善VIS初始化的魯棒性，我們利用LIS系統來估計系統的狀態X和IMU的偏置b。因為直接可以透過雷達的觀測得到深度資訊。我們首先初始化LIS系統獲得x和b。然後我們透過插值來把鐳射幀和視覺幀透過時間戳對齊。假設IMU的偏置在兩個圖象關鍵幀之間是常數。最後把LIS系統初始化得到的x和b作為VIS初始值，這很有效的改善初始化的速度和魯棒性。
2）深度特徵關聯：根據VIS初始化的結果，我們利用視覺里程計對齊視覺幀和雷達幀。由於當前的3D lidar掃描的是稀疏的點，我們把多幀鐳射點雲組合起來得到一個稠密的深度圖。為了把特徵和深度值關聯起來，我們把視覺特徵和雷達點投影到以相機為圓心的單位圓上。然後對深度點進行降取樣並利用極座標進行儲存保證點的密度是常數的。我們利用二維的KD-tree（極座標）來找視覺特徵點周圍最近的三個點，最後特徵點的深度計算的是相機中心和特徵點的連線的長度，這條線和三個深度點得到的平面在笛卡爾座標系中有交點。視覺化如下圖a所示，特徵深度是虛線的長度。
我們透過檢查和特徵點最近的三個深度點之間的距離來進一步驗證關聯的特徵深度。這是因為堆疊來自不同時間戳的鐳射雷達幀可能會導致來自不同物件的深度模糊。這種情況的圖示在圖3（b）中。在時間t_i處觀察到的深度點以綠色表示。相機在t_j移至新位置並觀測到了灰色的深度點。但是在t_i觀測到的被灰色線框起來的點，由於點雲堆疊在t_j時刻可能還是能看到。所以使用來自不同物件的深度點來關聯特徵深度會導致估算不準確。與類似Laser-Visual-Inertial Odometry and Mapping with High Robustness and Low Drift論文中的做法一樣，我們透過檢查特徵周圍深度點之間的最大距離，如果最大距離大於2m，就不對該特徵點進行資料關聯。
圖4顯示了對齊的深度圖和視覺特徵。圖4的a和c，利用視覺里程計對齊點雲得到深度圖投影到了影象上。在影象b和d上，透過深度圖恢復視覺特徵的被標記成了綠色。需要注意的是，雖然圖4a中深度點覆蓋了影象的大部分割槽域，但是由於很多特徵落在窗戶的角上導致深度關聯檢驗失敗。
3）失敗檢測：如果運動變化劇烈，光照變化或者環境缺少紋理就會導致VIS系統失敗。這是由於在這種場景中跟蹤的特徵點會顯著減少，而特徵太少會導致最佳化失敗。當VIS系統失敗的時候會導致IMU的bias很大，所以當VIS系統跟蹤的特徵點少於一個閾值或者IMU估計出來的bias大於一個閾值的時候我們認為VIS系統失敗。主動失效檢測對於本文的系統是必需的，以便其故障不會影響LIS系統。一旦檢測到故障，VIS將重新初始化並通知LIS。
4）閉環檢測：本文利用DBoW2來做閉環檢測，對於每一個新來的影象關鍵幀，檢測出BRIEF描述子並把他和原來檢測的描述子匹配。透過DBoW2檢測到的閉環候選幀的時間戳會給到LIS系統來做進一步的驗證。
C。雷達慣導系統
如圖5所示，提出的雷達慣導系統是基於LIO-SAM的，主要是利用因子圖來做全域性的位姿最佳化。
主要有四種約束，IMU的預積分約束，視覺里程計的約束，雷達里程計的約束和閉環檢測的約束加入到因子圖中參與最佳化。鐳射雷達里程計約束來自掃描匹配，在這裡，我們將當前的鐳射雷達關鍵幀與全域性特徵圖進行匹配。閉環檢測的約束候選幀首先由VIS提供，然後透過掃描匹配進一步最佳化。我們為特徵圖維護了一個鐳射雷達關鍵幀的滑動視窗，這保證了有限的計算複雜性。當機器人位姿變化超過閾值時，將選擇一個新的鐳射雷達關鍵幀。丟棄位於關鍵幀之間的普通鐳射雷達幀。選擇新的鐳射雷達關鍵幀後，新的機器人狀態x將作為節點新增到因子圖中。以這種方式新增關鍵幀，不僅可以在記憶體消耗和地圖密度之間取得平衡，而且還有助於維護相對稀疏的因子圖以進行實時最佳化。可以透過LIO-SAM的論文了解細節。在以下各節中，我們重點介紹提高系統魯棒性的新策略。
1）初始值估計：透過實驗發現初始值對於連續的掃描匹配有著重要的作用，尤其是在劇烈運動的情況下。在LIS系統初始化之前和之後初值的來源是不同的。在LIS系統初始化之前，我們假設機器人在初始位置靜止，然後假設IMU的bias和噪聲都為0，對IMU原始值積分得到兩個雷達關鍵幀的位置和姿態資訊作為scan-match的初值。經過實驗發現這種方法能在有挑戰的場景（初始速度小於10m/s，角速度小於180°/s）中初始化系統。一旦LIS系統初始化完成，我們估計因子圖中的IMU的bias，機器人的位姿，速度。然後把這些量傳到VIS系統中完成其初始化。
在LIS系統初始化完成之後，我們可以透過兩個途徑得到Scan-match的初值：IMU的積分和VIS系統。當VIS系統的里程計可以輸出位姿時我們以他為初值，如果VIS系統報告失效的時候，我們利用IMU的積分作為初值。這個過程在紋理豐富和缺乏紋理的環境中都增加了初值的精度和魯棒性。
2）失敗檢測：儘管鐳射雷達可以得到場景中很遠範圍內的細節，但是也會在一些場景中導致掃描匹配失敗，如圖6所示。我們利用On Degeneracy of Optimization-based State Estimation Problems論文中介紹的方法來檢測LIS系統是不是失敗了。
scan-match的非線性最佳化可以寫成一個線性最小二乘的形式：
其中A和b是從T處的線性化獲得的。當$A^TA$的最小特徵值小於第一次最佳化迭代的閾值時，LIS報告失敗，此時不會將鐳射雷達里程計約束新增到因子圖中。可以從On Degeneracy of Optimization-based State Estimation Problems看到這些假設所基於的詳細分析。
實驗
裝置：十六線的Velodyne雷達、FLIR BFS-U3-04S2M-CS的單目相機、MicroStrain 3DM-GX5-25的IMU、Reach RS+ GPS、Intel i7-10710U in Ubuntu Linux
作者在三個自己錄製的資料集上進行了測試都取得了很不錯的效果。
備註：
作者也是我們
「3D視覺從入門到精通」
特邀嘉賓：
一個超乾貨的3D視覺學習社群
本文僅做學術分享，如有侵權，請聯絡刪文。
3D視覺工坊-VSLAM交流群
已建立3D視覺工坊-VSLAM微信交流群！想要進VSLAM學習交流群的同學，可以直接加微訊號：CV_LAB。加的時候備註一下：VSLAM
+學校+暱稱
，即可。然後就可以拉你進群了。
強烈推薦大家關注
3D視覺工坊知乎
賬號和
3D視覺工坊
微信公眾號，可以快速瞭解到最新優質的3D視覺與SLAM論文。