Merge pull request #1578 from borglab/elimination-debugging

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;
     }

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 <gtsam/inference/inferenceExceptions.h>
+
+#include <sstream>
+
+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

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 <gtsam/global_includes.h>
+#include <gtsam/inference/Key.h>
+
 #include <exception>
 
 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 {
+/** 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() 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.";
-    }
-  };
 
-}
+  InconsistentEliminationRequested(
+      const KeySet& keys,
+      const KeyFormatter& key_formatter = DefaultKeyFormatter);
+
+  ~InconsistentEliminationRequested() noexcept override {}
+
+  const char* what() const noexcept override;
+};
+}  // namespace gtsam

gtsam/linear/tests/testGaussianFactorGraph.cpp

@@ -21,6 +21,7 @@
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/GaussianConditional.h>
 #include <gtsam/linear/GaussianBayesNet.h>
+#include <gtsam/inference/Symbol.h>
 #include <gtsam/inference/VariableSlots.h>
 #include <gtsam/inference/VariableIndex.h>
 #include <gtsam/base/debug.h>
@@ -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<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());
+  }
+
+  // Test large number of keys
+  fg = GaussianFactorGraph();
+  for (size_t i = 0; i < 1000; i++) {
+    fg.emplace_shared<JacobianFactor>(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;