Throw DisconnectedGraphException when trying to eliminate a disconnected graph (previously failed with a cryptic error)

gtsam/inference/EliminationTree-inl.h

@@ -114,10 +114,12 @@ typename EliminationTree<FACTOR>::shared_ptr EliminationTree<FACTOR>::Create(
   tic(2, "assemble tree");
   vector<shared_ptr> trees(n);
   for (Index k = 1; k <= n; k++) {
-    Index j = n - k;
-    trees[j].reset(new EliminationTree(j));
-    if (parents[j] != none)
+    Index j = n - k;  // Start at the last variable and loop down to 0
+    trees[j].reset(new EliminationTree(j));  // Create a new node on this variable
+    if (parents[j] != none)  // If this node has a parent, add it to the parent's children
       trees[parents[j]]->add(trees[j]);
+    else if(!structure[j].empty() && j != n - 1) // If a node other than the last has no parents, this is a forest
+      throw DisconnectedGraphException();
   }
   toc(2, "assemble tree");
 
@@ -134,12 +136,6 @@ typename EliminationTree<FACTOR>::shared_ptr EliminationTree<FACTOR>::Create(
   }
   toc(3, "hang factors");
 
-  // Assert that all other nodes have parents, i.e. that this is not a forest.
-#ifndef NDEBUG
-  for(typename vector<shared_ptr>::const_iterator tree=trees.begin(); tree!=trees.end()-1; ++tree)
-    assert((*tree) != shared_ptr());
-#endif
-
   if(debug)
     trees.back()->print("ETree: ");
 

gtsam/inference/EliminationTree.h

@@ -117,4 +117,23 @@ public:
   typename BayesNet::shared_ptr eliminate(Eliminate function) const;
 };
 
+
+/**
+ * An exception thrown when attempting to eliminate a disconnected factor
+ * graph, which is not currently possible in gtsam.  If you need to work with
+ * disconnected graphs, a workaround is to create zero-information factors to
+ * bridge the disconnects.  To do this, create any factor type (e.g.
+ * BetweenFactor or RangeFactor) with the noise model
+ * <tt>\ref sharedPrecision(dim, 0.0)</tt>, where \c dim is the appropriate
+ * dimensionality for that factor.
+ */
+struct DisconnectedGraphException : public std::exception {
+  DisconnectedGraphException() {}
+  virtual ~DisconnectedGraphException() throw() {}
+
+  /// Returns the string "Attempting to eliminate a disconnected graph - this is not currently possible in gtsam."
+  virtual const char* what() const throw() {
+    return "Attempting to eliminate a disconnected graph - this is not currently possible in gtsam."; }
+};
+
 }

gtsam/inference/tests/testEliminationTree.cpp

@@ -106,6 +106,17 @@ TEST(EliminationTree, eliminate )
   CHECK(assert_equal(expected,actual));
 }
 
+/* ************************************************************************* */
+TEST(EliminationTree, disconnected_graph) {
+  SymbolicFactorGraph fg;
+  fg.push_factor(0, 1);
+  fg.push_factor(0, 2);
+  fg.push_factor(1, 2);
+  fg.push_factor(3, 4);
+
+  CHECK_EXCEPTION(SymbolicEliminationTree::Create(fg), DisconnectedGraphException);
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;