論文閱讀：GCLO: Ground Constrained LiDAR Odometry with Low-drifts for GPS-denied Indoor Environments (ICRA 2022)

Motivation

在具體的工程實(shí)踐中，lidar在地下停車(chē)場(chǎng)容易沿垂直方向向上漂移，導(dǎo)致建圖結(jié)果不佳。具體原因是當(dāng)入射角較大時(shí)，激光雷達(dá)測(cè)量的深度可能會(huì)出現(xiàn)偏差。因此，當(dāng)車(chē)輛在地面上移動(dòng)時(shí)，從道路上觀察到的點(diǎn)會(huì)稍微彎曲，LO 估計(jì)的軌跡會(huì)沿垂直方向漂移。因此為了消除 LiDAR 測(cè)量偏差并壓縮可預(yù)測(cè)的位姿漂移，通常做法是需要針對(duì)不同的傳感器調(diào)整比例因子，并且需要根據(jù)環(huán)境溫度產(chǎn)生差異，所以為了提升魯棒性，加入地面平面的考量減少無(wú)關(guān)項(xiàng)的影響是一個(gè)實(shí)用的選擇。下圖是motivation的可視化，具體表達(dá)的是見(jiàn)圖結(jié)果的地面點(diǎn)與擬合的地平面之間的距離的俯視圖，a圖表示當(dāng)車(chē)輛在地下停車(chē)場(chǎng)的地平面上移動(dòng)時(shí)，LO 結(jié)果向上漂移，所以最終地圖里表示的地面是一個(gè)扭曲向上的弓形，b圖表示當(dāng)采用本文提出的方法后的地面見(jiàn)圖效果。

作者：晃晃悠悠的虛無(wú)周 | 微信公眾號(hào)「3D視覺(jué)工坊」

Contribution

1. 提出了一種利用地面觀測(cè)約束來(lái)減少多層室內(nèi)和平坦室外環(huán)境中的位姿漂移的 LO 方法。

2. 提出了一種通過(guò)檢測(cè)連續(xù)關(guān)鍵幀之間的急劇變化來(lái)關(guān)聯(lián)地面平面地標(biāo)的地面匹配方法。

3. 基于KITTI 和 HIK 數(shù)據(jù)集的實(shí)驗(yàn)結(jié)果說(shuō)明了準(zhǔn)確性，并且討論了在算法泛化方面所存在的局限性。

Content

1. 關(guān)于無(wú)限平面參數(shù)化的討論

本文最終選定的無(wú)限平面參數(shù)化方式為closest point(CP)，未選的方式包括：Hesse form(HF), 球坐標(biāo)系表示和單位四元數(shù)表示，他們分別具有如下缺點(diǎn)：1）HF包含法向量n和平面到坐標(biāo)系原點(diǎn)的距離，這往往是過(guò)參數(shù)化的，所以它會(huì)在最小二乘優(yōu)化中受到奇異信息矩陣的影響，并且難以維護(hù)法向量。2）球坐標(biāo)系表示包含具有方位角和仰角的法向量，但是，當(dāng)仰角等于 ±Π /2 時(shí)，會(huì)產(chǎn)生歧義。3）單位四元數(shù)表示平面存在的問(wèn)題在于，目前尚未能清楚描述四元數(shù)和平面之間的物理聯(lián)系雖然在CP表示的情況下，當(dāng)給定坐標(biāo)系的原點(diǎn)位于平面上時(shí)，也會(huì)遇到奇異性問(wèn)題。但是由于地平面與車(chē)輛上配備的 LiDAR 有一定距離，因此本文可以避免 CP 表示的奇異性。另外由于本文將地面建模為無(wú)限平面地標(biāo)，因此在不同幀之間轉(zhuǎn)換地面參數(shù)至關(guān)重要，所以采用 HF作為地面轉(zhuǎn)換的中間參數(shù)使得轉(zhuǎn)換方程變得容易，具體過(guò)程如下：假設(shè)幀a存在一個(gè)地面用HF表示為：

，點(diǎn)是該平面的點(diǎn)，即

，給定a幀和b幀之間存在變換：

，帶入可得：

，從而可以得到新的HF表示：

，最后將HF表示轉(zhuǎn)為CP表示：

