Merge branch 'feature/sampson-epipolar-error' into feature/essential-…

…mat-with-approx-k
borglab · Jun 9, 2021 · 2e69d09 · 2e69d09
2 parents 15f8b41 + 65211f8
commit 2e69d09
Show file tree

Hide file tree

Showing 4 changed files with 23 additions and 17 deletions.
diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp
@@ -111,7 +111,7 @@ double EssentialMatrix::error(const Vector3& vA, const Vector3& vB,
 
   // 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();
@@ -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
@@ -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
@@ -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
@@ -25,7 +25,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);
 
@@ -120,7 +120,8 @@ TEST(EssentialMatrixFactor, ExpressionFactor) {
   Key key(1);
   for (size_t i = 0; i < 5; i++) {
     boost::function<double(const EssentialMatrix&, OptionalJacobian<1, 5>)> 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<EssentialMatrix> E_(key); // leaf expression
     Expression<double> expr(f, E_); // unary expression
 
@@ -146,9 +147,12 @@ TEST(EssentialMatrixFactor, ExpressionFactorRotationOnly) {
   Key key(1);
   for (size_t i = 0; i < 5; i++) {
     boost::function<double(const EssentialMatrix&, OptionalJacobian<1, 5>)> f =
-        boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2);
-    boost::function<EssentialMatrix(const Rot3&, const Unit3&, OptionalJacobian<5, 3>,
-                                    OptionalJacobian<5, 2>)> g;
+        boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2, boost::none,
+                    boost::none);
+    boost::function<EssentialMatrix(const Rot3&, const Unit3&,
+                                    OptionalJacobian<5, 3>,
+                                    OptionalJacobian<5, 2>)>
+        g;
     Expression<Rot3> R_(key);
     Expression<Unit3> d_(trueDirection);
     Expression<EssentialMatrix> E_(&EssentialMatrix::FromRotationAndDirection, R_, d_);
@@ -195,9 +199,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(419.07, 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
@@ -519,7 +523,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