diff --git a/gtsam/inference/inferenceExceptions.h b/gtsam/inference/inferenceExceptions.h index 061abeb85..339accad6 100644 --- a/gtsam/inference/inferenceExceptions.h +++ b/gtsam/inference/inferenceExceptions.h @@ -29,7 +29,7 @@ namespace gtsam { * full elimination routine was called with an ordering that does not include * all of the variables. */ class InconsistentEliminationRequested : public std::exception { - KeySet keys_; + KeyVector keys_; const KeyFormatter& keyFormatter = DefaultKeyFormatter; public: @@ -38,21 +38,26 @@ class InconsistentEliminationRequested : public std::exception { InconsistentEliminationRequested( const KeySet& keys, const KeyFormatter& key_formatter = DefaultKeyFormatter) - : keys_(keys), keyFormatter(key_formatter) {} + : keys_(keys.begin(), keys.end()), 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) << ", "; + 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(); - // remove final comma and space. - keys.pop_back(); - keys.pop_back(); - static std::string msg = + std::string msg = "An inference algorithm was called with inconsistent " "arguments. " "The\n" @@ -64,7 +69,8 @@ class InconsistentEliminationRequested : public std::exception { "that\n" "does not include all of the variables.\n"; msg += ("Leftover keys after elimination: " + keys); - return msg.c_str(); + // `new` to allocate memory on heap instead of stack + return (new std::string(msg))->c_str(); } }; } // namespace gtsam diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index b140c3d85..65d05b018 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -465,7 +465,31 @@ TEST(GaussianFactorGraph, InconsistentEliminationMessage) { "an ordering " "that\n" "does not include all of the variables.\n" - "Leftover keys after elimination: 2, x3"; + "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()); } }