From eca2bb5d8a916043608866792c13e7cb86cae10a Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Wed, 30 Oct 2024 10:27:51 -0700
Subject: [PATCH] epipolarLine

---
 gtsam/geometry/FundamentalMatrix.cpp | 28 ++++++++++++++++++++++++++++
 gtsam/geometry/FundamentalMatrix.h   |  7 +++++++
 2 files changed, 35 insertions(+)

diff --git a/gtsam/geometry/FundamentalMatrix.cpp b/gtsam/geometry/FundamentalMatrix.cpp
index 5e8c26e62..837ba7263 100644
--- a/gtsam/geometry/FundamentalMatrix.cpp
+++ b/gtsam/geometry/FundamentalMatrix.cpp
@@ -74,6 +74,20 @@ Matrix3 FundamentalMatrix::matrix() const {
          V_.transpose().matrix();
 }
 
+Vector3 FundamentalMatrix::epipolarLine(const Point2& p,
+                                        OptionalJacobian<3, 7> H) {
+  Vector3 point(p.x(), p.y(), 1);  // Create a point in homogeneous coordinates
+  Vector3 line = matrix() * point;  // Compute the epipolar line
+
+  if (H) {
+    // Compute the Jacobian if requested
+    throw std::runtime_error(
+        "FundamentalMatrix::epipolarLine: Jacobian not implemented yet.");
+  }
+
+  return line;  // Return the epipolar line
+}
+
 void FundamentalMatrix::print(const std::string& s) const {
   std::cout << s << matrix() << std::endl;
 }
@@ -116,6 +130,20 @@ Matrix3 SimpleFundamentalMatrix::matrix() const {
   return Ka().transpose().inverse() * E_.matrix() * Kb().inverse();
 }
 
+Vector3 SimpleFundamentalMatrix::epipolarLine(const Point2& p,
+                                              OptionalJacobian<3, 7> H) {
+  Vector3 point(p.x(), p.y(), 1);  // Create a point in homogeneous coordinates
+  Vector3 line = matrix() * point;  // Compute the epipolar line
+
+  if (H) {
+    // Compute the Jacobian if requested
+    throw std::runtime_error(
+        "SimpleFundamentalMatrix::epipolarLine: Jacobian not implemented yet.");
+  }
+
+  return line;  // Return the epipolar line
+}
+
 void SimpleFundamentalMatrix::print(const std::string& s) const {
   std::cout << s << " E:\n"
             << E_.matrix() << "\nfa: " << fa_ << "\nfb: " << fb_
diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h
index c114c2b5b..df2c2881a 100644
--- a/gtsam/geometry/FundamentalMatrix.h
+++ b/gtsam/geometry/FundamentalMatrix.h
@@ -7,6 +7,7 @@
 
 #pragma once
 
+#include <gtsam/base/OptionalJacobian.h>
 #include <gtsam/geometry/EssentialMatrix.h>
 #include <gtsam/geometry/Rot3.h>
 #include <gtsam/geometry/Unit3.h>
@@ -86,6 +87,9 @@ class GTSAM_EXPORT FundamentalMatrix {
   /// Return the fundamental matrix representation
   Matrix3 matrix() const;
 
+  /// Computes the epipolar line in a (left) for a given point in b (right)
+  Vector3 epipolarLine(const Point2& p, OptionalJacobian<3, 7> H = {});
+
   /// @name Testable
   /// @{
   /// Print the FundamentalMatrix
@@ -161,6 +165,9 @@ class GTSAM_EXPORT SimpleFundamentalMatrix {
   /// F = Ka^(-T) * E * Kb^(-1)
   Matrix3 matrix() const;
 
+  /// Computes the epipolar line in a (left) for a given point in b (right)
+  Vector3 epipolarLine(const Point2& p, OptionalJacobian<3, 7> H = {});
+
   /// @name Testable
   /// @{
   /// Print the SimpleFundamentalMatrix