From 38ed1a5cdd7a4810a7c8a20b5e1744e04d12f78d Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Fri, 26 Feb 2010 05:29:45 +0000
Subject: [PATCH] more compulsive checking, more efficient removing

---
 cpp/FactorGraph-inl.h | 20 ++++++++++++++------
 1 file changed, 14 insertions(+), 6 deletions(-)

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<int> FactorGraph<Factor>::factors(const Symbol& key) const {
 template<class Factor>
 vector<boost::shared_ptr<Factor> >
 FactorGraph<Factor>::findAndRemoveFactors(const Symbol& key) {
-	vector<sharedFactor> found;
 
-	const list<int>& 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<int>& factorsAssociatedWithKey = it->second;
+
+	vector<sharedFactor> 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;
 }