diff --git a/gtsam/geometry/Cal3.cpp b/gtsam/geometry/Cal3.cpp index 240d01e12..31e9bf834 100644 --- a/gtsam/geometry/Cal3.cpp +++ b/gtsam/geometry/Cal3.cpp @@ -62,11 +62,11 @@ void Cal3::print(const std::string& s) const { gtsam::print((Matrix)K(), s); } /* ************************************************************************* */ bool Cal3::equals(const Cal3& K, double tol) const { - return (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); + 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); } /* ************************************************************************* */ -} // namespace gtsam +} // \ namespace gtsam diff --git a/gtsam/geometry/Cal3.h b/gtsam/geometry/Cal3.h index 13cd4de69..7384fe958 100644 --- a/gtsam/geometry/Cal3.h +++ b/gtsam/geometry/Cal3.h @@ -68,7 +68,7 @@ void calibrateJacobians(const Cal& calibration, const Point2& pn, */ class GTSAM_EXPORT Cal3 { protected: - double fx_, fy_, s_, u0_, v0_; + double fx_, fy_, s_, u0_, v0_; ///< focal length, skew and principal point public: enum { dimension = 5 }; @@ -113,7 +113,7 @@ class GTSAM_EXPORT Cal3 { const Cal3& cal); /// print with optional string - virtual void print(const std::string& s = "Cal3") const; + 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; diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index fe08fc5fb..58ccae04f 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -32,7 +32,7 @@ namespace gtsam { */ class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base { - typedef Cal3DS2_Base Base; + using Base = Cal3DS2_Base; public: diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h index 536fb1161..0c6a4a3cd 100644 --- a/gtsam/geometry/Cal3DS2_Base.h +++ b/gtsam/geometry/Cal3DS2_Base.h @@ -41,9 +41,9 @@ namespace gtsam { */ 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 + 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 }; diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index 6fc37b0d1..7a0931ff5 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -47,7 +47,7 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base { private: - double xi_; // mirror parameter + double xi_; ///< mirror parameter public: diff --git a/gtsam/geometry/Cal3_S2Stereo.cpp b/gtsam/geometry/Cal3_S2Stereo.cpp index b4e70202e..43f0fb12f 100644 --- a/gtsam/geometry/Cal3_S2Stereo.cpp +++ b/gtsam/geometry/Cal3_S2Stereo.cpp @@ -32,7 +32,7 @@ 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::abs(b_ - other.baseline()) < tol); + return Cal3_S2::equals(*base, tol) && (std::fabs(b_ - other.baseline()) < tol); } /* ************************************************************************* */