From e86c9465bb1a1669d7e5d75a0c25c5d26f642326 Mon Sep 17 00:00:00 2001
From: Michael Kaess <michaelkaess@gmail.com>
Date: Mon, 6 Sep 2010 20:34:59 +0000
Subject: [PATCH] calculating the number of nonzero entries in R corresponding
 to the Bayes tree

---
 inference/ISAM2-inl.h  |  4 +++-
 slam/GaussianISAM2.cpp | 27 +++++++++++++++++++++++++++
 slam/GaussianISAM2.h   |  3 +++
 3 files changed, 33 insertions(+), 1 deletion(-)

diff --git a/inference/ISAM2-inl.h b/inference/ISAM2-inl.h
index 8f82bf009..6bbae8314 100644
--- a/inference/ISAM2-inl.h
+++ b/inference/ISAM2-inl.h
@@ -221,12 +221,14 @@ namespace gtsam {
 		int maxClique = 0;
 		double avgClique = 0;
 		int numCliques = 0;
+		int nnzR = 0;
 		if (counter>0) { // cannot call on empty tree
 			GaussianISAM2_P::CliqueData cdata =  this->getCliqueData();
 			GaussianISAM2_P::CliqueStats cstats = cdata.getStats();
 			maxClique = cstats.maxConditionalSize;
 			avgClique = cstats.avgConditionalSize;
 			numCliques = cdata.conditionalSizes.size();
+			nnzR = calculate_nnz(this->root());
 		}
 		counter++;
 #endif
@@ -267,7 +269,7 @@ namespace gtsam {
 #ifdef PRINT_STATS
 		cout << "linear: #newKeys: " << newKeys.size() << " #affectedVariables: " << affectedKeys.size()
 		  << " #affectedFactors: " << factors.size() << " maxCliqueSize: " << maxClique
-		  << " avgCliqueSize: " << avgClique << " #Cliques: " << numCliques << endl;
+		  << " avgCliqueSize: " << avgClique << " #Cliques: " << numCliques << " nnzR: " << nnzR << endl;
 #endif
 
 		toc("linear_lookup1");
diff --git a/slam/GaussianISAM2.cpp b/slam/GaussianISAM2.cpp
index 7dbd7ec0f..67396eb29 100644
--- a/slam/GaussianISAM2.cpp
+++ b/slam/GaussianISAM2.cpp
@@ -53,4 +53,31 @@ VectorConfig optimize2(const GaussianISAM2_P& bayesTree, double threshold) {
 	return result;
 }
 
+/* ************************************************************************* */
+void nnz_internal(const GaussianISAM2::sharedClique& clique, int& result) {
+	// go through the conditionals of this clique
+	GaussianISAM2::Clique::const_reverse_iterator it;
+	for (it = clique->rbegin(); it!=clique->rend(); it++) {
+		GaussianConditional::shared_ptr cg = *it;
+		int dimSep = 0;
+		for (GaussianConditional::const_iterator matrix_it = cg->parentsBegin(); matrix_it != cg->parentsEnd(); matrix_it++) {
+			dimSep += matrix_it->second.size2();
+		}
+		int dimR = cg->dim();
+    result += (dimR+1)*dimR/2 + dimSep*dimR;
+  }
+	// traverse the children
+	BOOST_FOREACH(const GaussianISAM2::sharedClique& child, clique->children_) {
+		nnz_internal(child, result);
+	}
+}
+
+/* ************************************************************************* */
+int calculate_nnz(const GaussianISAM2::sharedClique& clique) {
+	int result = 0;
+	// starting from the root, add up entries of frontal and conditional matrices of each conditional
+	nnz_internal(clique, result);
+	return result;
+}
+
 } /// namespace gtsam
diff --git a/slam/GaussianISAM2.h b/slam/GaussianISAM2.h
index 3acf840a8..b8b52bd28 100644
--- a/slam/GaussianISAM2.h
+++ b/slam/GaussianISAM2.h
@@ -35,4 +35,7 @@ namespace gtsam {
 	// optimize the BayesTree, starting from the root
 	VectorConfig optimize2(const GaussianISAM2_P& bayesTree, double threshold = 0.);
 
+	// calculate the number of non-zero entries for the tree starting at clique (use root for complete matrix)
+	int calculate_nnz(const GaussianISAM2::sharedClique& clique);
+
 }/// namespace gtsam