Remove inheritance, use abc language

gtsam/geometry/FundamentalMatrix.cpp

@@ -10,21 +10,20 @@
 namespace gtsam {
 
 //*************************************************************************
-Point2 FundamentalMatrix::transfer(const Matrix3& F01, const Point2& p1,
+Point2 Transfer(const Matrix3& Fca, const Point2& pa,  //
-                                   const Matrix3& F02, const Point2& p2) {
+                const Matrix3& Fcb, const Point2& pb) {
-  // Create lines in camera 0 from projections of the other two cameras
+  // Create lines in camera a from projections of the other two cameras
-  Vector3 line1 = F01 * Vector3(p1.x(), p1.y(), 1);
+  Vector3 line_a = Fca * Vector3(pa.x(), pa.y(), 1);
-  Vector3 line2 = F02 * Vector3(p2.x(), p2.y(), 1);
+  Vector3 line_b = Fcb * Vector3(pb.x(), pb.y(), 1);
 
   // Cross the lines to find the intersection point
-  Vector3 intersectionPoint = line1.cross(line2);
+  Vector3 intersectionPoint = line_a.cross(line_b);
 
   // Normalize the intersection point
   intersectionPoint /= intersectionPoint(2);
 
   return intersectionPoint.head<2>();  // Return the 2D point
 }
-
 //*************************************************************************
 GeneralFundamentalMatrix::GeneralFundamentalMatrix(const Matrix3& F) {
   // Perform SVD

gtsam/geometry/FundamentalMatrix.h

@@ -13,63 +13,6 @@
 
 namespace gtsam {
 
-/**
- * @brief Abstract base class for FundamentalMatrix
- *
- * This class provides a common interface for all types of fundamental matrices.
- * It declares a virtual function `matrix()` that must be implemented by derived
- * classes. The `matrix()` function returns a 3x3 matrix representation of the
- * fundamental matrix.
- */
-class GTSAM_EXPORT FundamentalMatrix {
- public:
-  /**
-   * @brief Returns a 3x3 matrix representation of the fundamental matrix
-   *
-   * @return A 3x3 matrix representing the fundamental matrix
-   */
-  virtual Matrix3 matrix() const = 0;
-
-  /**
-   * @brief Virtual destructor to ensure proper cleanup of derived classes
-   */
-  virtual ~FundamentalMatrix() {}
-
-  /**
-   * @brief Transfer projections from cameras 1 and 2 to camera 0
-   *
-   * Take two fundamental matrices F01 and F02, and two points p1 and p2, and
-   * returns the 2D point in camera 0 where the epipolar lines intersect.
-   */
-  static Point2 transfer(const Matrix3& F01, const Point2& p1,
-                         const Matrix3& F02, const Point2& p2);
-};
-
-/// Represents a set of three fundamental matrices for transferring points
-/// between three cameras.
-template <typename F>
-struct TripleF {
-  F F01, F12, F20;
-
-  /// Transfers a point from cameras 1,2 to camera 0.
-  Point2 transfer0(const Point2& p1, const Point2& p2) {
-    return FundamentalMatrix::transfer(F01.matrix(), p1,
-                                       F20.matrix().transpose(), p2);
-  }
-
-  /// Transfers a point from camera 0,2 to camera 1.
-  Point2 transfer1(const Point2& p0, const Point2& p2) {
-    return FundamentalMatrix::transfer(F01.matrix().transpose(), p0,
-                                       F12.matrix(), p2);
-  }
-
-  /// Transfers a point from camera 0,1 to camera 2.
-  Point2 transfer2(const Point2& p0, const Point2& p1) {
-    return FundamentalMatrix::transfer(F20.matrix(), p0,
-                                       F12.matrix().transpose(), p1);
-  }
-};
-
 /**
  * @class GeneralFundamentalMatrix
  * @brief Represents a general fundamental matrix.
@@ -78,7 +21,7 @@ struct TripleF {
  * that describes the relationship between two images. It is parameterized by a
  * left rotation U, a scalar s, and a right rotation V.
  */
-class GTSAM_EXPORT GeneralFundamentalMatrix : public FundamentalMatrix {
+class GTSAM_EXPORT GeneralFundamentalMatrix {
  private:
   Rot3 U_;    ///< Left rotation
   double s_;  ///< Scalar parameter for S
@@ -112,7 +55,7 @@ class GTSAM_EXPORT GeneralFundamentalMatrix : public FundamentalMatrix {
   GeneralFundamentalMatrix(const Matrix3& F);
 
   /// Return the fundamental matrix representation
-  Matrix3 matrix() const override;
+  Matrix3 matrix() const;
 
   /// @name Testable
   /// @{
@@ -146,7 +89,7 @@ class GTSAM_EXPORT GeneralFundamentalMatrix : public FundamentalMatrix {
  * parameterization of the essential matrix and focal lengths for left and right
  * cameras. Principal points are not part of the manifold but a convenience.
  */
-class GTSAM_EXPORT SimpleFundamentalMatrix : public FundamentalMatrix {
+class GTSAM_EXPORT SimpleFundamentalMatrix {
  private:
   EssentialMatrix E_;  ///< Essential matrix
   double fa_;          ///< Focal length for left camera
@@ -173,7 +116,7 @@ class GTSAM_EXPORT SimpleFundamentalMatrix : public FundamentalMatrix {
   Matrix3 rightK() const;
 
   /// Return the fundamental matrix representation
-  Matrix3 matrix() const override;
+  Matrix3 matrix() const;
 
   /// @name Testable
   /// @{
@@ -198,6 +141,37 @@ class GTSAM_EXPORT SimpleFundamentalMatrix : public FundamentalMatrix {
   /// @}
 };
 
+/**
+ * @brief Transfer projections from cameras a and b to camera c
+ *
+ * Take two fundamental matrices Fca and Fcb, and two points pa and pb, and
+ * returns the 2D point in view (c) where the epipolar lines intersect.
+ */
+Point2 Transfer(const Matrix3& Fca, const Point2& pa, const Matrix3& Fcb,
+                const Point2& pb);
+
+/// Represents a set of three fundamental matrices for transferring points
+/// between three cameras.
+template <typename F>
+struct TripleF {
+  F Fab, Fbc, Fca;
+
+  /// Transfers a point from cameras b,c to camera a.
+  Point2 transferToA(const Point2& pb, const Point2& pc) {
+    return Transfer(Fab.matrix(), pb, Fca.matrix().transpose(), pc);
+  }
+
+  /// Transfers a point from camera a,c to camera b.
+  Point2 transferToB(const Point2& pa, const Point2& pc) {
+    return Transfer(Fab.matrix().transpose(), pa, Fbc.matrix(), pc);
+  }
+
+  /// Transfers a point from cameras a,b to camera c.
+  Point2 transferToC(const Point2& pa, const Point2& pb) {
+    return Transfer(Fca.matrix(), pa, Fbc.matrix().transpose(), pb);
+  }
+};
+
 template <>
 struct traits<GeneralFundamentalMatrix>
     : public internal::Manifold<GeneralFundamentalMatrix> {};

gtsam/geometry/tests/testFundamentalMatrix.cpp

@@ -7,7 +7,6 @@
 
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/Testable.h>
-#include <gtsam/base/numericalDerivative.h>
 #include <gtsam/geometry/FundamentalMatrix.h>
 #include <gtsam/geometry/Rot3.h>
 #include <gtsam/geometry/SimpleCamera.h>
@@ -205,9 +204,9 @@ TEST(TripleF, Transfer) {
   auto triplet = generateTripleF(cameraPoses);
 
   // Check that they are all equal
-  EXPECT(triplet.F01.equals(triplet.F12, 1e-9));
+  EXPECT(triplet.Fab.equals(triplet.Fbc, 1e-9));
-  EXPECT(triplet.F12.equals(triplet.F20, 1e-9));
+  EXPECT(triplet.Fbc.equals(triplet.Fca, 1e-9));
-  EXPECT(triplet.F20.equals(triplet.F01, 1e-9));
+  EXPECT(triplet.Fca.equals(triplet.Fab, 1e-9));
 
   // Now project a point into the three cameras
   const Point3 P(0.1, 0.2, 0.3);
@@ -222,9 +221,9 @@ TEST(TripleF, Transfer) {
   }
 
   // Check that transfer works
-  EXPECT(assert_equal<Point2>(p[0], triplet.transfer0(p[1], p[2]), 1e-9));
+  EXPECT(assert_equal<Point2>(p[0], triplet.transferToA(p[1], p[2]), 1e-9));
-  EXPECT(assert_equal<Point2>(p[1], triplet.transfer1(p[0], p[2]), 1e-9));
+  EXPECT(assert_equal<Point2>(p[1], triplet.transferToB(p[0], p[2]), 1e-9));
-  EXPECT(assert_equal<Point2>(p[2], triplet.transfer2(p[0], p[1]), 1e-9));
+  EXPECT(assert_equal<Point2>(p[2], triplet.transferToC(p[0], p[1]), 1e-9));
 }
 
 //*************************************************************************