Base class

gtsam/geometry/FundamentalMatrix.h

@@ -13,7 +13,33 @@
 
 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 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;
+};
+
+/**
+ * @class GeneralFundamentalMatrix
+ * @brief Represents a general fundamental matrix.
+ *
+ * This class represents a general fundamental matrix, which is a 3x3 matrix
+ * that describes the relationship between two images. It is parameterized by a
+ * left rotation U, a scalar s, and a right rotation V.
+ */
+class GeneralFundamentalMatrix : public FundamentalMatrix {
  private:
   Rot3 U_;    ///< Left rotation
   double s_;  ///< Scalar parameter for S
@@ -21,14 +47,23 @@ class FundamentalMatrix {
 
  public:
   /// Default constructor
-  FundamentalMatrix() : U_(Rot3()), s_(1.0), V_(Rot3()) {}
+  GeneralFundamentalMatrix() : U_(Rot3()), s_(1.0), V_(Rot3()) {}
 
-  /// Construct from U, V, and scalar s
+  /**
-  FundamentalMatrix(const Rot3& U, double s, const Rot3& V)
+   * @brief Construct from U, V, and scalar s
+   *
+   * Initializes the GeneralFundamentalMatrix with the given left rotation U,
+   * scalar s, and right rotation V.
+   *
+   * @param U Left rotation matrix
+   * @param s Scalar parameter for the fundamental matrix
+   * @param V Right rotation matrix
+   */
+  GeneralFundamentalMatrix(const Rot3& U, double s, const Rot3& V)
       : U_(U), s_(s), V_(V) {}
 
   /// Return the fundamental matrix representation
-  Matrix3 matrix() const {
+  Matrix3 matrix() const override {
     return U_.matrix() * Vector3(1, s_, 1).asDiagonal() *
            V_.transpose().matrix();
   }
@@ -36,15 +71,16 @@ class FundamentalMatrix {
   /// @name Testable
   /// @{
 
-  /// Print the FundamentalMatrix
+  /// Print the GeneralFundamentalMatrix
   void print(const std::string& s = "") const {
     std::cout << s << "U:\n"
               << U_.matrix() << "\ns: " << s_ << "\nV:\n"
               << V_.matrix() << std::endl;
   }
 
-  /// Check if the FundamentalMatrix is equal to another within a tolerance
+  /// Check if the GeneralFundamentalMatrix is equal to another within a
-  bool equals(const FundamentalMatrix& other, double tol = 1e-9) const {
+  /// tolerance
+  bool equals(const GeneralFundamentalMatrix& other, double tol = 1e-9) const {
     return U_.equals(other.U_, tol) && std::abs(s_ - other.s_) < tol &&
            V_.equals(other.V_, tol);
   }
@@ -57,8 +93,8 @@ class FundamentalMatrix {
   inline static size_t Dim() { return dimension; }
   inline size_t dim() const { return dimension; }
 
-  /// Return local coordinates with respect to another FundamentalMatrix
+  /// Return local coordinates with respect to another GeneralFundamentalMatrix
-  Vector localCoordinates(const FundamentalMatrix& F) const {
+  Vector localCoordinates(const GeneralFundamentalMatrix& F) const {
     Vector result(7);
     result.head<3>() = U_.localCoordinates(F.U_);
     result(3) = F.s_ - s_;  // Difference in scalar
@@ -66,12 +102,12 @@ class FundamentalMatrix {
     return result;
   }
 
-  /// Retract the given vector to get a new FundamentalMatrix
+  /// Retract the given vector to get a new GeneralFundamentalMatrix
-  FundamentalMatrix retract(const Vector& delta) const {
+  GeneralFundamentalMatrix retract(const Vector& delta) const {
     Rot3 newU = U_.retract(delta.head<3>());
     double newS = s_ + delta(3);  // Update scalar
     Rot3 newV = V_.retract(delta.tail<3>());
-    return FundamentalMatrix(newU, newS, newV);
+    return GeneralFundamentalMatrix(newU, newS, newV);
   }
 
   /// @}
@@ -85,7 +121,7 @@ class 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 SimpleFundamentalMatrix {
+class SimpleFundamentalMatrix : FundamentalMatrix {
  private:
   EssentialMatrix E_;  ///< Essential matrix
   double fa_;          ///< Focal length for left camera
@@ -106,7 +142,7 @@ class SimpleFundamentalMatrix {
       : E_(E), fa_(fa), fb_(fb), ca_(ca), cb_(cb) {}
 
   /// Return the fundamental matrix representation
-  Matrix3 matrix() const {
+  Matrix3 matrix() const override {
     Matrix3 Ka, Kb;
     Ka << fa_, 0, ca_.x(), 0, fa_, ca_.y(), 0, 0, 1;  // Left calibration
     Kb << fb_, 0, cb_.x(), 0, fb_, cb_.y(), 0, 0, 1;  // Right calibration
@@ -158,8 +194,8 @@ class SimpleFundamentalMatrix {
 };
 
 template <>
-struct traits<FundamentalMatrix>
+struct traits<GeneralFundamentalMatrix>
-    : public internal::Manifold<FundamentalMatrix> {};
+    : public internal::Manifold<GeneralFundamentalMatrix> {};
 
 template <>
 struct traits<SimpleFundamentalMatrix>

gtsam/geometry/tests/testFundamentalMatrix.cpp

@@ -16,34 +16,34 @@ using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 
-GTSAM_CONCEPT_TESTABLE_INST(FundamentalMatrix)
+GTSAM_CONCEPT_TESTABLE_INST(GeneralFundamentalMatrix)
-GTSAM_CONCEPT_MANIFOLD_INST(FundamentalMatrix)
+GTSAM_CONCEPT_MANIFOLD_INST(GeneralFundamentalMatrix)
 
 //*************************************************************************
 // Create two rotations and corresponding fundamental matrix F
 Rot3 trueU = Rot3::Yaw(M_PI_2);
 Rot3 trueV = Rot3::Yaw(M_PI_4);
 double trueS = 0.5;
-FundamentalMatrix trueF(trueU, trueS, trueV);
+GeneralFundamentalMatrix trueF(trueU, trueS, trueV);
 
 //*************************************************************************
-TEST(FundamentalMatrix, localCoordinates) {
+TEST(GeneralFundamentalMatrix, localCoordinates) {
   Vector expected = Z_7x1;  // Assuming 7 dimensions for U, V, and s
   Vector actual = trueF.localCoordinates(trueF);
   EXPECT(assert_equal(expected, actual, 1e-8));
 }
 
 //*************************************************************************
-TEST(FundamentalMatrix, retract) {
+TEST(GeneralFundamentalMatrix, retract) {
-  FundamentalMatrix actual = trueF.retract(Z_7x1);
+  GeneralFundamentalMatrix actual = trueF.retract(Z_7x1);
   EXPECT(assert_equal(trueF, actual));
 }
 
 //*************************************************************************
-TEST(FundamentalMatrix, RoundTrip) {
+TEST(GeneralFundamentalMatrix, RoundTrip) {
   Vector7 d;
   d << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7;
-  FundamentalMatrix hx = trueF.retract(d);
+  GeneralFundamentalMatrix hx = trueF.retract(d);
   Vector actual = trueF.localCoordinates(hx);
   EXPECT(assert_equal(d, actual, 1e-8));
 }