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

convert graph and isam2 from pointer to object

release/4.3a0
Varun Agrawal 2025-02-06 16:35:56 -05:00
parent 98904e08bf
commit 67ffd1e1ae
1 changed files with 6 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
examples/ISAM2_City10000.cpp
View File

@ -109,26 +109,26 @@ 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)) * odom_pose); initial_.insert(X(keyT), results.at<Pose2>(X(keyS)) * odom_pose);
graph->add( graph.add(
BetweenFactor<Pose2>(X(keyS), X(keyT), odom_pose, kPoseNoiseModel)); BetweenFactor<Pose2>(X(keyS), X(keyT), odom_pose, kPoseNoiseModel));
pose_count++; pose_count++;
} else { // loop } else { // loop
int id = index % num_measurements; int id = index % num_measurements;
if (is_with_ambiguity && id % 2 == 0) { if (is_with_ambiguity && id % 2 == 0) {
graph->add(BetweenFactor<Pose2>(X(keyS), X(keyT), odom_pose, graph.add(BetweenFactor<Pose2>(X(keyS), X(keyT), odom_pose,
kPoseNoiseModel)); kPoseNoiseModel));
} else { } else {
graph->add(BetweenFactor<Pose2>( graph.add(BetweenFactor<Pose2>(
X(keyS), X(keyT), odom_pose, X(keyS), X(keyT), odom_pose,
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) {