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

bug fix in wildfire alg; more stats

release/4.3a0
Michael Kaess 2010-09-09 19:17:17 +00:00
parent e3de72bd05
commit f8cf500aff
4 changed files with 93 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
inference/ISAM2-inl.h
View File

@ -19,7 +19,7 @@ using namespace boost::assign;
#include <gtsam/inference/ISAM2.h>
#if 1 // timing
#if 1 // timing - note: adds some time when applied in inner loops
#include <sys/time.h>
// simple class for accumulating execution timing information by name
class Timing {
@ -236,10 +236,10 @@ namespace gtsam {
// 1. Remove top of Bayes tree and convert to a factor graph:
// (a) For each affected variable, remove the corresponding clique and all parents up to the root.
// (b) Store orphaned sub-trees \BayesTree_{O} of removed cliques.
const list<Symbol> newKeys = newFactors.keys();
const list<Symbol> markedKeys = newFactors.keys();
Cliques orphans;
BayesNet<GaussianConditional> affectedBayesNet;
this->removeTop(newKeys, affectedBayesNet, orphans);
this->removeTop(markedKeys, affectedBayesNet, orphans);
// FactorGraph<GaussianFactor> factors(affectedBayesNet);
// bug was here: we cannot reuse the original factors, because then the cached factors get messed up
@ -258,16 +258,15 @@ namespace gtsam {
list<Symbol> tmp = affectedBayesNet.ordering();
affectedKeys.insert(tmp.begin(), tmp.end());
// Save number of affected variables
lastAffectedVariableCount = affectedKeys.size();
FactorGraph<GaussianFactor> factors(*relinearizeAffectedFactors(affectedKeys)); // todo: no need to relinearize here, should have cached linearized factors
// Save number of affected factors
lastAffectedMarkedCount = markedKeys.size();
lastAffectedVariableCount = affectedKeys.size();
lastAffectedFactorCount = factors.size();
// output for generating figures
#ifdef PRINT_STATS
cout << "linear: #newKeys: " << newKeys.size() << " #affectedVariables: " << affectedKeys.size()
// output for generating figures
cout << "linear: #markedKeys: " << markedKeys.size() << " #affectedVariables: " << affectedKeys.size()
<< " #affectedFactors: " << factors.size() << " maxCliqueSize: " << maxClique
<< " avgCliqueSize: " << avgClique << " #Cliques: " << numCliques << " nnzR: " << nnzR << endl;
#endif
@ -287,10 +286,10 @@ namespace gtsam {
// 3. Re-order and eliminate the factor graph into a Bayes net (Algorithm [alg:eliminate]), and re-assemble into a new Bayes tree (Algorithm [alg:BayesTree])
// create an ordering for the new and contaminated factors
// newKeys are passed in: those variables will be forced to the end in the ordering
set<Symbol> newKeysSet;
newKeysSet.insert(newKeys.begin(), newKeys.end());
Ordering ordering = factors.getConstrainedOrdering(newKeysSet); // intelligent ordering
// markedKeys are passed in: those variables will be forced to the end in the ordering
set<Symbol> markedKeysSet;
markedKeysSet.insert(markedKeys.begin(), markedKeys.end());
Ordering ordering = factors.getConstrainedOrdering(markedKeysSet); // intelligent ordering
// Ordering ordering = factors.getOrdering(); // original ordering, yields in bad performance
// eliminate into a Bayes net
@ -401,7 +400,11 @@ namespace gtsam {
factors = relinearizeAffectedFactors(affectedKeys);
toc("nonlin_relin");
cout << "nonlinear: #marked: " << marked.size() << " #affected: " << affectedKeys.size() << " #factors: " << factors->size() << endl;
lastNonlinearMarkedCount = marked.size();
lastNonlinearAffectedVariableCount = affectedKeys.size();
lastNonlinearAffectedFactorCount = factors->size();
// cout << "nonlinear: #marked: " << marked.size() << " #affected: " << affectedKeys.size() << " #factors: " << factors->size() << endl;
// add the cached intermediate results from the boundary of the orphans ...
FactorGraph<GaussianFactor> cachedBoundary = getCachedBoundaryFactors(orphans);
@ -438,6 +441,14 @@ namespace gtsam {
const NonlinearFactorGraph<Config>& newFactors, const Config& newTheta,
double wildfire_threshold, double relinearize_threshold, bool relinearize) {
lastAffectedVariableCount = 0;
lastAffectedFactorCount = 0;
lastAffectedCliqueCount = 0;
lastAffectedMarkedCount = 0;
lastNonlinearMarkedCount = 0;
lastNonlinearAffectedVariableCount = 0;
lastNonlinearAffectedFactorCount = 0;
tic("all");
tic("step1");
@ -473,7 +484,7 @@ namespace gtsam {
toc("step6");
// todo: not part of algorithm in paper: linearization point and delta_ do not fit... have to update delta again
//todo delta_ = optimize2(*this, wildfire_threshold);
delta_ = optimize2(*this, wildfire_threshold);
toc("all");

4
inference/ISAM2.h
View File

@ -79,6 +79,10 @@ namespace gtsam {
size_t lastAffectedVariableCount;
size_t lastAffectedFactorCount;
size_t lastAffectedCliqueCount;
size_t lastAffectedMarkedCount;
size_t lastNonlinearMarkedCount;
size_t lastNonlinearAffectedVariableCount;
size_t lastNonlinearAffectedFactorCount;
private:

65
slam/GaussianISAM2.cpp
View File

@ -18,38 +18,89 @@ template class ISAM2<GaussianConditional, planarSLAM::Config>;
namespace gtsam {
/* ************************************************************************* */
void optimize2(const GaussianISAM2::sharedClique& clique, double threshold, VectorConfig& result) {
void optimize2(const GaussianISAM2::sharedClique& clique, double threshold, set<Symbol>& changed, VectorConfig& result) {
// if none of the variables in this clique (frontal and separator!) changed
// significantly, then by the running intersection property, none of the
// cliques in the children need to be processed
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
// only solve if at least one of the separator variables changed
// significantly, ie. is in the set "changed"
bool found = true;
if (cg->nrParents()>0) {
found = false;
BOOST_FOREACH(const Symbol& key, cg->parents()) {
if (changed.find(key)!=changed.end()) {
found = true;
}
}
}
if (found) {
// Solve for that variable
Vector x = cg->solve(result);
process_children = true;
// store result in partial solution
result.insert(cg->key(), x);
// if change is above threshold, add to set of changed variables
if (max(abs(x)) >= threshold) {
changed.insert(cg->key());
process_children = true;
}
result.insert(cg->key(), x); // store result in partial solution
}
}
if (process_children) {
BOOST_FOREACH(const GaussianISAM2::sharedClique& child, clique->children_) {
optimize2(child, threshold, result);
optimize2(child, threshold, changed, result);
}
}
}
/* ************************************************************************* */
// fast version without threshold
void optimize2(const GaussianISAM2::sharedClique& clique, VectorConfig& result) {
// 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);
// store result in partial solution
result.insert(cg->key(), x);
}
BOOST_FOREACH(const GaussianISAM2::sharedClique& child, clique->children_) {
optimize2(child, result);
}
}
/* ************************************************************************* */
VectorConfig optimize2(const GaussianISAM2& bayesTree, double threshold) {
VectorConfig result;
set<Symbol> changed;
// starting from the root, call optimize on each conditional
optimize2(bayesTree.root(), threshold, result);
if (threshold<=0.) {
optimize2(bayesTree.root(), result);
} else {
optimize2(bayesTree.root(), threshold, changed, result);
}
return result;
}
/* ************************************************************************* */
VectorConfig optimize2(const GaussianISAM2_P& bayesTree, double threshold) {
VectorConfig result;
set<Symbol> changed;
// starting from the root, call optimize on each conditional
optimize2(bayesTree.root(), threshold, result);
if (threshold<=0.) {
optimize2(bayesTree.root(), result);
} else {
optimize2(bayesTree.root(), threshold, changed, result);
}
return result;
}
@ -64,7 +115,7 @@ void nnz_internal(const GaussianISAM2::sharedClique& clique, int& result) {
dimSep += matrix_it->second.size2();
}
int dimR = cg->dim();
result += (dimR+1)*dimR/2 + dimSep*dimR;
result += ((dimR+1)*dimR)/2 + dimSep*dimR;
}
// traverse the children
BOOST_FOREACH(const GaussianISAM2::sharedClique& child, clique->children_) {

3
slam/GaussianISAM2.h
View File

@ -18,9 +18,6 @@ namespace gtsam {
typedef ISAM2<GaussianConditional, simulated2D::Config> GaussianISAM2;
// recursively optimize this conditional and all subtrees
void optimize2(const GaussianISAM2::sharedClique& clique, double threshold, VectorConfig& result);
// optimize the BayesTree, starting from the root
VectorConfig optimize2(const GaussianISAM2& bayesTree, double threshold = 0.);