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

Don't optimize every time

release/4.3a0
Frank Dellaert 2025-01-29 17:54:04 -05:00
parent 36f2a3d298
commit 555a2173a3
1 changed files with 13 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
examples/Hybrid_City10000.cpp
View File

@ -43,7 +43,7 @@ using symbol_shorthand::L;
using symbol_shorthand::M; using symbol_shorthand::M;
using symbol_shorthand::X; using symbol_shorthand::X;
const size_t kMaxLoopCount = 3000; // Example default value const size_t kMaxLoopCount = 2000; // Example default value
auto kPriorNoiseModel = noiseModel::Diagonal::Sigmas( auto kPriorNoiseModel = noiseModel::Diagonal::Sigmas(
(Vector(3) << 0.0001, 0.0001, 0.0001).finished()); (Vector(3) << 0.0001, 0.0001, 0.0001).finished());
@ -123,8 +123,8 @@ class Experiment {
HybridGaussianFactorGraph linearized = *graph.linearize(initial); HybridGaussianFactorGraph linearized = *graph.linearize(initial);
smoother.update(linearized, maxNrHypotheses); smoother.update(linearized, maxNrHypotheses);
graph.resize(0); graph.resize(0);
HybridValues delta = smoother.hybridBayesNet().optimize(); // HybridValues delta = smoother.hybridBayesNet().optimize();
result->insert_or_assign(initial.retract(delta.continuous())); // result->insert_or_assign(initial.retract(delta.continuous()));
} }
public: public:
@ -202,6 +202,7 @@ class Experiment {
graph_.push_back(mixtureFactor); graph_.push_back(mixtureFactor);
discreteCount++; discreteCount++;
doSmootherUpdate = true; doSmootherUpdate = true;
std::cout << "mixtureFactor: " << keyS << " " << keyT << std::endl;
} else { } else {
graph_.add(BetweenFactor<Pose2>(X(keyS), X(keyT), odomPose, graph_.add(BetweenFactor<Pose2>(X(keyS), X(keyT), odomPose,
kPoseNoiseModel)); kPoseNoiseModel));
@ -212,6 +213,8 @@ class Experiment {
// Loop closure // Loop closure
HybridNonlinearFactor loopFactor = HybridNonlinearFactor loopFactor =
hybridLoopClosureFactor(loopCount, keyS, keyT, odomPose); hybridLoopClosureFactor(loopCount, keyS, keyT, odomPose);
// print loop closure event keys:
std::cout << "Loop closure: " << keyS << " " << keyT << std::endl;
graph_.add(loopFactor); graph_.add(loopFactor);
doSmootherUpdate = true; doSmootherUpdate = true;
loopCount++; loopCount++;
@ -224,6 +227,7 @@ class Experiment {
afterUpdate = clock(); afterUpdate = clock();
smootherUpdateTimes.push_back({index, afterUpdate - beforeUpdate}); smootherUpdateTimes.push_back({index, afterUpdate - beforeUpdate});
gttoc_(SmootherUpdate); gttoc_(SmootherUpdate);
doSmootherUpdate = false;
} }
// Record timing for odometry edges only // Record timing for odometry edges only
@ -297,7 +301,8 @@ int main() {
Experiment experiment(findExampleDataFile("T1_city10000_04.txt")); Experiment experiment(findExampleDataFile("T1_city10000_04.txt"));
// Experiment experiment("../data/mh_T1_city10000_04.txt"); //Type #1 only // Experiment experiment("../data/mh_T1_city10000_04.txt"); //Type #1 only
// Experiment experiment("../data/mh_T3b_city10000_10.txt"); //Type #3 only // Experiment experiment("../data/mh_T3b_city10000_10.txt"); //Type #3 only
// Experiment experiment("../data/mh_T1_T3_city10000_04.txt"); //Type #1 + Type #3 // Experiment experiment("../data/mh_T1_T3_city10000_04.txt"); //Type #1 +
// Type #3
// Run the experiment // Run the experiment
experiment.run(kMaxLoopCount); experiment.run(kMaxLoopCount);