stream printing for all calibration models

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()
+  os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
-     << ", px: " << cal.px() << ", py: " << cal.py() << " }";
+     << ", px: " << cal.px() << ", py: " << cal.py();
   return os;
 }
 

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");

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;
 

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_);

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;
 

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");

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;
 

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");

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;
 

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);

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;
 

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()
+  // Use the base class version since it is identical.
-     << ", px: " << cal.px() << ", py: " << cal.py() << " }";
+  os << (Cal3&)cal;
   return os;
 }
 

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()
+  os << (Cal3_S2&)cal;
-     << ", px: " << cal.px() << ", py: " << cal.py()
+  os << ", b: " << cal.baseline();
-     << ", 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;
 }
 

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
     /// @{