diff --git a/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp b/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp index 6cc70ef46..8a77bc5b9 100644 --- a/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp +++ b/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp @@ -38,6 +38,32 @@ struct traits : public Testable }; } + +/* ************************************************************************* */ +TEST(PartialPriorFactor, JacobianAtIdentity) +{ + Key poseKey(1); + Pose3 measurement = Pose3::identity(); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(1, 0.25); + + TestPartialPriorFactor factor(poseKey, kIndexTy, measurement.translation().x(), model); + + // Create a linearization point at the zero-error point + Pose3 pose = Pose3::identity(); + + // Calculate numerical derivatives. + Matrix expectedH1 = numericalDerivative11( + boost::bind(&TestPartialPriorFactor::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, JacobianPartialTranslation) { Key poseKey(1);