這篇部落格

 這篇論文“Robust High Accuracy Visual-Inertial-Laser SLAM System”發表於2019年的 IROS 會議上。它提出了一個融合了相機、慣性元件、鐳射這三種感測器的系統。通過不同感測器之間的互補作用，該系統，較之於視覺慣性系統和鐳射系統，具有更好的魯棒性。

論文摘要

 純視覺SLAM在光照變化明顯、紋理較少的環境中容易跟蹤丟失，因此研究者將慣性測量元件(IMU)加入到視覺SLAM中，組成 VI-SLAM (視覺慣性SLAM)。但作者發現， IMU 只能在短期內解決純視覺SLAM 所面對的問題，如果機器長期工作在光照變化明顯、紋理缺失的環境中，系統仍會跟蹤丟失(因為IMU的偏差是隨機變化的，長時間不修正會直接影響位姿估計)。此外，作者還指出了鐳射SLAM的不足：在結構性特徵缺失的環境中（(比如在走廊)）會跟蹤丟失。所以作者將視覺慣性、鐳射兩類SLAM相結合，搭建了基於三種感測器工作的更加魯棒的系統。該系統可大致分為視覺慣性和鐳射兩大位姿估計模組。它先通過視覺慣性模組估計出位姿的初值，再根據鐳射掃描的結果完成位姿的優化。兩個模組可以來聯合工作，也可以各自獨立工作。這也是該系統魯棒的原因：當一個模組跟丟了，系統也可以只通過剩餘模組完成定位和建圖。

一些假設和標註

1、假設相機內參$K$已知，同時三個感測器(相機、IMU、鐳射)之間的外參矩陣（相對位姿變換關係）也已知；
2、預設這三個感測器在時間上是同步的！！(這點很重要)；
3、三個感測器都有各自的座標系，依次記為相機：$C$，IMU：$I$，鐳射：$L$。此外將世界座標系記為 $W$ 。在初始化後，將 $L$ 座標系作為主要觀測座標系（系統主要記錄3D點在 $L$ 上的座標）；
4、使用 $S_i$ 表示 $i$ 時刻的某個座標系。並用 $T_b^a$ 表示從 b 到 a 的位姿變換矩陣；
5、用 $X_j^S$ 表示地圖點 $j$ 在 $S$ 座標系上的三維齊次座標；
6、為了方便，下文用 VI 代表 “視覺慣性” 。

系統總覽

 系統先通過一個緊耦合的 VI 方法估計位姿，再通過鐳射掃描的結果來優化該估計值，最後再完成建圖。整個過程的框圖如下所示：

 下面介紹每個模組。

VI 里程計

 該系統的 VI 前端是基於 VINS-Mono 系統實現的，主要細節可檢視 VINS-Mono 的論文。

掃描匹配（scan matching）優化

 （這部分主要涉及鐳射定位，主要是 LOAM系統中的知識）掃描匹配（scan matching）模組的工作流程框圖如下所示：

 鐳射雷達會連續檢測到地圖點，並獲得其在對應時刻 $L_{t_i}$ 座標系下的座標。以第 $k$ 次鐳射掃描為例， 記 $t_k$ 為這次掃描的開始時間， $t_{k+1}$ 為結束時間。令 $\widetilde{P_k}$ 為第 $k$ 次掃描過程中檢測到的所有 3D 點的集合。因為所有感測器是時間同步的，同時 $t_k$、$t_{k+1}$ 時刻機器的位姿已由 VI 里程計估計出，所以可通過 IMU 與鐳射感測器間的外參矩陣估計出 $L_k$、 $L_{k+1}$ 間的位姿關係，如下式：

式中 $T_I^L$ 是 IMU 與鐳射感測器間的外參矩陣， $\widetilde{T_{I_{k+1}}^{I_k}}$ 是 VI 里程計估計的結果。

 此時要引入一個假設：在 $t_k$、$t_{k+1}$間，機器以恆定的速度運動。此假設使得我們能通過線性插值的方法獲得 $t_i\in [t_k,t_{k+1}]$ 時刻，$L_i$ 與$L_k$間的位姿關係，如下所示：

 通過上式計算的位姿，可以將任意 $t_i$ 時刻檢測到的點都轉換到 $L_{k+1}$ 座標系上表示：

