diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index cfc508ac7..8d75d767c 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -100,6 +100,12 @@ void Similarity3::print(const std::string& s) const { std::cout << "s: " << scale() << std::endl; } +std::ostream &operator<<(std::ostream &os, const Similarity3& p) { + os << "[" << p.rotation().xyz().transpose() << " " << p.translation().vector().transpose() << " " << + p.scale() << "]\';"; + return os; +} + Similarity3 Similarity3::ChartAtOrigin::Retract(const Vector7& v, ChartJacobian H) { // Will retracting or localCoordinating R work if R is not a unit rotation? // Also, how do we actually get s out? Seems like we need to store it somewhere. diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam_unstable/geometry/Similarity3.h index 89f1010c4..65de32589 100644 --- a/gtsam_unstable/geometry/Similarity3.h +++ b/gtsam_unstable/geometry/Similarity3.h @@ -70,6 +70,8 @@ public: /// Print with optional string void print(const std::string& s) const; + GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Similarity3& p); + /// @} /// @name Group /// @{