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