近日，智行者高翔博士帶領的定位團隊撰寫的論文《Faster-LIO: Lightweight Tightly Coupled Lidar-inertial Odometry using Parallel Sparse Incremental Voxels》被國際公認的自動駕駛領域TOP級期刊IEEE Robotics and Automation Letters收錄刊登。

該文章主要對激光雷達算法進行了深入探討，本文提出了一種基于iVox(incremental voxels)的算法，以快速跟蹤旋轉的激光雷達-慣性里程計(LIO)方法固態激光雷達掃描。在該算法中，智行者定位團隊使用iVox作為點云空間數據結構，即從傳統的體素修改，支持增量插入和并行近似k-NN查詢。該算法可以有效的降低點云配準時的耗時，也不會影響LIO的精度表現。

干貨內容如下：

前言

眾所周知SLAM現在越來越卷了。卷來卷去，大概有幾種卷的方向：

一是卷精度。然而同樣傳感器做成的數據集大體來說不會有數量級上的精度差異，大部分論文都會說在某個數據集上得到了百分之幾的精度提升，然而原因比較玄學，不好歸因，實際當中也不一定看的出來。

二是卷魯棒性。魯棒性倒是非常實在，別人跑丟了的數據我跑成功了，魯棒性自然就好。不過開源的數據集相比真實數據來說九牛一毛。開源數據集有十來個數據就顯得不錯了，實際當中往往是幾百幾千的車輛和機器人在跑，數據集那幾個包才哪到哪，能體現的魯棒性指標很有限。

三是卷效率。效率也是實打實能看到的。別人算100ms，我算50ms，那效率就實實在在地快了一倍。別人要工控機，我用嵌入式;別人占滿CPU，我跑一半的CPU，那整個系統就更流暢絲滑。而且卷效率沒那么玄學，哪哪算的快了都可以找到原因，和數據集關系也不大。這就是我們這次選擇卷效率的一個理由。

Lidar-inertial odometry (LIO)是SLAM這邊卷的還不那么厲害的方向之一。純Lidar的SLAM已經比較穩定了，幾個開源方案(cartographer, Loam系列)跑得也挺順，雖然大體上要慢一些。不過到了LIO，你會發現一些神奇的現象：開源的LIO大部分只能在自己的數據集上跑，換一個數據集就很容易飛或者掛。前期的方案考慮的東西太少，在后出的數據集上通常會出問題。近期的方案則相對要穩定一些，但依然沒有純Lidar方案那么穩定，各種指標也有一定的提升空間。

本文要談的Faster-LIO是基于FastLIO2開發的。FastLIO2是開源LIO中比較優秀的一個，前端用了增量的kdtree(ikd-tree)，后端用了迭代ESKF(IEKF)，流程短，計算快。Faster-LIO則把ikd-tree替換成了iVox(后文介紹)，順帶優化了一些代碼邏輯，實現了更快的LIO。我們在典型的32線激光雷達中可以取得100-200Hz左右的計算頻率，在固態雷達中甚至可以達到1000-2000Hz，能夠達到FastLIO2的1.5-2倍左右的速度。當然具體數值和計算平臺相關。讀者也可以用自己的平臺測試一下Faster-LIO在你機器上的表現。

介紹

我們就不聊LIO在什么自動駕駛或者無人機上的應用背景之類的話題了，直接切入主題。

大體來講，LIO系統的整個計算流程是比較固定的：它們從IMU中得到一個粗略的估計，然后把雷達的數據與一些歷史數據做配準，最后用某種狀態估計算法進行濾波或者優化。IMU部分的處理差別不大，所以LIO系統的計算效率主要與點云算法和后端算法相關，我們大致分三個方面：

點云最近鄰的數據結構。點云配準的基本問題是計算給定點與歷史點云的最近鄰，通常需要依賴一些最近鄰的數據結構。這些數據結構又大體分為樹類的(tree like)和體素類(voxel like)的。廣義的，高維的最近鄰問題是一個比較復雜的問題，但LIO里的最近鄰則是低維的、增量式的問題。于是，像R*樹、B* 樹等靜態的數據結構并不是非常適合LIO。FastLIO2里提出使用增量式的kdtree來處理最近鄰，我們則認為增量的體素更適合LIO系統。

