diff --git a/gtsam/geometry/Cal3.h b/gtsam/geometry/Cal3.h index 5ce7be8ca..6d3973080 100644 --- a/gtsam/geometry/Cal3.h +++ b/gtsam/geometry/Cal3.h @@ -68,7 +68,9 @@ void calibrateJacobians(const Cal& calibration, const Point2& pn, */ class GTSAM_EXPORT Cal3 { protected: - double fx_, fy_, s_, u0_, v0_; ///< focal length, skew and principal point + 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 }; @@ -79,14 +81,14 @@ class GTSAM_EXPORT Cal3 { /// @{ /// Create a default calibration that leaves coordinates unchanged - Cal3() : fx_(1), fy_(1), s_(0), u0_(0), v0_(0) {} + 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 Vector& d) + Cal3(const Vector5& d) : fx_(d(0)), fy_(d(1)), s_(d(2)), u0_(d(3)), v0_(d(4)) {} /** @@ -162,7 +164,7 @@ class GTSAM_EXPORT Cal3 { Matrix3 matrix() const { return K(); } #endif - /// return inverted calibration matrix inv(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; diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index dc99f5259..d128c329b 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -31,12 +31,12 @@ namespace gtsam { class GTSAM_EXPORT Cal3Bundler : public Cal3 { private: - double f_; ///< focal length - double k1_, k2_; ///< radial distortion - double tol_; ///< tolerance value when calibrating + double f_ = 1.0f; ///< focal length + double k1_ = 0.0f, k2_ = 0.0f; ///< radial distortion + double tol_ = 1e-5; ///< tolerance value when calibrating // NOTE: image center parameters (u0, v0) are not optimized - // but are constants. + // but are treated as constants. public: @@ -46,7 +46,7 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 { /// @{ /// Default constructor - Cal3Bundler() : Cal3(), f_(1), k1_(0), k2_(0), tol_(1e-5) {} + Cal3Bundler() = default; /** * Constructor diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index 58ccae04f..03a4d8d46 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -42,11 +42,11 @@ public: /// @{ /// 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() {} @@ -54,7 +54,7 @@ public: /// @name Advanced Constructors /// @{ - Cal3DS2(const Vector &v) : Base(v) {} + Cal3DS2(const Vector9 &v) : Base(v) {} /// @} /// @name Testable diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h index 0c6a4a3cd..e0de6af61 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_ = 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 }; @@ -52,7 +52,7 @@ class GTSAM_EXPORT Cal3DS2_Base : public Cal3 { /// @{ /// Default Constructor with only unit focal length - Cal3DS2_Base() : Cal3(), k1_(0), k2_(0), p1_(0), p2_(0), tol_(1e-5) {} + 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) @@ -69,7 +69,7 @@ class GTSAM_EXPORT Cal3DS2_Base : public Cal3 { /// @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)), diff --git a/gtsam/geometry/Cal3Fisheye.h b/gtsam/geometry/Cal3Fisheye.h index 3eaf9f46d..f41a9b0d7 100644 --- a/gtsam/geometry/Cal3Fisheye.h +++ b/gtsam/geometry/Cal3Fisheye.h @@ -48,8 +48,9 @@ namespace gtsam { */ class GTSAM_EXPORT Cal3Fisheye : public Cal3 { private: - 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 }; @@ -60,7 +61,7 @@ class GTSAM_EXPORT Cal3Fisheye : public Cal3 { /// @{ /// Default Constructor with only unit focal length - Cal3Fisheye() : Cal3(), 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, diff --git a/gtsam/geometry/Cal3Unified.cpp b/gtsam/geometry/Cal3Unified.cpp index dc963a46e..d260dd725 100644 --- a/gtsam/geometry/Cal3Unified.cpp +++ b/gtsam/geometry/Cal3Unified.cpp @@ -25,10 +25,6 @@ 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; @@ -136,7 +132,7 @@ 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(); } diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index 673d9e2c9..e1368db4c 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -45,11 +45,10 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base { using This = Cal3Unified; using Base = Cal3DS2_Base; -private: + private: + double xi_ = 0.0f; ///< mirror parameter - double xi_; ///< mirror parameter - -public: + public: enum { dimension = 10 }; @@ -57,11 +56,11 @@ public: /// @{ /// 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,7 +68,8 @@ 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 @@ -88,6 +88,9 @@ public: /// mirror parameter inline double xi() const { return xi_;} + /// Return all parameters as a vector + Vector10 vector() const ; + /** * convert intrinsic coordinates xy to image coordinates uv * @param p point in intrinsic coordinates @@ -117,7 +120,7 @@ public: 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 ; } @@ -125,9 +128,6 @@ public: /// Return dimensions of calibration manifold object static size_t Dim() { return dimension; } - /// Return all parameters as a vector - Vector10 vector() const ; - /// @} private: diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 0a6b0e164..37edc46c4 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -42,14 +42,14 @@ class GTSAM_EXPORT Cal3_S2 : public Cal3 { /// @{ /// Create a default calibration that leaves coordinates unchanged - Cal3_S2() : Cal3() {} + Cal3_S2() = default; /// constructor from doubles 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) : Cal3(d) {} + Cal3_S2(const Vector5& d) : Cal3(d) {} /** * Easy constructor, takes fov in degrees, asssumes zero skew, unit aspect diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h index 585807543..0c68ef6b4 100644 --- a/gtsam/geometry/Cal3_S2Stereo.h +++ b/gtsam/geometry/Cal3_S2Stereo.h @@ -29,7 +29,7 @@ namespace gtsam { */ class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 { private: - double b_; + double b_ = 1.0f; ///< Stereo baseline. public: @@ -42,7 +42,7 @@ namespace gtsam { /// @ /// default calibration leaves coordinates unchanged - Cal3_S2Stereo() : Cal3_S2(1, 1, 0, 0, 0), b_(1.0) {} + Cal3_S2Stereo() = default; /// constructor from doubles Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, @@ -50,7 +50,7 @@ namespace gtsam { : Cal3_S2(fx, fy, s, u0, v0), b_(b) {} /// constructor from vector - Cal3_S2Stereo(const Vector& d) + 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 @@ -61,6 +61,7 @@ namespace gtsam { /// @name Testable /// @{ + /// print with optional string void print(const std::string& s = "") const override; /// Check if equal up to specified tolerance