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

36
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
if (relinFromLast) {
theta_ = expmap(theta_, deltaMarked_);
}
//// 2 - Add new factors (for later relinearization)
@ -154,6 +160,8 @@ namespace gtsam {
// todo - not in lyx yet: relin requires more than just removing the cliques corresponding to the variables!!!
// It's about factors!!!
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_);
@ -164,8 +172,9 @@ namespace gtsam {
}
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,6 +190,9 @@ namespace gtsam {
//// 6 - find factors connected to affected variables
//// 7 - linearize
FactorGraph<GaussianFactor> factors;
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();
@ -188,17 +200,18 @@ namespace gtsam {
// 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
FactorGraph<GaussianFactor> factors = relinearizeAffectedFactors(affectedKeys);
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,6 +245,8 @@ namespace gtsam {
marked_.clear();
deltaMarked_ = VectorConfig(); // clear
if (relinearize) { // decides about next step!!!
for (VectorConfig::const_iterator it = delta_.begin(); it!=delta_.end(); it++) {
Symbol key = it->first;
Vector v = it->second;
@ -243,16 +258,17 @@ namespace gtsam {
// 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_;}