點云殘差的計算方式。自動駕駛里普遍偏向不直接使用點到點的殘差，而是使用點到線或點到面的殘差。點到點的殘差形式雖然簡單，但雷達點云和RGBD點云相比，更加稀疏，在車輛運動過程中不見得都能打到同一個點，而且點云也往往會被降采樣后再進行處理，所以并不太適合在自動駕駛中使用。LOAM系列會計算點云特征，實際當中特征提取要花的時間是不可忽略的，甚至是主要計算部分。

狀態估計算法的選型。LIO和VIO中普遍會使用介于單幀EKF和批量優化之間的方案，例如IEKF、MSCKF、Sliding Window Filter等等。這其中又以IEKF算是最簡單有效的一類，既有迭代來保證精度，又不需要像預積分系統那樣算一堆雅可比矩陣。

所以這樣一看，能進一步挖掘的地方主要是LIO的近鄰結構。Kd樹類結構的優勢在于，可以嚴格地查詢K近鄰，不會多也不會少;也可以以范圍或盒子形式來查詢最近鄰(range search/box search)。查詢過程中也以設置附加條件比如最大距離，實現快速的近似最近鄰查找(Aproximate Nearest Neighbor, ANN)。然而傳統的Kd樹是不帶增量結構的。像ikd-tree這種帶增量加點的結構，雖然不用完全重新構建，但也需要花額外的時間去維護這個樹的結構。我們不禁要問：LIO里真的需要嚴格的K近鄰搜索嗎?能不能放寬一點，使用更簡單的結構?

實際上點云配準往往不需要嚴格的K近鄰。如果K近鄰找到了一個很遠的點，拿這個點過來做點面殘差也是不合理的，這部分計算就是無效的。我們不妨讓近鄰結構本身就具有這種查找范圍限制，而kd樹即使帶了范圍限制，也需要一個節點一個節點地來遍歷，這種遍歷顯然也是會耗時的。于是我們來考慮一種基于體素的近鄰結構。考慮到點云的稀疏性，我們希望體素也能夠以稀疏的形式存儲。體素具備一些天然的優點：一是天然具有K近鄰時的范圍限制;二是增量構建的時候不需要額外的操作，刪除的時候也很方便;三是最近鄰的范圍也可預先定義，想多搜點就多搜點，想快點就少搜點，豐儉由人;四是很容易并行化或者GPU化。

于是FasterLIO使用了一種基于稀疏體素的近鄰結構iVox(incremental voxels)。我們會發現這種結構用來做LIO更加合適，可以有效的降低點云配準時的耗時，也不會影響LIO的精度表現。我們使用兩個版本的iVox：一種是線性的，一種是基于空間填充曲線的(偽希爾伯特空間曲線，pseudo-Hilbert curve, PHC)，下面來說明其原理。

iVox

iVox由空間中稀疏分布的體素組成。每個體素內部可以存在多個點，體素自身的網格坐標由空間哈希函數映射到哈希鍵值上，再組成哈希表。

哈希函數可以取一些典型形式。由于實際存儲的是三維點，我們使用簡單的空間哈希即可：

其中p為三維點，v為體素網絡，id是它的哈希鍵值，xor表示異或。

iVox內部點的存儲方式稱為它的底層結構。最簡單的底層結構是線性的，我們稱為線性的iVox;如果要存的點很多，我們利用空間填充曲線來存，稱為iVox-PHC。當然在LIO算法流程上，我們會避免在同一個體素中大量插入點而影響計算效率，所以兩種算法實際用起來差異不大。

K近鄰的查找

iVox里的K近鄰相對簡單。我們首先定義一個體素的近鄰范圍，典型的有0，6，18，26這幾種。實際當中主要用18和26.

在查找K近鄰時，先計算被查找的點落在哪個體素中，然后看預定義的范圍內是否存在有效的iVox。如果有，就把這些iVox內部的點也考慮進來。我們在每個iVox內部查找至多K個最近鄰，再進行合并即可。線性的iVox只須遍歷內部點，然后進行部分排序，再進行匯總。PHC的iVox則可以根據空間填充曲線上的索引值來查找最近鄰。

PHC是一種建立高維數據與低維數據映射的方法。離散的PHC可以看成把空間分成許多小格子，然后給每個格子找一個id的過程。于是，在查找某個點的最近鄰時，可以先看它在這個iVox里的曲線上ID，然后在這個ID周邊找到若干個近鄰，再返回其結果即可。在點數較多時，PHC的查找會比線性查找的復雜度更低一些。

