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

fixed wildfire/backsubstitution, execution much faster now even with very low threshold

release/4.3a0
Michael Kaess 2010-09-12 05:14:21 +00:00
parent 4bad086759
commit 64467634e3
4 changed files with 69 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
inference/ISAM2-inl.h
View File

@ -212,7 +212,7 @@ FactorGraph<GaussianFactor> ISAM2<Conditional, Config>::getCachedBoundaryFactors
/* ************************************************************************* */
template<class Conditional, class Config>
void ISAM2<Conditional, Config>::recalculate(const list<Symbol>& markedKeys, const FactorGraph<GaussianFactor>* newFactors) {
boost::shared_ptr<set<Symbol> > ISAM2<Conditional, Config>::recalculate(const list<Symbol>& markedKeys, const FactorGraph<GaussianFactor>* newFactors) {
// Input: BayesTree(this), newFactors
@ -330,6 +330,10 @@ void ISAM2<Conditional, Config>::recalculate(const list<Symbol>& markedKeys, con
toc("re-orphans");
// Output: BayesTree(this)
boost::shared_ptr<set<Symbol> > affectedKeysSet(new set<Symbol>());
affectedKeysSet->insert(affectedKeys.begin(), affectedKeys.end());
return affectedKeysSet;
}
/* ************************************************************************* */
@ -450,7 +454,7 @@ void ISAM2<Conditional, Config>::update(
tic("step8");
// 8. Redo top of Bayes tree
recalculate(markedKeys, &(*linearFactors));
boost::shared_ptr<set<Symbol> > replacedKeys = recalculate(markedKeys, &(*linearFactors));
toc("step8");
#else
recalculate(markedKeys);
@ -458,7 +462,11 @@ void ISAM2<Conditional, Config>::update(
tic("step9");
// 9. Solve
delta_ = optimize2(*this, wildfire_threshold);
// if (wildfire_threshold<=0.) {
// delta_ = *(optimize2(this->root()));
// } else {
optimize2(this->root(), wildfire_threshold, *replacedKeys, delta_); // modifies delta_
// }
toc("step9");
toc("all");

4
inference/ISAM2.h
View File

@ -72,7 +72,7 @@ public:
const Config calculateEstimate() const {return theta_.expmap(delta_);}
// estimate based on full delta (note that this is based on the current linearization point)
const Config calculateBestEstimate() const {return theta_.expmap(optimize2(*this, 0.));}
const Config calculateBestEstimate() const {return theta_.expmap(*optimize2(this->root()));}
const NonlinearFactorGraph<Config>& getFactorsUnsafe() const { return nonlinearFactors_; }
@ -90,7 +90,7 @@ private:
boost::shared_ptr<GaussianFactorGraph> relinearizeAffectedFactors(const std::list<Symbol>& affectedKeys) const;
FactorGraph<GaussianFactor> getCachedBoundaryFactors(Cliques& orphans);
void recalculate(const std::list<Symbol>& markedKeys, const FactorGraph<GaussianFactor>* newFactors = NULL);
boost::shared_ptr<std::set<Symbol> > recalculate(const std::list<Symbol>& markedKeys, const FactorGraph<GaussianFactor>* newFactors = NULL);
void linear_update(const FactorGraph<GaussianFactor>& newFactors);
void find_all(sharedClique clique, std::list<Symbol>& keys, const std::list<Symbol>& marked); // helper function

76
slam/GaussianISAM2.cpp
View File

@ -18,7 +18,8 @@ template class ISAM2<GaussianConditional, planarSLAM::Config>;
namespace gtsam {
/* ************************************************************************* */
void optimize2(const GaussianISAM2::sharedClique& clique, double threshold, set<Symbol>& changed, VectorConfig& result) {
void optimize2(const GaussianISAM2::sharedClique& clique, double threshold,
set<Symbol>& changed, const set<Symbol>& replaced, VectorConfig& delta) {
// 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
@ -29,10 +30,13 @@ void optimize2(const GaussianISAM2::sharedClique& clique, double threshold, set<
for (it = clique->rbegin(); it!=clique->rend(); it++) {
GaussianConditional::shared_ptr cg = *it;
// is this variable part of the top of the tree that has been redone?
bool redo = (replaced.find(cg->key()) != replaced.end());
// 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) {
if (!redo && cg->nrParents()>0) {
found = false;
BOOST_FOREACH(const Symbol& key, cg->parents()) {
if (changed.find(key)!=changed.end()) {
@ -42,66 +46,76 @@ void optimize2(const GaussianISAM2::sharedClique& clique, double threshold, set<
}
if (found) {
// Solve for that variable
Vector x = cg->solve(result);
Vector d = cg->solve(delta);
// have to process children; only if none of the variables in the
// clique were affected, and none of the variables in the clique
// had a variable in the separator that changed significantly
// can we be sure that the subtree is not affected
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;
// we change the delta unconditionally if redo, otherwise
// conditioned on the change being above the threshold
if (!redo) {
// change is measured against the previous delta!
if (delta.contains(cg->key())) {
Vector d_old = delta[cg->key()];
if (max(abs(d-d_old)) >= threshold) {
redo = true;
}
} else {
redo = true; // never created before, so we simply add it
}
}
// replace current entry in delta vector
if (redo) {
changed.insert(cg->key());
if (delta.contains(cg->key())) {
delta[cg->key()] = d; // replace existing entry
} else {
delta.insert(cg->key(), d); // insert new entry
}
}
}
}
if (process_children) {
BOOST_FOREACH(const GaussianISAM2::sharedClique& child, clique->children_) {
optimize2(child, threshold, changed, result);
optimize2(child, threshold, changed, replaced, delta);
}
}
}
/* ************************************************************************* */
// fast full version without threshold
void optimize2(const GaussianISAM2::sharedClique& clique, VectorConfig& result) {
void optimize2(const GaussianISAM2::sharedClique& clique, boost::shared_ptr<VectorConfig> delta) {
// 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);
Vector d = cg->solve(*delta);
// store result in partial solution
result.insert(cg->key(), x);
delta->insert(cg->key(), d);
}
BOOST_FOREACH(const GaussianISAM2::sharedClique& child, clique->children_) {
optimize2(child, result);
optimize2(child, delta);
}
}
/* ************************************************************************* */
VectorConfig optimize2(const GaussianISAM2& bayesTree, double threshold) {
VectorConfig result;
boost::shared_ptr<VectorConfig> optimize2(const GaussianISAM2::sharedClique& root) {
boost::shared_ptr<VectorConfig> delta(new VectorConfig);
set<Symbol> changed;
// starting from the root, call optimize on each conditional
if (threshold<=0.) {
optimize2(bayesTree.root(), result);
} else {
optimize2(bayesTree.root(), threshold, changed, result);
}
return result;
optimize2(root, delta);
return delta;
}
/* ************************************************************************* */
VectorConfig optimize2(const GaussianISAM2_P& bayesTree, double threshold) {
VectorConfig result;
void optimize2(const GaussianISAM2::sharedClique& root, double threshold, const set<Symbol>& keys, VectorConfig& delta) {
set<Symbol> changed;
// starting from the root, call optimize on each conditional
if (threshold<=0.) {
optimize2(bayesTree.root(), result);
} else {
optimize2(bayesTree.root(), threshold, changed, result);
}
return result;
optimize2(root, threshold, changed, keys, delta);
}
/* ************************************************************************* */

22
slam/GaussianISAM2.h
View File

@ -17,20 +17,20 @@
namespace gtsam {
typedef ISAM2<GaussianConditional, simulated2D::Config> GaussianISAM2;
// optimize the BayesTree, starting from the root
VectorConfig optimize2(const GaussianISAM2& bayesTree, double threshold = 0.);
// todo: copy'n'paste to avoid template hell
typedef ISAM2<GaussianConditional, planarSLAM::Config> GaussianISAM2_P;
// recursively optimize this conditional and all subtrees
// void optimize2(const GaussianISAM2_P::sharedClique& clique, VectorConfig& result);
// optimize the BayesTree, starting from the root
VectorConfig optimize2(const GaussianISAM2_P& bayesTree, double threshold = 0.);
boost::shared_ptr<VectorConfig> optimize2(const GaussianISAM2::sharedClique& root);
// optimize the BayesTree, starting from the root; "replaced" needs to contain
// all variables that are contained in the top of the Bayes tree that has been
// redone; "delta" is the current solution, an offset from the linearization
// point; "threshold" is the maximum change against the PREVIOUS delta for
// non-replaced variables that can be ignored, ie. the old delta entry is kept
// and recursive backsubstitution might eventually stop if none of the changed
// variables are contained in the subtree.
void optimize2(const GaussianISAM2::sharedClique& root,
double threshold, const std::set<Symbol>& replaced, VectorConfig& delta);
// calculate the number of non-zero entries for the tree starting at clique (use root for complete matrix)
int calculate_nnz(const GaussianISAM2::sharedClique& clique);