Merge pull request #1887 from borglab/feature/essential_transfer

EssentialTransferFactor, EssentialTransferFactorK, and python wrapping

examples/EssentialViewGraphExample.cpp

@@ -0,0 +1,138 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    EssentialViewGraphExample.cpp
+ * @brief   View-graph calibration with essential matrices.
+ * @author  Frank Dellaert
+ * @date    October 2024
+ */
+
+#include <gtsam/geometry/Cal3f.h>
+#include <gtsam/geometry/EssentialMatrix.h>
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Point2.h>
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/inference/EdgeKey.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/sfm/TransferFactor.h>  // Contains EssentialTransferFactorK
+
+#include <vector>
+
+#include "SFMdata.h"  // For createPoints() and posesOnCircle()
+
+using namespace std;
+using namespace gtsam;
+using namespace symbol_shorthand;  // For K(symbol)
+
+// Main function
+int main(int argc, char* argv[]) {
+  // Define the camera calibration parameters
+  Cal3f cal(50.0, 50.0, 50.0);
+
+  // Create the set of 8 ground-truth landmarks
+  vector<Point3> points = createPoints();
+
+  // Create the set of 4 ground-truth poses
+  vector<Pose3> poses = posesOnCircle(4, 30);
+
+  // Calculate ground truth essential matrices, 1 and 2 poses apart
+  auto E1 = EssentialMatrix::FromPose3(poses[0].between(poses[1]));
+  auto E2 = EssentialMatrix::FromPose3(poses[0].between(poses[2]));
+
+  // Simulate measurements from each camera pose
+  std::array<std::array<Point2, 8>, 4> p;
+  for (size_t i = 0; i < 4; ++i) {
+    PinholeCamera<Cal3f> camera(poses[i], cal);
+    for (size_t j = 0; j < 8; ++j) {
+      p[i][j] = camera.project(points[j]);
+    }
+  }
+
+  // Create the factor graph
+  NonlinearFactorGraph graph;
+  using Factor = EssentialTransferFactorK<Cal3f>;
+
+  for (size_t a = 0; a < 4; ++a) {
+    size_t b = (a + 1) % 4;  // Next camera
+    size_t c = (a + 2) % 4;  // Camera after next
+
+    // Vectors to collect tuples for each factor
+    std::vector<std::tuple<Point2, Point2, Point2>> tuples1, tuples2, tuples3;
+
+    // Collect data for the three factors
+    for (size_t j = 0; j < 8; ++j) {
+      tuples1.emplace_back(p[a][j], p[b][j], p[c][j]);
+      tuples2.emplace_back(p[a][j], p[c][j], p[b][j]);
+      tuples3.emplace_back(p[c][j], p[b][j], p[a][j]);
+    }
+
+    // Add transfer factors between views a, b, and c.
+    graph.emplace_shared<Factor>(EdgeKey(a, c), EdgeKey(b, c), tuples1);
+    graph.emplace_shared<Factor>(EdgeKey(a, b), EdgeKey(b, c), tuples2);
+    graph.emplace_shared<Factor>(EdgeKey(a, c), EdgeKey(a, b), tuples3);
+  }
+
+  // Formatter for printing keys
+  auto formatter = [](Key key) {
+    if (Symbol(key).chr() == 'k') {
+      return (string)Symbol(key);
+    } else {
+      EdgeKey edge(key);
+      return (std::string)edge;
+    }
+  };
+
+  graph.print("Factor Graph:\n", formatter);
+
+  // Create a delta vector to perturb the ground truth (small perturbation)
+  Vector5 delta;
+  delta << 1, 1, 1, 1, 1;
+  delta *= 1e-2;
+
+  // Create the initial estimate for essential matrices
+  Values initialEstimate;
+  for (size_t a = 0; a < 4; ++a) {
+    size_t b = (a + 1) % 4;  // Next camera
+    size_t c = (a + 2) % 4;  // Camera after next
+    initialEstimate.insert(EdgeKey(a, b), E1.retract(delta));
+    initialEstimate.insert(EdgeKey(a, c), E2.retract(delta));
+  }
+
+  // Insert initial calibrations (using K symbol)
+  for (size_t i = 0; i < 4; ++i) {
+    initialEstimate.insert(K(i), cal);
+  }
+
+  initialEstimate.print("Initial Estimates:\n", formatter);
+  graph.printErrors(initialEstimate, "Initial Errors:\n", formatter);
+
+  // Optimize the graph and print results
+  LevenbergMarquardtParams params;
+  params.setlambdaInitial(1000.0);  // Initialize lambda to a high value
+  params.setVerbosityLM("SUMMARY");
+  Values result =
+      LevenbergMarquardtOptimizer(graph, initialEstimate, params).optimize();
+
+  cout << "Initial error = " << graph.error(initialEstimate) << endl;
+  cout << "Final error = " << graph.error(result) << endl;
+
+  result.print("Final Results:\n", formatter);
+
+  E1.print("Ground Truth E1:\n");
+  E2.print("Ground Truth E2:\n");
+
+  return 0;
+}

examples/ViewGraphExample.cpp

@@ -114,7 +114,7 @@ int main(int argc, char* argv[]) {
     initialEstimate.insert(EdgeKey(a, c), F2.retract(delta));
   }
   initialEstimate.print("Initial Estimates:\n", formatter);
-    graph.printErrors(initialEstimate, "errors: ", formatter);
+  graph.printErrors(initialEstimate, "errors: ", formatter);
 
   /* Optimize the graph and print results */
   LevenbergMarquardtParams params;

gtsam/geometry/Cal3Bundler.cpp

