diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp index 020c94fdb0..446228fcc6 100644 --- a/gtsam/geometry/EssentialMatrix.cpp +++ b/gtsam/geometry/EssentialMatrix.cpp @@ -103,14 +103,57 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb, /* ************************************************************************* */ double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, // OptionalJacobian<1, 5> H) const { + + // compute the epipolar lines + Point3 lB = E_ * vB; + Point3 lA = E_.transpose() * vA; + + + // compute the algebraic error as a simple dot product. + double algebraic_error = dot(vA, lB); + + // compute the line-norms for the two lines + Matrix23 I; + I.setIdentity(); + Matrix21 nA = I * lA; + Matrix21 nB = I * lB; + double nA_sq_norm = nA.squaredNorm(); + double nB_sq_norm = nB.squaredNorm(); + + // compute the normalizing denominator and finally the sampson error + double denominator = sqrt(nA_sq_norm + nB_sq_norm); + double sampson_error = algebraic_error / denominator; + if (H) { // See math.lyx - Matrix13 HR = vA.transpose() * E_ * skewSymmetric(-vB); - Matrix12 HD = vA.transpose() * skewSymmetric(-rotation().matrix() * vB) + // computing the derivatives of the numerator w.r.t. E + Matrix13 numerator_H_R = vA.transpose() * E_ * skewSymmetric(-vB); + Matrix12 numerator_H_D = vA.transpose() * skewSymmetric(-rotation().matrix() * vB) * direction().basis(); - *H << HR, HD; + + + // computing the derivatives of the denominator w.r.t. E + Matrix12 denominator_H_nA = nA.transpose() / denominator; + Matrix12 denominator_H_nB = nB.transpose() / denominator; + Matrix13 denominator_H_lA = denominator_H_nA * I; + Matrix13 denominator_H_lB = denominator_H_nB * I; + Matrix33 lB_H_R = E_ * skewSymmetric(-vB); + Matrix32 lB_H_D = skewSymmetric(-rotation().matrix() * vB) * direction().basis(); + Matrix33 lA_H_R = skewSymmetric(E_.matrix().transpose() * vA) * + rotation().matrix().transpose(); + Matrix32 lA_H_D = rotation().inverse().matrix() * skewSymmetric(vA) * direction().basis(); + + Matrix13 denominator_H_R = denominator_H_lA * lA_H_R + denominator_H_lB * lB_H_R; + Matrix12 denominator_H_D = denominator_H_lA * lA_H_D + denominator_H_lB * lB_H_D; + + Matrix15 denominator_H; + denominator_H << denominator_H_R, denominator_H_D; + Matrix15 numerator_H; + numerator_H << numerator_H_R, numerator_H_D; + + *H = numerator_H / denominator - algebraic_error * denominator_H / (denominator * denominator); } - return dot(vA, E_ * vB); + return sampson_error; } /* ************************************************************************* */ diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 909576aa01..4f698047c8 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -159,7 +159,7 @@ class EssentialMatrix { return E.rotate(cRb); } - /// epipolar error, algebraic + /// epipolar error, sampson GTSAM_EXPORT double error(const Vector3& vA, const Vector3& vB, OptionalJacobian<1, 5> H = boost::none) const; diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index 86a498cdc7..71ea44bd10 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -241,6 +241,62 @@ TEST (EssentialMatrix, epipoles) { EXPECT(assert_equal(e2, E.epipole_b())); } +//************************************************************************* +TEST (EssentialMatrix, errorValue) { + // Use two points to get error + Point3 a(1, -2, 1); + Point3 b(3, 1, 1); + + // compute the expected error + // E = [0, 0, 0; 0, 0, -1; 1, 0, 0] + // line for b = [0, -1, 3] + // line for a = [1, 0, 2] + // algebraic error = 5 + // norm of line for b = 1 + // norm of line for a = 1 + // sampson error = 5 / sqrt(1^2 + 1^2) + double expected = 3.535533906; + + // check the error + double actual = trueE.error(a, b); + EXPECT(assert_equal(expected, actual, 1e-6)); +} + +//************************************************************************* +double error_(const Rot3& R, const Unit3& t){ + // Use two points to get error + Point3 a(1, -2, 1); + Point3 b(3, 1, 1); + + EssentialMatrix E = EssentialMatrix::FromRotationAndDirection(R, t); + return E.error(a, b); +} +TEST (EssentialMatrix, errorJacobians) { + // Use two points to get error + Point3 a(1, -2, 1); + Point3 b(3, 1, 1); + + Rot3 c1Rc2 = Rot3::Ypr(0.1, -0.2, 0.3); + Point3 c1Tc2(0.4, 0.5, 0.6); + EssentialMatrix E(c1Rc2, Unit3(c1Tc2)); + + // Use numerical derivatives to calculate the expected Jacobian + Matrix13 HRexpected; + Matrix12 HDexpected; + HRexpected = numericalDerivative21( + error_, E.rotation(), E.direction(), 1e-8); + HDexpected = numericalDerivative22( + error_, E.rotation(), E.direction(), 1e-8); + Matrix15 HEexpected; + HEexpected << HRexpected, HDexpected; + + Matrix15 HEactual; + E.error(a, b, HEactual); + + // Verify the Jacobian is correct + EXPECT(assert_equal(HEexpected, HEactual, 1e-8)); +} + /* ************************************************************************* */ int main() { TestResult tr;