Merge pull request #685 from borglab/fix/pose2-print

Fix Pose2 print

gtsam/base/Testable.h

@@ -72,10 +72,10 @@ namespace gtsam {
   }; // \ Testable
 
   inline void print(float v, const std::string& s = "") {
-    std::cout << (s == "" ? s : s + " ") << v << std::endl;
+    std::cout << (s.empty() ? s : s + " ") << v << std::endl;
   }
   inline void print(double v, const std::string& s = "") {
-    std::cout << (s == "" ? s : s + " ") << v << std::endl;
+    std::cout << (s.empty() ? s : s + " ") << v << std::endl;
   }
 
   /** Call equal on the object */

gtsam/base/TestableAssertions.h

@@ -372,16 +372,19 @@ bool assert_stdout_equal(const std::string& expected, const V& actual) {
 
 /**
  * Capture print function output and compare against string.
+ *
+ * @param s: Optional string to pass to the print() method.
  */
-template<class V>
-bool assert_print_equal(const std::string& expected, const V& actual) {
+template <class V>
+bool assert_print_equal(const std::string& expected, const V& actual,
+                        const std::string& s = "") {
   // Redirect output to buffer so we can compare
   std::stringstream buffer;
   // Save the original output stream so we can reset later
   std::streambuf* old = std::cout.rdbuf(buffer.rdbuf());
 
   // We test against actual std::cout for faithful reproduction
-  actual.print();
+  actual.print(s);
 
   // Get output string and reset stdout
   std::string actual_ = buffer.str();

gtsam/geometry/Pose2.cpp

@@ -48,7 +48,7 @@ Matrix3 Pose2::matrix() const {
 
 /* ************************************************************************* */
 void Pose2::print(const string& s) const {
-  cout << s << this << endl;
+  std::cout << (s.empty() ? s : s + " ") << *this << std::endl;
 }
 
 /* ************************************************************************* */

gtsam/geometry/tests/testPose2.cpp

@@ -14,16 +14,17 @@
  * @brief  Unit tests for Pose2 class
  */
 
-#include <gtsam/geometry/Pose2.h>
-#include <gtsam/geometry/Point2.h>
-#include <gtsam/geometry/Rot2.h>
+#include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/Testable.h>
-#include <gtsam/base/testLie.h>
+#include <gtsam/base/TestableAssertions.h>
 #include <gtsam/base/lieProxies.h>
+#include <gtsam/base/testLie.h>
+#include <gtsam/geometry/Point2.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Rot2.h>
 
-#include <CppUnitLite/TestHarness.h>
+#include <boost/assign/std/vector.hpp>  // for operator +=
 #include <boost/optional.hpp>
-#include <boost/assign/std/vector.hpp> // for operator +=
 #include <cmath>
 #include <iostream>
 
@@ -910,6 +911,22 @@ TEST(Pose2 , TransformCovariance3) {
   }
 }
 
+/* ************************************************************************* */
+TEST(Pose2, Print) {
+  Pose2 pose(Rot2::identity(), Point2(1, 2));
+
+  // Generate the expected output
+  string s = "Planar Pose";
+  string expected_stdout = "(1, 2, 0)";
+  string expected1 = expected_stdout + "\n";
+  string expected2 = s + " " + expected1;
+
+  EXPECT(assert_stdout_equal(expected_stdout, pose));
+
+  EXPECT(assert_print_equal(expected1, pose));
+  EXPECT(assert_print_equal(expected2, pose, s));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/navigation/CombinedImuFactor.cpp

@@ -167,7 +167,7 @@ gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const {
 //------------------------------------------------------------------------------
 void CombinedImuFactor::print(const string& s,
     const KeyFormatter& keyFormatter) const {
-  cout << (s == "" ? s : s + "\n") << "CombinedImuFactor("
+  cout << (s.empty() ? s : s + "\n") << "CombinedImuFactor("
        << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << ","
        << keyFormatter(this->key3()) << "," << keyFormatter(this->key4()) << ","
        << keyFormatter(this->key5()) << "," << keyFormatter(this->key6())

gtsam/navigation/ImuFactor.cpp

@@ -130,7 +130,7 @@ std::ostream& operator<<(std::ostream& os, const ImuFactor& f) {
 
 //------------------------------------------------------------------------------
 void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
-  cout << (s == "" ? s : s + "\n") << "ImuFactor(" << keyFormatter(this->key1())
+  cout << (s.empty() ? s : s + "\n") << "ImuFactor(" << keyFormatter(this->key1())
        << "," << keyFormatter(this->key2()) << "," << keyFormatter(this->key3())
        << "," << keyFormatter(this->key4()) << "," << keyFormatter(this->key5())
        << ")\n";
@@ -226,7 +226,7 @@ std::ostream& operator<<(std::ostream& os, const ImuFactor2& f) {
 //------------------------------------------------------------------------------
 void ImuFactor2::print(const string& s,
     const KeyFormatter& keyFormatter) const {
-  cout << (s == "" ? s : s + "\n") << "ImuFactor2("
+  cout << (s.empty() ? s : s + "\n") << "ImuFactor2("
        << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << ","
        << keyFormatter(this->key3()) << ")\n";
   cout << *this << endl;

gtsam/navigation/PreintegratedRotation.cpp

@@ -26,7 +26,7 @@ using namespace std;
 namespace gtsam {
 
 void PreintegratedRotationParams::print(const string& s) const {
-  cout << (s == "" ? s : s + "\n") << endl;
+  cout << (s.empty() ? s : s + "\n") << endl;
   cout << "gyroscopeCovariance:\n[\n" << gyroscopeCovariance << "\n]" << endl;
   if (omegaCoriolis)
     cout << "omegaCoriolis = (" << omegaCoriolis->transpose() << ")" << endl;

gtsam/navigation/PreintegrationBase.cpp

@@ -46,7 +46,7 @@ ostream& operator<<(ostream& os, const PreintegrationBase& pim) {
 
 //------------------------------------------------------------------------------
 void PreintegrationBase::print(const string& s) const {
-  cout << (s == "" ? s : s + "\n") << *this << endl;
+  cout << (s.empty() ? s : s + "\n") << *this << endl;
 }
 
 //------------------------------------------------------------------------------

gtsam/nonlinear/Values.cpp

@@ -75,7 +75,7 @@ namespace gtsam {
 
   /* ************************************************************************* */
   void Values::print(const string& str, const KeyFormatter& keyFormatter) const {
-    cout << str << (str == "" ? "" : "\n");
+    cout << str << (str.empty() ? "" : "\n");
     cout << "Values with " << size() << " values:\n";
     for(const_iterator key_value = begin(); key_value != end(); ++key_value) {
       cout << "Value " << keyFormatter(key_value->key) << ": ";

gtsam/sfm/ShonanAveraging.h

@@ -91,7 +91,8 @@ struct GTSAM_EXPORT ShonanAveragingParameters {
   bool getCertifyOptimality() const { return certifyOptimality; }
 
   /// Print the parameters and flags used for rotation averaging.
-  void print() const {
+  void print(const std::string &s = "") const {
+    std::cout << (s.empty() ? s : s + " ");
     std::cout << " ShonanAveragingParameters: " << std::endl;
     std::cout << " alpha: " << alpha << std::endl;
     std::cout << " beta: " << beta << std::endl;