Merge pull request #226 from borglab/printing-updates

Printing updates

gtsam/base/Matrix.cpp

@@ -142,7 +142,7 @@ void print(const Matrix& A, const string &s, ostream& stream) {
   static const Eigen::IOFormat matlab(
       Eigen::StreamPrecision, // precision
       0, // flags
-      " ", // coeffSeparator
+      ", ", // coeffSeparator
       ";\n", // rowSeparator
       " \t",  // rowPrefix
       "", // rowSuffix

gtsam/geometry/Pose3.cpp

@@ -106,7 +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_.x() << ", " << t_.y() << ", " << t_.z() << "]\';";
+  cout << t_ << ";" << endl;
 }
 
 /* ************************************************************************* */

gtsam/geometry/tests/testPose3.cpp

@@ -993,6 +993,32 @@ TEST(Pose3, Create) {
   EXPECT(assert_equal(numericalDerivative22<Pose3,Rot3,Point3>(create, R, P2), actualH2, 1e-9));
 }
 
+/* ************************************************************************* */
+TEST(Pose3, print) {
+  std::stringstream redirectStream;
+  std::streambuf* ssbuf = redirectStream.rdbuf();
+  std::streambuf* oldbuf  = std::cout.rdbuf();
+  // redirect cout to redirectStream
+  std::cout.rdbuf(ssbuf);
+
+  Pose3 pose(Rot3::identity(), Point3(1, 2, 3));
+  // output is captured to redirectStream
+  pose.print();
+
+  // Generate the expected output
+  std::stringstream expected;
+  Point3 translation(1, 2, 3);
+  expected << '[' << translation.x() << ", " << translation.y() << ", " << translation.z() << "]\';";
+
+  // reset cout to the original stream
+  std::cout.rdbuf(oldbuf);
+
+  // Get substring corresponding to translation part
+  std::string actual = redirectStream.str().substr(47, 11);
+
+  CHECK_EQUAL(expected.str(), actual);
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;