Added and wrapped determinant functions for BayesTree

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 scalarGaussian(size_t key, double mu, double sigma);
-gtsam::GaussianBayesNet simpleGaussian(size_t key, const Vector& 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, 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);
+//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* allocateVectorValues(const gtsam::GaussianBayesNet& bn);
-gtsam::VectorValues optimize(const gtsam::GaussianBayesNet& bn);
+//gtsam::VectorValues optimize(const gtsam::GaussianBayesNet& bn);
-void optimizeInPlace(const gtsam::GaussianBayesNet& bn, gtsam::VectorValues& x);
+//void optimizeInPlace(const gtsam::GaussianBayesNet& bn, gtsam::VectorValues& x);
-gtsam::VectorValues optimizeGradientSearch(const gtsam::GaussianBayesNet& bn);
+//gtsam::VectorValues optimizeGradientSearch(const gtsam::GaussianBayesNet& bn);
-void optimizeGradientSearchInPlace(const gtsam::GaussianBayesNet& bn, gtsam::VectorValues& grad);
+//void optimizeGradientSearchInPlace(const gtsam::GaussianBayesNet& bn, gtsam::VectorValues& grad);
-gtsam::VectorValues backSubstitute(const gtsam::GaussianBayesNet& bn, const gtsam::VectorValues& gx);
+//gtsam::VectorValues backSubstitute(const gtsam::GaussianBayesNet& bn, const gtsam::VectorValues& gx);
-gtsam::VectorValues backSubstituteTranspose(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);
+//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);
+//gtsam::VectorValues gradient(const gtsam::GaussianBayesNet& bayesNet, const gtsam::VectorValues& x0);
-void gradientAtZero(const gtsam::GaussianBayesNet& bayesNet, const gtsam::VectorValues& g);*/
+//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;

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

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
 
 
 

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>