diff --git a/inference/FactorGraph-inl.h b/inference/FactorGraph-inl.h index 725d77413..56243a894 100644 --- a/inference/FactorGraph-inl.h +++ b/inference/FactorGraph-inl.h @@ -430,18 +430,16 @@ namespace gtsam { // 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 size_t& i, factorsAssociatedWithKey) { + if (it != indices_.end()) { + const list& factorsAssociatedWithKey = it->second; + BOOST_FOREACH(const size_t& 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) } - + } indices_.erase(key); return found;