增量地圖更新

iVox的增量地圖更新比kd樹簡單很多。簡而言之，算出增加點對應的體素網格，直接往里添加即可。如果是PHC的，則還需要算一下PHC的曲線位置。除此之外沒有其他操作了。

在FastLIO2中，系統會刪除一部分歷史點云，讓局部地圖跟隨車輛前進。在iVox里，由于遍歷整個iVox局部地圖是比較慢的，我們讓這個過程變為被動刪除，而不是主動地在每幀計算后進行刪除。于是我們給局部地圖添加一個LRU緩存(least recently used)，在進行近鄰搜索時，記錄哪些體素是最近使用過的，那么不怎么使用的體素自然被移動到隊尾。我們會設置一個局部地圖的容量，超過最大容量時，就刪除那些很久未使用的體素。刪除操作是針對整個體素的，內部的點會被全部刪除。這種局部地圖緩存策略也會讓局部地圖跟隨車輛運動，而實際操作的地方更少。

iVox的具體參數和復雜度等細節見論文和代碼，這里不再描述。

實驗

實驗部分主要包含仿真實驗和數據集實驗。

仿真實驗

仿真實驗是在一個隨機生成的點云里進行K近鄰查找以及新增地圖點的實驗。我們對比了Kdtree flann, ikd-tree, nanoflann R-tree, faiss-IVF, nmslib幾個庫。耗時與地圖點數的關系圖如下：

可以看到iVox在K近鄰查找和新增的耗時都是很少的，但它們隨地圖點數的增長會更快，畢竟單個iVox里的點會變多。順便說一句，PCL Kdtree的查詢速度(這里寫的flann)也是杠杠的，只是沒有增量接口。

我們也比較了iVox的K近鄰質量問題。大部分時候iVox的K近鄰不是嚴格的，因為天然的有范圍限制。我們把K近鄰的結果與暴力搜索的結果進行比對，可以得到它的召回率(recall)。各算法的召回率與時間曲線如下：

如果要求iVox有很高的召回，那就不得不設置很大的體素尺寸或者很大的搜索近鄰，這時候iVox的查找時間會增長很快。不過LIO系統在70%左右的召回率下就能很好地工作，所以實際也沒啥大不了的。

數據集實驗

數據集實驗主要比較整個LIO系統的耗時和計算精度。由于Faster-LIO框架與FastLIO2基本相同，我們時間上對標的也主要是FastLIO2，其他系統主要是用來做個參考。32線雷達的詳細步驟算法耗時如下圖所示：

可見主要的耗時在IEKF+ICP的迭代過程。使用iVox替代iKd-tree時，我們縮短的也主要是這個過程。UTBM數據集要明顯一些，我們可以把差不多20ms的IEKF迭代降到5-8ms左右，整個流程可以從30ms左右降到10ms左右，實現明顯的效率提升。

我們也繪制了時間軸上的耗時曲線，如下：

兩個版本的LIO的運算效率不會隨著時間有明顯變化，而FasterLIO要明顯用時更低一些。對LIO-SAM、LiLi-OM的一些計算用時指標可以參考論文的表格。

在精度方面，考慮到LIO默認不帶回環檢測，所以我們主要評價每百米的漂移誤差指標(百分比形式)，見下表。

實際上，只要不漂，LIO精度都是差不多的，天下哪有改改算法就提升精度的事情(逃)。

iVox也可以被集成到其他LO或LIO里，但是大部分方案里，最近鄰并不是主要的計算瓶頸，gtsam/ceres什么的耗時相比最近鄰那可太多了。我們也嘗試了把iVox集成到Lego-LOAM里，發現主要只是省了增量地圖構建那部分時間，優化方面沒什么變化(點少)。所以iVox與FastLIO倒是相性更好一些。

結論與聲明

本文提出了一種更快速的LIO方法，使用iVox作為最近鄰方案，在同等精度指標下可以明顯提升LIO計算速度。

我們也歡迎讀者自己測試Faster-LIO的計算性能。祝大家科研愉快。

同時，智行者還提供了開源代碼以更方便社區使用。

原文標題：頂刊收錄!智行者提出全新基于ivox激光雷達算法

文章出處：【微信公眾號：智行者科技】歡迎添加關注!文章轉載請注明出處。

審核編輯：湯梓紅