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

simplified alg, but no measureable speed improvement

release/4.3a0
Michael Kaess 2010-01-22 18:29:27 +00:00
parent 351cdd18c2
commit ab155999cf
2 changed files with 15 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
cpp/ISAM2-inl.h
View File

@ -74,8 +74,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class Conditional, class Config> template<class Conditional, class Config>
FactorGraph<NonlinearFactor<Config> > list<int> ISAM2<Conditional, Config>::getAffectedFactors(const list<Symbol>& keys) const {
ISAM2<Conditional, Config>::getAffectedFactors(const list<Symbol>& keys) const {
FactorGraph<NonlinearFactor<Config> > allAffected; FactorGraph<NonlinearFactor<Config> > allAffected;
list<int> indices; list<int> indices;
BOOST_FOREACH(const Symbol& key, keys) { BOOST_FOREACH(const Symbol& key, keys) {
@ -84,10 +83,7 @@ namespace gtsam {
} }
indices.sort(); indices.sort();
indices.unique(); indices.unique();
BOOST_FOREACH(int i, indices) { return indices;
allAffected.push_back(nonlinearFactors_[i]);
}
return allAffected;
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -98,21 +94,20 @@ namespace gtsam {
list<Symbol> affectedKeysList; // todo: shouldn't have to convert back to list... list<Symbol> affectedKeysList; // todo: shouldn't have to convert back to list...
affectedKeysList.insert(affectedKeysList.begin(), affectedKeys.begin(), affectedKeys.end()); affectedKeysList.insert(affectedKeysList.begin(), affectedKeys.begin(), affectedKeys.end());
FactorGraph<NonlinearFactor<Config> > candidates = getAffectedFactors(affectedKeysList); list<int> candidates = getAffectedFactors(affectedKeysList);
NonlinearFactorGraph<Config> nonlinearAffectedFactors; NonlinearFactorGraph<Config> nonlinearAffectedFactors;
typename FactorGraph<NonlinearFactor<Config> >::const_iterator it; BOOST_FOREACH(int idx, candidates) {
for(it = candidates.begin(); it != candidates.end(); it++) {
bool inside = true; bool inside = true;
BOOST_FOREACH(const Symbol& key, (*it)->keys()) { BOOST_FOREACH(const Symbol& key, nonlinearFactors_[idx]->keys()) {
if (affectedKeys.find(key) == affectedKeys.end()) { if (affectedKeys.find(key) == affectedKeys.end()) {
inside = false; inside = false;
break; break;
} }
} }
if (inside) if (inside)
nonlinearAffectedFactors.push_back(*it); nonlinearAffectedFactors.push_back(nonlinearFactors_[idx]);
} }
return nonlinearAffectedFactors.linearize(theta_); return nonlinearAffectedFactors.linearize(theta_);
@ -161,8 +156,14 @@ namespace gtsam {
// basically calculate all the keys contained in the factors that contain any of the keys... // basically calculate all the keys contained in the factors that contain any of the keys...
// the goal is to relinearize all variables directly affected by new factors // the goal is to relinearize all variables directly affected by new factors
FactorGraph<NonlinearFactor<Config> > allAffected = getAffectedFactors(marked_); list<int> allAffected = getAffectedFactors(marked_);
marked_ = allAffected.keys(); set<Symbol> accumulate;
BOOST_FOREACH(int idx, allAffected) {
list<Symbol> tmp = nonlinearFactors_[idx]->keys();
accumulate.insert(tmp.begin(), tmp.end());
}
marked_.clear();
marked_.insert(marked_.begin(), accumulate.begin(), accumulate.end());
// merge keys of new factors with mask // merge keys of new factors with mask
const list<Symbol> newKeys = newFactors.keys(); const list<Symbol> newKeys = newFactors.keys();

2
cpp/ISAM2.h
View File

@ -85,7 +85,7 @@ namespace gtsam {
private: private:
FactorGraph<NonlinearFactor<Config> > getAffectedFactors(const std::list<Symbol>& keys) const; std::list<int> getAffectedFactors(const std::list<Symbol>& keys) const;
FactorGraph<GaussianFactor> relinearizeAffectedFactors(const std::set<Symbol>& affectedKeys) const; FactorGraph<GaussianFactor> relinearizeAffectedFactors(const std::set<Symbol>& affectedKeys) const;
FactorGraph<GaussianFactor> getCachedBoundaryFactors(Cliques& orphans); FactorGraph<GaussianFactor> getCachedBoundaryFactors(Cliques& orphans);