From 65211f8b0a88a8b3545fb90f4d8aa52c24e831cd Mon Sep 17 00:00:00 2001
From: Ayush Baid <ayushrakeshbaid@gmail.com>
Date: Wed, 9 Jun 2021 16:18:14 -0700
Subject: [PATCH] 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 cfe76ff044..586fcfbb78 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 0f8403bc82..5293f1c7fa 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 f079e15c55..999bf34b9b 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 36029932da..d69eb6d2dc 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