此時，將轉換後所有在 $L_{k+1}$ 座標系中的點的集合記為 $P_k$。

 根據 $P_k$ 中各點區域性表面的平滑程度，決定某點屬於邊緣特徵點或平面特徵點。將提取出來的特徵點的集合記為 $F_k$ 。此時，再根據已經通過優化得出的 $L_k$ 與世界座標系的相對位姿結果 $T_{L_k}^W$ 和 公式 (1) ，推出 $L_{k+1}$ 與世界座標系之間的相對位姿估計值 $\widetilde{T_{L_{k+1}}^W}$ ：

 通過 公式(4) 求得的 $\widetilde{T_{L_{k+1}}^W}$ 將 $F_k$ 包含的所有特徵點都投影到世界座標系中，併為它們在已構建好的地圖中尋找相匹配的線邊緣、平面塊等結構特徵（由此就建立了 $L_{k+1}$ 與地圖的聯絡。根據這個聯絡就能構建出圖優化的邊）。然後計算 $F_k$ 中特徵點到相匹配的線邊緣或平面塊的距離 $d$ 。這個過程可以用下面的函式 $f$ 表示：

（PS:特徵點尋找匹配的邊緣線和平面塊的方法，以及距離的計算方法可在 “LOAM: Lidar odometry and mapping in real time” 論文中檢視）
 將所有 $d_i$ 相加，即為需要優化的變數：

 由此，鐳射的掃描匹配（scan matching）的結果建模出了一個非線性優化問題。通過牛頓梯度下降的方法使 $d$ 的值趨近於 0 （因為理論上特徵點到與其匹配的線或面的距離應該為 0）。如果結果能夠收斂，則能獲得一個關於鐳射感測器的優化後位姿：$T_{L_{k+1}}^W$。最後通過 $T_{L_{k+1}}^W$ 將第 $k$ 次鐳射掃描獲得的點雲圖轉換到世界座標系中，更新地圖。
（個人想法：從系統的流程可以看出，系統對於 VI 模組和鐳射掃描匹配模組採用的是鬆耦合的方式（先 VI 估計再掃描匹配優化）。因此鐳射模組優化的效果比較依賴於 VI 的估計初值。這或許能通過緊耦合的方式來改善，但會提高系統的複雜度且降低各模組的靈活性）

提高系統魯棒性的措施

 如最初在摘要中所說的，視覺慣性SLAM 和鐳射SLAM 在某些環境下都會存在跟蹤丟失的風險。所以作者構建的這個系統主要是由兩個獨立性較強的模組（VI 位姿估計模組和鐳射掃描匹配模組）組成的。在某個模組失效後，另一個能夠獨立工作，以保證系統的正常執行。這就是該系統具有較高魯棒性的原因。
 根據系統工作時所使用的模組的不同，可分成以下三種工作模式：
（1）正常工作模式：
  此時 VI 和鐳射 模組都正常工作，系統按照前幾節描述的流程完成定位和建圖；
（2）scan to scan matching 工作模式：
  此時 VI 模組跟蹤丟失，啟動 “兩次鐳射掃描間的匹配（scan to scan matching）” 定位模組（自己取的名字，見笑見笑）。這個模組是基於 LOAM 系統構造的，它的大致工作過程和上一小節中的 “掃描匹配（scan matching）優化” 相似，只是這裡估計的是 $L_k$ 和$L_{k-1}$ 之間的位姿 $T_{L_{k-1}}^{L_k}$ ，而不是 $T_{L_k}^W$。此模式下系統的工作流程圖如下所示：

（PS：當發生以下幾種情況時，系統會認定 VI 模組失效：當前跟蹤到的特徵點很少、IMU 偏差變化較大、VI 滑動視窗中的位姿估計值與先前估計的結果偏差較大）
（3）僅使用 VI 工作模式：
  此時系統僅使用 VI 模組完成定位和建圖，流程圖如下所示：

（PS：此模式主要是在結構資訊缺失的環境中使用）
 這三種工作模式使得系統能在多種不同的、難度較高的環境下繼續工作。同時，當環境條件變好後，系統仍會重新啟用之前失效的模組，變回正常的工作模式。
（疑惑：以第三個模式為例，當鐳射模組重新工作時，由 VI 模組構建的地圖作為鐳射模組的初始地圖，還是鐳射模組會重新構建新的地圖？即 VI 和鐳射模組兩者所構建的地圖是否能夠互相使用。）

