From d3ac33ac18617cb7f8a6484a025f4ab34df0b65e Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 18 Jun 2020 11:10:44 -0500
Subject: [PATCH] Cleaned up printing of NavState

---
 gtsam/navigation/NavState.cpp           |  8 ++++----
 gtsam/navigation/tests/testNavState.cpp | 12 ++++++++++++
 2 files changed, 16 insertions(+), 4 deletions(-)

diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp
index 50949c761..1d191416f 100644
--- a/gtsam/navigation/NavState.cpp
+++ b/gtsam/navigation/NavState.cpp
@@ -88,15 +88,15 @@ Matrix7 NavState::matrix() const {
 
 //------------------------------------------------------------------------------
 ostream& operator<<(ostream& os, const NavState& state) {
-  os << "R:" << state.attitude();
-  os << "p:" << state.position() << endl;
-  os << "v:" << Point3(state.velocity()) << endl;
+  os << "R: " << state.attitude() << "\n";
+  os << "p: " << state.position() << "\n";
+  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;
 }
 
 //------------------------------------------------------------------------------
diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp
index 8ea8ce9b5..57945020c 100644
--- a/gtsam/navigation/tests/testNavState.cpp
+++ b/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;