From 70bab8e00c0e310c9bd2c9fa1486e6723861f093 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 2 Dec 2020 17:15:10 -0500 Subject: [PATCH] Consistent and better formatting --- gtsam/geometry/Cal3Bundler.cpp | 27 ++-- gtsam/geometry/Cal3Bundler.h | 47 ++---- gtsam/geometry/Cal3DS2.cpp | 11 +- gtsam/geometry/Cal3DS2.h | 33 ++-- gtsam/geometry/Cal3DS2_Base.cpp | 29 ++-- gtsam/geometry/Cal3Unified.h | 41 +++-- gtsam/geometry/Cal3_S2Stereo.cpp | 3 +- gtsam/geometry/Cal3_S2Stereo.h | 179 ++++++++++----------- gtsam/geometry/tests/testCal3Bundler.cpp | 47 +++--- gtsam/geometry/tests/testCal3DFisheye.cpp | 7 +- gtsam/geometry/tests/testCal3DS2.cpp | 73 ++++----- gtsam/geometry/tests/testCal3Unified.cpp | 67 ++++---- gtsam/geometry/tests/testCal3_S2.cpp | 61 +++---- gtsam/geometry/tests/testCal3_S2Stereo.cpp | 52 +----- 14 files changed, 298 insertions(+), 379 deletions(-) diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index 31beac73e..e03562452 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -15,11 +15,11 @@ * @author ydjian */ -#include #include +#include +#include #include #include -#include namespace gtsam { @@ -39,9 +39,7 @@ Vector4 Cal3Bundler::k() const { } /* ************************************************************************* */ -Vector3 Cal3Bundler::vector() const { - return Vector3(fx_, k1_, k2_); -} +Vector3 Cal3Bundler::vector() const { return Vector3(fx_, k1_, k2_); } /* ************************************************************************* */ std::ostream& operator<<(std::ostream& os, const Cal3Bundler& cal) { @@ -52,7 +50,8 @@ std::ostream& operator<<(std::ostream& os, const Cal3Bundler& cal) { /* ************************************************************************* */ void Cal3Bundler::print(const std::string& s) const { - gtsam::print((Vector)(Vector(5) << fx_, k1_, k2_, u0_, v0_).finished(), s + ".K"); + gtsam::print((Vector)(Vector(5) << fx_, k1_, k2_, u0_, v0_).finished(), + s + ".K"); } /* ************************************************************************* */ @@ -64,8 +63,8 @@ bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const { } /* ************************************************************************* */ -Point2 Cal3Bundler::uncalibrate(const Point2& p, // - OptionalJacobian<2, 3> Dcal, OptionalJacobian<2, 2> Dp) const { +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) @@ -93,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_) / 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); @@ -118,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); diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index 50b392096..0016ded2d 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -30,7 +30,6 @@ namespace gtsam { * \nosubgrouping */ class GTSAM_EXPORT Cal3Bundler : public Cal3 { - private: double k1_ = 0.0f, k2_ = 0.0f; ///< radial distortion double tol_ = 1e-5; ///< tolerance value when calibrating @@ -40,7 +39,6 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 { // but are treated as constants. public: - enum { dimension = 3 }; /// @name Standard Constructors @@ -83,40 +81,28 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 { /// @{ /// distorsion parameter k1 - inline double k1() const { - return k1_; - } + inline double k1() const { return k1_; } /// distorsion parameter k2 - inline double k2() const { - return k2_; - } + inline double k2() const { return k2_; } /// image center in x - inline double px() const { - return u0_; - } + inline double px() const { return u0_; } /// image center in y - inline double py() const { - return v0_; - } + 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) + Matrix3 K() const override; ///< 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 { - return u0_; - } + inline double u0() const { return u0_; } /// get parameter v0 - inline double v0() const { - return v0_; - } + inline double v0() const { return v0_; } #endif /** @@ -128,7 +114,7 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 { * @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 @@ -138,8 +124,7 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 { * @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 @@ -172,15 +157,14 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 { } private: - /// @} /// @name Advanced Interface /// @{ /** Serialization function */ friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int /*version*/) { + 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_); @@ -189,13 +173,12 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 { } /// @} - }; -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 71aa2738d..f93386ea7 100644 --- a/gtsam/geometry/Cal3DS2.cpp +++ b/gtsam/geometry/Cal3DS2.cpp @@ -16,11 +16,11 @@ * @author Varun Agrawal */ -#include #include +#include +#include #include #include -#include namespace gtsam { @@ -31,9 +31,7 @@ std::ostream& operator<<(std::ostream& os, const Cal3DS2& cal) { } /* ************************************************************************* */ -void Cal3DS2::print(const std::string& s_) const { - Base::print(s_); -} +void Cal3DS2::print(const std::string& s_) const { Base::print(s_); } /* ************************************************************************* */ bool Cal3DS2::equals(const Cal3DS2& K, double tol) const { @@ -50,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 ad4406b76..58d35c2ec 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -11,7 +11,8 @@ /** * @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 @@ -31,11 +32,9 @@ namespace gtsam { * \nosubgrouping */ class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base { - using Base = Cal3DS2_Base; -public: - + public: enum { dimension = 9 }; /// @name Standard Constructors @@ -54,7 +53,7 @@ public: /// @name Advanced Constructors /// @{ - Cal3DS2(const Vector9 &v) : Base(v) {} + Cal3DS2(const Vector9& v) : Base(v) {} /// @} /// @name Testable @@ -75,10 +74,10 @@ 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 override { return Dim(); } @@ -97,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 71ce32ccb..a3f7026b9 100644 --- a/gtsam/geometry/Cal3DS2_Base.cpp +++ b/gtsam/geometry/Cal3DS2_Base.cpp @@ -16,11 +16,11 @@ * @author Varun Agrawal */ -#include #include +#include +#include #include #include -#include namespace gtsam { @@ -54,23 +54,23 @@ bool Cal3DS2_Base::equals(const Cal3DS2_Base& K, double tol) const { } /* ************************************************************************* */ -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; @@ -84,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; } @@ -100,7 +100,7 @@ Point2 Cal3DS2_Base::uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal, 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); @@ -190,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/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index 4c456ec24..ee388c8c1 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -28,20 +28,21 @@ 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² + P.y² + 1}), P.y / (1 + xi * \sqrt{P.x² + P.y² + 1})] + * 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; @@ -49,7 +50,6 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base { double xi_ = 0.0f; ///< mirror parameter public: - enum { dimension = 10 }; /// @name Standard Constructors @@ -90,10 +90,10 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base { /// @{ /// mirror parameter - inline double xi() const { return xi_;} + inline double xi() const { return xi_; } /// Return all parameters as a vector - Vector10 vector() const ; + Vector10 vector() const; /** * convert intrinsic coordinates xy to image coordinates uv @@ -103,8 +103,8 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base { * @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, @@ -121,10 +121,10 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base { /// @{ /// 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 - Vector localCoordinates(const Cal3Unified& T2) const ; + Vector localCoordinates(const Cal3Unified& T2) const; /// Return dimensions of calibration manifold object virtual size_t dim() const override { return Dim(); } @@ -134,25 +134,20 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base { /// @} -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_S2Stereo.cpp b/gtsam/geometry/Cal3_S2Stereo.cpp index 8bd733e95..56ceaf516 100644 --- a/gtsam/geometry/Cal3_S2Stereo.cpp +++ b/gtsam/geometry/Cal3_S2Stereo.cpp @@ -38,7 +38,8 @@ void Cal3_S2Stereo::print(const std::string& s) const { /* ************************************************************************* */ bool Cal3_S2Stereo::equals(const Cal3_S2Stereo& other, double tol) const { const Cal3_S2* base = dynamic_cast(&other); - return (Cal3_S2::equals(*base, tol) && std::fabs(b_ - other.baseline()) < tol); + return (Cal3_S2::equals(*base, tol) && + std::fabs(b_ - other.baseline()) < tol); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h index ee316251a..d7bf34e61 100644 --- a/gtsam/geometry/Cal3_S2Stereo.h +++ b/gtsam/geometry/Cal3_S2Stereo.h @@ -22,121 +22,116 @@ 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. +/** + * @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: + public: + enum { dimension = 6 }; - enum { dimension = 6 }; + ///< shared pointer to stereo calibration object + using shared_ptr = boost::shared_ptr; - ///< shared pointer to stereo calibration object - using shared_ptr = boost::shared_ptr; + /// @name Standard Constructors + /// @ - /// @name Standard Constructors - /// @ + /// default calibration leaves coordinates unchanged + Cal3_S2Stereo() = default; - /// 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 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)) {} - /// 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) {} - /// 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) {} + /// @} + /// @name Testable + /// @{ - /// @} - /// @name Testable - /// @{ + /// Output stream operator + GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, + const Cal3_S2Stereo& cal); - /// Output stream operator - GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, - const Cal3_S2Stereo& cal); + /// print with optional string + void print(const std::string& s = "") const override; - /// print with optional string - void print(const std::string& s = "") const override; + /// Check if equal up to specified tolerance + bool equals(const Cal3_S2Stereo& other, double tol = 10e-9) const; - /// Check if equal up to specified tolerance - bool equals(const Cal3_S2Stereo& other, double tol = 10e-9) const; + /// @} + /// @name Standard Interface + /// @{ - /// @} - /// @name Standard Interface - /// @{ + /// return calibration, same for left and right + const Cal3_S2& calibration() const { return *this; } - /// return calibration, same for left and right - const Cal3_S2& calibration() const { return *this; } + /// return calibration matrix K, same for left and right + Matrix3 K() const override { return Cal3_S2::K(); } - /// return calibration matrix K, same for left and right - Matrix3 K() const override { return Cal3_S2::K(); } + /// return baseline + inline double baseline() const { return b_; } - /// return baseline - inline double baseline() const { return b_; } + /// vectorized form (column-wise) + Vector6 vector() const { + Vector6 v; + v << Cal3_S2::vector(), b_; + return v; + } - /// vectorized form (column-wise) - Vector6 vector() const { - Vector6 v; - v << Cal3_S2::vector(), b_; - return v; - } + /// @} + /// @name Manifold + /// @{ - /// @} - /// @name Manifold - /// @{ + /// return DOF, dimensionality of tangent space + inline size_t dim() const override { return Dim(); } - /// return DOF, dimensionality of tangent space - inline size_t dim() const override { return Dim(); } + /// return DOF, dimensionality of tangent space + inline static size_t Dim() { return dimension; } - /// return DOF, dimensionality of tangent space - inline 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(fx() + d(0), fy() + d(1), skew() + d(2), px() + d(3), + py() + d(4), b_ + d(5)); + } - /// 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)); - } + /// Unretraction for the calibration + Vector6 localCoordinates(const Cal3_S2Stereo& T2) const { + return T2.vector() - vector(); + } - /// Unretraction for the calibration - Vector6 localCoordinates(const Cal3_S2Stereo& T2) const { - return T2.vector() - vector(); - } + /// @} + /// @name Advanced Interface + /// @{ - /// @} - /// @name Advanced Interface - /// @{ + 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_); + } + /// @} +}; - 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_); - } - /// @} +// Define GTSAM traits +template <> +struct traits : public internal::Manifold {}; - }; +template <> +struct traits : public internal::Manifold { +}; - // 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 8e6fe983f..b821d295b 100644 --- a/gtsam/geometry/tests/testCal3Bundler.cpp +++ b/gtsam/geometry/tests/testCal3Bundler.cpp @@ -26,27 +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) { 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()) ; + 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); @@ -54,20 +54,24 @@ 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); } +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)); + 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)); + CHECK(assert_equal(numerical1, Dcal, 1e-7)); + CHECK(assert_equal(numerical2, Dp, 1e-7)); } /* ************************************************************************* */ @@ -79,14 +83,12 @@ 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) { @@ -99,8 +101,8 @@ TEST(Cal3Bundler, retract) { 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)); } /* ************************************************************************* */ @@ -114,5 +116,8 @@ TEST(Cal3_S2, Print) { } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testCal3DFisheye.cpp b/gtsam/geometry/tests/testCal3DFisheye.cpp index 7a73e7490..28064a92c 100644 --- a/gtsam/geometry/tests/testCal3DFisheye.cpp +++ b/gtsam/geometry/tests/testCal3DFisheye.cpp @@ -191,16 +191,15 @@ 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)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testCal3DS2.cpp b/gtsam/geometry/tests/testCal3DS2.cpp index e4dc3e806..7ef6e5001 100644 --- a/gtsam/geometry/tests/testCal3DS2.cpp +++ b/gtsam/geometry/tests/testCal3DS2.cpp @@ -14,7 +14,6 @@ * @brief Unit tests for Cal3DS2 calibration model. */ - #include #include #include @@ -26,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) { @@ -80,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; @@ -93,21 +91,21 @@ 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); + 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; + 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)); } /* ************************************************************************* */ @@ -122,5 +120,8 @@ TEST(Cal3DS2, Print) { } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testCal3Unified.cpp b/gtsam/geometry/tests/testCal3Unified.cpp index ff759d1cd..648bb358c 100644 --- a/gtsam/geometry/tests/testCal3Unified.cpp +++ b/gtsam/geometry/tests/testCal3Unified.cpp @@ -20,8 +20,8 @@ #include #include -#include #include +#include using namespace gtsam; @@ -36,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) { @@ -88,27 +86,24 @@ 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); +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); @@ -116,13 +111,12 @@ TEST(Cal3Unified, retract) { 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; @@ -130,7 +124,7 @@ TEST(Cal3Unified, DerivedValue) Cal3Unified calafter = values.at(key); - CHECK(assert_equal(cal,calafter,1e-9)); + CHECK(assert_equal(cal, calafter, 1e-9)); } /* ************************************************************************* */ @@ -146,5 +140,8 @@ TEST(Cal3Unified, Print) { } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +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 9b7941c91..41be5ea8e 100644 --- a/gtsam/geometry/tests/testCal3_S2.cpp +++ b/gtsam/geometry/tests/testCal3_S2.cpp @@ -34,27 +34,27 @@ static Point2 p_xy(2, 3); 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); + 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, 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))); } /* ************************************************************************* */ @@ -63,16 +63,18 @@ Point2 uncalibrate_(const Cal3_S2& k, const Point2& pt) { } TEST(Cal3_S2, Duncalibrate1) { - Matrix25 computed; K.uncalibrate(p, computed, boost::none); + 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); + 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) { @@ -81,42 +83,42 @@ Point2 calibrate_(const Cal3_S2& k, const Point2& 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)); + 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, 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)); + 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)); + 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); + 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; + 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)); } /* ************************************************************************* */ @@ -124,7 +126,7 @@ 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)); } @@ -145,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 index f823a8b97..e6a591b5f 100644 --- a/gtsam/geometry/tests/testCal3_S2Stereo.cpp +++ b/gtsam/geometry/tests/testCal3_S2Stereo.cpp @@ -31,7 +31,7 @@ static Point2 p_uv(1320.3, 1740); static Point2 p_xy(2, 3); /* ************************************************************************* */ -TEST(Cal3_S2Stereo, easy_constructor) { +TEST(Cal3_S2Stereo, Constructor) { Cal3_S2Stereo expected(554.256, 554.256, 0, 640 / 2, 480 / 2, 3); double fov = 60; // degrees @@ -42,7 +42,7 @@ TEST(Cal3_S2Stereo, easy_constructor) { } /* ************************************************************************* */ -TEST(Cal3_S2Stereo, calibrate) { +TEST(Cal3_S2Stereo, Calibrate) { Point2 intrinsic(2, 3); Point2 expectedimage(1320.3, 1740); Point2 imagecoordinates = K.uncalibrate(intrinsic); @@ -51,56 +51,14 @@ TEST(Cal3_S2Stereo, calibrate) { } /* ************************************************************************* */ -TEST(Cal3_S2Stereo, calibrate_homogeneous) { +TEST(Cal3_S2Stereo, CalibrateHomogeneous) { Vector3 intrinsic(2, 3, 1); Vector3 image(1320.3, 1740, 1); CHECK(assert_equal((Vector)intrinsic, (Vector)K.calibrate(image))); } -//TODO(Varun) Add calibrate and uncalibrate methods -// /* ************************************************************************* */ -// Point2 uncalibrate_(const Cal3_S2Stereo& k, const Point2& pt) { -// return k.uncalibrate(pt); -// } - -// TEST(Cal3_S2Stereo, Duncalibrate1) { -// Matrix26 computed; -// K.uncalibrate(p, computed, boost::none); -// Matrix numerical = numericalDerivative21(uncalibrate_, K, p); -// CHECK(assert_equal(numerical, computed, 1e-8)); -// } - -// /* ************************************************************************* */ -// TEST(Cal3_S2Stereo, Duncalibrate2) { -// Matrix computed; -// K.uncalibrate(p, boost::none, computed); -// Matrix numerical = numericalDerivative22(uncalibrate_, K, p); -// CHECK(assert_equal(numerical, computed, 1e-9)); -// } - -// Point2 calibrate_(const Cal3_S2Stereo& k, const Point2& pt) { -// return k.calibrate(pt); -// } -// /* ************************************************************************* */ -// TEST(Cal3_S2Stereo, 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_S2Stereo, 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_S2Stereo, assert_equal) { +TEST(Cal3_S2Stereo, Equal) { CHECK(assert_equal(K, K, 1e-9)); Cal3_S2Stereo K1(500, 500, 0.1, 640 / 2, 480 / 2, 1); @@ -108,7 +66,7 @@ TEST(Cal3_S2Stereo, assert_equal) { } /* ************************************************************************* */ -TEST(Cal3_S2Stereo, retract) { +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);