Added determinant function to GaussianBayesNet and added a unit test

gtsam/linear/GaussianBayesNet.cpp

@@ -202,6 +202,17 @@ VectorValues rhs(const GaussianBayesNet& bn) {
   return *result;
 }
 
+/* ************************************************************************* */
+double determinant(const GaussianBayesNet& bayesNet) {
+	double logDet = 0.0;
+
+	BOOST_FOREACH(boost::shared_ptr<const GaussianConditional> cg, bayesNet){
+		logDet += cg->get_R().diagonal().unaryExpr(ptr_fun<double,double>(log)).sum();
+	}
+
+	return exp(logDet);
+}
+
 /* ************************************************************************* */
 
 } // namespace gtsam

gtsam/linear/GaussianBayesNet.h

@@ -97,4 +97,15 @@ namespace gtsam {
    */
   VectorValues rhs(const GaussianBayesNet&);
 
+  /**
+   * Computes the determinant of a GassianBayesNet
+   * A GaussianBayesNet 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 bayesNet The input GaussianBayesNet
+   * @return The determinant
+   */
+  double determinant(const GaussianBayesNet& bayesNet);
+
 } /// namespace gtsam

tests/testGaussianBayesNet.cpp

@@ -201,6 +201,31 @@ TEST( GaussianBayesNet, backSubstituteTranspose )
   EXPECT(assert_equal(y,actual));
 }
 
+/* ************************************************************************* */
+// Tests computing Determinant
+TEST( GaussianBayesNet, DeterminantTest )
+{
+	GaussianBayesNet cbn;
+	cbn += boost::shared_ptr<GaussianConditional>(new GaussianConditional(
+					0, Vector_( 2, 3.0, 4.0 ), Matrix_(2, 2, 1.0, 3.0, 0.0, 4.0 ),
+					1, Matrix_(2, 2, 2.0, 1.0, 2.0, 3.0),
+					ones(2)));
+
+	cbn += boost::shared_ptr<GaussianConditional>(new GaussianConditional(
+					1, Vector_( 2, 5.0, 6.0 ), Matrix_(2, 2, 1.0, 1.0, 0.0, 3.0 ),
+					2, Matrix_(2, 2, 1.0, 0.0, 5.0, 2.0),
+					ones(2)));
+
+	cbn += boost::shared_ptr<GaussianConditional>(new GaussianConditional(
+			3, Vector_( 2, 7.0, 8.0 ), Matrix_(2, 2, 1.0, 1.0, 0.0, 5.0 ),
+			ones(2)));
+
+	double expectedDeterminant = 60;
+	double actualDeterminant = determinant(cbn);
+
+  EXPECT_DOUBLES_EQUAL( expectedDeterminant, actualDeterminant, 1e-9);
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
 /* ************************************************************************* */