ostream operator

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");
+  cout << s << *this << endl;
-  position().print(s + ".p");
-  gtsam::print((Vector) v_, s + ".v");
 }
 
 //------------------------------------------------------------------------------

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