論文閱讀及復(fù)現(xiàn)——《CT_ICP: Real-time Elastic LiDAR Odometry with Loop Closure》

這篇具有很好參考價(jià)值的文章主要介紹了論文閱讀及復(fù)現(xiàn)——《CT_ICP: Real-time Elastic LiDAR Odometry with Loop Closure》。希望對(duì)大家有所幫助。如果存在錯(cuò)誤或未考慮完全的地方，請(qǐng)大家不吝賜教，您也可以點(diǎn)擊"舉報(bào)違法"按鈕提交疑問。

論文網(wǎng)址：https://arxiv.org/abs/2109.12979
源碼網(wǎng)址：https://github.com/jedeschaud/ct_icp
復(fù)現(xiàn)過程：https://blog.csdn.net/qq_44164791/article/details/132188049?spm=1001.2014.3001.5502

1. 主要貢獻(xiàn)

提出了一個(gè)具有掃描內(nèi)姿態(tài)連續(xù)性和掃描間不連續(xù)性的新型彈性激光雷達(dá)里程計(jì)

使用稀疏體素結(jié)構(gòu)存儲(chǔ)稠密點(diǎn)云的局部地圖，可以達(dá)到實(shí)時(shí)處理的速度。

做了大量實(shí)驗(yàn)，在7個(gè)高頻運(yùn)動(dòng)駕駛場(chǎng)景數(shù)據(jù)集上進(jìn)行了實(shí)驗(yàn)，所有的代碼均開源

提出了一個(gè)回環(huán)檢測(cè)的方法，還實(shí)現(xiàn)了位姿后端，構(gòu)成了一個(gè)完整的SLAM。

2. 相關(guān)說明

CT-ICP 定義的軌跡在掃描內(nèi)連續(xù)，在掃描間間斷。在一個(gè)掃描過程中，軌跡可以使用開始位姿和結(jié)束位姿表示。CT-ICP 的最終軌跡是不連續(xù)的，因?yàn)橐淮螔呙璧拈_始位姿不等于上一次掃描的結(jié)束位姿。這種方法考慮到插值不適用于不規(guī)則運(yùn)動(dòng)。

激光雷達(dá)里程計(jì)在開發(fā)環(huán)境中依然有累積誤差，這會(huì)導(dǎo)致軌跡偏移?；丨h(huán)閉合可以全局矯正軌跡，但回環(huán)檢測(cè)仍然是一個(gè)開放性的問題?，F(xiàn)在大多數(shù)解決方法都主要依賴配準(zhǔn)法（ registration methods）直接閉合回環(huán)。這樣只能處理小規(guī)模軌跡和較小偏移。很多位置識(shí)別方法都在每一個(gè)掃描中識(shí)別位置，這對(duì)環(huán)境變化很敏感，更適合駕駛場(chǎng)景。最近和多基于深度學(xué)習(xí)的方法被提出，但由于訓(xùn)練樣本，這些方法不適用于新環(huán)境。CT-ICP 的回環(huán)閉合算法作用于投影在一個(gè)高程圖像上的匯聚點(diǎn)云（aggregated point clouds）。該方法需要傳感器的運(yùn)動(dòng)接近 2 維，且需要估計(jì)重力矢量，該方法可以用于任何滿足這些條件的激光雷達(dá)里程計(jì)。CT-ICP 在局部地圖中檢測(cè)回環(huán)。因此 CT-ICP 不需要檢查每次掃描是否與之前的位置一樣。這在在線 SLAM 場(chǎng)景中更高效。

3. 激光里程計(jì)

3.1 里程計(jì)公式構(gòu)建

在每一幀，里程計(jì)具有兩個(gè)位姿，分別是幀起始位姿和幀結(jié)束位姿, 時(shí)間也一樣:
論文閱讀及復(fù)現(xiàn)——《CT_ICP: Real-time Elastic LiDAR Odometry with Loop Closure》,SLAM,# 理論學(xué)習(xí),論文閱讀,ubuntu
與其他里程計(jì)不同的是，當(dāng)前幀的起始位姿不等于上一幀的結(jié)束位姿，這兩個(gè)位姿之間還存在一個(gè)鄰近約束。

定義求解問題如下:
論文閱讀及復(fù)現(xiàn)——《CT_ICP: Real-time Elastic LiDAR Odometry with Loop Closure》,SLAM,# 理論學(xué)習(xí),論文閱讀,ubuntu

