diff --git a/gtsam/geometry/Cal3.cpp b/gtsam/geometry/Cal3.cpp new file mode 100644 index 000000000..41de47f46 --- /dev/null +++ b/gtsam/geometry/Cal3.cpp @@ -0,0 +1,75 @@ +/* ---------------------------------------------------------------------------- + + * 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 Cal3.cpp + * @brief Common code for all calibration models. + * @author Frank Dellaert + */ + +#include + +#include +#include +#include + +namespace gtsam { + +/* ************************************************************************* */ +Cal3::Cal3(double fov, int w, int h) + : s_(0), u0_((double)w / 2.0), v0_((double)h / 2.0) { + double a = fov * M_PI / 360.0; // fov/2 in radians + fx_ = double(w) / (2.0 * tan(a)); + fy_ = fx_; +} + +/* ************************************************************************* */ +Cal3::Cal3(const std::string& path) { + const auto buffer = path + std::string("/calibration_info.txt"); + std::ifstream infile(buffer, std::ios::in); + + if (infile && !infile.eof()) { + infile >> fx_ >> fy_ >> s_ >> u0_ >> v0_; + } else { + throw std::runtime_error("Cal3: Unable to load the calibration"); + } + + infile.close(); +} + +/* ************************************************************************* */ +std::ostream& operator<<(std::ostream& os, const Cal3& cal) { + os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew() + << ", px: " << cal.px() << ", py: " << cal.py(); + return os; +} + +/* ************************************************************************* */ +void Cal3::print(const std::string& s) const { gtsam::print((Matrix)K(), s); } + +/* ************************************************************************* */ +bool Cal3::equals(const Cal3& K, double tol) const { + return (std::fabs(fx_ - K.fx_) < tol && std::fabs(fy_ - K.fy_) < tol && + std::fabs(s_ - K.s_) < tol && std::fabs(u0_ - K.u0_) < tol && + std::fabs(v0_ - K.v0_) < tol); +} + +Matrix3 Cal3::inverse() const { + const double fxy = fx_ * fy_, sv0 = s_ * v0_, fyu0 = fy_ * u0_; + Matrix3 K_inverse; + K_inverse << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0, 1.0 / fy_, + -v0_ / fy_, 0.0, 0.0, 1.0; + return K_inverse; +} + +/* ************************************************************************* */ + +} // \ namespace gtsam diff --git a/gtsam/geometry/Cal3.h b/gtsam/geometry/Cal3.h index d9e12f7d2..74c9868f3 100644 --- a/gtsam/geometry/Cal3.h +++ b/gtsam/geometry/Cal3.h @@ -30,7 +30,7 @@ namespace gtsam { * Jacobians of `calibrate` using `uncalibrate`. * This is useful when there are iterative operations in the `calibrate` * function which make computing jacobians difficult. - * + * * Given f(pi, pn) = uncalibrate(pn) - pi, and g(pi) = calibrate, we can * easily compute the Jacobians: * df/pi = -I (pn and pi are independent args) @@ -61,6 +61,143 @@ void calibrateJacobians(const Cal& calibration, const Point2& pn, } } -//TODO(Varun) Make common base class for all calibration models. +/** + * @brief Common base class for all calibration models. + * @addtogroup geometry + * \nosubgrouping + */ +class GTSAM_EXPORT Cal3 { + protected: + double fx_ = 1.0f, fy_ = 1.0f; ///< focal length + double s_ = 0.0f; ///< skew + double u0_ = 0.0f, v0_ = 0.0f; ///< principal point + + public: + enum { dimension = 5 }; + ///< shared pointer to calibration object + using shared_ptr = boost::shared_ptr; + + /// @name Standard Constructors + /// @{ + + /// Create a default calibration that leaves coordinates unchanged + Cal3() = default; + + /// constructor from doubles + Cal3(double fx, double fy, double s, double u0, double v0) + : fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0) {} + + /// constructor from vector + Cal3(const Vector5& d) + : fx_(d(0)), fy_(d(1)), s_(d(2)), u0_(d(3)), v0_(d(4)) {} + + /** + * Easy constructor, takes fov in degrees, asssumes zero skew, unit aspect + * @param fov field of view in degrees + * @param w image width + * @param h image height + */ + Cal3(double fov, int w, int h); + + /// @} + /// @name Advanced Constructors + /// @{ + + /** + * Load calibration parameters from `calibration_info.txt` file located in + * `path` directory. + * + * The contents of calibration file should be the 5 parameters in order: + * `fx, fy, s, u0, v0` + * + * @param path path to directory containing `calibration_info.txt`. + */ + Cal3(const std::string& path); + + /// @} + /// @name Testable + /// @{ + + /// Output stream operator + GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, + const Cal3& cal); + + /// print with optional string + virtual void print(const std::string& s = "") const; + + /// Check if equal up to specified tolerance + bool equals(const Cal3& K, double tol = 10e-9) const; + + /// @} + /// @name Standard Interface + /// @{ + + /// focal length x + inline double fx() const { return fx_; } + + /// focal length y + inline double fy() const { return fy_; } + + /// aspect ratio + inline double aspectRatio() const { return fx_ / fy_; } + + /// skew + inline double skew() const { return s_; } + + /// image center in x + inline double px() const { return u0_; } + + /// image center in y + inline double py() const { return v0_; } + + /// return the principal point + Point2 principalPoint() const { return Point2(u0_, v0_); } + + /// vectorized form (column-wise) + Vector5 vector() const { + Vector5 v; + v << fx_, fy_, s_, u0_, v0_; + return v; + } + + /// return calibration matrix K + virtual Matrix3 K() const { + Matrix3 K; + K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0; + return K; + } + +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 + /** @deprecated The following function has been deprecated, use K above */ + Matrix3 matrix() const { return K(); } +#endif + + /// Return inverted calibration matrix inv(K) + Matrix3 inverse() const; + + /// return DOF, dimensionality of tangent space + inline virtual size_t dim() const { return Dim(); } + + /// return DOF, dimensionality of tangent space + inline static size_t Dim() { return dimension; } + + /// @} + /// @name Advanced Interface + /// @{ + + private: + /// 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(fy_); + ar& BOOST_SERIALIZATION_NVP(s_); + ar& BOOST_SERIALIZATION_NVP(u0_); + ar& BOOST_SERIALIZATION_NVP(v0_); + } + + /// @} +}; } // \ namespace gtsam diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index a73bfec52..e03562452 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -15,28 +15,19 @@ * @author ydjian */ -#include #include +#include +#include #include #include -#include namespace gtsam { -/* ************************************************************************* */ -Cal3Bundler::Cal3Bundler() : - f_(1), k1_(0), k2_(0), u0_(0), v0_(0), tol_(1e-5) { -} - -/* ************************************************************************* */ -Cal3Bundler::Cal3Bundler(double f, double k1, double k2, double u0, double v0, - double tol) - : f_(f), k1_(k1), k2_(k2), u0_(u0), v0_(v0), tol_(tol) {} - /* ************************************************************************* */ Matrix3 Cal3Bundler::K() const { + // This function is needed to ensure skew = 0; Matrix3 K; - K << f_, 0, u0_, 0, f_, v0_, 0, 0, 1; + K << fx_, 0, u0_, 0, fy_, v0_, 0, 0, 1.0; return K; } @@ -48,35 +39,42 @@ Vector4 Cal3Bundler::k() const { } /* ************************************************************************* */ -Vector3 Cal3Bundler::vector() const { - return Vector3(f_, k1_, k2_); +Vector3 Cal3Bundler::vector() const { return Vector3(fx_, k1_, k2_); } + +/* ************************************************************************* */ +std::ostream& operator<<(std::ostream& os, const Cal3Bundler& cal) { + os << "f: " << cal.fx() << ", k1: " << cal.k1() << ", k2: " << cal.k2() + << ", px: " << cal.px() << ", py: " << cal.py(); + return os; } /* ************************************************************************* */ void Cal3Bundler::print(const std::string& s) const { - gtsam::print((Vector)(Vector(5) << f_, k1_, k2_, u0_, v0_).finished(), s + ".K"); + gtsam::print((Vector)(Vector(5) << fx_, k1_, k2_, u0_, v0_).finished(), + s + ".K"); } /* ************************************************************************* */ bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const { - if (std::abs(f_ - K.f_) > tol || std::abs(k1_ - K.k1_) > tol - || std::abs(k2_ - K.k2_) > tol || std::abs(u0_ - K.u0_) > tol - || std::abs(v0_ - K.v0_) > tol) - return false; - return true; + 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); } /* ************************************************************************* */ -Point2 Cal3Bundler::uncalibrate(const Point2& p, // - OptionalJacobian<2, 3> Dcal, OptionalJacobian<2, 2> Dp) const { - // r = x^2 + y^2; - // g = (1 + k(1)*r + k(2)*r^2); +Point2 Cal3Bundler::uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal, + OptionalJacobian<2, 2> Dp) const { + // r = x² + y²; + // g = (1 + k(1)*r + k(2)*r²); // pi(:,i) = g * pn(:,i) const double x = p.x(), y = p.y(); const double r = x * x + y * y; const double g = 1. + (k1_ + k2_ * r) * r; const double u = g * x, v = g * y; + const double f_ = fx_; + // Derivatives make use of intermediate variables above if (Dcal) { double rx = r * x, ry = r * y; @@ -94,23 +92,22 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, // } /* ************************************************************************* */ -Point2 Cal3Bundler::calibrate(const Point2& pi, - OptionalJacobian<2, 3> Dcal, +Point2 Cal3Bundler::calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal, OptionalJacobian<2, 2> Dp) const { - // Copied from Cal3DS2 :-( - // but specialized with k1,k2 non-zero only and fx=fy and s=0 - double x = (pi.x() - u0_)/f_, y = (pi.y() - v0_)/f_; + // Copied from Cal3DS2 + // but specialized with k1, k2 non-zero only and fx=fy and s=0 + double x = (pi.x() - u0_) / fx_, y = (pi.y() - v0_) / fx_; const Point2 invKPi(x, y); - // initialize by ignoring the distortion at all, might be problematic for pixels around boundary + // initialize by ignoring the distortion at all, might be problematic for + // pixels around boundary Point2 pn(x, y); // iterate until the uncalibrate is close to the actual pixel coordinate const int maxIterations = 10; int iteration; for (iteration = 0; iteration < maxIterations; ++iteration) { - if (distance2(uncalibrate(pn), pi) <= tol_) - break; + if (distance2(uncalibrate(pn), pi) <= tol_) break; const double px = pn.x(), py = pn.y(), xx = px * px, yy = py * py; const double rr = xx + yy; const double g = (1 + k1_ * rr + k2_ * rr * rr); @@ -119,7 +116,8 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, if (iteration >= maxIterations) throw std::runtime_error( - "Cal3Bundler::calibrate fails to converge. need a better initialization"); + "Cal3Bundler::calibrate fails to converge. need a better " + "initialization"); calibrateJacobians(*this, pn, Dcal, Dp); @@ -150,14 +148,4 @@ Matrix25 Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const { return H; } -/* ************************************************************************* */ -Cal3Bundler Cal3Bundler::retract(const Vector& d) const { - return Cal3Bundler(f_ + d(0), k1_ + d(1), k2_ + d(2), u0_, v0_); -} - -/* ************************************************************************* */ -Vector3 Cal3Bundler::localCoordinates(const Cal3Bundler& T2) const { - return T2.vector() - vector(); -} - -} +} // \ namespace gtsam diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index 8c836b504..0016ded2d 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -14,6 +14,7 @@ * @brief Calibration used by Bundler * @date Sep 25, 2010 * @author Yong Dian Jian + * @author Varun Agrawal */ #pragma once @@ -28,23 +29,23 @@ namespace gtsam { * @addtogroup geometry * \nosubgrouping */ -class GTSAM_EXPORT Cal3Bundler { +class GTSAM_EXPORT Cal3Bundler : public Cal3 { + private: + double k1_ = 0.0f, k2_ = 0.0f; ///< radial distortion + double tol_ = 1e-5; ///< tolerance value when calibrating -private: - double f_; ///< focal length - double k1_, k2_; ///< radial distortion - double u0_, v0_; ///< image center, not a parameter to be optimized but a constant - double tol_; ///< tolerance value when calibrating - -public: + // 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. + public: enum { dimension = 3 }; /// @name Standard Constructors /// @{ /// Default constructor - Cal3Bundler(); + Cal3Bundler() = default; /** * Constructor @@ -56,7 +57,8 @@ public: * @param tol optional calibration tolerance value */ Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0, - double tol = 1e-5); + double tol = 1e-5) + : Cal3(f, f, 0, u0, v0), k1_(k1), k2_(k2), tol_(tol) {} virtual ~Cal3Bundler() {} @@ -64,8 +66,12 @@ public: /// @name Testable /// @{ + /// Output stream operator + GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, + const Cal3Bundler& cal); + /// print with optional string - void print(const std::string& s = "") const; + void print(const std::string& s = "") const override; /// assert equality up to a tolerance bool equals(const Cal3Bundler& K, double tol = 10e-9) const; @@ -74,64 +80,41 @@ public: /// @name Standard Interface /// @{ - Matrix3 K() const; ///< Standard 3*3 calibration matrix - Vector4 k() const; ///< Radial distortion parameters (4 of them, 2 0) + /// distorsion parameter k1 + inline double k1() const { return k1_; } + + /// distorsion 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) Vector3 vector() const; - /// focal length x - inline double fx() const { - return f_; - } - - /// focal length y - inline double fy() const { - return f_; - } - - /// distorsion parameter k1 - inline double k1() const { - return k1_; - } - - /// distorsion 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_; - } - #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 /// get parameter u0 - inline double u0() const { - return u0_; - } + inline double u0() const { return u0_; } /// get parameter v0 - inline double v0() const { - return v0_; - } + inline double v0() const { return v0_; } #endif - /** * @brief: convert intrinsic coordinates xy to image coordinates uv * Version of uncalibrate with derivatives * @param p point in intrinsic coordinates - * @param Dcal optional 2*3 Jacobian wrpt CalBundler parameters + * @param Dcal optional 2*3 Jacobian wrpt Cal3Bundler parameters * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in image coordinates */ Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = boost::none, - OptionalJacobian<2, 2> Dp = boost::none) const; + OptionalJacobian<2, 2> Dp = boost::none) const; /** * Convert a pixel coordinate to ideal coordinate xy @@ -141,8 +124,7 @@ public: * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in intrinsic coordinates */ - Point2 calibrate(const Point2& pi, - OptionalJacobian<2, 3> Dcal = boost::none, + Point2 calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal = boost::none, OptionalJacobian<2, 2> Dp = boost::none) const; /// @deprecated might be removed in next release, use uncalibrate @@ -158,48 +140,45 @@ public: /// @name Manifold /// @{ + /// return DOF, dimensionality of tangent space + virtual 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 - Cal3Bundler retract(const Vector& d) const; + inline Cal3Bundler retract(const Vector& d) const { + return Cal3Bundler(fx_ + d(0), k1_ + d(1), k2_ + d(2), u0_, v0_); + } /// Calculate local coordinates to another calibration - Vector3 localCoordinates(const Cal3Bundler& T2) const; - - /// dimensionality - virtual size_t dim() const { - return 3; + Vector3 localCoordinates(const Cal3Bundler& T2) const { + return T2.vector() - vector(); } - /// dimensionality - static size_t Dim() { - return 3; - } - -private: - + private: /// @} /// @name Advanced Interface /// @{ /** Serialization function */ friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_NVP(f_); - ar & BOOST_SERIALIZATION_NVP(k1_); - ar & BOOST_SERIALIZATION_NVP(k2_); - ar & BOOST_SERIALIZATION_NVP(u0_); - ar & BOOST_SERIALIZATION_NVP(v0_); - ar & BOOST_SERIALIZATION_NVP(tol_); + template + void serialize(Archive& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "Cal3Bundler", boost::serialization::base_object(*this)); + ar& BOOST_SERIALIZATION_NVP(k1_); + ar& BOOST_SERIALIZATION_NVP(k2_); + ar& BOOST_SERIALIZATION_NVP(tol_); } /// @} - }; -template<> +template <> struct traits : public internal::Manifold {}; -template<> +template <> struct traits : public internal::Manifold {}; -} // namespace gtsam +} // namespace gtsam diff --git a/gtsam/geometry/Cal3DS2.cpp b/gtsam/geometry/Cal3DS2.cpp index 070d16c6c..f93386ea7 100644 --- a/gtsam/geometry/Cal3DS2.cpp +++ b/gtsam/geometry/Cal3DS2.cpp @@ -13,28 +13,30 @@ * @file Cal3DS2.cpp * @date Feb 28, 2010 * @author ydjian + * @author Varun Agrawal */ -#include #include +#include +#include #include #include -#include namespace gtsam { /* ************************************************************************* */ -void Cal3DS2::print(const std::string& s_) const { - Base::print(s_); +std::ostream& operator<<(std::ostream& os, const Cal3DS2& cal) { + os << (Cal3DS2_Base&)cal; + return os; } +/* ************************************************************************* */ +void Cal3DS2::print(const std::string& s_) const { Base::print(s_); } + /* ************************************************************************* */ bool Cal3DS2::equals(const Cal3DS2& K, double tol) const { - if (std::abs(fx_ - K.fx_) > tol || std::abs(fy_ - K.fy_) > tol || std::abs(s_ - K.s_) > tol || - std::abs(u0_ - K.u0_) > tol || std::abs(v0_ - K.v0_) > tol || std::abs(k1_ - K.k1_) > tol || - std::abs(k2_ - K.k2_) > tol || std::abs(p1_ - K.p1_) > tol || std::abs(p2_ - K.p2_) > tol) - return false; - return true; + const Cal3DS2_Base* base = dynamic_cast(&K); + return Cal3DS2_Base::equals(*base, tol); } /* ************************************************************************* */ @@ -46,8 +48,5 @@ Cal3DS2 Cal3DS2::retract(const Vector& d) const { Vector Cal3DS2::localCoordinates(const Cal3DS2& T2) const { return T2.vector() - vector(); } - } /* ************************************************************************* */ - - diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index e66c3d124..58d35c2ec 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -11,9 +11,11 @@ /** * @file Cal3DS2.h - * @brief Calibration of a camera with radial distortion, calculations in base class Cal3DS2_Base + * @brief Calibration of a camera with radial distortion, calculations in base + * class Cal3DS2_Base * @date Feb 28, 2010 * @author ydjian + * @autho Varun Agrawal */ #pragma once @@ -30,22 +32,20 @@ namespace gtsam { * \nosubgrouping */ class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base { + using Base = Cal3DS2_Base; - typedef Cal3DS2_Base Base; - -public: - + public: enum { dimension = 9 }; /// @name Standard Constructors /// @{ /// Default Constructor with only unit focal length - Cal3DS2() : Base() {} + Cal3DS2() = default; - Cal3DS2(double fx, double fy, double s, double u0, double v0, - double k1, double k2, double p1 = 0.0, double p2 = 0.0, double tol = 1e-5) : - Base(fx, fy, s, u0, v0, k1, k2, p1, p2, tol) {} + Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, + double k2, double p1 = 0.0, double p2 = 0.0, double tol = 1e-5) + : Base(fx, fy, s, u0, v0, k1, k2, p1, p2, tol) {} virtual ~Cal3DS2() {} @@ -53,12 +53,16 @@ public: /// @name Advanced Constructors /// @{ - Cal3DS2(const Vector &v) : Base(v) {} + Cal3DS2(const Vector9& v) : Base(v) {} /// @} /// @name Testable /// @{ + /// Output stream operator + GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, + const Cal3DS2& cal); + /// print with optional string void print(const std::string& s = "") const override; @@ -70,16 +74,16 @@ public: /// @{ /// Given delta vector, update calibration - Cal3DS2 retract(const Vector& d) const ; + Cal3DS2 retract(const Vector& d) const; /// Given a different calibration, calculate update to obtain it - Vector localCoordinates(const Cal3DS2& T2) const ; + Vector localCoordinates(const Cal3DS2& T2) const; /// Return dimensions of calibration manifold object - virtual size_t dim() const { return dimension ; } + virtual size_t dim() const override { return Dim(); } /// Return dimensions of calibration manifold object - static size_t Dim() { return dimension; } + inline static size_t Dim() { return dimension; } /// @} /// @name Clone @@ -92,30 +96,24 @@ public: /// @} - -private: - + private: /// @name Advanced Interface /// @{ /** Serialization function */ friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int /*version*/) - { - ar & boost::serialization::make_nvp("Cal3DS2", - boost::serialization::base_object(*this)); + template + void serialize(Archive& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "Cal3DS2", boost::serialization::base_object(*this)); } /// @} - }; -template<> +template <> struct traits : public internal::Manifold {}; -template<> +template <> struct traits : public internal::Manifold {}; - } - diff --git a/gtsam/geometry/Cal3DS2_Base.cpp b/gtsam/geometry/Cal3DS2_Base.cpp index d175259f2..a3f7026b9 100644 --- a/gtsam/geometry/Cal3DS2_Base.cpp +++ b/gtsam/geometry/Cal3DS2_Base.cpp @@ -16,33 +16,14 @@ * @author Varun Agrawal */ -#include #include +#include +#include #include #include -#include namespace gtsam { -/* ************************************************************************* */ -Cal3DS2_Base::Cal3DS2_Base(const Vector& v) - : fx_(v(0)), - fy_(v(1)), - s_(v(2)), - u0_(v(3)), - v0_(v(4)), - k1_(v(5)), - k2_(v(6)), - p1_(v(7)), - p2_(v(8)) {} - -/* ************************************************************************* */ -Matrix3 Cal3DS2_Base::K() const { - Matrix3 K; - K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0; - return K; -} - /* ************************************************************************* */ Vector9 Cal3DS2_Base::vector() const { Vector9 v; @@ -50,6 +31,14 @@ Vector9 Cal3DS2_Base::vector() const { return v; } +/* ************************************************************************* */ +std::ostream& operator<<(std::ostream& os, const Cal3DS2_Base& cal) { + os << (Cal3&)cal; + os << ", k1: " << cal.k1() << ", k2: " << cal.k2() << ", p1: " << cal.p1() + << ", p2: " << cal.p2(); + return os; +} + /* ************************************************************************* */ void Cal3DS2_Base::print(const std::string& s_) const { gtsam::print((Matrix)K(), s_ + ".K"); @@ -58,31 +47,30 @@ void Cal3DS2_Base::print(const std::string& s_) const { /* ************************************************************************* */ bool Cal3DS2_Base::equals(const Cal3DS2_Base& K, double tol) const { - if (std::abs(fx_ - K.fx_) > tol || std::abs(fy_ - K.fy_) > tol || std::abs(s_ - K.s_) > tol || - std::abs(u0_ - K.u0_) > tol || std::abs(v0_ - K.v0_) > tol || std::abs(k1_ - K.k1_) > tol || - std::abs(k2_ - K.k2_) > tol || std::abs(p1_ - K.p1_) > tol || std::abs(p2_ - K.p2_) > tol) - return false; - return true; + 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(p1_ - K.p1_) < tol && + std::fabs(p2_ - K.p2_) < tol; } /* ************************************************************************* */ -static Matrix29 D2dcalibration(double x, double y, double xx, - double yy, double xy, double rr, double r4, double pnx, double pny, - const Matrix2& DK) { +static Matrix29 D2dcalibration(double x, double y, double xx, double yy, + double xy, double rr, double r4, double pnx, + double pny, const Matrix2& DK) { Matrix25 DR1; DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0; Matrix24 DR2; - DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, // - y * rr, y * r4, rr + 2 * yy, 2 * xy; + DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, // + y * rr, y * r4, rr + 2 * yy, 2 * xy; Matrix29 D; D << DR1, DK * DR2; return D; } /* ************************************************************************* */ -static Matrix2 D2dintrinsic(double x, double y, double rr, - double g, double k1, double k2, double p1, double p2, - const Matrix2& DK) { +static Matrix2 D2dintrinsic(double x, double y, double rr, double g, double k1, + double k2, double p1, double p2, + const Matrix2& DK) { const double drdx = 2. * x; const double drdy = 2. * y; const double dgdx = k1 * drdx + k2 * 2. * rr * drdx; @@ -96,8 +84,8 @@ static Matrix2 D2dintrinsic(double x, double y, double rr, const double dDydy = 2. * p2 * x + p1 * (drdy + 4. * y); Matrix2 DR; - DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, // - y * dgdx + dDydx, g + y * dgdy + dDydy; + DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, // + y * dgdx + dDydx, g + y * dgdy + dDydy; return DK * DR; } @@ -105,14 +93,14 @@ static Matrix2 D2dintrinsic(double x, double y, double rr, /* ************************************************************************* */ Point2 Cal3DS2_Base::uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal, OptionalJacobian<2, 2> Dp) const { - // rr = x^2 + y^2; - // g = (1 + k(1)*rr + k(2)*rr^2); - // dp = [2*k(3)*x*y + k(4)*(rr + 2*x^2); 2*k(4)*x*y + k(3)*(rr + 2*y^2)]; + // r² = x² + y²; + // g = (1 + k(1)*r² + k(2)*r⁴); + // dp = [2*k(3)*x*y + k(4)*(r² + 2*x²); 2*k(4)*x*y + k(3)*(r² + 2*y²)]; // pi(:,i) = g * pn(:,i) + dp; const double x = p.x(), y = p.y(), xy = x * y, xx = x * x, yy = y * y; const double rr = xx + yy; const double r4 = rr * rr; - const double g = 1. + k1_ * rr + k2_ * r4; // scaling factor + const double g = 1. + k1_ * rr + k2_ * r4; // scaling factor // tangential component const double dx = 2. * p1_ * xy + p2_ * (rr + 2. * xx); @@ -202,8 +190,5 @@ Matrix29 Cal3DS2_Base::D2d_calibration(const Point2& p) const { DK << fx_, s_, 0.0, fy_; return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK); } - } /* ************************************************************************* */ - - diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h index dbd6478e1..23e138838 100644 --- a/gtsam/geometry/Cal3DS2_Base.h +++ b/gtsam/geometry/Cal3DS2_Base.h @@ -33,51 +33,34 @@ namespace gtsam { * http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html * but using only k1,k2,p1, and p2 coefficients. * K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ] - * rr = Pn.x^2 + Pn.y^2 - * \hat{Pn} = (1 + k1*rr + k2*rr^2 ) Pn + [ 2*p1 Pn.x Pn.y + p2 (rr + 2 Pn.x^2) ; - * p1 (rr + 2 Pn.y^2) + 2*p2 Pn.x Pn.y ] - * pi = K*Pn + * r² = P.x² + P.y² + * P̂ = (1 + k1*r² + k2*r⁴) P + [ (2*p1 P.x P.y) + p2 (r² + 2 Pn.x²) + * p1 (r² + 2 Pn.y²) + (2*p2 Pn.x Pn.y) ] + * pi = K*P̂ */ -class GTSAM_EXPORT Cal3DS2_Base { - -protected: - double fx_, fy_, s_, u0_, v0_; // focal length, skew and principal point - double k1_, k2_; // radial 2nd-order and 4th-order - double p1_, p2_; // tangential distortion - double tol_ = 1e-5; // tolerance value when calibrating - -public: +class GTSAM_EXPORT Cal3DS2_Base : public Cal3 { + protected: + double k1_ = 0.0f, k2_ = 0.0f; ///< radial 2nd-order and 4th-order + double p1_ = 0.0f, p2_ = 0.0f; ///< tangential distortion + double tol_ = 1e-5; ///< tolerance value when calibrating + public: enum { dimension = 9 }; /// @name Standard Constructors /// @{ - /// Default Constructor with only unit focal length - Cal3DS2_Base() - : fx_(1), - fy_(1), - s_(0), - u0_(0), - v0_(0), - k1_(0), - k2_(0), - p1_(0), - p2_(0), - tol_(1e-5) {} + /// Default Constructor with only unit focal length + Cal3DS2_Base() = default; - Cal3DS2_Base(double fx, double fy, double s, double u0, double v0, double k1, - double k2, double p1 = 0.0, double p2 = 0.0, double tol = 1e-5) - : fx_(fx), - fy_(fy), - s_(s), - u0_(u0), - v0_(v0), - k1_(k1), - k2_(k2), - p1_(p1), - p2_(p2), - tol_(tol) {} + Cal3DS2_Base(double fx, double fy, double s, double u0, double v0, double k1, + double k2, double p1 = 0.0, double p2 = 0.0, double tol = 1e-5) + : Cal3(fx, fy, s, u0, v0), + k1_(k1), + k2_(k2), + p1_(p1), + p2_(p2), + tol_(tol) {} virtual ~Cal3DS2_Base() {} @@ -85,14 +68,23 @@ public: /// @name Advanced Constructors /// @{ - Cal3DS2_Base(const Vector &v) ; + Cal3DS2_Base(const Vector9& v) + : Cal3(v(0), v(1), v(2), v(3), v(4)), + k1_(v(5)), + k2_(v(6)), + p1_(v(7)), + p2_(v(8)) {} /// @} /// @name Testable /// @{ + /// Output stream operator + GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, + const Cal3DS2_Base& cal); + /// print with optional string - virtual void print(const std::string& s = "") const; + void print(const std::string& s = "") const override; /// assert equality up to a tolerance bool equals(const Cal3DS2_Base& K, double tol = 1e-8) const; @@ -101,35 +93,17 @@ public: /// @name Standard Interface /// @{ - /// focal length x - inline double fx() const { return fx_;} - - /// focal length x - inline double fy() const { return fy_;} - - /// skew - inline double skew() const { return s_;} - - /// image center in x - inline double px() const { return u0_;} - - /// image center in y - inline double py() const { return v0_;} - /// First distortion coefficient - inline double k1() const { return k1_;} + inline double k1() const { return k1_; } /// Second distortion coefficient - inline double k2() const { return k2_;} + inline double k2() const { return k2_; } /// First tangential distortion coefficient - inline double p1() const { return p1_;} + inline double p1() const { return p1_; } /// Second tangential distortion coefficient - inline double p2() const { return p2_;} - - /// return calibration matrix -- not really applicable - Matrix3 K() const; + inline double p2() const { return p2_; } /// return distortion parameter vector Vector4 k() const { return Vector4(k1_, k2_, p1_, p2_); } @@ -152,10 +126,16 @@ public: OptionalJacobian<2, 2> Dp = boost::none) const; /// Derivative of uncalibrate wrpt intrinsic coordinates - Matrix2 D2d_intrinsic(const Point2& p) const ; + Matrix2 D2d_intrinsic(const Point2& p) const; /// Derivative of uncalibrate wrpt the calibration parameters - Matrix29 D2d_calibration(const Point2& p) const ; + Matrix29 D2d_calibration(const Point2& p) const; + + /// return DOF, dimensionality of tangent space + virtual size_t dim() const override { return Dim(); } + + /// return DOF, dimensionality of tangent space + inline static size_t Dim() { return dimension; } /// @} /// @name Clone @@ -168,31 +148,23 @@ public: /// @} -private: - + private: /// @name Advanced Interface /// @{ /** 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(fy_); - ar & BOOST_SERIALIZATION_NVP(s_); - ar & BOOST_SERIALIZATION_NVP(u0_); - ar & BOOST_SERIALIZATION_NVP(v0_); - ar & BOOST_SERIALIZATION_NVP(k1_); - ar & BOOST_SERIALIZATION_NVP(k2_); - ar & BOOST_SERIALIZATION_NVP(p1_); - ar & BOOST_SERIALIZATION_NVP(p2_); - ar & BOOST_SERIALIZATION_NVP(tol_); + template + void serialize(Archive& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "Cal3DS2_Base", boost::serialization::base_object(*this)); + ar& BOOST_SERIALIZATION_NVP(k1_); + ar& BOOST_SERIALIZATION_NVP(k2_); + ar& BOOST_SERIALIZATION_NVP(p1_); + ar& BOOST_SERIALIZATION_NVP(p2_); + ar& BOOST_SERIALIZATION_NVP(tol_); } /// @} - }; - } - diff --git a/gtsam/geometry/Cal3Fisheye.cpp b/gtsam/geometry/Cal3Fisheye.cpp index 1ed1826ad..b9e60acee 100644 --- a/gtsam/geometry/Cal3Fisheye.cpp +++ b/gtsam/geometry/Cal3Fisheye.cpp @@ -13,6 +13,7 @@ * @file Cal3Fisheye.cpp * @date Apr 8, 2020 * @author ghaggin + * @author Varun Agrawal */ #include @@ -23,18 +24,6 @@ namespace gtsam { -/* ************************************************************************* */ -Cal3Fisheye::Cal3Fisheye(const Vector9& v) - : fx_(v[0]), - fy_(v[1]), - s_(v[2]), - u0_(v[3]), - v0_(v[4]), - k1_(v[5]), - k2_(v[6]), - k3_(v[7]), - k4_(v[8]) {} - /* ************************************************************************* */ Vector9 Cal3Fisheye::vector() const { Vector9 v; @@ -42,13 +31,6 @@ Vector9 Cal3Fisheye::vector() const { return v; } -/* ************************************************************************* */ -Matrix3 Cal3Fisheye::K() const { - Matrix3 K; - K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0; - return K; -} - /* ************************************************************************* */ double Cal3Fisheye::Scaling(double r) { static constexpr double threshold = 1e-8; @@ -157,6 +139,14 @@ Point2 Cal3Fisheye::calibrate(const Point2& uv, OptionalJacobian<2, 9> Dcal, return pi; } +/* ************************************************************************* */ +std::ostream& operator<<(std::ostream& os, const Cal3Fisheye& cal) { + os << (Cal3&)cal; + os << ", k1: " << cal.k1() << ", k2: " << cal.k2() << ", k3: " << cal.k3() + << ", k4: " << cal.k4(); + return os; +} + /* ************************************************************************* */ void Cal3Fisheye::print(const std::string& s_) const { gtsam::print((Matrix)K(), s_ + ".K"); @@ -165,24 +155,12 @@ void Cal3Fisheye::print(const std::string& s_) const { /* ************************************************************************* */ bool Cal3Fisheye::equals(const Cal3Fisheye& K, double tol) const { - if (std::abs(fx_ - K.fx_) > tol || std::abs(fy_ - K.fy_) > tol || - std::abs(s_ - K.s_) > tol || std::abs(u0_ - K.u0_) > tol || - std::abs(v0_ - K.v0_) > tol || std::abs(k1_ - K.k1_) > tol || - std::abs(k2_ - K.k2_) > tol || std::abs(k3_ - K.k3_) > tol || - std::abs(k4_ - K.k4_) > tol) - return false; - return true; + 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(k3_ - K.k3_) < tol && + std::fabs(k4_ - K.k4_) < tol; } /* ************************************************************************* */ -Cal3Fisheye Cal3Fisheye::retract(const Vector& d) const { - return Cal3Fisheye(vector() + d); -} -/* ************************************************************************* */ -Vector Cal3Fisheye::localCoordinates(const Cal3Fisheye& T2) const { - return T2.vector() - vector(); -} - -} // namespace gtsam -/* ************************************************************************* */ +} // \ namespace gtsam diff --git a/gtsam/geometry/Cal3Fisheye.h b/gtsam/geometry/Cal3Fisheye.h index 77e122f21..a394d2000 100644 --- a/gtsam/geometry/Cal3Fisheye.h +++ b/gtsam/geometry/Cal3Fisheye.h @@ -38,40 +38,35 @@ namespace gtsam { * Intrinsic coordinates: * [x_i;y_i] = [x/z; y/z] * Distorted coordinates: - * r^2 = (x_i)^2 + (y_i)^2 + * r² = (x_i)² + (y_i)² * th = atan(r) - * th_d = th(1 + k1*th^2 + k2*th^4 + k3*th^6 + k4*th^8) + * th_d = th(1 + k1*th² + k2*th⁴ + k3*th⁶ + k4*th⁸) * [x_d; y_d] = (th_d / r)*[x_i; y_i] * Pixel coordinates: * K = [fx s u0; 0 fy v0 ;0 0 1] * [u; v; 1] = K*[x_d; y_d; 1] */ -class GTSAM_EXPORT Cal3Fisheye { +class GTSAM_EXPORT Cal3Fisheye : public Cal3 { private: - double fx_, fy_, s_, u0_, v0_; // focal length, skew and principal point - double k1_, k2_, k3_, k4_; // fisheye distortion coefficients - double tol_ = 1e-5; // tolerance value when calibrating + double k1_ = 0.0f, k2_ = 0.0f; ///< fisheye distortion coefficients + double k3_ = 0.0f, k4_ = 0.0f; ///< fisheye distortion coefficients + double tol_ = 1e-5; ///< tolerance value when calibrating public: enum { dimension = 9 }; - typedef boost::shared_ptr - shared_ptr; ///< shared pointer to fisheye calibration object + ///< shared pointer to fisheye calibration object + using shared_ptr = boost::shared_ptr; /// @name Standard Constructors /// @{ /// Default Constructor with only unit focal length - Cal3Fisheye() - : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), k3_(0), k4_(0), tol_(1e-5) {} + Cal3Fisheye() = default; Cal3Fisheye(const double fx, const double fy, const double s, const double u0, const double v0, const double k1, const double k2, const double k3, const double k4, double tol = 1e-5) - : fx_(fx), - fy_(fy), - s_(s), - u0_(u0), - v0_(v0), + : Cal3(fx, fy, s, u0, v0), k1_(k1), k2_(k2), k3_(k3), @@ -84,27 +79,17 @@ class GTSAM_EXPORT Cal3Fisheye { /// @name Advanced Constructors /// @{ - explicit Cal3Fisheye(const Vector9& v); + explicit Cal3Fisheye(const Vector9& v) + : Cal3(v(0), v(1), v(2), v(3), v(4)), + k1_(v(5)), + k2_(v(6)), + k3_(v(7)), + k4_(v(8)) {} /// @} /// @name Standard Interface /// @{ - /// focal length x - inline double fx() const { return fx_; } - - /// focal length x - inline double fy() const { return fy_; } - - /// skew - inline double skew() const { return s_; } - - /// image center in x - inline double px() const { return u0_; } - - /// image center in y - inline double py() const { return v0_; } - /// First distortion coefficient inline double k1() const { return k1_; } @@ -117,9 +102,6 @@ class GTSAM_EXPORT Cal3Fisheye { /// Second tangential distortion coefficient inline double k4() const { return k4_; } - /// return calibration matrix - Matrix3 K() const; - /// return distortion parameter vector Vector4 k() const { return Vector4(k1_, k2_, k3_, k4_); } @@ -133,16 +115,21 @@ class GTSAM_EXPORT Cal3Fisheye { * @brief convert intrinsic coordinates [x_i; y_i] to (distorted) image * coordinates [u; v] * @param p point in intrinsic coordinates - * @param Dcal optional 2*9 Jacobian wrpt intrinsic parameters (fx, fy, ..., - * k4) + * @param Dcal optional 2*9 Jacobian wrpt intrinsic parameters * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates (xi, yi) * @return point in (distorted) image coordinates */ Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none, OptionalJacobian<2, 2> Dp = boost::none) const; - /// Convert (distorted) image coordinates [u;v] to intrinsic coordinates [x_i, - /// y_i] + /** + * Convert (distorted) image coordinates [u;v] to intrinsic coordinates [x_i, + * y_i] + * @param p point in image coordinates + * @param Dcal optional 2*9 Jacobian wrpt intrinsic parameters + * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates (xi, yi) + * @return point in intrinsic coordinates + */ Point2 calibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none, OptionalJacobian<2, 2> Dp = boost::none) const; @@ -150,8 +137,12 @@ class GTSAM_EXPORT Cal3Fisheye { /// @name Testable /// @{ + /// Output stream operator + GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, + const Cal3Fisheye& cal); + /// print with optional string - virtual void print(const std::string& s = "") const; + virtual void print(const std::string& s = "") const override; /// assert equality up to a tolerance bool equals(const Cal3Fisheye& K, double tol = 10e-9) const; @@ -160,17 +151,21 @@ class GTSAM_EXPORT Cal3Fisheye { /// @name Manifold /// @{ + /// Return dimensions of calibration manifold object + virtual size_t dim() const override { return Dim(); } + + /// Return dimensions of calibration manifold object + inline static size_t Dim() { return dimension; } + /// Given delta vector, update calibration - Cal3Fisheye retract(const Vector& d) const; + inline Cal3Fisheye retract(const Vector& d) const { + return Cal3Fisheye(vector() + d); + } /// Given a different calibration, calculate update to obtain it - Vector localCoordinates(const Cal3Fisheye& T2) const; - - /// Return dimensions of calibration manifold object - virtual size_t dim() const { return 9; } - - /// Return dimensions of calibration manifold object - static size_t Dim() { return 9; } + Vector localCoordinates(const Cal3Fisheye& T2) const { + return T2.vector() - vector(); + } /// @} /// @name Clone @@ -191,11 +186,8 @@ class GTSAM_EXPORT Cal3Fisheye { friend class boost::serialization::access; template void serialize(Archive& ar, const unsigned int /*version*/) { - ar& BOOST_SERIALIZATION_NVP(fx_); - ar& BOOST_SERIALIZATION_NVP(fy_); - ar& BOOST_SERIALIZATION_NVP(s_); - ar& BOOST_SERIALIZATION_NVP(u0_); - ar& BOOST_SERIALIZATION_NVP(v0_); + ar& boost::serialization::make_nvp( + "Cal3Fisheye", boost::serialization::base_object(*this)); ar& BOOST_SERIALIZATION_NVP(k1_); ar& BOOST_SERIALIZATION_NVP(k2_); ar& BOOST_SERIALIZATION_NVP(k3_); diff --git a/gtsam/geometry/Cal3Unified.cpp b/gtsam/geometry/Cal3Unified.cpp index f4ce0ed75..11aabcaa7 100644 --- a/gtsam/geometry/Cal3Unified.cpp +++ b/gtsam/geometry/Cal3Unified.cpp @@ -16,19 +16,15 @@ * @author Varun Agrawal */ -#include #include -#include +#include #include +#include #include namespace gtsam { -/* ************************************************************************* */ -Cal3Unified::Cal3Unified(const Vector &v): - Base(v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8]), xi_(v[9]) {} - /* ************************************************************************* */ Vector10 Cal3Unified::vector() const { Vector10 v; @@ -36,6 +32,13 @@ Vector10 Cal3Unified::vector() const { return v; } +/* ************************************************************************* */ +std::ostream& operator<<(std::ostream& os, const Cal3Unified& cal) { + os << (Cal3DS2_Base&)cal; + os << ", xi: " << cal.xi(); + return os; +} + /* ************************************************************************* */ void Cal3Unified::print(const std::string& s) const { Base::print(s); @@ -44,20 +47,14 @@ void Cal3Unified::print(const std::string& s) const { /* ************************************************************************* */ bool Cal3Unified::equals(const Cal3Unified& K, double tol) const { - if (std::abs(fx_ - K.fx_) > tol || std::abs(fy_ - K.fy_) > tol || std::abs(s_ - K.s_) > tol || - std::abs(u0_ - K.u0_) > tol || std::abs(v0_ - K.v0_) > tol || std::abs(k1_ - K.k1_) > tol || - std::abs(k2_ - K.k2_) > tol || std::abs(p1_ - K.p1_) > tol || std::abs(p2_ - K.p2_) > tol || - std::abs(xi_ - K.xi_) > tol) - return false; - return true; + const Cal3DS2_Base* base = dynamic_cast(&K); + return Cal3DS2_Base::equals(*base, tol) && std::fabs(xi_ - K.xi_) < tol; } /* ************************************************************************* */ // todo: make a fixed sized jacobian version of this -Point2 Cal3Unified::uncalibrate(const Point2& p, - OptionalJacobian<2,10> Dcal, - OptionalJacobian<2,2> Dp) const { - +Point2 Cal3Unified::uncalibrate(const Point2& p, OptionalJacobian<2, 10> Dcal, + OptionalJacobian<2, 2> Dp) const { // this part of code is modified from Cal3DS2, // since the second part of this model (after project to normalized plane) // is same as Cal3DS2 @@ -70,19 +67,19 @@ Point2 Cal3Unified::uncalibrate(const Point2& p, const double sqrt_nx = sqrt(xs * xs + ys * ys + 1.0); const double xi_sqrt_nx = 1.0 / (1 + xi * sqrt_nx); const double xi_sqrt_nx2 = xi_sqrt_nx * xi_sqrt_nx; - const double x = xs * xi_sqrt_nx, y = ys * xi_sqrt_nx; // points on NPlane + const double x = xs * xi_sqrt_nx, y = ys * xi_sqrt_nx; // points on NPlane // Part2: project NPlane point to pixel plane: use Cal3DS2 - Point2 m(x,y); + Point2 m(x, y); Matrix29 H1base; - Matrix2 H2base; // jacobians from Base class + Matrix2 H2base; // jacobians from Base class Point2 puncalib = Base::uncalibrate(m, H1base, H2base); // Inlined derivative for calibration if (Dcal) { // part1 Vector2 DU; - DU << -xs * sqrt_nx * xi_sqrt_nx2, // + DU << -xs * sqrt_nx * xi_sqrt_nx2, // -ys * sqrt_nx * xi_sqrt_nx2; *Dcal << H1base, H2base * DU; } @@ -91,10 +88,10 @@ Point2 Cal3Unified::uncalibrate(const Point2& p, if (Dp) { // part1 const double denom = 1.0 * xi_sqrt_nx2 / sqrt_nx; - const double mid = -(xi * xs*ys) * denom; + const double mid = -(xi * xs * ys) * denom; Matrix2 DU; - DU << (sqrt_nx + xi*(ys*ys + 1)) * denom, mid, // - mid, (sqrt_nx + xi*(xs*xs + 1)) * denom; + DU << (sqrt_nx + xi * (ys * ys + 1)) * denom, mid, // + mid, (sqrt_nx + xi * (xs * xs + 1)) * denom; *Dp << H2base * DU; } @@ -117,7 +114,6 @@ Point2 Cal3Unified::calibrate(const Point2& pi, OptionalJacobian<2, 10> Dcal, } /* ************************************************************************* */ Point2 Cal3Unified::nPlaneToSpace(const Point2& p) const { - const double x = p.x(), y = p.y(); const double xy2 = x * x + y * y; const double sq_xy = (xi_ + sqrt(1 + (1 - xi_ * xi_) * xy2)) / (xy2 + 1); @@ -127,7 +123,6 @@ Point2 Cal3Unified::nPlaneToSpace(const Point2& p) const { /* ************************************************************************* */ Point2 Cal3Unified::spaceToNPlane(const Point2& p) const { - const double x = p.x(), y = p.y(); const double sq_xy = 1 + xi_ * sqrt(x * x + y * y + 1); @@ -140,11 +135,10 @@ Cal3Unified Cal3Unified::retract(const Vector& d) const { } /* ************************************************************************* */ -Vector10 Cal3Unified::localCoordinates(const Cal3Unified& T2) const { +Vector Cal3Unified::localCoordinates(const Cal3Unified& T2) const { return T2.vector() - vector(); } -} /* ************************************************************************* */ - +} // \ namespace gtsam diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index 6fc37b0d1..ee388c8c1 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -28,40 +28,39 @@ namespace gtsam { /** - * @brief Calibration of a omni-directional camera with mirror + lens radial distortion + * @brief Calibration of a omni-directional camera with mirror + lens radial + * distortion * @addtogroup geometry * \nosubgrouping * * Similar to Cal3DS2, does distortion but has additional mirror parameter xi * K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ] - * Pn = [ P.x / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1}), P.y / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1})] - * rr = Pn.x^2 + Pn.y^2 - * \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ; - * k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ] + * Pn = [ P.x / (1 + xi * \sqrt{P.x² + P.y² + 1}), P.y / (1 + xi * \sqrt{P.x² + + * P.y² + 1})] + * r² = Pn.x² + Pn.y² + * \hat{pn} = (1 + k1*r² + k2*r⁴ ) pn + [ 2*k3 pn.x pn.y + k4 (r² + 2 Pn.x²) ; + * k3 (rr + 2 Pn.y²) + 2*k4 pn.x pn.y ] * pi = K*pn */ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base { + using This = Cal3Unified; + using Base = Cal3DS2_Base; - typedef Cal3Unified This; - typedef Cal3DS2_Base Base; - -private: - - double xi_; // mirror parameter - -public: + private: + double xi_ = 0.0f; ///< mirror parameter + public: enum { dimension = 10 }; /// @name Standard Constructors /// @{ /// Default Constructor with only unit focal length - Cal3Unified() : Base(), xi_(0) {} + Cal3Unified() = default; - Cal3Unified(double fx, double fy, double s, double u0, double v0, - double k1, double k2, double p1 = 0.0, double p2 = 0.0, double xi = 0.0) : - Base(fx, fy, s, u0, v0, k1, k2, p1, p2), xi_(xi) {} + Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, + double k2, double p1 = 0.0, double p2 = 0.0, double xi = 0.0) + : Base(fx, fy, s, u0, v0, k1, k2, p1, p2), xi_(xi) {} virtual ~Cal3Unified() {} @@ -69,12 +68,17 @@ public: /// @name Advanced Constructors /// @{ - Cal3Unified(const Vector &v) ; + Cal3Unified(const Vector10& v) + : Base(v(0), v(1), v(2), v(3), v(4), v(5), v(6), v(7), v(8)), xi_(v(9)) {} /// @} /// @name Testable /// @{ + /// Output stream operator + GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, + const Cal3Unified& cal); + /// print with optional string void print(const std::string& s = "") const override; @@ -86,7 +90,10 @@ public: /// @{ /// mirror parameter - inline double xi() const { return xi_;} + inline double xi() const { return xi_; } + + /// Return all parameters as a vector + Vector10 vector() const; /** * convert intrinsic coordinates xy to image coordinates uv @@ -96,8 +103,8 @@ public: * @return point in image coordinates */ Point2 uncalibrate(const Point2& p, - OptionalJacobian<2,10> Dcal = boost::none, - OptionalJacobian<2,2> Dp = boost::none) const ; + OptionalJacobian<2, 10> Dcal = boost::none, + OptionalJacobian<2, 2> Dp = boost::none) const; /// Conver a pixel coordinate to ideal coordinate Point2 calibrate(const Point2& p, OptionalJacobian<2, 10> Dcal = boost::none, @@ -114,41 +121,33 @@ public: /// @{ /// Given delta vector, update calibration - Cal3Unified retract(const Vector& d) const ; + Cal3Unified retract(const Vector& d) const; /// Given a different calibration, calculate update to obtain it - Vector10 localCoordinates(const Cal3Unified& T2) const ; + Vector localCoordinates(const Cal3Unified& T2) const; /// Return dimensions of calibration manifold object - virtual size_t dim() const { return dimension ; } + virtual size_t dim() const override { return Dim(); } /// Return dimensions of calibration manifold object - static size_t Dim() { return dimension; } - - /// Return all parameters as a vector - Vector10 vector() const ; + inline static size_t Dim() { return dimension; } /// @} -private: - + private: /** Serialization function */ friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int /*version*/) - { - ar & boost::serialization::make_nvp("Cal3Unified", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(xi_); + template + void serialize(Archive& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "Cal3Unified", boost::serialization::base_object(*this)); + ar& BOOST_SERIALIZATION_NVP(xi_); } - }; -template<> +template <> struct traits : public internal::Manifold {}; -template<> +template <> struct traits : public internal::Manifold {}; - } - diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp index b3d1be4b6..1a76c3f6f 100644 --- a/gtsam/geometry/Cal3_S2.cpp +++ b/gtsam/geometry/Cal3_S2.cpp @@ -22,95 +22,55 @@ #include namespace gtsam { -using namespace std; /* ************************************************************************* */ -Cal3_S2::Cal3_S2(double fov, int w, int h) : - s_(0), u0_((double) w / 2.0), v0_((double) h / 2.0) { - double a = fov * M_PI / 360.0; // fov/2 in radians - fx_ = (double) w / (2.0 * tan(a)); // old formula: fx_ = (double) w * tan(a); - fy_ = fx_; -} - -/* ************************************************************************* */ -Cal3_S2::Cal3_S2(const std::string &path) : - fx_(320), fy_(320), s_(0), u0_(320), v0_(140) { - - char buffer[200]; - buffer[0] = 0; - sprintf(buffer, "%s/calibration_info.txt", path.c_str()); - std::ifstream infile(buffer, std::ios::in); - - if (infile) - infile >> fx_ >> fy_ >> s_ >> u0_ >> v0_; - else { - throw std::runtime_error("Cal3_S2: Unable to load the calibration"); - } - - infile.close(); -} - -/* ************************************************************************* */ -ostream& operator<<(ostream& os, const Cal3_S2& cal) { - os << "{fx: " << cal.fx() << ", fy: " << cal.fy() << ", s:" << cal.skew() << ", px:" << cal.px() - << ", py:" << cal.py() << "}"; +std::ostream& operator<<(std::ostream& os, const Cal3_S2& cal) { + // Use the base class version since it is identical. + os << (Cal3&)cal; return os; } /* ************************************************************************* */ void Cal3_S2::print(const std::string& s) const { - gtsam::print((Matrix)matrix(), s); + gtsam::print((Matrix)K(), s); } /* ************************************************************************* */ bool Cal3_S2::equals(const Cal3_S2& K, double tol) const { - if (std::abs(fx_ - K.fx_) > tol) - return false; - if (std::abs(fy_ - K.fy_) > tol) - return false; - if (std::abs(s_ - K.s_) > tol) - return false; - if (std::abs(u0_ - K.u0_) > tol) - return false; - if (std::abs(v0_ - K.v0_) > tol) - return false; - return true; + return Cal3::equals(K, tol); } /* ************************************************************************* */ Point2 Cal3_S2::uncalibrate(const Point2& p, OptionalJacobian<2, 5> Dcal, - OptionalJacobian<2, 2> Dp) const { + OptionalJacobian<2, 2> Dp) const { const double x = p.x(), y = p.y(); - if (Dcal) - *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0; - if (Dp) - *Dp << fx_, s_, 0.0, fy_; + if (Dcal) *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0; + if (Dp) *Dp << fx_, s_, 0.0, fy_; return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_); } /* ************************************************************************* */ -Point2 Cal3_S2::calibrate(const Point2& p, OptionalJacobian<2,5> Dcal, - OptionalJacobian<2,2> Dp) const { - const double u = p.x(), v = p.y(); - double delta_u = u - u0_, delta_v = v - v0_; - double inv_fx = 1/ fx_, inv_fy = 1/fy_; - double inv_fy_delta_v = inv_fy * delta_v, inv_fx_s_inv_fy = inv_fx * s_ * inv_fy; - Point2 point(inv_fx * (delta_u - s_ * inv_fy_delta_v), - inv_fy_delta_v); - if(Dcal) - *Dcal << - inv_fx * point.x(), inv_fx * s_ * inv_fy * inv_fy_delta_v, -inv_fx * point.y(), - -inv_fx, inv_fx_s_inv_fy, - 0, -inv_fy * point.y(), 0, 0, -inv_fy; - if(Dp) - *Dp << inv_fx, -inv_fx_s_inv_fy, 0, inv_fy; - return point; +Point2 Cal3_S2::calibrate(const Point2& p, OptionalJacobian<2, 5> Dcal, + OptionalJacobian<2, 2> Dp) const { + const double u = p.x(), v = p.y(); + double delta_u = u - u0_, delta_v = v - v0_; + double inv_fx = 1 / fx_, inv_fy = 1 / fy_; + double inv_fy_delta_v = inv_fy * delta_v; + double inv_fx_s_inv_fy = inv_fx * s_ * inv_fy; + + Point2 point(inv_fx * (delta_u - s_ * inv_fy_delta_v), inv_fy_delta_v); + if (Dcal) { + *Dcal << -inv_fx * point.x(), inv_fx * s_ * inv_fy * inv_fy_delta_v, + -inv_fx * point.y(), -inv_fx, inv_fx_s_inv_fy, 0, -inv_fy * point.y(), + 0, 0, -inv_fy; + } + if (Dp) *Dp << inv_fx, -inv_fx_s_inv_fy, 0, inv_fy; + return point; } /* ************************************************************************* */ -Vector3 Cal3_S2::calibrate(const Vector3& p) const { - return matrix_inverse() * p; -} +Vector3 Cal3_S2::calibrate(const Vector3& p) const { return inverse() * p; } /* ************************************************************************* */ -} // namespace gtsam +} // namespace gtsam diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index f2848d0a3..93b98a7e1 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -21,6 +21,7 @@ #pragma once +#include #include namespace gtsam { @@ -30,31 +31,25 @@ namespace gtsam { * @addtogroup geometry * \nosubgrouping */ -class GTSAM_EXPORT Cal3_S2 { -private: - double fx_, fy_, s_, u0_, v0_; - -public: +class GTSAM_EXPORT Cal3_S2 : public Cal3 { + public: enum { dimension = 5 }; - typedef boost::shared_ptr shared_ptr; ///< shared pointer to calibration object + + ///< shared pointer to calibration object + using shared_ptr = boost::shared_ptr; /// @name Standard Constructors /// @{ /// Create a default calibration that leaves coordinates unchanged - Cal3_S2() : - fx_(1), fy_(1), s_(0), u0_(0), v0_(0) { - } + Cal3_S2() = default; /// constructor from doubles - Cal3_S2(double fx, double fy, double s, double u0, double v0) : - fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0) { - } + Cal3_S2(double fx, double fy, double s, double u0, double v0) + : Cal3(fx, fy, s, u0, v0) {} /// constructor from vector - Cal3_S2(const Vector &d) : - fx_(d(0)), fy_(d(1)), s_(d(2)), u0_(d(3)), v0_(d(4)) { - } + Cal3_S2(const Vector5& d) : Cal3(d) {} /** * Easy constructor, takes fov in degrees, asssumes zero skew, unit aspect @@ -62,141 +57,65 @@ public: * @param w image width * @param h image height */ - Cal3_S2(double fov, int w, int h); + Cal3_S2(double fov, int w, int h) : Cal3(fov, w, h) {} - /// @} - /// @name Advanced Constructors - /// @{ + /** + * Convert intrinsic coordinates xy to image coordinates uv, fixed derivaitves + * @param p point in intrinsic coordinates + * @param Dcal optional 2*5 Jacobian wrpt Cal3 parameters + * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates + * @return point in image coordinates + */ + Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 5> Dcal = boost::none, + OptionalJacobian<2, 2> Dp = boost::none) const; - /// load calibration from location (default name is calibration_info.txt) - Cal3_S2(const std::string &path); + /** + * Convert image coordinates uv to intrinsic coordinates xy + * @param p point in image coordinates + * @param Dcal optional 2*5 Jacobian wrpt Cal3 parameters + * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates + * @return point in intrinsic coordinates + */ + Point2 calibrate(const Point2& p, OptionalJacobian<2, 5> Dcal = boost::none, + OptionalJacobian<2, 2> Dp = boost::none) const; + + /** + * Convert homogeneous image coordinates to intrinsic coordinates + * @param p point in image coordinates + * @return point in intrinsic coordinates + */ + Vector3 calibrate(const Vector3& p) const; /// @} /// @name Testable /// @{ /// Output stream operator - GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Cal3_S2& cal); + GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, + const Cal3_S2& cal); /// print with optional string - void print(const std::string& s = "Cal3_S2") const; + void print(const std::string& s = "Cal3_S2") const override; /// Check if equal up to specified tolerance bool equals(const Cal3_S2& K, double tol = 10e-9) const; - /// @} - /// @name Standard Interface - /// @{ - - /// focal length x - inline double fx() const { - return fx_; - } - - /// focal length y - inline double fy() const { - return fy_; - } - - /// aspect ratio - inline double aspectRatio() const { - return fx_/fy_; - } - - /// skew - inline double skew() const { - return s_; - } - - /// image center in x - inline double px() const { - return u0_; - } - - /// image center in y - inline double py() const { - return v0_; - } - - /// return the principal point - Point2 principalPoint() const { - return Point2(u0_, v0_); - } - - /// vectorized form (column-wise) - Vector5 vector() const { - Vector5 v; - v << fx_, fy_, s_, u0_, v0_; - return v; - } - - /// return calibration matrix K - Matrix3 K() const { - Matrix3 K; - K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0; - return K; - } - - /** @deprecated The following function has been deprecated, use K above */ - Matrix3 matrix() const { - return K(); - } - - /// return inverted calibration matrix inv(K) - Matrix3 matrix_inverse() const { - const double fxy = fx_ * fy_, sv0 = s_ * v0_, fyu0 = fy_ * u0_; - Matrix3 K_inverse; - K_inverse << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0, - 1.0 / fy_, -v0_ / fy_, 0.0, 0.0, 1.0; - return K_inverse; - } - - /** - * convert intrinsic coordinates xy to image coordinates uv, fixed derivaitves - * @param p point in intrinsic coordinates - * @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters - * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates - * @return point in image coordinates - */ - Point2 uncalibrate(const Point2& p, OptionalJacobian<2,5> Dcal = boost::none, - OptionalJacobian<2,2> Dp = boost::none) const; - - /** - * convert image coordinates uv to intrinsic coordinates xy - * @param p point in image coordinates - * @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters - * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates - * @return point in intrinsic coordinates - */ - Point2 calibrate(const Point2& p, OptionalJacobian<2,5> Dcal = boost::none, - OptionalJacobian<2,2> Dp = boost::none) const; - - /** - * convert homogeneous image coordinates to intrinsic coordinates - * @param p point in image coordinates - * @return point in intrinsic coordinates - */ - Vector3 calibrate(const Vector3& p) const; - /// "Between", subtracts calibrations. between(p,q) == compose(inverse(p),q) inline Cal3_S2 between(const Cal3_S2& q, - OptionalJacobian<5,5> H1=boost::none, - OptionalJacobian<5,5> H2=boost::none) const { - if(H1) *H1 = -I_5x5; - if(H2) *H2 = I_5x5; - return Cal3_S2(q.fx_-fx_, q.fy_-fy_, q.s_-s_, q.u0_-u0_, q.v0_-v0_); + OptionalJacobian<5, 5> H1 = boost::none, + OptionalJacobian<5, 5> H2 = boost::none) const { + if (H1) *H1 = -I_5x5; + if (H2) *H2 = I_5x5; + return Cal3_S2(q.fx_ - fx_, q.fy_ - fy_, q.s_ - s_, q.u0_ - u0_, + q.v0_ - v0_); } - /// @} /// @name Manifold /// @{ /// return DOF, dimensionality of tangent space - inline size_t dim() const { return dimension; } - - /// return DOF, dimensionality of tangent space - static size_t Dim() { return dimension; } + inline static size_t Dim() { return dimension; } /// Given 5-dim tangent vector, create new calibration inline Cal3_S2 retract(const Vector& d) const { @@ -212,27 +131,22 @@ public: /// @name Advanced Interface /// @{ -private: - + private: /// 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(fy_); - ar & BOOST_SERIALIZATION_NVP(s_); - ar & BOOST_SERIALIZATION_NVP(u0_); - ar & BOOST_SERIALIZATION_NVP(v0_); + template + void serialize(Archive& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "Cal3_S2", boost::serialization::base_object(*this)); } /// @} - }; -template<> +template <> struct traits : public internal::Manifold {}; -template<> +template <> struct traits : public internal::Manifold {}; -} // \ namespace gtsam +} // \ namespace gtsam diff --git a/gtsam/geometry/Cal3_S2Stereo.cpp b/gtsam/geometry/Cal3_S2Stereo.cpp index 9b5aea4ed..9ef8c83a3 100644 --- a/gtsam/geometry/Cal3_S2Stereo.cpp +++ b/gtsam/geometry/Cal3_S2Stereo.cpp @@ -20,20 +20,56 @@ #include namespace gtsam { -using namespace std; + +/* ************************************************************************* */ +std::ostream& operator<<(std::ostream& os, const Cal3_S2Stereo& cal) { + os << (Cal3_S2&)cal; + os << ", b: " << cal.baseline(); + return os; +} /* ************************************************************************* */ void Cal3_S2Stereo::print(const std::string& s) const { - K_.print(s+"K: "); - std::cout << s << "Baseline: " << b_ << std::endl; - } + std::cout << s << (s != "" ? " " : ""); + std::cout << "K: " << (Matrix)K() << std::endl; + std::cout << "Baseline: " << b_ << std::endl; +} /* ************************************************************************* */ bool Cal3_S2Stereo::equals(const Cal3_S2Stereo& other, double tol) const { - if (std::abs(b_ - other.b_) > tol) return false; - return K_.equals(other.K_,tol); + const Cal3_S2* base = dynamic_cast(&other); + return (Cal3_S2::equals(*base, tol) && + std::fabs(b_ - other.baseline()) < tol); +} + +/* ************************************************************************* */ +Point2 Cal3_S2Stereo::uncalibrate(const Point2& p, OptionalJacobian<2, 6> Dcal, + OptionalJacobian<2, 2> Dp) const { + const double x = p.x(), y = p.y(); + if (Dcal) *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, 0.0, y, 0.0, 0.0, 1.0, 0.0; + if (Dp) *Dp << fx_, s_, 0.0, fy_; + return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_); +} + +/* ************************************************************************* */ +Point2 Cal3_S2Stereo::calibrate(const Point2& p, OptionalJacobian<2, 6> Dcal, + OptionalJacobian<2, 2> Dp) const { + const double u = p.x(), v = p.y(); + double delta_u = u - u0_, delta_v = v - v0_; + double inv_fx = 1 / fx_, inv_fy = 1 / fy_; + double inv_fy_delta_v = inv_fy * delta_v; + double inv_fx_s_inv_fy = inv_fx * s_ * inv_fy; + + Point2 point(inv_fx * (delta_u - s_ * inv_fy_delta_v), inv_fy_delta_v); + if (Dcal) { + *Dcal << -inv_fx * point.x(), inv_fx * s_ * inv_fy * inv_fy_delta_v, + -inv_fx * point.y(), -inv_fx, inv_fx_s_inv_fy, 0, 0, + -inv_fy * point.y(), 0, 0, -inv_fy, 0; + } + if (Dp) *Dp << inv_fx, -inv_fx_s_inv_fy, 0, inv_fy; + return point; } /* ************************************************************************* */ -} // namespace gtsam +} // namespace gtsam diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h index a6eb41b60..ae0052fd5 100644 --- a/gtsam/geometry/Cal3_S2Stereo.h +++ b/gtsam/geometry/Cal3_S2Stereo.h @@ -22,135 +22,143 @@ namespace gtsam { +/** + * @brief The most common 5DOF 3D->2D calibration, stereo version + * @addtogroup geometry + * \nosubgrouping + */ +class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 { + private: + double b_ = 1.0f; ///< Stereo baseline. + + public: + enum { dimension = 6 }; + + ///< shared pointer to stereo calibration object + using shared_ptr = boost::shared_ptr; + + /// @name Standard Constructors + /// @ + + /// default calibration leaves coordinates unchanged + Cal3_S2Stereo() = default; + + /// constructor from doubles + Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b) + : Cal3_S2(fx, fy, s, u0, v0), b_(b) {} + + /// constructor from vector + Cal3_S2Stereo(const Vector6& d) + : Cal3_S2(d(0), d(1), d(2), d(3), d(4)), b_(d(5)) {} + + /// easy constructor; field-of-view in degrees, assumes zero skew + Cal3_S2Stereo(double fov, int w, int h, double b) + : Cal3_S2(fov, w, h), b_(b) {} + /** - * @brief The most common 5DOF 3D->2D calibration, stereo version - * @addtogroup geometry - * \nosubgrouping + * Convert intrinsic coordinates xy to image coordinates uv, fixed derivaitves + * @param p point in intrinsic coordinates + * @param Dcal optional 2*6 Jacobian wrpt Cal3_S2Stereo parameters + * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates + * @return point in image coordinates */ - class GTSAM_EXPORT Cal3_S2Stereo { - private: + Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 6> Dcal = boost::none, + OptionalJacobian<2, 2> Dp = boost::none) const; - Cal3_S2 K_; - double b_; + /** + * Convert image coordinates uv to intrinsic coordinates xy + * @param p point in image coordinates + * @param Dcal optional 2*6 Jacobian wrpt Cal3_S2Stereo parameters + * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates + * @return point in intrinsic coordinates + */ + Point2 calibrate(const Point2& p, OptionalJacobian<2, 6> Dcal = boost::none, + OptionalJacobian<2, 2> Dp = boost::none) const; - public: + /** + * Convert homogeneous image coordinates to intrinsic coordinates + * @param p point in image coordinates + * @return point in intrinsic coordinates + */ + Vector3 calibrate(const Vector3& p) const { return Cal3_S2::calibrate(p); } - enum { dimension = 6 }; - typedef boost::shared_ptr shared_ptr; ///< shared pointer to stereo calibration object + /// @} + /// @name Testable + /// @{ - /// @name Standard Constructors - /// @ + /// Output stream operator + GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, + const Cal3_S2Stereo& cal); - /// default calibration leaves coordinates unchanged - Cal3_S2Stereo() : - K_(1, 1, 0, 0, 0), b_(1.0) { - } + /// print with optional string + void print(const std::string& s = "") const override; - /// constructor from doubles - Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b) : - K_(fx, fy, s, u0, v0), b_(b) { - } + /// Check if equal up to specified tolerance + bool equals(const Cal3_S2Stereo& other, double tol = 10e-9) const; - /// constructor from vector - Cal3_S2Stereo(const Vector &d): K_(d(0), d(1), d(2), d(3), d(4)), b_(d(5)){} + /// @} + /// @name Standard Interface + /// @{ - /// easy constructor; field-of-view in degrees, assumes zero skew - Cal3_S2Stereo(double fov, int w, int h, double b) : - K_(fov, w, h), b_(b) { - } + /// return calibration, same for left and right + const Cal3_S2& calibration() const { return *this; } - /// @} - /// @name Testable - /// @{ + /// return calibration matrix K, same for left and right + Matrix3 K() const override { return Cal3_S2::K(); } - void print(const std::string& s = "") const; + /// return baseline + inline double baseline() const { return b_; } - /// Check if equal up to specified tolerance - bool equals(const Cal3_S2Stereo& other, double tol = 10e-9) const; + /// vectorized form (column-wise) + Vector6 vector() const { + Vector6 v; + v << Cal3_S2::vector(), b_; + return v; + } - /// @} - /// @name Standard Interface - /// @{ + /// @} + /// @name Manifold + /// @{ - /// return calibration, same for left and right - const Cal3_S2& calibration() const { return K_;} + /// return DOF, dimensionality of tangent space + inline size_t dim() const override { return Dim(); } - /// return calibration matrix K, same for left and right - Matrix matrix() const { return K_.matrix();} + /// return DOF, dimensionality of tangent space + inline static size_t Dim() { return dimension; } - /// focal length x - inline double fx() const { return K_.fx();} + /// Given 6-dim tangent vector, create new calibration + inline Cal3_S2Stereo retract(const Vector& d) const { + return Cal3_S2Stereo(fx() + d(0), fy() + d(1), skew() + d(2), px() + d(3), + py() + d(4), b_ + d(5)); + } - /// focal length x - inline double fy() const { return K_.fy();} + /// Unretraction for the calibration + Vector6 localCoordinates(const Cal3_S2Stereo& T2) const { + return T2.vector() - vector(); + } - /// skew - inline double skew() const { return K_.skew();} + /// @} + /// @name Advanced Interface + /// @{ - /// image center in x - inline double px() const { return K_.px();} + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "Cal3_S2", boost::serialization::base_object(*this)); + ar& BOOST_SERIALIZATION_NVP(b_); + } + /// @} +}; - /// image center in y - inline double py() const { return K_.py();} +// Define GTSAM traits +template <> +struct traits : public internal::Manifold {}; - /// return the principal point - Point2 principalPoint() const { return K_.principalPoint();} +template <> +struct traits : public internal::Manifold { +}; - /// return baseline - inline double baseline() const { return b_; } - - /// vectorized form (column-wise) - Vector6 vector() const { - Vector6 v; - v << K_.vector(), b_; - return v; - } - - /// @} - /// @name Manifold - /// @{ - - /// return DOF, dimensionality of tangent space - inline size_t dim() const { return dimension; } - - /// return DOF, dimensionality of tangent space - static size_t Dim() { return dimension; } - - /// Given 6-dim tangent vector, create new calibration - inline Cal3_S2Stereo retract(const Vector& d) const { - return Cal3_S2Stereo(K_.fx() + d(0), K_.fy() + d(1), K_.skew() + d(2), K_.px() + d(3), K_.py() + d(4), b_ + d(5)); - } - - /// Unretraction for the calibration - Vector6 localCoordinates(const Cal3_S2Stereo& T2) const { - return T2.vector() - vector(); - } - - - /// @} - /// @name Advanced Interface - /// @{ - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int /*version*/) - { - ar & BOOST_SERIALIZATION_NVP(K_); - ar & BOOST_SERIALIZATION_NVP(b_); - } - /// @} - - }; - - // Define GTSAM traits - template<> - struct traits : public internal::Manifold { - }; - - template<> - struct traits : public internal::Manifold { - }; - -} // \ namespace gtsam +} // \ namespace gtsam diff --git a/gtsam/geometry/tests/testCal3Bundler.cpp b/gtsam/geometry/tests/testCal3Bundler.cpp index 448600266..b821d295b 100644 --- a/gtsam/geometry/tests/testCal3Bundler.cpp +++ b/gtsam/geometry/tests/testCal3Bundler.cpp @@ -11,11 +11,12 @@ /** * @file testCal3Bundler.cpp - * @brief Unit tests for transform derivatives + * @brief Unit tests for Bundler calibration model. */ #include #include +#include #include #include @@ -25,30 +26,27 @@ GTSAM_CONCEPT_TESTABLE_INST(Cal3Bundler) GTSAM_CONCEPT_MANIFOLD_INST(Cal3Bundler) static Cal3Bundler K(500, 1e-3, 1e-3, 1000, 2000); -static Point2 p(2,3); +static Point2 p(2, 3); /* ************************************************************************* */ -TEST( Cal3Bundler, vector) -{ +TEST(Cal3Bundler, vector) { Cal3Bundler K; Vector expected(3); expected << 1, 0, 0; - CHECK(assert_equal(expected,K.vector())); + CHECK(assert_equal(expected, K.vector())); } /* ************************************************************************* */ -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()) ; +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()); Point2 actual = K.uncalibrate(p); - CHECK(assert_equal(expected,actual)); + 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); @@ -56,26 +54,28 @@ TEST( Cal3Bundler, calibrate ) } /* ************************************************************************* */ -Point2 uncalibrate_(const Cal3Bundler& k, const Point2& pt) { return k.uncalibrate(pt); } +Point2 uncalibrate_(const Cal3Bundler& k, const Point2& pt) { + return k.uncalibrate(pt); +} -Point2 calibrate_(const Cal3Bundler& k, const Point2& pt) { return k.calibrate(pt); } - -/* ************************************************************************* */ -TEST( Cal3Bundler, Duncalibrate) -{ - Matrix Dcal, Dp; - Point2 actual = K.uncalibrate(p, Dcal, Dp); - Point2 expected(2182, 3773); - 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)); - CHECK(assert_equal(numerical2,Dp,1e-7)); +Point2 calibrate_(const Cal3Bundler& k, const Point2& pt) { + return k.calibrate(pt); } /* ************************************************************************* */ -TEST( Cal3Bundler, Dcalibrate) -{ +TEST(Cal3Bundler, Duncalibrate) { + Matrix Dcal, Dp; + Point2 actual = K.uncalibrate(p, Dcal, Dp); + Point2 expected(2182, 3773); + 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)); + CHECK(assert_equal(numerical2, Dp, 1e-7)); +} + +/* ************************************************************************* */ +TEST(Cal3Bundler, Dcalibrate) { Matrix Dcal, Dp; Point2 pn(0.5, 0.5); Point2 pi = K.uncalibrate(pn); @@ -83,27 +83,41 @@ TEST( Cal3Bundler, Dcalibrate) CHECK(assert_equal(pn, actual, 1e-7)); Matrix numerical1 = numericalDerivative21(calibrate_, K, pi); Matrix numerical2 = numericalDerivative22(calibrate_, K, pi); - CHECK(assert_equal(numerical1,Dcal,1e-5)); - CHECK(assert_equal(numerical2,Dp,1e-5)); + CHECK(assert_equal(numerical1, Dcal, 1e-5)); + CHECK(assert_equal(numerical2, Dp, 1e-5)); } /* ************************************************************************* */ -TEST( Cal3Bundler, assert_equal) -{ - CHECK(assert_equal(K,K,1e-7)); -} +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); - Vector d(3); + EXPECT_LONGS_EQUAL(3, expected.dim()); + + EXPECT_LONGS_EQUAL(Cal3Bundler::Dim(), 3); + EXPECT_LONGS_EQUAL(expected.dim(), 3); + + Vector3 d; d << 10, 1e-3, 1e-3; Cal3Bundler actual = K.retract(d); - CHECK(assert_equal(expected,actual,1e-7)); - CHECK(assert_equal(d,K.localCoordinates(actual),1e-7)); + CHECK(assert_equal(expected, actual, 1e-7)); + CHECK(assert_equal(d, K.localCoordinates(actual), 1e-7)); } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +TEST(Cal3_S2, Print) { + Cal3Bundler cal(1, 2, 3, 4, 5); + std::stringstream os; + os << "f: " << cal.fx() << ", k1: " << cal.k1() << ", k2: " << cal.k2() + << ", px: " << cal.px() << ", py: " << cal.py(); + + EXPECT(assert_stdout_equal(os.str(), cal)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testCal3DFisheye.cpp b/gtsam/geometry/tests/testCal3DFisheye.cpp index 6bfbe3e46..28064a92c 100644 --- a/gtsam/geometry/tests/testCal3DFisheye.cpp +++ b/gtsam/geometry/tests/testCal3DFisheye.cpp @@ -10,12 +10,13 @@ * -------------------------------------------------------------------------- */ /** - * @file testCal3DFisheye.cpp + * @file testCal3Fisheye.cpp * @brief Unit tests for fisheye calibration class * @author ghaggin */ #include +#include #include #include #include @@ -41,7 +42,11 @@ TEST(Cal3Fisheye, retract) { Cal3Fisheye expected(K.fx() + 1, K.fy() + 2, K.skew() + 3, K.px() + 4, K.py() + 5, K.k1() + 6, K.k2() + 7, K.k3() + 8, K.k4() + 9); - Vector d(9); + + EXPECT_LONGS_EQUAL(Cal3Fisheye::Dim(), 9); + EXPECT_LONGS_EQUAL(expected.dim(), 9); + + Vector9 d; d << 1, 2, 3, 4, 5, 6, 7, 8, 9; Cal3Fisheye actual = K.retract(d); CHECK(assert_equal(expected, actual, 1e-7)); @@ -186,16 +191,26 @@ Point2 calibrate_(const Cal3Fisheye& k, const Point2& pt) { } /* ************************************************************************* */ -TEST(Cal3Fisheye, Dcalibrate) -{ +TEST(Cal3Fisheye, Dcalibrate) { Point2 p(0.5, 0.5); Point2 pi = K.uncalibrate(p); Matrix Dcal, Dp; K.calibrate(pi, Dcal, Dp); Matrix numerical1 = numericalDerivative21(calibrate_, K, pi); - CHECK(assert_equal(numerical1,Dcal,1e-5)); + CHECK(assert_equal(numerical1, Dcal, 1e-5)); Matrix numerical2 = numericalDerivative22(calibrate_, K, pi); - CHECK(assert_equal(numerical2,Dp,1e-5)); + CHECK(assert_equal(numerical2, Dp, 1e-5)); +} + +/* ************************************************************************* */ +TEST(Cal3Fisheye, Print) { + Cal3Fisheye cal(1, 2, 3, 4, 5, 6, 7, 8, 9); + std::stringstream os; + os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew() + << ", px: " << cal.px() << ", py: " << cal.py() << ", k1: " << cal.k1() + << ", k2: " << cal.k2() << ", k3: " << cal.k3() << ", k4: " << cal.k4(); + + EXPECT(assert_stdout_equal(os.str(), cal)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testCal3DS2.cpp b/gtsam/geometry/tests/testCal3DS2.cpp index beed09883..7ef6e5001 100644 --- a/gtsam/geometry/tests/testCal3DS2.cpp +++ b/gtsam/geometry/tests/testCal3DS2.cpp @@ -11,12 +11,12 @@ /** * @file testCal3DS2.cpp - * @brief Unit tests for transform derivatives + * @brief Unit tests for Cal3DS2 calibration model. */ - #include #include +#include #include #include @@ -25,53 +25,53 @@ using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(Cal3DS2) GTSAM_CONCEPT_MANIFOLD_INST(Cal3DS2) -static Cal3DS2 K(500, 100, 0.1, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3); -static Point2 p(2,3); +static Cal3DS2 K(500, 100, 0.1, 320, 240, 1e-3, 2.0 * 1e-3, 3.0 * 1e-3, + 4.0 * 1e-3); +static Point2 p(2, 3); /* ************************************************************************* */ -TEST( Cal3DS2, uncalibrate) -{ - Vector k = K.k() ; - double r = p.x()*p.x() + p.y()*p.y() ; - double g = 1+k[0]*r+k[1]*r*r ; - double tx = 2*k[2]*p.x()*p.y() + k[3]*(r+2*p.x()*p.x()) ; - double ty = k[2]*(r+2*p.y()*p.y()) + 2*k[3]*p.x()*p.y() ; - Vector v_hat = (Vector(3) << g*p.x() + tx, g*p.y() + ty, 1.0).finished(); - Vector v_i = K.K() * v_hat ; - Point2 p_i(v_i(0)/v_i(2), v_i(1)/v_i(2)) ; +TEST(Cal3DS2, Uncalibrate) { + Vector k = K.k(); + double r = p.x() * p.x() + p.y() * p.y(); + double g = 1 + k[0] * r + k[1] * r * r; + double tx = 2 * k[2] * p.x() * p.y() + k[3] * (r + 2 * p.x() * p.x()); + double ty = k[2] * (r + 2 * p.y() * p.y()) + 2 * k[3] * p.x() * p.y(); + Vector v_hat = (Vector(3) << g * p.x() + tx, g * p.y() + ty, 1.0).finished(); + Vector v_i = K.K() * v_hat; + Point2 p_i(v_i(0) / v_i(2), v_i(1) / v_i(2)); Point2 q = K.uncalibrate(p); - CHECK(assert_equal(q,p_i)); + CHECK(assert_equal(q, p_i)); } -TEST( Cal3DS2, calibrate ) -{ +TEST(Cal3DS2, Calibrate) { Point2 pn(0.5, 0.5); Point2 pi = K.uncalibrate(pn); Point2 pn_hat = K.calibrate(pi); - CHECK( traits::Equals(pn, pn_hat, 1e-5)); + CHECK(traits::Equals(pn, pn_hat, 1e-5)); } -Point2 uncalibrate_(const Cal3DS2& k, const Point2& pt) { return k.uncalibrate(pt); } +Point2 uncalibrate_(const Cal3DS2& k, const Point2& pt) { + return k.uncalibrate(pt); +} /* ************************************************************************* */ -TEST( Cal3DS2, Duncalibrate1) -{ +TEST(Cal3DS2, Duncalibrate1) { Matrix computed; K.uncalibrate(p, computed, boost::none); Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7); - CHECK(assert_equal(numerical,computed,1e-5)); + CHECK(assert_equal(numerical, computed, 1e-5)); Matrix separate = K.D2d_calibration(p); - CHECK(assert_equal(numerical,separate,1e-5)); + CHECK(assert_equal(numerical, separate, 1e-5)); } /* ************************************************************************* */ -TEST( Cal3DS2, Duncalibrate2) -{ - Matrix computed; K.uncalibrate(p, boost::none, computed); +TEST(Cal3DS2, Duncalibrate2) { + Matrix computed; + K.uncalibrate(p, boost::none, computed); Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7); - CHECK(assert_equal(numerical,computed,1e-5)); + CHECK(assert_equal(numerical, computed, 1e-5)); Matrix separate = K.D2d_intrinsic(p); - CHECK(assert_equal(numerical,separate,1e-5)); + CHECK(assert_equal(numerical, separate, 1e-5)); } Point2 calibrate_(const Cal3DS2& k, const Point2& pt) { @@ -79,8 +79,7 @@ Point2 calibrate_(const Cal3DS2& k, const Point2& pt) { } /* ************************************************************************* */ -TEST( Cal3DS2, Dcalibrate) -{ +TEST(Cal3DS2, Dcalibrate) { Point2 pn(0.5, 0.5); Point2 pi = K.uncalibrate(pn); Matrix Dcal, Dp; @@ -92,20 +91,37 @@ TEST( Cal3DS2, Dcalibrate) } /* ************************************************************************* */ -TEST(Cal3DS2, assert_equal) { CHECK(assert_equal(K, K, 1e-5)); } +TEST(Cal3DS2, Equal) { CHECK(assert_equal(K, K, 1e-5)); } /* ************************************************************************* */ -TEST( Cal3DS2, retract) -{ +TEST(Cal3DS2, Retract) { Cal3DS2 expected(500 + 1, 100 + 2, 0.1 + 3, 320 + 4, 240 + 5, 1e-3 + 6, - 2.0 * 1e-3 + 7, 3.0 * 1e-3 + 8, 4.0 * 1e-3 + 9); - Vector d(9); - d << 1,2,3,4,5,6,7,8,9; + 2.0 * 1e-3 + 7, 3.0 * 1e-3 + 8, 4.0 * 1e-3 + 9); + + EXPECT_LONGS_EQUAL(Cal3DS2::Dim(), 9); + EXPECT_LONGS_EQUAL(expected.dim(), 9); + + Vector9 d; + d << 1, 2, 3, 4, 5, 6, 7, 8, 9; Cal3DS2 actual = K.retract(d); - CHECK(assert_equal(expected,actual,1e-7)); - CHECK(assert_equal(d,K.localCoordinates(actual),1e-7)); + CHECK(assert_equal(expected, actual, 1e-7)); + CHECK(assert_equal(d, K.localCoordinates(actual), 1e-7)); } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +TEST(Cal3DS2, Print) { + Cal3DS2 cal(1, 2, 3, 4, 5, 6, 7, 8, 9); + std::stringstream os; + os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew() + << ", px: " << cal.px() << ", py: " << cal.py() << ", k1: " << cal.k1() + << ", k2: " << cal.k2() << ", p1: " << cal.p1() << ", p2: " << cal.p2(); + + EXPECT(assert_stdout_equal(os.str(), cal)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testCal3Unified.cpp b/gtsam/geometry/tests/testCal3Unified.cpp index 8abb6fe04..648bb358c 100644 --- a/gtsam/geometry/tests/testCal3Unified.cpp +++ b/gtsam/geometry/tests/testCal3Unified.cpp @@ -10,17 +10,18 @@ * -------------------------------------------------------------------------- */ /** - * @file testCal3Unify.cpp - * @brief Unit tests for transform derivatives + * @file testCal3Unified.cpp + * @brief Unit tests for Cal3Unified calibration model. */ #include #include +#include #include #include -#include #include +#include using namespace gtsam; @@ -35,51 +36,49 @@ V = [0.1, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0, 0, 100, 105, 320, 240]; matlab toolbox available at http://homepages.laas.fr/~cmei/index.php/Toolbox */ -static Cal3Unified K(100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0.1); +static Cal3Unified K(100, 105, 0.0, 320, 240, 1e-3, 2.0 * 1e-3, 3.0 * 1e-3, + 4.0 * 1e-3, 0.1); static Point2 p(0.5, 0.7); /* ************************************************************************* */ -TEST( Cal3Unified, uncalibrate) -{ - Point2 p_i(364.7791831734982, 305.6677211952602) ; +TEST(Cal3Unified, Uncalibrate) { + Point2 p_i(364.7791831734982, 305.6677211952602); Point2 q = K.uncalibrate(p); - CHECK(assert_equal(q,p_i)); + CHECK(assert_equal(q, p_i)); } /* ************************************************************************* */ -TEST( Cal3Unified, spaceNplane) -{ +TEST(Cal3Unified, SpaceNplane) { Point2 q = K.spaceToNPlane(p); CHECK(assert_equal(Point2(0.441731600049497, 0.618424240069295), q)); CHECK(assert_equal(p, K.nPlaneToSpace(q))); } /* ************************************************************************* */ -TEST( Cal3Unified, calibrate) -{ +TEST(Cal3Unified, Calibrate) { Point2 pi = K.uncalibrate(p); Point2 pn_hat = K.calibrate(pi); - CHECK( traits::Equals(p, pn_hat, 1e-8)); + CHECK(traits::Equals(p, pn_hat, 1e-8)); } -Point2 uncalibrate_(const Cal3Unified& k, const Point2& pt) { return k.uncalibrate(pt); } +Point2 uncalibrate_(const Cal3Unified& k, const Point2& pt) { + return k.uncalibrate(pt); +} /* ************************************************************************* */ -TEST( Cal3Unified, Duncalibrate1) -{ +TEST(Cal3Unified, Duncalibrate1) { Matrix computed; K.uncalibrate(p, computed, boost::none); Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7); - CHECK(assert_equal(numerical,computed,1e-6)); + CHECK(assert_equal(numerical, computed, 1e-6)); } /* ************************************************************************* */ -TEST( Cal3Unified, Duncalibrate2) -{ +TEST(Cal3Unified, Duncalibrate2) { Matrix computed; K.uncalibrate(p, boost::none, computed); Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7); - CHECK(assert_equal(numerical,computed,1e-6)); + CHECK(assert_equal(numerical, computed, 1e-6)); } Point2 calibrate_(const Cal3Unified& k, const Point2& pt) { @@ -87,38 +86,37 @@ Point2 calibrate_(const Cal3Unified& k, const Point2& pt) { } /* ************************************************************************* */ -TEST( Cal3Unified, Dcalibrate) -{ +TEST(Cal3Unified, Dcalibrate) { Point2 pi = K.uncalibrate(p); Matrix Dcal, Dp; K.calibrate(pi, Dcal, Dp); Matrix numerical1 = numericalDerivative21(calibrate_, K, pi); - CHECK(assert_equal(numerical1,Dcal,1e-5)); + CHECK(assert_equal(numerical1, Dcal, 1e-5)); Matrix numerical2 = numericalDerivative22(calibrate_, K, pi); - CHECK(assert_equal(numerical2,Dp,1e-5)); + CHECK(assert_equal(numerical2, Dp, 1e-5)); } /* ************************************************************************* */ -TEST( Cal3Unified, assert_equal) -{ - CHECK(assert_equal(K,K,1e-9)); -} +TEST(Cal3Unified, Equal) { CHECK(assert_equal(K, K, 1e-9)); } /* ************************************************************************* */ -TEST( Cal3Unified, retract) -{ - Cal3Unified expected(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, - 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1); - Vector d(10); +TEST(Cal3Unified, Retract) { + Cal3Unified expected(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, 1e-3 + 7, + 2.0 * 1e-3 + 8, 3.0 * 1e-3 + 9, 4.0 * 1e-3 + 10, + 0.1 + 1); + + EXPECT_LONGS_EQUAL(Cal3Unified::Dim(), 10); + EXPECT_LONGS_EQUAL(expected.dim(), 10); + + Vector10 d; d << 2, 3, 4, 5, 6, 7, 8, 9, 10, 1; Cal3Unified actual = K.retract(d); - CHECK(assert_equal(expected,actual,1e-9)); - CHECK(assert_equal(d,K.localCoordinates(actual),1e-9)); + CHECK(assert_equal(expected, actual, 1e-9)); + CHECK(assert_equal(d, K.localCoordinates(actual), 1e-9)); } /* ************************************************************************* */ -TEST( Cal3Unified, DerivedValue) -{ +TEST(Cal3Unified, DerivedValue) { Values values; Cal3Unified cal(1, 2, 3, 4, 5, 6, 7, 8, 9, 10); Key key = 1; @@ -126,9 +124,24 @@ TEST( Cal3Unified, DerivedValue) Cal3Unified calafter = values.at(key); - CHECK(assert_equal(cal,calafter,1e-9)); + CHECK(assert_equal(cal, calafter, 1e-9)); } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +TEST(Cal3Unified, Print) { + Cal3Unified cal(0, 1, 2, 3, 4, 5, 6, 7, 8, 9); + std::stringstream os; + os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew() + << ", px: " << cal.px() << ", py: " << cal.py() << ", k1: " << cal.k1() + << ", k2: " << cal.k2() << ", p1: " << cal.p1() << ", p2: " << cal.p2() + << ", xi: " << cal.xi(); + + EXPECT(assert_stdout_equal(os.str(), cal)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testCal3_S2.cpp b/gtsam/geometry/tests/testCal3_S2.cpp index 55ea32e32..41be5ea8e 100644 --- a/gtsam/geometry/tests/testCal3_S2.cpp +++ b/gtsam/geometry/tests/testCal3_S2.cpp @@ -11,7 +11,7 @@ /** * @file testCal3_S2.cpp - * @brief Unit tests for transform derivatives + * @brief Unit tests for basic Cal3_S2 calibration model. */ #include @@ -31,90 +31,94 @@ static Point2 p_uv(1320.3, 1740); static Point2 p_xy(2, 3); /* ************************************************************************* */ -TEST( Cal3_S2, easy_constructor) -{ +TEST(Cal3_S2, Constructor) { Cal3_S2 expected(554.256, 554.256, 0, 640 / 2, 480 / 2); - double fov = 60; // degrees - size_t w=640,h=480; - Cal3_S2 actual(fov,w,h); + double fov = 60; // degrees + size_t w = 640, h = 480; + Cal3_S2 actual(fov, w, h); - CHECK(assert_equal(expected,actual,1e-3)); + CHECK(assert_equal(expected, actual, 1e-3)); } /* ************************************************************************* */ -TEST( Cal3_S2, calibrate) -{ - Point2 intrinsic(2,3); +TEST(Cal3_S2, Calibrate) { + Point2 intrinsic(2, 3); Point2 expectedimage(1320.3, 1740); Point2 imagecoordinates = K.uncalibrate(intrinsic); - CHECK(assert_equal(expectedimage,imagecoordinates)); - CHECK(assert_equal(intrinsic,K.calibrate(imagecoordinates))); + CHECK(assert_equal(expectedimage, imagecoordinates)); + CHECK(assert_equal(intrinsic, K.calibrate(imagecoordinates))); } /* ************************************************************************* */ -TEST( Cal3_S2, calibrate_homogeneous) { +TEST(Cal3_S2, CalibrateHomogeneous) { Vector3 intrinsic(2, 3, 1); Vector3 image(1320.3, 1740, 1); - CHECK(assert_equal((Vector)intrinsic,(Vector)K.calibrate(image))); + CHECK(assert_equal((Vector)intrinsic, (Vector)K.calibrate(image))); } /* ************************************************************************* */ -Point2 uncalibrate_(const Cal3_S2& k, const Point2& pt) { return k.uncalibrate(pt); } -TEST( Cal3_S2, Duncalibrate1) -{ - Matrix25 computed; K.uncalibrate(p, computed, boost::none); +Point2 uncalibrate_(const Cal3_S2& k, const Point2& pt) { + return k.uncalibrate(pt); +} + +TEST(Cal3_S2, Duncalibrate1) { + Matrix25 computed; + K.uncalibrate(p, computed, boost::none); Matrix numerical = numericalDerivative21(uncalibrate_, K, p); - CHECK(assert_equal(numerical,computed,1e-8)); + CHECK(assert_equal(numerical, computed, 1e-8)); } /* ************************************************************************* */ -TEST( Cal3_S2, Duncalibrate2) -{ - Matrix computed; K.uncalibrate(p, boost::none, computed); +TEST(Cal3_S2, Duncalibrate2) { + Matrix computed; + K.uncalibrate(p, boost::none, computed); Matrix numerical = numericalDerivative22(uncalibrate_, K, p); - CHECK(assert_equal(numerical,computed,1e-9)); + CHECK(assert_equal(numerical, computed, 1e-9)); } -Point2 calibrate_(const Cal3_S2& k, const Point2& pt) {return k.calibrate(pt); } -/* ************************************************************************* */ -TEST(Cal3_S2, Dcalibrate1) -{ - Matrix computed; - Point2 expected = K.calibrate(p_uv, computed, boost::none); - Matrix numerical = numericalDerivative21(calibrate_, K, p_uv); - CHECK(assert_equal(expected, p_xy, 1e-8)); - CHECK(assert_equal(numerical, computed, 1e-8)); +Point2 calibrate_(const Cal3_S2& k, const Point2& pt) { + return k.calibrate(pt); } /* ************************************************************************* */ -TEST(Cal3_S2, Dcalibrate2) -{ - Matrix computed; - Point2 expected = K.calibrate(p_uv, boost::none, computed); - Matrix numerical = numericalDerivative22(calibrate_, K, p_uv); - CHECK(assert_equal(expected, p_xy, 1e-8)); - CHECK(assert_equal(numerical, computed, 1e-8)); +TEST(Cal3_S2, Dcalibrate1) { + Matrix computed; + Point2 expected = K.calibrate(p_uv, computed, boost::none); + Matrix numerical = numericalDerivative21(calibrate_, K, p_uv); + CHECK(assert_equal(expected, p_xy, 1e-8)); + CHECK(assert_equal(numerical, computed, 1e-8)); } /* ************************************************************************* */ -TEST( Cal3_S2, assert_equal) -{ - CHECK(assert_equal(K,K,1e-9)); +TEST(Cal3_S2, Dcalibrate2) { + Matrix computed; + Point2 expected = K.calibrate(p_uv, boost::none, computed); + Matrix numerical = numericalDerivative22(calibrate_, K, p_uv); + CHECK(assert_equal(expected, p_xy, 1e-8)); + CHECK(assert_equal(numerical, computed, 1e-8)); +} + +/* ************************************************************************* */ +TEST(Cal3_S2, Equal) { + CHECK(assert_equal(K, K, 1e-9)); Cal3_S2 K1(500, 500, 0.1, 640 / 2, 480 / 2); - CHECK(assert_equal(K,K1,1e-9)); + CHECK(assert_equal(K, K1, 1e-9)); } /* ************************************************************************* */ -TEST( Cal3_S2, retract) -{ - Cal3_S2 expected(500+1, 500+2, 0.1+3, 640 / 2+4, 480 / 2+5); - Vector d(5); - d << 1,2,3,4,5; +TEST(Cal3_S2, Retract) { + Cal3_S2 expected(500 + 1, 500 + 2, 0.1 + 3, 640 / 2 + 4, 480 / 2 + 5); + + EXPECT_LONGS_EQUAL(Cal3_S2::Dim(), 5); + EXPECT_LONGS_EQUAL(expected.dim(), 5); + + Vector5 d; + d << 1, 2, 3, 4, 5; Cal3_S2 actual = K.retract(d); - CHECK(assert_equal(expected,actual,1e-7)); - CHECK(assert_equal(d,K.localCoordinates(actual),1e-7)); + CHECK(assert_equal(expected, actual, 1e-7)); + CHECK(assert_equal(d, K.localCoordinates(actual), 1e-7)); } /* ************************************************************************* */ @@ -122,18 +126,17 @@ TEST(Cal3_S2, between) { Cal3_S2 k1(5, 5, 5, 5, 5), k2(5, 6, 7, 8, 9); Matrix H1, H2; - EXPECT(assert_equal(Cal3_S2(0,1,2,3,4), k1.between(k2, H1, H2))); + EXPECT(assert_equal(Cal3_S2(0, 1, 2, 3, 4), k1.between(k2, H1, H2))); EXPECT(assert_equal(-I_5x5, H1)); EXPECT(assert_equal(I_5x5, H2)); - } /* ************************************************************************* */ TEST(Cal3_S2, Print) { Cal3_S2 cal(5, 5, 5, 5, 5); std::stringstream os; - os << "{fx: " << cal.fx() << ", fy: " << cal.fy() << ", s:" << cal.skew() << ", px:" << cal.px() - << ", py:" << cal.py() << "}"; + os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew() + << ", px: " << cal.px() << ", py: " << cal.py(); EXPECT(assert_stdout_equal(os.str(), cal)); } @@ -144,4 +147,3 @@ int main() { return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - diff --git a/gtsam/geometry/tests/testCal3_S2Stereo.cpp b/gtsam/geometry/tests/testCal3_S2Stereo.cpp new file mode 100644 index 000000000..070eee8fe --- /dev/null +++ b/gtsam/geometry/tests/testCal3_S2Stereo.cpp @@ -0,0 +1,129 @@ +/* ---------------------------------------------------------------------------- + + * 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 testCal3_S2Stereo.cpp + * @brief Unit tests for stereo-rig calibration model. + */ + +#include +#include +#include +#include +#include + +using namespace gtsam; + +GTSAM_CONCEPT_TESTABLE_INST(Cal3_S2Stereo) +GTSAM_CONCEPT_MANIFOLD_INST(Cal3_S2Stereo) + +static Cal3_S2Stereo K(500, 500, 0.1, 640 / 2, 480 / 2, 1); +static Point2 p(1, -2); +static Point2 p_uv(1320.3, 1740); +static Point2 p_xy(2, 3); + +/* ************************************************************************* */ +TEST(Cal3_S2Stereo, Constructor) { + Cal3_S2Stereo expected(554.256, 554.256, 0, 640 / 2, 480 / 2, 3); + + double fov = 60; // degrees + size_t w = 640, h = 480; + Cal3_S2Stereo actual(fov, w, h, 3); + + CHECK(assert_equal(expected, actual, 1e-3)); +} + +/* ************************************************************************* */ +TEST(Cal3_S2Stereo, Calibrate) { + Point2 intrinsic(2, 3); + Point2 expectedimage(1320.3, 1740); + Point2 imagecoordinates = K.uncalibrate(intrinsic); + CHECK(assert_equal(expectedimage, imagecoordinates)); + CHECK(assert_equal(intrinsic, K.calibrate(imagecoordinates))); +} + +/* ************************************************************************* */ +TEST(Cal3_S2Stereo, CalibrateHomogeneous) { + Vector3 intrinsic(2, 3, 1); + Vector3 image(1320.3, 1740, 1); + CHECK(assert_equal(intrinsic, K.calibrate(image))); +} + +/* ************************************************************************* */ +Point2 uncalibrate_(const Cal3_S2Stereo& k, const Point2& pt) { + return k.uncalibrate(pt); +} + +TEST(Cal3_S2Stereo, Duncalibrate) { + Matrix26 Dcal; + Matrix22 Dp; + K.uncalibrate(p, Dcal, Dp); + + Matrix numerical1 = numericalDerivative21(uncalibrate_, K, p); + CHECK(assert_equal(numerical1, Dcal, 1e-8)); + Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p); + CHECK(assert_equal(numerical2, Dp, 1e-9)); +} + +Point2 calibrate_(const Cal3_S2Stereo& K, const Point2& pt) { + return K.calibrate(pt); +} +/* ************************************************************************* */ +TEST(Cal3_S2Stereo, Dcalibrate) { + Matrix26 Dcal; + Matrix22 Dp; + Point2 expected = K.calibrate(p_uv, Dcal, Dp); + CHECK(assert_equal(expected, p_xy, 1e-8)); + + Matrix numerical1 = numericalDerivative21(calibrate_, K, p_uv); + CHECK(assert_equal(numerical1, Dcal, 1e-8)); + Matrix numerical2 = numericalDerivative22(calibrate_, K, p_uv); + CHECK(assert_equal(numerical2, Dp, 1e-8)); +} + +/* ************************************************************************* */ +TEST(Cal3_S2Stereo, Equal) { + CHECK(assert_equal(K, K, 1e-9)); + + Cal3_S2Stereo K1(500, 500, 0.1, 640 / 2, 480 / 2, 1); + CHECK(assert_equal(K, K1, 1e-9)); +} + +/* ************************************************************************* */ +TEST(Cal3_S2Stereo, Retract) { + Cal3_S2Stereo expected(500 + 1, 500 + 2, 0.1 + 3, 640 / 2 + 4, 480 / 2 + 5, + 7); + EXPECT_LONGS_EQUAL(Cal3_S2Stereo::Dim(), 6); + EXPECT_LONGS_EQUAL(expected.dim(), 6); + + Vector6 d; + d << 1, 2, 3, 4, 5, 6; + Cal3_S2Stereo actual = K.retract(d); + CHECK(assert_equal(expected, actual, 1e-7)); + CHECK(assert_equal(d, K.localCoordinates(actual), 1e-7)); +} + +/* ************************************************************************* */ +TEST(Cal3_S2Stereo, Print) { + Cal3_S2Stereo cal(5, 5, 5, 5, 5, 2); + std::stringstream os; + os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew() + << ", px: " << cal.px() << ", py: " << cal.py() + << ", b: " << cal.baseline(); + EXPECT(assert_stdout_equal(os.str(), cal)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 0d739c138..b33e7ad6e 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -849,43 +849,41 @@ namespace gtsam }; #include - class Cal3_S2 - { - // Standard Constructors - Cal3_S2(); - Cal3_S2(double fx, double fy, double s, double u0, double v0); - Cal3_S2(Vector v); - Cal3_S2(double fov, int w, int h); +class Cal3_S2 { + // Standard Constructors + Cal3_S2(); + Cal3_S2(double fx, double fy, double s, double u0, double v0); + Cal3_S2(Vector v); + Cal3_S2(double fov, int w, int h); - // Testable - void print(string s) const; - bool equals(const gtsam::Cal3_S2 &rhs, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::Cal3_S2& rhs, double tol) const; - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::Cal3_S2 retract(Vector v) const; - Vector localCoordinates(const gtsam::Cal3_S2 &c) const; + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Cal3_S2 retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3_S2& c) const; - // Action on Point2 - gtsam::Point2 calibrate(const gtsam::Point2 &p) const; - gtsam::Point2 uncalibrate(const gtsam::Point2 &p) const; + // Action on Point2 + gtsam::Point2 calibrate(const gtsam::Point2& p) const; + gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; - // Standard Interface - double fx() const; - double fy() const; - double skew() const; - double px() const; - double py() const; - gtsam::Point2 principalPoint() const; - Vector vector() const; - Matrix K() const; - Matrix matrix() const; - Matrix matrix_inverse() const; + // Standard Interface + double fx() const; + double fy() const; + double skew() const; + double px() const; + double py() const; + gtsam::Point2 principalPoint() const; + Vector vector() const; + Matrix K() const; + Matrix inverse() const; - // enabling serialization functionality - void serialize() const; - }; + // enabling serialization functionality + void serialize() const; +}; #include virtual class Cal3DS2_Base diff --git a/gtsam/inference/Symbol.h b/gtsam/inference/Symbol.h index 469082f16..42cdbb1c3 100644 --- a/gtsam/inference/Symbol.h +++ b/gtsam/inference/Symbol.h @@ -167,10 +167,11 @@ inline Key Z(std::uint64_t j) { return Symbol('z', j); } /** Generates symbol shorthands with alternative names different than the * one-letter predefined ones. */ class SymbolGenerator { - const char c_; + const unsigned char c_; public: - SymbolGenerator(const char c) : c_(c) {} + constexpr SymbolGenerator(const unsigned char c) : c_(c) {} Symbol operator()(const std::uint64_t j) const { return Symbol(c_, j); } + constexpr unsigned char chr() const { return c_; } }; /// traits diff --git a/gtsam/inference/tests/testKey.cpp b/gtsam/inference/tests/testKey.cpp index 64674c36f..98c5d36bf 100644 --- a/gtsam/inference/tests/testKey.cpp +++ b/gtsam/inference/tests/testKey.cpp @@ -59,6 +59,12 @@ TEST(Key, SymbolGenerator) { EXPECT(assert_equal(a1, ddz1)); } +/* ************************************************************************* */ +TEST(Key, SymbolGeneratorConstexpr) { + constexpr auto Z = gtsam::SymbolGenerator('x'); + EXPECT(assert_equal(Z.chr(), 'x')); +} + /* ************************************************************************* */ template Key KeyTestValue(); diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index 319129840..d4100b00a 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -11,13 +11,12 @@ /** * @file TranslationRecovery.cpp - * @author Frank Dellaert + * @author Frank Dellaert, Akshay Krishnan * @date March 2020 * @brief Source code for recovering translations when rotations are given */ -#include - +#include #include #include #include @@ -27,11 +26,45 @@ #include #include #include +#include #include +#include +#include + using namespace gtsam; using namespace std; +TranslationRecovery::TranslationRecovery( + const TranslationRecovery::TranslationEdges &relativeTranslations, + const LevenbergMarquardtParams &lmParams) + : params_(lmParams) { + // Some relative translations may be zero. We treat nodes that have a zero + // relativeTranslation as a single node. + + // A DSFMap is used to find sets of nodes that have a zero relative + // translation. Add the nodes in each edge to the DSFMap, and merge nodes that + // are connected by a zero relative translation. + DSFMap sameTranslationDSF; + for (const auto &edge : relativeTranslations) { + Key key1 = sameTranslationDSF.find(edge.key1()); + Key key2 = sameTranslationDSF.find(edge.key2()); + if (key1 != key2 && edge.measured().equals(Unit3(0.0, 0.0, 0.0))) { + sameTranslationDSF.merge(key1, key2); + } + } + // Use only those edges for which two keys have a distinct root in the DSFMap. + for (const auto &edge : relativeTranslations) { + Key key1 = sameTranslationDSF.find(edge.key1()); + Key key2 = sameTranslationDSF.find(edge.key2()); + if (key1 == key2) continue; + relativeTranslations_.emplace_back(key1, key2, edge.measured(), + edge.noiseModel()); + } + // Store the DSF map for post-processing results. + sameTranslationNodes_ = sameTranslationDSF.sets(); +} + NonlinearFactorGraph TranslationRecovery::buildGraph() const { NonlinearFactorGraph graph; @@ -44,13 +77,14 @@ NonlinearFactorGraph TranslationRecovery::buildGraph() const { return graph; } -void TranslationRecovery::addPrior(const double scale, - NonlinearFactorGraph *graph, - const SharedNoiseModel &priorNoiseModel) const { +void TranslationRecovery::addPrior( + const double scale, NonlinearFactorGraph *graph, + const SharedNoiseModel &priorNoiseModel) const { auto edge = relativeTranslations_.begin(); - graph->emplace_shared >(edge->key1(), Point3(0, 0, 0), priorNoiseModel); - graph->emplace_shared >(edge->key2(), scale * edge->measured().point3(), - edge->noiseModel()); + graph->emplace_shared >(edge->key1(), Point3(0, 0, 0), + priorNoiseModel); + graph->emplace_shared >( + edge->key2(), scale * edge->measured().point3(), edge->noiseModel()); } Values TranslationRecovery::initalizeRandomly() const { @@ -77,6 +111,19 @@ Values TranslationRecovery::run(const double scale) const { const Values initial = initalizeRandomly(); LevenbergMarquardtOptimizer lm(graph, initial, params_); Values result = lm.optimize(); + + // Nodes that were not optimized are stored in sameTranslationNodes_ as a map + // from a key that was optimized to keys that were not optimized. Iterate over + // map and add results for keys not optimized. + for (const auto &optimizedAndDuplicateKeys : sameTranslationNodes_) { + Key optimizedKey = optimizedAndDuplicateKeys.first; + std::set duplicateKeys = optimizedAndDuplicateKeys.second; + // Add the result for the duplicate key if it does not already exist. + for (const Key duplicateKey : duplicateKeys) { + if (result.exists(duplicateKey)) continue; + result.insert(duplicateKey, result.at(optimizedKey)); + } + } return result; } diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h index d5538f91b..c99836853 100644 --- a/gtsam/sfm/TranslationRecovery.h +++ b/gtsam/sfm/TranslationRecovery.h @@ -16,14 +16,16 @@ * @brief Recovering translations in an epipolar graph when rotations are given. */ +#include +#include +#include +#include + #include #include #include #include -#include -#include - namespace gtsam { // Set up an optimization problem for the unknown translations Ti in the world @@ -52,23 +54,30 @@ class TranslationRecovery { using TranslationEdges = std::vector>; private: + // Translation directions between camera pairs. TranslationEdges relativeTranslations_; + + // Parameters used by the LM Optimizer. LevenbergMarquardtParams params_; + // Map from a key in the graph to a set of keys that share the same + // translation. + std::map> sameTranslationNodes_; + public: /** * @brief Construct a new Translation Recovery object * * @param relativeTranslations the relative translations, in world coordinate - * frames, vector of BinaryMeasurements of Unit3, where each key of a measurement - * is a point in 3D. + * frames, vector of BinaryMeasurements of Unit3, where each key of a + * measurement is a point in 3D. * @param lmParams (optional) gtsam::LavenbergMarquardtParams that can be * used to modify the parameters for the LM optimizer. By default, uses the - * default LM parameters. + * default LM parameters. */ - TranslationRecovery(const TranslationEdges &relativeTranslations, - const LevenbergMarquardtParams &lmParams = LevenbergMarquardtParams()) - : relativeTranslations_(relativeTranslations), params_(lmParams) {} + TranslationRecovery( + const TranslationEdges &relativeTranslations, + const LevenbergMarquardtParams &lmParams = LevenbergMarquardtParams()); /** * @brief Build the factor graph to do the optimization. @@ -108,8 +117,8 @@ class TranslationRecovery { * * @param poses SE(3) ground truth poses stored as Values * @param edges pairs (a,b) for which a measurement w_aZb will be generated. - * @return TranslationEdges vector of binary measurements where the keys are - * the cameras and the measurement is the simulated Unit3 translation + * @return TranslationEdges vector of binary measurements where the keys are + * the cameras and the measurement is the simulated Unit3 translation * direction between the cameras. */ static TranslationEdges SimulateMeasurements( diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp index eb34ba803..7260fd5af 100644 --- a/tests/testTranslationRecovery.cpp +++ b/tests/testTranslationRecovery.cpp @@ -11,19 +11,29 @@ /** * @file testTranslationRecovery.cpp - * @author Frank Dellaert + * @author Frank Dellaert, Akshay Krishnan * @date March 2020 * @brief test recovering translations when rotations are given. */ -#include - #include + +#include #include using namespace std; using namespace gtsam; +// Returns the Unit3 direction as measured in the binary measurement, but +// computed from the input poses. Helper function used in the unit tests. +Unit3 GetDirectionFromPoses(const Values& poses, + const BinaryMeasurement& unitTranslation) { + const Pose3 wTa = poses.at(unitTranslation.key1()), + wTb = poses.at(unitTranslation.key2()); + const Point3 Ta = wTa.translation(), Tb = wTb.translation(); + return Unit3(Tb - Ta); +} + /* ************************************************************************* */ // We read the BAL file, which has 3 cameras in it, with poses. We then assume // the rotations are correct, but translations have to be estimated from @@ -48,43 +58,186 @@ TEST(TranslationRecovery, BAL) { const auto relativeTranslations = TranslationRecovery::SimulateMeasurements( poses, {{0, 1}, {0, 2}, {1, 2}}); - // Check - Unit3 w_aZb_stored; // measurement between 0 and 1 stored for next unit test - for(auto& unitTranslation : relativeTranslations) { - const Pose3 wTa = poses.at(unitTranslation.key1()), - wTb = poses.at(unitTranslation.key2()); - const Point3 Ta = wTa.translation(), Tb = wTb.translation(); - const Unit3 w_aZb = unitTranslation.measured(); - EXPECT(assert_equal(Unit3(Tb - Ta), w_aZb)); - if(unitTranslation.key1() == 0 && unitTranslation.key2() == 1) { - w_aZb_stored = unitTranslation.measured(); - } + // Check simulated measurements. + for (auto& unitTranslation : relativeTranslations) { + EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation), + unitTranslation.measured())); } TranslationRecovery algorithm(relativeTranslations); const auto graph = algorithm.buildGraph(); EXPECT_LONGS_EQUAL(3, graph.size()); - // Translation recovery, version 1 + // Run translation recovery const double scale = 2.0; const auto result = algorithm.run(scale); // Check result for first two translations, determined by prior EXPECT(assert_equal(Point3(0, 0, 0), result.at(0))); - EXPECT(assert_equal(Point3(2 * w_aZb_stored.point3()), result.at(1))); + EXPECT(assert_equal( + Point3(2 * GetDirectionFromPoses(poses, relativeTranslations[0])), + result.at(1))); // Check that the third translations is correct Point3 Ta = poses.at(0).translation(); Point3 Tb = poses.at(1).translation(); Point3 Tc = poses.at(2).translation(); - Point3 expected = - (Tc - Ta) * (scale / (Tb - Ta).norm()); + Point3 expected = (Tc - Ta) * (scale / (Tb - Ta).norm()); EXPECT(assert_equal(expected, result.at(2), 1e-4)); // TODO(frank): how to get stats back? // EXPECT_DOUBLES_EQUAL(0.0199833, actualError, 1e-5); } +TEST(TranslationRecovery, TwoPoseTest) { + // Create a dataset with 2 poses. + // __ __ + // \/ \/ + // 0 _____ 1 + // + // 0 and 1 face in the same direction but have a translation offset. + Values poses; + poses.insert(0, Pose3(Rot3(), Point3(0, 0, 0))); + poses.insert(1, Pose3(Rot3(), Point3(2, 0, 0))); + + auto relativeTranslations = + TranslationRecovery::SimulateMeasurements(poses, {{0, 1}}); + + // Check simulated measurements. + for (auto& unitTranslation : relativeTranslations) { + EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation), + unitTranslation.measured())); + } + + TranslationRecovery algorithm(relativeTranslations); + const auto graph = algorithm.buildGraph(); + EXPECT_LONGS_EQUAL(1, graph.size()); + + // Run translation recovery + const auto result = algorithm.run(/*scale=*/3.0); + + // Check result for first two translations, determined by prior + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0))); + EXPECT(assert_equal(Point3(3, 0, 0), result.at(1))); +} + +TEST(TranslationRecovery, ThreePoseTest) { + // Create a dataset with 3 poses. + // __ __ + // \/ \/ + // 0 _____ 1 + // \ __ / + // \\// + // 3 + // + // 0 and 1 face in the same direction but have a translation offset. 3 is in + // the same direction as 0 and 1, in between 0 and 1, with some Y axis offset. + + Values poses; + poses.insert(0, Pose3(Rot3(), Point3(0, 0, 0))); + poses.insert(1, Pose3(Rot3(), Point3(2, 0, 0))); + poses.insert(3, Pose3(Rot3(), Point3(1, -1, 0))); + + auto relativeTranslations = TranslationRecovery::SimulateMeasurements( + poses, {{0, 1}, {1, 3}, {3, 0}}); + + // Check simulated measurements. + for (auto& unitTranslation : relativeTranslations) { + EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation), + unitTranslation.measured())); + } + + TranslationRecovery algorithm(relativeTranslations); + const auto graph = algorithm.buildGraph(); + EXPECT_LONGS_EQUAL(3, graph.size()); + + const auto result = algorithm.run(/*scale=*/3.0); + + // Check result + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0))); + EXPECT(assert_equal(Point3(3, 0, 0), result.at(1))); + EXPECT(assert_equal(Point3(1.5, -1.5, 0), result.at(3))); +} + +TEST(TranslationRecovery, ThreePosesIncludingZeroTranslation) { + // Create a dataset with 3 poses. + // __ __ + // \/ \/ + // 0 _____ 1 + // 2 <| + // + // 0 and 1 face in the same direction but have a translation offset. 2 is at + // the same point as 1 but is rotated, with little FOV overlap. + Values poses; + poses.insert(0, Pose3(Rot3(), Point3(0, 0, 0))); + poses.insert(1, Pose3(Rot3(), Point3(2, 0, 0))); + poses.insert(2, Pose3(Rot3::RzRyRx(-M_PI / 2, 0, 0), Point3(2, 0, 0))); + + auto relativeTranslations = + TranslationRecovery::SimulateMeasurements(poses, {{0, 1}, {1, 2}}); + + // Check simulated measurements. + for (auto& unitTranslation : relativeTranslations) { + EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation), + unitTranslation.measured())); + } + + TranslationRecovery algorithm(relativeTranslations); + const auto graph = algorithm.buildGraph(); + // There is only 1 non-zero translation edge. + EXPECT_LONGS_EQUAL(1, graph.size()); + + // Run translation recovery + const auto result = algorithm.run(/*scale=*/3.0); + + // Check result + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0))); + EXPECT(assert_equal(Point3(3, 0, 0), result.at(1))); + EXPECT(assert_equal(Point3(3, 0, 0), result.at(2))); +} + +TEST(TranslationRecovery, FourPosesIncludingZeroTranslation) { + // Create a dataset with 4 poses. + // __ __ + // \/ \/ + // 0 _____ 1 + // \ __ 2 <| + // \\// + // 3 + // + // 0 and 1 face in the same direction but have a translation offset. 2 is at + // the same point as 1 but is rotated, with very little FOV overlap. 3 is in + // the same direction as 0 and 1, in between 0 and 1, with some Y axis offset. + + Values poses; + poses.insert(0, Pose3(Rot3(), Point3(0, 0, 0))); + poses.insert(1, Pose3(Rot3(), Point3(2, 0, 0))); + poses.insert(2, Pose3(Rot3::RzRyRx(-M_PI / 2, 0, 0), Point3(2, 0, 0))); + poses.insert(3, Pose3(Rot3(), Point3(1, -1, 0))); + + auto relativeTranslations = TranslationRecovery::SimulateMeasurements( + poses, {{0, 1}, {1, 2}, {1, 3}, {3, 0}}); + + // Check simulated measurements. + for (auto& unitTranslation : relativeTranslations) { + EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation), + unitTranslation.measured())); + } + + TranslationRecovery algorithm(relativeTranslations); + const auto graph = algorithm.buildGraph(); + EXPECT_LONGS_EQUAL(3, graph.size()); + + // Run translation recovery + const auto result = algorithm.run(/*scale=*/4.0); + + // Check result + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0))); + EXPECT(assert_equal(Point3(4, 0, 0), result.at(1))); + EXPECT(assert_equal(Point3(4, 0, 0), result.at(2))); + EXPECT(assert_equal(Point3(2, -2, 0), result.at(3))); +} + /* ************************************************************************* */ int main() { TestResult tr;