removing Sampson error + some tests cleanup

borglab · Jun 14, 2021 · 14f8b8a · 14f8b8a
1 parent f1ea57a
commit 14f8b8a
Show file tree

Hide file tree

Showing 7 changed files with 326 additions and 233 deletions.
diff --git a/examples/CreateSFMExampleData.cpp b/examples/CreateSFMExampleData.cpp
@@ -32,11 +32,7 @@ void createExampleBALFile(const string& filename, const vector<Point3>& P,
 
   // Class that will gather all data
   SfmData data;
-
-  // Create two cameras
-  Rot3 aRb = Rot3::Yaw(M_PI_2);
-  Point3 aTb(0.1, 0, 0);
-  Pose3 identity, aPb(aRb, aTb);
+  // Create two cameras and add them to data
   data.cameras.push_back(SfmCamera(pose1, K));
   data.cameras.push_back(SfmCamera(pose2, K));
 
@@ -75,7 +71,7 @@ void create5PointExample1() {
   Point3(0, 0.5, 0.5), Point3(0, -0.5, 0.5);
 
   // Assumes example is run in ${GTSAM_TOP}/build/examples
-  const string filename = "../../examples/data/5pointExample1.txt";
+  const string filename = "../../examples/Data/5pointExample1.txt";
   createExampleBALFile(filename, P, pose1, pose2);
 }
 
@@ -94,7 +90,7 @@ void create5PointExample2() {
   Point3(0, 50, 50), Point3(0, -50, 50), Point3(-20, 0, 80), Point3(20, -50, 80);
 
   // Assumes example is run in ${GTSAM_TOP}/build/examples
-  const string filename = "../../examples/data/5pointExample2.txt";
+  const string filename = "../../examples/Data/5pointExample2.txt";
   Cal3Bundler K(500, 0, 0);
   createExampleBALFile(filename, P, pose1, pose2,K);
 }
@@ -111,35 +107,34 @@ void create18PointExample1() {
 
   // Create test data, we need 15 points
   vector<Point3> P;
-  P += Point3(-0.1, -0.5, 1), Point3(-0.1, 0, 1), Point3(-0.1, 0.5, 1),       //
-      Point3(0, -0.5, 1), Point3(0, 0, 1), Point3(0, 0.5, 1),                 //
-      Point3(0.1, -0.5, 1), Point3(0.1, 0, 1), Point3(0.1, 0.5, 1),           //
-      Point3(-0.1, -0.5, 0.5), Point3(-0.1, 0, 0.5), Point3(-0.1, 0.5, 0.5),  //
-      Point3(0, -0.5, 0.5), Point3(0, 0, 0.5), Point3(0, 0.5, 0.5),           //
+  P += Point3(0, 0, 1), Point3(-0.1, 0, 1), Point3(0.1, 0, 1),		  //
+      Point3(0, 0.5, 0.5), Point3(0, -0.5, 0.5), Point3(-1, -0.5, 2),     //
+      Point3(-1, 0.5, 2), Point3(0.25, -0.5, 1.5), Point3(0.25, 0.5, 1.5),//
+      Point3(-0.1, -0.5, 0.5), Point3(0.1, -0.5, 1), Point3(0.1, 0.5, 1), //
+      Point3(-0.1, 0, 0.5), Point3(-0.1, 0.5, 0.5), Point3(0, 0, 0.5),    //
       Point3(0.1, -0.5, 0.5), Point3(0.1, 0, 0.5), Point3(0.1, 0.5, 0.5);
 
   // Assumes example is run in ${GTSAM_TOP}/build/examples
-  const string filename = "../../examples/data/18pointExample1.txt";
+  const string filename = "../../examples/Data/18pointExample1.txt";
   createExampleBALFile(filename, P, pose1, pose2);
 }
 
 /* ************************************************************************* */
-void create11PointExample2() {
+void create11PointExample1() {
   // Create two cameras poses
   Rot3 aRb = Rot3::Yaw(M_PI_2);
   Point3 aTb(10, 0, 0);
   Pose3 pose1, pose2(aRb, aTb);
 
   // Create test data, we need 11 points
   vector<Point3> P;
-  P += Point3(0, 0, 100), Point3(0, 0, 100), Point3(0, 0, 100),  //
-      Point3(-10, 0, 100), Point3(10, 0, 100),                   //
-      Point3(0, 50, 50), Point3(0, -50, 50),                     //
-      Point3(-10, 50, 50), Point3(10, -50, 50),                  //
-      Point3(-20, 0, 80), Point3(20, -50, 80);
+  P += Point3(0, 0, 100), Point3(-10, 0, 100), Point3(10, 0, 100),	//
+      Point3(0, 50, 50), Point3(0, -50, 50), Point3(-20, 0, 80),	// 
+      Point3(20, -50, 80), Point3(0, 0, 100), Point3(0, 0, 100),	//
+      Point3(-10, 50, 50), Point3(10, -50, 50);
 
   // Assumes example is run in ${GTSAM_TOP}/build/examples
-  const string filename = "../../examples/data/11pointExample2.txt";
+  const string filename = "../../examples/Data/11pointExample1.txt";
   Cal3Bundler K(500, 0, 0);
   createExampleBALFile(filename, P, pose1, pose2, K);
 }
@@ -150,7 +145,7 @@ int main(int argc, char* argv[]) {
   create5PointExample1();
   create5PointExample2();
   create18PointExample1();
-  create11PointExample2();
+  create11PointExample1();
   return 0;
 }
 

diff --git a/examples/Data/18pointExample1.txt b/examples/Data/18pointExample1.txt
@@ -0,0 +1,131 @@
+2 18 36
+
+0 0 0 -0
+1 0 -6.123233995736766344e-18 -0.10000000000000000555
+0 1 -0.10000000000000000555 -0
+1 1 -1.2246467991473532688e-17 -0.2000000000000000111
+0 2 0.10000000000000000555 -0
+1 2 0 -0
+0 3 0 -1
+1 3 1 -0.20000000000000006661
+0 4 0 1
+1 4 -1 -0.19999999999999995559
+0 5 -0.5 0.25
+1 5 -0.25000000000000005551 -0.55000000000000004441
+0 6 -0.5 -0.25
+1 6 0.24999999999999997224 -0.55000000000000004441
+0 7 0.16666666666666665741 0.33333333333333331483
+1 7 -0.33333333333333331483 0.10000000000000000555
+0 8 0.16666666666666665741 -0.33333333333333331483
+1 8 0.33333333333333331483 0.099999999999999977796
+0 9 -0.2000000000000000111 1
+1 9 -1 -0.39999999999999996669
+0 10 0.10000000000000000555 0.5
+1 10 -0.5 3.0616169978683830179e-17
+0 11 0.10000000000000000555 -0.5
+1 11 0.5 -3.0616169978683830179e-17
+0 12 -0.2000000000000000111 -0
+1 12 -2.4492935982947065376e-17 -0.4000000000000000222
+0 13 -0.2000000000000000111 -1
+1 13 1 -0.40000000000000007772
+0 14 0 -0
+1 14 -1.2246467991473532688e-17 -0.2000000000000000111
+0 15 0.2000000000000000111 1
+1 15 -1 6.1232339957367660359e-17
+0 16 0.2000000000000000111 -0
+1 16 0 -0
+0 17 0.2000000000000000111 -1
+1 17 1 -6.1232339957367660359e-17
+
+3.141592653589793116
+                   0
+                   0
+-0
+0
+0
+1
+0
+0
+
+2.2214414690791830509
+2.2214414690791826068
+                    0
+-6.123233995736766344e-18
+-0.10000000000000000555
+0
+1
+0
+0
+
+0
+0
+1
+
+-0.10000000000000000555
+0
+1
+
+0.10000000000000000555
+0
+1
+
+0
+0.5
+0.5
+
+0
+-0.5
+0.5
+
+-1
+-0.5
+2
+
+-1
+0.5
+2
+
+0.25
+-0.5
+1.5
+
+0.25
+0.5
+1.5
+
+-0.10000000000000000555
+-0.5
+0.5
+
+0.10000000000000000555
+-0.5
+1
+
+0.10000000000000000555
+0.5
+1
+
+-0.10000000000000000555
+0
+0.5
+
+-0.10000000000000000555
+0.5
+0.5
+
+0
+0
+0.5
+
+0.10000000000000000555
+-0.5
+0.5
+
+0.10000000000000000555
+0
+0.5
+
+0.10000000000000000555
+0.5
+0.5
+
diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp
@@ -101,80 +101,16 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb,
 }
 
 /* ************************************************************************* */
-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;
-
-  // 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();
-  Matrix21 nA = I * lA;
-  Matrix21 nB = I * lB;
-  double nA_sq_norm = nA.squaredNorm();
-  double nB_sq_norm = nB.squaredNorm();
-
-  // compute the normalizing denominator and finally the sampson error
-  double denominator = sqrt(nA_sq_norm + nB_sq_norm);
-  double sampson_error = algebraic_error / denominator;
-
-  if (HE) {
+double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, //
+    OptionalJacobian<1, 5> H) const {
+  if (H) {
     // See math.lyx
-    // computing the derivatives of the numerator w.r.t. E
-    Matrix13 numerator_H_R = vA.transpose() * E_ * skewSymmetric(-vB);
-    Matrix12 numerator_H_D = vA.transpose() *
-                             skewSymmetric(-rotation().matrix() * vB) *
-                             direction().basis();
-
-    // computing the derivatives of the denominator w.r.t. E
-    Matrix12 denominator_H_nA = nA.transpose() / denominator;
-    Matrix12 denominator_H_nB = nB.transpose() / denominator;
-    Matrix13 denominator_H_lA = denominator_H_nA * I;
-    Matrix13 denominator_H_lB = denominator_H_nB * I;
-    Matrix33 lB_H_R = E_ * skewSymmetric(-vB);
-    Matrix32 lB_H_D =
-        skewSymmetric(-rotation().matrix() * vB) * direction().basis();
-    Matrix33 lA_H_R = skewSymmetric(E_.matrix().transpose() * vA);
-    Matrix32 lA_H_D =
-        rotation().inverse().matrix() * skewSymmetric(vA) * direction().basis();
-
-    Matrix13 denominator_H_R =
-        denominator_H_lA * lA_H_R + denominator_H_lB * lB_H_R;
-    Matrix12 denominator_H_D =
-        denominator_H_lA * lA_H_D + denominator_H_lB * lB_H_D;
-
-    Matrix15 denominator_H;
-    denominator_H << denominator_H_R, denominator_H_D;
-    Matrix15 numerator_H;
-    numerator_H << numerator_H_R, numerator_H_D;
-
-    *HE = 2 * sampson_error * (numerator_H / denominator -
-         algebraic_error * denominator_H / (denominator * denominator));
+    Matrix13 HR = vA.transpose() * E_ * skewSymmetric(-vB);
+    Matrix12 HD = vA.transpose() * skewSymmetric(-rotation().matrix() * vB)
+        * direction().basis();
+    *H << HR, HD;
   }
-
-  if (HvA){
-    Matrix13 numerator_H_vA = vB.transpose() * matrix().transpose();
-    Matrix13 denominator_H_vA = nA.transpose() * I * matrix().transpose() / 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 = 2 * sampson_error * (numerator_H_vB / denominator - 
-        algebraic_error * denominator_H_vB / (denominator * denominator));
-  }
-
-  return sampson_error * sampson_error;
+  return dot(vA, E_ * vB);
 }
 
 /* ************************************************************************* */

diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h
@@ -159,11 +159,9 @@ class EssentialMatrix {
     return E.rotate(cRb);
   }
 
-  /// epipolar error, sampson squared
+  /// epipolar error, algebraic
   GTSAM_EXPORT double error(const Vector3& vA, const Vector3& vB,
-      OptionalJacobian<1, 5> HE = boost::none, 
-      OptionalJacobian<1, 3> HvA = boost::none,
-      OptionalJacobian<1, 3> HvB = boost::none) const;
+      OptionalJacobian<1, 5> H = boost::none) const;
 
   /// @}
 

diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp
@@ -241,70 +241,10 @@ TEST (EssentialMatrix, epipoles) {
   EXPECT(assert_equal(e2, E.epipole_b()));
 }
 
-//*************************************************************************
-TEST(EssentialMatrix, errorValue) {
-  // Use two points to get error
-  Point3 a(1, -2, 1);
-  Point3 b(3, 1, 1);
-
-  // compute the expected error
-  // E = [0, 0, 0; 0, 0, -1; 1, 0, 0]
-  // line for b = [0, -1, 3]
-  // line for a = [1, 0, 2]
-  // algebraic error = 5
-  // norm of line for b = 1
-  // norm of line for a = 1
-  // sampson error = 5^2 / 1^2 + 1^2
-  double expected = 12.5;
-
-  // check the error
-  double actual = trueE.error(a, b);
-  EXPECT(assert_equal(expected, actual, 1e-6));
-}
-
-//*************************************************************************
-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 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);
-  EssentialMatrix E(c1Rc2, Unit3(c1Tc2));
-
-  // Use numerical derivatives to calculate the expected Jacobian
-  Matrix13 HRexpected;
-  Matrix12 HDexpected;
-  Matrix13 HvAexpected;
-  Matrix13 HvBexpected;
-  HRexpected = numericalDerivative41<double, Rot3, Unit3, Point3, Point3>(error_, E.rotation(),
-                                                          E.direction(), vA, vB);
-  HDexpected = numericalDerivative42<double, Rot3, Unit3, Point3, Point3>(error_, E.rotation(),
-                                                          E.direction(), vA, vB);
-  HvAexpected = numericalDerivative43<double, Rot3, Unit3, Point3, Point3>(error_, E.rotation(),
-                                                          E.direction(), vA, vB);
-  HvBexpected = numericalDerivative44<double, Rot3, Unit3, Point3, Point3>(error_, E.rotation(),
-                                                          E.direction(), vA, vB);
-  Matrix15 HEexpected;
-  HEexpected << HRexpected, HDexpected;
-
-  Matrix15 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));
-}
-
 /* ************************************************************************* */
 int main() {
   TestResult tr;
   return TestRegistry::runAllTests(tr);
 }
 /* ************************************************************************* */
+