From 43b0225557fc057e5cdca3e1b657512c1387906c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 10 Dec 2020 10:07:49 -0500 Subject: [PATCH 1/2] explicitly initialize Point3 --- gtsam/slam/tests/testInitializePose3.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/gtsam/slam/tests/testInitializePose3.cpp b/gtsam/slam/tests/testInitializePose3.cpp index 01ec9edb1..dfe3d24ae 100644 --- a/gtsam/slam/tests/testInitializePose3.cpp +++ b/gtsam/slam/tests/testInitializePose3.cpp @@ -75,8 +75,14 @@ NonlinearFactorGraph graph2() { g.add(BetweenFactor(x0, x1, pose0.between(pose1), noiseModel::Isotropic::Precision(6, 1.0))); g.add(BetweenFactor(x1, x2, pose1.between(pose2), noiseModel::Isotropic::Precision(6, 1.0))); g.add(BetweenFactor(x2, x3, pose2.between(pose3), noiseModel::Isotropic::Precision(6, 1.0))); - g.add(BetweenFactor(x2, x0, Pose3(Rot3::Ypr(0.1,0,0.1), Point3()), noiseModel::Isotropic::Precision(6, 0.0))); // random pose, but zero information - g.add(BetweenFactor(x0, x3, Pose3(Rot3::Ypr(0.5,-0.2,0.2), Point3(10,20,30)), noiseModel::Isotropic::Precision(6, 0.0))); // random pose, but zero informatoin + // random pose, but zero information + g.add(BetweenFactor(x2, x0, + Pose3(Rot3::Ypr(0.1, 0, 0.1), Point3(0, 0, 0)), + noiseModel::Isotropic::Precision(6, 0.0))); + // random pose, but zero informatoin + g.add(BetweenFactor( + x0, x3, Pose3(Rot3::Ypr(0.5, -0.2, 0.2), Point3(10, 20, 30)), + noiseModel::Isotropic::Precision(6, 0.0))); g.addPrior(x0, pose0, model); return g; } From 592c572f8c757e96aacf685939f42f2e5cdf1a0c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 10 Dec 2020 11:10:36 -0500 Subject: [PATCH 2/2] formatting --- gtsam/slam/tests/testInitializePose3.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/gtsam/slam/tests/testInitializePose3.cpp b/gtsam/slam/tests/testInitializePose3.cpp index dfe3d24ae..995a109fa 100644 --- a/gtsam/slam/tests/testInitializePose3.cpp +++ b/gtsam/slam/tests/testInitializePose3.cpp @@ -76,13 +76,14 @@ NonlinearFactorGraph graph2() { g.add(BetweenFactor(x1, x2, pose1.between(pose2), noiseModel::Isotropic::Precision(6, 1.0))); g.add(BetweenFactor(x2, x3, pose2.between(pose3), noiseModel::Isotropic::Precision(6, 1.0))); // random pose, but zero information - g.add(BetweenFactor(x2, x0, - Pose3(Rot3::Ypr(0.1, 0, 0.1), Point3(0, 0, 0)), - noiseModel::Isotropic::Precision(6, 0.0))); - // random pose, but zero informatoin + auto noise_zero_info = noiseModel::Isotropic::Precision(6, 0.0); + g.add(BetweenFactor( + x2, x0, Pose3(Rot3::Ypr(0.1, 0.0, 0.1), Point3(0.0, 0.0, 0.0)), + noise_zero_info)); + // random pose, but zero information g.add(BetweenFactor( x0, x3, Pose3(Rot3::Ypr(0.5, -0.2, 0.2), Point3(10, 20, 30)), - noiseModel::Isotropic::Precision(6, 0.0))); + noise_zero_info)); g.addPrior(x0, pose0, model); return g; }