Added formatting objects to linear and inference objects to allow for printing meaningful keys

gtsam/inference/BayesTree-inl.h

@@ -130,10 +130,10 @@ namespace gtsam {
 
 	/* ************************************************************************* */
 	template<class CONDITIONAL, class CLIQUE>
-	void BayesTree<CONDITIONAL,CLIQUE>::Cliques::print(const std::string& s) const {
+	void BayesTree<CONDITIONAL,CLIQUE>::Cliques::print(const std::string& s, const IndexFormatter& indexFormatter) const {
 	  std::cout << s << ":\n";
 		BOOST_FOREACH(sharedClique clique, *this)
-				clique->printTree();
+				clique->printTree("", indexFormatter);
 	}
 
 	/* ************************************************************************* */
@@ -324,14 +324,14 @@ namespace gtsam {
 
 	/* ************************************************************************* */
 	template<class CONDITIONAL, class CLIQUE>
-	void BayesTree<CONDITIONAL,CLIQUE>::print(const std::string& s) const {
+	void BayesTree<CONDITIONAL,CLIQUE>::print(const std::string& s, const IndexFormatter& indexFormatter) const {
 		if (root_.use_count() == 0) {
 			printf("WARNING: BayesTree.print encountered a forest...\n");
 			return;
 		}
 		std::cout << s << ": clique size == " << size() << ", node size == " << nodes_.size() << std::endl;
 		if (nodes_.empty()) return;
-		root_->printTree("");
+		root_->printTree("", indexFormatter);
 	}
 
 	/* ************************************************************************* */

gtsam/inference/BayesTree.h

@@ -70,7 +70,8 @@ namespace gtsam {
 
 		// A convenience class for a list of shared cliques
 		struct Cliques : public std::list<sharedClique> {
-			void print(const std::string& s = "Cliques") const;
+			void print(const std::string& s = "Cliques",
+					const IndexFormatter& indexFormatter = &(boost::lexical_cast<std::string, Index>)) const;
 			bool equals(const Cliques& other, double tol = 1e-9) const;
 		};
 
@@ -175,7 +176,8 @@ namespace gtsam {
 		bool equals(const BayesTree<CONDITIONAL,CLIQUE>& other, double tol = 1e-9) const;
 
 		/** print */
-		void print(const std::string& s = "") const;
+		void print(const std::string& s = "",
+				const IndexFormatter& indexFormatter = &(boost::lexical_cast<std::string, Index>) ) const;
 
 		/// @}
 		/// @name Standard Interface

gtsam/inference/BayesTreeCliqueBase-inl.h

@@ -50,8 +50,8 @@ namespace gtsam {
 
   /* ************************************************************************* */
   template<class DERIVED, class CONDITIONAL>
-  void BayesTreeCliqueBase<DERIVED,CONDITIONAL>::print(const std::string& s) const {
+  void BayesTreeCliqueBase<DERIVED,CONDITIONAL>::print(const std::string& s, const IndexFormatter& indexFormatter) const {
-    conditional_->print(s);
+    conditional_->print(s, indexFormatter);
   }
 
   /* ************************************************************************* */
@@ -65,10 +65,10 @@ namespace gtsam {
 
   /* ************************************************************************* */
   template<class DERIVED, class CONDITIONAL>
-  void BayesTreeCliqueBase<DERIVED,CONDITIONAL>::printTree(const std::string& indent) const {
+  void BayesTreeCliqueBase<DERIVED,CONDITIONAL>::printTree(const std::string& indent, const IndexFormatter& indexFormatter) const {
-    asDerived(this)->print(indent);
+    asDerived(this)->print(indent, indexFormatter);
     BOOST_FOREACH(const derived_ptr& child, children_)
-      child->printTree(indent+"  ");
+      child->printTree(indent+"  ", indexFormatter);
   }
 
   /* ************************************************************************* */

gtsam/inference/BayesTreeCliqueBase.h

@@ -93,10 +93,10 @@ namespace gtsam {
     }
 
     /** print this node */
-    void print(const std::string& s = "") const;
+    void print(const std::string& s = "", const IndexFormatter& indexFormatter = &(boost::lexical_cast<std::string, Index>) ) const;
 
     /** print this node and entire subtree below it */
-    void printTree(const std::string& indent="") const;
+    void printTree(const std::string& indent="", const IndexFormatter& indexFormatter = &(boost::lexical_cast<std::string, Index>) ) const;
 
   	/// @}
   	/// @name Standard Interface

gtsam/inference/Conditional.h

@@ -23,6 +23,7 @@
 #include <boost/utility.hpp> // for noncopyable
 #include <boost/foreach.hpp>
 #include <boost/range/iterator_range.hpp>
+#include <boost/lexical_cast.hpp>
 #include <boost/serialization/nvp.hpp>
 #include <gtsam/inference/Factor.h>
 
@@ -98,13 +99,16 @@ public:
   Conditional(KeyType key) : FactorType(key), nrFrontals_(1) { assertInvariants(); }
 
   /** Single parent */
-  Conditional(KeyType key, KeyType parent) : FactorType(key, parent), nrFrontals_(1) { assertInvariants(); }
+  Conditional(KeyType key, KeyType parent)
+  : FactorType(key, parent), nrFrontals_(1) { assertInvariants(); }
 
   /** Two parents */
-  Conditional(KeyType key, KeyType parent1, KeyType parent2) : FactorType(key, parent1, parent2), nrFrontals_(1) { assertInvariants(); }
+  Conditional(KeyType key, KeyType parent1, KeyType parent2)
+  : FactorType(key, parent1, parent2), nrFrontals_(1) { assertInvariants(); }
 
   /** Three parents */
-  Conditional(KeyType key, KeyType parent1, KeyType parent2, KeyType parent3) : FactorType(key, parent1, parent2, parent3), nrFrontals_(1) { assertInvariants(); }
+  Conditional(KeyType key, KeyType parent1, KeyType parent2, KeyType parent3)
+  : FactorType(key, parent1, parent2, parent3), nrFrontals_(1) { assertInvariants(); }
 
 	/// @}
 	/// @name Advanced Constructors
@@ -126,8 +130,9 @@ public:
 	/// @name Testable
 	/// @{
 
-  /** print */
+  /** print with optional formatter */
-  void print(const std::string& s = "Conditional") const;
+  void print(const std::string& s = "Conditional",
+  		const boost::function<std::string(KEY)>& formatter = &(boost::lexical_cast<std::string, KEY>) ) const;
 
   /** check equality */
   template<class DERIVED>
@@ -196,11 +201,11 @@ private:
 
 /* ************************************************************************* */
 template<typename KEY>
-void Conditional<KEY>::print(const std::string& s) const {
+void Conditional<KEY>::print(const std::string& s, const boost::function<std::string(KEY)>& formatter) const {
   std::cout << s << " P(";
-  BOOST_FOREACH(KeyType key, frontals()) std::cout << " " << key;
+  BOOST_FOREACH(KeyType key, frontals()) std::cout << " " << formatter(key);
   	if (nrParents()>0) std::cout << " |";
-  BOOST_FOREACH(KeyType parent, parents()) std::cout << " " << parent;
+  BOOST_FOREACH(KeyType parent, parents()) std::cout << " " << formatter(parent);
   	std::cout << ")" << std::endl;
 }
 

gtsam/linear/GaussianConditional.cpp

@@ -129,16 +129,16 @@ GaussianConditional& GaussianConditional::operator=(const GaussianConditional& r
 }
 
 /* ************************************************************************* */
-void GaussianConditional::print(const string &s) const
+void GaussianConditional::print(const string &s, const IndexFormatter& formatter) const
 {
   cout << s << ": density on ";
   for(const_iterator it = beginFrontals(); it != endFrontals(); ++it) {
-  	cout << (boost::format("[%1%]")%(*it)).str() << " ";
+  	cout << (boost::format("[%1%]")%(formatter(*it))).str() << " ";
   }
   cout << endl;
   gtsam::print(Matrix(get_R()),"R");
   for(const_iterator it = beginParents() ; it != endParents() ; ++it ) {
-    gtsam::print(Matrix(get_S(it)), (boost::format("A[%1%]")%(*it)).str());
+    gtsam::print(Matrix(get_S(it)), (boost::format("A[%1%]")%(formatter(*it))).str());
   }
   gtsam::print(Vector(get_d()),"d");
   gtsam::print(sigmas_,"sigmas");

gtsam/linear/GaussianConditional.h

@@ -138,7 +138,8 @@ public:
 	GaussianConditional& operator=(const GaussianConditional& rhs);
 
 	/** print */
-	void print(const std::string& = "GaussianConditional") const;
+	void print(const std::string& = "GaussianConditional",
+			const IndexFormatter& formatter = &(boost::lexical_cast<std::string, Index>)) const;
 
 	/** equals function */
 	bool equals(const GaussianConditional &cg, double tol = 1e-9) const;

gtsam/linear/GaussianFactor.h

@@ -23,6 +23,8 @@
 #include <gtsam/base/Matrix.h>
 #include <gtsam/inference/IndexFactor.h>
 
+#include <boost/lexical_cast.hpp>
+
 #include <string>
 #include <utility>
 
@@ -81,7 +83,9 @@ namespace gtsam {
     typedef boost::shared_ptr<GaussianFactor> shared_ptr;
 
     // Implementing Testable interface
-    virtual void print(const std::string& s = "") const = 0;
+    virtual void print(const std::string& s = "",
+    		const IndexFormatter& formatter = &(boost::lexical_cast<std::string, Index>)) const = 0;
+
     virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const = 0;
 
     virtual double error(const VectorValues& c) const = 0; /**  0.5*(A*x-b)'*D*(A*x-b) */

gtsam/linear/HessianFactor.cpp

@@ -292,11 +292,11 @@ HessianFactor& HessianFactor::operator=(const HessianFactor& rhs) {
 }
 
 /* ************************************************************************* */
-void HessianFactor::print(const std::string& s) const {
+void HessianFactor::print(const std::string& s, const IndexFormatter& formatter) const {
 	cout << s << "\n";
 	cout << " keys: ";
 	for(const_iterator key=this->begin(); key!=this->end(); ++key)
-		cout << *key << "(" << this->getDim(key) << ") ";
+		cout << formatter(*key) << "(" << this->getDim(key) << ") ";
 	cout << "\n";
 	gtsam::print(Matrix(info_.range(0,info_.nBlocks(), 0,info_.nBlocks()).selfadjointView<Eigen::Upper>()), "Ab^T * Ab: ");
 }

gtsam/linear/HessianFactor.h

@@ -218,7 +218,8 @@ namespace gtsam {
 		}
 
     /** Print the factor for debugging and testing (implementing Testable) */
-    virtual void print(const std::string& s = "") const;
+    virtual void print(const std::string& s = "",
+    		const IndexFormatter& formatter = &(boost::lexical_cast<std::string, Index>)) const;
 
     /** Compare to another factor for testing (implementing Testable) */
     virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const;

gtsam/linear/JacobianFactor.cpp

@@ -244,15 +244,15 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  void JacobianFactor::print(const string& s) const {
+  void JacobianFactor::print(const string& s, const IndexFormatter& formatter) const {
     cout << s << "\n";
     if (empty()) {
       cout << " empty, keys: ";
-      BOOST_FOREACH(const Index& key, keys()) { cout << key << " "; }
+      BOOST_FOREACH(const Index& key, keys()) { cout << formatter(key) << " "; }
       cout << endl;
     } else {
       for(const_iterator key=begin(); key!=end(); ++key)
-      	cout << boost::format("A[%1%]=\n")%*key << getA(key) << endl;
+      	cout << boost::format("A[%1%]=\n")%formatter(*key) << getA(key) << endl;
       cout << "b=" << getb() << endl;
       model_->print("model");
     }

gtsam/linear/JacobianFactor.h

@@ -151,7 +151,8 @@ namespace gtsam {
     }
 
     // Implementing Testable interface
-    virtual void print(const std::string& s = "") const;
+    virtual void print(const std::string& s = "",
+    		const IndexFormatter& formatter = &(boost::lexical_cast<std::string, Index>)) const;
     virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const;
 
     Vector unweighted_error(const VectorValues& c) const; /** (A*x-b) */

gtsam/nonlinear/ISAM2.h

@@ -254,7 +254,8 @@ public:
 
   /** Construct from an elimination result */
   ISAM2Clique(const std::pair<sharedConditional, boost::shared_ptr<ConditionalType::FactorType> >& result) :
-    Base(result.first), cachedFactor_(result.second), gradientContribution_(result.first->get_R().cols() + result.first->get_S().cols()) {
+    Base(result.first), cachedFactor_(result.second),
+    gradientContribution_(result.first->get_R().cols() + result.first->get_S().cols()) {
     // Compute gradient contribution
     const ConditionalType& conditional(*result.first);
     // Rewrite -(R * P')'*d   as   -(d' * R * P')'   for computational speed reasons
@@ -278,14 +279,18 @@ public:
   const Vector& gradientContribution() const { return gradientContribution_; }
 
   bool equals(const This& other, double tol=1e-9) const {
-    return Base::equals(other) && ((!cachedFactor_ && !other.cachedFactor_) || (cachedFactor_ && other.cachedFactor_ && cachedFactor_->equals(*other.cachedFactor_, tol)));
+    return Base::equals(other) &&
+    		((!cachedFactor_ && !other.cachedFactor_)
+    				|| (cachedFactor_ && other.cachedFactor_
+    						&& cachedFactor_->equals(*other.cachedFactor_, tol)));
   }
 
   /** print this node */
-  void print(const std::string& s = "") const {
+  void print(const std::string& s = "",
-    Base::print(s);
+  		const IndexFormatter& formatter = &(boost::lexical_cast<std::string, Index>)) const {
+    Base::print(s,formatter);
     if(cachedFactor_)
-      cachedFactor_->print(s + "Cached: ");
+      cachedFactor_->print(s + "Cached: ", formatter);
     else
       std::cout << s << "Cached empty" << std::endl;
     if(gradientContribution_.rows() != 0)

gtsam/nonlinear/NonlinearISAM.cpp

@@ -30,19 +30,6 @@ using namespace std;
 namespace gtsam {
 
 
-/* ************************************************************************* */
-// Create an index formatter that looks up the Key in an inverse ordering, then
-// formats the key using the provided key formatter, used in saveGraph.
-struct OrderingIndexFormatter {
-  Ordering::InvertedMap inverseOrdering;
-  const KeyFormatter& keyFormatter;
-  OrderingIndexFormatter(const Ordering& ordering, const KeyFormatter& keyFormatter) :
-      inverseOrdering(ordering.invert()), keyFormatter(keyFormatter) {}
-  string operator()(Index index) {
-    return keyFormatter(inverseOrdering.at(index));
-  }
-};
-
 /* ************************************************************************* */
 void NonlinearISAM::saveGraph(const string& s, const KeyFormatter& keyFormatter) const {
   isam_.saveGraph(s, OrderingIndexFormatter(ordering_, keyFormatter));

gtsam/nonlinear/Ordering.h

@@ -258,5 +258,17 @@ public:
   bool equals(const Unordered &t, double tol=0) const;
 };
 
+// Create an index formatter that looks up the Key in an inverse ordering, then
+// formats the key using the provided key formatter, used in saveGraph.
+struct OrderingIndexFormatter {
+  Ordering::InvertedMap inverseOrdering;
+  const KeyFormatter& keyFormatter;
+  OrderingIndexFormatter(const Ordering& ordering, const KeyFormatter& keyFormatter) :
+      inverseOrdering(ordering.invert()), keyFormatter(keyFormatter) {}
+  std::string operator()(Index index) {
+    return keyFormatter(inverseOrdering.at(index));
   }
+};
+
+} // \namespace gtsam