fixing jacobians and reformatting

borglab · Jun 9, 2021 · 0ecd8ab · 0ecd8ab
1 parent 8a2ce7e
commit 0ecd8ab
Show file tree

Hide file tree

Showing 2 changed files with 27 additions and 26 deletions.
diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp
@@ -101,17 +101,15 @@ 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> H) 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); 
-  
+  double algebraic_error = dot(vA, lB);
+
   // compute the line-norms for the two lines
   Matrix23 I;
   I.setIdentity();
@@ -128,30 +126,34 @@ double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, //
     // 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();
-
+    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) * 
-      rotation().matrix().transpose();
-    Matrix32 lA_H_D = rotation().inverse().matrix() * skewSymmetric(vA) * direction().basis();
+    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;
+    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;
 
-    *H  = numerator_H / denominator - algebraic_error * denominator_H / (denominator * denominator);
+    *H = numerator_H / denominator -
+         algebraic_error * denominator_H / (denominator * denominator);
   }
   return sampson_error;
 }

diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp
@@ -242,7 +242,7 @@ TEST (EssentialMatrix, epipoles) {
 }
 
 //*************************************************************************
-TEST (EssentialMatrix, errorValue) {
+TEST(EssentialMatrix, errorValue) {
   // Use two points to get error
   Point3 a(1, -2, 1);
   Point3 b(3, 1, 1);
@@ -254,7 +254,7 @@ 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) 
+  // sampson error = 5 / sqrt(1^2 + 1^2)
   double expected = 3.535533906;
 
   // check the error
@@ -263,15 +263,15 @@ TEST (EssentialMatrix, errorValue) {
 }
 
 //*************************************************************************
-double error_(const Rot3& R, const Unit3& t){
+double error_(const Rot3& R, const Unit3& t) {
   // Use two points to get error
   Point3 a(1, -2, 1);
   Point3 b(3, 1, 1);
 
   EssentialMatrix E = EssentialMatrix::FromRotationAndDirection(R, t);
   return E.error(a, b);
 }
-TEST (EssentialMatrix, errorJacobians) {
+TEST(EssentialMatrix, errorJacobians) {
   // Use two points to get error
   Point3 a(1, -2, 1);
   Point3 b(3, 1, 1);
@@ -283,18 +283,18 @@ TEST (EssentialMatrix, errorJacobians) {
   // Use numerical derivatives to calculate the expected Jacobian
   Matrix13 HRexpected;
   Matrix12 HDexpected;
-  HRexpected = numericalDerivative21<double, Rot3, Unit3>(
-      error_, E.rotation(), E.direction(), 1e-8);
-  HDexpected = numericalDerivative22<double, Rot3, Unit3>(
-      error_, E.rotation(), E.direction(), 1e-8);
+  HRexpected = numericalDerivative21<double, Rot3, Unit3>(error_, E.rotation(),
+                                                          E.direction());
+  HDexpected = numericalDerivative22<double, Rot3, Unit3>(error_, E.rotation(),
+                                                          E.direction());
   Matrix15 HEexpected;
   HEexpected << HRexpected, HDexpected;
 
   Matrix15 HEactual;
   E.error(a, b, HEactual);
 
   // Verify the Jacobian is correct
-  EXPECT(assert_equal(HEexpected, HEactual, 1e-8));
+  EXPECT(assert_equal(HEexpected, HEactual, 1e-5));
 }
 
 /* ************************************************************************* */
@@ -303,4 +303,3 @@ int main() {
   return TestRegistry::runAllTests(tr);
 }
 /* ************************************************************************* */
-