2.系統(tǒng)框架

系統(tǒng)框架如上圖。在預(yù)處理模塊中，使用EKF方法融合 IMU 和車(chē)輪編碼器，以提供高頻運(yùn)動(dòng)估計(jì)結(jié)果。LiDAR 點(diǎn)云的運(yùn)動(dòng)畸變通過(guò)使用運(yùn)動(dòng)估計(jì)結(jié)果來(lái)補(bǔ)償。去畸變的 LiDAR 點(diǎn)云基于點(diǎn)到平面的 ICP 算法來(lái)估計(jì)連續(xù)掃描幀之間的相對(duì)位姿變換。維護(hù)以傳感器為中心的滑窗，從而減輕 LiDAR 掃描的稀疏性并提高 LO 的準(zhǔn)確性。在每個(gè)關(guān)鍵幀，滑動(dòng)圖被記錄為局部地圖。從局部地圖中分割地平面，并使用加權(quán)最小二乘法估計(jì) CP 參數(shù)。之后，關(guān)聯(lián)在不同關(guān)鍵幀觀察到的地平面，并且構(gòu)建一個(gè)位姿圖并聯(lián)合優(yōu)化殘差。3.以傳感器為中心滑窗的scan2map

1）Scan2map注冊(cè)假設(shè)在時(shí)間k的時(shí)候，全局位姿為，并且滑窗地圖已經(jīng)基于幀進(jìn)行了維護(hù)，當(dāng)一個(gè)新的掃面幀在時(shí)間k+1進(jìn)入的時(shí)候，通過(guò)點(diǎn)到面的ICP估計(jì)出相對(duì)變換和對(duì)應(yīng)的協(xié)方差?；爸悬c(diǎn)的坐標(biāo)和不確定性協(xié)方差可以通過(guò)下述公式變換到新入幀：

其中，

是點(diǎn)關(guān)于旋轉(zhuǎn)分量的雅可比。2）以傳感器為中心的滑窗地圖更新滑窗圖在更新觀測(cè)特征的同時(shí)需要過(guò)濾掉具有較大不確定性的點(diǎn)。主要步驟如下：a)首先，新注冊(cè)的激光點(diǎn)需要先檢查預(yù)定義的距離閾值，之后和滑窗地圖點(diǎn)關(guān)聯(lián)。b)然后，選擇關(guān)聯(lián)的滑窗點(diǎn)，并將它們的不確定性重置為關(guān)聯(lián)的新掃描點(diǎn)的協(xié)方差矩陣，稱之為觀測(cè)誤差。c)具有大不確定性的滑窗點(diǎn)會(huì)被過(guò)濾掉。在實(shí)驗(yàn)中，只要對(duì)應(yīng)的協(xié)方差矩陣的跡大于閾值，該點(diǎn)就會(huì)被消除。d)最后，將沒(méi)有關(guān)聯(lián)成功的激光點(diǎn)作為新的觀測(cè)值添加到滑窗圖中。3)地面提取和建?？紤]到停車(chē)場(chǎng)的地面大多都平坦，所以采用無(wú)限平面進(jìn)行建模。對(duì)于每個(gè)關(guān)鍵幀，首先從以傳感器為中心的滑窗地圖中分割出地面點(diǎn)。然后基于這些分割出的地面點(diǎn)集應(yīng)用RANSAC獲得初始的CP表示。RANSAC獲得的表示肯定有噪聲，因此定義一個(gè)加權(quán)最小二乘問(wèn)題來(lái)最小化點(diǎn)到地面的距離：

其中，是第i個(gè)測(cè)量的不確定性的逆，是點(diǎn)pi的不確定性。關(guān)于殘差對(duì)于的雅克比可以定義為：

相應(yīng)的局部增量可以表示為：

新的平面參數(shù)可以根據(jù)局部增量進(jìn)行更新：

最終的不確定性可以近似為：

4)地面平面匹配假設(shè)已知j個(gè)歷史關(guān)鍵幀：

