From 41b0b903768931a63f704be4d8e204e454090544 Mon Sep 17 00:00:00 2001
From: Alex Cunningham <alexgcunningham@gmail.com>
Date: Wed, 22 May 2013 17:27:42 +0000
Subject: [PATCH] Added nrNodes() and checkConsistency() to BayesTree

---
 gtsam.h                         |  2 ++
 gtsam/inference/BayesTree-inl.h | 23 +++++++++++++++++++++--
 gtsam/inference/BayesTree.h     | 10 ++++++++++
 3 files changed, 33 insertions(+), 2 deletions(-)

diff --git a/gtsam.h b/gtsam.h
index dbd3d9fad..841c4b01c 100644
--- a/gtsam.h
+++ b/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>
diff --git a/gtsam/inference/BayesTree-inl.h b/gtsam/inference/BayesTree-inl.h
index 9f672e56f..f3aeacb36 100644
--- a/gtsam/inference/BayesTree-inl.h
+++ b/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
diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h
index 8f57531ae..a6f5b6e81 100644
--- a/gtsam/inference/BayesTree.h
+++ b/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 */