泡泡點雲時空，帶你精讀點雲領域頂級會議文章
標題：
1。 Fast-lio： A fast， robust lidar-inertial odometry package by tightly-coupled iterated kalman filter
2。 Fast-lio2： Fast direct lidar-inertial odometry。
作者：
1。 Wei Xu and Fu Zhang。
2。 Wei Xu， Yixi Cai， Dongjiao He， Jiarong Lin， Fu Zhang
1。 RAL 2021
2。 TRO 2022
編譯：曹明
稽核：Lionheart，志勇
這是點雲時空推送的第260篇推文，歡迎個人轉發朋友圈；其他機構或自媒體如需轉載，後臺留言申請授權，同時歡迎留言交流，大家的支援和鼓勵是我們前進的動力。
創新點
Fast-LIO的主要貢獻在於，儘管它的問題構建框架與LINS（LINS： A Lidar-Inertial State Estimator for Robust and Efficient Navigation） 相似，均為Error-state iKF，但其
反向傳播
過程能夠有效緩解點雲畸變；此外，其
重新推導了一種求卡爾曼增益的方式
，使得其計算的複雜度依賴於狀態的維度而不是觀測的維度。這就使其能夠使用更多的特徵點構建問題並且求解得更快，從而使其效率與精度均優於LINS。
Fast-LIO2則是一個更加完整的系統。在Fast-LIO的基礎上，其提出了
增量式的kd-Tree（ikd-Tree）
，使得kd-Tree能夠增量式的刪除或新增電雲。得益於ikd-Tree，Fast-LIO2不再是類似LOAM般的提取edge特徵與plane特徵，而是
直接將每個三維點與地圖配準
。因此，其能夠較穩定地執行在一些較難提取手工特徵的場景中。
整體框架
這裡以Fast-LIO2的流程為例（Fast-LIO1沒有iKD-Tree的部分，並且提取特徵點而不是利用所有點進行觀測）。首先利用IMU積分對狀態進行前項傳播，獲得當前時刻狀態的預測，並且透過反向傳播對當前幀點雲進行畸變去除。隨後對去畸變的每一個點，構建觀測模型、計算殘差與雅克比，最後透過迭代卡爾曼濾波融合輸出定位結果。其地圖形式是iKD-Tree，會根據估計的狀態將每一幀點雲加入到KD-Tree中，並且自動刪除規定地圖大小外的點雲。
論文關鍵點介紹
Fast-LIO相關翻譯已經在泡泡機器人中有過推送，這裡嘗試給出Fast-LIO & Fast-LIO2的論文中的關鍵部分的個人理解與看法。主要分為以下幾個關鍵點：
誤差狀態迭代卡爾曼濾波器（Error-State Kalman Filter）（Fast-LIO&Fast-LIO2）。
反向傳播（Fast-LIO&Fast-LIO2）。
卡爾曼增益的加速求解（Fast-LIO&Fast-LIO2）。
IKD-Tree（Fast-LIO2）。
直接法配準（Fast-LIO2）。
1. 誤差狀態迭代卡爾曼濾波器
自LINS首次運用誤差狀態迭代卡爾曼濾波器到緊耦合的鐳射雷達IMU里程計中後，誤差狀態迭代卡爾曼濾波器逐漸成為近來的多感測器SLAM框架的正規化（LINS， Fast-LIO系列以及R2LIVE系列）。這裡嘗試回答兩個問題：
a. Error-State Kalman filter VS Extend Kalman filter
EsKF 通常在求解的時候相較於EKF能夠得到更好的結果。我個人的理解是EKF嘗試透過線性化來近似名義狀態的變化，而EsKF則嘗試透過線性化來近似誤差狀態的變化，如下圖所示。
正如上圖所述，由於誤差狀態的變化的線性度比名義狀態好很多，因此其一階偏導的近似也更好，F函式近似得更好，自然協方差也傳播得更好。
體現在數學公式上，EKF的離散時間狀態轉移公式為：
該系統是個非線性系統，在傳播的時候需要用F（x） （f（x）的導數）來對f（x）求近似。
而ESKF的離散時間狀態轉移公式為
是一個線性時變系統，因為F（t）與δx（t）無關。
這就是論文“Extended Kalman filter vs。 error state Kalman filter for aircraft attitude estimation”的觀點，其說明了EKF的system model是個非線性系統，而EsKF的system model是個線性時變系統，這導致了後者的系統線性程度比前者更高，因此狀態量的噪聲的協方差傳播得更好。
b. 迭代卡爾曼濾波與高斯牛頓法的等價性。
遊大佬已經在知乎中解釋了，詳情請見知乎專欄。這裡套用其結論為：
“這裡的證明可以說明IEKF和高斯牛頓具備相同的問題求解能力，如果能構造出一個更好的problem structure，那使用filter based solver一樣可以得到很好的結果。”
2. 反向傳播
鐳射雷達由於每幀點雲都有不可忽視的採集時間（例如10Hz釋出的點雲，每幀採集時間就為0。1s），因而每個三維點都有屬於其自身的採集的時間戳。如果僅用點雲的時間戳（通常為一幀最後一個點的時間戳）的狀態來將每個三維點轉換到世界座標系中，則因為時間戳的不匹配而引入了點雲畸變。在Fast-LIO及續作的框架中，前項傳播透過積分IMU的測量，從而更新當前的狀態。而Fast-LIO的反向傳播就是為了進行運動補償從而消除運動畸變，其流程為根據最後一個點的機器人狀態與倒數第二個點的時間戳，倒推倒數第二個點時機器人的狀態，再根據倒數第二個點倒推倒數第三個點，一直到所有點被採集時機器人的狀態都能獲得。
3. 卡爾曼增益的加速求解
在卡爾曼濾波中，卡爾曼增益通常是透過下式獲得：
其中P為狀態的協方差，H為觀測模型的Jacobian矩陣，R為輸入的協方差。該式涉及到對矩陣HPH^+R取逆，而該矩陣的維度等同於觀測的數量（FastLIO中為特徵點的數量，FastLIO2中為所有點的數量）。這導致求逆的開銷大到難以承受。Fast LIO中提到，透過直接求解其構建的最大後驗估計問題（MAP），能夠將協方差的增益轉換為以下形式：
矩陣H^TR^-1H+P^-1的維度就與狀態有關，則求逆的開銷便大大降低了。Fast-LIO的附錄中提到，利用矩陣求逆定理（SMW恆等式），上述兩個式子可以互相轉換。
4. IKD-Tree
基於鐳射的SLAM在做特徵關聯的時候，通常都是要查詢上一幀或者區域性地圖裡的最近鄰點。所使用的資料結構通常是KD-Tree。然而原始的KD-Tree版本不支援增量式的更新，即對當前幀點雲構建KD-Tree後，要向該幀點雲中新增新的點，只能新增完後重新構建一個新的KD-Tree，增加了額外的計算開銷。為此，Fast-LIO2提出了i（ncremental）KD-Tree，能夠增量式的向KD-Tree中新增點與刪除點。
其關鍵的思想是為KD-Tree裡面的每個節點添加了一個屬性，用以判斷以該節點為根節點的子樹是否平衡。判斷一顆樹是否不平衡的條件為：
左子樹或右子樹的節點的數量小於所有節點的數量* a。其中a為預先設定的比例。
或者子樹中被標記為刪除的節點的數量大於子樹所有節點數量 * b。其中b為預先設定的比例。
在新增一個三維點到iKD-Tree後，會自底向上地找到最後一個不平衡的節點，然後將以該節點為根節點的子樹裡的所有三維點提取出來，重新構建一個平衡的KD-Tree。由此，保證了重建的操作僅在小規模的子樹內進行，從而減小了計算量。
5. 直接法配準
由於利用iKD-Tree節省了計算量，Fast-LIO2可以不再只用部分的特徵三維點（edge或者plane）來進行配準，而是用整個點雲（經過降取樣後）的所有點來進行配準。Fast-LIO2認為，空間中的區域都應當是連續的，每個三維點在一個足夠小的半徑內都可以視為一個平面。因此，對於單個點，其配準的觀測模型為：
其中，u為該平面的法向量，q為該平面的另外一點，n為該點的測量噪聲。該觀測模型的思想與一些經典的ICP演算法，例如點到面的ICP比較接近。
Fast-LIO2認為，部分點由於其曲率不明顯，因此不能被提取為特徵點，但這些點仍能夠作為觀測來約束位姿，因此將這些點加入到問題的構建中也能夠更好地提升精度與防止里程計退化。
注意到Fast-LIO2做實驗的時候用的是LIVOX固態鐳射雷達，該鐳射雷達視場角較小，比較容易因為快速運動而出現特徵匹配困難的問題，因此推測這也是Fast-LIO2嘗試將所有點加入到問題構建的原因之一。
程式碼流程與關鍵點解析