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

relinearizing part of a BayesTree, requiring caching of intermediate results during elimination

release/4.3a0
Michael Kaess 2010-01-17 06:06:20 +00:00
parent a3fa194ca1
commit fbe425b966
4 changed files with 132 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
cpp/BayesTree-inl.h
View File

@ -235,12 +235,12 @@ namespace gtsam {
/* ************************************************************************* */
template<class Conditional>
void BayesTree<Conditional>::print(const string& s) const {
cout << s << ": size == " << size() << endl;
if (nodes_.empty()) return;
if (root_.use_count() == 0) {
printf("WARNING: Forest...\n");
printf("WARNING: BayesTree.print encountered a forest...\n");
return;
}
cout << s << ": size == " << size() << endl;
if (nodes_.empty()) return;
root_->printTree("");
}

132
cpp/ISAM2-inl.h
View File

@ -22,6 +22,44 @@ 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) {
// combine the factors of all nodes connected to the variable to be eliminated
// if no factors are connected to key, returns an empty factor
boost::shared_ptr<GaussianFactor> joint_factor = removeAndCombineFactors(graph,key);
// eliminate that joint factor
boost::shared_ptr<GaussianFactor> factor;
boost::shared_ptr<GaussianConditional> conditional;
boost::tie(conditional, factor) = joint_factor->eliminate(key);
// remember the intermediate result to be able to later restart computation in the middle
cached[key] = factor;
// add new factor on separator back into the graph
if (!factor->empty()) graph.push_back(factor);
// return the conditional Gaussian
return conditional;
}
// from GaussianFactorGraph.cpp, see _eliminateOne above
GaussianBayesNet _eliminate(FactorGraph<GaussianFactor>& graph, cachedFactors& cached, const Ordering& ordering) {
GaussianBayesNet chordalBayesNet; // empty
BOOST_FOREACH(string key, ordering) {
GaussianConditional::shared_ptr cg = _eliminateOne(graph, cached, key);
chordalBayesNet.push_back(cg);
}
return chordalBayesNet;
}
GaussianBayesNet _eliminate_const(const FactorGraph<GaussianFactor>& graph, cachedFactors& cached, const Ordering& ordering) {
// make a copy that can be modified locally
FactorGraph<GaussianFactor> graph_ignored = graph;
return _eliminate(graph_ignored, cached, ordering);
}
/** Create an empty Bayes Tree */
template<class Conditional, class Config>
ISAM2<Conditional, Config>::ISAM2() : BayesTree<Conditional>() {}
@ -29,7 +67,10 @@ 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), config_(config) {
// todo: repeats calculation above, just to set "cached"
_eliminate_const(nlfg.linearize(config), cached, ordering);
}
/* ************************************************************************* */
template<class Conditional, class Config>
@ -42,7 +83,6 @@ namespace gtsam {
config_.insert(it->first, it->second);
}
}
nonlinearFactors_.push_back(newFactors);
FactorGraph<GaussianFactor> newFactorsLinearized = newFactors.linearize(config_);
@ -50,8 +90,8 @@ namespace gtsam {
FactorGraph<GaussianFactor> affectedFactors;
boost::tie(affectedFactors, orphans) = this->removeTop(newFactorsLinearized);
#if 1
#if 0
// find the corresponding original nonlinear factors, and relinearize them
NonlinearFactorGraph<Config> nonlinearAffectedFactors;
set<int> idxs; // avoid duplicates by putting index into set
@ -75,29 +115,92 @@ namespace gtsam {
BOOST_FOREACH(int idx, idxs) {
nonlinearAffectedFactors.push_back(nonlinearFactors_[idx]);
}
FactorGraph<GaussianFactor> factors = nonlinearAffectedFactors.linearize(config_);
#else
NonlinearFactorGraph<Config> nonlinearAffectedFactors;
// retrieve all factors that ONLY contain the affected variables
// (note that the remaining stuff is summarized in the cached factors)
list<string> affectedKeys = affectedFactors.keys();
typename FactorGraph<NonlinearFactor<Config> >::iterator it;
for(it = nonlinearFactors_.begin(); it != nonlinearFactors_.end(); it++) {
bool inside = true;
BOOST_FOREACH(string key, (*it)->keys()) {
if (find(affectedKeys.begin(), affectedKeys.end(), key) == affectedKeys.end())
inside = false;
}
if (inside)
nonlinearAffectedFactors.push_back(*it);
}
FactorGraph<GaussianFactor> factors = nonlinearAffectedFactors.linearize(config_);
// recover intermediate 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);
it--;
string key = *it;
// retrieve the cached factor and add to boundary
cachedBoundary.push_back(cached[key]);
}
factors.push_back(cachedBoundary);
#endif
#if 0
printf("**************\n");
nonlinearFactors_.linearize(config).print("all factors");
printf("--------------\n");
newFactorsLinearized.print("newFactorsLinearized");
printf("--------------\n");
factors.print("factors");
printf("--------------\n");
affectedFactors.print("affectedFactors");
printf("--------------\n");
#endif
// add the new factors themselves
factors.push_back(newFactorsLinearized);
#endif
affectedFactors.push_back(newFactorsLinearized);
// create an ordering for the new and contaminated factors
Ordering ordering;
if (true) {
ordering = /*affectedF*/factors.getOrdering();
ordering = factors.getOrdering();
} else {
list<string> keys = /*affectedF*/factors.keys();
list<string> keys = factors.keys();
keys.sort(); // todo: correct sorting order?
ordering = keys;
}
#if 0
ordering.print();
factors.print("factors BEFORE");
printf("--------------\n");
#endif
// eliminate into a Bayes net
BayesNet<Conditional> bayesNet = eliminate<GaussianFactor, Conditional>(affectedFactors,ordering);
BayesNet<Conditional> bayesNet = _eliminate(factors, cached, ordering);
#if 1
BayesNet<Conditional> bayesNetTest = eliminate<GaussianFactor, Conditional>(factors,ordering); // todo - debug only
// check if relinearized agrees with correct solution
affectedFactors.push_back(newFactorsLinearized);
#if 0
affectedFactors.print("affectedFactors BEFORE");
printf("--------------\n");
#endif
BayesNet<Conditional> bayesNetTest = eliminate<GaussianFactor, GaussianConditional>(affectedFactors, ordering);
if (!bayesNet.equals(bayesNetTest)) {
printf("differ\n");
bayesNet.print();
@ -106,6 +209,8 @@ namespace gtsam {
}
#endif
nonlinearFactors_.push_back(newFactors);
// insert conditionals back in, straight into the topless bayesTree
typename BayesNet<Conditional>::const_reverse_iterator rit;
for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit )
@ -125,6 +230,15 @@ namespace gtsam {
template<class Conditional, class Config>
void ISAM2<Conditional, Config>::update(const NonlinearFactorGraph<Config>& newFactors, const Config& config) {
#if 0
printf("8888888888888888\n");
try {
this->print("BayesTree");
} catch (char * c) {};
printf("8888888888888888\n");
#endif
Cliques orphans;
this->update_internal(newFactors, config, orphans);
}

5
cpp/ISAM2.h
View File

@ -23,6 +23,8 @@
namespace gtsam {
typedef std::map<std::string, GaussianFactor::shared_ptr> cachedFactors;
template<class Conditional, class Config>
class ISAM2: public BayesTree<Conditional> {
@ -32,6 +34,9 @@ namespace gtsam {
Config config_;
NonlinearFactorGraph<Config> nonlinearFactors_;
// cached intermediate results for restarting computation in the middle
cachedFactors cached;
public:
/** Create an empty Bayes Tree */

2
cpp/testGaussianISAM2.cpp
View File

@ -19,7 +19,7 @@ using namespace boost::assign;
using namespace std;
using namespace gtsam;
/* ************************************************************************* *
/* ************************************************************************* */
TEST( ISAM2, ISAM2_smoother )
{
// Create smoother with 7 nodes