From e9738c70a62b7a5503eeff7ec1d06ee9674b2519 Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Wed, 9 Jun 2021 15:22:08 -0700 Subject: [PATCH] adding jacobians on input points --- gtsam/geometry/EssentialMatrix.cpp | 25 +++++++++++++--- gtsam/geometry/EssentialMatrix.h | 4 ++- gtsam/geometry/tests/testEssentialMatrix.cpp | 29 +++++++++++-------- .../slam/tests/testEssentialMatrixFactor.cpp | 12 +++++--- 4 files changed, 49 insertions(+), 21 deletions(-) diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp index 10d3873385..cfe76ff044 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 4f698047c8..0f8403bc82 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 acb817ae73..f079e15c55 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)); } /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 977528b53b..36029932da 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -117,7 +117,8 @@ TEST(EssentialMatrixFactor, ExpressionFactor) { Key key(1); for (size_t i = 0; i < 5; i++) { boost::function)> f = - boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2); + boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2, boost::none, + boost::none); Expression E_(key); // leaf expression Expression expr(f, E_); // unary expression @@ -143,9 +144,12 @@ TEST(EssentialMatrixFactor, ExpressionFactorRotationOnly) { Key key(1); for (size_t i = 0; i < 5; i++) { boost::function)> f = - boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2); - boost::function, - OptionalJacobian<5, 2>)> g; + boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2, boost::none, + boost::none); + boost::function, + OptionalJacobian<5, 2>)> + g; Expression R_(key); Expression d_(trueDirection); Expression E_(&EssentialMatrix::FromRotationAndDirection, R_, d_);