12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix loop closure factor

release/4.3a0
Varun Agrawal 2025-01-30 10:49:29 -05:00
parent 5b37647e11
commit b60baff4df
1 changed files with 5 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
examples/Hybrid_City10000.cpp
View File

@ -91,12 +91,14 @@ HybridNonlinearFactor HybridLoopClosureFactor(size_t loop_counter, size_t key_s,
const Pose2& measurement) { const Pose2& measurement) {
DiscreteKey l(L(loop_counter), 2); DiscreteKey l(L(loop_counter), 2);
auto open_loop_model = noiseModel::Diagonal::Sigmas(Vector3::Ones() * 10);
auto f0 = std::make_shared<BetweenFactor<Pose2>>( auto f0 = std::make_shared<BetweenFactor<Pose2>>(
X(key_s), X(key_t), measurement, X(key_s), X(key_t), measurement, open_loop_model);
noiseModel::Diagonal::Sigmas(Vector3::Ones() * 10));
auto f1 = std::make_shared<BetweenFactor<Pose2>>( auto f1 = std::make_shared<BetweenFactor<Pose2>>(
X(key_s), X(key_t), measurement, pose_noise_model); X(key_s), X(key_t), measurement, pose_noise_model);
std::vector<NonlinearFactorValuePair> factors{{f0, 0.0}, {f1, 0.0}}; std::vector<NonlinearFactorValuePair> factors{
{f0, open_loop_model->negLogConstant()},
{f1, pose_noise_model->negLogConstant()}};
HybridNonlinearFactor mixtureFactor(l, {f0, f1}); HybridNonlinearFactor mixtureFactor(l, {f0, f1});
return mixtureFactor; return mixtureFactor;
} }