//*************************************************************************
TEST (EssentialMatrixFactor3, extraTest) {
// The "true E" in the body frame is
EssentialMatrix bodyE = cRb.inverse() * trueE;
for (size_t i = 0; i < 5; i++) {
EssentialMatrixFactor3 factor(100, i, pA(i), pB(i), cRb, model2, K);
// Check evaluation
Point3 P1 = data.tracks[i].p;
const Point2 pi = camera2.project(P1);
Point2 reprojectionError(pi - pB(i));
Vector expected = reprojectionError.vector();
Matrix Hactual1, Hactual2;
double d(baseline / P1.z());
Vector actual = factor.evaluateError(bodyE, d, Hactual1, Hactual2);
EXPECT(assert_equal(expected, actual, 1e-7));
// Use numerical derivatives to calculate the expected Jacobian
Matrix Hexpected1, Hexpected2;
boost::function<Vector(const EssentialMatrix&, double)> f = boost::bind(
&EssentialMatrixFactor3::evaluateError, &factor, _1, _2, boost::none,
boost::none);
Hexpected1 = numericalDerivative21<Vector2, EssentialMatrix, double>(f, bodyE, d);
Hexpected2 = numericalDerivative22<Vector2, EssentialMatrix, double>(f, bodyE, d);
// Verify the Jacobian is correct
EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6));
EXPECT(assert_equal(Hexpected2, Hactual2, 1e-8));
}
}