閉環檢測和臨近檢測

 老規矩，為了消除累積誤差，系統需要有閉環檢測的能力。因為當前系統搭載了視覺、鐳射感測器，所以它實現閉環檢測的方式有兩種：
 1、基於視覺和詞袋向量的閉環檢測，也就是最經典的視覺閉環檢測方法；
 2、基於鐳射雷達的臨近檢測。
作者指出，第一種方法存在一定缺點：必須觀測到相同物體，才能檢測到閉環(對機器的觀測視角有要求)。而第二種方法則沒有這個限制，因為鐳射雷達是 360° 掃描的。為了避免過多的約束和計算，某個關鍵幀( KF )在實現視覺閉環檢測後，將不再進行臨近檢測。
 閉環檢測想必大家都較為了解，所以這裡主要介紹一下臨近檢測（這部分內容也是第一次看，理解不對之處望諒解和指出），它的過程圖如下：

 首先在 KF 資料庫中計算各 KF 與當前關鍵幀( $K{F}_c$)在世界座標系下的相對距離（即二者三維座標的距離）。記所有與 $K{F}_c$ 距離小於 $R_1$ 關鍵幀的集合為 $KF{s}_{loop}$ 。以 $KF{s}_{loop}$ 中的一個關鍵幀（$K{F}_a$）為例：
 在 $K{F}_c$ 和 $K{F}_a$ 已有的估計位姿基礎上，求出兩者間的相對位姿 $T_{K{F}_c}^{K{F}_c}$ ，再根據先前提到的 “兩次鐳射掃描間的匹配方法” ，優化 $T_{K{F}_c}^{K{F}_c}$ 。此時如果 $K{F}_c$ 和 $K{F}_a$ 之間的相對距離小於 $R_2$，則兩幀滿足臨近檢測要求。
（PS：每個關鍵幀包含機器的位姿、對應圖片的特徵點和從鐳射掃描中獲得的幾何特徵點資訊）
 對 $K{F}_c$ 和 $KF{s}_{loop}$ 中每個 KFs 都進行如上操作。如果有多個 KFs 滿足臨近檢測要求，則只選擇產生時間最早的那個 KF 與 $K{F}_c$ 構成閉環（因為這樣可以獲得較大的閉環，有利於消除更多的累積誤差）。此外，假設 $K{F}_c$ 的編號為 $k$ ，那麼編號為 $k-e,...,k-1$的 KF 不會參與臨近檢測的判定（ e 的值根據經驗設定）。這樣可以降低計算量。
（個人理解：從論文的實驗結果來看，同時使用視覺閉環和臨近檢測與只使用其中之一相比，最終精度的差距不是很大。但是為了保證系統的兩個模組的獨立性，這兩個檢測方法需要要同時存在。）

全域性位姿圖優化

 當完成閉環產生後，系統將執行全域性位姿圖優化。圖中 KF 作為頂點，它包含三種邊：
1、相鄰 KF 之間的連續邊，這個邊計算的是相鄰 KF 間的相對平移和旋轉變換：

2、閉環檢測邊。這個邊計算的是閉環 KF 之間的相對平移和旋轉變換，使用的是 PnP方法。
3、臨近檢測邊。和閉環邊類似，只不過採用的是 “兩次鐳射掃描間的匹配方法”。
 所以，圖中頂點 $i$ 和 $j$ 之間的誤差可表示為：

（個人理解：式中減號左邊的項可能由 IMU 預積分獲得，因為 IMU 測量值的預積分結果可能更接近於真值）
整個優化圖中的總誤差為：

式中$A、B、C$ 分別對應上述 1、2、3 類邊。這個非線性優化問題通過 Ceres Solver解決。為了降低計算量，系統會限制 KF 資料庫中關鍵幀的數量。構成閉環的 KFs 會被保留，而那些與自己鄰近 KFs 距離較近的關鍵幀將被剔除。

總結

 這個系統應該是多感測器融合 SLAM 中較簡單的一個實現方法。它將視覺慣性和鐳射匹配兩種位姿估計方法進行鬆耦合（先 VI 估計，再鐳射匹配優化），讓兩個不同、相對獨立的模組共同完成機器的位姿估計。這使得系統的工作流程更為清晰，降低了複雜度，同時也保證系統能夠靈活地對切換工作模式，以應對不同的環境條件。此外，系統在閉環檢測時採用的兩種不同方法，也能提高整體精度。