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)
initial_.insert(X(keyT), results.at<Pose2>(X(keyS)) * odom_pose);
graph->add(
graph.add(
BetweenFactor<Pose2>(X(keyS), X(keyT), odom_pose, kPoseNoiseModel));
pose_count++;
} else { // loop
int id = index % num_measurements;
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));
} else {
graph->add(BetweenFactor<Pose2>(
graph.add(BetweenFactor<Pose2>(
X(keyS), X(keyT), odom_pose,
noiseModel::Diagonal::Sigmas(Vector3::Ones() * 10.0)));
}
index++;
}
isam2->update(*graph, initial_);
graph->resize(0);
isam2.update(*graph, initial_);
graph.resize(0);
initial_.clear();
results = isam2->calculateBestEstimate();
results = isam2.calculateBestEstimate();
// Print loop index and time taken in processor clock ticks
if (index % 50 == 0 && keyS != keyT - 1) {