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

new fluid relinearization algorithm, in sync with lyx

release/4.3a0
Michael Kaess 2010-01-21 07:38:37 +00:00
parent 9db7623f80
commit 8fd0c2ae72
6 changed files with 108 additions and 98 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
cpp/GaussianISAM2.cpp
View File

@ -18,47 +18,38 @@ using namespace gtsam;
namespace gtsam {
/* ************************************************************************* */
void optimize2(const GaussianISAM2::sharedClique& clique, VectorConfig& result) {
void optimize2(const GaussianISAM2::sharedClique& clique, double threshold, VectorConfig& result) {
bool process_children = false;
// parents are assumed to already be solved and available in result
GaussianISAM2::Clique::const_reverse_iterator it;
for (it = clique->rbegin(); it!=clique->rend(); it++) {
GaussianConditional::shared_ptr cg = *it;
Vector x = cg->solve(result); // Solve for that variable
if (max(abs(x)) >= threshold) {
process_children = true;
}
result.insert(cg->key(), x); // store result in partial solution
}
BOOST_FOREACH(GaussianISAM2::sharedClique child, clique->children_) {
optimize2(child, result);
if (process_children) {
BOOST_FOREACH(GaussianISAM2::sharedClique child, clique->children_) {
optimize2(child, threshold, result);
}
}
}
/* ************************************************************************* */
VectorConfig optimize2(const GaussianISAM2& bayesTree) {
VectorConfig optimize2(const GaussianISAM2& bayesTree, double threshold) {
VectorConfig result;
// starting from the root, call optimize on each conditional
optimize2(bayesTree.root(), result);
optimize2(bayesTree.root(), threshold, result);
return result;
}
#if 0
/* ************************************************************************* */
void optimize2(const GaussianISAM2_P::sharedClique& clique, VectorConfig& result) {
// parents are assumed to already be solved and available in result
GaussianISAM2_P::Clique::const_reverse_iterator it;
for (it = clique->rbegin(); it!=clique->rend(); it++) {
GaussianConditional::shared_ptr cg = *it;
result.insert(cg->key(), cg->solve(result)); // store result in partial solution
}
BOOST_FOREACH(GaussianISAM2_P::sharedClique child, clique->children_) {
optimize2(child, result);
}
}
#endif
/* ************************************************************************* */
VectorConfig optimize2(const GaussianISAM2_P& bayesTree) {
VectorConfig optimize2(const GaussianISAM2_P& bayesTree, double threshold) {
VectorConfig result;
// starting from the root, call optimize on each conditional
optimize2(bayesTree.root(), result);
optimize2(bayesTree.root(), threshold, result);
return result;
}

6
cpp/GaussianISAM2.h
View File

@ -19,10 +19,10 @@ namespace gtsam {
typedef ISAM2<GaussianConditional, simulated2D::Config> GaussianISAM2;
// recursively optimize this conditional and all subtrees
void optimize2(const GaussianISAM2::sharedClique& clique, VectorConfig& result);
void optimize2(const GaussianISAM2::sharedClique& clique, double threshold, VectorConfig& result);
// optimize the BayesTree, starting from the root
VectorConfig optimize2(const GaussianISAM2& bayesTree);
VectorConfig optimize2(const GaussianISAM2& bayesTree, double threshold = 0.);
// todo: copy'n'paste to avoid template hell
@ -33,6 +33,6 @@ namespace gtsam {
// void optimize2(const GaussianISAM2_P::sharedClique& clique, VectorConfig& result);
// optimize the BayesTree, starting from the root
VectorConfig optimize2(const GaussianISAM2_P& bayesTree);
VectorConfig optimize2(const GaussianISAM2_P& bayesTree, double threshold = 0.);
}/// namespace gtsam

135
cpp/ISAM2-inl.h
View File

@ -113,7 +113,7 @@ namespace gtsam {
nonlinearAffectedFactors.push_back(*it);
}
return nonlinearAffectedFactors.linearize(linPoint_); // todo: use shared_ptr to avoid copying?
return nonlinearAffectedFactors.linearize(linPoint_);
}
/* ************************************************************************* */
@ -129,78 +129,61 @@ namespace gtsam {
cachedBoundary.push_back(cached_[key]);
}
return cachedBoundary; // todo: use shared_ptr to avoid copying?
return cachedBoundary;
}
/* ************************************************************************* */
template<class Conditional, class Config>
void ISAM2<Conditional, Config>::update_internal(const NonlinearFactorGraph<Config>& newFactors,
const Config& config, Cliques& orphans) {
const Config& config, Cliques& orphans, double wildfire_threshold, double relinearize_threshold) {
list<Symbol> newFactorsKeys = newFactors.keys();
#if 1 // 0=relinearize all in each step
// relinearize all keys that are in newFactors, and already exist (not new variables!)
list<Symbol> keysToRelinearize;
BOOST_FOREACH(const Symbol& key, newFactorsKeys) {
if (nonlinearFactors_.involves(key))
keysToRelinearize.push_back(key);
}
// 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
boost::shared_ptr<FactorGraph<NonlinearFactor<Config> > > allAffected = getAffectedFactors(keysToRelinearize);
list<Symbol> keysToBeRemoved = allAffected->keys();
#else
// debug only: full relinearization in each step
list<Symbol> keysToRelinearize = nonlinearFactors_.keys();
list<Symbol> keysToBeRemoved = nonlinearFactors_.keys();
#endif
// remove affected factors
BayesNet<GaussianConditional> affectedBayesNet;
this->removeTop(keysToBeRemoved, affectedBayesNet, 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);
// relinearize the affected factors ...
list<Symbol> affectedKeys = affectedBayesNet.ordering(); // all keys in conditionals, there cannot be others because path to root included
FactorGraph<GaussianFactor> factors = relinearizeAffectedFactors(affectedKeys);
// ... add the cached intermediate results from the boundary of the orphans ...
FactorGraph<GaussianFactor> cachedBoundary = getCachedBoundaryFactors(orphans);
factors.push_back(cachedBoundary);
//// 1 - add in new information
// add new variables
linPoint_.insert(config);
// ... and finally add the new linearized factors themselves
FactorGraph<GaussianFactor> newFactorsLinearized = newFactors.linearize(linPoint_);
factors.push_back(newFactorsLinearized);
// create an ordering for the new and contaminated factors
Ordering ordering;
if (true) {
ordering = factors.getOrdering();
} else {
list<Symbol> keys = factors.keys();
keys.sort(); // todo: correct sorting order?
ordering = keys;
}
// eliminate into a Bayes net
BayesNet<Conditional> bayesNet = _eliminate(factors, cached_, ordering);
// remember the new factors for later relinearization
nonlinearFactors_.push_back(newFactors);
// merge keys of new factors with mask
const list<Symbol> newKeys = newFactors.keys();
marked_.insert(marked_.begin(), newKeys.begin(), newKeys.end());
// eliminate duplicates
marked_.sort();
marked_.unique();
//// 2 - invalidate all cliques involving marked variables
// remove affected factors
BayesNet<GaussianConditional> affectedBayesNet;
this->removeTop(marked_, affectedBayesNet, orphans);
//// 3 - find factors connected to marked variables
//// 4 - linearize
// ordering provides all keys in conditionals, there cannot be others because path to root included
list<Symbol> affectedKeys = affectedBayesNet.ordering();
// todo - remerge in keys of new factors
affectedKeys.insert(affectedKeys.begin(), newKeys.begin(), newKeys.end());
// eliminate duplicates
affectedKeys.sort();
affectedKeys.unique();
FactorGraph<GaussianFactor> factors = relinearizeAffectedFactors(affectedKeys);
// add the cached intermediate results from the boundary of the orphans ...
FactorGraph<GaussianFactor> cachedBoundary = getCachedBoundaryFactors(orphans);
factors.push_back(cachedBoundary);
//// 5 - eliminate and add orphans back in
// create an ordering for the new and contaminated factors
Ordering ordering = factors.getOrdering();
// eliminate into a Bayes net
BayesNet<Conditional> bayesNet = _eliminate(factors, cached_, ordering);
// insert conditionals back in, straight into the topless bayesTree
typename BayesNet<Conditional>::const_reverse_iterator rit;
for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit ) {
@ -209,23 +192,43 @@ namespace gtsam {
// add orphans to the bottom of the new tree
BOOST_FOREACH(sharedClique orphan, orphans) {
Symbol key = findParentClique(orphan->separator_, ordering);
sharedClique parent = (*this)[key];
parent->children_ += orphan;
orphan->parent_ = parent; // set new parent!
}
// update solution - todo: potentially expensive
delta_ = optimize2(*this);
//// 6 - update solution
VectorConfig delta = optimize2(*this, wildfire_threshold);
//// 7 - mark variables, if significant change
marked_.clear();
VectorConfig 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);
}
}
//// 8 - relinearize selected variables
linPoint_ = expmap(linPoint_, deltaMarked);
}
template<class Conditional, class Config>
void ISAM2<Conditional, Config>::update(const NonlinearFactorGraph<Config>& newFactors, const Config& config) {
void ISAM2<Conditional, Config>::update(
const NonlinearFactorGraph<Config>& newFactors, const Config& config,
double wildfire_threshold, double relinearize_threshold) {
Cliques orphans;
this->update_internal(newFactors, config, orphans);
this->update_internal(newFactors, config, orphans, wildfire_threshold, relinearize_threshold);
}
/* ************************************************************************* */

16
cpp/ISAM2.h
View File

@ -34,15 +34,15 @@ namespace gtsam {
// current linearization point
Config linPoint_;
// most recent solution
VectorConfig delta_;
// for keeping all original nonlinear factors
NonlinearFactorGraph<Config> nonlinearFactors_;
// cached intermediate results for restarting computation in the middle
CachedFactors cached_;
// variables that have been updated, requiring the corresponding factors to be relinearized
std::list<Symbol> marked_;
public:
/** Create an empty Bayes Tree */
@ -62,11 +62,13 @@ namespace gtsam {
/**
* ISAM2. (update_internal provides access to list of orphans for drawing purposes)
*/
void update_internal(const NonlinearFactorGraph<Config>& newFactors, const Config& config, Cliques& orphans);
void update(const NonlinearFactorGraph<Config>& newFactors, const Config& config);
void update_internal(const NonlinearFactorGraph<Config>& newFactors,
const Config& config, Cliques& orphans,
double wildfire_threshold, double relinearize_threshold);
void update(const NonlinearFactorGraph<Config>& newFactors, const Config& config,
double wildfire_threshold = 0., double relinearize_threshold = 0.);
const Config estimate() const {return expmap(linPoint_, delta_);}
const Config linearizationPoint() const {return linPoint_;}
const Config estimate() const {return linPoint_;}
private:

7
cpp/Vector.cpp
View File

@ -211,6 +211,13 @@ namespace gtsam {
return s;
}
/* ************************************************************************* */
Vector abs(const Vector& v) {
Vector s(v.size());
transform(v.begin(), v.end(), s.begin(), ::fabs);
return s;
}
/* ************************************************************************* */
double max(const Vector &a) {
return *(std::max_element(a.begin(), a.end()));

7
cpp/Vector.h
View File

@ -184,6 +184,13 @@ Vector reciprocal(const Vector &a);
*/
Vector esqrt(const Vector& v);
/**
* absolut values of vector elements
* @param a vector
* @return [abs(a(i))]
*/
Vector abs(const Vector& v);
/**
* return the max element of a vector
* @param a vector