From 40929e9cc370fc717aeeeae2adf69975736f620b Mon Sep 17 00:00:00 2001
From: Alex Cunningham <alexgcunningham@gmail.com>
Date: Mon, 4 Feb 2013 21:00:43 +0000
Subject: [PATCH] Added and wrapped determinant functions for BayesTree

---
 gtsam.h                              | 30 ++++++++++++++--------------
 gtsam/linear/GaussianBayesTree-inl.h | 22 +++++++++++++++++++-
 gtsam/linear/GaussianBayesTree.cpp   |  9 +++++++++
 gtsam/linear/GaussianBayesTree.h     | 17 ++++++++++++++++
 4 files changed, 62 insertions(+), 16 deletions(-)

diff --git a/gtsam.h b/gtsam.h
index 8358a8e86..f5d163141 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -871,7 +871,6 @@ virtual class SymbolicBayesNet  : gtsam::SymbolicBayesNetBase {
   bool permuteSeparatorWithInverse(const gtsam::Permutation& inversePermutation);
 };
 
-#include <gtsam/inference/SymbolicFactorGraph.h>
 typedef gtsam::BayesTreeClique<gtsam::IndexConditional> SymbolicBayesTreeClique;
 typedef gtsam::BayesTree<gtsam::IndexConditional, gtsam::SymbolicBayesTreeClique> SymbolicBayesTreeBase;
 virtual class SymbolicBayesTree : gtsam::SymbolicBayesTreeBase {
@@ -1114,21 +1113,21 @@ virtual class GaussianBayesNet  : gtsam::GaussianBayesNetBase {
 
 //Non-Class methods found in GaussianBayesNet.h
 //FIXME: No MATLAB documentation for these functions!
-/*gtsam::GaussianBayesNet scalarGaussian(size_t key, double mu, double sigma);
-gtsam::GaussianBayesNet simpleGaussian(size_t key, const Vector& mu, double sigma);
-void push_front(gtsam::GaussianBayesNet& bn, size_t key, Vector d, Matrix R, size_t name1, Matrix S, Vector sigmas);
-void push_front(gtsam::GaussianBayesNet& bn, size_t key, Vector d, Matrix R, size_t name1, Matrix S, size_t name2, Matrix T, Vector sigmas);
-gtsam::VectorValues* allocateVectorValues(const gtsam::GaussianBayesNet& bn);
-gtsam::VectorValues optimize(const gtsam::GaussianBayesNet& bn);
-void optimizeInPlace(const gtsam::GaussianBayesNet& bn, gtsam::VectorValues& x);
-gtsam::VectorValues optimizeGradientSearch(const gtsam::GaussianBayesNet& bn);
-void optimizeGradientSearchInPlace(const gtsam::GaussianBayesNet& bn, gtsam::VectorValues& grad);
-gtsam::VectorValues backSubstitute(const gtsam::GaussianBayesNet& bn, const gtsam::VectorValues& gx);
-gtsam::VectorValues backSubstituteTranspose(const gtsam::GaussianBayesNet& bn, const gtsam::VectorValues& gx);
-pair<Matrix, Vector> matrix(const gtsam::GaussianBayesNet& bn);
+//gtsam::GaussianBayesNet scalarGaussian(size_t key, double mu, double sigma);
+//gtsam::GaussianBayesNet simpleGaussian(size_t key, const Vector& mu, double sigma);
+//void push_front(gtsam::GaussianBayesNet& bn, size_t key, Vector d, Matrix R, size_t name1, Matrix S, Vector sigmas);
+//void push_front(gtsam::GaussianBayesNet& bn, size_t key, Vector d, Matrix R, size_t name1, Matrix S, size_t name2, Matrix T, Vector sigmas);
+//gtsam::VectorValues* allocateVectorValues(const gtsam::GaussianBayesNet& bn);
+//gtsam::VectorValues optimize(const gtsam::GaussianBayesNet& bn);
+//void optimizeInPlace(const gtsam::GaussianBayesNet& bn, gtsam::VectorValues& x);
+//gtsam::VectorValues optimizeGradientSearch(const gtsam::GaussianBayesNet& bn);
+//void optimizeGradientSearchInPlace(const gtsam::GaussianBayesNet& bn, gtsam::VectorValues& grad);
+//gtsam::VectorValues backSubstitute(const gtsam::GaussianBayesNet& bn, const gtsam::VectorValues& gx);
+//gtsam::VectorValues backSubstituteTranspose(const gtsam::GaussianBayesNet& bn, const gtsam::VectorValues& gx);
+//pair<Matrix, Vector> matrix(const gtsam::GaussianBayesNet& bn);
 double determinant(const gtsam::GaussianBayesNet& bayesNet);
-gtsam::VectorValues gradient(const gtsam::GaussianBayesNet& bayesNet, const gtsam::VectorValues& x0);
-void gradientAtZero(const gtsam::GaussianBayesNet& bayesNet, const gtsam::VectorValues& g);*/
+//gtsam::VectorValues gradient(const gtsam::GaussianBayesNet& bayesNet, const gtsam::VectorValues& x0);
+//void gradientAtZero(const gtsam::GaussianBayesNet& bayesNet, const gtsam::VectorValues& g);
 
 #include <gtsam/linear/GaussianBayesTree.h>
 typedef gtsam::BayesTreeClique<gtsam::GaussianConditional> GaussianBayesTreeClique;
@@ -1145,6 +1144,7 @@ gtsam::VectorValues optimize(const gtsam::GaussianBayesTree& bayesTree);
 gtsam::VectorValues optimizeGradientSearch(const gtsam::GaussianBayesTree& bayesTree);
 gtsam::VectorValues gradient(const gtsam::GaussianBayesTree& bayesTree, const gtsam::VectorValues& x0);
 gtsam::VectorValues* allocateVectorValues(const gtsam::GaussianBayesTree& bt);
+double determinant(const gtsam::GaussianBayesTree& bayesTree);
 
 virtual class GaussianFactor {
   void print(string s) const;
diff --git a/gtsam/linear/GaussianBayesTree-inl.h b/gtsam/linear/GaussianBayesTree-inl.h
index 4e398ef7f..4d659019d 100644
--- a/gtsam/linear/GaussianBayesTree-inl.h
+++ b/gtsam/linear/GaussianBayesTree-inl.h
@@ -23,6 +23,10 @@
 
 #include <gtsam/linear/GaussianBayesTree.h> // Only to help Eclipse
 
+#include <stdarg.h>
+
+using namespace std;
+
 namespace gtsam {
 
 /* ************************************************************************* */
@@ -36,6 +40,22 @@ void optimizeInPlace(const typename BAYESTREE::sharedClique& clique, VectorValue
   BOOST_FOREACH(const typename BAYESTREE::sharedClique& child, clique->children_)
     optimizeInPlace<BAYESTREE>(child, result);
 }
+
+/* ************************************************************************* */
+template<class BAYESTREE>
+double logDeterminant(const typename BAYESTREE::sharedClique& clique) {
+  double result = 0.0;
+
+  // this clique
+  result += clique->conditional()->get_R().diagonal().unaryExpr(ptr_fun<double,double>(log)).sum();
+
+  // sum of children
+  BOOST_FOREACH(const typename BAYESTREE::sharedClique& child, clique->children_)
+    result += logDeterminant<BAYESTREE>(child);
+
+  return result;
 }
 
-}
+/* ************************************************************************* */
+} // \namespace internal
+} // \namespace gtsam
diff --git a/gtsam/linear/GaussianBayesTree.cpp b/gtsam/linear/GaussianBayesTree.cpp
index c9fbb375d..7011524e4 100644
--- a/gtsam/linear/GaussianBayesTree.cpp
+++ b/gtsam/linear/GaussianBayesTree.cpp
@@ -80,7 +80,16 @@ void gradientAtZero(const GaussianBayesTree& bayesTree, VectorValues& g) {
   gradientAtZero(FactorGraph<JacobianFactor>(bayesTree), g);
 }
 
+/* ************************************************************************* */
+double determinant(const GaussianBayesTree& bayesTree) {
+  if (!bayesTree.root())
+    return 0.0;
+
+  return exp(internal::logDeterminant<GaussianBayesTree>(bayesTree.root()));
 }
+/* ************************************************************************* */
+
+} // \namespace gtsam
 
 
 
diff --git a/gtsam/linear/GaussianBayesTree.h b/gtsam/linear/GaussianBayesTree.h
index 91480d43a..3487e9d2d 100644
--- a/gtsam/linear/GaussianBayesTree.h
+++ b/gtsam/linear/GaussianBayesTree.h
@@ -91,6 +91,23 @@ VectorValues gradient(const GaussianBayesTree& bayesTree, const VectorValues& x0
  */
 void gradientAtZero(const GaussianBayesTree& bayesTree, VectorValues& g);
 
+/**
+ * Computes the determinant of a GassianBayesTree
+ * A GassianBayesTree is an upper triangular matrix and for an upper triangular matrix
+ * determinant is the product of the diagonal elements. Instead of actually multiplying
+ * we add the logarithms of the diagonal elements and take the exponent at the end
+ * because this is more numerically stable.
+ * @param bayesTree The input GassianBayesTree
+ * @return The determinant
+ */
+double determinant(const GaussianBayesTree& bayesTree);
+
+
+namespace internal {
+template<class BAYESTREE>
+double logDeterminant(const typename BAYESTREE::sharedClique& clique);
+}
+
 }
 
 #include <gtsam/linear/GaussianBayesTree-inl.h>