diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 89ed8f0ae0..d4f7b23659 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -14,6 +14,7 @@ #include #include #include +#include #include #include @@ -359,26 +360,13 @@ TEST(EssentialMatrixFactor4, factor) { // Check evaluation Vector1 expected; expected << 0; - Matrix HEactual; - Matrix HKactual; - Vector actual = factor.evaluateError(trueE, trueK, HEactual, HKactual); + Vector actual = factor.evaluateError(trueE, trueK); EXPECT(assert_equal(expected, actual, 1e-7)); - // Use numerical derivatives to calculate the expected Jacobian - Matrix HEexpected; - Matrix HKexpected; - typedef Eigen::Matrix Vector1; - boost::function f = - boost::bind(&EssentialMatrixFactor4::evaluateError, factor, _1, - _2, boost::none, boost::none); - HEexpected = numericalDerivative21( - f, trueE, trueK); - HKexpected = numericalDerivative22( - f, trueE, trueK); - - // Verify the Jacobian is correct - EXPECT(assert_equal(HEexpected, HEactual, 1e-8)); - EXPECT(assert_equal(HKexpected, HKactual, 1e-8)); + Values truth; + truth.insert(keyE, trueE); + truth.insert(keyK, trueK); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, truth, 1e-6, 1e-7); } }