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

allow pure linear steps (disabled)

release/4.3a0
Michael Kaess 2010-01-23 00:21:34 +00:00
parent 80d335ed77
commit 7bc4ee65da
2 changed files with 58 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

96
cpp/ISAM2-inl.h
View File

@ -132,13 +132,19 @@ namespace gtsam {
/* ************************************************************************* */
template<class Conditional, class Config>
void ISAM2<Conditional, Config>::update_internal(const NonlinearFactorGraph<Config>& newFactors,
const Config& newTheta, Cliques& orphans, double wildfire_threshold, double relinearize_threshold) {
const Config& newTheta, Cliques& orphans, double wildfire_threshold, double relinearize_threshold, bool relinearize) {
// marked_ = nonlinearFactors_.keys(); // debug only ////////////
// only relinearize if requested in previous step AND necessary (ie. at least one variable changes)
relinearize = true; // todo - switched off
bool relinFromLast = true; //marked_.size() > 0;
//// 1 - relinearize selected variables
theta_ = expmap(theta_, deltaMarked_);
if (relinFromLast) {
theta_ = expmap(theta_, deltaMarked_);
}
//// 2 - Add new factors (for later relinearization)
@ -154,18 +160,21 @@ namespace gtsam {
// todo - not in lyx yet: relin requires more than just removing the cliques corresponding to the variables!!!
// It's about factors!!!
// 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
list<int> allAffected = getAffectedFactors(marked_);
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());
if (relinFromLast) {
// mark variables that have to be removed as invalid (removeFATtop)
// 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
list<int> allAffected = getAffectedFactors(marked_);
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());
} // else: marked_ is empty anyways
// merge keys of new factors with mask
// also mark variables that are affected by new factors as invalid
const list<Symbol> newKeys = newFactors.keys();
marked_.insert(marked_.begin(), newKeys.begin(), newKeys.end());
// eliminate duplicates
@ -181,24 +190,28 @@ namespace gtsam {
//// 6 - find factors connected to affected variables
//// 7 - linearize
// ordering provides all keys in conditionals, there cannot be others because path to root included
set<Symbol> affectedKeys;
list<Symbol> tmp = affectedBayesNet.ordering();
affectedKeys.insert(tmp.begin(), tmp.end());
FactorGraph<GaussianFactor> factors;
// todo - remerge in keys of new factors
affectedKeys.insert(newKeys.begin(), newKeys.end());
#if 0 // no longer needed for set
// eliminate duplicates
affectedKeys.sort();
affectedKeys.unique();
#endif
if (relinFromLast) {
// ordering provides all keys in conditionals, there cannot be others because path to root included
set<Symbol> affectedKeys;
list<Symbol> tmp = affectedBayesNet.ordering();
affectedKeys.insert(tmp.begin(), tmp.end());
FactorGraph<GaussianFactor> factors = relinearizeAffectedFactors(affectedKeys);
// todo - remerge in keys of new factors
affectedKeys.insert(newKeys.begin(), newKeys.end());
// add the cached intermediate results from the boundary of the orphans ...
FactorGraph<GaussianFactor> cachedBoundary = getCachedBoundaryFactors(orphans);
factors.push_back(cachedBoundary);
factors = relinearizeAffectedFactors(affectedKeys);
// add the cached intermediate results from the boundary of the orphans ...
FactorGraph<GaussianFactor> cachedBoundary = getCachedBoundaryFactors(orphans);
factors.push_back(cachedBoundary);
} else {
// reuse the old factors
FactorGraph<GaussianFactor> tmp(affectedBayesNet);
factors.push_back(tmp);
factors.push_back(newFactors.linearize(theta_));
}
//// 8 - eliminate and add orphans back in
@ -232,27 +245,30 @@ namespace gtsam {
marked_.clear();
deltaMarked_ = VectorConfig(); // clear
for (VectorConfig::const_iterator it = delta_.begin(); it!=delta_.end(); it++) {
Symbol key = it->first;
Vector v = it->second;
if (max(abs(v)) >= relinearize_threshold) {
marked_.push_back(key);
deltaMarked_.insert(key, v);
}
}
if (relinearize) { // decides about next step!!!
// not part of the formal algorithm, but needed to allow initialization of new variables outside by the user
thetaFuture_ = expmap(thetaFuture_, deltaMarked_);
for (VectorConfig::const_iterator it = delta_.begin(); it!=delta_.end(); it++) {
Symbol key = it->first;
Vector v = it->second;
if (max(abs(v)) >= relinearize_threshold) {
marked_.push_back(key);
deltaMarked_.insert(key, v);
}
}
// not part of the formal algorithm, but needed to allow initialization of new variables outside by the user
thetaFuture_ = expmap(thetaFuture_, deltaMarked_);
}
}
template<class Conditional, class Config>
void ISAM2<Conditional, Config>::update(
const NonlinearFactorGraph<Config>& newFactors, const Config& newTheta,
double wildfire_threshold, double relinearize_threshold) {
double wildfire_threshold, double relinearize_threshold, bool relinearize) {
Cliques orphans;
this->update_internal(newFactors, newTheta, orphans, wildfire_threshold, relinearize_threshold);
this->update_internal(newFactors, newTheta, orphans, wildfire_threshold, relinearize_threshold, relinearize);
}

4
cpp/ISAM2.h
View File

@ -70,9 +70,9 @@ namespace gtsam {
*/
void update_internal(const NonlinearFactorGraph<Config>& newFactors,
const Config& newTheta, Cliques& orphans,
double wildfire_threshold, double relinearize_threshold);
double wildfire_threshold, double relinearize_threshold, bool relinearize);
void update(const NonlinearFactorGraph<Config>& newFactors, const Config& newTheta,
double wildfire_threshold = 0., double relinearize_threshold = 0.);
double wildfire_threshold = 0., double relinearize_threshold = 0., bool relinearize = true);
// needed to create initial estimates (note that this will be the linearization point in the next step!)
const Config getLinearizationPoint() const {return thetaFuture_;}