diff --git a/gtsam/inference/EliminationTree-inl.h b/gtsam/inference/EliminationTree-inl.h index dddbe8e7e..13b305457 100644 --- a/gtsam/inference/EliminationTree-inl.h +++ b/gtsam/inference/EliminationTree-inl.h @@ -114,10 +114,12 @@ typename EliminationTree::shared_ptr EliminationTree::Create( tic(2, "assemble tree"); vector 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::shared_ptr EliminationTree::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::const_iterator tree=trees.begin(); tree!=trees.end()-1; ++tree) - assert((*tree) != shared_ptr()); -#endif - if(debug) trees.back()->print("ETree: "); diff --git a/gtsam/inference/EliminationTree.h b/gtsam/inference/EliminationTree.h index 82480f99e..e6efb1cde 100644 --- a/gtsam/inference/EliminationTree.h +++ b/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 + * \ref sharedPrecision(dim, 0.0), 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."; } +}; + } diff --git a/gtsam/inference/tests/testEliminationTree.cpp b/gtsam/inference/tests/testEliminationTree.cpp index 8c59eb2e5..4d50e7583 100644 --- a/gtsam/inference/tests/testEliminationTree.cpp +++ b/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;