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

kUpdateFrequency for 3x win!

release/4.3a0
Frank Dellaert 2025-01-31 00:14:05 -05:00
parent 1e0ad3b246
commit fce62a61b5
1 changed files with 12 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
examples/Hybrid_City10000.cpp
View File

@ -43,9 +43,11 @@ using symbol_shorthand::L;
using symbol_shorthand::M; using symbol_shorthand::M;
using symbol_shorthand::X; using symbol_shorthand::X;
const size_t kMaxLoopCount = 2000; // Example default value const size_t kMaxLoopCount = 3000; // Example default value
const size_t kUpdateFrequency = 3; // 3000: {1: 62s, 2: 21s, 3: 20s, 4: 31s, 5:39s}
const size_t kMaxNrHypotheses = 10; const size_t kMaxNrHypotheses = 10;
auto kOpenLoopModel = noiseModel::Diagonal::Sigmas(Vector3::Ones() * 10); auto kOpenLoopModel = noiseModel::Diagonal::Sigmas(Vector3::Ones() * 10);
auto kPriorNoiseModel = noiseModel::Diagonal::Sigmas( auto kPriorNoiseModel = noiseModel::Diagonal::Sigmas(
@ -172,6 +174,9 @@ class Experiment {
std::vector<std::pair<size_t, double>> smootherUpdateTimes; std::vector<std::pair<size_t, double>> smootherUpdateTimes;
smootherUpdateTimes.push_back({index, afterUpdate - beforeUpdate}); smootherUpdateTimes.push_back({index, afterUpdate - beforeUpdate});
// Flag to decide whether to run smoother update
size_t numberOfHybridFactors = 0;
// Start main loop // Start main loop
size_t keyS = 0, keyT = 0; size_t keyS = 0, keyT = 0;
clock_t startTime = clock(); clock_t startTime = clock();
@ -192,9 +197,6 @@ class Experiment {
poseArray[i] = Pose2(x, y, rad); poseArray[i] = Pose2(x, y, rad);
} }
// Flag to decide whether to run smoother update
bool doSmootherUpdate = false;
// Take the first one as the initial estimate // Take the first one as the initial estimate
Pose2 odomPose = poseArray[0]; Pose2 odomPose = poseArray[0];
if (keyS == keyT - 1) { if (keyS == keyT - 1) {
@ -206,7 +208,7 @@ class Experiment {
numMeasurements, keyS, keyT, m, poseArray, kPoseNoiseModel); numMeasurements, keyS, keyT, m, poseArray, kPoseNoiseModel);
graph_.push_back(mixtureFactor); graph_.push_back(mixtureFactor);
discreteCount++; discreteCount++;
doSmootherUpdate = true; numberOfHybridFactors += 1;
std::cout << "mixtureFactor: " << keyS << " " << keyT << std::endl; 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,
@ -221,18 +223,20 @@ class Experiment {
// print loop closure event keys: // print loop closure event keys:
std::cout << "Loop closure: " << keyS << " " << keyT << std::endl; std::cout << "Loop closure: " << keyS << " " << keyT << std::endl;
graph_.add(loopFactor); graph_.add(loopFactor);
doSmootherUpdate = true; numberOfHybridFactors += 1;
loopCount++; loopCount++;
} }
if (doSmootherUpdate) { if (numberOfHybridFactors>=kUpdateFrequency) {
// print the keys involved in the smoother update
std::cout << "Smoother update: " << graph_.size() << std::endl;
gttic_(SmootherUpdate); gttic_(SmootherUpdate);
beforeUpdate = clock(); beforeUpdate = clock();
smootherUpdate(smoother_, graph_, initial_, kMaxNrHypotheses, &result_); smootherUpdate(smoother_, graph_, initial_, kMaxNrHypotheses, &result_);
afterUpdate = clock(); afterUpdate = clock();
smootherUpdateTimes.push_back({index, afterUpdate - beforeUpdate}); smootherUpdateTimes.push_back({index, afterUpdate - beforeUpdate});
gttoc_(SmootherUpdate); gttoc_(SmootherUpdate);
doSmootherUpdate = false; numberOfHybridFactors = 0;
} }
// Record timing for odometry edges only // Record timing for odometry edges only