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.cpp b/gtsam/inference/inferenceExceptions.cpp new file mode 100644 index 000000000..8c2a95337 --- /dev/null +++ b/gtsam/inference/inferenceExceptions.cpp @@ -0,0 +1,60 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file inferenceExceptions.cpp + * @brief Exceptions that may be thrown by inference algorithms + * @author Richard Roberts, Varun Agrawal + * @date Apr 25, 2013 + */ + +#include + +#include + +namespace gtsam { + +InconsistentEliminationRequested::InconsistentEliminationRequested( + const KeySet& keys, const KeyFormatter& key_formatter) + : keys_(keys.begin(), keys.end()), keyFormatter(key_formatter) {} + +const char* InconsistentEliminationRequested::what() const noexcept { + // Format keys for printing + std::stringstream sstr; + size_t nrKeysToDisplay = std::min(size_t(4), keys_.size()); + for (size_t i = 0; i < nrKeysToDisplay; i++) { + sstr << keyFormatter(keys_.at(i)); + if (i < nrKeysToDisplay - 1) { + sstr << ", "; + } + } + if (keys_.size() > nrKeysToDisplay) { + sstr << ", ... (total " << keys_.size() << " keys)"; + } + sstr << "."; + std::string keys = sstr.str(); + + 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); + // `new` to allocate memory on heap instead of stack + return (new std::string(msg))->c_str(); +} +} // namespace gtsam diff --git a/gtsam/inference/inferenceExceptions.h b/gtsam/inference/inferenceExceptions.h index fa3e3cb25..eb5d79e6f 100644 --- a/gtsam/inference/inferenceExceptions.h +++ b/gtsam/inference/inferenceExceptions.h @@ -12,30 +12,35 @@ /** * @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 { + KeyVector keys_; + const KeyFormatter& keyFormatter = DefaultKeyFormatter; -} + public: + InconsistentEliminationRequested() noexcept {} + + InconsistentEliminationRequested( + const KeySet& keys, + const KeyFormatter& key_formatter = DefaultKeyFormatter); + + ~InconsistentEliminationRequested() noexcept override {} + + const char* what() const noexcept override; +}; +} // namespace gtsam diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index 41ee9471e..2ba00abc1 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include @@ -457,6 +458,64 @@ 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()); + } + + // Test large number of keys + fg = GaussianFactorGraph(); + for (size_t i = 0; i < 1000; i++) { + fg.emplace_shared(i, -I_2x2, i + 1, I_2x2, + Vector2(2.0, -1.0), unit2); + } + + 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, 3, 4, 5, ... (total 999 keys)."; + EXPECT(expected_exception_message == exc.what()); + } +} /* ************************************************************************* */ int main() { TestResult tr;