diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 6266c328f..860eaa85c 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -74,11 +74,17 @@ Matrix7 NavState::matrix() const { return T; } +//------------------------------------------------------------------------------ +ostream& operator<<(ostream& os, const NavState& state) { + os << "R:" << state.attitude(); + os << "p:" << state.position() << endl; + os << "v:" << Point3(state.velocity()) << endl; + return os; +} + //------------------------------------------------------------------------------ void NavState::print(const string& s) const { - attitude().print(s + ".R"); - position().print(s + ".p"); - gtsam::print((Vector) v_, s + ".v"); + cout << s << *this << endl; } //------------------------------------------------------------------------------ diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index 9561aa77b..439e8fceb 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -124,6 +124,9 @@ public: /// @name Testable /// @{ + /// Output stream operator + GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const NavState& state); + /// print void print(const std::string& s = "") const; @@ -229,6 +232,8 @@ public: false, OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 = boost::none) const; + /// @} + private: /// @{ /// serialization