adding jacobians on input points

borglab · Jun 9, 2021 · 15f8b41 · 15f8b41
1 parent 582afe1
commit 15f8b41
Show file tree

Hide file tree

Showing 3 changed files with 41 additions and 17 deletions.
diff --git 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
@@ -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
@@ -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<double, Rot3, Unit3>(error_, E.rotation(),
-                                                          E.direction());
-  HDexpected = numericalDerivative22<double, Rot3, Unit3>(error_, E.rotation(),
-                                                          E.direction());
+  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;
-  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));
 }
 
 /* ************************************************************************* */