：GTSAM因子圖優(yōu)化與閉環(huán)檢測(cè)代碼全解)
LIO-SAM-DetailedNote源碼解析(五)GTSAM因子圖優(yōu)化與閉環(huán)檢測(cè)代碼全解【免費(fèi)下載鏈接】LIO-SAM-DetailedNoteLIO-SAM源碼詳細(xì)注釋3D SLAM融合激光、IMU、GPS項(xiàng)目地址: https://gitcode.com/gh_mirrors/li/LIO-SAM-DetailedNoteLIO-SAM-DetailedNote 是一個(gè)帶詳細(xì)中文注釋的 LIO-SAM 3D SLAM 項(xiàng)目融合激光雷達(dá)、IMU、GPS 三類傳感器實(shí)現(xiàn)高精定位。本篇作為源碼系列第五篇完整拆解 LIO-SAM 最核心的兩塊代碼基于 GTSAM 的因子圖優(yōu)化ISAM2 增量求解、里程計(jì)/GPS/閉環(huán)三類因子的構(gòu)建與閉環(huán)檢測(cè)候選幀搜索 ICP 匹配。讀完你能看懂整個(gè)系統(tǒng)如何把漂移的里程計(jì)軌跡拉回正軌。一、因子圖優(yōu)化在 LIO-SAM 中的位置LIO-SAM 的全部邏輯只有 4 個(gè) cpp 文件因子圖優(yōu)化與閉環(huán)檢測(cè)都集中在MapOptimization節(jié)點(diǎn)中實(shí)現(xiàn)見 src/mapOptmization.cpp注意原項(xiàng)目文件名就是 Optmization 拼寫。四個(gè)節(jié)點(diǎn)的協(xié)作關(guān)系可參考 ROS 節(jié)點(diǎn)拓?fù)鋱D整個(gè)數(shù)據(jù)流可以概括為一句話scan-to-map 給出當(dāng)前幀位姿初值 → 封裝成因子加入 GTSAM 因子圖 → ISAM2 增量?jī)?yōu)化修正所有歷史關(guān)鍵幀位姿閉環(huán)檢測(cè)在后臺(tái)線程周期性執(zhí)行發(fā)現(xiàn)閉環(huán)后把閉環(huán)邊也變成因子加入圖中。二、先看懂 LIO-SAM 的因子圖長(zhǎng)什么樣LIO-SAM 的因子圖非常簡(jiǎn)潔每個(gè)關(guān)鍵幀位姿是一個(gè)Pose3變量節(jié)點(diǎn)6 自由度3 平移 3 旋轉(zhuǎn)節(jié)點(diǎn)之間用不同的因子連邊因子類型GTSAM 類含義噪聲水平先驗(yàn)因子PriorFactorPose3第一幀位姿固定為原點(diǎn)附近較松平移 1e-2里程計(jì)因子BetweenFactorPose3相鄰兩關(guān)鍵幀的相對(duì)位姿觀測(cè)很緊1e-6 級(jí)GPS 因子GPSFactor當(dāng)前關(guān)鍵幀的絕對(duì)位置約束按 GPS 協(xié)方差下限 1m2閉環(huán)因子BetweenFactorPose3時(shí)間上相距很遠(yuǎn)的兩幀之間的位姿變換由 ICP 擬合得分決定變量節(jié)點(diǎn)與因子的聲明集中在 mapOptmization.cppNonlinearFactorGraph gtSAMgraph存圖ISAM2 *isam是增量?jī)?yōu)化器。三、ISAM2 增量?jī)?yōu)化器為什么選它在構(gòu)造函數(shù)中可以看到 ISAM2 的兩個(gè)關(guān)鍵參數(shù)mapOptmization.cppISAM2Params parameters; parameters.relinearizeThreshold 0.1; // 解變化超過0.1就重新線性化 parameters.relinearizeSkip 1; // 每隔1次更新重線性化 isam new ISAM2(parameters);與一次性求解整張圖不同ISAM2 是增量式每加入一個(gè)新關(guān)鍵幀只把新增的因子喂給它舊圖結(jié)構(gòu)Cholesky 分解得以復(fù)用所以 LIO-SAM 能實(shí)時(shí)跑圖優(yōu)化。閉環(huán)修正后舊幀位姿變化大relinearizeThreshold 0.1保證解變化超過閾值時(shí)自動(dòng)重新線性化兼顧速度與精度。 補(bǔ)充IMU 預(yù)積分模塊 src/imuPreintegration.cpp 內(nèi)部也有一張小 GTSAM 圖狀態(tài)為位姿速度IMU 偏置并且每 100 幀激光里程計(jì)就重置一次優(yōu)化器、用邊緣化協(xié)方差做先驗(yàn)防止圖無限膨脹——這是工程上保持優(yōu)化效率的常用技巧。四、關(guān)鍵幀策略哪些幀會(huì)進(jìn)入因子圖并不是每幀激光都進(jìn)圖。saveFrame()mapOptmization.cpp計(jì)算當(dāng)前幀與上一關(guān)鍵幀的位姿增量只有滿足其一才保存為關(guān)鍵幀旋轉(zhuǎn)增量roll / pitch / yaw 任一超過surroundingkeyframeAddingAngleThreshold默認(rèn) 0.2 rad即約 11.5°平移距離超過surroundingkeyframeAddingDistThreshold默認(rèn) 1 m。閾值在 config/params.yaml 中配置。這既控制了圖的規(guī)模也避免了大量冗余節(jié)點(diǎn)。五、三大因子詳解addOdomFactor / addGPSFactor / addLoopFactor5.1 里程計(jì)因子addOdomFactor邏輯在 mapOptmization.cpp分兩種情況第一幀添加PriorFactorPose3先驗(yàn)因子噪聲對(duì)角陣為(1e-2, 1e-2, π2, 1e8, 1e8, 1e8)——平移和朝向很松而 roll/pitch 的 1e8 意味著首幀水平姿態(tài)幾乎被鎖死為整個(gè)坐標(biāo)系定基調(diào)。后續(xù)幀添加BetweenFactorPose3觀測(cè)值是上一關(guān)鍵幀 → 當(dāng)前幀的相對(duì)位姿噪聲為(1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4)。注意平移噪聲1e-6遠(yuǎn)小于旋轉(zhuǎn)噪聲1e-4因?yàn)?LIO-SAM 的 scan-to-map 對(duì)平移估計(jì)最可靠。5.2 GPS 因子addGPSFactorGPS 因子是全局錨點(diǎn)防止長(zhǎng)距離累積漂移但有嚴(yán)格的篩選條件mapOptmization.cpp首尾關(guān)鍵幀距離不足 5 m 時(shí)不添加剛起步?jīng)]必要當(dāng)前幀位姿協(xié)方差已經(jīng)很小poseCovThreshold以下時(shí)不添加——圖優(yōu)化自身已經(jīng)足夠準(zhǔn)就別讓 GPS 拉偏了只取當(dāng)前幀時(shí)刻 ±0.2 s 窗口內(nèi)的 GPS 里程計(jì)GPS 自身協(xié)方差超過gpsCovThreshold的無效與上一個(gè) GPS 因子距離不足 5 m 時(shí)跳過每隔 5 m 才加一個(gè) GPS 因子。關(guān)于高度若useGpsElevation為 false默認(rèn)則 z 直接取激光里程計(jì)的值并設(shè)很小噪聲即GPS 只約束平面位置不管高度——因?yàn)?GPS 高度噪聲大。5.3 閉環(huán)因子addLoopFactor閉環(huán)因子最簡(jiǎn)單也最值錢把閉環(huán)線程塞進(jìn)loopIndexQueue / loopPoseQueue / loopNoiseQueue三個(gè)隊(duì)列的數(shù)據(jù)逐條取出構(gòu)建BetweenFactorPose3加入圖中mapOptmization.cpp。它的噪聲大小不是拍腦袋定的而是來自 ICP 擬合得分見第七節(jié)。六、優(yōu)化執(zhí)行saveKeyFramesAndFactor 的完整流程saveKeyFramesAndFactor()mapOptmization.cpp是每幀主流程的收口步驟如下saveFrame()判斷是否為關(guān)鍵幀不是則直接返回依次調(diào)用addOdomFactor()、addGPSFactor()、addLoopFactor()把新因子掛到gtSAMgraph上調(diào)用isam-update(gtSAMgraph, initialEstimate)增量?jī)?yōu)化若本輪有因子被標(biāo)記為aLoopIsClosed加了 GPS 或閉環(huán)因子則額外連續(xù)update()五次讓所有受影響的舊關(guān)鍵幀位姿充分收斂——這是閉環(huán)后整條軌跡一起動(dòng)的由來清空?qǐng)DgtSAMgraph.resize(0)從isamCurrentEstimate取回所有關(guān)鍵幀的新位姿用isam-marginalCovariance()獲取當(dāng)前幀協(xié)方差更新路徑、發(fā)布里程計(jì)。七、閉環(huán)檢測(cè)從候選搜索到 ICP 匹配閉環(huán)在獨(dú)立線程loopClosureThread()mapOptmization.cpp中按loopClosureFrequency默認(rèn) 1 Hz運(yùn)行核心是performLoopClosure()mapOptmization.cpp第 1 步找候選閉環(huán)幀detectLoopClosureDistance用 KD-Tree 在全部歷史關(guān)鍵幀中做半徑搜索historyKeyframeSearchRadius默認(rèn) 1 m找出空間位置上離我很近的關(guān)鍵幀再?gòu)闹刑暨x**時(shí)間差超過historyKeyframeSearchTimeDiff默認(rèn) 20 s**的那一幀作為候選——空間近而時(shí)間遠(yuǎn)才可能是閉環(huán)mapOptmization.cpp。第 2 步提取特征并降采樣用loopFindNearKeyframes()提取當(dāng)前關(guān)鍵幀1 幀與候選閉環(huán)幀前后相鄰若干幀historyKeyframeSearchNum默認(rèn) 15 幀的角點(diǎn)面點(diǎn)特征并降采樣當(dāng)前幀特征不足 300 點(diǎn)或局部 map 不足 1000 點(diǎn)則放棄。第 3 步ICP 配準(zhǔn)并驗(yàn)證icp.align()執(zhí)行 PCL 的 ICP最大迭代 100 次必須同時(shí)滿足hasConverged()為真且getFitnessScore() historyKeyframeFitnessScore默認(rèn) 0.4否則丟棄閉環(huán)邊的噪聲用fitnessScore填充 6 維對(duì)角陣——配準(zhǔn)越準(zhǔn)閉環(huán)約束越緊這是一個(gè)很優(yōu)雅的自適應(yīng)設(shè)計(jì)。第 4 步不直接改位姿注意代碼注釋特別強(qiáng)調(diào)閉環(huán)時(shí)不立即更新當(dāng)前幀位姿而是把閉環(huán)邊數(shù)據(jù)推入隊(duì)列等下一幀saveKeyFramesAndFactor()執(zhí)行時(shí)作為因子進(jìn)圖由圖優(yōu)化統(tǒng)一修正全局軌跡。這樣保證了軌跡的數(shù)學(xué)一致性。閉環(huán)成功后RViz 中會(huì)看到閉環(huán)點(diǎn)青球與閉環(huán)邊黃色連線效果如下官方文檔目錄中還有閉環(huán)效果演示動(dòng)畫可以觀察軌跡回環(huán)收斂的過程八、關(guān)鍵參數(shù)速查與小結(jié)參數(shù)默認(rèn)值作用surroundingkeyframeAddingDistThreshold1.0 m關(guān)鍵幀最小平移間隔surroundingkeyframeAddingAngleThreshold0.2 rad關(guān)鍵幀最小旋轉(zhuǎn)間隔historyKeyframeSearchRadius1.0 m閉環(huán)候選空間搜索半徑historyKeyframeSearchTimeDiff20.0 s閉環(huán)候選最小時(shí)間差historyKeyframeFitnessScore0.4ICP 閉環(huán)接受閾值gpsCovThreshold0.5GPS 協(xié)方差上限imuRPYWeight0.1IMU 姿態(tài)對(duì) roll/pitch 的融合權(quán)重一句話小結(jié)LIO-SAM 的高精度來自緊的里程計(jì)因子維持局部一致性 GPS/閉環(huán)因子提供全局約束ISAM2 的增量求解讓它實(shí)時(shí)可行閉環(huán)檢測(cè)則用空間近、時(shí)間遠(yuǎn) ICP 雙重驗(yàn)證的樸素而穩(wěn)健策略找到回環(huán)再把配準(zhǔn)質(zhì)量轉(zhuǎn)化為因子噪聲——這套工程設(shè)計(jì)對(duì)理解任何激光 SLAM 系統(tǒng)都是很好的范本。本倉(cāng)庫(kù)每個(gè)源碼文件都有詳細(xì)中文注釋配合 README.md 的整體流程說明與 frames.pdf 的坐標(biāo)系定義文檔可以逐行對(duì)照學(xué)習(xí)。【免費(fèi)下載鏈接】LIO-SAM-DetailedNoteLIO-SAM源碼詳細(xì)注釋3D SLAM融合激光、IMU、GPS項(xiàng)目地址: https://gitcode.com/gh_mirrors/li/LIO-SAM-DetailedNote創(chuàng)作聲明：本文部分內(nèi)容由AI輔助生成（AIGC），僅供參考 LIO-SAM-DetailedNote源碼解析(五)：GTSAM因子圖優(yōu)化與閉環(huán)檢測(cè)代碼全解
LIO-SAM-DetailedNote源碼解析(五)GTSAM因子圖優(yōu)化與閉環(huán)檢測(cè)代碼全解【免費(fèi)下載鏈接】LIO-SAM-DetailedNoteLIO-SAM源碼詳細(xì)注釋3D SLAM融合激光、IMU、GPS項(xiàng)目地址: https://gitcode.com/gh_mirrors/li/LIO-SAM-DetailedNoteLIO-SAM-DetailedNote 是一個(gè)帶詳細(xì)中文注釋的 LIO-SAM 3D SLAM 項(xiàng)目融合激光雷達(dá)、IMU、GPS 三類傳感器實(shí)現(xiàn)高精定位。本篇作為源碼系列第五篇完整拆解 LIO-SAM 最核心的兩塊代碼基于 GTSAM 的因子圖優(yōu)化ISAM2 增量求解、里程計(jì)/GPS/閉環(huán)三類因子的構(gòu)建與閉環(huán)檢測(cè)候選幀搜索 ICP 匹配。讀完你能看懂整個(gè)系統(tǒng)如何把漂移的里程計(jì)軌跡拉回正軌。一、因子圖優(yōu)化在 LIO-SAM 中的位置LIO-SAM 的全部邏輯只有 4 個(gè) cpp 文件因子圖優(yōu)化與閉環(huán)檢測(cè)都集中在MapOptimization節(jié)點(diǎn)中實(shí)現(xiàn)見 src/mapOptmization.cpp注意原項(xiàng)目文件名就是 Optmization 拼寫。四個(gè)節(jié)點(diǎn)的協(xié)作關(guān)系可參考 ROS 節(jié)點(diǎn)拓?fù)鋱D整個(gè)數(shù)據(jù)流可以概括為一句話scan-to-map 給出當(dāng)前幀位姿初值 → 封裝成因子加入 GTSAM 因子圖 → ISAM2 增量?jī)?yōu)化修正所有歷史關(guān)鍵幀位姿閉環(huán)檢測(cè)在后臺(tái)線程周期性執(zhí)行發(fā)現(xiàn)閉環(huán)后把閉環(huán)邊也變成因子加入圖中。二、先看懂 LIO-SAM 的因子圖長(zhǎng)什么樣LIO-SAM 的因子圖非常簡(jiǎn)潔每個(gè)關(guān)鍵幀位姿是一個(gè)Pose3變量節(jié)點(diǎn)6 自由度3 平移 3 旋轉(zhuǎn)節(jié)點(diǎn)之間用不同的因子連邊因子類型GTSAM 類含義噪聲水平先驗(yàn)因子PriorFactorPose3第一幀位姿固定為原點(diǎn)附近較松平移 1e-2里程計(jì)因子BetweenFactorPose3相鄰兩關(guān)鍵幀的相對(duì)位姿觀測(cè)很緊1e-6 級(jí)GPS 因子GPSFactor當(dāng)前關(guān)鍵幀的絕對(duì)位置約束按 GPS 協(xié)方差下限 1m2閉環(huán)因子BetweenFactorPose3時(shí)間上相距很遠(yuǎn)的兩幀之間的位姿變換由 ICP 擬合得分決定變量節(jié)點(diǎn)與因子的聲明集中在 mapOptmization.cppNonlinearFactorGraph gtSAMgraph存圖ISAM2 *isam是增量?jī)?yōu)化器。三、ISAM2 增量?jī)?yōu)化器為什么選它在構(gòu)造函數(shù)中可以看到 ISAM2 的兩個(gè)關(guān)鍵參數(shù)mapOptmization.cppISAM2Params parameters; parameters.relinearizeThreshold 0.1; // 解變化超過0.1就重新線性化 parameters.relinearizeSkip 1; // 每隔1次更新重線性化 isam new ISAM2(parameters);與一次性求解整張圖不同ISAM2 是增量式每加入一個(gè)新關(guān)鍵幀只把新增的因子喂給它舊圖結(jié)構(gòu)Cholesky 分解得以復(fù)用所以 LIO-SAM 能實(shí)時(shí)跑圖優(yōu)化。閉環(huán)修正后舊幀位姿變化大relinearizeThreshold 0.1保證解變化超過閾值時(shí)自動(dòng)重新線性化兼顧速度與精度。 補(bǔ)充IMU 預(yù)積分模塊 src/imuPreintegration.cpp 內(nèi)部也有一張小 GTSAM 圖狀態(tài)為位姿速度IMU 偏置并且每 100 幀激光里程計(jì)就重置一次優(yōu)化器、用邊緣化協(xié)方差做先驗(yàn)防止圖無限膨脹——這是工程上保持優(yōu)化效率的常用技巧。四、關(guān)鍵幀策略哪些幀會(huì)進(jìn)入因子圖并不是每幀激光都進(jìn)圖。saveFrame()mapOptmization.cpp計(jì)算當(dāng)前幀與上一關(guān)鍵幀的位姿增量只有滿足其一才保存為關(guān)鍵幀旋轉(zhuǎn)增量roll / pitch / yaw 任一超過surroundingkeyframeAddingAngleThreshold默認(rèn) 0.2 rad即約 11.5°平移距離超過surroundingkeyframeAddingDistThreshold默認(rèn) 1 m。閾值在 config/params.yaml 中配置。這既控制了圖的規(guī)模也避免了大量冗余節(jié)點(diǎn)。五、三大因子詳解addOdomFactor / addGPSFactor / addLoopFactor5.1 里程計(jì)因子addOdomFactor邏輯在 mapOptmization.cpp分兩種情況第一幀添加PriorFactorPose3先驗(yàn)因子噪聲對(duì)角陣為(1e-2, 1e-2, π2, 1e8, 1e8, 1e8)——平移和朝向很松而 roll/pitch 的 1e8 意味著首幀水平姿態(tài)幾乎被鎖死為整個(gè)坐標(biāo)系定基調(diào)。后續(xù)幀添加BetweenFactorPose3觀測(cè)值是上一關(guān)鍵幀 → 當(dāng)前幀的相對(duì)位姿噪聲為(1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4)。注意平移噪聲1e-6遠(yuǎn)小于旋轉(zhuǎn)噪聲1e-4因?yàn)?LIO-SAM 的 scan-to-map 對(duì)平移估計(jì)最可靠。5.2 GPS 因子addGPSFactorGPS 因子是全局錨點(diǎn)防止長(zhǎng)距離累積漂移但有嚴(yán)格的篩選條件mapOptmization.cpp首尾關(guān)鍵幀距離不足 5 m 時(shí)不添加剛起步?jīng)]必要當(dāng)前幀位姿協(xié)方差已經(jīng)很小poseCovThreshold以下時(shí)不添加——圖優(yōu)化自身已經(jīng)足夠準(zhǔn)就別讓 GPS 拉偏了只取當(dāng)前幀時(shí)刻 ±0.2 s 窗口內(nèi)的 GPS 里程計(jì)GPS 自身協(xié)方差超過gpsCovThreshold的無效與上一個(gè) GPS 因子距離不足 5 m 時(shí)跳過每隔 5 m 才加一個(gè) GPS 因子。關(guān)于高度若useGpsElevation為 false默認(rèn)則 z 直接取激光里程計(jì)的值并設(shè)很小噪聲即GPS 只約束平面位置不管高度——因?yàn)?GPS 高度噪聲大。5.3 閉環(huán)因子addLoopFactor閉環(huán)因子最簡(jiǎn)單也最值錢把閉環(huán)線程塞進(jìn)loopIndexQueue / loopPoseQueue / loopNoiseQueue三個(gè)隊(duì)列的數(shù)據(jù)逐條取出構(gòu)建BetweenFactorPose3加入圖中mapOptmization.cpp。它的噪聲大小不是拍腦袋定的而是來自 ICP 擬合得分見第七節(jié)。六、優(yōu)化執(zhí)行saveKeyFramesAndFactor 的完整流程saveKeyFramesAndFactor()mapOptmization.cpp是每幀主流程的收口步驟如下saveFrame()判斷是否為關(guān)鍵幀不是則直接返回依次調(diào)用addOdomFactor()、addGPSFactor()、addLoopFactor()把新因子掛到gtSAMgraph上調(diào)用isam-update(gtSAMgraph, initialEstimate)增量?jī)?yōu)化若本輪有因子被標(biāo)記為aLoopIsClosed加了 GPS 或閉環(huán)因子則額外連續(xù)update()五次讓所有受影響的舊關(guān)鍵幀位姿充分收斂——這是閉環(huán)后整條軌跡一起動(dòng)的由來清空?qǐng)DgtSAMgraph.resize(0)從isamCurrentEstimate取回所有關(guān)鍵幀的新位姿用isam-marginalCovariance()獲取當(dāng)前幀協(xié)方差更新路徑、發(fā)布里程計(jì)。七、閉環(huán)檢測(cè)從候選搜索到 ICP 匹配閉環(huán)在獨(dú)立線程loopClosureThread()mapOptmization.cpp中按loopClosureFrequency默認(rèn) 1 Hz運(yùn)行核心是performLoopClosure()mapOptmization.cpp第 1 步找候選閉環(huán)幀detectLoopClosureDistance用 KD-Tree 在全部歷史關(guān)鍵幀中做半徑搜索historyKeyframeSearchRadius默認(rèn) 1 m找出空間位置上離我很近的關(guān)鍵幀再?gòu)闹刑暨x**時(shí)間差超過historyKeyframeSearchTimeDiff默認(rèn) 20 s**的那一幀作為候選——空間近而時(shí)間遠(yuǎn)才可能是閉環(huán)mapOptmization.cpp。第 2 步提取特征并降采樣用loopFindNearKeyframes()提取當(dāng)前關(guān)鍵幀1 幀與候選閉環(huán)幀前后相鄰若干幀historyKeyframeSearchNum默認(rèn) 15 幀的角點(diǎn)面點(diǎn)特征并降采樣當(dāng)前幀特征不足 300 點(diǎn)或局部 map 不足 1000 點(diǎn)則放棄。第 3 步ICP 配準(zhǔn)并驗(yàn)證icp.align()執(zhí)行 PCL 的 ICP最大迭代 100 次必須同時(shí)滿足hasConverged()為真且getFitnessScore() historyKeyframeFitnessScore默認(rèn) 0.4否則丟棄閉環(huán)邊的噪聲用fitnessScore填充 6 維對(duì)角陣——配準(zhǔn)越準(zhǔn)閉環(huán)約束越緊這是一個(gè)很優(yōu)雅的自適應(yīng)設(shè)計(jì)。第 4 步不直接改位姿注意代碼注釋特別強(qiáng)調(diào)閉環(huán)時(shí)不立即更新當(dāng)前幀位姿而是把閉環(huán)邊數(shù)據(jù)推入隊(duì)列等下一幀saveKeyFramesAndFactor()執(zhí)行時(shí)作為因子進(jìn)圖由圖優(yōu)化統(tǒng)一修正全局軌跡。這樣保證了軌跡的數(shù)學(xué)一致性。閉環(huán)成功后RViz 中會(huì)看到閉環(huán)點(diǎn)青球與閉環(huán)邊黃色連線效果如下官方文檔目錄中還有閉環(huán)效果演示動(dòng)畫可以觀察軌跡回環(huán)收斂的過程八、關(guān)鍵參數(shù)速查與小結(jié)參數(shù)默認(rèn)值作用surroundingkeyframeAddingDistThreshold1.0 m關(guān)鍵幀最小平移間隔surroundingkeyframeAddingAngleThreshold0.2 rad關(guān)鍵幀最小旋轉(zhuǎn)間隔historyKeyframeSearchRadius1.0 m閉環(huán)候選空間搜索半徑historyKeyframeSearchTimeDiff20.0 s閉環(huán)候選最小時(shí)間差historyKeyframeFitnessScore0.4ICP 閉環(huán)接受閾值gpsCovThreshold0.5GPS 協(xié)方差上限imuRPYWeight0.1IMU 姿態(tài)對(duì) roll/pitch 的融合權(quán)重一句話小結(jié)LIO-SAM 的高精度來自緊的里程計(jì)因子維持局部一致性 GPS/閉環(huán)因子提供全局約束ISAM2 的增量求解讓它實(shí)時(shí)可行閉環(huán)檢測(cè)則用空間近、時(shí)間遠(yuǎn) ICP 雙重驗(yàn)證的樸素而穩(wěn)健策略找到回環(huán)再把配準(zhǔn)質(zhì)量轉(zhuǎn)化為因子噪聲——這套工程設(shè)計(jì)對(duì)理解任何激光 SLAM 系統(tǒng)都是很好的范本。本倉(cāng)庫(kù)每個(gè)源碼文件都有詳細(xì)中文注釋配合 README.md 的整體流程說明與 frames.pdf 的坐標(biāo)系定義文檔可以逐行對(duì)照學(xué)習(xí)。【免費(fèi)下載鏈接】LIO-SAM-DetailedNoteLIO-SAM源碼詳細(xì)注釋3D SLAM融合激光、IMU、GPS項(xiàng)目地址: https://gitcode.com/gh_mirrors/li/LIO-SAM-DetailedNote創(chuàng)作聲明：本文部分內(nèi)容由AI輔助生成（AIGC），僅供參考