Merge pull request #1888 from borglab/feature/wrapF

F wrapper and bugfix

examples/SFMdata.h

@@ -56,7 +56,7 @@ std::vector<Point3> createPoints() {
 
 /**
  * Create a set of ground-truth poses
- *  Default values give a circular trajectory, radius 30 at pi/4 intervals,
+ * Default values give a circular trajectory, radius 30 at pi/4 intervals,
  * always facing the circle center
  */
 std::vector<Pose3> createPoses(

examples/ViewGraphExample.cpp

@@ -38,7 +38,7 @@ using namespace gtsam;
 /* ************************************************************************* */
 int main(int argc, char* argv[]) {
   // Define the camera calibration parameters
-  Cal3_S2 K(50.0, 50.0, 0.0, 50.0, 50.0);
+  Cal3_S2 cal(50.0, 50.0, 0.0, 50.0, 50.0);
 
   // Create the set of 8 ground-truth landmarks
   vector<Point3> points = createPoints();
@@ -47,13 +47,13 @@ int main(int argc, char* argv[]) {
   vector<Pose3> poses = posesOnCircle(4, 30);
 
   // Calculate ground truth fundamental matrices, 1 and 2 poses apart
-  auto F1 = FundamentalMatrix(K, poses[0].between(poses[1]), K);
-  auto F2 = FundamentalMatrix(K, poses[0].between(poses[2]), K);
+  auto F1 = FundamentalMatrix(cal.K(), poses[0].between(poses[1]), cal.K());
+  auto F2 = FundamentalMatrix(cal.K(), poses[0].between(poses[2]), cal.K());
 
   // Simulate measurements from each camera pose
   std::array<std::array<Point2, 8>, 4> p;
   for (size_t i = 0; i < 4; ++i) {
-    PinholeCamera<Cal3_S2> camera(poses[i], K);
+    PinholeCamera<Cal3_S2> camera(poses[i], cal);
     for (size_t j = 0; j < 8; ++j) {
       p[i][j] = camera.project(points[j]);
     }

gtsam/geometry/FundamentalMatrix.cpp

@@ -2,10 +2,12 @@
  * @file FundamentalMatrix.cpp
  * @brief FundamentalMatrix classes
  * @author Frank Dellaert
- * @date Oct 23, 2024
+ * @date October 2024
  */
 
+#include <gtsam/base/Matrix.h>
 #include <gtsam/geometry/FundamentalMatrix.h>
+#include <gtsam/geometry/Point2.h>
 
 namespace gtsam {
 
@@ -26,6 +28,11 @@ Point2 EpipolarTransfer(const Matrix3& Fca, const Point2& pa,  //
 }
 
 //*************************************************************************
+FundamentalMatrix::FundamentalMatrix(const Matrix3& U, double s,
+                                     const Matrix3& V) {
+  initialize(U, s, V);
+}
+
 FundamentalMatrix::FundamentalMatrix(const Matrix3& F) {
   // Perform SVD
   Eigen::JacobiSVD<Matrix3> svd(F, Eigen::ComputeFullU | Eigen::ComputeFullV);
@@ -47,28 +54,24 @@ FundamentalMatrix::FundamentalMatrix(const Matrix3& F) {
         "The input matrix does not represent a valid fundamental matrix.");
   }
 
-  // Ensure the second singular value is recorded as s
-  s_ = singularValues(1);
+  initialize(U, singularValues(1), V);
+}
 
-  // Check if U is a reflection
-  if (U.determinant() < 0) {
-    U = -U;
-    s_ = -s_;  // Change sign of scalar if U is a reflection
-  }
+void FundamentalMatrix::initialize(Matrix3 U, double s, Matrix3 V) {
+  // Check if U is a reflection and flip third column if so
+  if (U.determinant() < 0) U.col(2) *= -1;
 
-  // Check if V is a reflection
-  if (V.determinant() < 0) {
-    V = -V;
-    s_ = -s_;  // Change sign of scalar if U is a reflection
-  }
+  // Same check for V
+  if (V.determinant() < 0) V.col(2) *= -1;
 
-  // Assign the rotations
   U_ = Rot3(U);
+  s_ = s;
   V_ = Rot3(V);
 }
 
 Matrix3 FundamentalMatrix::matrix() const {
-  return U_.matrix() * Vector3(1, s_, 0).asDiagonal() * V_.transpose().matrix();
+  return U_.matrix() * Vector3(1.0, s_, 0).asDiagonal() *
+         V_.transpose().matrix();
 }
 
 void FundamentalMatrix::print(const std::string& s) const {
@@ -90,9 +93,9 @@ Vector FundamentalMatrix::localCoordinates(const FundamentalMatrix& F) const {
 }
 
 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>());
+  const Rot3 newU = U_.retract(delta.head<3>());
+  const double newS = s_ + delta(3);
+  const Rot3 newV = V_.retract(delta.tail<3>());
   return FundamentalMatrix(newU, newS, newV);
 }
 

gtsam/geometry/FundamentalMatrix.h

@@ -2,7 +2,7 @@
  * @file FundamentalMatrix.h
  * @brief FundamentalMatrix classes
  * @author Frank Dellaert
- * @date Oct 23, 2024
+ * @date October 2024
  */
 
 #pragma once
@@ -15,11 +15,15 @@ namespace gtsam {
 
 /**
  * @class FundamentalMatrix
- * @brief Represents a general fundamental matrix.
+ * @brief Represents a fundamental matrix in computer vision, which encodes the
+ * epipolar geometry between two views.
  *
- * 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.
+ * The FundamentalMatrix class encapsulates the fundamental matrix, which
+ * relates corresponding points in stereo images. It is parameterized by two
+ * rotation matrices (U and V) and a scalar parameter (s).
+ * Using these values, the fundamental matrix is represented as
+ *
+ *   F = U * diag(1, s, 0) * V^T
  */
 class GTSAM_EXPORT FundamentalMatrix {
  private:
@@ -34,15 +38,10 @@ class GTSAM_EXPORT FundamentalMatrix {
   /**
    * @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
+   * Initializes the FundamentalMatrix From the SVD representation
+   * U*diag(1,s,0)*V^T. It will internally convert to using SO(3).
    */
-  FundamentalMatrix(const Rot3& U, double s, const Rot3& V)
-      : U_(U), s_(s), V_(V) {}
+  FundamentalMatrix(const Matrix3& U, double s, const Matrix3& V);
 
   /**
    * @brief Construct from a 3x3 matrix using SVD
@@ -54,22 +53,35 @@ class GTSAM_EXPORT FundamentalMatrix {
    */
   FundamentalMatrix(const Matrix3& F);
 
+  /**
+   * @brief Construct from essential matrix and calibration matrices
+   *
+   * Initializes the FundamentalMatrix from the given essential matrix E
+   * and calibration matrices Ka and Kb, using
+   *   F = Ka^(-T) * E * Kb^(-1)
+   * and then calls constructor that decomposes F via SVD.
+   *
+   * @param E Essential matrix
+   * @param Ka Calibration matrix for the left camera
+   * @param Kb Calibration matrix for the right camera
+   */
+  FundamentalMatrix(const Matrix3& Ka, const EssentialMatrix& E,
+                    const Matrix3& Kb)
+      : FundamentalMatrix(Ka.transpose().inverse() * E.matrix() *
+                          Kb.inverse()) {}
+
   /**
    * @brief Construct from calibration matrices Ka, Kb, and pose aPb
    *
    * Initializes the FundamentalMatrix from the given calibration
    * matrices Ka and Kb, and the pose aPb.
    *
-   * @tparam CAL Calibration type, expected to have a matrix() method
    * @param Ka Calibration matrix for the left camera
    * @param aPb Pose from the left to the right camera
    * @param Kb Calibration matrix for the right camera
    */
-  template <typename CAL>
-  FundamentalMatrix(const CAL& Ka, const Pose3& aPb, const CAL& Kb)
-      : FundamentalMatrix(Ka.K().transpose().inverse() *
-                          EssentialMatrix::FromPose3(aPb).matrix() *
-                          Kb.K().inverse()) {}
+  FundamentalMatrix(const Matrix3& Ka, const Pose3& aPb, const Matrix3& Kb)
+      : FundamentalMatrix(Ka, EssentialMatrix::FromPose3(aPb), Kb) {}
 
   /// Return the fundamental matrix representation
   Matrix3 matrix() const;
@@ -96,6 +108,13 @@ class GTSAM_EXPORT FundamentalMatrix {
   /// Retract the given vector to get a new FundamentalMatrix
   FundamentalMatrix retract(const Vector& delta) const;
   /// @}
+ private:
+  /// Private constructor for internal use
+  FundamentalMatrix(const Rot3& U, double s, const Rot3& V)
+      : U_(U), s_(s), V_(V) {}
+
+  /// Initialize SO(3) matrices from general O(3) matrices
+  void initialize(Matrix3 U, double s, Matrix3 V);
 };
 
 /**

gtsam/geometry/geometry.i

@@ -578,6 +578,8 @@ class Unit3 {
   // Standard Constructors
   Unit3();
   Unit3(const gtsam::Point3& pose);
+  Unit3(double x, double y, double z);
+  Unit3(const gtsam::Point2& p, double f);
 
   // Testable
   void print(string s = "") const;
@@ -620,10 +622,10 @@ class EssentialMatrix {
   EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb);
 
   // Constructors from Pose3
-  gtsam::EssentialMatrix FromPose3(const gtsam::Pose3& _1P2_);
+  static gtsam::EssentialMatrix FromPose3(const gtsam::Pose3& _1P2_);
 
-  gtsam::EssentialMatrix FromPose3(const gtsam::Pose3& _1P2_,
-                            Eigen::Ref<Eigen::MatrixXd> H);
+  static gtsam::EssentialMatrix FromPose3(const gtsam::Pose3& _1P2_,
+                                          Eigen::Ref<Eigen::MatrixXd> H);
 
   // Testable
   void print(string s = "") const;
@@ -903,6 +905,59 @@ class Cal3Bundler {
   void serialize() const;
 };
 
+#include <gtsam/geometry/FundamentalMatrix.h>
+
+// FundamentalMatrix class
+class FundamentalMatrix {
+  // Constructors
+  FundamentalMatrix();
+  FundamentalMatrix(const gtsam::Matrix3& U, double s, const gtsam::Matrix3& V);
+  FundamentalMatrix(const gtsam::Matrix3& F);
+
+  // Overloaded constructors for specific calibration types
+  FundamentalMatrix(const gtsam::Matrix3& Ka, const gtsam::EssentialMatrix& E,
+                    const gtsam::Matrix3& Kb);
+  FundamentalMatrix(const gtsam::Matrix3& Ka, const gtsam::Pose3& aPb,
+                    const gtsam::Matrix3& Kb);
+
+  // Methods
+  gtsam::Matrix3 matrix() const;
+
+  // Testable
+  void print(const std::string& s = "") const;
+  bool equals(const gtsam::FundamentalMatrix& other, double tol = 1e-9) const;
+
+  // Manifold
+  static size_t Dim();
+  size_t dim() const;
+  gtsam::Vector localCoordinates(const gtsam::FundamentalMatrix& F) const;
+  gtsam::FundamentalMatrix retract(const gtsam::Vector& delta) const;
+};
+
+// SimpleFundamentalMatrix class
+class SimpleFundamentalMatrix {
+  // Constructors
+  SimpleFundamentalMatrix();
+  SimpleFundamentalMatrix(const gtsam::EssentialMatrix& E, double fa, double fb,
+                          const gtsam::Point2& ca, const gtsam::Point2& cb);
+
+  // Methods
+  gtsam::Matrix3 matrix() const;
+
+  // Testable
+  void print(const std::string& s = "") const;
+  bool equals(const gtsam::SimpleFundamentalMatrix& other, double tol = 1e-9) const;
+
+  // Manifold
+  static size_t Dim();
+  size_t dim() const;
+  gtsam::Vector localCoordinates(const gtsam::SimpleFundamentalMatrix& F) const;
+  gtsam::SimpleFundamentalMatrix retract(const gtsam::Vector& delta) const;
+};
+
+gtsam::Point2 EpipolarTransfer(const gtsam::Matrix3& Fca, const gtsam::Point2& pa,
+                               const gtsam::Matrix3& Fcb, const gtsam::Point2& pb);
+
 #include <gtsam/geometry/CalibratedCamera.h>
 class CalibratedCamera {
   // Standard Constructors and Named Constructors

gtsam/geometry/tests/testFundamentalMatrix.cpp

@@ -6,12 +6,15 @@
  */
 
 #include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Matrix.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>
 
+#include <cmath>
+
 using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
@@ -24,21 +27,38 @@ GTSAM_CONCEPT_MANIFOLD_INST(FundamentalMatrix)
 Rot3 trueU = Rot3::Yaw(M_PI_2);
 Rot3 trueV = Rot3::Yaw(M_PI_4);
 double trueS = 0.5;
-FundamentalMatrix trueF(trueU, trueS, trueV);
+FundamentalMatrix trueF(trueU.matrix(), trueS, trueV.matrix());
 
 //*************************************************************************
-TEST(FundamentalMatrix, localCoordinates) {
+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) {
+TEST(FundamentalMatrix, Retract) {
   FundamentalMatrix actual = trueF.retract(Z_7x1);
   EXPECT(assert_equal(trueF, actual));
 }
 
+//*************************************************************************
+// Test conversion from F matrices, including non-rotations
+TEST(FundamentalMatrix, Conversion) {
+  Matrix3 U = trueU.matrix();
+  Matrix3 V = trueV.matrix();
+  std::vector<FundamentalMatrix> Fs = {
+      FundamentalMatrix(U, trueS, V), FundamentalMatrix(U, trueS, -V),
+      FundamentalMatrix(-U, trueS, V), FundamentalMatrix(-U, trueS, -V)};
+
+  for (const auto& trueF : Fs) {
+    const Matrix3 F = trueF.matrix();
+    FundamentalMatrix actual(F);
+    // We check the matrices as the underlying representation is not unique
+    CHECK(assert_equal(F, actual.matrix()));
+  }
+}
+
 //*************************************************************************
 TEST(FundamentalMatrix, RoundTrip) {
   Vector7 d;
@@ -61,14 +81,14 @@ TEST(SimpleStereo, Conversion) {
 }
 
 //*************************************************************************
-TEST(SimpleStereo, localCoordinates) {
+TEST(SimpleStereo, LocalCoordinates) {
   Vector expected = Z_7x1;
   Vector actual = stereoF.localCoordinates(stereoF);
   EXPECT(assert_equal(expected, actual, 1e-8));
 }
 
 //*************************************************************************
-TEST(SimpleStereo, retract) {
+TEST(SimpleStereo, Retract) {
   SimpleFundamentalMatrix actual = stereoF.retract(Z_9x1);
   EXPECT(assert_equal(stereoF, actual));
 }

gtsam/nonlinear/nonlinear.i

@@ -11,6 +11,7 @@ namespace gtsam {
 #include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/CalibratedCamera.h>
 #include <gtsam/geometry/EssentialMatrix.h>
+#include <gtsam/geometry/FundamentalMatrix.h>
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/Point3.h>
@@ -537,6 +538,8 @@ class ISAM2 {
   template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
                      gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
                      gtsam::Cal3Bundler, gtsam::EssentialMatrix,
+                     gtsam::Cal3Bundler, gtsam::FundamentalMatrix,
+                     gtsam::Cal3Bundler, gtsam::SimpleFundamentalMatrix,
                      gtsam::PinholeCamera<gtsam::Cal3_S2>,
                      gtsam::PinholeCamera<gtsam::Cal3Bundler>,
                      gtsam::PinholeCamera<gtsam::Cal3Fisheye>,