From c709932f9807ddc02a696ec689965cc31da8a416 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 14 Nov 2022 10:46:38 -0500 Subject: [PATCH 1/3] Add unit test exposing GaussianBayesTree determinant bug --- gtsam/linear/tests/testGaussianBayesTree.cpp | 43 ++++++++++++++++---- 1 file changed, 36 insertions(+), 7 deletions(-) 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);} /* ************************************************************************* */ From 3dcf9d8da810cb4225a2c242dd82de343d55cf2a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 14 Nov 2022 10:54:03 -0500 Subject: [PATCH 2/3] fix bug in GaussianBayesTree::logDeterminant --- gtsam/linear/GaussianBayesTree.cpp | 56 +++++++++++++++++++++--------- 1 file changed, 39 insertions(+), 17 deletions(-) diff --git a/gtsam/linear/GaussianBayesTree.cpp b/gtsam/linear/GaussianBayesTree.cpp index 13c19bce6..a83475e26 100644 --- a/gtsam/linear/GaussianBayesTree.cpp +++ b/gtsam/linear/GaussianBayesTree.cpp @@ -31,18 +31,37 @@ namespace gtsam { template class BayesTreeCliqueBase; template class BayesTree; - /* ************************************************************************* */ - namespace internal - { - /* ************************************************************************* */ - double logDeterminant(const GaussianBayesTreeClique::shared_ptr& clique, - double& parentSum) { - parentSum += clique->conditional() - ->R() - .diagonal() - .unaryExpr([](double x) { return log(x); }) - .sum(); - return 0; + /* ************************************************************************ */ + namespace internal { + + /** + * @brief Struct to help with traversing the Bayes Tree + * for log-determinant computation. + * Records the data which is passed to the child nodes in pre-order visit. + */ + struct LogDeterminantData { + // Use pointer so we can get the full result after tree traversal + double* logDet; + LogDeterminantData(double* logDet) + : logDet(logDet) {} + }; + /* ************************************************************************ */ + LogDeterminantData& logDeterminant( + const GaussianBayesTreeClique::shared_ptr& clique, + LogDeterminantData& parentSum) { + auto cg = clique->conditional(); + double logDet; + if (cg->get_model()) { + Vector diag = cg->R().diagonal(); + cg->get_model()->whitenInPlace(diag); + logDet = diag.unaryExpr([](double x) { return log(x); }).sum(); + } else { + logDet = + cg->R().diagonal().unaryExpr([](double x) { return log(x); }).sum(); + } + // Add the current clique's log-determinant to the overall sum + (*parentSum.logDet) += logDet; + return parentSum; } } // namespace internal @@ -87,7 +106,14 @@ namespace gtsam { return 0.0; } else { double sum = 0.0; - treeTraversal::DepthFirstForest(*this, sum, internal::logDeterminant); + // Store the log-determinant in this struct. + internal::LogDeterminantData rootData(&sum); + // No need to do anything for post-operation. + treeTraversal::no_op visitorPost; + // Limits OpenMP threads if we're mixing TBB and OpenMP + TbbOpenMPMixedScope threadLimiter; + // Traverse the GaussianBayesTree depth first and call logDeterminant on each node. + treeTraversal::DepthFirstForestParallel(*this, rootData, internal::logDeterminant, visitorPost); return sum; } } @@ -106,7 +132,3 @@ namespace gtsam { } // \namespace gtsam - - - - From 93079f32e1b2a13a9dd06433f8489882cf7a10b0 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 14 Nov 2022 10:58:02 -0500 Subject: [PATCH 3/3] docstring updates --- gtsam/linear/tests/testGaussianBayesTree.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/linear/tests/testGaussianBayesTree.cpp b/gtsam/linear/tests/testGaussianBayesTree.cpp index 3b960508d..18b4674b5 100644 --- a/gtsam/linear/tests/testGaussianBayesTree.cpp +++ b/gtsam/linear/tests/testGaussianBayesTree.cpp @@ -322,13 +322,13 @@ TEST(GaussianBayesTree, determinant_and_smallestEigenvalue) { } /* ************************************************************************* */ -/** Test to expose bug in GaussianBayesTree::logDeterminant */ +/// 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 + // 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);