diff --git a/cpp/FactorGraph-inl.h b/cpp/FactorGraph-inl.h index 6ba612d0e..f58b7f981 100644 --- a/cpp/FactorGraph-inl.h +++ b/cpp/FactorGraph-inl.h @@ -234,13 +234,21 @@ list FactorGraph::factors(const Symbol& key) const { template vector > FactorGraph::findAndRemoveFactors(const Symbol& key) { - vector found; - const list& indices = indices_.at(key); - BOOST_FOREACH(const int& i, indices) { - if(factors_[i] == NULL) continue; // skip NULL factors - found.push_back(factors_[i]); // add to found - remove(i); // set factor to NULL. + // find all factor indices associated with the key + Indices::const_iterator it = indices_.find(key); + if (it==indices_.end()) + throw std::invalid_argument( + "FactorGraph::findAndRemoveFactors: key " + + (string)key + " not found"); + const list& factorsAssociatedWithKey = it->second; + + vector found; + BOOST_FOREACH(const int& i, factorsAssociatedWithKey) { + sharedFactor& fi = factors_.at(i); // throws exception ! + if(fi == NULL) continue; // skip NULL factors + found.push_back(fi); // add to found + fi.reset(); // set factor to NULL == remove(i) } return found; }