From 17e9b73fb620b70a854c6747004da3f9aa9d4013 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 1 Dec 2020 17:23:42 -0500 Subject: [PATCH] Refactor Bundler and Fisheye models --- gtsam/geometry/Cal3Bundler.cpp | 30 ++---------- gtsam/geometry/Cal3Bundler.h | 66 ++++++++++++++------------- gtsam/geometry/Cal3Fisheye.cpp | 42 +++-------------- gtsam/geometry/Cal3Fisheye.h | 83 ++++++++++++++-------------------- 4 files changed, 79 insertions(+), 142 deletions(-) diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index a73bfec52..8c524e1cc 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -23,16 +23,6 @@ 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 { Matrix3 K; @@ -59,11 +49,9 @@ void Cal3Bundler::print(const std::string& s) const { /* ************************************************************************* */ 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; + return (std::fabs(f_ - K.f_) < 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); } /* ************************************************************************* */ @@ -150,14 +138,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..dc99f5259 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -28,15 +28,17 @@ namespace gtsam { * @addtogroup geometry * \nosubgrouping */ -class GTSAM_EXPORT Cal3Bundler { +class GTSAM_EXPORT Cal3Bundler : public Cal3 { -private: + 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: image center parameters (u0, v0) are not optimized + // but are constants. + + public: enum { dimension = 3 }; @@ -44,7 +46,7 @@ public: /// @{ /// Default constructor - Cal3Bundler(); + Cal3Bundler() : Cal3(), f_(1), k1_(0), k2_(0), tol_(1e-5) {} /** * Constructor @@ -56,7 +58,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), f_(f), k1_(k1), k2_(k2), tol_(tol) {} virtual ~Cal3Bundler() {} @@ -65,7 +68,7 @@ public: /// @{ /// 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,11 +77,6 @@ public: /// @name Standard Interface /// @{ - Matrix3 K() const; ///< 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_; @@ -109,6 +107,11 @@ public: return v0_; } + Matrix3 K() const; ///< Standard 3*3 calibration matrix + Vector4 k() const; ///< Radial distortion parameters (4 of them, 2 0) + + Vector3 vector() const; + #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 /// get parameter u0 inline double u0() const { @@ -121,12 +124,11 @@ public: } #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 */ @@ -158,23 +160,23 @@ public: /// @name Manifold /// @{ + /// return DOF, dimensionality of tangent space + virtual size_t dim() const { return dimension; } + + /// return DOF, dimensionality of tangent space + 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(f_ + 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 @@ -184,12 +186,12 @@ private: 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_); + ar& boost::serialization::make_nvp( + "Cal3Bundler", boost::serialization::base_object(*this)); + ar& BOOST_SERIALIZATION_NVP(f_); + ar& BOOST_SERIALIZATION_NVP(k1_); + ar& BOOST_SERIALIZATION_NVP(k2_); + ar& BOOST_SERIALIZATION_NVP(tol_); } /// @} diff --git a/gtsam/geometry/Cal3Fisheye.cpp b/gtsam/geometry/Cal3Fisheye.cpp index 1ed1826ad..55020f581 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; @@ -165,24 +147,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..0d8921bd9 100644 --- a/gtsam/geometry/Cal3Fisheye.h +++ b/gtsam/geometry/Cal3Fisheye.h @@ -46,11 +46,10 @@ namespace gtsam { * 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_, k2_, k3_, k4_; ///< fisheye distortion coefficients + double tol_ = 1e-5; ///< tolerance value when calibrating public: enum { dimension = 9 }; @@ -61,17 +60,12 @@ class GTSAM_EXPORT Cal3Fisheye { /// @{ /// 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() : Cal3(), k1_(0), k2_(0), k3_(0), k4_(0), tol_(1e-5) {} 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 +78,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 +101,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 +114,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; @@ -151,7 +137,7 @@ class GTSAM_EXPORT Cal3Fisheye { /// @{ /// 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 +146,21 @@ class GTSAM_EXPORT Cal3Fisheye { /// @name Manifold /// @{ + /// Return dimensions of calibration manifold object + virtual size_t dim() const { return dimension; } + + /// Return dimensions of calibration manifold object + 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 +181,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_);