epipolarLine

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_

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