search

gisbi-kim / SC-A-LOAM

Robust LiDAR SLAM with a versatile plug-and-play loop closing and pose-graph optimization.
438 stars 95 forks source link

how to set loop factor noise？ #28

Open aiqibaobei opened 4 months ago

aiqibaobei commented 4 months ago

Dear author,

Hello! Thank you for your excellent contribution! I am currently using sc-pgo+ros2 to optimize the results of FastLIO. After obtaining the correct loop closure factors and adding them to the pose graph, I found that I did not get the expected results. However, I noticed that when I replaced the loop noise code segment with this code segment, it seemed that the loop closure had an effect. Could you please help me identify the issue?

code 1：
double loopNoiseScore = 0.5; // constant is ok... gtsam::Vector robustNoiseVector6(6); // gtsam::Pose3 factor has 6 elements (6D) robustNoiseVector6 << loopNoiseScore, loopNoiseScore, loopNoiseScore, loopNoiseScore, loopNoiseScore, loopNoiseScore; robustLoopNoise = gtsam::noiseModel::Robust::Create( gtsam::noiseModel::mEstimator::Cauchy::Create(1), // optional: replacing Cauchy by DCS or GemanMcClure is okay but Cauchy is empirically good. gtsam::noiseModel::Diagonal::Variances(robustNoiseVector6) );

code 2： gtsam::Vector loopNoiseVector6(6); double loopScore = 1e-7; loopNoiseVector6 << loopScore,loopScore,loopScore,loopScore,loopScore,loopScore; robustLoopNoise = gtsam::noiseModel::Diagonal::Variances(loopNoiseVector6);