diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index c41846217..9806b435c 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/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 cg, bayesNet){ + logDet += cg->get_R().diagonal().unaryExpr(ptr_fun(log)).sum(); + } + + return exp(logDet); +} + /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index e769695ca..171d061ee 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/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 diff --git a/tests/testGaussianBayesNet.cpp b/tests/testGaussianBayesNet.cpp index 8b6199ec5..7f746397c 100644 --- a/tests/testGaussianBayesNet.cpp +++ b/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(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(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(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);} /* ************************************************************************* */