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;
// 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
// if no factors are connected to key, returns an empty factor
@ -47,7 +47,7 @@ namespace gtsam {
// from GaussianFactorGraph.cpp, see _eliminateOne above
GaussianBayesNet _eliminate(FactorGraph<GaussianFactor>& graph, cachedFactors& cached, const Ordering& ordering) {
GaussianBayesNet chordalBayesNet; // empty
BOOST_FOREACH(string key, ordering) {
BOOST_FOREACH(const Symbol& key, ordering) {
GaussianConditional::shared_ptr cg = _eliminateOne(graph, cached, key);
chordalBayesNet.push_back(cg);
}
@ -67,7 +67,7 @@ namespace gtsam {
/** Create a Bayes Tree from a nonlinear factor graph */
template<class Conditional, class 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"
_eliminate_const(nlfg.linearize(config), cached, ordering);
}
@ -76,36 +76,41 @@ namespace gtsam {
// retrieve all factors that ONLY contain the affected variables
// (note that the remaining stuff is summarized in the cached factors)
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;
typename FactorGraph<NonlinearFactor<Config> >::iterator it;
for(it = nonlinearFactors_.begin(); it != nonlinearFactors_.end(); it++) {
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())
inside = false;
}
if (inside)
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>
FactorGraph<GaussianFactor> ISAM2<Conditional, Config>::getCachedBoundaryFactors(Cliques& orphans) {
// add intermediate (linearized) factors from cache that are passed into the affected area
FactorGraph<GaussianFactor> cachedBoundary;
BOOST_FOREACH(sharedClique orphan, orphans) {
// find the last variable that is not part of the separator
string oneTooFar = orphan->separator_.front();
list<string> keys = orphan->keys();
list<string>::iterator it = find(keys.begin(), keys.end(), oneTooFar);
list<Symbol> keys = orphan->keys();
list<Symbol>::iterator it = find(keys.begin(), keys.end(), oneTooFar);
it--;
string key = *it;
const Symbol& key = *it;
// retrieve the cached factor and add to boundary
cachedBoundary.push_back(cached[key]);
}
return cachedBoundary;
}
@ -116,20 +121,21 @@ namespace gtsam {
// 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++) {
if (!config_.contains(it->first)) {
config_.insert(it->first, it->second);
if (!linPoint_.contains(it->first)) {
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
FactorGraph<GaussianFactor> affectedFactors;
boost::tie(affectedFactors, orphans) = this->removeTop(newFactorsLinearized);
// relinearize the affected factors ...
list<string> affectedKeys = affectedFactors.keys();
FactorGraph<GaussianFactor> factors = relinearizeAffectedFactors(affectedKeys);
list<Symbol> affectedKeys = affectedFactors.keys();
FactorGraph<GaussianFactor> factors = relinearizeAffectedFactors(affectedKeys); // todo: searches through all factors, potentially expensive
// ... add the cached intermediate results from the boundary of the orphans ...
FactorGraph<GaussianFactor> cachedBoundary = getCachedBoundaryFactors(orphans);
@ -143,7 +149,7 @@ namespace gtsam {
if (true) {
ordering = factors.getOrdering();
} else {
list<string> keys = factors.keys();
list<Symbol> keys = factors.keys();
keys.sort(); // todo: correct sorting order?
ordering = keys;
}
@ -162,16 +168,17 @@ namespace gtsam {
// add orphans to the bottom of the new tree
BOOST_FOREACH(sharedClique orphan, orphans) {
string key = orphan->separator_.front();
Symbol key = orphan->separator_.front();
sharedClique parent = (*this)[key];
parent->children_ += orphan;
orphan->parent_ = parent; // set new parent!
}
// update solution
VectorConfig solution = optimize2(*this);
solution.print();
// update solution - todo: potentially expensive
VectorConfig delta = optimize2(*this);
// delta.print();
estimate_ += delta;
}