Merge branch 'develop' into check-isam

.github/workflows/build-macos.yml

@@ -25,15 +25,15 @@ jobs:
         # Github Actions requires a single row to be added to the build matrix.
         # See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
         name: [
-          macos-12-xcode-14.2,
+          macos-13-xcode-14.2,
           macos-14-xcode-15.4,
         ]
 
         build_type: [Debug, Release]
         build_unstable: [ON]
         include:
-          - name: macos-12-xcode-14.2
-            os: macos-12
+          - name: macos-13-xcode-14.2
+            os: macos-13
             compiler: xcode
             version: "14.2"
 

.github/workflows/build-python.yml

@@ -30,7 +30,7 @@ jobs:
           [
             ubuntu-20.04-gcc-9,
             ubuntu-20.04-clang-9,
-            macos-12-xcode-14.2,
+            macos-13-xcode-14.2,
             macos-14-xcode-15.4,
             windows-2022-msbuild,
           ]
@@ -48,8 +48,8 @@ jobs:
             compiler: clang
             version: "9"
 
-          - name: macos-12-xcode-14.2
-            os: macos-12
+          - name: macos-13-xcode-14.2
+            os: macos-13
             compiler: xcode
             version: "14.2"
 

CMakeLists.txt

@@ -12,6 +12,12 @@ if (POLICY CMP0167)
 cmake_policy(SET CMP0167 OLD) # Don't complain about boost
 endif()
 
+# allow parent project to override options
+if (POLICY CMP0077)
+  cmake_policy(SET CMP0077 NEW)
+endif(POLICY CMP0077)
+
+
 # Set the version number for the library
 set (GTSAM_VERSION_MAJOR 4)
 set (GTSAM_VERSION_MINOR 3)

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