1. 局部地圖和魯棒的配置文件
這部分論文里寫的有點(diǎn)像是實(shí)驗(yàn)報(bào)告，兩段話，大致講了體素格參數(shù)配置和雷達(dá)點(diǎn)云幀插入標(biāo)準(zhǔn)的大致準(zhǔn)則(判斷角度變化是否大于閾值)。
2. 回環(huán)檢測(cè)和后段
思路和SC有點(diǎn)類似，但是實(shí)現(xiàn)方式不一樣
將地圖中的每個(gè)點(diǎn)插入到2D高程網(wǎng)格，使每個(gè)像素點(diǎn)保持在最大高度，在2D網(wǎng)格中，通過Zmin和Zmax的差值獲取高程圖像，然后提取旋轉(zhuǎn)不變的2D特征并且和高程網(wǎng)格一起保存。提取到匹配對(duì)后，通過ICP和RANSAC約束位姿…
總體而言，限制很多，首先要求是運(yùn)動(dòng)盡量是平面運(yùn)動(dòng)，然后要求將Z軸和地平面法向量對(duì)齊，這個(gè)回環(huán)不如用SC來代替。

3.2 局部地圖與健壯性

我們使用過往的掃描作為局部地圖。體素用于存儲(chǔ)世界坐標(biāo)系中的點(diǎn)，為了快速訪問臨近體素，體素使用稀疏的數(shù)據(jù)結(jié)構(gòu)實(shí)現(xiàn)，而不是使用 kd 樹。
所以與kd樹相比，其訪問時(shí)間復(fù)雜度是線性的，而不是對(duì)數(shù)。對(duì)于駕駛場(chǎng)景來說，高頻運(yùn)動(dòng)場(chǎng)景下，局部地圖的體素尺寸是1米和0.8米。

體素的尺寸就是地圖的柵格大小，它決定鄰近搜索半徑和局部地圖的詳細(xì)程度。每個(gè)體素存儲(chǔ)的點(diǎn)達(dá)到 20 個(gè)，為了避免冗余，任意兩個(gè)點(diǎn)的距離都不小于10厘米。為了構(gòu)造點(diǎn) $p^{W}_i$ 的近鄰（用于計(jì)算法向量 $n^i$ 和平面權(quán)重 $a^i$ ），我們?cè)诘貓D中離該點(diǎn)最近的 27 個(gè)（邊長(zhǎng)為 3 的立方體）體素中找出k=20個(gè)最近點(diǎn)。處理完當(dāng)前掃描后，點(diǎn)被加入局部地圖。體素被占滿時(shí)，其中的點(diǎn)將被刪除（為什么）。我們的局部地圖不像 pyLiDAR的F2M一樣，局部地圖不會(huì)使用滑動(dòng)窗口丟棄點(diǎn)云。

如果向地圖中添加錯(cuò)誤的數(shù)據(jù)，或者方向變化較快，使用滑動(dòng)窗口丟棄點(diǎn)云的地圖很容易受影響。對(duì)于方向變化快的數(shù)據(jù)，我們提出一個(gè)健壯性的方案：檢測(cè)困難情況（快速的方向變化）和失敗的注冊(cè)（位置不一致或者大量新關(guān)鍵點(diǎn)落入空體素），使用更保守的參數(shù)重新注冊(cè)；如果方向變化大于5度，我們就不會(huì)把新的掃描結(jié)果插入地圖，否則很可能偏離方向。健壯性的提高會(huì)增加一定的時(shí)間復(fù)雜度。

4. 回環(huán)檢測(cè)與后端

CT-ICP 的回環(huán)閉合算法在內(nèi)存中維持一個(gè)窗口，窗口內(nèi)是里程計(jì)最近添加進(jìn)來的掃描。當(dāng)窗口大小等于 $NmapN_{map}$ 個(gè)掃描時(shí)，點(diǎn)就會(huì)被加入位于窗口中心位置的那個(gè)掃描的點(diǎn)云中。解釋：假如窗口大小是 9，如果窗口內(nèi)有 9 個(gè)掃描，就把窗口內(nèi)的點(diǎn)加入第 5 個(gè)掃描的點(diǎn)云中。這是因?yàn)橐话闱闆r下，第 5 個(gè)掃描與其它 8 個(gè)掃描的重合最多。

地圖的每個(gè)點(diǎn)都會(huì)被插入一個(gè) 2 維的高程?hào)鸥?，讓每個(gè)像素的點(diǎn)保持最大高度。在這個(gè) 2 維柵格內(nèi)，以點(diǎn)的最小z坐標(biāo) $z_{min}$ z和最大 z 坐標(biāo) $z_{max}$ 作為 z 軸的范圍，得到高程圖像。然后提取 2 維旋轉(zhuǎn)不變特征，與高程?hào)鸥褚粔K保存在內(nèi)存中。除了最后 $N_{overlap}$ 個(gè)掃描，其它都被移出窗口。

每一個(gè)新創(chuàng)建的高程圖像（包含 $N_{map} - N_{overlap}$ 個(gè)掃描），都會(huì)與內(nèi)存中的高程圖像匹配。使用 2 維剛體變換和 RANSAC 算法，把新創(chuàng)建的高程圖像變換到與內(nèi)存中保存的高程圖像一致。如果能匹配，就對(duì)高程?hào)鸥竦狞c(diǎn)云，使用 ICP 改善算法（Open3D 的 ICP 算法）提高 2 維剛體變換，得到一個(gè)精確的 6 自由度閉環(huán)約束。為了減少候選數(shù)量，使用閾值 $n_{candidates} = 10$ 篩選與當(dāng)前高程?hào)鸥褡罱?10 個(gè)高程?hào)鸥?。這個(gè)閾值與誤差有關(guān)，閾值過小導(dǎo)致閉環(huán)的漏檢。

CT-ICP 的后端使用 g2o 實(shí)現(xiàn)標(biāo)準(zhǔn)的位姿圖。位姿圖定期添加新位姿，但只有檢測(cè)到閉環(huán)時(shí)，才對(duì)整個(gè)軌跡作全局優(yōu)化。

CT-ICP 的算法需要傳感器在平面上運(yùn)動(dòng)（上下運(yùn)動(dòng)幅度?。?，還需要外參標(biāo)定以對(duì)齊 z 坐標(biāo)與實(shí)際平面。這限制了傳感器和運(yùn)動(dòng)。如果重力矢量或者局部平面已知，高程圖像就能正確地投影，這個(gè)限制也就沒有了。但我們?cè)诘?Ⅴ 部分說了，在室外場(chǎng)景中，使用向上的傳感器，使我們的閉環(huán)檢測(cè)成功檢測(cè)到一些閉環(huán)。

5. 實(shí)驗(yàn)結(jié)果

5.1 里程計(jì)實(shí)驗(yàn)結(jié)果

論文閱讀及復(fù)現(xiàn)——《CT_ICP: Real-time Elastic LiDAR Odometry with Loop Closure》,SLAM,# 理論學(xué)習(xí),論文閱讀,ubuntu
表中數(shù)據(jù)在 5 個(gè)駕駛場(chǎng)景數(shù)據(jù)集和2個(gè)高頻運(yùn)動(dòng)場(chǎng)景數(shù)據(jù)集上的相對(duì)變換誤差 RTE。AVG 是所有 sequences 上 RTE 的平均值，ΔT是每個(gè)掃描的平均耗時(shí)。KITTI-corrected 是唯一一個(gè)被校正過的數(shù)據(jù)集，其它數(shù)據(jù)集的點(diǎn)云都是原始未校正的。

從表中可以看出，所有里程計(jì)方法在經(jīng)過運(yùn)動(dòng)校正的 KITTI-corrected 數(shù)據(jù)集上表現(xiàn)接近。處理未校正的數(shù)據(jù)集 KITTI-raw 和 KITTI-360 時(shí)，其它方法的表現(xiàn)明顯變差，而 CT-ICP 在未校正的數(shù)據(jù)集上的表現(xiàn)接近其在校正過的數(shù)據(jù)集上的表現(xiàn)。在校正過的數(shù)據(jù)集上，CT-ICP 的彈性策略不起作用，這表明了CT-ICP的地圖和scan-to-map算法的有效性。在 KITTI 的在線評(píng)測(cè)上，CT-ICP 以 0.59% 的 RTE 排名第一。
??表中KITTI-corrected 數(shù)據(jù)集上，CT-ICP 的結(jié)果與 MULLS 不同，這是因?yàn)?CT-ICP 把一套參數(shù)用于所有 sequence，而 MULLS 針對(duì)城市、高速公路和村莊場(chǎng)景，使用三套不同的參數(shù)。

