using fixed size matrix, and adding jacobian in homogeneous conversion

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,7 +31,11 @@ class EssentialMatrix {
 
  public:
   /// Static function to convert Point2 to homogeneous coordinates
-  static Vector3 Homogeneous(const Point2& p) {
+  static Vector3 Homogeneous(const Point2& p,
+                             OptionalJacobian<3, 2> H = boost::none) {
+    if (H) {
+      H->setIdentity();
+    }
     return Vector3(p.x(), p.y(), 1);
   }
 

gtsam/geometry/tests/testEssentialMatrix.cpp

@@ -241,6 +241,18 @@ 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;