diff --git a/examples/EssentialViewGraphExample.cpp b/examples/EssentialViewGraphExample.cpp new file mode 100644 index 0000000000..205708c62d --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include // Contains EssentialTransferFactorK + +#include + +#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 points = createPoints(); + + // Create the set of 4 ground-truth poses + vector 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, 4> p; + for (size_t i = 0; i < 4; ++i) { + PinholeCamera 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; + + 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> 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(EdgeKey(a, c), EdgeKey(b, c), tuples1); + graph.emplace_shared(EdgeKey(a, b), EdgeKey(b, c), tuples2); + graph.emplace_shared(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; +} \ No newline at end of file diff --git a/examples/ViewGraphExample.cpp b/examples/ViewGraphExample.cpp index 23393fa20c..a33e1853d7 100644 --- a/examples/ViewGraphExample.cpp +++ b/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; diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index 7e008aec35..1520e596a2 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/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(&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(K), tol) && + std::fabs(k1_ - K.k1_) < tol && std::fabs(k2_ - K.k2_) < tol; } /* ************************************************************************* */ diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index 725c1481f3..081688d48c 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -19,7 +19,7 @@ #pragma once -#include +#include #include 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; - /// @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_); } diff --git a/gtsam/geometry/Cal3f.cpp b/gtsam/geometry/Cal3f.cpp new file mode 100644 index 0000000000..319155dc91 --- /dev/null +++ b/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 +#include +#include + +#include + +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(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 \ No newline at end of file diff --git a/gtsam/geometry/Cal3f.h b/gtsam/geometry/Cal3f.h new file mode 100644 index 0000000000..f053f3d112 --- /dev/null +++ b/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 + +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; + + /// @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 + 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 : public internal::Manifold {}; + +template <> +struct traits : public internal::Manifold {}; + +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index a90de48e55..42d2fe5505 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -644,8 +644,36 @@ class EssentialMatrix { double error(gtsam::Vector vA, gtsam::Vector vB); }; +#include +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 -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 Dcal, Eigen::Ref 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 -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 -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 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 -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 -class Cal3Bundler { +virtual class Cal3f : gtsam::Cal3 { // 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); + Cal3f(); + Cal3f(double fx, double u0, double v0); // 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(); - size_t dim() const; - gtsam::Cal3Bundler retract(gtsam::Vector v) const; - gtsam::Vector localCoordinates(const gtsam::Cal3Bundler& c) const; + gtsam::Cal3f retract(gtsam::Vector v) const; + gtsam::Vector localCoordinates(const gtsam::Cal3f& c) const; // Action on Point2 gtsam::Point2 calibrate(const gtsam::Point2& p) const; @@ -891,15 +874,32 @@ class Cal3Bundler { Eigen::Ref Dp) const; // Standard Interface - double fx() const; - double fy() const; + double f() const; + + // enabling serialization functionality + void serialize() const; +}; + + +#include +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 PinholeCameraCal3_S2; typedef gtsam::PinholeCamera PinholeCameraCal3DS2; typedef gtsam::PinholeCamera PinholeCameraCal3Unified; typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; +typedef gtsam::PinholeCamera PinholeCameraCal3f; typedef gtsam::PinholeCamera PinholeCameraCal3Fisheye; #include diff --git a/gtsam/geometry/tests/testCal3Bundler.cpp b/gtsam/geometry/tests/testCal3Bundler.cpp index 020cab2f94..15e633923e 100644 --- a/gtsam/geometry/tests/testCal3Bundler.cpp +++ b/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); - Point2 expected(1000 + g * p.x(), 2000 + g * p.y()); + double distortion = 1.0 + v[1] * r + v[2] * r * r; + 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()); diff --git a/gtsam/geometry/tests/testCal3f.cpp b/gtsam/geometry/tests/testCal3f.cpp new file mode 100644 index 0000000000..d21d39f7fe --- /dev/null +++ b/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 +#include +#include +#include +#include + +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::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); +} +/* ************************************************************************* */ \ No newline at end of file diff --git a/gtsam/inference/EdgeKey.h b/gtsam/inference/EdgeKey.h index bf1bf60300..3d95e4c061 100644 --- a/gtsam/inference/EdgeKey.h +++ b/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_; } diff --git a/gtsam/inference/inference.i b/gtsam/inference/inference.i index b25e2fd628..5084355a8c 100644 --- a/gtsam/inference/inference.i +++ b/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 +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 class Ordering { /// Type of ordering to use diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index f493fe8f6e..09c234630e 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/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::PinholeCamera, gtsam::PinholeCamera, gtsam::PinholeCamera, gtsam::PinholeCamera, + gtsam::PinholeCamera, 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 , gtsam::PinholeCamera, gtsam::PinholeCamera, diff --git a/gtsam/nonlinear/values.i b/gtsam/nonlinear/values.i index 3582a3249f..99548d8ec0 100644 --- a/gtsam/nonlinear/values.i +++ b/gtsam/nonlinear/values.i @@ -4,13 +4,14 @@ namespace gtsam { +#include #include #include +#include #include #include -#include -#include #include +#include #include #include #include @@ -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::PinholeCamera& camera); + void insert(size_t j, const gtsam::EssentialMatrix& E); + 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& camera); + void insert(size_t j, const gtsam::PinholeCamera& camera); + void insert(size_t j, const gtsam::PinholeCamera& camera); + void insert(size_t j, const gtsam::PinholeCamera& camera); void insert(size_t j, const gtsam::PinholeCamera& camera); void insert(size_t j, const gtsam::PinholeCamera& camera); - void insert(size_t j, const gtsam::PinholePose& camera); void insert(size_t j, const gtsam::PinholePose& camera); + void insert(size_t j, const gtsam::PinholePose& camera); + void insert(size_t j, const gtsam::PinholePose& camera); + void insert(size_t j, const gtsam::PinholePose& camera); void insert(size_t j, const gtsam::PinholePose& camera); void insert(size_t j, const gtsam::PinholePose& 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::PinholeCamera& camera); + void update(size_t j, const gtsam::EssentialMatrix& E); + 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& camera); + void update(size_t j, const gtsam::PinholeCamera& camera); + void update(size_t j, const gtsam::PinholeCamera& camera); + void update(size_t j, const gtsam::PinholeCamera& camera); void update(size_t j, const gtsam::PinholeCamera& camera); void update(size_t j, const gtsam::PinholeCamera& camera); - void update(size_t j, const gtsam::PinholePose& camera); void update(size_t j, const gtsam::PinholePose& camera); + void update(size_t j, const gtsam::PinholePose& camera); + void update(size_t j, const gtsam::PinholePose& camera); + void update(size_t j, const gtsam::PinholePose& camera); void update(size_t j, const gtsam::PinholePose& camera); void update(size_t j, const gtsam::PinholePose& 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, - const gtsam::EssentialMatrix& essential_matrix); - void insert_or_assign(size_t j, - const gtsam::PinholeCamera& camera); - void insert_or_assign(size_t j, - const gtsam::PinholeCamera& camera); - void insert_or_assign(size_t j, - const gtsam::PinholeCamera& camera); - void insert_or_assign(size_t j, - const gtsam::PinholeCamera& camera); - void insert_or_assign(size_t j, - const gtsam::PinholePose& camera); - void insert_or_assign(size_t j, - const gtsam::PinholePose& camera); - void insert_or_assign(size_t j, - const gtsam::PinholePose& camera); - void insert_or_assign(size_t j, - const gtsam::PinholePose& camera); - void insert_or_assign(size_t j, - const gtsam::imuBias::ConstantBias& constant_bias); + void insert_or_assign(size_t j, const gtsam::EssentialMatrix& E); + void insert_or_assign(size_t j, const gtsam::FundamentalMatrix& F); + void insert_or_assign(size_t j, const gtsam::SimpleFundamentalMatrix& F); + void insert_or_assign(size_t j, const gtsam::PinholeCamera& camera); + void insert_or_assign(size_t j, const gtsam::PinholeCamera& camera); + void insert_or_assign(size_t j, const gtsam::PinholeCamera& camera); + void insert_or_assign(size_t j, const gtsam::PinholeCamera& camera); + void insert_or_assign(size_t j, const gtsam::PinholeCamera& camera); + void insert_or_assign(size_t j, const gtsam::PinholeCamera& camera); + void insert_or_assign(size_t j, const gtsam::PinholePose& camera); + void insert_or_assign(size_t j, const gtsam::PinholePose& camera); + void insert_or_assign(size_t j, const gtsam::PinholePose& camera); + void insert_or_assign(size_t j, const gtsam::PinholePose& camera); + void insert_or_assign(size_t j, const gtsam::PinholePose& camera); + void insert_or_assign(size_t j, const gtsam::PinholePose& 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::PinholeCamera, + gtsam::EssentialMatrix, + gtsam::FundamentalMatrix, + gtsam::SimpleFundamentalMatrix, gtsam::PinholeCamera, + gtsam::PinholeCamera, + gtsam::PinholeCamera, + gtsam::PinholeCamera, gtsam::PinholeCamera, gtsam::PinholeCamera, - gtsam::PinholePose, gtsam::PinholePose, + gtsam::PinholePose, + gtsam::PinholePose, + gtsam::PinholePose, gtsam::PinholePose, gtsam::PinholePose, gtsam::imuBias::ConstantBias, diff --git a/gtsam/sfm/TransferFactor.h b/gtsam/sfm/TransferFactor.h index d2085f57ea..c8b249ddcb 100644 --- a/gtsam/sfm/TransferFactor.h +++ b/gtsam/sfm/TransferFactor.h @@ -16,12 +16,66 @@ #pragma once #include +#include #include #include +#include #include +#include + namespace gtsam { +/** + * Base class that holds the EdgeKeys and provides the getMatrices method. + */ +template +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 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 -class TransferFactor : public NoiseModelFactorN { - EdgeKey key1_, key2_; ///< the two EdgeKeys - std::vector> - triplets_; ///< Point triplets - +class TransferFactor : public NoiseModelFactorN, public TransferEdges { 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(model, key1, key2), - key1_(key1), - key2_(key2), - triplets_({std::make_tuple(pa, pb, pc)}) {} + using Base = NoiseModelFactorN; + using Triplet = std::tuple; + + protected: + std::vector triplets_; ///< Point triplets. + public: /** * @brief Constructor that accepts a vector of point triplets. * - * @param key1 First EdgeKey specifying F1: (a, c) or (c, a). - * @param key2 Second EdgeKey specifying F2: (b, c) or (c, b). + * @param edge1 First EdgeKey specifying F1: (a, c) or (c, a). + * @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( - EdgeKey key1, EdgeKey key2, - const std::vector>& triplets, - const SharedNoiseModel& model = nullptr) - : NoiseModelFactorN(model, key1, key2), - key1_(key1), - key2_(key2), + TransferFactor(EdgeKey edge1, EdgeKey edge2, + const std::vector& triplets, + const SharedNoiseModel& model = nullptr) + : Base(model, edge1, edge2), + TransferEdges(edge1, edge2), triplets_(triplets) {} - // Create Matrix3 objects based on EdgeKey configurations - std::pair 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 of errors returns 2*N vector. Vector evaluateError(const F& F1, const F& F2, OptionalMatrixType H1 = nullptr, OptionalMatrixType H2 = nullptr) const override { - std::function transfer = [&](const F& F1, - const F& F2) { + std::function errorFunction = [&](const F& f1, + const F& f2) { Vector errors(2 * triplets_.size()); size_t idx = 0; - auto [Fca, Fcb] = getMatrices(F1, F2); - for (const auto& tuple : triplets_) { - const auto& [pa, pb, pc] = tuple; - Point2 transferredPoint = EpipolarTransfer(Fca, pa, Fcb, pb); - Vector2 error = transferredPoint - pc; - errors.segment<2>(idx) = error; + auto [Fca, Fcb] = this->getMatrices(f1, f2); + for (const auto& [pa, pb, pc] : triplets_) { + errors.segment<2>(idx) = EpipolarTransfer(Fca, pa, Fcb, pb) - pc; idx += 2; } return errors; }; - if (H1) *H1 = numericalDerivative21(transfer, F1, F2); - if (H2) *H2 = numericalDerivative22(transfer, F1, F2); - return transfer(F1, F2); + + if (H1) *H1 = numericalDerivative21(errorFunction, 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 +class EssentialTransferFactor : public TransferFactor { + using EM = EssentialMatrix; + using Triplet = std::tuple; + std::shared_ptr calibration_; ///< Shared pointer to calibration object + + public: + using Base = TransferFactor; + using This = EssentialTransferFactor; + using shared_ptr = std::shared_ptr; + + /** + * @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& triplets, + const std::shared_ptr& 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 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 +class EssentialTransferFactorK + : public NoiseModelFactorN, + TransferEdges { + using EM = EssentialMatrix; + using Triplet = std::tuple; + std::vector triplets_; ///< Point triplets + + public: + using Base = NoiseModelFactorN; + using This = EssentialTransferFactorK; + using shared_ptr = std::shared_ptr; + + /** + * @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& 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(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 + 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 \ No newline at end of file diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 142e65d7e1..a15b73ea19 100644 --- a/gtsam/sfm/sfm.i +++ b/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 +#include +template +virtual class TransferFactor : gtsam::NoiseModelFactor { + TransferFactor(gtsam::EdgeKey edge1, gtsam::EdgeKey edge2, + const std::vector>& triplets, + const gtsam::noiseModel::Base* model = nullptr); +}; + +#include +#include +#include +template +virtual class EssentialTransferFactor : gtsam::NoiseModelFactor { + EssentialTransferFactor(gtsam::EdgeKey edge1, gtsam::EdgeKey edge2, + const std::vector>& triplets, + const K* calibration, + const gtsam::noiseModel::Base* model = nullptr); +}; + +template +virtual class EssentialTransferFactorK : gtsam::NoiseModelFactor { + EssentialTransferFactorK(gtsam::EdgeKey edge1, gtsam::EdgeKey edge2, + const std::vector>& triplets, + const gtsam::noiseModel::Base* model = nullptr); +}; + #include virtual class ShonanFactor3 : gtsam::NoiseModelFactor { diff --git a/gtsam/sfm/tests/testTransferFactor.cpp b/gtsam/sfm/tests/testTransferFactor.cpp index 2dca12c2ac..ced3d2ce7c 100644 --- a/gtsam/sfm/tests/testTransferFactor.cpp +++ b/gtsam/sfm/tests/testTransferFactor.cpp @@ -1,19 +1,23 @@ /* * @file testTransferFactor.cpp - * @brief Test TransferFactor class - * @author Your Name - * @date October 23, 2024 + * @brief Test TransferFactor classes + * @author Frank Dellaert + * @date October 2024 */ #include +#include #include #include #include +#include + 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 generateCameraPoses() { std::array cameraPoses; const double radius = 1.0; @@ -48,8 +52,12 @@ std::array cameraPoses = generateCameraPoses(); auto triplet = generateTripleF(cameraPoses); double focalLength = 1000; Point2 principalPoint(640 / 2, 480 / 2); -const Cal3_S2 K(focalLength, focalLength, 0.0, // - principalPoint.x(), principalPoint.y()); +const Cal3_S2 cal(focalLength, focalLength, 0.0, // + principalPoint.x(), principalPoint.y()); +// Create cameras +auto f = [](const Pose3& pose) { return PinholeCamera(pose, cal); }; +std::array, 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 p; for (size_t i = 0; i < 3; ++i) { - // Project the point into each camera - PinholeCameraCal3_S2 camera(cameraPoses[i], K); - p[i] = camera.project(P); + p[i] = cameras[i].project(P); } // Create a TransferFactor EdgeKey key01(0, 1), key12(1, 2), key20(2, 0); TransferFactor // - factor0{key01, key20, p[1], p[2], p[0]}, - factor1{key12, key01, p[2], p[0], p[1]}, - factor2{key20, key12, p[0], p[1], p[2]}; + factor0{key01, key20, {{p[1], p[2], p[0]}}}, + factor1{key12, key01, {{p[2], p[0], p[1]}}}, + 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 points3D; std::vector> 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 p; for (size_t i = 0; i < 3; ++i) { - // Project the point into each camera - PinholeCameraCal3_S2 camera(cameraPoses[i], K); - p[i] = camera.project(P); + p[i] = cameras[i].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 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(cal); + EssentialTransferFactor 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 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 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); } -//************************************************************************* +//************************************************************************* \ No newline at end of file diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 9070d191b9..f0fc3f796c 100644 --- a/python/CMakeLists.txt +++ b/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) diff --git a/python/README.md b/python/README.md index e81be74fcd..b80c952614 100644 --- a/python/README.md +++ b/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 /python/dev_requirements.txt diff --git a/python/gtsam/examples/EssentialViewGraphExample.py b/python/gtsam/examples/EssentialViewGraphExample.py new file mode 100644 index 0000000000..184d3f7e3e --- /dev/null +++ b/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() diff --git a/python/gtsam/examples/SFMExample.py b/python/gtsam/examples/SFMExample.py index c8d1f1271c..d8d0ae1dfe 100644 --- a/python/gtsam/examples/SFMExample.py +++ b/python/gtsam/examples/SFMExample.py @@ -9,20 +9,22 @@ 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( - measurement, measurement_noise, X(i), L(j), K) + factor = GenericProjectionFactorCal3_S2(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') - print('initial error = {}'.format(graph.error(initial_estimate))) - print('final error = {}'.format(graph.error(result))) + result.print("Final results:\n") + print("initial error = {}".format(graph.error(initial_estimate))) + 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() diff --git a/python/gtsam/examples/ViewGraphComparison.py b/python/gtsam/examples/ViewGraphComparison.py new file mode 100644 index 0000000000..1eb43cec88 --- /dev/null +++ b/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() diff --git a/python/gtsam/examples/ViewGraphExample.py b/python/gtsam/examples/ViewGraphExample.py new file mode 100644 index 0000000000..2dc41ec4bc --- /dev/null +++ b/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()