From 42b053740232b5b64347fecdf89441c0a73f28f9 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 1 Dec 2020 17:23:10 -0500 Subject: [PATCH] Refactor all Cal3D based models --- gtsam/geometry/Cal3DS2.cpp | 8 +-- gtsam/geometry/Cal3DS2.h | 1 + gtsam/geometry/Cal3DS2_Base.cpp | 28 ++------ gtsam/geometry/Cal3DS2_Base.h | 119 +++++++++++--------------------- gtsam/geometry/Cal3Unified.cpp | 15 ++-- 5 files changed, 54 insertions(+), 117 deletions(-) diff --git a/gtsam/geometry/Cal3DS2.cpp b/gtsam/geometry/Cal3DS2.cpp index 070d16c6c..b4595a4dc 100644 --- a/gtsam/geometry/Cal3DS2.cpp +++ b/gtsam/geometry/Cal3DS2.cpp @@ -13,6 +13,7 @@ * @file Cal3DS2.cpp * @date Feb 28, 2010 * @author ydjian + * @author Varun Agrawal */ #include @@ -30,11 +31,8 @@ void Cal3DS2::print(const std::string& s_) const { /* ************************************************************************* */ 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); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index e66c3d124..fe08fc5fb 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -14,6 +14,7 @@ * @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 diff --git a/gtsam/geometry/Cal3DS2_Base.cpp b/gtsam/geometry/Cal3DS2_Base.cpp index d175259f2..016c9dfa2 100644 --- a/gtsam/geometry/Cal3DS2_Base.cpp +++ b/gtsam/geometry/Cal3DS2_Base.cpp @@ -24,25 +24,6 @@ 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; @@ -58,11 +39,10 @@ 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; } /* ************************************************************************* */ diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h index dbd6478e1..536fb1161 100644 --- a/gtsam/geometry/Cal3DS2_Base.h +++ b/gtsam/geometry/Cal3DS2_Base.h @@ -34,50 +34,34 @@ namespace gtsam { * 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) ; + * \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 */ -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_, k2_; // radial 2nd-order and 4th-order + double p1_, p2_; // 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() : Cal3(), k1_(0), k2_(0), p1_(0), p2_(0), tol_(1e-5) {} - 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 +69,19 @@ public: /// @name Advanced Constructors /// @{ - Cal3DS2_Base(const Vector &v) ; + Cal3DS2_Base(const Vector& 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 /// @{ /// 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 +90,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 +123,10 @@ 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; /// @} /// @name Clone @@ -168,31 +139,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/Cal3Unified.cpp b/gtsam/geometry/Cal3Unified.cpp index f4ce0ed75..dc963a46e 100644 --- a/gtsam/geometry/Cal3Unified.cpp +++ b/gtsam/geometry/Cal3Unified.cpp @@ -26,8 +26,8 @@ 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]) {} +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 { @@ -44,12 +44,8 @@ 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; } /* ************************************************************************* */ @@ -144,7 +140,6 @@ Vector10 Cal3Unified::localCoordinates(const Cal3Unified& T2) const { return T2.vector() - vector(); } -} /* ************************************************************************* */ - +} // \ namespace gtsam