EssentialTransferFactor

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 (Cal3::equals(*base, tol) && std::fabs(k1_ - K.k1_) < tol &&
-          std::fabs(k2_ - K.k2_) < tol && std::fabs(u0_ - K.u0_) < tol &&
-          std::fabs(v0_ - K.v0_) < tol);
+  return Cal3f::equals(static_cast<const Cal3f&>(K), tol) &&
+         std::fabs(k1_ - K.k1_) < tol && std::fabs(k2_ - K.k2_) < 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 tol_ = 1e-5;             ///< tolerance value when calibrating
+  double k1_ = 0.0, k2_ = 0.0;  ///< radial distortion coefficients
+  double tol_ = 1e-5;           ///< tolerance value when calibrating
 
-  // NOTE: We use the base class fx to represent the common focal length.
-  // Also, image center parameters (u0, v0) are not optimized
-  // but are treated as constants.
+  // Note: u0 and v0 are constants and not optimized.
 
  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 tol optional tolerance threshold value for iterative minimization
+   * @param pi point in image coordinates
    * @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