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

add ambiguous loop closures to ISAM2 version of City10000

release/4.3a0
Varun Agrawal 2025-01-30 10:58:46 -05:00
parent 3c67e1fc21
commit f808348d52
1 changed files with 11 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
examples/ISAM2_City10000.cpp
View File

@ -150,12 +150,21 @@ int main(int argc, char* argv[]) {
if (key_s == key_t - 1) { // new X(key)
init_values.insert(X(key_t), results.at<Pose2>(X(key_s)) * odom_pose);
graph->add(BetweenFactor<Pose2>(X(key_s), X(key_t), odom_pose,
pose_noise_model));
pose_count++;
} else { // loop
int id = index % num_measurements;
if (is_with_ambiguity && id % 2 == 0) {
graph->add(BetweenFactor<Pose2>(X(key_s), X(key_t), odom_pose,
pose_noise_model));
} else {
graph->add(BetweenFactor<Pose2>(
X(key_s), X(key_t), odom_pose,
noiseModel::Diagonal::Sigmas(Vector3::Ones() * 10.0)));
}
index++;
}
graph->add(
BetweenFactor<Pose2>(X(key_s), X(key_t), odom_pose, pose_noise_model));
isam2->update(*graph, init_values);
graph->resize(0);