diff --git a/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp b/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp index 55be97d9d..8a3a2a63b 100644 --- a/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp +++ b/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp @@ -11,6 +11,7 @@ #include #include +#include #include #include #include @@ -19,8 +20,9 @@ using namespace std; using namespace gtsam; +namespace NM = gtsam::noiseModel; -// Pose representation is [ Rx Ry Rz Tx Ty Tz ]. +// Pose3 tangent representation is [ Rx Ry Rz Tx Ty Tz ]. static const int kIndexRx = 0; static const int kIndexRy = 1; static const int kIndexRz = 2; @@ -29,30 +31,152 @@ static const int kIndexTy = 4; static const int kIndexTz = 5; -typedef PartialPriorFactor TestPartialPriorFactor; +typedef PartialPriorFactor TestPartialPriorFactor2; +typedef PartialPriorFactor TestPartialPriorFactor3; +typedef std::vector Indices; /// traits namespace gtsam { template<> -struct traits : public Testable { -}; +struct traits : public Testable {}; + +template<> +struct traits : public Testable {}; } /* ************************************************************************* */ -TEST(PartialPriorFactor, JacobianAtIdentity) -{ +TEST(PartialPriorFactor, Constructors2) { + Key poseKey(1); + Pose2 measurement(-13.1, 3.14, -0.73); + + // Prior on x component of translation. + TestPartialPriorFactor2 factor1(poseKey, 0, measurement.x(), NM::Isotropic::Sigma(1, 0.25)); + CHECK(assert_equal(1, factor1.prior().rows())); + CHECK(assert_equal(measurement.x(), factor1.prior()(0))); + CHECK(assert_container_equality({ 0 }, factor1.indices())); + + // Prior on full translation vector. + const Indices t_indices = { 0, 1 }; + TestPartialPriorFactor2 factor2(poseKey, t_indices, measurement.translation(), NM::Isotropic::Sigma(2, 0.25)); + CHECK(assert_equal(2, factor2.prior().rows())); + CHECK(assert_equal(measurement.translation(), factor2.prior())); + CHECK(assert_container_equality(t_indices, factor2.indices())); + + // Prior on theta. + TestPartialPriorFactor2 factor3(poseKey, 2, measurement.theta(), NM::Isotropic::Sigma(1, 0.1)); + CHECK(assert_equal(1, factor3.prior().rows())); + CHECK(assert_equal(measurement.theta(), factor3.prior()(0))); + CHECK(assert_container_equality({ 2 }, factor3.indices())); +} + +/* ************************************************************************* */ +TEST(PartialPriorFactor, JacobianPartialTranslation2) { + Key poseKey(1); + Pose2 measurement(-13.1, 3.14, -0.73); + + // Prior on x component of translation. + TestPartialPriorFactor2 factor(poseKey, 0, measurement.x(), NM::Isotropic::Sigma(1, 0.25)); + + Pose2 pose = measurement; // Zero-error linearization point. + + // Calculate numerical derivatives. + Matrix expectedH1 = numericalDerivative11( + boost::bind(&TestPartialPriorFactor2::evaluateError, &factor, _1, boost::none), pose); + + // Use the factor to calculate the derivative. + Matrix actualH1; + factor.evaluateError(pose, actualH1); + + // Verify we get the expected error. + CHECK(assert_equal(expectedH1, actualH1, 1e-5)); +} + +/* ************************************************************************* */ +TEST(PartialPriorFactor, JacobianFullTranslation2) { + Key poseKey(1); + Pose2 measurement(-6.0, 3.5, 0.123); + + // Prior on x component of translation. + TestPartialPriorFactor2 factor(poseKey, { 0, 1 }, measurement.translation(), NM::Isotropic::Sigma(2, 0.25)); + + Pose2 pose = measurement; // Zero-error linearization point. + + // Calculate numerical derivatives. + Matrix expectedH1 = numericalDerivative11( + boost::bind(&TestPartialPriorFactor2::evaluateError, &factor, _1, boost::none), pose); + + // Use the factor to calculate the derivative. + Matrix actualH1; + factor.evaluateError(pose, actualH1); + + // Verify we get the expected error. + CHECK(assert_equal(expectedH1, actualH1, 1e-5)); +} + +/* ************************************************************************* */ +TEST(PartialPriorFactor, JacobianTheta) { + Key poseKey(1); + Pose2 measurement(-1.0, 0.4, -2.5); + + // Prior on x component of translation. + TestPartialPriorFactor2 factor(poseKey, 2, measurement.theta(), NM::Isotropic::Sigma(1, 0.25)); + + Pose2 pose = measurement; // Zero-error linearization point. + + // Calculate numerical derivatives. + Matrix expectedH1 = numericalDerivative11( + boost::bind(&TestPartialPriorFactor2::evaluateError, &factor, _1, boost::none), pose); + + // Use the factor to calculate the derivative. + Matrix actualH1; + factor.evaluateError(pose, actualH1); + + // Verify we get the expected error. + CHECK(assert_equal(expectedH1, actualH1, 1e-5)); +} + +/* ************************************************************************* */ +TEST(PartialPriorFactor, Constructors3) { + Key poseKey(1); + Pose3 measurement(Rot3::RzRyRx(-0.17, 0.567, M_PI), Point3(10.0, -2.3, 3.14)); + + // Single component of translation. + TestPartialPriorFactor3 factor1(poseKey, kIndexTy, measurement.y(), + NM::Isotropic::Sigma(1, 0.25)); + CHECK(assert_equal(1, factor1.prior().rows())); + CHECK(assert_equal(measurement.y(), factor1.prior()(0))); + CHECK(assert_container_equality({ kIndexTy }, factor1.indices())); + + // Full translation vector. + const Indices t_indices = { kIndexTx, kIndexTy, kIndexTz }; + TestPartialPriorFactor3 factor2(poseKey, t_indices, measurement.translation(), + NM::Isotropic::Sigma(3, 0.25)); + CHECK(assert_equal(3, factor2.prior().rows())); + CHECK(assert_equal(measurement.translation(), factor2.prior())); + CHECK(assert_container_equality(t_indices, factor2.indices())); + + // Full tangent vector of rotation. + const Indices r_indices = { kIndexRx, kIndexRy, kIndexRz }; + TestPartialPriorFactor3 factor3(poseKey, r_indices, Rot3::Logmap(measurement.rotation()), + NM::Isotropic::Sigma(3, 0.1)); + CHECK(assert_equal(3, factor3.prior().rows())); + CHECK(assert_equal(Rot3::Logmap(measurement.rotation()), factor3.prior())); + CHECK(assert_container_equality(r_indices, factor3.indices())); +} + +/* ************************************************************************* */ +TEST(PartialPriorFactor, JacobianAtIdentity3) { Key poseKey(1); Pose3 measurement = Pose3::identity(); - SharedNoiseModel model = noiseModel::Isotropic::Sigma(1, 0.25); + SharedNoiseModel model = NM::Isotropic::Sigma(1, 0.25); - TestPartialPriorFactor factor(poseKey, kIndexTy, measurement.translation().y(), model); + TestPartialPriorFactor3 factor(poseKey, kIndexTy, measurement.translation().y(), model); - // Create a linearization point at the zero-error point - Pose3 pose = Pose3::identity(); + Pose3 pose = measurement; // Zero-error linearization point. // Calculate numerical derivatives. Matrix expectedH1 = numericalDerivative11( - boost::bind(&TestPartialPriorFactor::evaluateError, &factor, _1, boost::none), pose); + boost::bind(&TestPartialPriorFactor3::evaluateError, &factor, _1, boost::none), pose); // Use the factor to calculate the derivative. Matrix actualH1; @@ -63,19 +187,18 @@ TEST(PartialPriorFactor, JacobianAtIdentity) } /* ************************************************************************* */ -TEST(PartialPriorFactor, JacobianPartialTranslation) { +TEST(PartialPriorFactor, JacobianPartialTranslation3) { Key poseKey(1); Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0)); - SharedNoiseModel model = noiseModel::Isotropic::Sigma(1, 0.25); + SharedNoiseModel model = NM::Isotropic::Sigma(1, 0.25); - TestPartialPriorFactor factor(poseKey, kIndexTy, measurement.translation().y(), model); + TestPartialPriorFactor3 factor(poseKey, kIndexTy, measurement.translation().y(), model); - // Create a linearization point at the zero-error point - Pose3 pose(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0)); + Pose3 pose = measurement; // Zero-error linearization point. // Calculate numerical derivatives. Matrix expectedH1 = numericalDerivative11( - boost::bind(&TestPartialPriorFactor::evaluateError, &factor, _1, boost::none), pose); + boost::bind(&TestPartialPriorFactor3::evaluateError, &factor, _1, boost::none), pose); // Use the factor to calculate the derivative. Matrix actualH1; @@ -86,20 +209,20 @@ TEST(PartialPriorFactor, JacobianPartialTranslation) { } /* ************************************************************************* */ -TEST(PartialPriorFactor, JacobianFullTranslation) { +TEST(PartialPriorFactor, JacobianFullTranslation3) { Key poseKey(1); Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0)); - SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25); + SharedNoiseModel model = NM::Isotropic::Sigma(3, 0.25); std::vector translationIndices = { kIndexTx, kIndexTy, kIndexTz }; - TestPartialPriorFactor factor(poseKey, translationIndices, measurement.translation(), model); + TestPartialPriorFactor3 factor(poseKey, translationIndices, measurement.translation(), model); // Create a linearization point at the zero-error point - Pose3 pose(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0)); + Pose3 pose = measurement; // Zero-error linearization point. // Calculate numerical derivatives. Matrix expectedH1 = numericalDerivative11( - boost::bind(&TestPartialPriorFactor::evaluateError, &factor, _1, boost::none), pose); + boost::bind(&TestPartialPriorFactor3::evaluateError, &factor, _1, boost::none), pose); // Use the factor to calculate the derivative. Matrix actualH1; @@ -110,20 +233,43 @@ TEST(PartialPriorFactor, JacobianFullTranslation) { } /* ************************************************************************* */ -TEST(PartialPriorFactor, JacobianFullRotation) { +TEST(PartialPriorFactor, JacobianTxTz3) { Key poseKey(1); - Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0)); - SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25); + Pose3 measurement(Rot3::RzRyRx(-0.17, 0.567, M_PI), Point3(10.0, -2.3, 3.14)); + SharedNoiseModel model = NM::Isotropic::Sigma(2, 0.25); - std::vector rotationIndices = { kIndexRx, kIndexRy, kIndexRz }; - TestPartialPriorFactor factor(poseKey, rotationIndices, Rot3::Logmap(measurement.rotation()), model); + std::vector translationIndices = { kIndexTx, kIndexTz }; + TestPartialPriorFactor3 factor(poseKey, translationIndices, + (Vector(2) << measurement.x(), measurement.z()).finished(), model); - // Create a linearization point at the zero-error point - Pose3 pose(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0)); + Pose3 pose = measurement; // Zero-error linearization point. // Calculate numerical derivatives. Matrix expectedH1 = numericalDerivative11( - boost::bind(&TestPartialPriorFactor::evaluateError, &factor, _1, boost::none), pose); + boost::bind(&TestPartialPriorFactor3::evaluateError, &factor, _1, boost::none), pose); + + // Use the factor to calculate the derivative. + Matrix actualH1; + factor.evaluateError(pose, actualH1); + + // Verify we get the expected error. + CHECK(assert_equal(expectedH1, actualH1, 1e-5)); +} + +/* ************************************************************************* */ +TEST(PartialPriorFactor, JacobianFullRotation3) { + Key poseKey(1); + Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0)); + SharedNoiseModel model = NM::Isotropic::Sigma(3, 0.25); + + std::vector rotationIndices = { kIndexRx, kIndexRy, kIndexRz }; + TestPartialPriorFactor3 factor(poseKey, rotationIndices, Rot3::Logmap(measurement.rotation()), model); + + Pose3 pose = measurement; // Zero-error linearization point. + + // Calculate numerical derivatives. + Matrix expectedH1 = numericalDerivative11( + boost::bind(&TestPartialPriorFactor3::evaluateError, &factor, _1, boost::none), pose); // Use the factor to calculate the derivative. Matrix actualH1;