Simple F version

gtsam/geometry/FundamentalMatrix.h

@@ -1,12 +1,13 @@
 /*
  * @file FundamentalMatrix.h
- * @brief FundamentalMatrix class
+ * @brief FundamentalMatrix classes
  * @author Frank Dellaert
  * @date Oct 23, 2024
  */
 
 #pragma once
 
+#include <gtsam/geometry/EssentialMatrix.h>
 #include <gtsam/geometry/Rot3.h>
 #include <gtsam/geometry/Unit3.h>
 
@@ -76,8 +77,92 @@ class FundamentalMatrix {
   /// @}
 };
 
+/**
+ * @class SimpleFundamentalMatrix
+ * @brief Class for representing a simple fundamental matrix.
+ *
+ * This class represents a simple fundamental matrix, which is a
+ * 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 {
+ private:
+  EssentialMatrix E_;  ///< Essential matrix
+  double fa_;          ///< Focal length for left camera
+  double fb_;          ///< Focal length for right camera
+  Point2 ca_;          ///< Principal point for left camera
+  Point2 cb_;          ///< Principal point for right camera
+
+ public:
+  /// Default constructor
+  SimpleFundamentalMatrix()
+      : E_(), fa_(1.0), fb_(1.0), ca_(0.0, 0.0), cb_(0.0, 0.0) {}
+
+  /// Construct from essential matrix and focal lengths
+  SimpleFundamentalMatrix(const EssentialMatrix& E,  //
+                          double fa, double fb,
+                          const Point2& ca = Point2(0.0, 0.0),
+                          const Point2& cb = Point2(0.0, 0.0))
+      : E_(E), fa_(fa), fb_(fb), ca_(ca), cb_(cb) {}
+
+  /// Return the fundamental matrix representation
+  Matrix3 matrix() const {
+    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
+    return Ka * E_.matrix() * Kb.inverse();
+  }
+
+  /// @name Testable
+  /// @{
+
+  /// Print the SimpleFundamentalMatrix
+  void print(const std::string& s = "") const {
+    std::cout << s << "E:\n"
+              << E_.matrix() << "\nfa: " << fa_ << "\nfb: " << fb_
+              << "\nca: " << ca_ << "\ncb: " << cb_ << std::endl;
+  }
+
+  /// Check equality within a tolerance
+  bool equals(const SimpleFundamentalMatrix& other, double tol = 1e-9) const {
+    return E_.equals(other.E_, tol) && std::abs(fa_ - other.fa_) < tol &&
+           std::abs(fb_ - other.fb_) < tol && (ca_ - other.ca_).norm() < tol &&
+           (cb_ - other.cb_).norm() < tol;
+  }
+  /// @}
+
+  /// @name Manifold
+  /// @{
+  enum { dimension = 7 };  // 5 for E, 1 for fa, 1 for fb
+  inline static size_t Dim() { return dimension; }
+  inline size_t dim() const { return dimension; }
+
+  /// Return local coordinates with respect to another
+  /// SimpleFundamentalMatrix
+  Vector localCoordinates(const SimpleFundamentalMatrix& F) const {
+    Vector result(7);
+    result.head<5>() = E_.localCoordinates(F.E_);
+    result(5) = F.fa_ - fa_;  // Difference in fa
+    result(6) = F.fb_ - fb_;  // Difference in fb
+    return result;
+  }
+
+  /// Retract the given vector to get a new SimpleFundamentalMatrix
+  SimpleFundamentalMatrix retract(const Vector& delta) const {
+    EssentialMatrix newE = E_.retract(delta.head<5>());
+    double newFa = fa_ + delta(5);  // Update fa
+    double newFb = fb_ + delta(6);  // Update fb
+    return SimpleFundamentalMatrix(newE, newFa, newFb, ca_, cb_);
+  }
+  /// @}
+};
+
 template <>
 struct traits<FundamentalMatrix>
     : public internal::Manifold<FundamentalMatrix> {};
 
+template <>
+struct traits<SimpleFundamentalMatrix>
+    : public internal::Manifold<SimpleFundamentalMatrix> {};
+
 }  // namespace gtsam

gtsam/geometry/tests/testFundamentalMatrix.cpp

@@ -1,6 +1,6 @@
 /*
  * @file testFundamentalMatrix.cpp
- * @brief Test FundamentalMatrix class
+ * @brief Test FundamentalMatrix classes
  * @author Frank Dellaert
  * @date October 23, 2024
  */
@@ -48,6 +48,38 @@ TEST(FundamentalMatrix, RoundTrip) {
   EXPECT(assert_equal(d, actual, 1e-8));
 }
 
+//*************************************************************************
+// Create essential matrix and focal lengths for
+// SimpleFundamentalMatrix
+EssentialMatrix trueE;  // Assuming a default constructor is available
+double trueFa = 1.0;
+double trueFb = 1.0;
+Point2 trueCa(0.0, 0.0);
+Point2 trueCb(0.0, 0.0);
+SimpleFundamentalMatrix trueSimpleF(trueE, trueFa, trueFb, trueCa, trueCb);
+
+//*************************************************************************
+TEST(SimpleFundamentalMatrix, localCoordinates) {
+  Vector expected = Z_7x1;
+  Vector actual = trueSimpleF.localCoordinates(trueSimpleF);
+  EXPECT(assert_equal(expected, actual, 1e-8));
+}
+
+//*************************************************************************
+TEST(SimpleFundamentalMatrix, retract) {
+  SimpleFundamentalMatrix actual = trueSimpleF.retract(Z_9x1);
+  EXPECT(assert_equal(trueSimpleF, actual));
+}
+
+//*************************************************************************
+TEST(SimpleFundamentalMatrix, RoundTrip) {
+  Vector7 d;
+  d << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7;
+  SimpleFundamentalMatrix hx = trueSimpleF.retract(d);
+  Vector actual = trueSimpleF.localCoordinates(hx);
+  EXPECT(assert_equal(d, actual, 1e-8));
+}
+
 //*************************************************************************
 int main() {
   TestResult tr;