Recursive print

gtsam/nonlinear/Expression-inl.h

@@ -29,7 +29,7 @@ namespace gtsam {
 
 template<typename T>
 void Expression<T>::print(const std::string& s) const {
-  std::cout << s << *root_ << std::endl;
+  root_->print(s);
 }
 
 template<typename T>
@@ -155,7 +155,7 @@ size_t Expression<T>::traceSize() const {
 
 template<typename T>
 T Expression<T>::valueAndDerivatives(const Values& values,
-    const FastVector<Key>& keys, const FastVector<int>& dims,
+    const KeyVector& keys, const FastVector<int>& dims,
     std::vector<Matrix>& H) const {
 
   // H should be pre-allocated
@@ -205,6 +205,7 @@ T Expression<T>::valueAndJacobianMap(const Values& values,
 
   internal::ExecutionTrace<T> trace;
   T value(this->traceExecution(values, trace, traceStorage));
+  GTSAM_PRINT(trace);
   trace.startReverseAD1(jacobians);
 
 #ifdef _MSC_VER
@@ -219,7 +220,7 @@ typename Expression<T>::KeysAndDims Expression<T>::keysAndDims() const {
   std::map<Key, int> map;
   dims(map);
   size_t n = map.size();
-  KeysAndDims pair = std::make_pair(FastVector<Key>(n), FastVector<int>(n));
+  KeysAndDims pair = std::make_pair(KeyVector(n), FastVector<int>(n));
   boost::copy(map | boost::adaptors::map_keys, pair.first.begin());
   boost::copy(map | boost::adaptors::map_values, pair.second.begin());
   return pair;

gtsam/nonlinear/Expression.h

@@ -173,11 +173,11 @@ public:
 private:
 
   /// Keys and dimensions in same order
-  typedef std::pair<FastVector<Key>, FastVector<int> > KeysAndDims;
+  typedef std::pair<KeyVector, FastVector<int> > KeysAndDims;
   KeysAndDims keysAndDims() const;
 
   /// private version that takes keys and dimensions, returns derivatives
-  T valueAndDerivatives(const Values& values, const FastVector<Key>& keys,
+  T valueAndDerivatives(const Values& values, const KeyVector& keys,
       const FastVector<int>& dims, std::vector<Matrix>& H) const;
 
   /// trace execution, very unsafe

gtsam/nonlinear/internal/ExpressionNode.h

@@ -78,13 +78,14 @@ public:
   virtual ~ExpressionNode() {
   }
 
+  /// Print
+  virtual void print(const std::string& indent = "") const = 0;
+
   /// Streaming
   GTSAM_EXPORT
-  friend std::ostream &operator<<(std::ostream &os,
+  friend std::ostream& operator<<(std::ostream& os, const ExpressionNode& node) {
-      const ExpressionNode& node) {
     os << "Expression of type " << typeid(T).name();
-    if (node.traceSize_ > 0)
+    if (node.traceSize_ > 0) os << ", trace size = " << node.traceSize_;
-      os << ", trace size = " << node.traceSize_;
     os << "\n";
     return os;
   }
@@ -133,6 +134,11 @@ public:
   virtual ~ConstantExpression() {
   }
 
+  /// Print
+  virtual void print(const std::string& indent = "") const {
+    std::cout << indent << "Constant" << std::endl;
+  }
+
   /// Return value
   virtual T value(const Values& values) const {
     return constant_;
@@ -159,7 +165,7 @@ class LeafExpression: public ExpressionNode<T> {
       key_(key) {
   }
 
-  friend class Expression<T> ;
+  friend class Expression<T>;
 
 public:
 
@@ -167,6 +173,11 @@ public:
   virtual ~LeafExpression() {
   }
 
+  /// Print
+  virtual void print(const std::string& indent = "") const {
+    std::cout << indent << "Leaf, key = " << key_ << std::endl;
+  }
+
   /// Return keys that play in this expression
   virtual std::set<Key> keys() const {
     std::set<Key> keys;
@@ -218,7 +229,7 @@ class UnaryExpression: public ExpressionNode<T> {
     ExpressionNode<T>::traceSize_ = upAligned(sizeof(Record)) + e1.traceSize();
   }
 
-  friend class Expression<T> ;
+  friend class Expression<T>;
 
 public:
 
@@ -226,6 +237,12 @@ public:
   virtual ~UnaryExpression() {
   }
 
+  /// Print
+  virtual void print(const std::string& indent = "") const {
+    std::cout << indent << "UnaryExpression" << std::endl;
+    expression1_->print(indent + "  ");
+  }
+
   /// Return value
   virtual T value(const Values& values) const {
     return function_(expression1_->value(values), boost::none);
@@ -320,7 +337,7 @@ class BinaryExpression: public ExpressionNode<T> {
         upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize();
   }
 
-  friend class Expression<T> ;
+  friend class Expression<T>;
   friend class ::ExpressionFactorBinaryTest;
 
 public:
@@ -329,6 +346,13 @@ public:
   virtual ~BinaryExpression() {
   }
 
+  /// Print
+  virtual void print(const std::string& indent = "") const {
+    std::cout << indent << "BinaryExpression" << std::endl;
+    expression1_->print(indent + "  ");
+    expression2_->print(indent + "  ");
+  }
+
   /// Return value
   virtual T value(const Values& values) const {
     using boost::none;
@@ -420,7 +444,7 @@ class TernaryExpression: public ExpressionNode<T> {
         e1.traceSize() + e2.traceSize() + e3.traceSize();
   }
 
-  friend class Expression<T> ;
+  friend class Expression<T>;
 
 public:
 
@@ -428,6 +452,14 @@ public:
   virtual ~TernaryExpression() {
   }
 
+  /// Print
+  virtual void print(const std::string& indent = "") const {
+    std::cout << indent << "TernaryExpression" << std::endl;
+    expression1_->print(indent + "  ");
+    expression2_->print(indent + "  ");
+    expression3_->print(indent + "  ");
+  }
+
   /// Return value
   virtual T value(const Values& values) const {
     using boost::none;