diff --git a/gtsam/inference/IndexFactor.cpp b/gtsam/inference/IndexFactor.cpp index bd5052345..d427d0a95 100644 --- a/gtsam/inference/IndexFactor.cpp +++ b/gtsam/inference/IndexFactor.cpp @@ -16,6 +16,7 @@ * @created Oct 17, 2010 */ +#include #include #include #include @@ -31,9 +32,22 @@ IndexFactor::IndexFactor(const IndexConditional& c) : Base(static_cast::shared_ptr, IndexFactor::shared_ptr> IndexFactor::CombineAndEliminate( const FactorGraph& factors, size_t nrFrontals) { + + FastSet variables; + BOOST_FOREACH(const shared_ptr& factor, factors) { + BOOST_FOREACH(Index var, *factor) { + variables.insert(var); } } + + if(variables.size() < 1) + throw invalid_argument("IndexFactor::CombineAndEliminate called on factors with zero total variables."); + pair::shared_ptr, shared_ptr> result; - result.second = Combine(factors, VariableSlots(factors)); - result.first = result.second->eliminate(nrFrontals); + result.first.reset(new BayesNet()); + typename FastSet::const_iterator var; + for(var = variables.begin(); result.first->size() < nrFrontals; ++var) + result.first->push_back(IndexConditional::FromRange(var, variables.end(), 1)); + result.second.reset(new IndexFactor(var, variables.end())); + return result; }