，并且每個(gè)關(guān)鍵幀都有從里程計(jì)獲得的位姿和其對(duì)應(yīng)的觀察到的地面參數(shù)及其不確定性，接下來(lái)的問(wèn)題可以表述為：如何確定哪個(gè)歷史地平面與當(dāng)前地平面共面。一個(gè)直接的方案是通過(guò)在歐式空間的一個(gè)相同的坐標(biāo)系中比較它們的參數(shù)來(lái)匹配地平面。但是如果漂移過(guò)大的話會(huì)錯(cuò)誤地關(guān)聯(lián)地平面。相反，基于一個(gè)先驗(yàn)知識(shí) ( 地面通常是結(jié)構(gòu)化比較好的，并且在論文motivation的環(huán)境（室內(nèi)停車(chē)場(chǎng)）中，地面連接存在明顯差異。)，

如上圖所述，當(dāng)?shù)孛孀兓臅r(shí)候，CP變化極其明顯，因此可以通過(guò)如下公式檢測(cè)CP參數(shù)的劇烈變化從而關(guān)聯(lián)對(duì)應(yīng)的地平面

其中f()代指：

然后通過(guò)如下公式估計(jì)的不確定性 （假設(shè)短時(shí)間內(nèi)的LO結(jié)果足夠精確，所以不需要計(jì)算LO的不確定性）

5)聯(lián)合位姿圖優(yōu)化在確定地平面之間的匹配關(guān)系后，將地面約束融合到位姿圖優(yōu)化框架中，從而壓縮 LO 的漂移，如下圖所示。

假設(shè)在世界坐標(biāo)系存在一個(gè)地面P，其CP表示為：。然后位姿為的幀觀測(cè)到地面P得到對(duì)應(yīng)的CP表示和HF表示

，運(yùn)用之前提到的公式，以HF為中介，將平面在當(dāng)前坐標(biāo)系轉(zhuǎn)換到全局坐標(biāo)系，從而可以從觀測(cè)到的CP與估計(jì)的CP之間構(gòu)建對(duì)應(yīng)的殘差：

殘差關(guān)于和的雅可比表示為：

最后通過(guò)LM方法聯(lián)合優(yōu)化LO和地平面殘差：

6)其他細(xì)節(jié)本文在每個(gè)關(guān)鍵幀提取一個(gè)地平面作為地面觀測(cè)。但是，此策略不適用于所有目標(biāo)環(huán)境。首先，CP 參數(shù)可能會(huì)因減速帶而產(chǎn)生噪聲。其次，地面不能總是被建模為平面地標(biāo)，因?yàn)閺澢吐菪碌涝诘叵峦＼?chē)場(chǎng)很常見(jiàn)。第三，當(dāng)關(guān)鍵幀位于多層路口的交叉區(qū)域時(shí)，該關(guān)鍵幀對(duì)應(yīng)兩個(gè)地平面。采用如下幾種方法來(lái)處理上述情況。首先，使用 Huber 損失函數(shù)來(lái)減少異常值（例如減速帶）對(duì) CP 參數(shù)估計(jì)的影響。其次，通過(guò)計(jì)算殘差和不確定性來(lái)評(píng)估地面的平面度。平面度低的地面不會(huì)被建模為平面地標(biāo)，相應(yīng)的觀測(cè)約束也不會(huì)被融合。第三，當(dāng)?shù)孛嫫ヅ洳幻鞔_時(shí)，在某些指標(biāo)（平面度、點(diǎn)數(shù)）下只選擇一個(gè)地平面。這樣，無(wú)論關(guān)鍵幀在停車(chē)場(chǎng)的哪個(gè)位置，每個(gè)關(guān)鍵幀都只擁有一個(gè)具有精確估計(jì)CP參數(shù)的地平面。4.實(shí)驗(yàn)

本文采用的數(shù)據(jù)集是HIK和KITTI。HIK是海康內(nèi)部數(shù)據(jù)集，包含了三種場(chǎng)景，室內(nèi)，校園環(huán)境，城市公路。KITTI不作贅述，大家都知道。最終的實(shí)驗(yàn)數(shù)據(jù)記錄如下：

注意，上表中的A/B,A指的是GCLO*, B指的是GCLO，GCLO*代表不加入地面約束。

