diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index 7e008aec3..1520e596a 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 725c1481f..081688d48 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 000000000..319155dc9 --- /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 000000000..e30da688c --- /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 + 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 a90de48e5..6421ce264 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; diff --git a/gtsam/geometry/tests/testCal3Bundler.cpp b/gtsam/geometry/tests/testCal3Bundler.cpp index 020cab2f9..15e633923 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 000000000..d21d39f7f --- /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