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

restored lost changes

release/4.3a0
Michael Kaess 2010-01-18 04:53:17 +00:00
parent 42f4ff228b
commit 6b2190159d
1 changed files with 27 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
cpp/ISAM2-inl.h
View File

@ -23,7 +23,7 @@ namespace gtsam {
using namespace std; using namespace std;
// from inference-inl.h - need to additionally return the newly created factor for caching // from inference-inl.h - need to additionally return the newly created factor for caching
boost::shared_ptr<GaussianConditional> _eliminateOne(FactorGraph<GaussianFactor>& graph, cachedFactors& cached, const string& key) { boost::shared_ptr<GaussianConditional> _eliminateOne(FactorGraph<GaussianFactor>& graph, cachedFactors& cached, const Symbol& key) {
// combine the factors of all nodes connected to the variable to be eliminated // combine the factors of all nodes connected to the variable to be eliminated
// if no factors are connected to key, returns an empty factor // if no factors are connected to key, returns an empty factor
@ -47,7 +47,7 @@ namespace gtsam {
// from GaussianFactorGraph.cpp, see _eliminateOne above // from GaussianFactorGraph.cpp, see _eliminateOne above
GaussianBayesNet _eliminate(FactorGraph<GaussianFactor>& graph, cachedFactors& cached, const Ordering& ordering) { GaussianBayesNet _eliminate(FactorGraph<GaussianFactor>& graph, cachedFactors& cached, const Ordering& ordering) {
GaussianBayesNet chordalBayesNet; // empty GaussianBayesNet chordalBayesNet; // empty
BOOST_FOREACH(string key, ordering) { BOOST_FOREACH(const Symbol& key, ordering) {
GaussianConditional::shared_ptr cg = _eliminateOne(graph, cached, key); GaussianConditional::shared_ptr cg = _eliminateOne(graph, cached, key);
chordalBayesNet.push_back(cg); chordalBayesNet.push_back(cg);
} }
@ -67,7 +67,7 @@ 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), config_(config) { : BayesTree<Conditional>(nlfg.linearize(config).eliminate(ordering)), nonlinearFactors_(nlfg), linPoint_(config), estimate_(config) {
// 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);
} }
@ -76,36 +76,41 @@ 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<string>& affectedKeys) { FactorGraph<GaussianFactor> ISAM2<Conditional, Config>::relinearizeAffectedFactors(const list<Symbol>& affectedKeys) {
NonlinearFactorGraph<Config> nonlinearAffectedFactors; NonlinearFactorGraph<Config> nonlinearAffectedFactors;
typename FactorGraph<NonlinearFactor<Config> >::iterator it; typename FactorGraph<NonlinearFactor<Config> >::iterator it;
for(it = nonlinearFactors_.begin(); it != nonlinearFactors_.end(); it++) { for(it = nonlinearFactors_.begin(); it != nonlinearFactors_.end(); it++) {
bool inside = true; bool inside = true;
BOOST_FOREACH(string key, (*it)->keys()) { BOOST_FOREACH(const Symbol& key, (*it)->keys()) {
if (find(affectedKeys.begin(), affectedKeys.end(), key) == affectedKeys.end()) if (find(affectedKeys.begin(), affectedKeys.end(), key) == affectedKeys.end())
inside = false; inside = false;
} }
if (inside) if (inside)
nonlinearAffectedFactors.push_back(*it); nonlinearAffectedFactors.push_back(*it);
} }
return nonlinearAffectedFactors.linearize(config_);
return nonlinearAffectedFactors.linearize(linPoint_);
} }
/* ************************************************************************* */ /* ************************************************************************* */
// find intermediate (linearized) factors from cache that are passed into the affected area
template<class Conditional, class Config> template<class Conditional, class Config>
FactorGraph<GaussianFactor> ISAM2<Conditional, Config>::getCachedBoundaryFactors(Cliques& orphans) { FactorGraph<GaussianFactor> ISAM2<Conditional, Config>::getCachedBoundaryFactors(Cliques& orphans) {
// add intermediate (linearized) factors from cache that are passed into the affected area
FactorGraph<GaussianFactor> cachedBoundary; FactorGraph<GaussianFactor> cachedBoundary;
BOOST_FOREACH(sharedClique orphan, orphans) { BOOST_FOREACH(sharedClique orphan, orphans) {
// find the last variable that is not part of the separator // find the last variable that is not part of the separator
string oneTooFar = orphan->separator_.front(); string oneTooFar = orphan->separator_.front();
list<string> keys = orphan->keys(); list<Symbol> keys = orphan->keys();
list<string>::iterator it = find(keys.begin(), keys.end(), oneTooFar); list<Symbol>::iterator it = find(keys.begin(), keys.end(), oneTooFar);
it--; it--;
string key = *it; const Symbol& key = *it;
// retrieve the cached factor and add to boundary // retrieve the cached factor and add to boundary
cachedBoundary.push_back(cached[key]); cachedBoundary.push_back(cached[key]);
} }
return cachedBoundary; return cachedBoundary;
} }
@ -116,20 +121,21 @@ namespace gtsam {
// copy variables into config_, but don't overwrite existing entries (current linearization point!) // copy variables into config_, but don't overwrite existing entries (current linearization point!)
for (typename Config::const_iterator it = config.begin(); it!=config.end(); it++) { for (typename Config::const_iterator it = config.begin(); it!=config.end(); it++) {
if (!config_.contains(it->first)) { if (!linPoint_.contains(it->first)) {
config_.insert(it->first, it->second); linPoint_.insert(it->first, it->second);
estimate_.insert(it->first, it->second);
} }
} }
FactorGraph<GaussianFactor> newFactorsLinearized = newFactors.linearize(config_); FactorGraph<GaussianFactor> newFactorsLinearized = newFactors.linearize(linPoint_);
// Remove the contaminated part of the Bayes tree // Remove the contaminated part of the Bayes tree
FactorGraph<GaussianFactor> affectedFactors; FactorGraph<GaussianFactor> affectedFactors;
boost::tie(affectedFactors, orphans) = this->removeTop(newFactorsLinearized); boost::tie(affectedFactors, orphans) = this->removeTop(newFactorsLinearized);
// relinearize the affected factors ... // relinearize the affected factors ...
list<string> affectedKeys = affectedFactors.keys(); list<Symbol> affectedKeys = affectedFactors.keys();
FactorGraph<GaussianFactor> factors = relinearizeAffectedFactors(affectedKeys); FactorGraph<GaussianFactor> factors = relinearizeAffectedFactors(affectedKeys); // todo: searches through all factors, potentially expensive
// ... add the cached intermediate results from the boundary of the orphans ... // ... add the cached intermediate results from the boundary of the orphans ...
FactorGraph<GaussianFactor> cachedBoundary = getCachedBoundaryFactors(orphans); FactorGraph<GaussianFactor> cachedBoundary = getCachedBoundaryFactors(orphans);
@ -143,7 +149,7 @@ namespace gtsam {
if (true) { if (true) {
ordering = factors.getOrdering(); ordering = factors.getOrdering();
} else { } else {
list<string> keys = factors.keys(); list<Symbol> keys = factors.keys();
keys.sort(); // todo: correct sorting order? keys.sort(); // todo: correct sorting order?
ordering = keys; ordering = keys;
} }
@ -162,16 +168,17 @@ namespace gtsam {
// add orphans to the bottom of the new tree // add orphans to the bottom of the new tree
BOOST_FOREACH(sharedClique orphan, orphans) { BOOST_FOREACH(sharedClique orphan, orphans) {
string key = orphan->separator_.front(); Symbol key = orphan->separator_.front();
sharedClique parent = (*this)[key]; sharedClique parent = (*this)[key];
parent->children_ += orphan; parent->children_ += orphan;
orphan->parent_ = parent; // set new parent! orphan->parent_ = parent; // set new parent!
} }
// update solution // update solution - todo: potentially expensive
VectorConfig solution = optimize2(*this); VectorConfig delta = optimize2(*this);
solution.print(); // delta.print();
estimate_ += delta;
} }