update InconsistentEliminationRequested to show leftover keys

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())
+      if(!factorGraph->empty()) {
-        throw InconsistentEliminationRequested();
+        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())
+      if(!factorGraph->empty()) {
-        throw InconsistentEliminationRequested();
+        throw InconsistentEliminationRequested(factorGraph->keys());
+      }
       // Return the Bayes tree
       return bayesTree;
     }

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 <gtsam/global_includes.h>
+
 #include <exception>
+#include <sstream>
 
 namespace gtsam {
 
-  /** An inference algorithm was called with inconsistent arguments.  The factor graph, ordering, or
+/** An inference algorithm was called with inconsistent arguments.  The factor
-   *  variable index were inconsistent with each other, or a full elimination routine was called
+ * graph, ordering, or variable index were inconsistent with each other, or a
-   *  with an ordering that does not include all of the variables. */
+ * full elimination routine was called with an ordering that does not include
-  class InconsistentEliminationRequested : public std::exception {
+ * 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 {
-      return
+    // Format keys for printing
-        "An inference algorithm was called with inconsistent arguments.  The\n"
+    std::stringstream sstr;
-        "factor graph, ordering, or variable index were inconsistent with each\n"
+    for (auto key : keys_) {
-        "other, or a full elimination routine was called with an ordering that\n"
+      sstr << keyFormatter(key) << ", ";
-        "does not include all of the variables.";
     }
-  };
+    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

gtsam/linear/tests/testGaussianFactorGraph.cpp

@@ -18,16 +18,16 @@
  *  @author Richard Roberts
  **/
 
-#include <gtsam/linear/GaussianFactorGraph.h>
-#include <gtsam/linear/GaussianConditional.h>
-#include <gtsam/linear/GaussianBayesNet.h>
-#include <gtsam/inference/VariableSlots.h>
-#include <gtsam/inference/VariableIndex.h>
-#include <gtsam/base/debug.h>
-#include <gtsam/base/VerticalBlockMatrix.h>
-
-#include <gtsam/base/TestableAssertions.h>
 #include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/TestableAssertions.h>
+#include <gtsam/base/VerticalBlockMatrix.h>
+#include <gtsam/base/debug.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/inference/VariableIndex.h>
+#include <gtsam/inference/VariableSlots.h>
+#include <gtsam/linear/GaussianBayesNet.h>
+#include <gtsam/linear/GaussianConditional.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
 
 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<JacobianFactor>(0, 10 * I_2x2, -1.0 * Vector::Ones(2),
+                                    unit2);
+  fg.emplace_shared<JacobianFactor>(0, -10 * I_2x2, 1, 10 * I_2x2,
+                                    Vector2(2.0, -1.0), unit2);
+  fg.emplace_shared<JacobianFactor>(1, -5 * I_2x2, 2, 5 * I_2x2,
+                                    Vector2(-1.0, 1.5), unit2);
+  fg.emplace_shared<JacobianFactor>(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;