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>
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;
}