Added nrNodes() and checkConsistency() to BayesTree

gtsam.h

@@ -811,6 +811,7 @@ virtual class BayesTree {
     //Standard Interface
   //size_t findParentClique(const gtsam::IndexVector& parents) const;
     size_t size();
+    size_t nrNodes() const;
     void saveGraph(string s) const;
     CLIQUE* root() const;
     void clear();
@@ -818,6 +819,7 @@ virtual class BayesTree {
     void insert(const CLIQUE* subtree);
     size_t numCachedSeparatorMarginals() const;
     CLIQUE* clique(size_t j) const;
+    bool checkConsistency() const;
 };
 
 template<CONDITIONAL>

gtsam/inference/BayesTree-inl.h

@@ -799,5 +799,24 @@ namespace gtsam {
     }
   }
 
-}
+  /* ************************************************************************* */
-/// namespace gtsam
+  template<class CONDITIONAL, class CLIQUE>
+  bool BayesTree<CONDITIONAL,CLIQUE>::checkConsistency() const {
+    // Verify all nodes are mapped to initialized cliques
+    bool result = true;
+    for (gtsam::Index idx=0; idx<nodes_.size(); ++idx) {
+      const sharedClique& clique = nodes_[idx];
+      // Bad clique check
+      if (!clique) {
+        if (result)
+          std::cout << "BayesTree consistency check failures:" << std::endl;
+        std::cout << "  Null pointer clique entry for node " << idx << std::endl;
+        result = false;
+        break;
+      }
+    }
+    return result;
+  }
+  /* ************************************************************************* */
+
+} // \namespace gtsam

gtsam/inference/BayesTree.h

@@ -162,6 +162,9 @@ namespace gtsam {
         return 0;
     }
 
+    /** number of nodes */
+    inline size_t nrNodes() const { return nodes_.size(); }
+
     /** Check if there are any cliques in the tree */
     inline bool empty() const {
       return nodes_.empty();
@@ -287,6 +290,13 @@ namespace gtsam {
       return shared_ptr(new This(*this));
     }
 
+    /**
+     * Verify internal consistency of indexing
+     * Prints results to stdout
+     * @return true iff correct
+     */
+    bool checkConsistency() const;
+
   protected:
 
     /** private helper method for saving the Tree to a text file in GraphViz format */