Cleaned up printing of NavState

gtsam/navigation/NavState.cpp

@@ -88,15 +88,15 @@ Matrix7 NavState::matrix() const {
 
 //------------------------------------------------------------------------------
 ostream& operator<<(ostream& os, const NavState& state) {
-  os << "R:" << state.attitude();
+  os << "R: " << state.attitude() << "\n";
-  os << "p:" << state.position() << endl;
+  os << "p: " << state.position() << "\n";
-  os << "v:" << Point3(state.velocity()) << endl;
+  os << "v: " << Point3(state.velocity());
   return os;
 }
 
 //------------------------------------------------------------------------------
 void NavState::print(const string& s) const {
-  cout << s << *this << endl;
+  cout << (s.empty() ? s : s + " ") << *this << endl;
 }
 
 //------------------------------------------------------------------------------

gtsam/navigation/tests/testNavState.cpp

@@ -237,6 +237,18 @@ TEST(NavState, CorrectPIM) {
   EXPECT(assert_equal(numericalDerivative22(correctPIM, kState1, xi), aH2));
 }
 
+/* ************************************************************************* */
+TEST(NavState, Stream)
+{
+  NavState state;
+
+  std::ostringstream os;
+  os << state;
+  string expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\np: [0, 0, 0]'\nv: [0, 0, 0]'";
+  EXPECT(os.str() == expected);
+}
+
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;