注意，上圖中，kitti分成了兩個(gè)子集，子集1旨在說(shuō)明自己算法有優(yōu)勢(shì)的場(chǎng)景，子集2旨在分析為什么算法在這些場(chǎng)景不行，作者以Seq.09舉例分析，如下圖所示，雖然每個(gè)關(guān)鍵幀的地面是局部平坦的，但地面的坡度是逐漸變化的。最終不正確的關(guān)聯(lián)結(jié)果會(huì)導(dǎo)致更高的位姿漂移。因此與 GCLO* 相比，GCLO 結(jié)果的旋轉(zhuǎn)和平移誤差分別增長(zhǎng)了 36% 和 18%。

Conclusion

這篇文章的作者利用多層室內(nèi)和平坦室外環(huán)境的地面約束來(lái)優(yōu)化 LO 的位姿漂移。雖然最初是為 AVP 應(yīng)用而設(shè)計(jì)的，但實(shí)驗(yàn)證明對(duì)于其他環(huán)境也很有價(jià)值，并且也通過(guò)實(shí)驗(yàn)結(jié)果對(duì)缺點(diǎn)進(jìn)行了分析與討論，展現(xiàn)出了有希望的改進(jìn)方向，主要原因就是由于grounds的建模和關(guān)聯(lián)方法使得GCLO對(duì)一般場(chǎng)景的適應(yīng)性較差。所以后續(xù)可以考慮整合其他的建模方法或者額外的語(yǔ)義信息，以提高準(zhǔn)確性和魯棒性。本文僅做學(xué)術(shù)分享，如有侵權(quán)，請(qǐng)聯(lián)系刪文。

3D視覺(jué)工坊精品課程官網(wǎng)：3dcver.com
1.面向自動(dòng)駕駛領(lǐng)域的多傳感器數(shù)據(jù)融合技術(shù)
2.面向自動(dòng)駕駛領(lǐng)域的3D點(diǎn)云目標(biāo)檢測(cè)全棧學(xué)習(xí)路線！(單模態(tài)+多模態(tài)/數(shù)據(jù)+代碼)
3.徹底搞透視覺(jué)三維重建：原理剖析、代碼講解、及優(yōu)化改進(jìn)
4.國(guó)內(nèi)首個(gè)面向工業(yè)級(jí)實(shí)戰(zhàn)的點(diǎn)云處理課程
5.激光-視覺(jué)-IMU-GPS融合SLAM算法梳理和代碼講解
6.徹底搞懂視覺(jué)-慣性SLAM：基于VINS-Fusion正式開(kāi)課啦
7.徹底搞懂基于LOAM框架的3D激光SLAM: 源碼剖析到算法優(yōu)化
8.徹底剖析室內(nèi)、室外激光SLAM關(guān)鍵算法原理、代碼和實(shí)戰(zhàn)(cartographer+LOAM +LIO-SAM)
9.從零搭建一套結(jié)構(gòu)光3D重建系統(tǒng)[理論+源碼+實(shí)踐]
10.單目深度估計(jì)方法：算法梳理與代碼實(shí)現(xiàn)
11.自動(dòng)駕駛中的深度學(xué)習(xí)模型部署實(shí)戰(zhàn)
12.相機(jī)模型與標(biāo)定(單目+雙目+魚(yú)眼）
13.重磅！四旋翼飛行器：算法與實(shí)戰(zhàn)
14.ROS2從入門(mén)到精通：理論與實(shí)戰(zhàn)
15.國(guó)內(nèi)首個(gè)3D缺陷檢測(cè)教程：理論、源碼與實(shí)戰(zhàn)
16.基于Open3D的點(diǎn)云處理入門(mén)與實(shí)戰(zhàn)教程

更多干貨

歡迎加入【3D視覺(jué)工坊】交流群，方向涉及3D視覺(jué)、計(jì)算機(jī)視覺(jué)、深度學(xué)習(xí)、vSLAM、激光SLAM、立體視覺(jué)、自動(dòng)駕駛、點(diǎn)云處理、三維重建、多視圖幾何、結(jié)構(gòu)光、多傳感器融合、VR/AR、學(xué)術(shù)交流、求職交流等。工坊致力于干貨輸出，為3D領(lǐng)域貢獻(xiàn)自己的力量！歡迎大家一起交流成長(zhǎng)~

添加小助手微信：dddvision，備注學(xué)校/公司+姓名+研究方向即可加入工坊一起學(xué)習(xí)進(jìn)步。