Merge pull request #1884 from borglab/feature/F_matrix

Two fundamental matrix classes

gtsam/base/Manifold.h

@@ -162,7 +162,7 @@ struct FixedDimension {
   typedef const int value_type;
   static const int value = traits<T>::dimension;
   static_assert(value != Eigen::Dynamic,
-      "FixedDimension instantiated for dymanically-sized type.");
+      "FixedDimension instantiated for dynamically-sized type.");
 };
 } // \ namespace gtsam
 

gtsam/geometry/FundamentalMatrix.cpp

@@ -0,0 +1,150 @@
+/*
+ * @file FundamentalMatrix.cpp
+ * @brief FundamentalMatrix classes
+ * @author Frank Dellaert
+ * @date Oct 23, 2024
+ */
+
+#include <gtsam/geometry/FundamentalMatrix.h>
+
+namespace gtsam {
+
+//*************************************************************************
+Point2 Transfer(const Matrix3& Fca, const Point2& pa,  //
+                const Matrix3& Fcb, const Point2& pb) {
+  // Create lines in camera a from projections of the other two cameras
+  Vector3 line_a = Fca * Vector3(pa.x(), pa.y(), 1);
+  Vector3 line_b = Fcb * Vector3(pb.x(), pb.y(), 1);
+
+  // Cross the lines to find the intersection point
+  Vector3 intersectionPoint = line_a.cross(line_b);
+
+  // Normalize the intersection point
+  intersectionPoint /= intersectionPoint(2);
+
+  return intersectionPoint.head<2>();  // Return the 2D point
+}
+//*************************************************************************
+FundamentalMatrix::FundamentalMatrix(const Matrix3& F) {
+  // Perform SVD
+  Eigen::JacobiSVD<Matrix3> svd(F, Eigen::ComputeFullU | Eigen::ComputeFullV);
+
+  // Extract U and V
+  Matrix3 U = svd.matrixU();
+  Matrix3 V = svd.matrixV();
+  Vector3 singularValues = svd.singularValues();
+
+  // Scale the singular values
+  double scale = singularValues(0);
+  if (scale != 0) {
+    singularValues /= scale;  // Normalize the first singular value to 1.0
+  }
+
+  // Check if the third singular value is close to zero (valid F condition)
+  if (std::abs(singularValues(2)) > 1e-9) {
+    throw std::invalid_argument(
+        "The input matrix does not represent a valid fundamental matrix.");
+  }
+
+  // Ensure the second singular value is recorded as s
+  s_ = singularValues(1);
+
+  // Check if U is a reflection
+  if (U.determinant() < 0) {
+    U = -U;
+    s_ = -s_;  // Change sign of scalar if U is a reflection
+  }
+
+  // Check if V is a reflection
+  if (V.determinant() < 0) {
+    V = -V;
+    s_ = -s_;  // Change sign of scalar if U is a reflection
+  }
+
+  // Assign the rotations
+  U_ = Rot3(U);
+  V_ = Rot3(V);
+}
+
+Matrix3 FundamentalMatrix::matrix() const {
+  return U_.matrix() * Vector3(1, s_, 0).asDiagonal() * V_.transpose().matrix();
+}
+
+void FundamentalMatrix::print(const std::string& s) const {
+  std::cout << s << "U:\n"
+            << U_.matrix() << "\ns: " << s_ << "\nV:\n"
+            << V_.matrix() << std::endl;
+}
+
+bool FundamentalMatrix::equals(const FundamentalMatrix& other,
+                               double tol) const {
+  return U_.equals(other.U_, tol) && std::abs(s_ - other.s_) < tol &&
+         V_.equals(other.V_, tol);
+}
+
+Vector FundamentalMatrix::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;
+}
+
+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);
+}
+
+//*************************************************************************
+Matrix3 SimpleFundamentalMatrix::leftK() const {
+  Matrix3 K;
+  K << fa_, 0, ca_.x(), 0, fa_, ca_.y(), 0, 0, 1;
+  return K;
+}
+
+Matrix3 SimpleFundamentalMatrix::rightK() const {
+  Matrix3 K;
+  K << fb_, 0, cb_.x(), 0, fb_, cb_.y(), 0, 0, 1;
+  return K;
+}
+
+Matrix3 SimpleFundamentalMatrix::matrix() const {
+  return leftK().transpose().inverse() * E_.matrix() * rightK().inverse();
+}
+
+void SimpleFundamentalMatrix::print(const std::string& s) const {
+  std::cout << s << " E:\n"
+            << E_.matrix() << "\nfa: " << fa_ << "\nfb: " << fb_
+            << "\nca: " << ca_.transpose() << "\ncb: " << cb_.transpose()
+            << std::endl;
+}
+
+bool SimpleFundamentalMatrix::equals(const SimpleFundamentalMatrix& other,
+                                     double tol) 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;
+}
+
+Vector SimpleFundamentalMatrix::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;
+}
+
+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_);
+}
+
+//*************************************************************************
+
+}  // namespace gtsam

gtsam/geometry/FundamentalMatrix.h

@@ -0,0 +1,183 @@
+/*
+ * @file FundamentalMatrix.h
+ * @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>
+
+namespace gtsam {
+
+/**
+ * @class FundamentalMatrix
+ * @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 GTSAM_EXPORT 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()) {}
+
+  /**
+   * @brief Construct from U, V, and scalar s
+   *
+   * Initializes the FundamentalMatrix 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
+   */
+  FundamentalMatrix(const Rot3& U, double s, const Rot3& V)
+      : U_(U), s_(s), V_(V) {}
+
+  /**
+   * @brief Construct from a 3x3 matrix using SVD
+   *
+   * Initializes the FundamentalMatrix by performing SVD on the given
+   * matrix and ensuring U and V are not reflections.
+   *
+   * @param F A 3x3 matrix representing the fundamental matrix
+   */
+  FundamentalMatrix(const Matrix3& F);
+
+  /// Return the fundamental matrix representation
+  Matrix3 matrix() const;
+
+  /// @name Testable
+  /// @{
+  /// Print the FundamentalMatrix
+  void print(const std::string& s = "") const;
+
+  /// Check if the FundamentalMatrix is equal to another within a
+  /// tolerance
+  bool equals(const FundamentalMatrix& other, double tol = 1e-9) const;
+  /// @}
+
+  /// @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;
+
+  /// Retract the given vector to get a new FundamentalMatrix
+  FundamentalMatrix retract(const Vector& delta) const;
+  /// @}
+};
+
+/**
+ * @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 GTSAM_EXPORT 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 left calibration matrix
+  Matrix3 leftK() const;
+
+  /// Return the right calibration matrix
+  Matrix3 rightK() const;
+
+  /// Return the fundamental matrix representation
+  Matrix3 matrix() const;
+
+  /// @name Testable
+  /// @{
+  /// Print the SimpleFundamentalMatrix
+  void print(const std::string& s = "") const;
+
+  /// Check equality within a tolerance
+  bool equals(const SimpleFundamentalMatrix& other, double tol = 1e-9) const;
+  /// @}
+
+  /// @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;
+
+  /// Retract the given vector to get a new SimpleFundamentalMatrix
+  SimpleFundamentalMatrix retract(const Vector& delta) const;
+  /// @}
+};
+
+/**
+ * @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.
+ */
+GTSAM_EXPORT 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<FundamentalMatrix>
+    : public internal::Manifold<FundamentalMatrix> {};
+
+template <>
+struct traits<SimpleFundamentalMatrix>
+    : public internal::Manifold<SimpleFundamentalMatrix> {};
+
+}  // namespace gtsam