From e9738c70a62b7a5503eeff7ec1d06ee9674b2519 Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Wed, 9 Jun 2021 15:22:08 -0700 Subject: [PATCH 1/2] 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 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)); } /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 977528b53..36029932d 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_); From 65211f8b0a88a8b3545fb90f4d8aa52c24e831cd Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Wed, 9 Jun 2021 16:18:14 -0700 Subject: [PATCH 2/2] moving to squared sampson error --- gtsam/geometry/EssentialMatrix.cpp | 12 +++++++----- gtsam/geometry/EssentialMatrix.h | 2 +- gtsam/geometry/tests/testEssentialMatrix.cpp | 4 ++-- gtsam/slam/tests/testEssentialMatrixFactor.cpp | 8 ++++---- 4 files changed, 14 insertions(+), 12 deletions(-) diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp index cfe76ff04..586fcfbb7 100644 --- a/gtsam/geometry/EssentialMatrix.cpp +++ b/gtsam/geometry/EssentialMatrix.cpp @@ -154,25 +154,27 @@ double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, Matrix15 numerator_H; numerator_H << numerator_H_R, numerator_H_D; - *HE = numerator_H / denominator - - algebraic_error * denominator_H / (denominator * denominator); + *HE = 2 * sampson_error * (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); + *HvA = 2 * sampson_error * (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); + *HvB = 2 * sampson_error * (numerator_H_vB / denominator - + algebraic_error * denominator_H_vB / (denominator * denominator)); } - return sampson_error; + return sampson_error * sampson_error; } /* ************************************************************************* */ diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 0f8403bc8..5293f1c7f 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -159,7 +159,7 @@ class EssentialMatrix { return E.rotate(cRb); } - /// epipolar error, sampson + /// epipolar error, sampson squared GTSAM_EXPORT double error(const Vector3& vA, const Vector3& vB, OptionalJacobian<1, 5> HE = boost::none, OptionalJacobian<1, 3> HvA = boost::none, diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index f079e15c5..999bf34b9 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -254,8 +254,8 @@ TEST(EssentialMatrix, errorValue) { // 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; + // sampson error = 5^2 / 1^2 + 1^2 + double expected = 12.5; // check the error double actual = trueE.error(a, b); diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 36029932d..d69eb6d2d 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -24,7 +24,7 @@ using namespace gtsam; // Noise model for first type of factor is evaluating algebraic error noiseModel::Isotropic::shared_ptr model1 = noiseModel::Isotropic::Sigma(1, - 0.01); + 1e-4); // Noise model for second type of factor is evaluating pixel coordinates noiseModel::Unit::shared_ptr model2 = noiseModel::Unit::Create(2); @@ -196,9 +196,9 @@ TEST (EssentialMatrixFactor, minimization) { (Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); initial.insert(1, initialE); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) - EXPECT_DOUBLES_EQUAL(313.85, graph.error(initial), 1e-2); + EXPECT_DOUBLES_EQUAL(59403.51, graph.error(initial), 1e-2); #else - EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); + EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); # TODO: redo this error #endif // Optimize @@ -410,7 +410,7 @@ TEST (EssentialMatrixFactor, extraMinimization) { initial.insert(1, initialE); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) - EXPECT_DOUBLES_EQUAL(313.85, graph.error(initial), 1e-2); + EXPECT_DOUBLES_EQUAL(59403.51, graph.error(initial), 1e-2); #else EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); #endif