diff --git a/gtsam/geometry/Cal3.cpp b/gtsam/geometry/Cal3.cpp index 6183c6e5e..ec39d7eda 100644 --- a/gtsam/geometry/Cal3.cpp +++ b/gtsam/geometry/Cal3.cpp @@ -49,8 +49,8 @@ Cal3::Cal3(const std::string& path) /* ************************************************************************* */ std::ostream& operator<<(std::ostream& os, const Cal3& cal) { - os << "{ fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew() - << ", px: " << cal.px() << ", py: " << cal.py() << " }"; + os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew() + << ", px: " << cal.px() << ", py: " << cal.py(); return os; } diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index 8c524e1cc..af2f881d8 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -42,6 +42,13 @@ Vector3 Cal3Bundler::vector() const { return Vector3(f_, k1_, k2_); } +/* ************************************************************************* */ +std::ostream& operator<<(std::ostream& os, const Cal3Bundler& cal) { + os << "f: " << cal.fx() << ", k1: " << cal.k1() << ", k2: " << cal.k2() + << ", px: " << cal.px() << ", py: " << cal.py(); + return os; +} + /* ************************************************************************* */ void Cal3Bundler::print(const std::string& s) const { gtsam::print((Vector)(Vector(5) << f_, k1_, k2_, u0_, v0_).finished(), s + ".K"); diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index d128c329b..76703f96f 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -67,6 +67,10 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 { /// @name Testable /// @{ + /// Output stream operator + GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, + const Cal3Bundler& cal); + /// print with optional string void print(const std::string& s = "") const override; diff --git a/gtsam/geometry/Cal3DS2.cpp b/gtsam/geometry/Cal3DS2.cpp index b4595a4dc..71aa2738d 100644 --- a/gtsam/geometry/Cal3DS2.cpp +++ b/gtsam/geometry/Cal3DS2.cpp @@ -24,6 +24,12 @@ namespace gtsam { +/* ************************************************************************* */ +std::ostream& operator<<(std::ostream& os, const Cal3DS2& cal) { + os << (Cal3DS2_Base&)cal; + return os; +} + /* ************************************************************************* */ void Cal3DS2::print(const std::string& s_) const { Base::print(s_); diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index 03a4d8d46..d0bed8652 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -60,6 +60,10 @@ public: /// @name Testable /// @{ + /// Output stream operator + GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, + const Cal3DS2& cal); + /// print with optional string void print(const std::string& s = "") const override; diff --git a/gtsam/geometry/Cal3DS2_Base.cpp b/gtsam/geometry/Cal3DS2_Base.cpp index 016c9dfa2..2741d0736 100644 --- a/gtsam/geometry/Cal3DS2_Base.cpp +++ b/gtsam/geometry/Cal3DS2_Base.cpp @@ -31,6 +31,14 @@ Vector9 Cal3DS2_Base::vector() const { return v; } +/* ************************************************************************* */ +std::ostream& operator<<(std::ostream& os, const Cal3DS2_Base& cal) { + os << (Cal3&)cal; + os << ", k1: " << cal.k1() << ", k2: " << cal.k2() << ", p1: " << cal.p1() + << ", p2: " << cal.p2(); + return os; +} + /* ************************************************************************* */ void Cal3DS2_Base::print(const std::string& s_) const { gtsam::print((Matrix)K(), s_ + ".K"); diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h index e0de6af61..713fa4e0e 100644 --- a/gtsam/geometry/Cal3DS2_Base.h +++ b/gtsam/geometry/Cal3DS2_Base.h @@ -80,6 +80,10 @@ class GTSAM_EXPORT Cal3DS2_Base : public Cal3 { /// @name Testable /// @{ + /// Output stream operator + GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, + const Cal3DS2_Base& cal); + /// print with optional string void print(const std::string& s = "") const override; diff --git a/gtsam/geometry/Cal3Fisheye.cpp b/gtsam/geometry/Cal3Fisheye.cpp index 55020f581..b9e60acee 100644 --- a/gtsam/geometry/Cal3Fisheye.cpp +++ b/gtsam/geometry/Cal3Fisheye.cpp @@ -139,6 +139,14 @@ Point2 Cal3Fisheye::calibrate(const Point2& uv, OptionalJacobian<2, 9> Dcal, return pi; } +/* ************************************************************************* */ +std::ostream& operator<<(std::ostream& os, const Cal3Fisheye& cal) { + os << (Cal3&)cal; + os << ", k1: " << cal.k1() << ", k2: " << cal.k2() << ", k3: " << cal.k3() + << ", k4: " << cal.k4(); + return os; +} + /* ************************************************************************* */ void Cal3Fisheye::print(const std::string& s_) const { gtsam::print((Matrix)K(), s_ + ".K"); diff --git a/gtsam/geometry/Cal3Fisheye.h b/gtsam/geometry/Cal3Fisheye.h index f41a9b0d7..738b2275d 100644 --- a/gtsam/geometry/Cal3Fisheye.h +++ b/gtsam/geometry/Cal3Fisheye.h @@ -137,6 +137,10 @@ class GTSAM_EXPORT Cal3Fisheye : public Cal3 { /// @name Testable /// @{ + /// Output stream operator + GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, + const Cal3Fisheye& cal); + /// print with optional string virtual void print(const std::string& s = "") const override; diff --git a/gtsam/geometry/Cal3Unified.cpp b/gtsam/geometry/Cal3Unified.cpp index d260dd725..80613bbf2 100644 --- a/gtsam/geometry/Cal3Unified.cpp +++ b/gtsam/geometry/Cal3Unified.cpp @@ -32,6 +32,13 @@ Vector10 Cal3Unified::vector() const { return v; } +/* ************************************************************************* */ +std::ostream& operator<<(std::ostream& os, const Cal3Unified& cal) { + os << (Cal3DS2_Base&)cal; + os << ", xi: " << cal.xi(); + return os; +} + /* ************************************************************************* */ void Cal3Unified::print(const std::string& s) const { Base::print(s); diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index e1368db4c..a2c5ebc5c 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -75,6 +75,10 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base { /// @name Testable /// @{ + /// Output stream operator + GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, + const Cal3Unified& cal); + /// print with optional string void print(const std::string& s = "") const override; diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp index 2d830a4a3..98c7bea38 100644 --- a/gtsam/geometry/Cal3_S2.cpp +++ b/gtsam/geometry/Cal3_S2.cpp @@ -25,8 +25,8 @@ namespace gtsam { /* ************************************************************************* */ std::ostream& operator<<(std::ostream& os, const Cal3_S2& cal) { - os << "{ fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew() - << ", px: " << cal.px() << ", py: " << cal.py() << " }"; + // Use the base class version since it is identical. + os << (Cal3&)cal; return os; } diff --git a/gtsam/geometry/Cal3_S2Stereo.cpp b/gtsam/geometry/Cal3_S2Stereo.cpp index 3bf7c3468..8bd733e95 100644 --- a/gtsam/geometry/Cal3_S2Stereo.cpp +++ b/gtsam/geometry/Cal3_S2Stereo.cpp @@ -23,16 +23,15 @@ namespace gtsam { /* ************************************************************************* */ std::ostream& operator<<(std::ostream& os, const Cal3_S2Stereo& cal) { - os << "{ fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew() - << ", px: " << cal.px() << ", py: " << cal.py() - << ", b: " << cal.baseline() << " }"; + os << (Cal3_S2&)cal; + os << ", b: " << cal.baseline(); return os; } /* ************************************************************************* */ void Cal3_S2Stereo::print(const std::string& s) const { std::cout << s << (s != "" ? " " : ""); - print("K: "); + std::cout << "K: " << (Matrix)K() << std::endl; std::cout << "Baseline: " << b_ << std::endl; } diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h index 0c68ef6b4..94341ec37 100644 --- a/gtsam/geometry/Cal3_S2Stereo.h +++ b/gtsam/geometry/Cal3_S2Stereo.h @@ -61,6 +61,10 @@ namespace gtsam { /// @name Testable /// @{ + /// 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; @@ -83,7 +87,7 @@ namespace gtsam { /// vectorized form (column-wise) Vector6 vector() const { Vector6 v; - v << vector(), b_; + v << Cal3_S2::vector(), b_; return v; } @@ -108,7 +112,6 @@ namespace gtsam { return T2.vector() - vector(); } - /// @} /// @name Advanced Interface /// @{