@@ -56,10 +56,8 @@ void Cal3Bundler::print(const std::string& s) const {
 
 /* ************************************************************************* */
 bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const {
-  const Cal3* base = dynamic_cast<const Cal3*>(&K);
+  return Cal3f::equals(static_cast<const Cal3f&>(K), tol) &&
-  return (Cal3::equals(*base, tol) && std::fabs(k1_ - K.k1_) < tol &&
+         std::fabs(k1_ - K.k1_) < tol && std::fabs(k2_ - K.k2_) < tol;
-          std::fabs(k2_ - K.k2_) < tol && std::fabs(u0_ - K.u0_) < tol &&
-          std::fabs(v0_ - K.v0_) < tol);
 }
 
 /* ************************************************************************* */

gtsam/geometry/Cal3Bundler.h

@@ -19,7 +19,7 @@
 
 #pragma once
 
-#include <gtsam/geometry/Cal3.h>
+#include <gtsam/geometry/Cal3f.h>
 #include <gtsam/geometry/Point2.h>
 
 namespace gtsam {
@@ -29,22 +29,18 @@ namespace gtsam {
  * @ingroup geometry
  * \nosubgrouping
  */
-class GTSAM_EXPORT Cal3Bundler : public Cal3 {
+class GTSAM_EXPORT Cal3Bundler : public Cal3f {
  private:
-  double k1_ = 0.0f, k2_ = 0.0f;  ///< radial distortion
+  double k1_ = 0.0, k2_ = 0.0;  ///< radial distortion coefficients
-  double tol_ = 1e-5;             ///< tolerance value when calibrating
+  double tol_ = 1e-5;           ///< tolerance value when calibrating
 
-  // NOTE: We use the base class fx to represent the common focal length.
+  // Note: u0 and v0 are constants and not optimized.
-  // Also, image center parameters (u0, v0) are not optimized
-  // but are treated as constants.
 
  public:
   enum { dimension = 3 };
-
-  ///< shared pointer to stereo calibration object
   using shared_ptr = std::shared_ptr<Cal3Bundler>;
 
-  /// @name Standard Constructors
+  /// @name Constructors
   /// @{
 
   /// Default constructor
@@ -61,9 +57,9 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
    */
   Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0,
               double tol = 1e-5)
-      : Cal3(f, f, 0, u0, v0), k1_(k1), k2_(k2), tol_(tol) {}
+      : Cal3f(f, u0, v0), k1_(k1), k2_(k2), tol_(tol) {}
 
-  ~Cal3Bundler() override {}
+  ~Cal3Bundler() override = default;
 
   /// @}
   /// @name Testable
@@ -77,24 +73,18 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
   void print(const std::string& s = "") const override;
 
   /// assert equality up to a tolerance
-  bool equals(const Cal3Bundler& K, double tol = 10e-9) const;
+  bool equals(const Cal3Bundler& K, double tol = 1e-9) const;
 
   /// @}
   /// @name Standard Interface
   /// @{
 
-  /// distorsion parameter k1
+  /// distortion parameter k1
   inline double k1() const { return k1_; }
 
-  /// distorsion parameter k2
+  /// distortion parameter k2
   inline double k2() const { return k2_; }
 
-  /// image center in x
-  inline double px() const { return u0_; }
-
-  /// image center in y
-  inline double py() const { return v0_; }
-
   Matrix3 K() const override;  ///< Standard 3*3 calibration matrix
   Vector4 k() const;  ///< Radial distortion parameters (4 of them, 2 0)
 
@@ -113,8 +103,7 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
 
   /**
    * Convert a pixel coordinate to ideal coordinate xy
-   * @param p point in image coordinates
+   * @param pi point in image coordinates
-   * @param tol optional tolerance threshold value for iterative minimization
    * @param Dcal optional 2*3 Jacobian wrpt Cal3Bundler parameters
    * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
    * @return point in intrinsic coordinates
@@ -135,14 +124,14 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
   /// @name Manifold
   /// @{
 
-  /// return DOF, dimensionality of tangent space
+  /// Return DOF, dimensionality of tangent space
   size_t dim() const override { return Dim(); }
 
-  /// return DOF, dimensionality of tangent space
+  /// Return DOF, dimensionality of tangent space
   inline static size_t Dim() { return dimension; }
 
   /// Update calibration with tangent space delta
-  inline Cal3Bundler retract(const Vector& d) const {
+  Cal3Bundler retract(const Vector& d) const {
     return Cal3Bundler(fx_ + d(0), k1_ + d(1), k2_ + d(2), u0_, v0_);
   }
 

gtsam/geometry/Cal3f.cpp

@@ -0,0 +1,106 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file   Cal3f.cpp
+ * @brief  Implementation file for Cal3f class
+ * @author Frank Dellaert
+ * @date   October 2024
+ */
+
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/Vector.h>
+#include <gtsam/geometry/Cal3f.h>
+
+#include <iostream>
+
+namespace gtsam {
+
+/* ************************************************************************* */
+Cal3f::Cal3f(double f, double u0, double v0) : Cal3(f, f, 0.0, u0, v0) {}
+
+/* ************************************************************************* */
+std::ostream& operator<<(std::ostream& os, const Cal3f& cal) {
+  os << "f: " << cal.fx_ << ", px: " << cal.u0_ << ", py: " << cal.v0_;
+  return os;
+}
+
+/* ************************************************************************* */
+void Cal3f::print(const std::string& s) const {
+  if (!s.empty()) std::cout << s << " ";
+  std::cout << *this << std::endl;
+}
+
+/* ************************************************************************* */
+bool Cal3f::equals(const Cal3f& K, double tol) const {
+  return Cal3::equals(static_cast<const Cal3&>(K), tol);
+}
+
+/* ************************************************************************* */
+Vector1 Cal3f::vector() const {
+  Vector1 v;
+  v << fx_;
+  return v;
+}
+
+/* ************************************************************************* */
+Point2 Cal3f::uncalibrate(const Point2& p, OptionalJacobian<2, 1> Dcal,
+                          OptionalJacobian<2, 2> Dp) const {
+  assert(fx_ == fy_ && s_ == 0.0);
+  const double x = p.x(), y = p.y();
+  const double u = fx_ * x + u0_;
+  const double v = fx_ * y + v0_;
+
+  if (Dcal) {
+    Dcal->resize(2, 1);
+    (*Dcal) << x, y;
+  }
+
+  if (Dp) {
+    Dp->resize(2, 2);
+    (*Dp) << fx_, 0.0,  //
+        0.0, fx_;
+  }
+
+  return Point2(u, v);
+}
+
+/* ************************************************************************* */
+Point2 Cal3f::calibrate(const Point2& pi, OptionalJacobian<2, 1> Dcal,
+                        OptionalJacobian<2, 2> Dp) const {
+  assert(fx_ == fy_ && s_ == 0.0);
+  const double u = pi.x(), v = pi.y();
+  double delta_u = u - u0_, delta_v = v - v0_;
+  double inv_f = 1.0 / fx_;
+  Point2 point(inv_f * delta_u, inv_f * delta_v);
+
+  if (Dcal) {
+    Dcal->resize(2, 1);
+    (*Dcal) << -inv_f * point.x(), -inv_f * point.y();
+  }
+
+  if (Dp) {
+    Dp->resize(2, 2);
+    (*Dp) << inv_f, 0.0,  //
+        0.0, inv_f;
+  }
+
+  return point;
+}
+
+}  // namespace gtsam

gtsam/geometry/Cal3f.h

@@ -0,0 +1,141 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file   Cal3f.h
+ * @brief  Calibration model with a single focal length and zero skew.
+ * @author Frank Dellaert
+ */
+
+#pragma once
+
+#include <gtsam/geometry/Cal3.h>
+
+namespace gtsam {
+
+/**
+ * @brief Calibration model with a single focal length and zero skew.
+ * @ingroup geometry
+ * \nosubgrouping
+ */
+class GTSAM_EXPORT Cal3f : public Cal3 {
+ public:
+  enum { dimension = 1 };
+  using shared_ptr = std::shared_ptr<Cal3f>;
+
+  /// @name Constructors
+  /// @{
+
+  /// Default constructor
+  Cal3f() = default;
+
+  /**
+   * Constructor
+   * @param f  focal length
+   * @param u0 image center x-coordinate (considered a constant)
+   * @param v0 image center y-coordinate (considered a constant)
+   */
+  Cal3f(double f, double u0, double v0);
+
+  ~Cal3f() override = default;
+
+  /// @}
+  /// @name Testable
+  /// @{
+
+  /// Output stream operator
+  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const Cal3f& cal);
+
+  /// print with optional string
+  void print(const std::string& s = "") const override;
+
+  /// assert equality up to a tolerance
+  bool equals(const Cal3f& K, double tol = 1e-9) const;
+
+  /// @}
+  /// @name Standard Interface
+  /// @{
+
+  /// focal length
+  inline double f() const { return fx_; }
+
+  /// vectorized form (column-wise)
+  Vector1 vector() const;
+
+  /**
+   * @brief: convert intrinsic coordinates xy to image coordinates uv
+   * Version of uncalibrate with derivatives
+   * @param p point in intrinsic coordinates
+   * @param Dcal optional 2*1 Jacobian wrpt focal length
+   * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
+   * @return point in image coordinates
+   */
+  Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 1> Dcal = {},
+                     OptionalJacobian<2, 2> Dp = {}) const;
+
+  /**
+   * Convert a pixel coordinate to ideal coordinate xy
+   * @param pi point in image coordinates
+   * @param Dcal optional 2*1 Jacobian wrpt focal length
+   * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
+   * @return point in intrinsic coordinates
+   */
+  Point2 calibrate(const Point2& pi, OptionalJacobian<2, 1> Dcal = {},
+                   OptionalJacobian<2, 2> Dp = {}) const;
+
+  /// @}
+  /// @name Manifold
+  /// @{
+
+  /// Return DOF, dimensionality of tangent space
+  size_t dim() const override { return Dim(); }
+
+  /// Return DOF, dimensionality of tangent space
+  inline static size_t Dim() { return dimension; }
+
+  /// Update calibration with tangent space delta
+  Cal3f retract(const Vector& d) const { return Cal3f(fx_ + d(0), u0_, v0_); }
+
+  /// Calculate local coordinates to another calibration
+  Vector1 localCoordinates(const Cal3f& T2) const {
+    return Vector1(T2.fx_ - fx_);
+  }
+
+  /// @}
+
+ private:
+#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class Archive>
+  void serialize(Archive& ar, const unsigned int /*version*/) {
+    ar& BOOST_SERIALIZATION_NVP(fx_);
+    ar& BOOST_SERIALIZATION_NVP(u0_);
+    ar& BOOST_SERIALIZATION_NVP(v0_);
+  }
+#endif
+
+  /// @}
+};
+
+template <>
+struct traits<Cal3f> : public internal::Manifold<Cal3f> {};
+
+template <>
+struct traits<const Cal3f> : public internal::Manifold<Cal3f> {};
+
+}  // namespace gtsam

gtsam/geometry/geometry.i

@@ -644,8 +644,36 @@ class EssentialMatrix {
   double error(gtsam::Vector vA, gtsam::Vector vB);
 };
 
+#include <gtsam/geometry/Cal3.h>
+virtual class Cal3 {
+  // Standard Constructors
+  Cal3();
+  Cal3(double fx, double fy, double s, double u0, double v0);
+  Cal3(gtsam::Vector v);
+
+  // Testable
+  void print(string s = "Cal3") const;
+  bool equals(const gtsam::Cal3& rhs, double tol) const;
+
+  // Standard Interface
+  double fx() const;
+  double fy() const;
+  double aspectRatio() const;
+  double skew() const;
+  double px() const;
+  double py() const;
+  gtsam::Point2 principalPoint() const;
+  gtsam::Vector vector() const;
+  gtsam::Matrix K() const;
+  gtsam::Matrix inverse() const;
+
+  // Manifold
+  static size_t Dim();
+  size_t dim() const;
+};
+
 #include <gtsam/geometry/Cal3_S2.h>
-class Cal3_S2 {
+virtual class Cal3_S2 : gtsam::Cal3 {
   // Standard Constructors
   Cal3_S2();
   Cal3_S2(double fx, double fy, double s, double u0, double v0);
@@ -672,23 +700,12 @@ class Cal3_S2 {
                             Eigen::Ref<Eigen::MatrixXd> Dcal,
                             Eigen::Ref<Eigen::MatrixXd> Dp) const;
 
-  // Standard Interface
-  double fx() const;
-  double fy() const;
-  double skew() const;
-  double px() const;
-  double py() const;
-  gtsam::Point2 principalPoint() const;
-  gtsam::Vector vector() const;
-  gtsam::Matrix K() const;
-  gtsam::Matrix inverse() const;
-
   // enabling serialization functionality
   void serialize() const;
 };
 
 #include <gtsam/geometry/Cal3DS2_Base.h>
-virtual class Cal3DS2_Base {
+virtual class Cal3DS2_Base : gtsam::Cal3 {
   // Standard Constructors
   Cal3DS2_Base();
 
@@ -696,16 +713,8 @@ virtual class Cal3DS2_Base {
   void print(string s = "") const;
 
   // Standard Interface
-  double fx() const;
-  double fy() const;
-  double skew() const;
-  double px() const;
-  double py() const;
   double k1() const;
   double k2() const;
-  gtsam::Matrix K() const;
-  gtsam::Vector k() const;
-  gtsam::Vector vector() const;
 
   // Action on Point2
   gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
@@ -785,7 +794,7 @@ virtual class Cal3Unified : gtsam::Cal3DS2_Base {
 };
 
 #include <gtsam/geometry/Cal3Fisheye.h>
-class Cal3Fisheye {
+virtual class Cal3Fisheye : gtsam::Cal3 {
   // Standard Constructors
   Cal3Fisheye();
   Cal3Fisheye(double fx, double fy, double s, double u0, double v0, double k1,
@@ -797,8 +806,6 @@ class Cal3Fisheye {
   bool equals(const gtsam::Cal3Fisheye& rhs, double tol) const;
 
   // Manifold
-  static size_t Dim();
-  size_t dim() const;
   gtsam::Cal3Fisheye retract(gtsam::Vector v) const;
   gtsam::Vector localCoordinates(const gtsam::Cal3Fisheye& c) const;
 
@@ -813,35 +820,23 @@ class Cal3Fisheye {
                             Eigen::Ref<Eigen::MatrixXd> Dp) const;
 
   // Standard Interface
-  double fx() const;
-  double fy() const;
-  double skew() const;
   double k1() const;
   double k2() const;
   double k3() const;
   double k4() const;
-  double px() const;
-  double py() const;
-  gtsam::Point2 principalPoint() const;
-  gtsam::Vector vector() const;
-  gtsam::Vector k() const;
-  gtsam::Matrix K() const;
-  gtsam::Matrix inverse() const;
 
   // enabling serialization functionality
   void serialize() const;
 };
 
 #include <gtsam/geometry/Cal3_S2Stereo.h>
-class Cal3_S2Stereo {
+virtual class Cal3_S2Stereo   : gtsam::Cal3{
   // Standard Constructors
   Cal3_S2Stereo();
   Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b);
   Cal3_S2Stereo(gtsam::Vector v);
 
   // Manifold
-  static size_t Dim();
-  size_t dim() const;
   gtsam::Cal3_S2Stereo retract(gtsam::Vector v) const;
   gtsam::Vector localCoordinates(const gtsam::Cal3_S2Stereo& c) const;
 
@@ -850,35 +845,23 @@ class Cal3_S2Stereo {
   bool equals(const gtsam::Cal3_S2Stereo& K, double tol) const;
 
   // Standard Interface
-  double fx() const;
-  double fy() const;
-  double skew() const;
-  double px() const;
-  double py() const;
-  gtsam::Matrix K() const;
-  gtsam::Point2 principalPoint() const;
   double baseline() const;
   gtsam::Vector6 vector() const;
-  gtsam::Matrix inverse() const;
 };
 
 #include <gtsam/geometry/Cal3Bundler.h>
-class Cal3Bundler {
+virtual class Cal3f : gtsam::Cal3 {
   // Standard Constructors
-  Cal3Bundler();
+  Cal3f();
-  Cal3Bundler(double fx, double k1, double k2, double u0, double v0);
+  Cal3f(double fx, double u0, double v0);
-  Cal3Bundler(double fx, double k1, double k2, double u0, double v0,
-              double tol);
 
   // Testable
   void print(string s = "") const;
-  bool equals(const gtsam::Cal3Bundler& rhs, double tol) const;
+  bool equals(const gtsam::Cal3f& rhs, double tol) const;
 
   // Manifold
-  static size_t Dim();
+  gtsam::Cal3f retract(gtsam::Vector v) const;
-  size_t dim() const;
+  gtsam::Vector localCoordinates(const gtsam::Cal3f& c) const;
-  gtsam::Cal3Bundler retract(gtsam::Vector v) const;
-  gtsam::Vector localCoordinates(const gtsam::Cal3Bundler& c) const;
 
   // Action on Point2
   gtsam::Point2 calibrate(const gtsam::Point2& p) const;
@@ -891,15 +874,32 @@ class Cal3Bundler {
                             Eigen::Ref<Eigen::MatrixXd> Dp) const;
 
   // Standard Interface
-  double fx() const;
+  double f() const;
-  double fy() const;
+
+  // enabling serialization functionality
+  void serialize() const;
+};
+
+
+#include <gtsam/geometry/Cal3Bundler.h>
+virtual class Cal3Bundler : gtsam::Cal3f {
+  // Standard Constructors
+  Cal3Bundler();
+  Cal3Bundler(double fx, double k1, double k2, double u0, double v0);
+  Cal3Bundler(double fx, double k1, double k2, double u0, double v0,
+              double tol);
+
+  // Testable
+  void print(string s = "") const;
+  bool equals(const gtsam::Cal3Bundler& rhs, double tol) const;
+
+  // Manifold
+  gtsam::Cal3Bundler retract(gtsam::Vector v) const;
+  gtsam::Vector localCoordinates(const gtsam::Cal3Bundler& c) const;
+
+  // Standard Interface
   double k1() const;
   double k2() const;
-  double px() const;
-  double py() const;
-  gtsam::Vector vector() const;
-  gtsam::Vector k() const;
-  gtsam::Matrix K() const;
 
   // enabling serialization functionality
   void serialize() const;
@@ -1071,6 +1071,7 @@ typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
 typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
 typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
 typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
+typedef gtsam::PinholeCamera<gtsam::Cal3f> PinholeCameraCal3f;
 typedef gtsam::PinholeCamera<gtsam::Cal3Fisheye> PinholeCameraCal3Fisheye;
 
 #include <gtsam/geometry/PinholePose.h>

gtsam/geometry/tests/testCal3Bundler.cpp

@@ -29,7 +29,7 @@ static Cal3Bundler K(500, 1e-3, 1e-3, 1000, 2000);
 static Point2 p(2, 3);
 
 /* ************************************************************************* */
-TEST(Cal3Bundler, vector) {
+TEST(Cal3Bundler, Vector) {
   Cal3Bundler K;
   Vector expected(3);
   expected << 1, 0, 0;
@@ -37,16 +37,19 @@ TEST(Cal3Bundler, vector) {
 }
 
 /* ************************************************************************* */
-TEST(Cal3Bundler, uncalibrate) {
+TEST(Cal3Bundler, Uncalibrate) {
   Vector v = K.vector();
   double r = p.x() * p.x() + p.y() * p.y();
-  double g = v[0] * (1 + v[1] * r + v[2] * r * r);
+  double distortion = 1.0 + v[1] * r + v[2] * r * r;
-  Point2 expected(1000 + g * p.x(), 2000 + g * p.y());
+  double u = K.px() + v[0] * distortion * p.x();
+  double v_coord = K.py() + v[0] * distortion * p.y();
+  Point2 expected(u, v_coord);
   Point2 actual = K.uncalibrate(p);
   CHECK(assert_equal(expected, actual));
 }
 
-TEST(Cal3Bundler, calibrate) {
+/* ************************************************************************* */
+TEST(Cal3Bundler, Calibrate) {
   Point2 pn(0.5, 0.5);
   Point2 pi = K.uncalibrate(pn);
   Point2 pn_hat = K.calibrate(pi);
@@ -63,11 +66,11 @@ Point2 calibrate_(const Cal3Bundler& k, const Point2& pt) {
 }
 
 /* ************************************************************************* */
-TEST(Cal3Bundler, DuncalibrateDefault) {
+TEST(Cal3Bundler, DUncalibrateDefault) {
   Cal3Bundler trueK(1, 0, 0);
   Matrix Dcal, Dp;
   Point2 actual = trueK.uncalibrate(p, Dcal, Dp);
-  Point2 expected = p;
+  Point2 expected(p);  // Since K is identity, uncalibrate should return p
   CHECK(assert_equal(expected, actual, 1e-7));
   Matrix numerical1 = numericalDerivative21(uncalibrate_, trueK, p);
   Matrix numerical2 = numericalDerivative22(uncalibrate_, trueK, p);
@@ -76,7 +79,7 @@ TEST(Cal3Bundler, DuncalibrateDefault) {
 }
 
 /* ************************************************************************* */
-TEST(Cal3Bundler, DcalibrateDefault) {
+TEST(Cal3Bundler, DCalibrateDefault) {
   Cal3Bundler trueK(1, 0, 0);
   Matrix Dcal, Dp;
   Point2 pn(0.5, 0.5);
@@ -90,11 +93,11 @@ TEST(Cal3Bundler, DcalibrateDefault) {
 }
 
 /* ************************************************************************* */
-TEST(Cal3Bundler, DuncalibratePrincipalPoint) {
+TEST(Cal3Bundler, DUncalibratePrincipalPoint) {
   Cal3Bundler K(5, 0, 0, 2, 2);
   Matrix Dcal, Dp;
   Point2 actual = K.uncalibrate(p, Dcal, Dp);
-  Point2 expected(12, 17);
+  Point2 expected(2.0 + 5.0 * p.x(), 2.0 + 5.0 * p.y());
   CHECK(assert_equal(expected, actual, 1e-7));
   Matrix numerical1 = numericalDerivative21(uncalibrate_, K, p);
   Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p);
@@ -103,7 +106,7 @@ TEST(Cal3Bundler, DuncalibratePrincipalPoint) {
 }
 
 /* ************************************************************************* */
-TEST(Cal3Bundler, DcalibratePrincipalPoint) {
+TEST(Cal3Bundler, DCalibratePrincipalPoint) {
   Cal3Bundler K(2, 0, 0, 2, 2);
   Matrix Dcal, Dp;
   Point2 pn(0.5, 0.5);
@@ -117,11 +120,18 @@ TEST(Cal3Bundler, DcalibratePrincipalPoint) {
 }
 
 /* ************************************************************************* */
-TEST(Cal3Bundler, Duncalibrate) {
+TEST(Cal3Bundler, DUncalibrate) {
   Matrix Dcal, Dp;
   Point2 actual = K.uncalibrate(p, Dcal, Dp);
-  Point2 expected(2182, 3773);
+  // Compute expected value manually
+  Vector v = K.vector();
+  double r2 = p.x() * p.x() + p.y() * p.y();
+  double distortion = 1.0 + v[1] * r2 + v[2] * r2 * r2;
+  Point2 expected(
+      K.px() + v[0] * distortion * p.x(),
+      K.py() + v[0] * distortion * p.y());
   CHECK(assert_equal(expected, actual, 1e-7));
+
   Matrix numerical1 = numericalDerivative21(uncalibrate_, K, p);
   Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p);
   CHECK(assert_equal(numerical1, Dcal, 1e-7));
@@ -129,7 +139,7 @@ TEST(Cal3Bundler, Duncalibrate) {
 }
 
 /* ************************************************************************* */
-TEST(Cal3Bundler, Dcalibrate) {
+TEST(Cal3Bundler, DCalibrate) {
   Matrix Dcal, Dp;
   Point2 pn(0.5, 0.5);
   Point2 pi = K.uncalibrate(pn);
@@ -145,7 +155,7 @@ TEST(Cal3Bundler, Dcalibrate) {
 TEST(Cal3Bundler, assert_equal) { CHECK(assert_equal(K, K, 1e-7)); }
 
 /* ************************************************************************* */
-TEST(Cal3Bundler, retract) {
+TEST(Cal3Bundler, Retract) {
   Cal3Bundler expected(510, 2e-3, 2e-3, 1000, 2000);
   EXPECT_LONGS_EQUAL(3, expected.dim());
 

gtsam/geometry/tests/testCal3f.cpp

@@ -0,0 +1,132 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+/**
+ * @file  testCal3F.cpp
+ * @brief Unit tests for the Cal3f calibration model.
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/TestableAssertions.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/geometry/Cal3f.h>
+
+using namespace gtsam;
+
+GTSAM_CONCEPT_TESTABLE_INST(Cal3f)
+GTSAM_CONCEPT_MANIFOLD_INST(Cal3f)
+
+static Cal3f K(500.0, 1000.0, 2000.0);  // f = 500, u0 = 1000, v0 = 2000
+static Point2 p(2.0, 3.0);
+
+/* ************************************************************************* */
+TEST(Cal3f, Vector) {
+  Cal3f K(1.0, 0.0, 0.0);
+  Vector expected(1);
+  expected << 1.0;
+  CHECK(assert_equal(expected, K.vector()));
+}
+
+/* ************************************************************************* */
+TEST(Cal3f, Uncalibrate) {
+  // Expected output: apply the intrinsic calibration matrix to point p
+  Matrix3 K_matrix = K.K();
+  Vector3 p_homogeneous(p.x(), p.y(), 1.0);
+  Vector3 expected_homogeneous = K_matrix * p_homogeneous;
+  Point2 expected(expected_homogeneous.x() / expected_homogeneous.z(),
+                  expected_homogeneous.y() / expected_homogeneous.z());
+
+  Point2 actual = K.uncalibrate(p);
+  CHECK(assert_equal(expected, actual));
+}
+
+/* ************************************************************************* */
+TEST(Cal3f, Calibrate) {
+  Point2 pi = K.uncalibrate(p);
+  Point2 pn = K.calibrate(pi);
+  CHECK(traits<Point2>::Equals(p, pn, 1e-9));
+}
+
+/* ************************************************************************* */
+Point2 uncalibrate_(const Cal3f& k, const Point2& pt) {
+  return k.uncalibrate(pt);
+}
+
+Point2 calibrate_(const Cal3f& k, const Point2& pt) { return k.calibrate(pt); }
+
+/* ************************************************************************* */
+TEST(Cal3f, DUncalibrate) {
+  Cal3f K(500.0, 1000.0, 2000.0);
+  Matrix Dcal, Dp;
+  Point2 actual = K.uncalibrate(p, Dcal, Dp);
+
+  // Expected value computed manually
+  Point2 expected = Point2(K.px() + K.fx() * p.x(), K.py() + K.fx() * p.y());
+  CHECK(assert_equal(expected, actual, 1e-9));
+
+  // Compute numerical derivatives
+  Matrix numerical1 = numericalDerivative21(uncalibrate_, K, p);
+  Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p);
+  CHECK(assert_equal(numerical1, Dcal, 1e-6));
+  CHECK(assert_equal(numerical2, Dp, 1e-6));
+}
+
+/* ************************************************************************* */
+TEST(Cal3f, DCalibrate) {
+  Point2 pi = K.uncalibrate(p);
+  Matrix Dcal, Dp;
+  Point2 actual = K.calibrate(pi, Dcal, Dp);
+  CHECK(assert_equal(p, actual, 1e-9));
+
+  // Compute numerical derivatives
+  Matrix numerical1 = numericalDerivative21(calibrate_, K, pi);
+  Matrix numerical2 = numericalDerivative22(calibrate_, K, pi);
+  CHECK(assert_equal(numerical1, Dcal, 1e-6));
+  CHECK(assert_equal(numerical2, Dp, 1e-6));
+}
+
+/* ************************************************************************* */
+TEST(Cal3f, Manifold) {
+  Cal3f K1(500.0, 1000.0, 2000.0);
+  Vector1 delta;
+  delta << 10.0;  // Increment focal length by 10
+
+  Cal3f K2 = K1.retract(delta);
+  CHECK(assert_equal(510.0, K2.fx(), 1e-9));
+  CHECK(assert_equal(K1.px(), K2.px(), 1e-9));
+  CHECK(assert_equal(K1.py(), K2.py(), 1e-9));
+
+  Vector1 delta_computed = K1.localCoordinates(K2);
+  CHECK(assert_equal(delta, delta_computed, 1e-9));
+}
+
+/* ************************************************************************* */
+TEST(Cal3f, Assert_equal) {
+  CHECK(assert_equal(K, K, 1e-9));
+  Cal3f K2(500.0, 1000.0, 2000.0);
+  CHECK(assert_equal(K, K2, 1e-9));
+}
+
+/* ************************************************************************* */
+TEST(Cal3f, Print) {
+  Cal3f cal(500.0, 1000.0, 2000.0);
+  std::stringstream os;
+  os << "f: " << cal.fx() << ", px: " << cal.px() << ", py: " << cal.py();
+
+  EXPECT(assert_stdout_equal(os.str(), cal));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */

gtsam/inference/EdgeKey.h

@@ -46,7 +46,10 @@ class GTSAM_EXPORT EdgeKey {
   /// @{
 
   /// Cast to Key
-  operator Key() const { return ((std::uint64_t)i_ << 32) | j_; }
+  Key key() const { return ((std::uint64_t)i_ << 32) | j_; }
+
+  /// Cast to Key
+  operator Key() const { return key(); }
 
   /// Retrieve high 32 bits
   inline std::uint32_t i() const { return i_; }

gtsam/inference/inference.i

@@ -96,6 +96,19 @@ unsigned char mrsymbolChr(size_t key);
 unsigned char mrsymbolLabel(size_t key);
 size_t mrsymbolIndex(size_t key);
 
+#include <gtsam/inference/EdgeKey.h>
+class EdgeKey {
+  EdgeKey(std::uint32_t i, std::uint32_t j);
+  EdgeKey(size_t key);
+  EdgeKey(const gtsam::EdgeKey& key);
+
+  std::uint32_t i() const;
+  std::uint32_t j() const;
+  size_t key() const;
+
+  void print(string s = "") const;
+};
+
 #include <gtsam/inference/Ordering.h>
 class Ordering {
   /// Type of ordering to use

gtsam/nonlinear/nonlinear.i

@@ -75,13 +75,20 @@ class NonlinearFactorGraph {
                  gtsam::Pose2,
                  gtsam::Pose3,
                  gtsam::Cal3_S2,
+                 gtsam::Cal3f,
+                 gtsam::Cal3Bundler,
                  gtsam::Cal3Fisheye,
                  gtsam::Cal3Unified,
                  gtsam::CalibratedCamera,
+                 gtsam::EssentialMatrix,
+                 gtsam::FundamentalMatrix,
+                 gtsam::SimpleFundamentalMatrix,
                  gtsam::PinholeCamera<gtsam::Cal3_S2>,
+                 gtsam::PinholeCamera<gtsam::Cal3f>,
                  gtsam::PinholeCamera<gtsam::Cal3Bundler>,
                  gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
                  gtsam::PinholeCamera<gtsam::Cal3Unified>,
+                 gtsam::PinholeCamera<gtsam::CalibratedCamera>,
                  gtsam::imuBias::ConstantBias}>
   void addPrior(size_t key, const T& prior,
                 const gtsam::noiseModel::Base* noiseModel);
@@ -537,9 +544,8 @@ class ISAM2 {
   gtsam::Values calculateEstimate() const;
   template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
                      gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
-                     gtsam::Cal3Bundler, gtsam::EssentialMatrix,
+                     gtsam::Cal3f, gtsam::Cal3Bundler, 
-                     gtsam::Cal3Bundler, gtsam::FundamentalMatrix,
+                     gtsam::EssentialMatrix, gtsam::FundamentalMatrix, gtsam::SimpleFundamentalMatrix,
-                     gtsam::Cal3Bundler, gtsam::SimpleFundamentalMatrix,
                      gtsam::PinholeCamera<gtsam::Cal3_S2>,
                      gtsam::PinholeCamera<gtsam::Cal3Bundler>,
                      gtsam::PinholeCamera<gtsam::Cal3Fisheye>,

gtsam/nonlinear/values.i

@@ -4,13 +4,14 @@
 
 namespace gtsam {
 
+#include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/geometry/Cal3DS2.h>
+#include <gtsam/geometry/Cal3f.h>
 #include <gtsam/geometry/Cal3Fisheye.h>
 #include <gtsam/geometry/Cal3Unified.h>
-#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>
@@ -67,7 +68,7 @@ class Values {
   // gtsam::Value at(size_t j) const;
 
   // The order is important: gtsam::Vector has to precede Point2/Point3 so `atVector`
-  // can work for those fixed-size vectors.
+  // can work for those fixed-size vectors. Same apparently for Cal3Bundler and Cal3f.
   void insert(size_t j, gtsam::Vector vector);
   void insert(size_t j, gtsam::Matrix matrix);
   void insert(size_t j, const gtsam::Point2& point2);
@@ -80,18 +81,25 @@ class Values {
   void insert(size_t j, const gtsam::Rot3& rot3);
   void insert(size_t j, const gtsam::Pose3& pose3);
   void insert(size_t j, const gtsam::Unit3& unit3);
+  void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler);
+  void insert(size_t j, const gtsam::Cal3f& cal3f);
   void insert(size_t j, const gtsam::Cal3_S2& cal3_s2);
   void insert(size_t j, const gtsam::Cal3DS2& cal3ds2);
-  void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler);
   void insert(size_t j, const gtsam::Cal3Fisheye& cal3fisheye);
   void insert(size_t j, const gtsam::Cal3Unified& cal3unified);
-  void insert(size_t j, const gtsam::EssentialMatrix& essential_matrix);
+  void insert(size_t j, const gtsam::EssentialMatrix& E);
-  void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3_S2>& camera);
+  void insert(size_t j, const gtsam::FundamentalMatrix& F);
+  void insert(size_t j, const gtsam::SimpleFundamentalMatrix& F);
   void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
+  void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3f>& camera);
+  void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3_S2>& camera);
+  void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3DS2>& camera);
   void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Fisheye>& camera);
   void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Unified>& camera);
-  void insert(size_t j, const gtsam::PinholePose<gtsam::Cal3_S2>& camera);
   void insert(size_t j, const gtsam::PinholePose<gtsam::Cal3Bundler>& camera);
+  void insert(size_t j, const gtsam::PinholePose<gtsam::Cal3f>& camera);
+  void insert(size_t j, const gtsam::PinholePose<gtsam::Cal3_S2>& camera);
+  void insert(size_t j, const gtsam::PinholePose<gtsam::Cal3DS2>& camera);
   void insert(size_t j, const gtsam::PinholePose<gtsam::Cal3Fisheye>& camera);
   void insert(size_t j, const gtsam::PinholePose<gtsam::Cal3Unified>& camera);
   void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
@@ -115,18 +123,25 @@ class Values {
   void update(size_t j, const gtsam::Rot3& rot3);
   void update(size_t j, const gtsam::Pose3& pose3);
   void update(size_t j, const gtsam::Unit3& unit3);
+  void update(size_t j, const gtsam::Cal3Bundler& cal3bundler);
+  void update(size_t j, const gtsam::Cal3f& cal3f);
   void update(size_t j, const gtsam::Cal3_S2& cal3_s2);
   void update(size_t j, const gtsam::Cal3DS2& cal3ds2);
-  void update(size_t j, const gtsam::Cal3Bundler& cal3bundler);
   void update(size_t j, const gtsam::Cal3Fisheye& cal3fisheye);
   void update(size_t j, const gtsam::Cal3Unified& cal3unified);
-  void update(size_t j, const gtsam::EssentialMatrix& essential_matrix);
+  void update(size_t j, const gtsam::EssentialMatrix& E);
-  void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3_S2>& camera);
+  void update(size_t j, const gtsam::FundamentalMatrix& F);
+  void update(size_t j, const gtsam::SimpleFundamentalMatrix& F);
   void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
+  void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3f>& camera);
+  void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3_S2>& camera);
+  void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3DS2>& camera);
   void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Fisheye>& camera);
   void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Unified>& camera);
-  void update(size_t j, const gtsam::PinholePose<gtsam::Cal3_S2>& camera);
   void update(size_t j, const gtsam::PinholePose<gtsam::Cal3Bundler>& camera);
+  void update(size_t j, const gtsam::PinholePose<gtsam::Cal3f>& camera);
+  void update(size_t j, const gtsam::PinholePose<gtsam::Cal3_S2>& camera);
+  void update(size_t j, const gtsam::PinholePose<gtsam::Cal3DS2>& camera);
   void update(size_t j, const gtsam::PinholePose<gtsam::Cal3Fisheye>& camera);
   void update(size_t j, const gtsam::PinholePose<gtsam::Cal3Unified>& camera);
   void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
@@ -147,31 +162,28 @@ class Values {
   void insert_or_assign(size_t j, const gtsam::Rot3& rot3);
   void insert_or_assign(size_t j, const gtsam::Pose3& pose3);
   void insert_or_assign(size_t j, const gtsam::Unit3& unit3);
+  void insert_or_assign(size_t j, const gtsam::Cal3Bundler& cal3bundler);
+  void insert_or_assign(size_t j, const gtsam::Cal3f& cal3f);
   void insert_or_assign(size_t j, const gtsam::Cal3_S2& cal3_s2);
   void insert_or_assign(size_t j, const gtsam::Cal3DS2& cal3ds2);
-  void insert_or_assign(size_t j, const gtsam::Cal3Bundler& cal3bundler);
   void insert_or_assign(size_t j, const gtsam::Cal3Fisheye& cal3fisheye);
   void insert_or_assign(size_t j, const gtsam::Cal3Unified& cal3unified);
-  void insert_or_assign(size_t j,
+  void insert_or_assign(size_t j, const gtsam::EssentialMatrix& E);
-                        const gtsam::EssentialMatrix& essential_matrix);
+  void insert_or_assign(size_t j, const gtsam::FundamentalMatrix& F);
-  void insert_or_assign(size_t j,
+  void insert_or_assign(size_t j, const gtsam::SimpleFundamentalMatrix& F);
-                        const gtsam::PinholeCamera<gtsam::Cal3_S2>& camera);
+  void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
-  void insert_or_assign(size_t j,
+  void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3f>& camera);
-                        const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
+  void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3_S2>& camera);
-  void insert_or_assign(size_t j,
+  void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3DS2>& camera);
-                        const gtsam::PinholeCamera<gtsam::Cal3Fisheye>& camera);
+  void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Fisheye>& camera);
-  void insert_or_assign(size_t j,
+  void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Unified>& camera);
-                        const gtsam::PinholeCamera<gtsam::Cal3Unified>& camera);
+  void insert_or_assign(size_t j, const gtsam::PinholePose<gtsam::Cal3Bundler>& camera);
-  void insert_or_assign(size_t j,
+  void insert_or_assign(size_t j, const gtsam::PinholePose<gtsam::Cal3f>& camera);
-                        const gtsam::PinholePose<gtsam::Cal3_S2>& camera);
+  void insert_or_assign(size_t j, const gtsam::PinholePose<gtsam::Cal3_S2>& camera);
-  void insert_or_assign(size_t j,
+  void insert_or_assign(size_t j, const gtsam::PinholePose<gtsam::Cal3DS2>& camera);
-                        const gtsam::PinholePose<gtsam::Cal3Bundler>& camera);
+  void insert_or_assign(size_t j, const gtsam::PinholePose<gtsam::Cal3Fisheye>& camera);
-  void insert_or_assign(size_t j,
+  void insert_or_assign(size_t j, const gtsam::PinholePose<gtsam::Cal3Unified>& camera);
-                        const gtsam::PinholePose<gtsam::Cal3Fisheye>& camera);
+  void insert_or_assign(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
-  void insert_or_assign(size_t j,
-                        const gtsam::PinholePose<gtsam::Cal3Unified>& camera);
-  void insert_or_assign(size_t j,
-                        const gtsam::imuBias::ConstantBias& constant_bias);
   void insert_or_assign(size_t j, const gtsam::NavState& nav_state);
   void insert_or_assign(size_t j, double c);
 
@@ -185,18 +197,25 @@ class Values {
                  gtsam::Rot3,
                  gtsam::Pose3,
                  gtsam::Unit3,
+                 gtsam::Cal3Bundler,
+                 gtsam::Cal3f, 
                  gtsam::Cal3_S2,
                  gtsam::Cal3DS2,
-                 gtsam::Cal3Bundler,
                  gtsam::Cal3Fisheye,
                  gtsam::Cal3Unified,
-                 gtsam::EssentialMatrix,
+                 gtsam::EssentialMatrix, 
-                 gtsam::PinholeCamera<gtsam::Cal3_S2>,
+                 gtsam::FundamentalMatrix, 
+                 gtsam::SimpleFundamentalMatrix,
                  gtsam::PinholeCamera<gtsam::Cal3Bundler>,
+                 gtsam::PinholeCamera<gtsam::Cal3f>,
+                 gtsam::PinholeCamera<gtsam::Cal3_S2>,
+                 gtsam::PinholeCamera<gtsam::Cal3DS2>,
                  gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
                  gtsam::PinholeCamera<gtsam::Cal3Unified>,
-                 gtsam::PinholePose<gtsam::Cal3_S2>,
                  gtsam::PinholePose<gtsam::Cal3Bundler>,
+                 gtsam::PinholePose<gtsam::Cal3f>,
+                 gtsam::PinholePose<gtsam::Cal3_S2>,
+                 gtsam::PinholePose<gtsam::Cal3DS2>,
                  gtsam::PinholePose<gtsam::Cal3Fisheye>,
                  gtsam::PinholePose<gtsam::Cal3Unified>,
                  gtsam::imuBias::ConstantBias,

gtsam/sfm/TransferFactor.h

@@ -16,12 +16,66 @@
 #pragma once
 
 #include <gtsam/base/numericalDerivative.h>
+#include <gtsam/geometry/EssentialMatrix.h>
 #include <gtsam/geometry/FundamentalMatrix.h>
 #include <gtsam/inference/EdgeKey.h>
+#include <gtsam/inference/Symbol.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 
+#include <cstdint>
+
 namespace gtsam {
 
+/**
+ * Base class that holds the EdgeKeys and provides the getMatrices method.
+ */
+template <typename F>
+class TransferEdges {
+ protected:
+  EdgeKey edge1_, edge2_;  ///< The two EdgeKeys.
+  uint32_t c_;             ///< The transfer target
+
+ public:
+  TransferEdges(EdgeKey edge1, EdgeKey edge2)
+      : edge1_(edge1), edge2_(edge2), c_(ViewC(edge1, edge2)) {}
+
+  /// Returns the view A index based on the EdgeKeys
+  static size_t ViewA(const EdgeKey& edge1, const EdgeKey& edge2) {
+    size_t c = ViewC(edge1, edge2);
+    return (edge1.i() == c) ? edge1.j() : edge1.i();
+  }
+
+  /// Returns the view B index based on the EdgeKeys
+  static size_t ViewB(const EdgeKey& edge1, const EdgeKey& edge2) {
+    size_t c = ViewC(edge1, edge2);
+    return (edge2.i() == c) ? edge2.j() : edge2.i();
+  }
+
+  /// Returns the view C index based on the EdgeKeys
+  static size_t ViewC(const EdgeKey& edge1, const EdgeKey& edge2) {
+    if (edge1.i() == edge2.i() || edge1.i() == edge2.j())
+      return edge1.i();
+    else if (edge1.j() == edge2.i() || edge1.j() == edge2.j())
+      return edge1.j();
+    else
+      throw std::runtime_error(
+          "EssentialTransferFactorK: No common key in edge keys.");
+  }
+
+  /// Create Matrix3 objects based on EdgeKey configurations.
+  std::pair<Matrix3, Matrix3> getMatrices(const F& F1, const F& F2) const {
+    // Determine whether to transpose F1
+    const Matrix3 Fca =
+        edge1_.i() == c_ ? F1.matrix() : F1.matrix().transpose();
+
+    // Determine whether to transpose F2
+    const Matrix3 Fcb =
+        edge2_.i() == c_ ? F2.matrix() : F2.matrix().transpose();
+
+    return {Fca, Fcb};
+  }
+};
+
 /**
  * Binary factor in the context of Structure from Motion (SfM).
  * It is used to transfer transfer corresponding points from two views to a
@@ -30,89 +84,219 @@ namespace gtsam {
  * the target view. Jacobians are done using numerical differentiation.
  */
 template <typename F>
-class TransferFactor : public NoiseModelFactorN<F, F> {
+class TransferFactor : public NoiseModelFactorN<F, F>, public TransferEdges<F> {
-  EdgeKey key1_, key2_;  ///< the two EdgeKeys
+ public:
-  std::vector<std::tuple<Point2, Point2, Point2>>
+  using Base = NoiseModelFactorN<F, F>;
-      triplets_;  ///< Point triplets
+  using Triplet = std::tuple<Point2, Point2, Point2>;
+
+ protected:
+  std::vector<Triplet> triplets_;  ///< Point triplets.
 
  public:
-  /**
-   * @brief Constructor for a single triplet of points
-   *
-   * @note: batching all points for the same transfer will be much faster.
-   *
-   * @param key1 First EdgeKey specifying F1: (a, c) or (c, a).
-   * @param key2 Second EdgeKey specifying F2: (b, c) or (c, b).
-   * @param pa The point in the first view (a).
-   * @param pb The point in the second view (b).
-   * @param pc The point in the third (and transfer target) view (c).
-   * @param model An optional SharedNoiseModel that defines the noise model
-   *              for this factor. Defaults to nullptr.
-   */
-  TransferFactor(EdgeKey key1, EdgeKey key2, const Point2& pa, const Point2& pb,
-                 const Point2& pc, const SharedNoiseModel& model = nullptr)
-      : NoiseModelFactorN<F, F>(model, key1, key2),
-        key1_(key1),
-        key2_(key2),
-        triplets_({std::make_tuple(pa, pb, pc)}) {}
-
   /**
    * @brief Constructor that accepts a vector of point triplets.
    *
-   * @param key1 First EdgeKey specifying F1: (a, c) or (c, a).
+   * @param edge1 First EdgeKey specifying F1: (a, c) or (c, a).
-   * @param key2 Second EdgeKey specifying F2: (b, c) or (c, b).
+   * @param edge2 Second EdgeKey specifying F2: (b, c) or (c, b).
    * @param triplets A vector of triplets containing (pa, pb, pc).
    * @param model An optional SharedNoiseModel that defines the noise model
    *              for this factor. Defaults to nullptr.
    */
-  TransferFactor(
+  TransferFactor(EdgeKey edge1, EdgeKey edge2,
-      EdgeKey key1, EdgeKey key2,
+                 const std::vector<Triplet>& triplets,
-      const std::vector<std::tuple<Point2, Point2, Point2>>& triplets,
+                 const SharedNoiseModel& model = nullptr)
-      const SharedNoiseModel& model = nullptr)
+      : Base(model, edge1, edge2),
-      : NoiseModelFactorN<F, F>(model, key1, key2),
+        TransferEdges<F>(edge1, edge2),
-        key1_(key1),
-        key2_(key2),
         triplets_(triplets) {}
 
-  // Create Matrix3 objects based on EdgeKey configurations
+  /// Vector of errors returns 2*N vector.
-  std::pair<Matrix3, Matrix3> getMatrices(const F& F1, const F& F2) const {
-    // Fill Fca and Fcb based on EdgeKey configurations
-    if (key1_.i() == key2_.i()) {
-      return {F1.matrix(), F2.matrix()};
-    } else if (key1_.i() == key2_.j()) {
-      return {F1.matrix(), F2.matrix().transpose()};
-    } else if (key1_.j() == key2_.i()) {
-      return {F1.matrix().transpose(), F2.matrix()};
-    } else if (key1_.j() == key2_.j()) {
-      return {F1.matrix().transpose(), F2.matrix().transpose()};
-    } else {
-      throw std::runtime_error(
-          "TransferFactor: invalid EdgeKey configuration.");
-    }
-  }
-
-  /// vector of errors returns 2*N vector
   Vector evaluateError(const F& F1, const F& F2,
                        OptionalMatrixType H1 = nullptr,
                        OptionalMatrixType H2 = nullptr) const override {
-    std::function<Vector(const F&, const F&)> transfer = [&](const F& F1,
+    std::function<Vector(const F&, const F&)> errorFunction = [&](const F& f1,
-                                                             const F& F2) {
+                                                                  const F& f2) {
       Vector errors(2 * triplets_.size());
       size_t idx = 0;
-      auto [Fca, Fcb] = getMatrices(F1, F2);
+      auto [Fca, Fcb] = this->getMatrices(f1, f2);
-      for (const auto& tuple : triplets_) {
+      for (const auto& [pa, pb, pc] : triplets_) {
-        const auto& [pa, pb, pc] = tuple;
+        errors.segment<2>(idx) = EpipolarTransfer(Fca, pa, Fcb, pb) - pc;
-        Point2 transferredPoint = EpipolarTransfer(Fca, pa, Fcb, pb);
-        Vector2 error = transferredPoint - pc;
-        errors.segment<2>(idx) = error;
         idx += 2;
       }
       return errors;
     };
-    if (H1) *H1 = numericalDerivative21<Vector, F, F>(transfer, F1, F2);
+
-    if (H2) *H2 = numericalDerivative22<Vector, F, F>(transfer, F1, F2);
+    if (H1) *H1 = numericalDerivative21(errorFunction, F1, F2);
-    return transfer(F1, F2);
+    if (H2) *H2 = numericalDerivative22(errorFunction, F1, F2);
+    return errorFunction(F1, F2);
   }
 };
 
+/**
+ * @class EssentialTransferFactor
+ * @brief Transfers points between views using essential matrices with a shared
+ * calibration.
+ *
+ * This factor is templated on the calibration class K and extends
+ * the TransferFactor for EssentialMatrices. It involves two essential matrices
+ * and a shared calibration object (K). The evaluateError function calibrates
+ * the image points, calls the base class's transfer method, and computes the
+ * error using bulk numerical differentiation.
+ */
+template <typename K>
+class EssentialTransferFactor : public TransferFactor<EssentialMatrix> {
+  using EM = EssentialMatrix;
+  using Triplet = std::tuple<Point2, Point2, Point2>;
+  std::shared_ptr<K> calibration_;  ///< Shared pointer to calibration object
+
+ public:
+  using Base = TransferFactor<EM>;
+  using This = EssentialTransferFactor<K>;
+  using shared_ptr = std::shared_ptr<This>;
+
+  /**
+   * @brief Constructor that accepts a vector of point triplets and a shared
+   * calibration.
+   *
+   * @param edge1 First EdgeKey specifying E1: (a, c) or (c, a)
+   * @param edge2 Second EdgeKey specifying E2: (b, c) or (c, b)
+   * @param triplets A vector of triplets containing (pa, pb, pc)
+   * @param calibration Shared pointer to calibration object
+   * @param model An optional SharedNoiseModel
+   */
+  EssentialTransferFactor(EdgeKey edge1, EdgeKey edge2,
+                          const std::vector<Triplet>& triplets,
+                          const std::shared_ptr<K>& calibration,
+                          const SharedNoiseModel& model = nullptr)
+      : Base(edge1, edge2, triplets, model), calibration_(calibration) {}
+
+  /// Transfer points pa and pb to view c and evaluate error.
+  Vector2 TransferError(const Matrix3& Eca, const Point2& pa,
+                        const Matrix3& Ecb, const Point2& pb,
+                        const Point2& pc) const {
+    const Point2 pA = calibration_->calibrate(pa);
+    const Point2 pB = calibration_->calibrate(pb);
+    const Point2 pC = EpipolarTransfer(Eca, pA, Ecb, pB);
+    return calibration_->uncalibrate(pC) - pc;
+  }
+
+  /// Evaluate error function
+  Vector evaluateError(const EM& E1, const EM& E2,
+                       OptionalMatrixType H1 = nullptr,
+                       OptionalMatrixType H2 = nullptr) const override {
+    std::function<Vector(const EM&, const EM&)> errorFunction =
+        [&](const EM& e1, const EM& e2) {
+          Vector errors(2 * this->triplets_.size());
+          size_t idx = 0;
+          auto [Eca, Ecb] = this->getMatrices(e1, e2);
+          for (const auto& [pa, pb, pc] : this->triplets_) {
+            errors.segment<2>(idx) = TransferError(Eca, pa, Ecb, pb, pc);
+            idx += 2;
+          }
+          return errors;
+        };
+
+    // Compute error
+    Vector errors = errorFunction(E1, E2);
+
+    // Compute Jacobians if requested
+    if (H1) *H1 = numericalDerivative21(errorFunction, E1, E2);
+    if (H2) *H2 = numericalDerivative22(errorFunction, E1, E2);
+
+    return errors;
+  }
+};
+
+/**
+ * @class EssentialTransferFactorK
+ * @brief Transfers points between views using essential matrices, optimizes for
+ * calibrations of the views, as well. Note that the EssentialMatrixFactor4 does
+ * something similar but without transfer.
+ *
+ * @note Derives calibration keys from edges, and uses symbol 'k'.
+ *
+ * This factor is templated on the calibration class K and extends
+ * the TransferFactor for EssentialMatrices. It involves two essential matrices
+ * and three calibration objects (Ka, Kb, Kc). The evaluateError
+ * function calibrates the image points, calls the base class's transfer method,
+ * and computes the error using bulk numerical differentiation.
+ */
+template <typename K>
+class EssentialTransferFactorK
+    : public NoiseModelFactorN<EssentialMatrix, EssentialMatrix, K, K, K>,
+      TransferEdges<EssentialMatrix> {
+  using EM = EssentialMatrix;
+  using Triplet = std::tuple<Point2, Point2, Point2>;
+  std::vector<Triplet> triplets_;  ///< Point triplets
+
+ public:
+  using Base = NoiseModelFactorN<EM, EM, K, K, K>;
+  using This = EssentialTransferFactorK<K>;
+  using shared_ptr = std::shared_ptr<This>;
+
+  /**
+   * @brief Constructor that accepts a vector of point triplets.
+   *
+   * @param edge1 First EdgeKey specifying E1: (a, c) or (c, a)
+   * @param edge2 Second EdgeKey specifying E2: (b, c) or (c, b)
+   * @param triplets A vector of triplets containing (pa, pb, pc)
+   * @param model An optional SharedNoiseModel
+   */
+  EssentialTransferFactorK(EdgeKey edge1, EdgeKey edge2,
+                           const std::vector<Triplet>& triplets,
+                           const SharedNoiseModel& model = nullptr)
+      : Base(model, edge1, edge2,
+             Symbol('k', ViewA(edge1, edge2)),   // calibration key for view a
+             Symbol('k', ViewB(edge1, edge2)),   // calibration key for view b
+             Symbol('k', ViewC(edge1, edge2))),  // calibration key for target c
+        TransferEdges<EM>(edge1, edge2),
+        triplets_(triplets) {}
+
+  /// Transfer points pa and pb to view c and evaluate error.
+  Vector2 TransferError(const Matrix3& Eca, const K& Ka, const Point2& pa,
+                        const Matrix3& Ecb, const K& Kb, const Point2& pb,
+                        const K& Kc, const Point2& pc) const {
+    const Point2 pA = Ka.calibrate(pa);
+    const Point2 pB = Kb.calibrate(pb);
+    const Point2 pC = EpipolarTransfer(Eca, pA, Ecb, pB);
+    return Kc.uncalibrate(pC) - pc;
+  }
+
+  /// Evaluate error function
+  Vector evaluateError(const EM& E1, const EM& E2, const K& Ka, const K& Kb,
+                       const K& Kc, OptionalMatrixType H1 = nullptr,
+                       OptionalMatrixType H2 = nullptr,
+                       OptionalMatrixType H3 = nullptr,
+                       OptionalMatrixType H4 = nullptr,
+                       OptionalMatrixType H5 = nullptr) const override {
+    std::function<Vector(const EM&, const EM&, const K&, const K&, const K&)>
+        errorFunction = [&](const EM& e1, const EM& e2, const K& kA,
+                            const K& kB, const K& kC) {
+          Vector errors(2 * triplets_.size());
+          size_t idx = 0;
+          auto [Eca, Ecb] = this->getMatrices(e1, e2);
+          for (const auto& [pa, pb, pc] : triplets_) {
+            errors.segment<2>(idx) =
+                TransferError(Eca, kA, pa, Ecb, kB, pb, kC, pc);
+            idx += 2;
+          }
+          return errors;
+        };
+
+    // Compute error
+    Vector errors = errorFunction(E1, E2, Ka, Kb, Kc);
+
+    // Compute Jacobians if requested
+    if (H1) *H1 = numericalDerivative51(errorFunction, E1, E2, Ka, Kb, Kc);
+    if (H2) *H2 = numericalDerivative52(errorFunction, E1, E2, Ka, Kb, Kc);
+    if (H3) *H3 = numericalDerivative53(errorFunction, E1, E2, Ka, Kb, Kc);
+    if (H4) *H4 = numericalDerivative54(errorFunction, E1, E2, Ka, Kb, Kc);
+    if (H5) *H5 = numericalDerivative55(errorFunction, E1, E2, Ka, Kb, Kc);
+
+    return errors;
+  }
+
+  /// Return the dimension of the factor
+  size_t dim() const override { return 2 * triplets_.size(); }
+};
+
 }  // namespace gtsam

gtsam/sfm/sfm.i

@@ -75,6 +75,33 @@ bool writeBAL(string filename, gtsam::SfmData& data);
 gtsam::Values initialCamerasEstimate(const gtsam::SfmData& db);
 gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData& db);
 
+#include <gtsam/sfm/TransferFactor.h>
+#include <gtsam/geometry/FundamentalMatrix.h>
+template <F = {gtsam::SimpleFundamentalMatrix, gtsam::FundamentalMatrix}>
+virtual class TransferFactor : gtsam::NoiseModelFactor {
+  TransferFactor(gtsam::EdgeKey edge1, gtsam::EdgeKey edge2,
+                 const std::vector<std::tuple<gtsam::Point2, gtsam::Point2, gtsam::Point2>>& triplets,
+                 const gtsam::noiseModel::Base* model = nullptr);
+};
+
+#include <gtsam/geometry/Cal3_S2.h>
+#include <gtsam/geometry/Cal3f.h>
+#include <gtsam/geometry/Cal3Bundler.h>
+template <K = {gtsam::Cal3_S2, gtsam::Cal3f, gtsam::Cal3Bundler}>
+virtual class EssentialTransferFactor : gtsam::NoiseModelFactor {
+  EssentialTransferFactor(gtsam::EdgeKey edge1, gtsam::EdgeKey edge2,
+                          const std::vector<std::tuple<gtsam::Point2, gtsam::Point2, gtsam::Point2>>& triplets,
+                          const K* calibration,
+                          const gtsam::noiseModel::Base* model = nullptr);
+};
+
+template <K = {gtsam::Cal3_S2, gtsam::Cal3f, gtsam::Cal3Bundler}>
+virtual class EssentialTransferFactorK : gtsam::NoiseModelFactor {
+  EssentialTransferFactorK(gtsam::EdgeKey edge1, gtsam::EdgeKey edge2,
+                          const std::vector<std::tuple<gtsam::Point2, gtsam::Point2, gtsam::Point2>>& triplets,
+                          const gtsam::noiseModel::Base* model = nullptr);
+};
+
 #include <gtsam/sfm/ShonanFactor.h>
 
 virtual class ShonanFactor3 : gtsam::NoiseModelFactor {

gtsam/sfm/tests/testTransferFactor.cpp

@@ -1,19 +1,23 @@
 /*
  * @file testTransferFactor.cpp
- * @brief Test TransferFactor class
+ * @brief Test TransferFactor classes
- * @author Your Name
+ * @author Frank Dellaert
- * @date October 23, 2024
+ * @date October 2024
  */
 
 #include <CppUnitLite/TestHarness.h>
+#include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/SimpleCamera.h>
 #include <gtsam/nonlinear/factorTesting.h>
 #include <gtsam/sfm/TransferFactor.h>
 
+#include <memory>
+
 using namespace gtsam;
+using symbol_shorthand::K;
 
 //*************************************************************************
-/// Generate three cameras on a circle, looking in
+/// Generate three cameras on a circle, looking inwards
 std::array<Pose3, 3> generateCameraPoses() {
   std::array<Pose3, 3> cameraPoses;
   const double radius = 1.0;
@@ -48,8 +52,12 @@ std::array<Pose3, 3> cameraPoses = generateCameraPoses();
 auto triplet = generateTripleF(cameraPoses);
 double focalLength = 1000;
 Point2 principalPoint(640 / 2, 480 / 2);
-const Cal3_S2 K(focalLength, focalLength, 0.0,  //
+const Cal3_S2 cal(focalLength, focalLength, 0.0,  //
-                principalPoint.x(), principalPoint.y());
+                  principalPoint.x(), principalPoint.y());
+// Create cameras
+auto f = [](const Pose3& pose) { return PinholeCamera<Cal3_S2>(pose, cal); };
+std::array<PinholeCamera<Cal3_S2>, 3> cameras = {
+    f(cameraPoses[0]), f(cameraPoses[1]), f(cameraPoses[2])};
 }  // namespace fixture
 
 //*************************************************************************
@@ -71,20 +79,17 @@ TEST(TransferFactor, Jacobians) {
 
   // Now project a point into the three cameras
   const Point3 P(0.1, 0.2, 0.3);
-
   std::array<Point2, 3> p;
   for (size_t i = 0; i < 3; ++i) {
-    // Project the point into each camera
+    p[i] = cameras[i].project(P);
-    PinholeCameraCal3_S2 camera(cameraPoses[i], K);
-    p[i] = camera.project(P);
   }
 
   // Create a TransferFactor
   EdgeKey key01(0, 1), key12(1, 2), key20(2, 0);
   TransferFactor<SimpleFundamentalMatrix>  //
-      factor0{key01, key20, p[1], p[2], p[0]},
+      factor0{key01, key20, {{p[1], p[2], p[0]}}},
-      factor1{key12, key01, p[2], p[0], p[1]},
+      factor1{key12, key01, {{p[2], p[0], p[1]}}},
-      factor2{key20, key12, p[0], p[1], p[2]};
+      factor2{key20, key12, {{p[0], p[1], p[2]}}};
 
   // Create Values with edge keys
   Values values;
@@ -107,19 +112,14 @@ TEST(TransferFactor, MultipleTuples) {
 
   // Now project multiple points into the three cameras
   const size_t numPoints = 5;  // Number of points to test
-  std::vector<Point3> points3D;
   std::vector<std::array<Point2, 3>> projectedPoints;
 
   // Generate random 3D points and project them
   for (size_t n = 0; n < numPoints; ++n) {
     Point3 P(0.1 * n, 0.2 * n, 0.3 + 0.1 * n);
-    points3D.push_back(P);
-
     std::array<Point2, 3> p;
     for (size_t i = 0; i < 3; ++i) {
-      // Project the point into each camera
+      p[i] = cameras[i].project(P);
-      PinholeCameraCal3_S2 camera(cameraPoses[i], K);
-      p[i] = camera.project(P);
     }
     projectedPoints.push_back(p);
   }
@@ -153,9 +153,102 @@ TEST(TransferFactor, MultipleTuples) {
   EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-7);
 }
 
+//*************************************************************************
+// Test for EssentialTransferFactor
+TEST(EssentialTransferFactor, Jacobians) {
+  using namespace fixture;
+
+  // Create EssentialMatrices between cameras
+  EssentialMatrix E01 =
+      EssentialMatrix::FromPose3(cameraPoses[0].between(cameraPoses[1]));
+  EssentialMatrix E02 =
+      EssentialMatrix::FromPose3(cameraPoses[0].between(cameraPoses[2]));
+
+  // Project a point into the three cameras
+  const Point3 P(0.1, 0.2, 0.3);
+  std::array<Point2, 3> p;
+  for (size_t i = 0; i < 3; ++i) {
+    p[i] = cameras[i].project(P);
+  }
+
+  // Create EdgeKeys
+  EdgeKey key01(0, 1);
+  EdgeKey key02(0, 2);
+
+  // Create an EssentialTransferFactor
+  auto sharedCal = std::make_shared<Cal3_S2>(cal);
+  EssentialTransferFactor<Cal3_S2> factor(key01, key02, {{p[1], p[2], p[0]}},
+                                          sharedCal);
+
+  // Create Values and insert variables
+  Values values;
+  values.insert(key01, E01);  // Essential matrix between views 0 and 1
+  values.insert(key02, E02);  // Essential matrix between views 0 and 2
+
+  // Check error
+  Matrix H1, H2, H3, H4, H5;
+  Vector error = factor.evaluateError(E01, E02,  //
+                                      &H1, &H2);
+  EXPECT(H1.rows() == 2 && H1.cols() == 5)
+  EXPECT(H2.rows() == 2 && H2.cols() == 5)
+  EXPECT(assert_equal(Vector::Zero(2), error, 1e-9))
+
+  // Check Jacobians
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-7);
+}
+
+//*************************************************************************
+// Test for EssentialTransferFactorK
+TEST(EssentialTransferFactorK, Jacobians) {
+  using namespace fixture;
+
+  // Create EssentialMatrices between cameras
+  EssentialMatrix E01 =
+      EssentialMatrix::FromPose3(cameraPoses[0].between(cameraPoses[1]));
+  EssentialMatrix E02 =
+      EssentialMatrix::FromPose3(cameraPoses[0].between(cameraPoses[2]));
+
+  // Project a point into the three cameras
+  const Point3 P(0.1, 0.2, 0.3);
+  std::array<Point2, 3> p;
+  for (size_t i = 0; i < 3; ++i) {
+    p[i] = cameras[i].project(P);
+  }
+
+  // Create EdgeKeys
+  EdgeKey key01(0, 1);
+  EdgeKey key02(0, 2);
+
+  // Create an EssentialTransferFactorK
+  EssentialTransferFactorK<Cal3_S2> factor(key01, key02, {{p[1], p[2], p[0]}});
+
+  // Create Values and insert variables
+  Values values;
+  values.insert(key01, E01);  // Essential matrix between views 0 and 1
+  values.insert(key02, E02);  // Essential matrix between views 0 and 2
+  values.insert(K(1), cal);   // Calibration for view A (view 1)
+  values.insert(K(2), cal);   // Calibration for view B (view 2)
+  values.insert(K(0), cal);   // Calibration for view C (view 0)
+
+  // Check error
+  Matrix H1, H2, H3, H4, H5;
+  Vector error = factor.evaluateError(E01, E02,       //
+                                      cal, cal, cal,  //
+                                      &H1, &H2, &H3, &H4, &H5);
+  EXPECT(H1.rows() == 2 && H1.cols() == 5)
+  EXPECT(H2.rows() == 2 && H2.cols() == 5)
+  EXPECT(H3.rows() == 2 && H3.cols() == 5)
+  EXPECT(H4.rows() == 2 && H4.cols() == 5)
+  EXPECT(H5.rows() == 2 && H5.cols() == 5)
+  EXPECT(assert_equal(Vector::Zero(2), error, 1e-9))
+
+  // Check Jacobians
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-7);
+}
+
 //*************************************************************************
 int main() {
   TestResult tr;
   return TestRegistry::runAllTests(tr);
 }
-//*************************************************************************
+//*************************************************************************

python/CMakeLists.txt

@@ -156,6 +156,11 @@ foreach(test_file ${GTSAM_PYTHON_TEST_FILES})
         get_filename_component(test_file_name ${test_file} NAME)
         configure_file(${test_file} "${GTSAM_MODULE_PATH}/tests/${test_file_name}" COPYONLY)
 endforeach()
+file(GLOB GTSAM_PYTHON_TEST_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/examples/*.py")
+foreach(test_file ${GTSAM_PYTHON_TEST_FILES})
+        get_filename_component(test_file_name ${test_file} NAME)
+        configure_file(${test_file} "${GTSAM_MODULE_PATH}/tests/${test_file_name}" COPYONLY)
+endforeach()
 file(GLOB GTSAM_PYTHON_UTIL_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/utils/*.py")
 foreach(util_file ${GTSAM_PYTHON_UTIL_FILES})
         configure_file(${util_file} "${GTSAM_MODULE_PATH}/utils/${test_file}" COPYONLY)

python/README.md

@@ -13,7 +13,7 @@ For instructions on updating the version of the [wrap library](https://github.co
   use the `-DGTSAM_PYTHON_VERSION=3.6` option when running `cmake` otherwise the default interpreter will be used.
 - If the interpreter is inside an environment (such as an anaconda environment or virtualenv environment),
   then the environment should be active while building GTSAM.
-- This wrapper needs `pyparsing(>=2.4.2)`, and `numpy(>=1.11.0)`. These can be installed as follows:
+- This wrapper needs [pyparsing(>=2.4.2)](https://github.com/pyparsing/pyparsing), [pybind-stubgen>=2.5.1](https://github.com/sizmailov/pybind11-stubgen) and [numpy(>=1.11.0)](https://numpy.org/). These can all be installed as follows:
 
   ```bash
   pip install -r <gtsam_folder>/python/dev_requirements.txt

python/gtsam/examples/EssentialViewGraphExample.py

@@ -0,0 +1,120 @@
+"""
+  GTSAM Copyright 2010, Georgia Tech Research Corporation,
+  Atlanta, Georgia 30332-0415
+  All Rights Reserved
+  Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+  See LICENSE for the license information
+"""
+
+"""
+Python version of EssentialViewGraphExample.cpp
+View-graph calibration with essential matrices.
+Author: Frank Dellaert
+Date: October 2024
+"""
+
+import numpy as np
+from gtsam.examples import SFMdata
+
+import gtsam
+from gtsam import Cal3f, EdgeKey, EssentialMatrix
+from gtsam import EssentialTransferFactorCal3f as Factor
+from gtsam import (LevenbergMarquardtOptimizer, LevenbergMarquardtParams,
+                   NonlinearFactorGraph, PinholeCameraCal3f, Values)
+
+# For symbol shorthand (e.g., X(0), L(1))
+K = gtsam.symbol_shorthand.K
+
+
+# Formatter function for printing keys
+def formatter(key):
+    sym = gtsam.Symbol(key)
+    if sym.chr() == ord("k"):
+        return f"k{sym.index()}"
+    else:
+        edge = EdgeKey(key)
+        return f"({edge.i()},{edge.j()})"
+
+
+def main():
+    # Define the camera calibration parameters
+    cal = Cal3f(50.0, 50.0, 50.0)
+
+    # Create the set of 8 ground-truth landmarks
+    points = SFMdata.createPoints()
+
+    # Create the set of 4 ground-truth poses
+    poses = SFMdata.posesOnCircle(4, 30)
+
+    # Calculate ground truth essential matrices, 1 and 2 poses apart
+    E1 = EssentialMatrix.FromPose3(poses[0].between(poses[1]))
+    E2 = EssentialMatrix.FromPose3(poses[0].between(poses[2]))
+
+    # Simulate measurements from each camera pose
+    p = [[None for _ in range(8)] for _ in range(4)]
+    for i in range(4):
+        camera = PinholeCameraCal3f(poses[i], cal)
+        for j in range(8):
+            p[i][j] = camera.project(points[j])
+
+    # Create the factor graph
+    graph = NonlinearFactorGraph()
+
+    for a in range(4):
+        b = (a + 1) % 4  # Next camera
+        c = (a + 2) % 4  # Camera after next
+
+        # Collect data for the three factors
+        tuples1 = []
+        tuples2 = []
+        tuples3 = []
+
+        for j in range(8):
+            tuples1.append((p[a][j], p[b][j], p[c][j]))
+            tuples2.append((p[a][j], p[c][j], p[b][j]))
+            tuples3.append((p[c][j], p[b][j], p[a][j]))
+
+        # Add transfer factors between views a, b, and c.
+        graph.add(Factor(EdgeKey(a, c), EdgeKey(b, c), tuples1))
+        graph.add(Factor(EdgeKey(a, b), EdgeKey(b, c), tuples2))
+        graph.add(Factor(EdgeKey(a, c), EdgeKey(a, b), tuples3))
+
+    graph.print("graph", formatter)
+
+    # Create a delta vector to perturb the ground truth (small perturbation)
+    delta = np.ones(5) * 1e-2
+
+    # Create the initial estimate for essential matrices
+    initialEstimate = Values()
+    for a in range(4):
+        b = (a + 1) % 4  # Next camera
+        c = (a + 2) % 4  # Camera after next
+        initialEstimate.insert(EdgeKey(a, b).key(), E1.retract(delta))
+        initialEstimate.insert(EdgeKey(a, c).key(), E2.retract(delta))
+
+    # Insert initial calibrations
+    for i in range(4):
+        initialEstimate.insert(K(i), cal)
+
+    # Optimize the graph and print results
+    params = LevenbergMarquardtParams()
+    params.setlambdaInitial(1000.0)  # Initialize lambda to a high value
+    params.setVerbosityLM("SUMMARY")
+    optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate, params)
+    result = optimizer.optimize()
+
+    print("Initial error = ", graph.error(initialEstimate))
+    print("Final error = ", graph.error(result))
+
+    # Print final results
+    print("Final Results:")
+    result.print("", formatter)
+
+    # Print ground truth essential matrices
+    print("Ground Truth E1:\n", E1)
+    print("Ground Truth E2:\n", E2)
+
+
+if __name__ == "__main__":
+    main()

python/gtsam/examples/SFMExample.py

@@ -9,20 +9,22 @@ See LICENSE for the license information
 A structure-from-motion problem on a simulated dataset
 """
 
-import gtsam
 import matplotlib.pyplot as plt
 import numpy as np
+
+import gtsam
 from gtsam import symbol_shorthand
+
 L = symbol_shorthand.L
 X = symbol_shorthand.X
 
 from gtsam.examples import SFMdata
-from gtsam import (Cal3_S2, DoglegOptimizer,
-                         GenericProjectionFactorCal3_S2, Marginals,
-                         NonlinearFactorGraph, PinholeCameraCal3_S2, Point3,
-                         Pose3, PriorFactorPoint3, PriorFactorPose3, Rot3, Values)
 from gtsam.utils import plot
 
+from gtsam import (Cal3_S2, DoglegOptimizer, GenericProjectionFactorCal3_S2,
+                   Marginals, NonlinearFactorGraph, PinholeCameraCal3_S2,
+                   PriorFactorPoint3, PriorFactorPose3, Values)
+
 
 def main():
     """
@@ -43,7 +45,7 @@ def main():
     Finally, once all of the factors have been added to our factor graph, we will want to
     solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values.
     GTSAM includes several nonlinear optimizers to perform this step. Here we will use a
-    trust-region method known as Powell's Degleg
+    trust-region method known as Powell's Dogleg
 
     The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the
     nonlinear functions around an initial linearization point, then solve the linear system
@@ -78,8 +80,7 @@ def main():
         camera = PinholeCameraCal3_S2(pose, K)
         for j, point in enumerate(points):
             measurement = camera.project(point)
-            factor = GenericProjectionFactorCal3_S2(
+            factor = GenericProjectionFactorCal3_S2(measurement, measurement_noise, X(i), L(j), K)
-                measurement, measurement_noise, X(i), L(j), K)
             graph.push_back(factor)
 
     # Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained
@@ -88,28 +89,29 @@ def main():
     point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
     factor = PriorFactorPoint3(L(0), points[0], point_noise)
     graph.push_back(factor)
-    graph.print('Factor Graph:\n')
+    graph.print("Factor Graph:\n")
 
     # Create the data structure to hold the initial estimate to the solution
     # Intentionally initialize the variables off from the ground truth
     initial_estimate = Values()
+    rng = np.random.default_rng()
     for i, pose in enumerate(poses):
-        transformed_pose = pose.retract(0.1*np.random.randn(6,1))
+        transformed_pose = pose.retract(0.1 * rng.standard_normal(6).reshape(6, 1))
         initial_estimate.insert(X(i), transformed_pose)
     for j, point in enumerate(points):
-        transformed_point = point + 0.1*np.random.randn(3)
+        transformed_point = point + 0.1 * rng.standard_normal(3)
         initial_estimate.insert(L(j), transformed_point)
-    initial_estimate.print('Initial Estimates:\n')
+    initial_estimate.print("Initial Estimates:\n")
 
     # Optimize the graph and print results
     params = gtsam.DoglegParams()
-    params.setVerbosity('TERMINATION')
+    params.setVerbosity("TERMINATION")
     optimizer = DoglegOptimizer(graph, initial_estimate, params)
-    print('Optimizing:')
+    print("Optimizing:")
     result = optimizer.optimize()
-    result.print('Final results:\n')
+    result.print("Final results:\n")
-    print('initial error = {}'.format(graph.error(initial_estimate)))
+    print("initial error = {}".format(graph.error(initial_estimate)))
-    print('final error = {}'.format(graph.error(result)))
+    print("final error = {}".format(graph.error(result)))
 
     marginals = Marginals(graph, result)
     plot.plot_3d_points(1, result, marginals=marginals)
@@ -117,5 +119,6 @@ def main():
     plot.set_axes_equal(1)
     plt.show()
 
-if __name__ == '__main__':
+
+if __name__ == "__main__":
     main()

python/gtsam/examples/ViewGraphComparison.py

@@ -0,0 +1,372 @@
+"""
+  Compare several methods for optimizing the view-graph.
+  We measure the distance from the ground truth in terms of the norm of 
+  local coordinates (geodesic distance) on the F-manifold. 
+  We also plot the final error of the optimization.
+  
+  Author: Frank Dellaert (with heavy assist from ChatGPT)
+  Date: October 2024
+"""
+
+import argparse
+
+import matplotlib.pyplot as plt
+import numpy as np
+from gtsam.examples import SFMdata
+
+import gtsam
+from gtsam import (
+    Cal3f,
+    EdgeKey,
+    EssentialMatrix,
+    FundamentalMatrix,
+    LevenbergMarquardtOptimizer,
+    LevenbergMarquardtParams,
+    NonlinearFactorGraph,
+    PinholeCameraCal3f,
+    SimpleFundamentalMatrix,
+    Values,
+)
+
+# For symbol shorthand (e.g., K(0), K(1))
+K = gtsam.symbol_shorthand.K
+
+# Methods to compare
+methods = ["SimpleF", "Fundamental", "Essential+Ks", "Calibrated"]
+
+
+# Formatter function for printing keys
+def formatter(key):
+    sym = gtsam.Symbol(key)
+    if sym.chr() == ord("k"):
+        return f"k{sym.index()}"
+    else:
+        edge = EdgeKey(key)
+        return f"({edge.i()},{edge.j()})"
+
+
+def simulate_geometry(num_cameras, rng, num_random_points=12):
+    """simulate geometry (points and poses)"""
+    # Define the camera calibration parameters
+    cal = Cal3f(50.0, 50.0, 50.0)
+
+    # Create the set of 8 ground-truth landmarks
+    points = SFMdata.createPoints()
+
+    # Create extra random points in the -10,10 cube around the origin
+    extra_points = rng.uniform(-10, 10, (num_random_points, 3))
+    points.extend([gtsam.Point3(p) for p in extra_points])
+
+    # Create the set of ground-truth poses
+    poses = SFMdata.posesOnCircle(num_cameras, 30)
+
+    return points, poses, cal
+
+
+def simulate_data(points, poses, cal, rng, noise_std):
+    """Simulate measurements from each camera pose"""
+    measurements = [[None for _ in points] for _ in poses]
+    for i, pose in enumerate(poses):
+        camera = PinholeCameraCal3f(pose, cal)
+        for j, point in enumerate(points):
+            projection = camera.project(point)
+            noise = rng.normal(0, noise_std, size=2)
+            measurements[i][j] = projection + noise
+
+    return measurements
+
+
+# Function to compute ground truth matrices
+def compute_ground_truth(method, poses, cal):
+    E1 = EssentialMatrix.FromPose3(poses[0].between(poses[1]))
+    E2 = EssentialMatrix.FromPose3(poses[0].between(poses[2]))
+    F1 = FundamentalMatrix(cal.K(), E1, cal.K())
+    F2 = FundamentalMatrix(cal.K(), E2, cal.K())
+    if method == "Fundamental":
+        return F1, F2
+    elif method == "SimpleF":
+        f = cal.fx()
+        c = cal.principalPoint()
+        SF1 = SimpleFundamentalMatrix(E1, f, f, c, c)
+        SF2 = SimpleFundamentalMatrix(E2, f, f, c, c)
+        return SF1, SF2
+    elif method == "Essential+Ks" or method == "Calibrated":
+        return E1, E2
+    else:
+        raise ValueError(f"Unknown method {method}")
+
+
+def build_factor_graph(method, num_cameras, measurements, cal):
+    """build the factor graph"""
+    graph = NonlinearFactorGraph()
+
+    if method == "Fundamental":
+        FactorClass = gtsam.TransferFactorFundamentalMatrix
+    elif method == "SimpleF":
+        FactorClass = gtsam.TransferFactorSimpleFundamentalMatrix
+    elif method == "Essential+Ks":
+        FactorClass = gtsam.EssentialTransferFactorKCal3f
+        # add priors on all calibrations:
+        for i in range(num_cameras):
+            model = gtsam.noiseModel.Isotropic.Sigma(1, 10.0)
+            graph.addPriorCal3f(K(i), cal, model)
+    elif method == "Calibrated":
+        FactorClass = gtsam.EssentialTransferFactorCal3f
+        # No priors on calibration needed
+    else:
+        raise ValueError(f"Unknown method {method}")
+
+    z = measurements  # shorthand
+
+    for a in range(num_cameras):
+        b = (a + 1) % num_cameras  # Next camera
+        c = (a + 2) % num_cameras  # Camera after next
+
+        # Vectors to collect tuples for each factor
+        tuples1 = []
+        tuples2 = []
+        tuples3 = []
+
+        # Collect data for the three factors
+        for j in range(len(measurements[0])):
+            tuples1.append((z[a][j], z[b][j], z[c][j]))
+            tuples2.append((z[a][j], z[c][j], z[b][j]))
+            tuples3.append((z[c][j], z[b][j], z[a][j]))
+
+        # Add transfer factors between views a, b, and c.
+        if method in ["Calibrated"]:
+            graph.add(FactorClass(EdgeKey(a, c), EdgeKey(b, c), tuples1, cal))
+            graph.add(FactorClass(EdgeKey(a, b), EdgeKey(b, c), tuples2, cal))
+            graph.add(FactorClass(EdgeKey(a, c), EdgeKey(a, b), tuples3, cal))
+        else:
+            graph.add(FactorClass(EdgeKey(a, c), EdgeKey(b, c), tuples1))
+            graph.add(FactorClass(EdgeKey(a, b), EdgeKey(b, c), tuples2))
+            graph.add(FactorClass(EdgeKey(a, c), EdgeKey(a, b), tuples3))
+
+    return graph
+
+
+def get_initial_estimate(method, num_cameras, ground_truth, cal):
+    """get initial estimate for method"""
+    initialEstimate = Values()
+    total_dimension = 0
+
+    if method in ["Fundamental", "SimpleF"]:
+        F1, F2 = ground_truth
+        for a in range(num_cameras):
+            b = (a + 1) % num_cameras  # Next camera
+            c = (a + 2) % num_cameras  # Camera after next
+            initialEstimate.insert(EdgeKey(a, b).key(), F1)
+            initialEstimate.insert(EdgeKey(a, c).key(), F2)
+            total_dimension += F1.dim() + F2.dim()
+    elif method in ["Essential+Ks", "Calibrated"]:
+        E1, E2 = ground_truth
+        for a in range(num_cameras):
+            b = (a + 1) % num_cameras  # Next camera
+            c = (a + 2) % num_cameras  # Camera after next
+            initialEstimate.insert(EdgeKey(a, b).key(), E1)
+            initialEstimate.insert(EdgeKey(a, c).key(), E2)
+            total_dimension += E1.dim() + E2.dim()
+    else:
+        raise ValueError(f"Unknown method {method}")
+
+    if method == "Essential+Ks":
+        # Insert initial calibrations
+        for i in range(num_cameras):
+            initialEstimate.insert(K(i), cal)
+            total_dimension += cal.dim()
+
+    print(f"Total dimension of the problem: {total_dimension}")
+    return initialEstimate
+
+
+def optimize(graph, initialEstimate, method):
+    """optimize the graph"""
+    params = LevenbergMarquardtParams()
+    params.setlambdaInitial(1e10)  # Initialize lambda to a high value
+    params.setlambdaUpperBound(1e10)
+    # params.setAbsoluteErrorTol(0.1)
+    params.setVerbosityLM("SUMMARY")
+    optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate, params)
+    result = optimizer.optimize()
+    iterations = optimizer.iterations()
+    return result, iterations
+
+
+def compute_distances(method, result, ground_truth, num_cameras, cal):
+    """Compute geodesic distances from ground truth"""
+    distances = []
+
+    F1, F2 = ground_truth["Fundamental"]
+
+    for a in range(num_cameras):
+        b = (a + 1) % num_cameras
+        c = (a + 2) % num_cameras
+        key_ab = EdgeKey(a, b).key()
+        key_ac = EdgeKey(a, c).key()
+
+        if method in ["Essential+Ks", "Calibrated"]:
+            E_est_ab = result.atEssentialMatrix(key_ab)
+            E_est_ac = result.atEssentialMatrix(key_ac)
+
+        # Compute estimated FundamentalMatrices
+        if method == "Fundamental":
+            F_est_ab = result.atFundamentalMatrix(key_ab)
+            F_est_ac = result.atFundamentalMatrix(key_ac)
+        elif method == "SimpleF":
+            SF_est_ab = result.atSimpleFundamentalMatrix(key_ab).matrix()
+            SF_est_ac = result.atSimpleFundamentalMatrix(key_ac).matrix()
+            F_est_ab = FundamentalMatrix(SF_est_ab)
+            F_est_ac = FundamentalMatrix(SF_est_ac)
+        elif method == "Essential+Ks":
+            # Retrieve calibrations from result:
+            cal_a = result.atCal3f(K(a))
+            cal_b = result.atCal3f(K(b))
+            cal_c = result.atCal3f(K(c))
+
+            # Convert estimated EssentialMatrices to FundamentalMatrices
+            F_est_ab = FundamentalMatrix(cal_a.K(), E_est_ab, cal_b.K())
+            F_est_ac = FundamentalMatrix(cal_a.K(), E_est_ac, cal_c.K())
+        elif method == "Calibrated":
+            # Use ground truth calibration
+            F_est_ab = FundamentalMatrix(cal.K(), E_est_ab, cal.K())
+            F_est_ac = FundamentalMatrix(cal.K(), E_est_ac, cal.K())
+        else:
+            raise ValueError(f"Unknown method {method}")
+
+        # Compute local coordinates (geodesic distance on the F-manifold)
+        dist_ab = np.linalg.norm(F1.localCoordinates(F_est_ab))
+        dist_ac = np.linalg.norm(F2.localCoordinates(F_est_ac))
+        distances.append(dist_ab)
+        distances.append(dist_ac)
+
+    return distances
+
+
+def plot_results(results):
+    """plot results"""
+    methods = list(results.keys())
+    final_errors = [results[method]["final_error"] for method in methods]
+    distances = [results[method]["distances"] for method in methods]
+    iterations = [results[method]["iterations"] for method in methods]
+
+    fig, ax1 = plt.subplots()
+
+    color = "tab:red"
+    ax1.set_xlabel("Method")
+    ax1.set_ylabel("Median Error (log scale)", color=color)
+    ax1.set_yscale("log")
+    ax1.bar(methods, final_errors, color=color, alpha=0.6)
+    ax1.tick_params(axis="y", labelcolor=color)
+
+    ax2 = ax1.twinx()
+    color = "tab:blue"
+    ax2.set_ylabel("Median Geodesic Distance", color=color)
+    ax2.plot(methods, distances, color=color, marker="o", linestyle="-")
+    ax2.tick_params(axis="y", labelcolor=color)
+
+    # Annotate the blue data points with the average number of iterations
+    for i, method in enumerate(methods):
+        ax2.annotate(
+            f"{iterations[i]:.1f}",
+            (i, distances[i]),
+            textcoords="offset points",
+            xytext=(0, 10),
+            ha="center",
+            color=color,
+        )
+
+    plt.title("Comparison of Methods (Labels show avg iterations)")
+    fig.tight_layout()
+    plt.show()
+
+
+# Main function
+def main():
+    # Parse command line arguments
+    parser = argparse.ArgumentParser(description="Compare Fundamental and Essential Matrix Methods")
+    parser.add_argument("--num_cameras", type=int, default=4, help="Number of cameras (default: 4)")
+    parser.add_argument("--num_extra_points", type=int, default=12, help="Number of extra random points (default: 12)")
+    parser.add_argument("--num_trials", type=int, default=5, help="Number of trials (default: 5)")
+    parser.add_argument("--seed", type=int, default=42, help="Random seed (default: 42)")
+    parser.add_argument("--noise_std", type=float, default=0.5, help="Standard deviation of noise (default: 0.5)")
+    args = parser.parse_args()
+
+    # Initialize the random number generator
+    rng = np.random.default_rng(seed=args.seed)
+
+    # Initialize results dictionary
+    results = {method: {"distances": [], "final_error": [], "iterations": []} for method in methods}
+
+    # Simulate geometry
+    points, poses, cal = simulate_geometry(args.num_cameras, rng, args.num_extra_points)
+
+    # Compute ground truth matrices
+    ground_truth = {method: compute_ground_truth(method, poses, cal) for method in methods}
+
+    # Get initial estimates
+    initial_estimate: dict[Values] = {
+        method: get_initial_estimate(method, args.num_cameras, ground_truth[method], cal) for method in methods
+    }
+    simple_f_result: Values = Values()
+
+    for trial in range(args.num_trials):
+        print(f"\nTrial {trial + 1}/{args.num_trials}")
+
+        # Simulate data
+        measurements = simulate_data(points, poses, cal, rng, args.noise_std)
+
+        for method in methods:
+            print(f"\nRunning method: {method}")
+
+            # Build the factor graph
+            graph = build_factor_graph(method, args.num_cameras, measurements, cal)
+
+            # For F, initialize from SimpleF:
+            if method == "Fundamental":
+                initial_estimate[method] = simple_f_result
+
+            # Optimize the graph
+            result, iterations = optimize(graph, initial_estimate[method], method)
+
+            # Store SimpleF result as a set of FundamentalMatrices
+            if method == "SimpleF":
+                simple_f_result = Values()
+                for a in range(args.num_cameras):
+                    b = (a + 1) % args.num_cameras  # Next camera
+                    c = (a + 2) % args.num_cameras  # Camera after next
+                    key_ab = EdgeKey(a, b).key()
+                    key_ac = EdgeKey(a, c).key()
+                    F1 = result.atSimpleFundamentalMatrix(key_ab).matrix()
+                    F2 = result.atSimpleFundamentalMatrix(key_ac).matrix()
+                    simple_f_result.insert(key_ab, FundamentalMatrix(F1))
+                    simple_f_result.insert(key_ac, FundamentalMatrix(F2))
+
+            # Compute distances from ground truth
+            distances = compute_distances(method, result, ground_truth, args.num_cameras, cal)
+
+            # Compute final error
+            final_error = graph.error(result)
+
+            # Store results
+            results[method]["distances"].extend(distances)
+            results[method]["final_error"].append(final_error)
+            results[method]["iterations"].append(iterations)
+
+            print(f"Method: {method}")
+            print(f"Final Error: {final_error:.3f}")
+            print(f"Mean Geodesic Distance: {np.mean(distances):.3f}")
+            print(f"Number of Iterations: {iterations}\n")
+
+    # Average results over trials
+    for method in methods:
+        results[method]["final_error"] = np.median(results[method]["final_error"])
+        results[method]["distances"] = np.median(results[method]["distances"])
+        results[method]["iterations"] = np.median(results[method]["iterations"])
+
+    # Plot results
+    plot_results(results)
+
+
+if __name__ == "__main__":
+    main()

python/gtsam/examples/ViewGraphExample.py

@@ -0,0 +1,111 @@
+"""
+  GTSAM Copyright 2010, Georgia Tech Research Corporation,
+  Atlanta, Georgia 30332-0415
+  All Rights Reserved
+  Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+  See LICENSE for the license information
+"""
+
+"""
+Python version of ViewGraphExample.cpp
+View-graph calibration on a simulated dataset, a la Sweeney 2015
+Author: Frank Dellaert
+Date: October 2024
+"""
+
+import numpy as np
+from gtsam.examples import SFMdata
+
+from gtsam import (Cal3_S2, EdgeKey, FundamentalMatrix,
+                   LevenbergMarquardtOptimizer, LevenbergMarquardtParams,
+                   NonlinearFactorGraph, PinholeCameraCal3_S2)
+from gtsam import TransferFactorFundamentalMatrix as Factor
+from gtsam import Values
+
+
+# Formatter function for printing keys
+def formatter(key):
+    edge = EdgeKey(key)
+    return f"({edge.i()},{edge.j()})"
+
+
+def main():
+    # Define the camera calibration parameters
+    cal = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
+
+    # Create the set of 8 ground-truth landmarks
+    points = SFMdata.createPoints()
+
+    # Create the set of 4 ground-truth poses
+    poses = SFMdata.posesOnCircle(4, 30)
+
+    # Calculate ground truth fundamental matrices, 1 and 2 poses apart
+    F1 = FundamentalMatrix(cal.K(), poses[0].between(poses[1]), cal.K())
+    F2 = FundamentalMatrix(cal.K(), poses[0].between(poses[2]), cal.K())
+
+    # Simulate measurements from each camera pose
+    p = [[None for _ in range(8)] for _ in range(4)]
+    for i in range(4):
+        camera = PinholeCameraCal3_S2(poses[i], cal)
+        for j in range(8):
+            p[i][j] = camera.project(points[j])
+
+    # Create the factor graph
+    graph = NonlinearFactorGraph()
+
+    for a in range(4):
+        b = (a + 1) % 4  # Next camera
+        c = (a + 2) % 4  # Camera after next
+
+        # Vectors to collect tuples for each factor
+        tuples1 = []
+        tuples2 = []
+        tuples3 = []
+
+        # Collect data for the three factors
+        for j in range(8):
+            tuples1.append((p[a][j], p[b][j], p[c][j]))
+            tuples2.append((p[a][j], p[c][j], p[b][j]))
+            tuples3.append((p[c][j], p[b][j], p[a][j]))
+
+        # Add transfer factors between views a, b, and c.
+        graph.add(Factor(EdgeKey(a, c), EdgeKey(b, c), tuples1))
+        graph.add(Factor(EdgeKey(a, b), EdgeKey(b, c), tuples2))
+        graph.add(Factor(EdgeKey(a, c), EdgeKey(a, b), tuples3))
+
+    # Print the factor graph
+    graph.print("Factor Graph:\n", formatter)
+
+    # Create a delta vector to perturb the ground truth
+    delta = np.array([1, 2, 3, 4, 5, 6, 7]) * 1e-5
+
+    # Create the data structure to hold the initial estimate to the solution
+    initialEstimate = Values()
+    for a in range(4):
+        b = (a + 1) % 4  # Next camera
+        c = (a + 2) % 4  # Camera after next
+        initialEstimate.insert(EdgeKey(a, b).key(), F1.retract(delta))
+        initialEstimate.insert(EdgeKey(a, c).key(), F2.retract(delta))
+
+    initialEstimate.print("Initial Estimates:\n", formatter)
+    graph.printErrors(initialEstimate, "Initial Errors:\n", formatter)
+
+    # Optimize the graph and print results
+    params = LevenbergMarquardtParams()
+    params.setlambdaInitial(1000.0)  # Initialize lambda to a high value
+    params.setVerbosityLM("SUMMARY")
+    optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate, params)
+    result = optimizer.optimize()
+
+    print(f"Initial error = {graph.error(initialEstimate)}")
+    print(f"Final error = {graph.error(result)}")
+
+    result.print("Final Results:\n", formatter)
+
+    print("Ground Truth F1:\n", F1.matrix())
+    print("Ground Truth F2:\n", F2.matrix())
+
+
+if __name__ == "__main__":
+    main()