From 3c68d20ff2829027b13f8ada89a2bc6de2656e01 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Thu, 3 Oct 2013 16:50:20 +0000
Subject: [PATCH] Fixed calculate_nnz and added unit test

---
 gtsam/nonlinear/ISAM2-inl.h |  6 +++---
 tests/testGaussianISAM2.cpp | 10 ++++++++++
 2 files changed, 13 insertions(+), 3 deletions(-)

diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h
index 0d33308fb..e7ee9780e 100644
--- a/gtsam/nonlinear/ISAM2-inl.h
+++ b/gtsam/nonlinear/ISAM2-inl.h
@@ -283,11 +283,11 @@ size_t optimizeWildfireNonRecursive(const boost::shared_ptr<CLIQUE>& root, doubl
 /* ************************************************************************* */
 template<class CLIQUE>
 void nnz_internal(const boost::shared_ptr<CLIQUE>& clique, int& result) {
-  int dimR = (*clique)->dim();
-  int dimSep = (*clique)->get_S().cols() - dimR;
+  int dimR = clique->conditional()->rows();
+  int dimSep = clique->conditional()->get_S().cols();
   result += ((dimR+1)*dimR)/2 + dimSep*dimR;
   // traverse the children
-  BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children_) {
+  BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children) {
     nnz_internal(child, result);
   }
 }
diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp
index 0e68589e7..4d789e271 100644
--- a/tests/testGaussianISAM2.cpp
+++ b/tests/testGaussianISAM2.cpp
@@ -815,6 +815,16 @@ TEST(ISAM2, marginalCovariance)
   EXPECT(assert_equal(expected, actual));
 }
 
+/* ************************************************************************* */
+TEST(ISAM2, calculate_nnz)
+{
+  ISAM2 isam = createSlamlikeISAM2();
+  int expected = 262;
+  int actual = calculate_nnz(isam.roots().front());
+
+  EXPECT_LONGS_EQUAL(expected, actual);
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
 /* ************************************************************************* */