From 073679f831e35c33eb347b2686d5ee59f9c23df4 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Fri, 12 Jul 2013 22:27:41 +0000
Subject: [PATCH] Pretty-printing in JacobianFactorUnordered and
 GaussianConditionalUnordered

---
 gtsam/linear/GaussianConditionalUnordered.cpp | 14 ++++---
 gtsam/linear/JacobianFactorUnordered.cpp      | 40 ++++++++++++-------
 2 files changed, 35 insertions(+), 19 deletions(-)

diff --git a/gtsam/linear/GaussianConditionalUnordered.cpp b/gtsam/linear/GaussianConditionalUnordered.cpp
index 246493d39..7af208b46 100644
--- a/gtsam/linear/GaussianConditionalUnordered.cpp
+++ b/gtsam/linear/GaussianConditionalUnordered.cpp
@@ -55,17 +55,21 @@ namespace gtsam {
   /* ************************************************************************* */
   void GaussianConditionalUnordered::print(const string &s, const IndexFormatter& formatter) const
   {
-    cout << s << ": density on ";
+    cout << s << "  Conditional density ";
     for(const_iterator it = beginFrontals(); it != endFrontals(); ++it) {
       cout << (boost::format("[%1%]")%(formatter(*it))).str() << " ";
     }
     cout << endl;
-    gtsam::print(Matrix(get_R()),"R");
+    cout << formatMatrixIndented("  R = ", get_R()) << endl;
     for(const_iterator it = beginParents() ; it != endParents() ; ++it ) {
-      gtsam::print(Matrix(getA(it)), (boost::format("S[%1%]")%(formatter(*it))).str());
+      cout << formatMatrixIndented((boost::format("  S[%1%] = ")%(formatter(*it))).str(), getA(it))
+        << endl;
     }
-    gtsam::print(Vector(getb()),"d");
-    model_->print("sigmas");
+    cout << formatMatrixIndented("  d = ", getb(), true) << "\n";
+    if(model_)
+      model_->print("  Noise model: ");
+    else
+      cout << "  No noise model" << endl;
   }    
 
   /* ************************************************************************* */
diff --git a/gtsam/linear/JacobianFactorUnordered.cpp b/gtsam/linear/JacobianFactorUnordered.cpp
index 2476411f2..c01113bf5 100644
--- a/gtsam/linear/JacobianFactorUnordered.cpp
+++ b/gtsam/linear/JacobianFactorUnordered.cpp
@@ -315,34 +315,46 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  void JacobianFactorUnordered::print(const string& s, const KeyFormatter& formatter) const {
-    cout << s << "\n";
-    if (empty()) {
-      cout << " empty, keys: ";
-      BOOST_FOREACH(const Key& key, keys()) { cout << formatter(key) << " "; }
-      cout << endl;
-    } else {
-      for(const_iterator key=begin(); key!=end(); ++key)
-        cout << boost::format("A[%1%]=\n")%formatter(*key) << getA(key) << endl;
-      cout << "b=" << getb() << endl;
-      model_->print("model");
+  void JacobianFactorUnordered::print(const string& s, const KeyFormatter& formatter) const
+  {
+    if(!s.empty())
+      cout << s << "\n";
+    for(const_iterator key = begin(); key != end(); ++key) {
+      cout <<
+        formatMatrixIndented((boost::format("  A[%1%] = ") % formatter(*key)).str(), getA(key))
+        << endl;
     }
+    cout << formatMatrixIndented("  b = ", getb(), true) << "\n";
+    if(model_)
+      model_->print("  Noise model: ");
+    else
+      cout << "  No noise model" << endl;
   }
 
   /* ************************************************************************* */
   // Check if two linear factors are equal
-  bool JacobianFactorUnordered::equals(const GaussianFactorUnordered& f_, double tol) const {
+  bool JacobianFactorUnordered::equals(const GaussianFactorUnordered& f_, double tol) const
+  {
     if(!dynamic_cast<const JacobianFactorUnordered*>(&f_))
       return false;
     else {
       const JacobianFactorUnordered& f(static_cast<const JacobianFactorUnordered&>(f_));
-      if (empty()) return (f.empty());
-      if(keys()!=f.keys() /*|| !model_->equals(lf->model_, tol)*/)
+
+      // Check keys
+      if(keys() != f.keys())
         return false;
 
+      // Check noise model
+      if(model_ && !f.model_ || !model_ && f.model_)
+        return false;
+      if(model_ && f.model_ && !model_->equals(*f.model_, tol))
+        return false;
+
+      // Check matrix sizes
       if (!(Ab_.rows() == f.Ab_.rows() && Ab_.cols() == f.Ab_.cols()))
         return false;
 
+      // Check matrix contents
       constABlock Ab1(Ab_.range(0, Ab_.nBlocks()));
       constABlock Ab2(f.Ab_.range(0, f.Ab_.nBlocks()));
       for(size_t row=0; row< (size_t) Ab1.rows(); ++row)