diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp index 10d387338..cfe76ff04 100644 --- a/gtsam/geometry/EssentialMatrix.cpp +++ b/gtsam/geometry/EssentialMatrix.cpp @@ -101,8 +101,10 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb, } /* ************************************************************************* */ -double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, // - OptionalJacobian<1, 5> H) const { +double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, + OptionalJacobian<1, 5> HE, + OptionalJacobian<1, 3> HvA, + OptionalJacobian<1, 3> HvB) const { // compute the epipolar lines Point3 lB = E_ * vB; Point3 lA = E_.transpose() * vA; @@ -122,7 +124,7 @@ double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, // double denominator = sqrt(nA_sq_norm + nB_sq_norm); double sampson_error = algebraic_error / denominator; - if (H) { + if (HE) { // See math.lyx // computing the derivatives of the numerator w.r.t. E Matrix13 numerator_H_R = vA.transpose() * E_ * skewSymmetric(-vB); @@ -152,9 +154,24 @@ double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, // Matrix15 numerator_H; numerator_H << numerator_H_R, numerator_H_D; - *H = numerator_H / denominator - + *HE = numerator_H / denominator - algebraic_error * denominator_H / (denominator * denominator); } + + if (HvA){ + Matrix13 numerator_H_vA = vB.transpose() * matrix().transpose(); + Matrix13 denominator_H_vA = nA.transpose() * I * matrix().transpose() / denominator; + + *HvA = numerator_H_vA / denominator - algebraic_error * denominator_H_vA / (denominator * denominator); + } + + if (HvB){ + Matrix13 numerator_H_vB = vA.transpose() * matrix(); + Matrix13 denominator_H_vB = nB.transpose() * I * matrix() / denominator; + + *HvB = numerator_H_vB / denominator - algebraic_error * denominator_H_vB / (denominator * denominator); + } + return sampson_error; } diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 4f698047c..0f8403bc8 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -161,7 +161,9 @@ class EssentialMatrix { /// epipolar error, sampson GTSAM_EXPORT double error(const Vector3& vA, const Vector3& vB, - OptionalJacobian<1, 5> H = boost::none) const; + OptionalJacobian<1, 5> HE = boost::none, + OptionalJacobian<1, 3> HvA = boost::none, + OptionalJacobian<1, 3> HvB = boost::none) const; /// @} diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index acb817ae7..f079e15c5 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -263,18 +263,14 @@ TEST(EssentialMatrix, errorValue) { } //************************************************************************* -double error_(const Rot3& R, const Unit3& t) { - // Use two points to get error - Point3 a(1, -2, 1); - Point3 b(3, 1, 1); - +double error_(const Rot3& R, const Unit3& t, const Point3& a, const Point3& b) { 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); + Point3 vA(1, -2, 1); + Point3 vB(3, 1, 1); Rot3 c1Rc2 = Rot3::Ypr(0.1, -0.2, 0.3); Point3 c1Tc2(0.4, 0.5, 0.6); @@ -283,18 +279,27 @@ TEST(EssentialMatrix, errorJacobians) { // Use numerical derivatives to calculate the expected Jacobian Matrix13 HRexpected; Matrix12 HDexpected; - HRexpected = numericalDerivative21(error_, E.rotation(), - E.direction()); - HDexpected = numericalDerivative22(error_, E.rotation(), - E.direction()); + Matrix13 HvAexpected; + Matrix13 HvBexpected; + HRexpected = numericalDerivative41(error_, E.rotation(), + E.direction(), vA, vB); + HDexpected = numericalDerivative42(error_, E.rotation(), + E.direction(), vA, vB); + HvAexpected = numericalDerivative43(error_, E.rotation(), + E.direction(), vA, vB); + HvBexpected = numericalDerivative44(error_, E.rotation(), + E.direction(), vA, vB); Matrix15 HEexpected; HEexpected << HRexpected, HDexpected; Matrix15 HEactual; - E.error(a, b, HEactual); + Matrix13 HvAactual, HvBactual; + E.error(vA, vB, HEactual, HvAactual, HvBactual); // Verify the Jacobian is correct EXPECT(assert_equal(HEexpected, HEactual, 1e-5)); + EXPECT(assert_equal(HvAexpected, HvAactual, 1e-5)); + EXPECT(assert_equal(HvBexpected, HvBactual, 1e-5)); } /* ************************************************************************* */