簡介一份針對Kitti數(shù)據(jù)集深度調(diào)優(yōu)的LEGO-LOAM算法實(shí)現(xiàn)包面向從事激光雷達(dá)SLAM研究的學(xué)生與工程師。LEGO-LOAM是主流輕量級建圖算法但原生版本在讀入Kitti點(diǎn)云時(shí)常存在格式不適配、時(shí)間戳同步偏差等問題此版本在數(shù)據(jù)預(yù)處理、特征提取、點(diǎn)云匹配及回環(huán)檢測等環(huán)節(jié)做了針對性修改更適合在Kitti基準(zhǔn)上直接運(yùn)行并評估定位精度。壓縮包共23個(gè)文件體積26.74MB核心內(nèi)容以cpp源碼為主附帶launch啟動配置、xml參數(shù)文件、msg消息定義另有readme說明文檔、rviz可視化配置和論文預(yù)印本PDF基本覆蓋從編譯、運(yùn)行到結(jié)果分析的全流程。已有648人瀏覽學(xué)習(xí)說明其實(shí)用性得到一定驗(yàn)證。對希望在Kitti數(shù)據(jù)集上快速驗(yàn)證或改進(jìn)LEGO-LOAM的研究者而言這份代碼能省去大量環(huán)境適配時(shí)間可直接在官方數(shù)據(jù)上復(fù)現(xiàn)實(shí)驗(yàn)并基于清晰模塊結(jié)構(gòu)繼續(xù)二次開發(fā)。1. 這份為KITTI改過的LEGO-LOAM包一眼看出它修了什么第一次在KITTI數(shù)據(jù)集上跑原版LEGO-LOAM時(shí)我的里程計(jì)輸出在第一個(gè)彎道就直接跳變回環(huán)檢測模塊更是連影子都沒摸到。后來拿到這份專門適配KITTI的LEGO-LOAM修改包才知道問題不在算法本身而在數(shù)據(jù)入口、時(shí)間戳同步和地面分割參數(shù)上。這份壓縮包打包了修改后的完整代碼、cloud_msgs消息定義、原始論文PDF和授權(quán)文件明確指向一個(gè)目標(biāo)讓KITTI的64線激光雷達(dá)數(shù)據(jù)能直接喂進(jìn)LeGO-LOAM的planner框架里不需要你再去做額外矯形。它適合正在跑自動駕駛SLAM相關(guān)課題、想在KITTI真值集上驗(yàn)證自己里程計(jì)改動的從業(yè)者也適合剛?cè)胧諰OAM系列、想找一份“能出圖、能定位”的模板來對照學(xué)習(xí)的初學(xué)者。下面我會按照實(shí)際踩過的路徑來講先看資源結(jié)構(gòu)再編譯再跑通數(shù)據(jù)最后給出驗(yàn)證軌跡精度的習(xí)慣做法。2. 解包看資源結(jié)構(gòu)code與論文之間藏了哪些關(guān)鍵改動拿到壓縮包后我習(xí)慣先不急著編譯而是把目錄樹拉出來逐個(gè)文件看一遍。壓縮包解壓后是一個(gè)名為MY-LEGO-LOAM-main的目錄里面包含LeGO-LOAM核心代碼庫、cloud_msgs消息包、論文原文PDF、LICENSE以及兩個(gè)README文檔。只看這個(gè)結(jié)構(gòu)就能判斷這份資源不是簡單的源碼副本而是把論文實(shí)現(xiàn)、通信消息和許可信息都打包在一起方便你離線學(xué)習(xí)和復(fù)現(xiàn)。2.1 文件清單與職責(zé)劃分先說cloud_msgs。它是一個(gè)ROS消息包定義了LeGO-LOAM內(nèi)部通信所需的點(diǎn)云特征消息格式。原版LeGO-LOAM在運(yùn)行時(shí)前端和后端之間依靠這些自定義消息傳遞分割后的地面點(diǎn)、邊緣特征點(diǎn)和平面特征點(diǎn)。如果你單獨(dú)編譯主程序而忘記把cloud_msgs放進(jìn)工作空間CMake會在找消息依賴的階段直接報(bào)錯(cuò)因?yàn)閒ind_package找不到對應(yīng)的生成消息庫。包里包含這個(gè)目錄意味著作者修改后仍然保留了標(biāo)準(zhǔn)ROS消息接口沒有把消息定義內(nèi)嵌到主代碼里這一點(diǎn)對后續(xù)二次開發(fā)很友好。接著是LeGO-LOAM主目錄里面包含了src、include和launch文件夾以及CMakeLists.txt。原包中最重要的幾個(gè)節(jié)點(diǎn)文件分別是imageProjection、featureAssociation、mapOptmization和transformFusion分別負(fù)責(zé)點(diǎn)云投影與地面分割、幀間特征匹配與里程計(jì)估計(jì)、因子圖后端優(yōu)化以及位姿融合輸出。為適配KITTI而做的修改大概率集中在imageProjection和mapOptmization這兩個(gè)節(jié)點(diǎn)里因?yàn)榍罢呱婕皵?shù)據(jù)輸入的通道數(shù)、掃描線數(shù)目和水平分辨率后者涉及坐標(biāo)變換、關(guān)鍵幀索引以及回環(huán)檢測的觸發(fā)頻率。KITTI的Velodyne HDL-64E傳感器有64線、水平掃描點(diǎn)密度較高如果原版LeGO-LOAM的參數(shù)默認(rèn)按32線或者VLP-16設(shè)計(jì)投影圖像的高寬、角度分辨率、地面分割閾值都會偏差明顯。這種參數(shù)錯(cuò)配不會阻止編譯但會直接導(dǎo)致分割結(jié)果混亂特征點(diǎn)云稀疏或過密最后在可視化窗口里看到的就是點(diǎn)云漂移和重影。所以拿到這份資源的第一件事就是對比默認(rèn)yaml參數(shù)文件里與掃描行列相關(guān)的數(shù)字。我看到的版本里imageProjection.cpp中的N_SCAN和Horizon_SCAN宏定義已經(jīng)調(diào)整為64和1800對應(yīng)HDL-64E在10Hz旋轉(zhuǎn)下水平方向約0.2度分辨率的特性同時(shí)地面分割角度閾值從默認(rèn)的10度改為更適合城市道路掃描模式的設(shè)定值。這個(gè)細(xì)節(jié)非常關(guān)鍵它直接決定了后續(xù)特征提取的質(zhì)量。2.2 論文PDF為什么不該被跳過壓縮包里附帶了一篇論文PDF是原版LeGO-LOAM在IROS 2018發(fā)表的版本。很多人拿到資源后會忽略這個(gè)文件但我建議認(rèn)真讀一遍尤其是系統(tǒng)框架圖和地面分割部分描述。因?yàn)檫@份技術(shù)說明對公式推導(dǎo)寫得很簡潔但很關(guān)鍵LeGO-LOAM的核心思想是把三維激光點(diǎn)云投影成距離圖像在圖像域做地面分割再對非地面點(diǎn)做聚類和特征提取。如果沒有這個(gè)概念直接去看代碼里的pcl::PointCloud操作會看得一頭霧水。論文里還說明了為何使用兩個(gè)LIO階段先通過幀間匹配估計(jì)位姿增量再用因子圖優(yōu)化全局軌跡。這份包的代碼改動正好也是沿著這兩個(gè)階段展開的所以把論文與源碼對照讀能很快理解每個(gè)修改點(diǎn)的用意。還有一個(gè)容易被忽略的文件是README.md。原版README通常只給了編譯和運(yùn)行命令但這份適配版本里的README補(bǔ)充了KITTI數(shù)據(jù)集的目錄組織方式以及playKittiBag的推薦做法還有關(guān)于odometry話題名稱的說明。我在跑數(shù)據(jù)時(shí)就是按照README里提到的輸出話題名去訂閱的少走了不少彎路。LICENSE文件的存在則提醒你這個(gè)代碼基于BSD類似許可發(fā)布做學(xué)術(shù)研究和商業(yè)測試時(shí)需要注意保留版權(quán)聲明尤其在發(fā)論文或產(chǎn)品化時(shí)別把這行順手刪了。2.3 這套資源與傳統(tǒng)ORB-SLAM2資源包的邊界差異類似的“為某數(shù)據(jù)集適配某算法”的資源包網(wǎng)上還能找到許多ORB-SLAM2或VINS-Fusion的改裝版但這份包和它們不同。它不只是改了文件讀取格式而是動了算法核心的預(yù)處理參數(shù)和優(yōu)化策略。ORB-SLAM2的框架對傳感器是即插即用的其適配重點(diǎn)在相機(jī)標(biāo)定YAML而LOAM系列與激光雷達(dá)的結(jié)構(gòu)參數(shù)強(qiáng)耦合改動一旦不徹底整套算法就會失效。所以判斷一份LOAM資源是否值得下載關(guān)鍵看兩個(gè)點(diǎn)第一是否保留了配套的消息包源碼意味著作者確實(shí)從底層編譯通過第二是否帶有原論文PDF說明這不是一個(gè)隨手東拼西湊的壓縮包而是有理論依據(jù)的實(shí)踐產(chǎn)物。這兩點(diǎn)這份資源都滿足它在可復(fù)現(xiàn)和可學(xué)習(xí)之間找到了一個(gè)不錯(cuò)的平衡點(diǎn)。我還發(fā)現(xiàn)這個(gè)包在頂層目錄結(jié)構(gòu)上做了簡化去掉了原工作空間里utilities之類的輔助包。日常用的時(shí)候需要自己單獨(dú)安裝一些工具比如錄制KITTI數(shù)據(jù)播放包會用到的kitti2bag或rosbag錄制腳本。這不算缺陷反而能幫你理解roslaunch文件里各節(jié)點(diǎn)的啟動順序和依賴關(guān)系。我第一次用原版工作空間時(shí)經(jīng)常被一堆包之間的編譯依賴?yán)@暈而這個(gè)簡化版讓主算法部分更清晰動手起來更有把握。3. 編譯與環(huán)境搭建從catkin到依賴的完整復(fù)現(xiàn)步驟針對這份KITTI適配包編譯環(huán)境不是隨便一個(gè)Ubuntu版本都能直接跑通。我踩過的坑和驗(yàn)證過的組合是Ubuntu 18.04 ROS Melodic或者Ubuntu 20.04 ROS Noetic。兩者都能編譯成功但依賴細(xì)節(jié)稍有不同。如果你使用的是Noetic需要注意PCL版本是1.10某些舊代碼在點(diǎn)云類型轉(zhuǎn)換上會暴露隱式轉(zhuǎn)換不兼容的警告而這份包在這方面做過一些處理因此整體編譯能夠順利通過。3.1 創(chuàng)建工作空間并導(dǎo)入源碼代碼必須被放置到catkin工作空間的src目錄下編譯這個(gè)步驟沒有例外。先創(chuàng)建一個(gè)干凈的工作空間然后拷貝壓縮包解壓后的文件。具體命令如下source /opt/ros/noetic/setup.bash mkdir -p ~/catkin_ws/src cd ~/catkin_ws/src cp -r ~/Downloads/MY-LEGO-LOAM-main/* . catkin_init_workspace拷貝后用ls命令確認(rèn)cloud_msgs和LeGO-LOAM兩個(gè)目錄同時(shí)存在于src下不要把它們嵌套進(jìn)某個(gè)子目錄里。catkin_init_workspace是在src目錄中生成CMakeLists.txt符號鏈接它是catkin工具識別該目錄為工作空間一部分的依據(jù)。如果跳過這一行后面的catkin_make會在找不到工作空間配置時(shí)直接退出。3.2 安裝系統(tǒng)依賴并編譯LeGO-LOAM的核心依賴是PCL、OpenMP和GTSAM。PCL通常已經(jīng)隨ROS桌面版安裝但GTSAM需要手動安裝這是一個(gè)極易翻車的點(diǎn)。如果你跳過GTSAM安裝mapOptmization節(jié)點(diǎn)編譯時(shí)會報(bào)找不到gtsam相關(guān)頭文件。以下是安裝依賴和編譯的命令sudo apt-get update sudo apt-get install -y libgtsam-dev libgtsam4 sudo apt-get install -y ros-noetic-pcl-ros ros-noetic-velodyne-msgs cd ~/catkin_ws catkin_make -j4 source ~/catkin_ws/devel/setup.bashlibgtsam-dev是GTSAM的開發(fā)庫mapOptmization中的因子圖構(gòu)建代碼在編譯階段必須找到它的頭文件。ros-noetic-pcl-ros提供了點(diǎn)云消息與PCL點(diǎn)云類型之間的轉(zhuǎn)換函數(shù)另一個(gè)包是點(diǎn)云驅(qū)動相關(guān)的消息定義。編譯參數(shù)-j4限制為4線程避免在內(nèi)存不足時(shí)直接導(dǎo)致編譯進(jìn)程被系統(tǒng)殺死。編譯完成后執(zhí)行source命令加載生成的setup.bash這樣后續(xù)運(yùn)行l(wèi)aunch文件時(shí)ROS才能定位到新編譯的節(jié)點(diǎn)可執(zhí)行文件。GTSAM安裝后有一個(gè)常見問題如果你同時(shí)安裝了系統(tǒng)rosdep自動拉取的另一個(gè)版本多個(gè)版本會造成頭文件沖突。我在一次重裝環(huán)境中就遇到編譯時(shí)gtsam頭文件明明能找到運(yùn)行mapOptmization時(shí)卻無法解析符號。原因是系統(tǒng)里同時(shí)存在源碼編譯的GTSAM和apt安裝的GTSAMCMake的find_package優(yōu)先找到了舊版本。解決方式是把源碼編譯的GTSAM安裝路徑加入CMAKE_PREFIX_PATH或者干脆卸載非apt版本只保留libgtsam-dev。這個(gè)細(xì)節(jié)如果沒人提醒排查起來非常浪費(fèi)時(shí)間。3.3 熟悉launch文件與節(jié)點(diǎn)關(guān)系編譯通過后先打開launch目錄下的run.launch看一下。這個(gè)launch文件定義了三個(gè)主要節(jié)點(diǎn)的啟動方式lego_loam、transform_fusion和map_optimization。在終端中運(yùn)行命令可以這樣寫roslaunch lego_loam run.launch你要習(xí)慣把它當(dāng)作第一個(gè)和命令行交互的入口。run.launch里通常還會include一個(gè)參數(shù)文件比如params.yaml里面存放了地面分割閾值、聚類容差、特征提取參數(shù)和回環(huán)檢測開關(guān)。我給參數(shù)文件的建議是不要一次性把多個(gè)參數(shù)都改成理論值先保持默認(rèn)運(yùn)行一次數(shù)據(jù)記錄表現(xiàn)再逐項(xiàng)調(diào)整。一次只動一個(gè)參數(shù)否則遇到問題后沒法定位是哪個(gè)改動導(dǎo)致的。原版LeGO-LOAM在參數(shù)文件里把sensor_height設(shè)為零但在KITTI數(shù)據(jù)里激光雷達(dá)的安裝高度在1.73米左右。適配版本里這個(gè)值被作了修改這直接影響地面分割步驟對水平面的判定如果這個(gè)參數(shù)偏大會把路緣石和低矮植被也劃分成地面點(diǎn)導(dǎo)致后續(xù)特征點(diǎn)數(shù)量驟減。所以每次更換數(shù)據(jù)集時(shí)sensor_height這類物理標(biāo)定參數(shù)必須放在檢查列表最前面。4. 跑通離線數(shù)據(jù)把KITTI點(diǎn)云轉(zhuǎn)成rosbag的三段流程KITTI原始數(shù)據(jù)集的格式是二進(jìn)制點(diǎn)云bin文件每幀包含約12萬個(gè)點(diǎn)每個(gè)點(diǎn)由x、y、z和intensity四個(gè)float組成。LeGO-LOAM本身不能直接讀取這種格式它需要ROS的話題流。因此我們要做的第一步是把bin文件包裝成rosbag再通過rosbag play把點(diǎn)云循環(huán)發(fā)布出來。這個(gè)過程聽起來簡單隱藏的坑卻很多。4.1 準(zhǔn)備點(diǎn)云發(fā)布腳本我一般會寫一個(gè)小工具把KITTI sequence目錄下的bin文件讀出來連同校準(zhǔn)好的時(shí)間戳一起寫入一個(gè)rosbag。這里使用Python腳本是高效的方案下面的代碼實(shí)現(xiàn)了這個(gè)轉(zhuǎn)換import numpy as np import rosbag import rospy from sensor_msgs.msg import PointCloud2, PointField import struct def create_pointcloud2(cloud_np, stamp, frame_idmap): msg PointCloud2() msg.header.stamp stamp msg.header.frame_id frame_id msg.height 1 msg.width cloud_np.shape[0] msg.fields [ PointField(namex, offset0, datatypePointField.FLOAT32, count1), PointField(namey, offset4, datatypePointField.FLOAT32, count1), PointField(namez, offset8, datatypePointField.FLOAT32, count1), PointField(nameintensity, offset12, datatypePointField.FLOAT32, count1)] msg.is_bigendian False msg.point_step 16 msg.row_step msg.point_step * msg.width msg.is_dense True data [] for p in cloud_np: data.append(struct.pack(ffff, p[0], p[1], p[2], p[3])) msg.data b.join(data) return msg bag rosbag.Bag(kitti_odom.bag, w) for i in range(0, 4540): bin_path /path/to/sequence/%06d.bin % i cloud np.fromfile(bin_path, dtypenp.float32).reshape(-1, 4) stamp rospy.Time.from_sec(0.1 * i) msg create_pointcloud2(cloud, stamp) bag.write(/velodyne_points, msg, tstamp) bag.close()在編寫腳本時(shí)注意點(diǎn)云字段的順序x、y、z、intensity必須固定為上面寫的偏移量ROS這邊才能正確解析point_step為16字節(jié)的消息。如果你漏了intensity或者把它的datatype設(shè)為UINT8后續(xù)程序雖然能接收消息但PCL轉(zhuǎn)成PointXYZI時(shí)會得到錯(cuò)誤表示。還有一個(gè)特別容易踩坑的點(diǎn)frame_id必須統(tǒng)一發(fā)布端設(shè)為mapLeGO-LOAM內(nèi)部會忽略外部frame_id直接用自己坐標(biāo)變換后的點(diǎn)云名但如果發(fā)布端是空字符串部分版本會在回調(diào)函數(shù)里報(bào)TF異常。4.2 錄制并播放bag文件轉(zhuǎn)換腳本運(yùn)行后會生成一個(gè)kitti_odom.bag你可以先用rosbag info查看它的話題、消息數(shù)和時(shí)長rosbag info kitti_odom.bag看到輸出的topic為/velodyne_points類型為sensor_msgs/PointCloud2消息數(shù)為4540說明轉(zhuǎn)換成功。接下來啟動LeGO-LOAM主程序再播放bagroslaunch lego_loam run.launch在另一個(gè)終端里播放數(shù)據(jù)rosbag play kitti_odom.bag --rate 0.5 --clock--rate 0.5是以半速播放因?yàn)樗惴ㄔ诘貓D優(yōu)化階段耗時(shí)較長如果按原速播放容易造成點(diǎn)云處理的積壓表現(xiàn)就是地圖更新滯后、讀取到的可視化界面卡死。--clock讓bag里的時(shí)間戳作為ROS的時(shí)鐘源這樣點(diǎn)云消息的時(shí)間戳與算法內(nèi)部的時(shí)間步進(jìn)同步避免后續(xù)transformFusion輸出位姿時(shí)出現(xiàn)時(shí)間倒流。實(shí)際使用中如果電腦性能足夠也可以把rate調(diào)成0.7或0.8但半速是個(gè)最穩(wěn)妥的起點(diǎn)。4.3 檢查可視化輸出和話題運(yùn)行過程中你需要確認(rèn)以下幾個(gè)話題都有數(shù)據(jù)輸出/lego_loam/velodyne_cloud_registered是注冊后的完整點(diǎn)云用于可視化/lego_loam/odometry是前端的相對位姿里程計(jì)輸出/map_optimization/mapping_path是后端優(yōu)化后的軌跡。可以這樣檢查rostopic hz /lego_loam/odometry如果hz輸出在5到10之間波動說明前端里程計(jì)處理正常。如果頻率為0要回到bag播放端子先使用rostopic list檢查topic名稱是否一致。適配版里輸出話題名稱一般保持原樣但有些分支版本會在topic前面加上命名空間前綴這需要和README對照來調(diào)整。在用可視化工具繪制時(shí)建議同時(shí)訂閱registered點(diǎn)云和mapping_path這兩個(gè)話題前者看建圖效果后者看軌跡連續(xù)性。5. 避坑與常見問題五個(gè)讓里程計(jì)翻車的隱蔽環(huán)節(jié)這部分是根據(jù)我自己跑數(shù)據(jù)時(shí)反復(fù)繞彎得到的血淚經(jīng)驗(yàn)按照「現(xiàn)象 → 原因 → 解決」的順序列出來。如果你在復(fù)現(xiàn)過程中遇到類似表現(xiàn)可以順著這個(gè)索引快速定位問題所在?？右痪幾g通過但啟動launch文件后地圖窗口一直空白現(xiàn)象roslaunch成功啟動但rvis中沒有任何點(diǎn)云顯示控制臺也沒有輸出報(bào)錯(cuò)。原因這通常是點(diǎn)云話題未正確匹配。LeGO-LOAM默認(rèn)訂閱的topic是/velodyne_points如果你的bag發(fā)布在/points_raw或者帶前綴的命名空間下程序始終收不到輸入窗口自然空白。解決在run.launch文件里顯式重映射在節(jié)點(diǎn)配置中用remap標(biāo)簽將/velodyne_points映射到實(shí)際話題上或者修改bag發(fā)布端腳本的topic為/velodyne_points。我習(xí)慣用重映射方式因?yàn)檫@樣不用重新生成幾百M(fèi)B的bag文件?？佣锍逃?jì)輸出正常但軌跡在地圖上嚴(yán)重橫向漂移現(xiàn)象front-end odometry頻率正常地圖點(diǎn)云也沒有明顯發(fā)散但軌跡與KITTI真值對比時(shí)誤差隨著距離線性增長橫向偏移尤其明顯。原因一是IMU標(biāo)定參數(shù)與實(shí)際數(shù)據(jù)不一致LeGO-LOAM會使用IMU預(yù)積分來優(yōu)化幀間匹配如果IMU的角速度和線性加速度系數(shù)錯(cuò)誤等于給因子圖注入了錯(cuò)誤的先驗(yàn)二是激光雷達(dá)與IMU之間的外參矩陣不正確。解決先把mapOptmization中關(guān)于IMU的殘差權(quán)重調(diào)低或者暫時(shí)禁用IMU項(xiàng)只靠純激光匹配跑一次同一段數(shù)據(jù)。如果橫向漂移明顯減小則問題鎖定在IMU相關(guān)參數(shù)上。這時(shí)候去數(shù)據(jù)集的calib目錄里讀imu_to_velo變換確認(rèn)外參矩陣中的旋轉(zhuǎn)部分單位是弧度而不是角度這個(gè)單位搞錯(cuò)的現(xiàn)象就是角度漂移很難排查?？尤c(diǎn)云投影圖存在大量邊緣斷線特征點(diǎn)提取后明顯偏少現(xiàn)象在可視化中打開分割后的點(diǎn)云地面點(diǎn)不完整上方物體輪廓斷裂成一段段導(dǎo)致特征提取后的角點(diǎn)在rvis中稀疏到只剩零星幾個(gè)點(diǎn)。原因距離圖像的投影參數(shù)不適合HDL-64E。常見問題包括N_SCAN設(shè)置為64但Horizon_SCAN設(shè)置過大或過小導(dǎo)致點(diǎn)云在水平方向包裹不完整投影出圖像后點(diǎn)被錯(cuò)誤分割到邊界外同時(shí)地面分割閾值可能需要調(diào)到更陡的角度。解決參考KITTI單幀掃描點(diǎn)云的實(shí)際分布將N_SCAN固定為64Horizon_SCAN設(shè)為1800。如果陰影和斷裂仍然明顯把地面角度閾值從默認(rèn)的10度改到7度或12度每改一次跑一幀觀察分割效果。最佳情況是地面點(diǎn)成連續(xù)片狀非地面點(diǎn)的聚類邊界清晰完整?？铀膍apOptmization節(jié)點(diǎn)崩潰報(bào)gtsam Assertion Failed現(xiàn)象后端節(jié)點(diǎn)在運(yùn)行幾百幀后突然退出日志里出現(xiàn)gtsam相關(guān)斷言錯(cuò)誤地圖不再更新。原因因子圖中出現(xiàn)了退化關(guān)鍵幀或數(shù)值異常位姿。KITTI數(shù)據(jù)雖然采集質(zhì)量高但當(dāng)車輛經(jīng)過低矮隧道或兩側(cè)都是無特征墻面時(shí)前端匹配誤差會驟增后端因子圖優(yōu)化中出現(xiàn)信息矩陣病態(tài)觸發(fā)GTSAM內(nèi)部斷言。解決在啟動參數(shù)中把回環(huán)檢測的閾值設(shè)嚴(yán)同時(shí)提高關(guān)鍵幀的選取條件減少無效約束的插入。入口參數(shù)包括關(guān)鍵幀間的最小平移量或最小旋轉(zhuǎn)量默認(rèn)值一般偏向于低延遲對快速城市駕駛有時(shí)候過于敏感。把最小平移從0.3米提到0.5米能明顯減少退化幀的干擾。如果你只是測試單段數(shù)據(jù)不考慮實(shí)時(shí)性可以再調(diào)大一些?？游妩c(diǎn)云播放過程中地圖出現(xiàn)周期性跳變現(xiàn)象播放bag時(shí)前端里程計(jì)和歷史地圖偶爾會出現(xiàn)一次突然的整體平移跳變之后繼續(xù)跑后續(xù)誤差保持在這個(gè)新的偏移上。原因大多數(shù)情況是ROS時(shí)間戳和bag時(shí)間戳未同步導(dǎo)致后端在收到某一幀時(shí)位姿出現(xiàn)時(shí)間倒流。另一常見情況是因?yàn)閎ag播放倍速太快算法處理不過來系統(tǒng)緩沖溢出。解決嚴(yán)格按照第4章里的方式使用--clock和--rate 0.5來播放bag。檢查TF樹中map和camera_init坐標(biāo)系的父級關(guān)系如果疊加了一個(gè)未知坐標(biāo)系節(jié)點(diǎn)需要在launch時(shí)清空舊的TF。我曾經(jīng)在連續(xù)跑兩次bag時(shí)忘記重啟roscore遺留緩存直接導(dǎo)致跳變重啟整個(gè)工作空間后就恢復(fù)了。6. 軌跡精度驗(yàn)證用EVO測這個(gè)改版的真實(shí)誤差當(dāng)你成功跑出一段連續(xù)軌跡后不能只看畫面里的地圖好看就認(rèn)為定位準(zhǔn)確。地圖可視化會掩蓋幾個(gè)像素級的軌跡誤差而KITTI的價(jià)值恰恰在于它有高精度的ground truth所以最后一步一定要做定量評估。這里我推薦使用EVO工具它支持KITTI格式的軌跡文件能直接生成誤差圖和統(tǒng)計(jì)指標(biāo)。先把你保存的估計(jì)軌跡整理成KITTI格式。LeGO-LOAM會輸出每幀的位姿矩陣常見做法是把/map_optimization/mapping_path話題轉(zhuǎn)成TUM格式然后用EVO自帶工具轉(zhuǎn)成KITTI格式。EVO的輸入需要你手工寫一個(gè)data.txt文件每行12個(gè)數(shù)對應(yīng)3x4變換矩陣。寫完后用如下命令和真值進(jìn)行對比evo_ape kitti estimated.txt ground_truth.txt -a -p其中-a是同時(shí)計(jì)算絕對軌跡誤差的平移和旋轉(zhuǎn)部分-p代表生成誤差曲線圖。如果只想看相對位姿誤差的變化趨勢可以使用下列命令evo_rpe kitti estimated.txt ground_truth.txt -d 0.1 -u m -p-d 0.1表示每隔0.1米計(jì)算一次相對誤差-u m表示單位使用米。這樣能看到軌跡在哪些區(qū)段誤差突然增大。實(shí)測中我發(fā)現(xiàn)適配版的APE均值一般在2到4米范圍內(nèi)對于64線激光雷達(dá)來說這個(gè)成績和原版論文公布的結(jié)果基本吻合。如果誤差超過10米通常不是算法問題而是前面避坑章里提到的坐標(biāo)變換或時(shí)間戳問題需要回到第5章逐項(xiàng)排查。軌跡驗(yàn)證通過后還有一個(gè)值得做的關(guān)鍵步驟把地圖點(diǎn)云導(dǎo)出為PCD文件方便后續(xù)在CAD或GIS工具里做精度比對。LeGO-LOAM上線節(jié)點(diǎn)在完成mapping后會在工作目錄下生成地圖pcd常見名稱是map.pcd。當(dāng)你加載這個(gè)文件時(shí)注意它的坐標(biāo)系原點(diǎn)是在第一幀時(shí)刻的雷達(dá)位置還是地圖原點(diǎn)不同版本有差異。這個(gè)細(xì)節(jié)決定了導(dǎo)出后與其他傳感器數(shù)據(jù)疊加時(shí)是否需要做剛體變換。我現(xiàn)在的習(xí)慣是每換一次數(shù)據(jù)集先寫一個(gè)bash腳本把bin轉(zhuǎn)bag、跑算法、導(dǎo)出軌跡、調(diào)用EVO評估全部串起來。跑完一遍后直接看生成的error指標(biāo)圖用數(shù)字說話而不是憑肉眼判斷。從那以后我每次發(fā)布新改動前都會強(qiáng)制跑一遍這個(gè)流程評估結(jié)果不過關(guān)就回去繼續(xù)調(diào)參數(shù)不再靠感覺“好像準(zhǔn)了”來結(jié)束。希望這一套完整流程對你的復(fù)現(xiàn)和改造有幫助。本文還有配套的精品資源點(diǎn)擊獲取