From 2ecad47b9ef092379926f9784257c55dd245c24b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 8 Jul 2021 19:41:01 -0400 Subject: [PATCH 1/3] Added lots of tests for BetweenFactor --- gtsam/slam/tests/testBetweenFactor.cpp | 59 ++++++++++++++++++++++---- 1 file changed, 51 insertions(+), 8 deletions(-) diff --git a/gtsam/slam/tests/testBetweenFactor.cpp b/gtsam/slam/tests/testBetweenFactor.cpp index e99dba3bc0..31842bf800 100644 --- a/gtsam/slam/tests/testBetweenFactor.cpp +++ b/gtsam/slam/tests/testBetweenFactor.cpp @@ -1,18 +1,19 @@ /** - * @file testBetweenFactor.cpp + * @file testBetweenFactor.cpp * @brief - * @author Duy-Nguyen Ta - * @date Aug 2, 2013 + * @author Duy-Nguyen Ta, Varun Agrawal + * @date Aug 2, 2013 */ +#include +#include #include +#include #include #include +#include #include -#include -#include - using namespace boost::placeholders; using namespace gtsam; using namespace gtsam::symbol_shorthand; @@ -48,7 +49,6 @@ TEST(BetweenFactor, Rot3) { } /* ************************************************************************* */ -/* // Constructor scalar TEST(BetweenFactor, ConstructorScalar) { SharedNoiseModel model; @@ -56,6 +56,7 @@ TEST(BetweenFactor, ConstructorScalar) { BetweenFactor factor(1, 2, measured_value, model); } +/* ************************************************************************* */ // Constructor vector3 TEST(BetweenFactor, ConstructorVector3) { SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0); @@ -63,13 +64,55 @@ TEST(BetweenFactor, ConstructorVector3) { BetweenFactor factor(1, 2, measured_value, model); } +/* ************************************************************************* */ // Constructor dynamic sized vector TEST(BetweenFactor, ConstructorDynamicSizeVector) { SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 1.0); Vector measured_value(5); measured_value << 1, 2, 3, 4, 5; BetweenFactor factor(1, 2, measured_value, model); } -*/ + +/* ************************************************************************* */ +TEST(BetweenFactor, Point3Jacobians) { + SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0); + Point3 measured_value(1, 2, 3); + BetweenFactor factor(1, 2, measured_value, model); + + Values values; + values.insert(1, Point3(0, 0, 0)); + values.insert(2, Point3(1, 2, 3)); + Vector3 error = factor.evaluateError(Point3(0, 0, 0), Point3(1, 2, 3)); + EXPECT(assert_equal(Vector3::Zero(), error, 1e-9)); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +} + +/* ************************************************************************* */ +TEST(BetweenFactor, Rot3Jacobians) { + SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0); + Rot3 measured_value = Rot3::Ry(M_PI_2); + BetweenFactor factor(1, 2, measured_value, model); + + Values values; + values.insert(1, Rot3()); + values.insert(2, Rot3::Ry(M_PI_2)); + Vector3 error = factor.evaluateError(Rot3(), Rot3::Ry(M_PI_2)); + EXPECT(assert_equal(Vector3::Zero(), error, 1e-9)); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +} + +/* ************************************************************************* */ +TEST(BetweenFactor, Pose3Jacobians) { + SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 1.0); + Pose3 measured_value(Rot3(), Point3(1, 2, 3)); + BetweenFactor factor(1, 2, measured_value, model); + + Values values; + values.insert(1, Pose3()); + values.insert(2, Pose3(Rot3(), Point3(1, 2, 3))); + Vector3 error = factor.evaluateError(Pose3(), Pose3(Rot3(), Point3(1, 2, 3))); + EXPECT(assert_equal(Vector3::Zero(), error, 1e-9)); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +} /* ************************************************************************* */ int main() { From 8b9e60156c5c895430624dfbdb27ae9fb1c5b472 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 9 Jul 2021 14:06:59 -0400 Subject: [PATCH 2/3] cleaner variables --- gtsam/slam/tests/testBetweenFactor.cpp | 31 +++++++++++++------------- 1 file changed, 16 insertions(+), 15 deletions(-) diff --git a/gtsam/slam/tests/testBetweenFactor.cpp b/gtsam/slam/tests/testBetweenFactor.cpp index 31842bf800..206d2b590e 100644 --- a/gtsam/slam/tests/testBetweenFactor.cpp +++ b/gtsam/slam/tests/testBetweenFactor.cpp @@ -52,31 +52,31 @@ TEST(BetweenFactor, Rot3) { // Constructor scalar TEST(BetweenFactor, ConstructorScalar) { SharedNoiseModel model; - double measured_value = 0.0; - BetweenFactor factor(1, 2, measured_value, model); + double measured = 0.0; + BetweenFactor factor(1, 2, measured, model); } /* ************************************************************************* */ // Constructor vector3 TEST(BetweenFactor, ConstructorVector3) { SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0); - Vector3 measured_value(1, 2, 3); - BetweenFactor factor(1, 2, measured_value, model); + Vector3 measured(1, 2, 3); + BetweenFactor factor(1, 2, measured, model); } /* ************************************************************************* */ // Constructor dynamic sized vector TEST(BetweenFactor, ConstructorDynamicSizeVector) { SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 1.0); - Vector measured_value(5); measured_value << 1, 2, 3, 4, 5; - BetweenFactor factor(1, 2, measured_value, model); + Vector measured(5); measured << 1, 2, 3, 4, 5; + BetweenFactor factor(1, 2, measured, model); } /* ************************************************************************* */ TEST(BetweenFactor, Point3Jacobians) { SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0); - Point3 measured_value(1, 2, 3); - BetweenFactor factor(1, 2, measured_value, model); + Point3 measured(1, 2, 3); + BetweenFactor factor(1, 2, measured, model); Values values; values.insert(1, Point3(0, 0, 0)); @@ -89,8 +89,8 @@ TEST(BetweenFactor, Point3Jacobians) { /* ************************************************************************* */ TEST(BetweenFactor, Rot3Jacobians) { SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0); - Rot3 measured_value = Rot3::Ry(M_PI_2); - BetweenFactor factor(1, 2, measured_value, model); + Rot3 measured = Rot3::Ry(M_PI_2); + BetweenFactor factor(1, 2, measured, model); Values values; values.insert(1, Rot3()); @@ -103,13 +103,14 @@ TEST(BetweenFactor, Rot3Jacobians) { /* ************************************************************************* */ TEST(BetweenFactor, Pose3Jacobians) { SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 1.0); - Pose3 measured_value(Rot3(), Point3(1, 2, 3)); - BetweenFactor factor(1, 2, measured_value, model); + Pose3 measured(Rot3(), Point3(1, 2, 3)); + BetweenFactor factor(1, 2, measured, model); + Pose3 pose1, pose2(Rot3(), Point3(1, 2, 3)); Values values; - values.insert(1, Pose3()); - values.insert(2, Pose3(Rot3(), Point3(1, 2, 3))); - Vector3 error = factor.evaluateError(Pose3(), Pose3(Rot3(), Point3(1, 2, 3))); + values.insert(1, pose1); + values.insert(2, pose2); + Vector3 error = factor.evaluateError(pose1, pose2); EXPECT(assert_equal(Vector3::Zero(), error, 1e-9)); EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); } From 2e4016932491bc7888cd9108e7667f7faf00e819 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 9 Jul 2021 14:07:19 -0400 Subject: [PATCH 3/3] fix dimension for Pose3 test --- gtsam/slam/tests/testBetweenFactor.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/slam/tests/testBetweenFactor.cpp b/gtsam/slam/tests/testBetweenFactor.cpp index 206d2b590e..a1f8774e5a 100644 --- a/gtsam/slam/tests/testBetweenFactor.cpp +++ b/gtsam/slam/tests/testBetweenFactor.cpp @@ -110,8 +110,8 @@ TEST(BetweenFactor, Pose3Jacobians) { Values values; values.insert(1, pose1); values.insert(2, pose2); - Vector3 error = factor.evaluateError(pose1, pose2); - EXPECT(assert_equal(Vector3::Zero(), error, 1e-9)); + Vector6 error = factor.evaluateError(pose1, pose2); + EXPECT(assert_equal(Vector6::Zero(), error, 1e-9)); EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); }