KITTI-CARLA 數(shù)據(jù)集的速度變化比 KITTI 快。在該數(shù)據(jù)集上，CT-ICP 的表現(xiàn)比其它方法都好：CT-ICP 的 RTE 僅為 0.09%，其它方法的最好結(jié)果是 0.81%。這證明 CT-ICP 的彈性策略和掃描間位姿不連續(xù)策略可以有效彌補(bǔ)位姿誤差。

不同于 KITTI 的 64 線激光雷達(dá)，ParisLuco 數(shù)據(jù)集的雷達(dá)是 32 線的。由于獲取于市中心，ParisLuco 是低慣性的（low inertia）。在該數(shù)據(jù)集上，CT-ICP 能超過使用稠密地圖的IMLS-SLAM。

NCD 和 NCLT 數(shù)據(jù)集使用棍子和平衡車采集，所以不穩(wěn)定。而且它們的每個(gè) sequence 包含更多掃描，環(huán)境也多樣（植被、道路、室內(nèi)、室外），因此更具挑戰(zhàn)性。此外這兩個(gè)數(shù)據(jù)集還有很多邊界情況，比如快速地從室內(nèi)轉(zhuǎn)移到室外、急轉(zhuǎn)彎、運(yùn)動(dòng)的高頻間斷引起的掃描大變形。一般的雷達(dá)里程計(jì)很難處理這些情況。表 1 表明 CT-ICP 在 NCD 數(shù)據(jù)集上表現(xiàn)優(yōu)秀。在 NCLT 數(shù)據(jù)集上的實(shí)驗(yàn)也證明了 CT-ICP 的健壯性，因?yàn)?CT-ICP 在處理邊界情況時(shí)，RTE 比 pyLiDAR F2M 還低。

為了專門驗(yàn)證 CT-ICP 的主要貢獻(xiàn)，即其彈性策略，只使用勻速運(yùn)動(dòng)模型扭曲（distort）掃描，以測(cè)試?yán)锍逃?jì)。在 KITTI-raw 上，RTE 從 0.55% 增加到 0.79%，在 KITTI-360 上，RTE 從 0.45% 增加到 0.60%。

5.2 回環(huán)檢測(cè)實(shí)驗(yàn)結(jié)果

在所有數(shù)據(jù)集上，CT-ICP 使用的回環(huán)閉合模型都是 CT-ICP 加回環(huán)閉合，其中
$N_{map} = 100N$ ， $N_{overlap} = 30N$ 。 $z_{min}$ 略低于標(biāo)注值（使用傳感器獲取一個(gè)近似的外部標(biāo)定）， $z_{max}$ 比 $z_{min}$ 大 10 米。對(duì)于 NCLT，使用剛體變換把向下的 z 軸變換成向上。每個(gè)高程珊格的計(jì)算時(shí)間加上與前一個(gè)高程珊格的匹配時(shí)間，平均需要 1.1 秒。位姿圖優(yōu)化平均需要 1.2 秒。當(dāng)前的回環(huán)閉合由主線程負(fù)責(zé)，利用多線程可以實(shí)現(xiàn)實(shí)時(shí)。
論文閱讀及復(fù)現(xiàn)——《CT_ICP: Real-time Elastic LiDAR Odometry with Loop Closure》,SLAM,# 理論學(xué)習(xí),論文閱讀,ubuntu
CT-ICP 里程計(jì)估計(jì)出的軌跡和回環(huán)閉合校正效果。第 1 幅圖是在 KITTI-raw 數(shù)據(jù)集 sequence 00 的 4541 個(gè)掃描上的實(shí)驗(yàn)結(jié)果，第 2 幅圖是在 KITTI-360 數(shù)據(jù)集 sequence 06 的 9698 個(gè)掃描上的實(shí)驗(yàn)結(jié)果，第 3 幅圖是在 KITTI-CARLA 數(shù)據(jù)集 Town 01 的 5000 個(gè)掃描上的實(shí)驗(yàn)結(jié)果，第 4 幅圖是在 NCD 數(shù)據(jù)集 $01_short_experiment$ 的 15301 個(gè)掃描上的實(shí)驗(yàn)結(jié)果，第 5 幅圖是在 NCLT 數(shù)據(jù)集 2012-01-08 的 42764 個(gè)掃描上的實(shí)驗(yàn)結(jié)果。
論文閱讀及復(fù)現(xiàn)——《CT_ICP: Real-time Elastic LiDAR Odometry with Loop Closure》,SLAM,# 理論學(xué)習(xí),論文閱讀,ubuntu
上圖圖是在 KITTI-360 數(shù)據(jù)集 sequence 00 的 11501 個(gè)掃描上，回環(huán)閉合的量化結(jié)果。左上部是投影局部地圖得到的一個(gè)高程圖像，右上部是 CT-ICP 里程計(jì)軌跡和回環(huán)閉合（CT-ICP 加回環(huán)閉合）校正效果，下部是在左上圖所示的局部地圖中發(fā)現(xiàn)的多個(gè)閉環(huán)約束（使用綠色表示）。
論文閱讀及復(fù)現(xiàn)——《CT_ICP: Real-time Elastic LiDAR Odometry with Loop Closure》,SLAM,# 理論學(xué)習(xí),論文閱讀,ubuntu
表是回環(huán)閉合在每個(gè)數(shù)據(jù)集的一個(gè) sequence 上的實(shí)驗(yàn)結(jié)果。ATE 是固定了估計(jì)軌跡于實(shí)際軌跡間的剛體變換后的平均絕對(duì)軌跡誤差，單位是米。 $N_{loop}$ 是檢測(cè)到的回環(huán)數(shù)量。LO 代表激光雷達(dá)里程計(jì)，LC 代表回環(huán)閉合。

