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

gtsam/geometry/tests/testFundamentalMatrix.cpp

@@ -0,0 +1,234 @@
+/*
+ * @file testFundamentalMatrix.cpp
+ * @brief Test FundamentalMatrix classes
+ * @author Frank Dellaert
+ * @date October 23, 2024
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/geometry/FundamentalMatrix.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/geometry/SimpleCamera.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));
+}
+
+//*************************************************************************
+// Create the simplest SimpleFundamentalMatrix, a stereo pair
+EssentialMatrix defaultE(Rot3(), Unit3(1, 0, 0));
+Point2 zero(0.0, 0.0);
+SimpleFundamentalMatrix stereoF(defaultE, 1.0, 1.0, zero, zero);
+
+//*************************************************************************
+TEST(SimpleStereo, Conversion) {
+  FundamentalMatrix convertedF(stereoF.matrix());
+  EXPECT(assert_equal(stereoF.matrix(), convertedF.matrix(), 1e-8));
+}
+
+//*************************************************************************
+TEST(SimpleStereo, localCoordinates) {
+  Vector expected = Z_7x1;
+  Vector actual = stereoF.localCoordinates(stereoF);
+  EXPECT(assert_equal(expected, actual, 1e-8));
+}
+
+//*************************************************************************
+TEST(SimpleStereo, retract) {
+  SimpleFundamentalMatrix actual = stereoF.retract(Z_9x1);
+  EXPECT(assert_equal(stereoF, actual));
+}
+
+//*************************************************************************
+TEST(SimpleStereo, RoundTrip) {
+  Vector7 d;
+  d << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7;
+  SimpleFundamentalMatrix hx = stereoF.retract(d);
+  Vector actual = stereoF.localCoordinates(hx);
+  EXPECT(assert_equal(d, actual, 1e-8));
+}
+
+//*************************************************************************
+TEST(SimpleStereo, EpipolarLine) {
+  // Create a point in b
+  Point3 p_b(0, 2, 1);
+  // Convert the point to a horizontal line in a
+  Vector3 l_a = stereoF.matrix() * p_b;
+  // Check if the line is horizontal at height 2
+  EXPECT(assert_equal(Vector3(0, -1, 2), l_a));
+}
+
+//*************************************************************************
+// Create a stereo pair, but in pixels not normalized coordinates.
+// We're still using zero principal points here.
+double focalLength = 1000;
+SimpleFundamentalMatrix pixelStereo(defaultE, focalLength, focalLength, zero,
+                                    zero);
+
+//*************************************************************************
+TEST(PixelStereo, Conversion) {
+  auto expected = pixelStereo.matrix();
+
+  FundamentalMatrix convertedF(pixelStereo.matrix());
+
+  // Check equality of F-matrices up to a scale
+  auto actual = convertedF.matrix();
+  actual *= expected(1, 2) / actual(1, 2);
+  EXPECT(assert_equal(expected, actual, 1e-5));
+}
+
+//*************************************************************************
+TEST(PixelStereo, PointInBToHorizontalLineInA) {
+  // Create a point in b
+  Point3 p_b = Point3(0, 300, 1);
+  // Convert the point to a horizontal line in a
+  Vector3 l_a = pixelStereo.matrix() * p_b;
+  // Check if the line is horizontal at height 2
+  EXPECT(assert_equal(Vector3(0, -0.001, 0.3), l_a));
+}
+
+//*************************************************************************
+// Create a stereo pair with the right camera rotated 90 degrees
+Rot3 aRb = Rot3::Rz(M_PI_2);  // Rotate 90 degrees around the Z-axis
+EssentialMatrix rotatedE(aRb, Unit3(1, 0, 0));
+SimpleFundamentalMatrix rotatedPixelStereo(rotatedE, focalLength, focalLength,
+                                           zero, zero);
+
+//*************************************************************************
+TEST(RotatedPixelStereo, Conversion) {
+  auto expected = rotatedPixelStereo.matrix();
+
+  FundamentalMatrix convertedF(rotatedPixelStereo.matrix());
+
+  // Check equality of F-matrices up to a scale
+  auto actual = convertedF.matrix();
+  actual *= expected(1, 2) / actual(1, 2);
+  EXPECT(assert_equal(expected, actual, 1e-4));
+}
+
+//*************************************************************************
+TEST(RotatedPixelStereo, PointInBToHorizontalLineInA) {
+  // Create a point in b
+  Point3 p_b = Point3(300, 0, 1);
+  // Convert the point to a horizontal line in a
+  Vector3 l_a = rotatedPixelStereo.matrix() * p_b;
+  // Check if the line is horizontal at height 2
+  EXPECT(assert_equal(Vector3(0, -0.001, 0.3), l_a));
+}
+
+//*************************************************************************
+// Now check that principal points also survive conversion
+Point2 principalPoint(640 / 2, 480 / 2);
+SimpleFundamentalMatrix stereoWithPrincipalPoints(rotatedE, focalLength,
+                                                  focalLength, principalPoint,
+                                                  principalPoint);
+
+//*************************************************************************
+TEST(stereoWithPrincipalPoints, Conversion) {
+  auto expected = stereoWithPrincipalPoints.matrix();
+
+  FundamentalMatrix convertedF(stereoWithPrincipalPoints.matrix());
+
+  // Check equality of F-matrices up to a scale
+  auto actual = convertedF.matrix();
+  actual *= expected(1, 2) / actual(1, 2);
+  EXPECT(assert_equal(expected, actual, 1e-4));
+}
+
+//*************************************************************************
+/// Generate three cameras on a circle, looking in
+std::array<Pose3, 3> generateCameraPoses() {
+  std::array<Pose3, 3> cameraPoses;
+  const double radius = 1.0;
+  for (int i = 0; i < 3; ++i) {
+    double angle = i * 2.0 * M_PI / 3.0;
+    double c = cos(angle), s = sin(angle);
+    Rot3 aRb({-s, c, 0}, {0, 0, -1}, {-c, -s, 0});
+    cameraPoses[i] = {aRb, Point3(radius * c, radius * s, 0)};
+  }
+  return cameraPoses;
+}
+
+//*************************************************************************
+/// Function to generate a TripleF from camera poses
+TripleF<SimpleFundamentalMatrix> generateTripleF(
+    const std::array<Pose3, 3>& cameraPoses) {
+  std::array<SimpleFundamentalMatrix, 3> F;
+  for (size_t i = 0; i < 3; ++i) {
+    size_t j = (i + 1) % 3;
+    const Pose3 iPj = cameraPoses[i].between(cameraPoses[j]);
+    EssentialMatrix E(iPj.rotation(), Unit3(iPj.translation()));
+    F[i] = {E, focalLength, focalLength, principalPoint, principalPoint};
+  }
+  return {F[0], F[1], F[2]};  // Return a TripleF instance
+}
+
+//*************************************************************************
+TEST(TripleF, Transfer) {
+  // Generate cameras on a circle, as well as fundamental matrices
+  auto cameraPoses = generateCameraPoses();
+  auto triplet = generateTripleF(cameraPoses);
+
+  // Check that they are all equal
+  EXPECT(triplet.Fab.equals(triplet.Fbc, 1e-9));
+  EXPECT(triplet.Fbc.equals(triplet.Fca, 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);
+  const Cal3_S2 K(focalLength, focalLength, 0.0,  //
+                  principalPoint.x(), principalPoint.y());
+
+  std::array<Point2, 3> p;
+  for (size_t i = 0; i < 3; ++i) {
+    // Project the point into each camera
+    PinholeCameraCal3_S2 camera(cameraPoses[i], K);
+    p[i] = camera.project(P);
+  }
+
+  // Check that transfer works
+  EXPECT(assert_equal<Point2>(p[0], triplet.transferToA(p[1], 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.transferToC(p[0], p[1]), 1e-9));
+}
+
+//*************************************************************************
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+//*************************************************************************