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

selective update of linearization point

release/4.3a0
Michael Kaess 2010-01-19 00:15:46 +00:00
parent 0e6607d160
commit a2bce15c4b
1 changed files with 41 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
cpp/ISAM2-inl.h
View File

@ -119,20 +119,48 @@ namespace gtsam {
void ISAM2<Conditional, Config>::update_internal(const NonlinearFactorGraph<Config>& newFactors, void ISAM2<Conditional, Config>::update_internal(const NonlinearFactorGraph<Config>& newFactors,
const Config& config, Cliques& orphans) { const Config& config, Cliques& orphans) {
// todo: updates all variables... needs to be synced with removeTop!!! // determine which variables to relinearize
linPoint_ = expmap(linPoint_, delta_);
// add new variables
linPoint_.insert(config);
FactorGraph<GaussianFactor> newFactorsLinearized = newFactors.linearize(linPoint_);
// Remove the contaminated part of the Bayes tree
FactorGraph<GaussianFactor> affectedFactors; FactorGraph<GaussianFactor> affectedFactors;
// list<Symbol> keysToBeRemoved = newFactorsLinearized.keys(); // todo list<Symbol> newFactorsKeys = newFactors.keys();
list<Symbol> keysToBeRemoved = nonlinearFactors_.keys();
// relinearize all keys that are in newFactors, and already exist (not new variables!)
list<Symbol> keysToRelinearize;
list<Symbol> oldKeys = nonlinearFactors_.keys();
BOOST_FOREACH(const Symbol& key, newFactorsKeys) {
if (find(oldKeys.begin(), oldKeys.end(), key)!=oldKeys.end())
keysToRelinearize.push_back(key);
}
#if 1
// 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
FactorGraph<NonlinearFactor<Config> > allAffected;
typename FactorGraph<NonlinearFactor<Config> >::iterator it;
for(it = nonlinearFactors_.begin(); it != nonlinearFactors_.end(); it++) {
bool affected = false;
BOOST_FOREACH(const Symbol& key, (*it)->keys()) {
if (find(newFactorsKeys.begin(), newFactorsKeys.end(), key) != newFactorsKeys.end())
affected = true;
}
if (affected)
allAffected.push_back(*it);
}
list<Symbol> keysToBeRemoved = allAffected.keys();
#else
list<Symbol> keysToBeRemoved = nonlinearFactors_.keys(); // todo/debug - relinearize all (a cumbersome way to do batch)
#endif
this->removeTop(keysToBeRemoved, affectedFactors, orphans); this->removeTop(keysToBeRemoved, affectedFactors, orphans);
// selectively update the linearization point
VectorConfig selected_delta;
BOOST_FOREACH(const Symbol& key, keysToRelinearize) {
if (delta_.contains(key)) // after constructor call, delta is empty...
selected_delta.insert(key, delta_[key]);
}
linPoint_ = expmap(linPoint_, selected_delta); // todo-debug only
// relinearize the affected factors ... // relinearize the affected factors ...
list<Symbol> affectedKeys = affectedFactors.keys(); list<Symbol> affectedKeys = affectedFactors.keys();
FactorGraph<GaussianFactor> factors = relinearizeAffectedFactors(affectedKeys); // todo: searches through all factors, potentially expensive FactorGraph<GaussianFactor> factors = relinearizeAffectedFactors(affectedKeys); // todo: searches through all factors, potentially expensive
@ -141,7 +169,10 @@ namespace gtsam {
FactorGraph<GaussianFactor> cachedBoundary = getCachedBoundaryFactors(orphans); FactorGraph<GaussianFactor> cachedBoundary = getCachedBoundaryFactors(orphans);
factors.push_back(cachedBoundary); factors.push_back(cachedBoundary);
// add new variables
linPoint_.insert(config);
// ... and finally add the new linearized factors themselves // ... and finally add the new linearized factors themselves
FactorGraph<GaussianFactor> newFactorsLinearized = newFactors.linearize(linPoint_);
factors.push_back(newFactorsLinearized); factors.push_back(newFactorsLinearized);
// create an ordering for the new and contaminated factors // create an ordering for the new and contaminated factors