Merge pull request #358 from borglab/feature/rot-print

Consistent Rot3 printing

gtsam/base/Matrix.cpp

@@ -136,20 +136,24 @@ Vector operator^(const Matrix& A, const Vector & v) {
   return A.transpose() * v;
 }
 
+const Eigen::IOFormat& matlabFormat() {
+  static const Eigen::IOFormat matlab(
+    Eigen::StreamPrecision, // precision
+    Eigen::DontAlignCols, // flags set such that rowSpacers are not added
+    ", ", // coeffSeparator
+    ";\n", // rowSeparator
+    "\t",  // rowPrefix
+    "", // rowSuffix
+    "[\n", // matPrefix
+    "\n]" // matSuffix
+  );
+  return matlab;
+}
+
 /* ************************************************************************* */
 //3 argument call
 void print(const Matrix& A, const string &s, ostream& stream) {
-  static const Eigen::IOFormat matlab(
-      Eigen::StreamPrecision, // precision
-      0, // flags
-      ", ", // coeffSeparator
-      ";\n", // rowSeparator
-      "\t",  // rowPrefix
-      "", // rowSuffix
-      "[\n", // matPrefix
-      "\n]" // matSuffix
-      );
-  cout << s << A.format(matlab) << endl;
+  cout << s << A.format(matlabFormat()) << endl;
 }
 
 /* ************************************************************************* */

gtsam/base/Matrix.h

@@ -76,6 +76,10 @@ GTSAM_MAKE_MATRIX_DEFS(9);
 typedef Eigen::Block<Matrix> SubMatrix;
 typedef Eigen::Block<const Matrix> ConstSubMatrix;
 
+// Matrix formatting arguments when printing.
+// Akin to Matlab style.
+const Eigen::IOFormat& matlabFormat();
+
 /**
  * equals with a tolerance
  */

gtsam/geometry/Pose3.cpp

@@ -106,9 +106,7 @@ Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y,
 
 /* ************************************************************************* */
 void Pose3::print(const string& s) const {
-  cout << s;
-  R_.print("R:\n");
-  cout << t_ << ";" << endl;
+  cout << (s.empty() ? s : s + " ") << *this << endl;
 }
 
 /* ************************************************************************* */
@@ -439,9 +437,9 @@ boost::optional<Pose3> align(const vector<Point3Pair>& baPointPairs) {
 
 /* ************************************************************************* */
 std::ostream &operator<<(std::ostream &os, const Pose3& pose) {
-  os << pose.rotation() << "\n";
-  const Point3& t = pose.translation();
-  os << '[' << t.x() << ", " << t.y() << ", " << t.z() << "]\';\n";
+  // Both Rot3 and Point3 have ostream definitions so we use them.
+  os << "R: " << pose.rotation() << "\n";
+  os << "t: " << pose.translation();
   return os;
 }
 

gtsam/geometry/Rot3.cpp

@@ -32,7 +32,8 @@ namespace gtsam {
 
 /* ************************************************************************* */
 void Rot3::print(const std::string& s) const {
-  gtsam::print((Matrix)matrix(), s);
+  cout << (s.empty() ? "R: " : s + " ");
+  gtsam::print(static_cast<Matrix>(matrix()));
 }
 
 /* ************************************************************************* */
@@ -222,10 +223,7 @@ pair<Matrix3, Vector3> RQ(const Matrix3& A) {
 
 /* ************************************************************************* */
 ostream &operator<<(ostream &os, const Rot3& R) {
-  os << "\n";
-  os << '|' << R.r1().x() << ", " << R.r2().x() << ", " << R.r3().x() << "|\n";
-  os << '|' << R.r1().y() << ", " << R.r2().y() << ", " << R.r3().y() << "|\n";
-  os << '|' << R.r1().z() << ", " << R.r2().z() << ", " << R.r3().z() << "|\n";
+  os << R.matrix().format(matlabFormat());
   return os;
 }
 

gtsam/geometry/Rot3.h

@@ -254,7 +254,7 @@ namespace gtsam {
     /// @{
 
     /** print */
-    void print(const std::string& s="R") const;
+    void print(const std::string& s="") const;
 
     /** equals with an tolerance */
     bool equals(const Rot3& p, double tol = 1e-9) const;

gtsam/geometry/tests/testPose3.cpp

@@ -862,7 +862,8 @@ TEST( Pose3, stream)
   Pose3 T;
   std::ostringstream os;
   os << T;
-  EXPECT(os.str() == "\n|1, 0, 0|\n|0, 1, 0|\n|0, 0, 1|\n\n[0, 0, 0]';\n");
+  string expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\nt: [0, 0, 0]'";
+  EXPECT(os.str() == expected);
 }
 
 //******************************************************************************
@@ -1032,19 +1033,22 @@ TEST(Pose3, print) {
   std::stringstream expected;
   Point3 translation(1, 2, 3);
 
+  // Add expected rotation
+  expected << "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\n";
+
 #ifdef GTSAM_TYPEDEF_POINTS_TO_VECTORS
   expected << "1\n"
               "2\n"
               "3;\n";
 #else
-  expected << '[' << translation.x() << ", " << translation.y() << ", " << translation.z() << "]\';";
+  expected << "t: [" << translation.x() << ", " << translation.y() << ", " << translation.z() << "]'\n";
 #endif
 
   // reset cout to the original stream
   std::cout.rdbuf(oldbuf);
 
   // Get substring corresponding to translation part
-  std::string actual = redirectStream.str().substr(38, 11);
+  std::string actual = redirectStream.str();
 
   CHECK_EQUAL(expected.str(), actual);
 }

gtsam/geometry/tests/testRot3.cpp

@@ -608,7 +608,8 @@ TEST( Rot3, stream)
   Rot3 R;
   std::ostringstream os;
   os << R;
-  EXPECT(os.str() == "\n|1, 0, 0|\n|0, 1, 0|\n|0, 0, 1|\n");
+  string expected = "[\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]";
+  EXPECT(os.str() == expected);
 }
 
 /* ************************************************************************* */

gtsam/linear/JacobianFactor.cpp

@@ -421,21 +421,11 @@ JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph,
 /* ************************************************************************* */
 void JacobianFactor::print(const string& s,
     const KeyFormatter& formatter) const {
-  static const Eigen::IOFormat matlab(
-      Eigen::StreamPrecision, // precision
-      0, // flags
-      " ", // coeffSeparator
-      ";\n", // rowSeparator
-      "\t",  // rowPrefix
-      "", // rowSuffix
-      "[\n", // matPrefix
-      "\n  ]" // matSuffix
-      );
   if (!s.empty())
     cout << s << "\n";
   for (const_iterator key = begin(); key != end(); ++key) {
     cout << boost::format("  A[%1%] = ") % formatter(*key);
-    cout << getA(key).format(matlab) << endl;
+    cout << getA(key).format(matlabFormat()) << endl;
   }
   cout << formatMatrixIndented("  b = ", getb(), true) << "\n";
   if (model_)

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;
 }
 
 //------------------------------------------------------------------------------

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;

gtsam/nonlinear/tests/testFunctorizedFactor.cpp

@@ -131,7 +131,7 @@ TEST(FunctorizedFactor, Print) {
       "FunctorizedFactor(X0)\n"
       "  measurement: [\n"
       "	1, 0;\n"
-      " 	0, 1\n"
+      "	0, 1\n"
       "]\n"
       "  noise model sigmas: 1 1 1 1 1 1 1 1 1\n";