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

changed main algorithm to allow recovery of exact solution

release/4.3a0
Michael Kaess 2010-01-22 06:28:12 +00:00
parent 75e29dc015
commit 1d093e388d
2 changed files with 60 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

79
cpp/ISAM2-inl.h
View File

@ -67,16 +67,16 @@ namespace gtsam {
/** Create a Bayes Tree from a nonlinear factor graph */ /** Create a Bayes Tree from a nonlinear factor graph */
template<class Conditional, class Config> template<class Conditional, class Config>
ISAM2<Conditional, Config>::ISAM2(const NonlinearFactorGraph<Config>& nlfg, const Ordering& ordering, const Config& config) ISAM2<Conditional, Config>::ISAM2(const NonlinearFactorGraph<Config>& nlfg, const Ordering& ordering, const Config& config)
: BayesTree<Conditional>(nlfg.linearize(config).eliminate(ordering)), nonlinearFactors_(nlfg), theta_(config) { : BayesTree<Conditional>(nlfg.linearize(config).eliminate(ordering)), theta_(config), thetaFuture_(config), nonlinearFactors_(nlfg) {
// todo: repeats calculation above, just to set "cached" // todo: repeats calculation above, just to set "cached"
_eliminate_const(nlfg.linearize(config), cached_, ordering); _eliminate_const(nlfg.linearize(config), cached_, ordering);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Conditional, class Config> template<class Conditional, class Config>
boost::shared_ptr<FactorGraph<NonlinearFactor<Config> > > FactorGraph<NonlinearFactor<Config> >
ISAM2<Conditional, Config>::getAffectedFactors(const list<Symbol>& keys) const { ISAM2<Conditional, Config>::getAffectedFactors(const list<Symbol>& keys) const {
boost::shared_ptr<FactorGraph<NonlinearFactor<Config> > > allAffected(new FactorGraph<NonlinearFactor<Config> >); FactorGraph<NonlinearFactor<Config> > allAffected;
list<int> indices; list<int> indices;
BOOST_FOREACH(const Symbol& key, keys) { BOOST_FOREACH(const Symbol& key, keys) {
const list<int> l = nonlinearFactors_.factors(key); const list<int> l = nonlinearFactors_.factors(key);
@ -85,7 +85,7 @@ namespace gtsam {
indices.sort(); indices.sort();
indices.unique(); indices.unique();
BOOST_FOREACH(int i, indices) { BOOST_FOREACH(int i, indices) {
allAffected->push_back(nonlinearFactors_[i]); allAffected.push_back(nonlinearFactors_[i]);
} }
return allAffected; return allAffected;
} }
@ -94,17 +94,19 @@ namespace gtsam {
// retrieve all factors that ONLY contain the affected variables // retrieve all factors that ONLY contain the affected variables
// (note that the remaining stuff is summarized in the cached factors) // (note that the remaining stuff is summarized in the cached factors)
template<class Conditional, class Config> template<class Conditional, class Config>
FactorGraph<GaussianFactor> ISAM2<Conditional, Config>::relinearizeAffectedFactors(const list<Symbol>& affectedKeys) const { FactorGraph<GaussianFactor> ISAM2<Conditional, Config>::relinearizeAffectedFactors(const set<Symbol>& affectedKeys) const {
boost::shared_ptr<FactorGraph<NonlinearFactor<Config> > > candidates = getAffectedFactors(affectedKeys); list<Symbol> affectedKeysList; // todo: shouldn't have to convert back to list...
affectedKeysList.insert(affectedKeysList.begin(), affectedKeys.begin(), affectedKeys.end());
FactorGraph<NonlinearFactor<Config> > candidates = getAffectedFactors(affectedKeysList);
NonlinearFactorGraph<Config> nonlinearAffectedFactors; NonlinearFactorGraph<Config> nonlinearAffectedFactors;
typename FactorGraph<NonlinearFactor<Config> >::const_iterator it; typename FactorGraph<NonlinearFactor<Config> >::const_iterator it;
for(it = candidates->begin(); it != candidates->end(); it++) { for(it = candidates.begin(); it != candidates.end(); it++) {
bool inside = true; bool inside = true;
BOOST_FOREACH(const Symbol& key, (*it)->keys()) { BOOST_FOREACH(const Symbol& key, (*it)->keys()) {
if (find(affectedKeys.begin(), affectedKeys.end(), key) == affectedKeys.end()) { if (affectedKeys.find(key) == affectedKeys.end()) {
inside = false; inside = false;
break; break;
} }
@ -135,26 +137,32 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class Conditional, class Config> template<class Conditional, class Config>
void ISAM2<Conditional, Config>::update_internal(const NonlinearFactorGraph<Config>& newFactors, void ISAM2<Conditional, Config>::update_internal(const NonlinearFactorGraph<Config>& newFactors,
const Config& theta_new, Cliques& orphans, double wildfire_threshold, double relinearize_threshold) { const Config& newTheta, Cliques& orphans, double wildfire_threshold, double relinearize_threshold) {
// marked_ = nonlinearFactors_.keys(); // debug only ////////////
// todo - debug only //// 1 - relinearize selected variables
// marked_ = nonlinearFactors_.keys();
theta_ = expmap(theta_, deltaMarked_);
//// 2 - Add new factors (for later relinearization)
//// 1 - Remember the new factors for later relinearization
nonlinearFactors_.push_back(newFactors); nonlinearFactors_.push_back(newFactors);
//// 2 - add in new information //// 3 - Initialize new variables
// add new variables
theta_.insert(theta_new); theta_.insert(newTheta);
thetaFuture_.insert(newTheta);
//// 4 - Mark affected variables as invalid
// todo - not in lyx yet: relin requires more than just removing the cliques corresponding to the variables!!! // todo - not in lyx yet: relin requires more than just removing the cliques corresponding to the variables!!!
// It's about factors!!! // It's about factors!!!
// 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
boost::shared_ptr<FactorGraph<NonlinearFactor<Config> > > allAffected = getAffectedFactors(marked_); FactorGraph<NonlinearFactor<Config> > allAffected = getAffectedFactors(marked_);
marked_ = allAffected->keys(); marked_ = allAffected.keys();
// merge keys of new factors with mask // merge keys of new factors with mask
const list<Symbol> newKeys = newFactors.keys(); const list<Symbol> newKeys = newFactors.keys();
@ -163,23 +171,27 @@ namespace gtsam {
marked_.sort(); marked_.sort();
marked_.unique(); marked_.unique();
//// 4 - removeTop invalidate all cliques involving marked variables //// 5 - removeTop invalidate all cliques involving marked variables
// remove affected factors // remove affected factors
BayesNet<GaussianConditional> affectedBayesNet; BayesNet<GaussianConditional> affectedBayesNet;
this->removeTop(marked_, affectedBayesNet, orphans); this->removeTop(marked_, affectedBayesNet, orphans);
//// 3 - find factors connected to affected variables //// 6 - find factors connected to affected variables
//// 4 - linearize //// 7 - linearize
// ordering provides all keys in conditionals, there cannot be others because path to root included // ordering provides all keys in conditionals, there cannot be others because path to root included
list<Symbol> affectedKeys = affectedBayesNet.ordering(); set<Symbol> affectedKeys;
list<Symbol> tmp = affectedBayesNet.ordering();
affectedKeys.insert(tmp.begin(), tmp.end());
// todo - remerge in keys of new factors // todo - remerge in keys of new factors
affectedKeys.insert(affectedKeys.begin(), newKeys.begin(), newKeys.end()); affectedKeys.insert(newKeys.begin(), newKeys.end());
#if 0 // no longer needed for set
// eliminate duplicates // eliminate duplicates
affectedKeys.sort(); affectedKeys.sort();
affectedKeys.unique(); affectedKeys.unique();
#endif
FactorGraph<GaussianFactor> factors = relinearizeAffectedFactors(affectedKeys); FactorGraph<GaussianFactor> factors = relinearizeAffectedFactors(affectedKeys);
@ -187,7 +199,7 @@ namespace gtsam {
FactorGraph<GaussianFactor> cachedBoundary = getCachedBoundaryFactors(orphans); FactorGraph<GaussianFactor> cachedBoundary = getCachedBoundaryFactors(orphans);
factors.push_back(cachedBoundary); factors.push_back(cachedBoundary);
//// 5 - eliminate and add orphans back in //// 8 - eliminate and add orphans back in
// create an ordering for the new and contaminated factors // create an ordering for the new and contaminated factors
Ordering ordering = factors.getOrdering(); Ordering ordering = factors.getOrdering();
@ -211,36 +223,35 @@ namespace gtsam {
orphan->parent_ = parent; // set new parent! orphan->parent_ = parent; // set new parent!
} }
//// 6 - update solution //// 9 - update solution
VectorConfig delta = optimize2(*this, wildfire_threshold); delta_ = optimize2(*this, wildfire_threshold);
//// 7 - mark variables, if significant change //// 10 - mark variables, if significant change
marked_.clear(); marked_.clear();
VectorConfig deltaMarked; deltaMarked_ = VectorConfig(); // clear
for (VectorConfig::const_iterator it = delta.begin(); it!=delta.end(); it++) { for (VectorConfig::const_iterator it = delta_.begin(); it!=delta_.end(); it++) {
Symbol key = it->first; Symbol key = it->first;
Vector v = it->second; Vector v = it->second;
if (max(abs(v)) >= relinearize_threshold) { if (max(abs(v)) >= relinearize_threshold) {
marked_.push_back(key); marked_.push_back(key);
deltaMarked.insert(key, v); deltaMarked_.insert(key, v);
} }
} }
//// 8 - relinearize selected variables // not part of the formal algorithm, but needed to allow initialization of new variables outside by the user
thetaFuture_ = expmap(thetaFuture_, deltaMarked_);
theta_ = expmap(theta_, deltaMarked);
} }
template<class Conditional, class Config> template<class Conditional, class Config>
void ISAM2<Conditional, Config>::update( void ISAM2<Conditional, Config>::update(
const NonlinearFactorGraph<Config>& newFactors, const Config& config, const NonlinearFactorGraph<Config>& newFactors, const Config& newTheta,
double wildfire_threshold, double relinearize_threshold) { double wildfire_threshold, double relinearize_threshold) {
Cliques orphans; Cliques orphans;
this->update_internal(newFactors, config, orphans, wildfire_threshold, relinearize_threshold); this->update_internal(newFactors, newTheta, orphans, wildfire_threshold, relinearize_threshold);
} }

20
cpp/ISAM2.h
View File

@ -34,6 +34,7 @@ namespace gtsam {
// current linearization point // current linearization point
Config theta_; Config theta_;
Config thetaFuture_; // lin point of next iteration
// for keeping all original nonlinear factors // for keeping all original nonlinear factors
NonlinearFactorGraph<Config> nonlinearFactors_; NonlinearFactorGraph<Config> nonlinearFactors_;
@ -41,6 +42,10 @@ namespace gtsam {
// cached intermediate results for restarting computation in the middle // cached intermediate results for restarting computation in the middle
CachedFactors cached_; CachedFactors cached_;
// the linear solution, an update to the estimate in theta
VectorConfig delta_;
VectorConfig deltaMarked_;
// variables that have been updated, requiring the corresponding factors to be relinearized // variables that have been updated, requiring the corresponding factors to be relinearized
std::list<Symbol> marked_; std::list<Symbol> marked_;
@ -64,19 +69,24 @@ namespace gtsam {
* ISAM2. (update_internal provides access to list of orphans for drawing purposes) * ISAM2. (update_internal provides access to list of orphans for drawing purposes)
*/ */
void update_internal(const NonlinearFactorGraph<Config>& newFactors, void update_internal(const NonlinearFactorGraph<Config>& newFactors,
const Config& config, Cliques& orphans, const Config& newTheta, Cliques& orphans,
double wildfire_threshold, double relinearize_threshold); double wildfire_threshold, double relinearize_threshold);
void update(const NonlinearFactorGraph<Config>& newFactors, const Config& config, 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.);
const Config estimate() const {return theta_;} // needed to create initial estimates (note that this will be the linearization point in the next step!)
const Config getLinearizationPoint() const {return thetaFuture_;}
// estimate based on incomplete delta (threshold!)
const Config calculateEstimate() const {return expmap(theta_, delta_);}
// estimate based on full delta (note that this is based on the actual current linearization point)
const Config calculateBestEstimate() const {return expmap(theta_, optimize2(*this, 0.));}
const std::list<Symbol>& getMarked() const { return marked_; } const std::list<Symbol>& getMarked() const { return marked_; }
private: private:
boost::shared_ptr<FactorGraph<NonlinearFactor<Config> > > getAffectedFactors(const std::list<Symbol>& keys) const; FactorGraph<NonlinearFactor<Config> > getAffectedFactors(const std::list<Symbol>& keys) const;
FactorGraph<GaussianFactor> relinearizeAffectedFactors(const std::list<Symbol>& affectedKeys) const; FactorGraph<GaussianFactor> relinearizeAffectedFactors(const std::set<Symbol>& affectedKeys) const;
FactorGraph<GaussianFactor> getCachedBoundaryFactors(Cliques& orphans); FactorGraph<GaussianFactor> getCachedBoundaryFactors(Cliques& orphans);
}; // ISAM2 }; // ISAM2