所謂“激光里程計”，是指利用激光雷達(LiDAR)進行遞推地位姿狀態(tài)估計的方法，其中最為著名的莫過于張楫博士的大作LOAM: LIDAR odometry and ming in real-time（后續(xù)還有更為完整的期刊論文版本）。目前比較杰出的激光里程計解決方案基本都沿用了LOAM的整體架構和思路，并在某些方面進行了改進和完善。關于LOAM的要點介紹和代碼解析可以去考古一下我當年的系列文章：

　　LOAM早期開源于github的代碼由于沒有使用第三方優(yōu)化計算庫，非線性優(yōu)化計算完全獨立編寫，且求導方式也采用的是對旋轉矩陣求導這種較為傳統(tǒng)的模式，因此整體而言代碼結構不夠簡潔。港科大沈劭劼老師團隊在其基礎上利用ceres對代碼進行了重新整理并發(fā)布(A-LOAM)，代碼的穩(wěn)定性和可復用性大大提升，但在改進過程中也忽略了一個十分重要的部分，也必然會引起很多問題，這個我們后續(xù)再展開說。

　　總而言之，這個系列的文章面向的是對LOAM已經有一定認識的讀者，因此很多細節(jié)就不再展開和贅述了，上車前請各位先自行了解相關預備基礎。

　　相較于LOAM的第一個區(qū)別：采用兩步法對原始點云的畸變進行校正，第一步在位姿計算中進行，第二步在點云地圖的保存和可視化時進行。

　　相較于LOAM的第二個區(qū)別：在位姿估計的非線性優(yōu)化計算過程中對特征點約束定權，即根據(jù)特征點平滑度(smothness)的大小確定特征匹配點對在非線性優(yōu)化中的作用，特征越明顯，其對優(yōu)化計算結果的影響越大。

　　相較于LOAM的第三個區(qū)別：摒棄了LOAM中odometry和ming兩線程并行的思路，將odometry的scan-to-scan的匹配移除，僅保留scan-to-map的匹配。因此，ming線程不再進行優(yōu)化計算，僅處理點云地圖的保存和可視化等工作。

　　floam_laser_processing_node 節(jié)點負責從LiDAR采集到的原始點云中提取線、面特征點，一方面起到點云數(shù)據(jù)降采樣的作用，另一方面提升點云匹配的準確性和魯棒性

　　F-LOAM中所采用的方法與LOAM基本完全一致，主要包括特征提取、畸變消除、位姿估計和地圖構建等四個模塊。

　　// 逐點計算平滑度，對應式(1)。由于點數(shù)始終為定值10，因此實際計算時沒有再除以10

　　這一部分是論文中提到的創(chuàng)新點之一，但實際在代碼實現(xiàn)上與A-LOAM是相同的。論文中說畸變校正需要在位姿估計優(yōu)化迭代中反復進行，因此比較耗時。我們從A-LOAM的代碼中看一下，便可理解這一點：

　　// 根據(jù)scan-to-scan的odom預測值將所有特征點統(tǒng)一至scan的起始時刻，即完成了畸變的校正

　　從代碼中可以看出，在每輪迭代的循環(huán)中都是要逐點進行畸變校正的，但是請注意，最外層的循環(huán)數(shù)只有2，也就是說只進行了兩次逐點的畸變校正計算。在完成位姿估計的優(yōu)化迭代后可以得到更為精確的到 的位姿變換，從而可以將畸變的特征點統(tǒng)一至scan的終止時刻，實現(xiàn)精確的畸變校正

　　然而，這段代碼顯然是不會被執(zhí)行的，也就是說實際上在LOAM中沒有對畸變點云進行最終的精確校正。在 laserMing 線程中與上述實現(xiàn)類似，也是在位姿估計的迭代計算中由 pointAssociateToMap 函數(shù)進行畸變校正，但循環(huán)同樣只有2次。

　　那么接下來在對比來看一下F-LOAM中的兩步法畸變校正，畸變校正的第一步在 odomEstimationClass.cpp 中實現(xiàn)。首先，根據(jù)勻速運動模型對當前位姿進行預測：

　　而后同樣是在位姿估計的迭代計算中由 pointAssociateToMap 函數(shù)進行畸變校正：

　　// 在非線性優(yōu)化方程中添加線和面特征約束，函數(shù)中包含了pointAssociateToMap的畸變校正過程

　　在優(yōu)化計算得到精確地后，可將原始的畸變點云由 pointAssociateToMap 函數(shù)進行畸變校正，這就是第二步。從代碼對比可以看出其實現(xiàn)與A-LOAM中的 laserMing 并無差異，但由于代碼結構上丟棄了 laserOdometry 線程，因此必然執(zhí)行效率有所提升。

　　J9九游會官方·登錄入口

　　這一部分是論文中提到的又一創(chuàng)新點，但實際在代碼實現(xiàn)上也與A-LOAM是相同的。論文中敘述的根據(jù)各點平滑度定權的策略并未在開源的代碼版本中體現(xiàn)(見上一代碼塊中ceres非線性優(yōu)化計算部分)，僅僅只是采用了Huber函數(shù)作為損失函數(shù)。

　　實驗結果在KITTI和AGV室內倉庫數(shù)據(jù)集上進行了測試，從結果上看精度與A-LOAM相差不大，略有提升，但時間消耗上大大縮減，證實了F-LOAM能夠在不影響精度的前提下大幅提升運行效率，能夠很輕量化的部署在算力有限的無人平臺上。結合上述代碼實現(xiàn)和對論文的解讀，我們也可以得出一個結論：LOAM的laserOdometry線程并非十分必要，在多數(shù)地面平臺（如：地面機器人，AGV，無人車等）中并不能為精度提升帶來幫助，反而影響了運行效率。請記得這個結論，后續(xù)可能會有其他問題的探討。

　　本公眾號原創(chuàng)內容版權屬計算機視覺life所有；從公開渠道收集、整理及授權轉載的非原創(chuàng)文字、圖片和音視頻資料，版權屬原作者。如果侵權，請聯(lián)系我們，會及時刪除

　　聲明：本文由入駐搜狐公眾平臺的作者撰寫，除搜狐官方賬號外，觀點僅代表作者本人，不代表搜狐立場。