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

selective incremental linearization works; bug fix in landmark initialization (frank01 again much faster)

release/4.3a0
Michael Kaess 2010-01-19 01:18:28 +00:00
parent a2bce15c4b
commit a45893a434
1 changed files with 7 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
cpp/ISAM2-inl.h
View File

@ -124,6 +124,8 @@ namespace gtsam {
FactorGraph<GaussianFactor> affectedFactors; FactorGraph<GaussianFactor> affectedFactors;
list<Symbol> newFactorsKeys = newFactors.keys(); list<Symbol> newFactorsKeys = newFactors.keys();
#if 1
// relinearize all keys that are in newFactors, and already exist (not new variables!) // relinearize all keys that are in newFactors, and already exist (not new variables!)
list<Symbol> keysToRelinearize; list<Symbol> keysToRelinearize;
list<Symbol> oldKeys = nonlinearFactors_.keys(); list<Symbol> oldKeys = nonlinearFactors_.keys();
@ -132,7 +134,6 @@ namespace gtsam {
keysToRelinearize.push_back(key); keysToRelinearize.push_back(key);
} }
#if 1
// 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; FactorGraph<NonlinearFactor<Config> > allAffected;
@ -147,10 +148,14 @@ namespace gtsam {
allAffected.push_back(*it); allAffected.push_back(*it);
} }
list<Symbol> keysToBeRemoved = allAffected.keys(); list<Symbol> keysToBeRemoved = allAffected.keys();
#else #else
list<Symbol> keysToBeRemoved = nonlinearFactors_.keys(); // todo/debug - relinearize all (a cumbersome way to do batch) // debug only: full relinearization in each step
list<Symbol> keysToRelinearize = nonlinearFactors_.keys();
list<Symbol> keysToBeRemoved = nonlinearFactors_.keys();
#endif #endif
// remove affected factors
this->removeTop(keysToBeRemoved, affectedFactors, orphans); this->removeTop(keysToBeRemoved, affectedFactors, orphans);
// selectively update the linearization point // selectively update the linearization point