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
+          - name: macos-13-xcode-14.2
-            os: macos-12
+            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
+          - name: macos-13-xcode-14.2
-            os: macos-12
+            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 F1 = FundamentalMatrix(cal.K(), poses[0].between(poses[1]), cal.K());
-  auto F2 = FundamentalMatrix(K, poses[0].between(poses[2]), 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
+  initialize(U, singularValues(1), V);
-  s_ = singularValues(1);
+}
 
-  // Check if U is a reflection
+void FundamentalMatrix::initialize(Matrix3 U, double s, Matrix3 V) {
-  if (U.determinant() < 0) {
+  // Check if U is a reflection and flip third column if so
-    U = -U;
+  if (U.determinant() < 0) U.col(2) *= -1;
-    s_ = -s_;  // Change sign of scalar if U is a reflection
-  }
 
-  // Check if V is a reflection
+  // Same check for V
-  if (V.determinant() < 0) {
+  if (V.determinant() < 0) V.col(2) *= -1;
-    V = -V;
-    s_ = -s_;  // Change sign of scalar if U is a reflection
-  }
 
-  // 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>());
+  const Rot3 newU = U_.retract(delta.head<3>());
-  double newS = s_ + delta(3);  // Update scalar
+  const double newS = s_ + delta(3);
-  Rot3 newV = V_.retract(delta.tail<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
+ * The FundamentalMatrix class encapsulates the fundamental matrix, which
- * that describes the relationship between two images. It is parameterized by a
+ * relates corresponding points in stereo images. It is parameterized by two
- * left rotation U, a scalar s, and a right rotation V.
+ * 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,
+   * Initializes the FundamentalMatrix From the SVD representation
-   * scalar s, and right rotation V.
+   * U*diag(1,s,0)*V^T. It will internally convert to using SO(3).
-   *
-   * @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)
+  FundamentalMatrix(const Matrix3& U, double s, const Matrix3& V);
-      : U_(U), s_(s), V_(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 Matrix3& Ka, const Pose3& aPb, const Matrix3& Kb)
-  FundamentalMatrix(const CAL& Ka, const Pose3& aPb, const CAL& Kb)
+      : FundamentalMatrix(Ka, EssentialMatrix::FromPose3(aPb), Kb) {}
-      : FundamentalMatrix(Ka.K().transpose().inverse() *
-                          EssentialMatrix::FromPose3(aPb).matrix() *
-                          Kb.K().inverse()) {}
 
   /// 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_,
+  static gtsam::EssentialMatrix FromPose3(const gtsam::Pose3& _1P2_,
-                            Eigen::Ref<Eigen::MatrixXd> H);
+                                          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/hybrid/HybridBayesNet.cpp

@@ -197,6 +197,30 @@ AlgebraicDecisionTree<Key> HybridBayesNet::errorTree(
   return result;
 }
 
+/* ************************************************************************* */
+double HybridBayesNet::negLogConstant(
+    const std::optional<DiscreteValues> &discrete) const {
+  double negLogNormConst = 0.0;
+  // Iterate over each conditional.
+  for (auto &&conditional : *this) {
+    if (discrete.has_value()) {
+      if (auto gm = conditional->asHybrid()) {
+        negLogNormConst += gm->choose(*discrete)->negLogConstant();
+      } else if (auto gc = conditional->asGaussian()) {
+        negLogNormConst += gc->negLogConstant();
+      } else if (auto dc = conditional->asDiscrete()) {
+        negLogNormConst += dc->choose(*discrete)->negLogConstant();
+      } else {
+        throw std::runtime_error(
+            "Unknown conditional type when computing negLogConstant");
+      }
+    } else {
+      negLogNormConst += conditional->negLogConstant();
+    }
+  }
+  return negLogNormConst;
+}
+
 /* ************************************************************************* */
 AlgebraicDecisionTree<Key> HybridBayesNet::discretePosterior(
     const VectorValues &continuousValues) const {

gtsam/hybrid/HybridBayesNet.h

@@ -237,6 +237,16 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
 
   using BayesNet::logProbability;  // expose HybridValues version
 
+  /**
+   * @brief Get the negative log of the normalization constant
+   * corresponding to the joint density represented by this Bayes net.
+   * Optionally index by `discrete`.
+   *
+   * @param discrete Optional DiscreteValues
+   * @return double
+   */
+  double negLogConstant(const std::optional<DiscreteValues> &discrete) const;
+
   /**
    * @brief Compute normalized posterior P(M|X=x) and return as a tree.
    *

gtsam/hybrid/HybridGaussianConditional.cpp

@@ -322,8 +322,11 @@ HybridGaussianConditional::shared_ptr HybridGaussianConditional::prune(
           const GaussianFactorValuePair &pair) -> GaussianFactorValuePair {
     if (max->evaluate(choices) == 0.0)
       return {nullptr, std::numeric_limits<double>::infinity()};
-    else
+    else {
-      return pair;
+      // Add negLogConstant_ back so that the minimum negLogConstant in the
+      // HybridGaussianConditional is set correctly.
+      return {pair.first, pair.second + negLogConstant_};
+    }
   };
 
   FactorValuePairs prunedConditionals = factors().apply(pruner);

gtsam/hybrid/HybridGaussianFactorGraph.cpp

@@ -59,10 +59,11 @@ using OrphanWrapper = BayesTreeOrphanWrapper<HybridBayesTree::Clique>;
 
 /// Result from elimination.
 struct Result {
+  // Gaussian conditional resulting from elimination.
   GaussianConditional::shared_ptr conditional;
-  double negLogK;
+  double negLogK;  // Negative log of the normalization constant K.
-  GaussianFactor::shared_ptr factor;
+  GaussianFactor::shared_ptr factor;  // Leftover factor 𝜏.
-  double scalar;
+  double scalar;                      // Scalar value associated with factor 𝜏.
 
   bool operator==(const Result &other) const {
     return conditional == other.conditional && negLogK == other.negLogK &&

gtsam/hybrid/tests/testHybridBayesNet.cpp

@@ -363,10 +363,6 @@ TEST(HybridBayesNet, Pruning) {
   AlgebraicDecisionTree<Key> expected(s.modes, leaves);
   EXPECT(assert_equal(expected, discretePosterior, 1e-6));
 
-  // Prune and get probabilities
-  auto prunedBayesNet = posterior->prune(2);
-  auto prunedTree = prunedBayesNet.discretePosterior(delta.continuous());
-
   // Verify logProbability computation and check specific logProbability value
   const DiscreteValues discrete_values{{M(0), 1}, {M(1), 1}};
   const HybridValues hybridValues{delta.continuous(), discrete_values};
@@ -381,10 +377,21 @@ TEST(HybridBayesNet, Pruning) {
   EXPECT_DOUBLES_EQUAL(logProbability, posterior->logProbability(hybridValues),
                        1e-9);
 
+  double negLogConstant = posterior->negLogConstant(discrete_values);
+
+  // The sum of all the mode densities
+  double normalizer =
+      AlgebraicDecisionTree<Key>(posterior->errorTree(delta.continuous()),
+                                 [](double error) { return exp(-error); })
+          .sum();
+
   // Check agreement with discrete posterior
-  // double density = exp(logProbability);
+  double density = exp(logProbability + negLogConstant) / normalizer;
-  // FAILS: EXPECT_DOUBLES_EQUAL(density, discretePosterior(discrete_values),
+  EXPECT_DOUBLES_EQUAL(density, discretePosterior(discrete_values), 1e-6);
-  // 1e-6);
+
+  // Prune and get probabilities
+  auto prunedBayesNet = posterior->prune(2);
+  auto prunedTree = prunedBayesNet.discretePosterior(delta.continuous());
 
   // Regression test on pruned logProbability tree
   std::vector<double> pruned_leaves = {0.0, 0.50758422, 0.0, 0.49241578};
@@ -392,7 +399,26 @@ TEST(HybridBayesNet, Pruning) {
   EXPECT(assert_equal(expected_pruned, prunedTree, 1e-6));
 
   // Regression
-  // FAILS: EXPECT_DOUBLES_EQUAL(density, prunedTree(discrete_values), 1e-9);
+  double pruned_logProbability = 0;
+  pruned_logProbability +=
+      prunedBayesNet.at(0)->asDiscrete()->logProbability(hybridValues);
+  pruned_logProbability +=
+      prunedBayesNet.at(1)->asHybrid()->logProbability(hybridValues);
+  pruned_logProbability +=
+      prunedBayesNet.at(2)->asHybrid()->logProbability(hybridValues);
+  pruned_logProbability +=
+      prunedBayesNet.at(3)->asHybrid()->logProbability(hybridValues);
+
+  double pruned_negLogConstant = prunedBayesNet.negLogConstant(discrete_values);
+
+  // The sum of all the mode densities
+  double pruned_normalizer =
+      AlgebraicDecisionTree<Key>(prunedBayesNet.errorTree(delta.continuous()),
+                                 [](double error) { return exp(-error); })
+          .sum();
+  double pruned_density =
+      exp(pruned_logProbability + pruned_negLogConstant) / pruned_normalizer;
+  EXPECT_DOUBLES_EQUAL(pruned_density, prunedTree(discrete_values), 1e-9);
 }
 
 /* ****************************************************************************/

gtsam/hybrid/tests/testHybridGaussianConditional.cpp

@@ -275,6 +275,11 @@ TEST(HybridGaussianConditional, Prune) {
 
     // Check that the pruned HybridGaussianConditional has 2 conditionals
     EXPECT_LONGS_EQUAL(2, pruned->nrComponents());
+
+    // Check that the minimum negLogConstant is set correctly
+    EXPECT_DOUBLES_EQUAL(
+        hgc.conditionals()({{M(1), 0}, {M(2), 1}})->negLogConstant(),
+        pruned->negLogConstant(), 1e-9);
   }
   {
     const std::vector<double> potentials{0.2, 0, 0.3, 0,  //
@@ -285,6 +290,9 @@ TEST(HybridGaussianConditional, Prune) {
 
     // Check that the pruned HybridGaussianConditional has 3 conditionals
     EXPECT_LONGS_EQUAL(3, pruned->nrComponents());
+
+    // Check that the minimum negLogConstant is correct
+    EXPECT_DOUBLES_EQUAL(hgc.negLogConstant(), pruned->negLogConstant(), 1e-9);
   }
 }
 

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>,

python/gtsam/examples/SFMExample.py

@@ -62,7 +62,7 @@ def main():
     points = SFMdata.createPoints()
 
     # Create the set of ground-truth poses
-    poses = SFMdata.createPoses(K)
+    poses = SFMdata.createPoses()
 
     # Create a factor graph
     graph = NonlinearFactorGraph()

python/gtsam/examples/SFMdata.py

@@ -3,14 +3,14 @@ A structure-from-motion example with landmarks
  - The landmarks form a 10 meter cube
  - The robot rotates around the landmarks, always facing towards the cube
 """
+
 # pylint: disable=invalid-name, E1101
 
 from typing import List
 
 import numpy as np
 
-import gtsam
+from gtsam import Point3, Pose3, Rot3
-from gtsam import Cal3_S2, Point3, Pose3
 
 
 def createPoints() -> List[Point3]:
@@ -28,16 +28,49 @@ def createPoints() -> List[Point3]:
     return points
 
 
-def createPoses(K: Cal3_S2) -> List[Pose3]:
+_M_PI_2 = np.pi / 2
-    """Generate a set of ground-truth camera poses arranged in a circle about the origin."""
+_M_PI_4 = np.pi / 4
-    radius = 40.0
+
-    height = 10.0
+
-    angles = np.linspace(0, 2 * np.pi, 8, endpoint=False)
+def createPoses(
-    up = gtsam.Point3(0, 0, 1)
+    init: Pose3 = Pose3(Rot3.Ypr(_M_PI_2, 0, -_M_PI_2), Point3(30, 0, 0)),
-    target = gtsam.Point3(0, 0, 0)
+    delta: Pose3 = Pose3(
-    poses = []
+        Rot3.Ypr(0, -_M_PI_4, 0),
-    for theta in angles:
+        Point3(np.sin(_M_PI_4) * 30, 0, 30 * (1 - np.sin(_M_PI_4))),
-        position = gtsam.Point3(radius * np.cos(theta), radius * np.sin(theta), height)
+    ),
-        camera = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up, K)
+    steps: int = 8,
-        poses.append(camera.pose())
+) -> List[Pose3]:
+    """
+    Create a set of ground-truth poses
+    Default values give a circular trajectory, radius 30 at pi/4 intervals,
+    always facing the circle center
+    """
+    poses = [init]
+    for _ in range(1, steps):
+        poses.append(poses[-1].compose(delta))
     return poses
+
+
+def posesOnCircle(num_cameras=8, R=30):
+    """Create regularly spaced poses with specified radius and number of cameras."""
+    theta = 2 * np.pi / num_cameras
+
+    # Initial pose at angle 0, position (R, 0, 0), facing the center with Y-axis pointing down
+    init_rotation = Rot3.Ypr(_M_PI_2, 0, -_M_PI_2)
+    init_position = np.array([R, 0, 0])
+    init = Pose3(init_rotation, init_position)
+
+    # Delta rotation: rotate by -theta around Z-axis (counterclockwise movement)
+    delta_rotation = Rot3.Ypr(0, -theta, 0)
+
+    # Delta translation in world frame
+    delta_translation_world = np.array([R * (np.cos(theta) - 1), R * np.sin(theta), 0])
+
+    # Transform delta translation to local frame of the camera
+    delta_translation_local = init.rotation().unrotate(delta_translation_world)
+
+    # Define delta pose
+    delta = Pose3(delta_rotation, delta_translation_local)
+
+    # Generate poses
+    return createPoses(init, delta, num_cameras)

python/gtsam/examples/TranslationAveragingExample.py

@@ -31,8 +31,7 @@ def get_data() -> Tuple[gtsam.Values, List[gtsam.BinaryMeasurementUnit3]]:
     that lie on a circle and face the center. The poses of 8 cameras are obtained from SFMdata
     and the unit translations directions between some camera pairs are computed from their
     global translations. """
-    fx, fy, s, u0, v0 = 50.0, 50.0, 0.0, 50.0, 50.0
+    wTc_list = SFMdata.createPoses()
-    wTc_list = SFMdata.createPoses(gtsam.Cal3_S2(fx, fy, s, u0, v0))
     # Rotations of the cameras in the world frame.
     wRc_values = gtsam.Values()
     # Normalized translation directions from camera i to camera j

python/gtsam/examples/VisualISAM2Example.py

@@ -73,7 +73,7 @@ def visual_ISAM2_example():
     points = SFMdata.createPoints()
 
     # Create the set of ground-truth poses
-    poses = SFMdata.createPoses(K)
+    poses = SFMdata.createPoses()
 
     # Create an iSAM2 object. Unlike iSAM1, which performs periodic batch steps
     # to maintain proper linearization and efficient variable ordering, iSAM2

python/gtsam/examples/VisualISAMExample.py

@@ -36,7 +36,7 @@ def main():
     # Create the set of ground-truth landmarks
     points = SFMdata.createPoints()
     # Create the set of ground-truth poses
-    poses = SFMdata.createPoses(K)
+    poses = SFMdata.createPoses()
 
     # Create a NonlinearISAM object which will relinearize and reorder the variables
     # every "reorderInterval" updates

python/gtsam/tests/test_FundamentalMatrix.py

@@ -0,0 +1,285 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+FundamentalMatrix unit tests.
+Author: Frank Dellaert
+"""
+
+# pylint: disable=no-name-in-module
+import unittest
+
+import numpy as np
+from gtsam.examples import SFMdata
+from numpy.testing import assert_almost_equal
+
+import gtsam
+from gtsam import (Cal3_S2, EssentialMatrix, FundamentalMatrix,
+                   PinholeCameraCal3_S2, Point2, Point3, Rot3,
+                   SimpleFundamentalMatrix, Unit3)
+
+
+class TestFundamentalMatrix(unittest.TestCase):
+
+    def setUp(self):
+        # Create two rotations and corresponding fundamental matrix F
+        self.trueU = Rot3.Yaw(np.pi / 2)
+        self.trueV = Rot3.Yaw(np.pi / 4)
+        self.trueS = 0.5
+        self.trueF = FundamentalMatrix(self.trueU.matrix(), self.trueS, self.trueV.matrix())
+
+    def test_localCoordinates(self):
+        expected = np.zeros(7)  # Assuming 7 dimensions for U, V, and s
+        actual = self.trueF.localCoordinates(self.trueF)
+        assert_almost_equal(expected, actual, decimal=8)
+
+    def test_retract(self):
+        actual = self.trueF.retract(np.zeros(7))
+        self.assertTrue(self.trueF.equals(actual, 1e-9))
+
+    def test_RoundTrip(self):
+        d = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7])
+        hx = self.trueF.retract(d)
+        actual = self.trueF.localCoordinates(hx)
+        assert_almost_equal(d, actual, decimal=8)
+
+
+class TestSimpleStereo(unittest.TestCase):
+
+    def setUp(self):
+        # Create the simplest SimpleFundamentalMatrix, a stereo pair
+        self.defaultE = EssentialMatrix(Rot3(), Unit3(1, 0, 0))
+        self.zero = Point2(0.0, 0.0)
+        self.stereoF = SimpleFundamentalMatrix(self.defaultE, 1.0, 1.0, self.zero, self.zero)
+
+    def test_Conversion(self):
+        convertedF = FundamentalMatrix(self.stereoF.matrix())
+        assert_almost_equal(self.stereoF.matrix(), convertedF.matrix(), decimal=8)
+
+    def test_localCoordinates(self):
+        expected = np.zeros(7)
+        actual = self.stereoF.localCoordinates(self.stereoF)
+        assert_almost_equal(expected, actual, decimal=8)
+
+    def test_retract(self):
+        actual = self.stereoF.retract(np.zeros(9))
+        self.assertTrue(self.stereoF.equals(actual, 1e-9))
+
+    def test_RoundTrip(self):
+        d = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7])
+        hx = self.stereoF.retract(d)
+        actual = self.stereoF.localCoordinates(hx)
+        assert_almost_equal(d, actual, decimal=8)
+
+    def test_EpipolarLine(self):
+        # Create a point in b
+        p_b = np.array([0, 2, 1])
+        # Convert the point to a horizontal line in a
+        l_a = self.stereoF.matrix() @ p_b
+        # Check if the line is horizontal at height 2
+        expected = np.array([0, -1, 2])
+        assert_almost_equal(l_a, expected, decimal=8)
+
+
+class TestPixelStereo(unittest.TestCase):
+
+    def setUp(self):
+        # Create a stereo pair in pixels, zero principal points
+        self.focalLength = 1000.0
+        self.defaultE = EssentialMatrix(Rot3(), Unit3(1, 0, 0))
+        self.zero = Point2(0.0, 0.0)
+        self.pixelStereo = SimpleFundamentalMatrix(
+            self.defaultE, self.focalLength, self.focalLength, self.zero, self.zero
+        )
+
+    def test_Conversion(self):
+        expected = self.pixelStereo.matrix()
+        convertedF = FundamentalMatrix(self.pixelStereo.matrix())
+        # Check equality of F-matrices up to a scale
+        actual = convertedF.matrix()
+        scale = expected[1, 2] / actual[1, 2]
+        actual *= scale
+        assert_almost_equal(expected, actual, decimal=5)
+
+    def test_PointInBToHorizontalLineInA(self):
+        # Create a point in b
+        p_b = np.array([0, 300, 1])
+        # Convert the point to a horizontal line in a
+        l_a = self.pixelStereo.matrix() @ p_b
+        # Check if the line is horizontal at height 0.3
+        expected = np.array([0, -0.001, 0.3])
+        assert_almost_equal(l_a, expected, decimal=8)
+
+
+class TestRotatedPixelStereo(unittest.TestCase):
+
+    def setUp(self):
+        # Create a stereo pair with the right camera rotated 90 degrees
+        self.focalLength = 1000.0
+        self.zero = Point2(0.0, 0.0)
+        self.aRb = Rot3.Rz(np.pi / 2)  # Rotate 90 degrees around the Z-axis
+        self.rotatedE = EssentialMatrix(self.aRb, Unit3(1, 0, 0))
+        self.rotatedPixelStereo = SimpleFundamentalMatrix(
+            self.rotatedE, self.focalLength, self.focalLength, self.zero, self.zero
+        )
+
+    def test_Conversion(self):
+        expected = self.rotatedPixelStereo.matrix()
+        convertedF = FundamentalMatrix(self.rotatedPixelStereo.matrix())
+        # Check equality of F-matrices up to a scale
+        actual = convertedF.matrix()
+        scale = expected[1, 2] / actual[1, 2]
+        actual *= scale
+        assert_almost_equal(expected, actual, decimal=4)
+
+    def test_PointInBToHorizontalLineInA(self):
+        # Create a point in b
+        p_b = np.array([300, 0, 1])
+        # Convert the point to a horizontal line in a
+        l_a = self.rotatedPixelStereo.matrix() @ p_b
+        # Check if the line is horizontal at height 0.3
+        expected = np.array([0, -0.001, 0.3])
+        assert_almost_equal(l_a, expected, decimal=8)
+
+
+class TestStereoWithPrincipalPoints(unittest.TestCase):
+
+    def setUp(self):
+        # Now check that principal points also survive conversion
+        self.focalLength = 1000.0
+        self.principalPoint = Point2(640 / 2, 480 / 2)
+        self.aRb = Rot3.Rz(np.pi / 2)
+        self.rotatedE = EssentialMatrix(self.aRb, Unit3(1, 0, 0))
+        self.stereoWithPrincipalPoints = SimpleFundamentalMatrix(
+            self.rotatedE, self.focalLength, self.focalLength, self.principalPoint, self.principalPoint
+        )
+
+    def test_Conversion(self):
+        expected = self.stereoWithPrincipalPoints.matrix()
+        convertedF = FundamentalMatrix(self.stereoWithPrincipalPoints.matrix())
+        # Check equality of F-matrices up to a scale
+        actual = convertedF.matrix()
+        scale = expected[1, 2] / actual[1, 2]
+        actual *= scale
+        assert_almost_equal(expected, actual, decimal=4)
+
+
+class TestTripleF(unittest.TestCase):
+
+    def setUp(self):
+        # Generate three cameras on a circle, looking in
+        self.cameraPoses = SFMdata.posesOnCircle(3, 1.0)
+        self.focalLength = 1000.0
+        self.principalPoint = Point2(640 / 2, 480 / 2)
+        self.triplet = self.generateTripleF(self.cameraPoses)
+
+    def generateTripleF(self, cameraPoses):
+        F = []
+        for i in range(3):
+            j = (i + 1) % 3
+            iPj = cameraPoses[i].between(cameraPoses[j])
+            E = EssentialMatrix(iPj.rotation(), Unit3(iPj.translation()))
+            F_ij = SimpleFundamentalMatrix(
+                E, self.focalLength, self.focalLength, self.principalPoint, self.principalPoint
+            )
+            F.append(F_ij)
+        return {"Fab": F[0], "Fbc": F[1], "Fca": F[2]}
+
+    def transferToA(self, pb, pc):
+        return gtsam.EpipolarTransfer(self.triplet["Fab"].matrix(), pb, self.triplet["Fca"].matrix().transpose(), pc)
+
+    def transferToB(self, pa, pc):
+        return gtsam.EpipolarTransfer(self.triplet["Fab"].matrix().transpose(), pa, self.triplet["Fbc"].matrix(), pc)
+
+    def transferToC(self, pa, pb):
+        return gtsam.EpipolarTransfer(self.triplet["Fca"].matrix(), pa, self.triplet["Fbc"].matrix().transpose(), pb)
+
+    def test_Transfer(self):
+        triplet = self.triplet
+        # Check that they are all equal
+        self.assertTrue(triplet["Fab"].equals(triplet["Fbc"], 1e-9))
+        self.assertTrue(triplet["Fbc"].equals(triplet["Fca"], 1e-9))
+        self.assertTrue(triplet["Fca"].equals(triplet["Fab"], 1e-9))
+
+        # Now project a point into the three cameras
+        P = Point3(0.1, 0.2, 0.3)
+        K = Cal3_S2(self.focalLength, self.focalLength, 0.0, self.principalPoint[0], self.principalPoint[1])
+
+        p = []
+        for i in range(3):
+            # Project the point into each camera
+            camera = PinholeCameraCal3_S2(self.cameraPoses[i], K)
+            p_i = camera.project(P)
+            p.append(p_i)
+
+        # Check that transfer works
+        assert_almost_equal(p[0], self.transferToA(p[1], p[2]), decimal=9)
+        assert_almost_equal(p[1], self.transferToB(p[0], p[2]), decimal=9)
+        assert_almost_equal(p[2], self.transferToC(p[0], p[1]), decimal=9)
+
+
+class TestManyCamerasCircle(unittest.TestCase):
+    N = 6
+
+    def setUp(self):
+        # Generate six cameras on a circle, looking in
+        self.cameraPoses = SFMdata.posesOnCircle(self.N, 1.0)
+        self.focalLength = 1000.0
+        self.principalPoint = Point2(640 / 2, 480 / 2)
+        self.manyFs = self.generateManyFs(self.cameraPoses)
+
+    def generateManyFs(self, cameraPoses):
+        F = []
+        for i in range(self.N):
+            j = (i + 1) % self.N
+            iPj = cameraPoses[i].between(cameraPoses[j])
+            E = EssentialMatrix(iPj.rotation(), Unit3(iPj.translation()))
+            F_ij = SimpleFundamentalMatrix(
+                E, self.focalLength, self.focalLength, self.principalPoint, self.principalPoint
+            )
+            F.append(F_ij)
+        return F
+
+    def test_Conversion(self):
+        for i in range(self.N):
+            expected = self.manyFs[i].matrix()
+            convertedF = FundamentalMatrix(self.manyFs[i].matrix())
+            # Check equality of F-matrices up to a scale
+            actual = convertedF.matrix()
+            scale = expected[1, 2] / actual[1, 2]
+            actual *= scale
+            # print(f"\n{np.round(expected, 3)}", f"\n{np.round(actual, 3)}")
+            assert_almost_equal(expected, actual, decimal=4)
+
+    def test_Transfer(self):
+        # Now project a point into the six cameras
+        P = Point3(0.1, 0.2, 0.3)
+        K = Cal3_S2(self.focalLength, self.focalLength, 0.0, self.principalPoint[0], self.principalPoint[1])
+
+        p = []
+        for i in range(self.N):
+            # Project the point into each camera
+            camera = PinholeCameraCal3_S2(self.cameraPoses[i], K)
+            p_i = camera.project(P)
+            p.append(p_i)
+
+        # Check that transfer works
+        for a in range(self.N):
+            b = (a + 1) % self.N
+            c = (a + 2) % self.N
+            # We transfer from a to b and from c to b,
+            # and check that the result lines up with the projected point in b.
+            transferred = gtsam.EpipolarTransfer(
+                self.manyFs[a].matrix().transpose(),  # need to transpose for a->b
+                p[a],
+                self.manyFs[c].matrix(),
+                p[c],
+            )
+            assert_almost_equal(p[b], transferred, decimal=9)
+
+
+if __name__ == "__main__":
+    unittest.main()