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

better naming

release/4.3a0
Varun Agrawal 2025-02-06 16:58:29 -05:00
parent 2fefb69d31
commit d511779f40
1 changed files with 12 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
examples/ISAM2_City10000.cpp
View File

@ -58,8 +58,8 @@ class Experiment {
parameters.relinearizeThreshold = 0.01; parameters.relinearizeThreshold = 0.01;
parameters.relinearizeSkip = 1; parameters.relinearizeSkip = 1;
ISAM2 isam2(parameters); ISAM2 isam2_(parameters);
NonlinearFactorGraph graph; NonlinearFactorGraph graph_;
Values initial_; Values initial_;
Values results; Values results;
@ -77,14 +77,14 @@ class Experiment {
// Set up initial prior // Set up initial prior
Pose2 priorPose(0, 0, 0); Pose2 priorPose(0, 0, 0);
initial_.insert(X(0), priorPose); initial_.insert(X(0), priorPose);
graph.addPrior<Pose2>(X(0), priorPose, kPriorNoiseModel); graph_.addPrior<Pose2>(X(0), priorPose, kPriorNoiseModel);
poseCount++; poseCount++;
// Initial update // Initial update
isam2.update(graph, initial_); isam2_.update(graph_, initial_);
graph.resize(0); graph_.resize(0);
initial_.clear(); initial_.clear();
results = isam2.calculateBestEstimate(); results = isam2_.calculateBestEstimate();
// Start main loop // Start main loop
size_t keyS = 0; size_t keyS = 0;
@ -110,27 +110,27 @@ class Experiment {
if (keyS == keyT - 1) { // new X(key) if (keyS == keyT - 1) { // new X(key)
initial_.insert(X(keyT), results.at<Pose2>(X(keyS)) * odomPose); initial_.insert(X(keyT), results.at<Pose2>(X(keyS)) * odomPose);
graph.add( graph_.add(
BetweenFactor<Pose2>(X(keyS), X(keyT), odomPose, kPoseNoiseModel)); BetweenFactor<Pose2>(X(keyS), X(keyT), odomPose, kPoseNoiseModel));
poseCount++; poseCount++;
} else { // loop } else { // loop
int id = index % numMeasurements; int id = index % numMeasurements;
if (isWithAmbiguity && id % 2 == 0) { if (isWithAmbiguity && id % 2 == 0) {
graph.add(BetweenFactor<Pose2>(X(keyS), X(keyT), odomPose, graph_.add(BetweenFactor<Pose2>(X(keyS), X(keyT), odomPose,
kPoseNoiseModel)); kPoseNoiseModel));
} else { } else {
graph.add(BetweenFactor<Pose2>( graph_.add(BetweenFactor<Pose2>(
X(keyS), X(keyT), odomPose, X(keyS), X(keyT), odomPose,
noiseModel::Diagonal::Sigmas(Vector3::Ones() * 10.0))); noiseModel::Diagonal::Sigmas(Vector3::Ones() * 10.0)));
} }
index++; index++;
} }
isam2.update(graph, initial_); isam2_.update(graph_, initial_);
graph.resize(0); graph_.resize(0);
initial_.clear(); initial_.clear();
results = isam2.calculateBestEstimate(); results = isam2_.calculateBestEstimate();
// Print loop index and time taken in processor clock ticks // Print loop index and time taken in processor clock ticks
if (index % 50 == 0 && keyS != keyT - 1) { if (index % 50 == 0 && keyS != keyT - 1) {