在各數(shù)據(jù)集的其中一個(gè) sequence 上的實(shí)驗(yàn)結(jié)果。首先，記錄 CT-ICP 在 KITTI-raw、KITTI-360 和 ParisLuco 數(shù)據(jù)集上，回環(huán)閉合的最好結(jié)果。KITTI-360 的 ATE 在閉合前后變化很大，這是因?yàn)樗?sequence 最長(zhǎng)。KITTI-CARLA 的 ATE 在回環(huán)閉合前后變化不大，這是因?yàn)樵摂?shù)據(jù)集包含的路徑幾何形狀簡(jiǎn)單，也就是說平面大且平整。所以掃描匹配的挑戰(zhàn)性主要受傳感器采集數(shù)據(jù)時(shí)的運(yùn)動(dòng)情況影響。CT-ICP 的對(duì)齊效果接近完美，達(dá)到了 ATE 為 21 厘米的精度，如圖 3 所示，其估計(jì)的軌跡與標(biāo)注值幾乎重合。

最后，在每個(gè) sequence 上檢測(cè)多個(gè)回環(huán)。回環(huán)的數(shù)量取決于高程?hào)鸥竦臉?gòu)造頻率，也就是和重疊大小 $N_{overlap}N$ 以及地圖大小 $N_{map}N$ 有關(guān)。每到交叉路口，就計(jì)算回環(huán)約束。重疊大小 $N_{overlap}N$ 越大，越能檢測(cè)出更多回環(huán)（包括連續(xù)的局部地圖間的約束）。

6. 總結(jié)

CT-ICP 提出了一個(gè)新的實(shí)時(shí)里程計(jì)。在包含駕駛場(chǎng)景和高頻運(yùn)動(dòng)場(chǎng)景的 7 個(gè)數(shù)據(jù)集上，進(jìn)行了多方面的評(píng)比，CT-ICP 都超過現(xiàn)有方法。

核心是連續(xù)的掃描匹配算法，它在優(yōu)化時(shí)，彈性地變換一個(gè)新掃描，以彌補(bǔ)采集數(shù)據(jù)時(shí)的運(yùn)動(dòng)。

CT-ICP 提供所有代碼和數(shù)據(jù)集以供驗(yàn)證所有實(shí)驗(yàn)結(jié)果。

在IMLS的基礎(chǔ)上，建模幀內(nèi)和幀間約束

展望： CT-ICP 會(huì)著重于后端，進(jìn)一步延伸掃描外的連續(xù)策略，充分利用提出的回環(huán)閉合算法。文章來源地址http://www.zghlxwxcb.cn/news/detail-665404.html

到了這里，關(guān)于論文閱讀及復(fù)現(xiàn)——《CT_ICP: Real-time Elastic LiDAR Odometry with Loop Closure》的文章就介紹完了。如果您還想了解更多內(nèi)容，請(qǐng)?jiān)谟疑辖撬阉鱐OY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！