reverting jacobian computation from homogeneous function

Author: ayushbaid

gtsam/geometry/EssentialMatrix.h

@@ -7,10 +7,10 @@
 
 #pragma once
 
-#include <gtsam/base/Manifold.h>
-#include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/Unit3.h>
+#include <gtsam/geometry/Point2.h>
+#include <gtsam/base/Manifold.h>
 
 #include <iosfwd>
 #include <string>
@@ -31,11 +31,7 @@ class EssentialMatrix {
 
  public:
   /// Static function to convert Point2 to homogeneous coordinates
-  static Vector3 Homogeneous(const Point2& p,
-                             OptionalJacobian<3, 2> H = boost::none) {
-    if (H) {
-      H->setIdentity();
-    }
+  static Vector3 Homogeneous(const Point2& p) {
     return Vector3(p.x(), p.y(), 1);
   }
 

gtsam/geometry/tests/testEssentialMatrix.cpp

@@ -241,18 +241,6 @@ TEST (EssentialMatrix, epipoles) {
   EXPECT(assert_equal(e2, E.epipole_b()));
 }
 
-//*************************************************************************
-TEST(EssentialMatrix, Homogeneous) {
-  Point2 input(5.0, 1.3);
-  Vector3 expected(5.0, 1.3, 1.0);
-  Matrix32 expectedH;
-  expectedH << 1.0, 0.0, 0.0, 1.0, 0.0, 0.0;
-  Matrix32 actualH;
-  Vector3 actual = EssentialMatrix::Homogeneous(input, actualH);
-  EXPECT(assert_equal(actual, expected));
-  EXPECT(assert_equal(actualH, expectedH));
-}
-
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/slam/EssentialMatrixFactor.h

@@ -354,28 +354,28 @@ class EssentialMatrixFactor4
       const EssentialMatrix& E, const CALIBRATION& K,
       boost::optional<Matrix&> H1 = boost::none,
       boost::optional<Matrix&> H2 = boost::none) const override {
-    Vector error(1);
     // converting from pixel coordinates to normalized coordinates cA and cB
     JacobianCalibration cA_H_K;  // dcA/dK
     JacobianCalibration cB_H_K;  // dcB/dK
     Point2 cA = K.calibrate(pA_, H2 ? &cA_H_K : 0);
     Point2 cB = K.calibrate(pB_, H2 ? &cB_H_K : 0);
 
-    // Homogeneous the coordinates
-    Matrix32 vA_H_cA, vB_H_cB;
-    Vector3 vA = EssentialMatrix::Homogeneous(cA, H2 ? &vA_H_cA : 0);
-    Vector3 vB = EssentialMatrix::Homogeneous(cB, H2 ? &vB_H_cB : 0);
+    // convert to homogeneous coordinates
+    Vector3 vA = EssentialMatrix::Homogeneous(cA);
+    Vector3 vB = EssentialMatrix::Homogeneous(cB);
 
     if (H2) {
       // compute the jacobian of error w.r.t K
 
       // error function f = vA.T * E * vB
       // H2 = df/dK = vB.T * E.T * dvA/dK + vA.T * E * dvB/dK
       // where dvA/dK = dvA/dcA * dcA/dK, dVB/dK = dvB/dcB * dcB/dK
-      *H2 = vB.transpose() * E.matrix().transpose() * vA_H_cA * cA_H_K +
-            vA.transpose() * E.matrix() * vB_H_cB * cB_H_K;
+      // and dvA/dcA = dvB/dcB = [[1, 0], [0, 1], [0, 0]]
+      *H2 = vB.transpose() * E.matrix().transpose().leftCols<2>() * cA_H_K +
+            vA.transpose() * E.matrix().leftCols<2>() * cB_H_K;
     }
 
+    Vector error(1);
     error << E.error(vA, vB, H1);
 
     return error;