12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

more compulsive checking, more efficient removing

release/4.3a0
Frank Dellaert 2010-02-26 05:29:45 +00:00
parent 73848d9e0e
commit 38ed1a5cdd
1 changed files with 14 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
cpp/FactorGraph-inl.h
View File

@ -234,13 +234,21 @@ list<int> FactorGraph<Factor>::factors(const Symbol& key) const {
template<class Factor> template<class Factor>
vector<boost::shared_ptr<Factor> > vector<boost::shared_ptr<Factor> >
FactorGraph<Factor>::findAndRemoveFactors(const Symbol& key) { FactorGraph<Factor>::findAndRemoveFactors(const Symbol& key) {
vector<sharedFactor> found;
const list<int>& indices = indices_.at(key); // find all factor indices associated with the key
BOOST_FOREACH(const int& i, indices) { Indices::const_iterator it = indices_.find(key);
if(factors_[i] == NULL) continue; // skip NULL factors if (it==indices_.end())
found.push_back(factors_[i]); // add to found throw std::invalid_argument(
remove(i); // set factor to NULL. "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; return found;
} }