From 64cd43636235bfba3cb42ba5a1c61ee89b20c2b0 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 15 Jul 2023 16:53:17 -0400 Subject: [PATCH] update InconsistentEliminationRequested to show leftover keys --- .../inference/EliminateableFactorGraph-inst.h | 10 +-- gtsam/inference/inferenceExceptions.h | 63 ++++++++++++++----- .../linear/tests/testGaussianFactorGraph.cpp | 52 ++++++++++++--- 3 files changed, 95 insertions(+), 30 deletions(-) diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index 8a524e353..f6b11f785 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -74,8 +74,9 @@ namespace gtsam { EliminationTreeType etree(asDerived(), (*variableIndex).get(), ordering); const auto [bayesNet, factorGraph] = etree.eliminate(function); // If any factors are remaining, the ordering was incomplete - if(!factorGraph->empty()) - throw InconsistentEliminationRequested(); + if(!factorGraph->empty()) { + throw InconsistentEliminationRequested(factorGraph->keys()); + } // Return the Bayes net return bayesNet; } @@ -136,8 +137,9 @@ namespace gtsam { JunctionTreeType junctionTree(etree); const auto [bayesTree, factorGraph] = junctionTree.eliminate(function); // If any factors are remaining, the ordering was incomplete - if(!factorGraph->empty()) - throw InconsistentEliminationRequested(); + if(!factorGraph->empty()) { + throw InconsistentEliminationRequested(factorGraph->keys()); + } // Return the Bayes tree return bayesTree; } diff --git a/gtsam/inference/inferenceExceptions.h b/gtsam/inference/inferenceExceptions.h index fa3e3cb25..061abeb85 100644 --- a/gtsam/inference/inferenceExceptions.h +++ b/gtsam/inference/inferenceExceptions.h @@ -12,30 +12,59 @@ /** * @file inferenceExceptions.h * @brief Exceptions that may be thrown by inference algorithms - * @author Richard Roberts + * @author Richard Roberts, Varun Agrawal * @date Apr 25, 2013 */ #pragma once #include + #include +#include namespace gtsam { - /** An inference algorithm was called with inconsistent arguments. The factor graph, ordering, or - * variable index were inconsistent with each other, or a full elimination routine was called - * with an ordering that does not include all of the variables. */ - class InconsistentEliminationRequested : public std::exception { - public: - InconsistentEliminationRequested() noexcept {} - ~InconsistentEliminationRequested() noexcept override {} - const char* what() const noexcept override { - return - "An inference algorithm was called with inconsistent arguments. The\n" - "factor graph, ordering, or variable index were inconsistent with each\n" - "other, or a full elimination routine was called with an ordering that\n" - "does not include all of the variables."; - } - }; +/** An inference algorithm was called with inconsistent arguments. The factor + * graph, ordering, or variable index were inconsistent with each other, or a + * full elimination routine was called with an ordering that does not include + * all of the variables. */ +class InconsistentEliminationRequested : public std::exception { + KeySet keys_; + const KeyFormatter& keyFormatter = DefaultKeyFormatter; -} + public: + InconsistentEliminationRequested() noexcept {} + + InconsistentEliminationRequested( + const KeySet& keys, + const KeyFormatter& key_formatter = DefaultKeyFormatter) + : keys_(keys), keyFormatter(key_formatter) {} + + ~InconsistentEliminationRequested() noexcept override {} + const char* what() const noexcept override { + // Format keys for printing + std::stringstream sstr; + for (auto key : keys_) { + sstr << keyFormatter(key) << ", "; + } + std::string keys = sstr.str(); + // remove final comma and space. + keys.pop_back(); + keys.pop_back(); + + static std::string msg = + "An inference algorithm was called with inconsistent " + "arguments. " + "The\n" + "factor graph, ordering, or variable index were " + "inconsistent with " + "each\n" + "other, or a full elimination routine was called with " + "an ordering " + "that\n" + "does not include all of the variables.\n"; + msg += ("Leftover keys after elimination: " + keys); + return msg.c_str(); + } +}; +} // namespace gtsam diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index e9e626296..b140c3d85 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -18,16 +18,16 @@ * @author Richard Roberts **/ -#include -#include -#include -#include -#include -#include -#include - -#include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include using namespace std; using namespace gtsam; @@ -435,6 +435,40 @@ TEST(GaussianFactorGraph, ProbPrime) { EXPECT_DOUBLES_EQUAL(expected, gfg.probPrime(values), 1e-12); } +TEST(GaussianFactorGraph, InconsistentEliminationMessage) { + // Create empty graph + GaussianFactorGraph fg; + SharedDiagonal unit2 = noiseModel::Unit::Create(2); + + using gtsam::symbol_shorthand::X; + fg.emplace_shared(0, 10 * I_2x2, -1.0 * Vector::Ones(2), + unit2); + fg.emplace_shared(0, -10 * I_2x2, 1, 10 * I_2x2, + Vector2(2.0, -1.0), unit2); + fg.emplace_shared(1, -5 * I_2x2, 2, 5 * I_2x2, + Vector2(-1.0, 1.5), unit2); + fg.emplace_shared(2, -5 * I_2x2, X(3), 5 * I_2x2, + Vector2(-1.0, 1.5), unit2); + + Ordering ordering{0, 1}; + + try { + fg.eliminateSequential(ordering); + } catch (const exception& exc) { + std::string expected_exception_message = "An inference algorithm was called with inconsistent " + "arguments. " + "The\n" + "factor graph, ordering, or variable index were " + "inconsistent with " + "each\n" + "other, or a full elimination routine was called with " + "an ordering " + "that\n" + "does not include all of the variables.\n" + "Leftover keys after elimination: 2, x3"; + EXPECT(expected_exception_message == exc.what()); + } +} /* ************************************************************************* */ int main() { TestResult tr;