Initial check-in

gtsam/geometry/FundamentalMatrix.h

@@ -0,0 +1,83 @@
+/*
+ * @file FundamentalMatrix.h
+ * @brief FundamentalMatrix class
+ * @author Frank Dellaert
+ * @date Oct 23, 2024
+ */
+
+#pragma once
+
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/geometry/Unit3.h>
+
+namespace gtsam {
+
+class FundamentalMatrix {
+ private:
+  Rot3 U_;    ///< Left rotation
+  double s_;  ///< Scalar parameter for S
+  Rot3 V_;    ///< Right rotation
+
+ public:
+  /// Default constructor
+  FundamentalMatrix() : U_(Rot3()), s_(1.0), V_(Rot3()) {}
+
+  /// Construct from U, V, and scalar s
+  FundamentalMatrix(const Rot3& U, double s, const Rot3& V)
+      : U_(U), s_(s), V_(V) {}
+
+  /// Return the fundamental matrix representation
+  Matrix3 matrix() const {
+    return U_.matrix() * Vector3(1, s_, 1).asDiagonal() *
+           V_.transpose().matrix();
+  }
+
+  /// @name Testable
+  /// @{
+
+  /// Print the FundamentalMatrix
+  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
+  bool equals(const FundamentalMatrix& other, double tol = 1e-9) const {
+    return U_.equals(other.U_, tol) && std::abs(s_ - other.s_) < tol &&
+           V_.equals(other.V_, tol);
+  }
+
+  /// @}
+
+  /// @name Manifold
+  /// @{
+  enum { dimension = 7 };  // 3 for U, 1 for s, 3 for V
+  inline static size_t Dim() { return dimension; }
+  inline size_t dim() const { return dimension; }
+
+  /// Return local coordinates with respect to another FundamentalMatrix
+  Vector localCoordinates(const FundamentalMatrix& F) const {
+    Vector result(7);
+    result.head<3>() = U_.localCoordinates(F.U_);
+    result(3) = F.s_ - s_;  // Difference in scalar
+    result.tail<3>() = V_.localCoordinates(F.V_);
+    return result;
+  }
+
+  /// Retract the given vector to get a new FundamentalMatrix
+  FundamentalMatrix 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);
+  }
+
+  /// @}
+};
+
+template <>
+struct traits<FundamentalMatrix>
+    : public internal::Manifold<FundamentalMatrix> {};
+
+}  // namespace gtsam

gtsam/geometry/tests/testFundamentalMatrix.cpp

@@ -0,0 +1,56 @@
+/*
+ * @file testFundamentalMatrix.cpp
+ * @brief Test FundamentalMatrix class
+ * @author Frank Dellaert
+ * @date October 23, 2024
+ */
+
+#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/Unit3.h>
+
+using namespace std::placeholders;
+using namespace std;
+using namespace gtsam;
+
+GTSAM_CONCEPT_TESTABLE_INST(FundamentalMatrix)
+GTSAM_CONCEPT_MANIFOLD_INST(FundamentalMatrix)
+
+//*************************************************************************
+// 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);
+
+//*************************************************************************
+TEST(FundamentalMatrix, 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) {
+  FundamentalMatrix actual = trueF.retract(Z_7x1);
+  EXPECT(assert_equal(trueF, actual));
+}
+
+//*************************************************************************
+TEST(FundamentalMatrix, RoundTrip) {
+  Vector7 d;
+  d << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7;
+  FundamentalMatrix hx = trueF.retract(d);
+  Vector actual = trueF.localCoordinates(hx);
+  EXPECT(assert_equal(d, actual, 1e-8));
+}
+
+//*************************************************************************
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */