RAL2023開源I基于多輸入Surfel的激光慣性連續(xù)時間里程計和建圖
作者：敖丙 ?| 來源：3D視覺工坊
在公眾號「3D視覺工坊」后臺，回復(fù)「原論文」可獲取論文pdf和代碼鏈接。
添加微信：dddvisiona，備注：SLAM，拉你入群。文末附行業(yè)細(xì)分群。
雖然與全局地圖的特征關(guān)聯(lián)有很大的好處，但為了防止計算呈指數(shù)級增長，大多數(shù)基于激光雷達(dá)的里程計和建圖方法都選擇將特征與一個體素尺度的局部地圖關(guān)聯(lián)。不同體素尺度的曲面可以組織成樹狀結(jié)構(gòu)，因此我們提出了一種基于八叉樹的多尺度曲面全局地圖，該地圖可以增量更新。這減小了重復(fù)計算整個地圖的k-d樹的耗時。該系統(tǒng)還可以從單個或多個傳感器獲取輸入，從而增強(qiáng)退化情況下的魯棒性。我們還提出了一種點(diǎn)到表面（PTS）關(guān)聯(lián)方案，對PTS和IMU預(yù)積分因子進(jìn)行連續(xù)時間優(yōu)化，以及閉環(huán)和束調(diào)整（BA），提供了一個完整的激光雷達(dá)慣性連續(xù)時間建圖定位框架。在公共和內(nèi)部數(shù)據(jù)集上的實(shí)驗(yàn)表明，與其他最先進(jìn)的方法相比，我們的系統(tǒng)具有優(yōu)勢。這里也推薦「3D視覺工坊」新課程《徹底剖析激光-視覺-IMU-GPS融合SLAM算法：理論推導(dǎo)、代碼講解和實(shí)戰(zhàn)》。
一個完整的激光雷達(dá)慣性里程計和測繪框架，具有前端里程計、閉環(huán)和全局姿態(tài)圖優(yōu)化，能夠集成多個激光雷達(dá)輸入。
一種多尺度點(diǎn)對面（PTS）關(guān)聯(lián)策略以及滑動窗口上PTS和IMU預(yù)積分因子的連續(xù)時間優(yōu)化。
基于UFOMap框架的分層多尺度surfel地圖的實(shí)現(xiàn)，實(shí)現(xiàn)了全局地圖的增量更新和高效查詢。
在公共和內(nèi)部數(shù)據(jù)集上進(jìn)行了大量實(shí)驗(yàn)，以驗(yàn)證多尺度surfel關(guān)聯(lián)策略的結(jié)果和效果。
系統(tǒng)基本框架
首先是將來自不同傳感器的數(shù)據(jù)進(jìn)行同步，然后進(jìn)行IMU傳播、激光雷達(dá)數(shù)據(jù)運(yùn)動補(bǔ)償。其次，執(zhí)行scan2map的PTS關(guān)聯(lián)匹配、IMU預(yù)積分，構(gòu)建殘差調(diào)用ceres進(jìn)行圖優(yōu)化、邊緣化，若發(fā)生回環(huán)則添加回環(huán)約束并更新地圖。
surfel 和 UFOMap
surfel屬性
如圖所示，每個點(diǎn)集都是一個surfel，持有各自的屬性；基于八叉樹的UFOMap中，一個節(jié)點(diǎn)i對應(yīng)一個體素（voxel）,節(jié)點(diǎn)深度表示體素尺度。葉子節(jié)點(diǎn)深度是0,其父節(jié)點(diǎn)深度是1,依次類推。節(jié)點(diǎn)i中的一個曲面（surfel)定義：包含在該體素內(nèi)的點(diǎn)集；
每個surfel中存儲: ,分別表示surfel中點(diǎn)數(shù)、點(diǎn)集、協(xié)方差分布相關(guān)?；谶@些屬性，可以快速計算得到重心、協(xié)方差分布、類平面度、法向量、重心到平面的距離。
surfel的添加和刪除（新關(guān)鍵幀加入后，surfel map更新方式）
假設(shè)父節(jié)點(diǎn)包含子節(jié)點(diǎn)和，則i的surfel屬性計算方式：
如果節(jié)點(diǎn)被刪除，則的屬性更新方式為：
即使增量更新的點(diǎn)云中數(shù)量只有一個點(diǎn)，上述公式依然可用，此時.
PTS關(guān)聯(lián)匹配
主要分為兩步：
1).對于每個激光點(diǎn)，在surfel map中找到所有匹配的節(jié)點(diǎn)（每個節(jié)點(diǎn)都是一個體素）；
具體匹配條件如下：
a. 體素深度在1和之間（葉節(jié)點(diǎn)忽略）；
b. ?且即 ?應(yīng)至少具有個最小點(diǎn)，并且平面度足夠大.
c. 體素的立方體與以為中心半徑為的球體相交，其中是用戶定義的參數(shù)。
2).計算激光點(diǎn)到的距離。
如果距離小于閾值,則將一組PTS系數(shù)將被添加到成功關(guān)聯(lián)的集合中，然后用于在優(yōu)化階段構(gòu)建代價函數(shù)。
PTS系數(shù)：
激光匹配殘差計算公式：
圖優(yōu)化、閉環(huán)
殘差包括：IMU預(yù)積分殘差、激光PTS關(guān)聯(lián)匹配殘差、回環(huán)約束、幀間約束。
需要注意的是，優(yōu)化使用的是滑窗策略，回環(huán)結(jié)束后需要重構(gòu)局部地圖。
邊緣化
在每次優(yōu)化之后，檢查姿態(tài)估計結(jié)果是否可以被邊緣化以成為關(guān)鍵幀。為此，在現(xiàn)有關(guān)鍵幀中搜索的五個最近鄰居。如果到其所有鄰居的歐幾里得距離或旋轉(zhuǎn)距離超過閾值，則其相關(guān)的去畸變點(diǎn)云將被插入到surfel map中。然后將邊緣化，并將其去畸變的點(diǎn)云存儲在緩沖區(qū)中，以便在閉環(huán)的情況下重用。
數(shù)據(jù)集：NTU VIRAL Dataset, Newer College Dataset，In-house Dataset
對比方案：MARS，LIO-SAM，F(xiàn)AST-LIO2
表1提供了NTU VIRAL數(shù)據(jù)集上的性能表現(xiàn)（rmse誤差）：SLICT < FAT-LIO2 < MARS/LIO-SAM，MARS和LIO-SAM軌跡會出現(xiàn)發(fā)散的情況。
SLICT目前無法保證實(shí)時性，如果使用多線程耗時可能有所改善，兩次優(yōu)化之間的耗時接近160ms，其中優(yōu)化求解耗時接近50ms，其余耗時部分是去畸變、特征關(guān)聯(lián)以及關(guān)鍵幀邊緣化。這里也推薦「3D視覺工坊」新課程《徹底剖析激光-視覺-IMU-GPS融合SLAM算法：理論推導(dǎo)、代碼講解和實(shí)戰(zhàn)》。
實(shí)驗(yàn)參數(shù)設(shè)置與NTU VIRAL數(shù)據(jù)集中的設(shè)置類似，但對于SLICT，我們?yōu)槊總€間隔分配2個狀態(tài)估計量。這是主要因?yàn)閿?shù)據(jù)集的IMU，即Ouster激光雷達(dá)的內(nèi)置IMU，頻率僅為100Hz。（請注意，LIO-SAM在這種情況下無法運(yùn)行，因?yàn)樗枰狪MU的方位估計。)
由于ATV的高速（高達(dá)30km/h），MARS在所有三個實(shí)驗(yàn)中都會發(fā)散，LIO SAM在最后一個實(shí)驗(yàn)中也會發(fā)散。在沒有閉環(huán)的情況下，SLICT仍然具有最佳的精度，并且閉環(huán)進(jìn)一步提高了精度。我們還添加了LIO-SAM的帶回環(huán)的測試結(jié)果，使之能夠與SLICT進(jìn)行比較。
由于環(huán)境的高程變化很大（最高點(diǎn)和最低點(diǎn)之間的高度高達(dá)15m），定位誤差也很大，在z方向維度上最明顯。與圖中 NewerCollege數(shù)據(jù)集相比，我們可以看到z方向的誤差現(xiàn)在幾乎翻了一倍。
在本文中，我們提出了一個完整的多輸入激光雷達(dá)慣性測距和測繪系統(tǒng)，稱為SLICT。SLICT的特點(diǎn)是可以使用UFOMap框架進(jìn)行增量更新的多尺度全局surfel map。我們還提出了一種將激光雷達(dá)點(diǎn)與曲面（PTS）相關(guān)聯(lián)的方法，以及PTS和IMU預(yù)積分因子的連續(xù)時間最大后驗(yàn)優(yōu)化。我們在公共數(shù)據(jù)集上展示了有競爭力的性能，即使不開閉環(huán)。為了實(shí)現(xiàn)一個完整的系統(tǒng)，我們添加了一個簡單而有效的閉環(huán)機(jī)制，并用我們的新數(shù)據(jù)集證明了它的有用性。為了社區(qū)的利益，我們發(fā)布了源代碼。SLICT的擴(kuò)展仍有許多可能性。首先，通過一些自適應(yīng)策略可以提高PTS關(guān)聯(lián)的效率。此外，UFOMap框架允許將語義信息集成到surfel中，這有可能改進(jìn)關(guān)聯(lián)過程。對于小漂移，基本的閉環(huán)機(jī)制就足夠了，但未來也可以集成更先進(jìn)的方法。
