diff --git a/gtsam/linear/tests/testGaussianBayesTree.cpp b/gtsam/linear/tests/testGaussianBayesTree.cpp index c5601af27..3b960508d 100644 --- a/gtsam/linear/tests/testGaussianBayesTree.cpp +++ b/gtsam/linear/tests/testGaussianBayesTree.cpp @@ -15,18 +15,18 @@ * @author Kai Ni */ -#include #include - -#include -#include // for operator += -#include // for operator += - #include #include -#include +#include #include #include +#include + +#include +#include // for operator += +#include // for operator += +#include using namespace boost::assign; using namespace std::placeholders; @@ -321,6 +321,35 @@ TEST(GaussianBayesTree, determinant_and_smallestEigenvalue) { EXPECT_DOUBLES_EQUAL(expectedDeterminant,actualDeterminant,expectedDeterminant*1e-6);// relative tolerance } +/* ************************************************************************* */ +/** Test to expose bug in GaussianBayesTree::logDeterminant */ +TEST(GaussianBayesTree, LogDeterminant) { + using symbol_shorthand::L; + using symbol_shorthand::X; + + // Create a factor graph that will result in a bayes tree with at least 2 + // nodes + GaussianFactorGraph fg; + Key x1 = X(1), x2 = X(2), l1 = L(1); + SharedDiagonal unit2 = noiseModel::Unit::Create(2); + fg += JacobianFactor(x1, 10 * I_2x2, -1.0 * Vector2::Ones(), unit2); + fg += JacobianFactor(x2, 10 * I_2x2, x1, -10 * I_2x2, Vector2(2.0, -1.0), + unit2); + fg += JacobianFactor(l1, 5 * I_2x2, x1, -5 * I_2x2, Vector2(0.0, 1.0), unit2); + fg += + JacobianFactor(x2, -5 * I_2x2, l1, 5 * I_2x2, Vector2(-1.0, 1.5), unit2); + fg += JacobianFactor(x3, 10 * I_2x2, x2, -10 * I_2x2, Vector2(2.0, -1.0), + unit2); + fg += JacobianFactor(x3, 10 * I_2x2, -1.0 * Vector2::Ones(), unit2); + + // create corresponding Bayes net and Bayes tree: + boost::shared_ptr bn = fg.eliminateSequential(); + boost::shared_ptr bt = fg.eliminateMultifrontal(); + + // Test logDeterminant + EXPECT_DOUBLES_EQUAL(bn->logDeterminant(), bt->logDeterminant(), 1e-9); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */