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

speedup; cleanup and reordering to get in sync with paper

release/4.3a0
Michael Kaess 2010-09-11 21:07:37 +00:00
parent 9be2f3c102
commit 4bad086759
2 changed files with 64 additions and 84 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

145
inference/ISAM2-inl.h
View File

@ -120,17 +120,17 @@ boost::shared_ptr<GaussianConditional> _eliminateOne(FactorGraph<GaussianFactor>
} }
// from GaussianFactorGraph.cpp, see _eliminateOne above // from GaussianFactorGraph.cpp, see _eliminateOne above
GaussianBayesNet _eliminate(FactorGraph<GaussianFactor>& graph, CachedFactors& cached, const Ordering& ordering) { boost::shared_ptr<GaussianBayesNet> _eliminate(FactorGraph<GaussianFactor>& graph, CachedFactors& cached, const Ordering& ordering) {
GaussianBayesNet chordalBayesNet; // empty boost::shared_ptr<GaussianBayesNet> chordalBayesNet(new GaussianBayesNet()); // empty
BOOST_FOREACH(const Symbol& 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);
} }
return chordalBayesNet; return chordalBayesNet;
} }
// special const version used in constructor below // special const version used in constructor below
GaussianBayesNet _eliminate_const(const FactorGraph<GaussianFactor>& graph, CachedFactors& cached, const Ordering& ordering) { boost::shared_ptr<GaussianBayesNet> _eliminate_const(const FactorGraph<GaussianFactor>& graph, CachedFactors& cached, const Ordering& ordering) {
// make a copy that can be modified locally // make a copy that can be modified locally
FactorGraph<GaussianFactor> graph_ignored = graph; FactorGraph<GaussianFactor> graph_ignored = graph;
return _eliminate(graph_ignored, cached, ordering); return _eliminate(graph_ignored, cached, ordering);
@ -169,18 +169,20 @@ list<size_t> ISAM2<Conditional, Config>::getAffectedFactors(const list<Symbol>&
// (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>
boost::shared_ptr<GaussianFactorGraph> ISAM2<Conditional, Config>::relinearizeAffectedFactors boost::shared_ptr<GaussianFactorGraph> ISAM2<Conditional, Config>::relinearizeAffectedFactors
(const set<Symbol>& affectedKeys) const { (const list<Symbol>& affectedKeys) const {
list<Symbol> affectedKeysList; // todo: shouldn't have to convert back to list... list<size_t> candidates = getAffectedFactors(affectedKeys);
affectedKeysList.insert(affectedKeysList.begin(), affectedKeys.begin(), affectedKeys.end());
list<size_t> candidates = getAffectedFactors(affectedKeysList);
NonlinearFactorGraph<Config> nonlinearAffectedFactors; NonlinearFactorGraph<Config> nonlinearAffectedFactors;
// for fast lookup below
set<Symbol> affectedKeysSet;
affectedKeysSet.insert(affectedKeys.begin(), affectedKeys.end());
BOOST_FOREACH(size_t idx, candidates) { BOOST_FOREACH(size_t idx, candidates) {
bool inside = true; bool inside = true;
BOOST_FOREACH(const Symbol& key, nonlinearFactors_[idx]->keys()) { BOOST_FOREACH(const Symbol& key, nonlinearFactors_[idx]->keys()) {
if (affectedKeys.find(key) == affectedKeys.end()) { if (affectedKeysSet.find(key) == affectedKeysSet.end()) {
inside = false; inside = false;
break; break;
} }
@ -189,7 +191,6 @@ boost::shared_ptr<GaussianFactorGraph> ISAM2<Conditional, Config>::relinearizeAf
nonlinearAffectedFactors.push_back(nonlinearFactors_[idx]); nonlinearAffectedFactors.push_back(nonlinearFactors_[idx]);
} }
// TODO: temporary might be expensive, return shared pointer ?
return nonlinearAffectedFactors.linearize(theta_); return nonlinearAffectedFactors.linearize(theta_);
} }
@ -255,11 +256,8 @@ void ISAM2<Conditional, Config>::recalculate(const list<Symbol>& markedKeys, con
tic("re-lookup"); tic("re-lookup");
// ordering provides all keys in conditionals, there cannot be others because path to root included // ordering provides all keys in conditionals, there cannot be others because path to root included
set<Symbol> affectedKeys; list<Symbol> affectedKeys = affectedBayesNet.ordering();
list<Symbol> tmp = affectedBayesNet.ordering(); FactorGraph<GaussianFactor> factors(*relinearizeAffectedFactors(affectedKeys));
affectedKeys.insert(tmp.begin(), tmp.end());
FactorGraph<GaussianFactor> factors(*relinearizeAffectedFactors(affectedKeys)); // todo: no need to relinearize here, should have cached linearized factors
lastAffectedMarkedCount = markedKeys.size(); lastAffectedMarkedCount = markedKeys.size();
lastAffectedVariableCount = affectedKeys.size(); lastAffectedVariableCount = affectedKeys.size();
@ -292,16 +290,18 @@ void ISAM2<Conditional, Config>::recalculate(const list<Symbol>& markedKeys, con
// 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]) // 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])
tic("re-order");
// create an ordering for the new and contaminated factors // create an ordering for the new and contaminated factors
// markedKeys are passed in: those variables will be forced to the end in the ordering // markedKeys are passed in: those variables will be forced to the end in the ordering
set<Symbol> markedKeysSet; set<Symbol> markedKeysSet;
markedKeysSet.insert(markedKeys.begin(), markedKeys.end()); markedKeysSet.insert(markedKeys.begin(), markedKeys.end());
Ordering ordering = factors.getConstrainedOrdering(markedKeysSet); // intelligent ordering Ordering ordering = factors.getConstrainedOrdering(markedKeysSet); // intelligent ordering
// Ordering ordering = factors.getOrdering(); // original ordering, yields bad performance // Ordering ordering = factors.getOrdering(); // original ordering, yields bad performance
toc("re-order");
// eliminate into a Bayes net // eliminate into a Bayes net
tic("eliminate"); tic("eliminate");
BayesNet<Conditional> bayesNet = _eliminate(factors, cached_, ordering); boost::shared_ptr<GaussianBayesNet> bayesNet = _eliminate(factors, cached_, ordering);
toc("eliminate"); toc("eliminate");
tic("re-assemble"); tic("re-assemble");
@ -310,7 +310,7 @@ void ISAM2<Conditional, Config>::recalculate(const list<Symbol>& markedKeys, con
// insert conditionals back in, straight into the topless bayesTree // insert conditionals back in, straight into the topless bayesTree
typename BayesNet<Conditional>::const_reverse_iterator rit; typename BayesNet<Conditional>::const_reverse_iterator rit;
for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit ) for ( rit=bayesNet->rbegin(); rit != bayesNet->rend(); ++rit )
this->insert(*rit, index); this->insert(*rit, index);
// Save number of affectedCliques // Save number of affectedCliques
@ -358,57 +358,15 @@ void ISAM2<Conditional, Config>::find_all(sharedClique clique, list<Symbol>& key
} }
} }
/* ************************************************************************* */
template<class Conditional, class Config>
list<Symbol> ISAM2<Conditional, Config>::fluid_relinearization(double relinearize_threshold, VectorConfig& deltaMarked) {
// Input: nonlinear factors factors_, linearization point theta_, Bayes tree (this), delta_
// 1. Mark variables in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}.
tic("fluid-mark");
list<Symbol> marked;
for (VectorConfig::const_iterator it = delta_.begin(); it!=delta_.end(); it++) {
const Symbol& key = it->first;
const Vector& v = it->second;
if (max(abs(v)) >= relinearize_threshold) {
marked.push_back(key);
deltaMarked.insert(key, v);
}
}
toc("fluid-mark");
list<Symbol> affectedSymbols;
if (marked.size()>0) {
// 3. Mark all cliques that involve marked variables \Theta_{J} and all their ancestors.
// mark all cliques that involve marked variables
affectedSymbols = marked; // add all marked
tic("fluid-find_all");
find_all(this->root(), affectedSymbols, marked); // add other cliques that have the marked ones in the separator
affectedSymbols.sort(); // remove duplicates
affectedSymbols.unique();
toc("fluid-find_all");
// 4. From the leaves to the top, if a clique is marked:
// re-linearize the original factors in \Factors associated with the clique,
// add the cached marginal factors from its children, and re-eliminate.
// todo: for simplicity, currently simply remove the top and recreate it using the original ordering
//recalculate(affectedSymbols);
// Output: updated Bayes tree (this), updated linearization point theta_
}
return affectedSymbols;
}
/* ************************************************************************* */ /* ************************************************************************* */
template<class Conditional, class Config> template<class Conditional, class Config>
void ISAM2<Conditional, Config>::update( void ISAM2<Conditional, Config>::update(
const NonlinearFactorGraph<Config>& newFactors, const Config& newTheta, const NonlinearFactorGraph<Config>& newFactors, const Config& newTheta,
double wildfire_threshold, double relinearize_threshold, bool relinearize) { double wildfire_threshold, double relinearize_threshold, bool relinearize) {
static int count = 0;
count++;
lastAffectedVariableCount = 0; lastAffectedVariableCount = 0;
lastAffectedFactorCount = 0; lastAffectedFactorCount = 0;
lastAffectedCliqueCount = 0; lastAffectedCliqueCount = 0;
@ -442,43 +400,66 @@ void ISAM2<Conditional, Config>::update(
delta_ = optimize2(*this, wildfire_threshold); delta_ = optimize2(*this, wildfire_threshold);
#endif #endif
tic("step4");
// 4. Mark nonlinear update (includes change in theta_)
VectorConfig deltaMarked; VectorConfig deltaMarked;
if (relinearize) { if (relinearize && count%10 == 0) { // todo: every n steps
list<Symbol> markedRelin = fluid_relinearization(relinearize_threshold, deltaMarked); // in: delta_, theta_, nonlinearFactors_, this tic("step4");
// 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}.
list<Symbol> markedRelin;
for (VectorConfig::const_iterator it = delta_.begin(); it!=delta_.end(); it++) {
const Symbol& key = it->first;
const Vector& v = it->second;
if (max(abs(v)) >= relinearize_threshold) {
markedRelin.push_back(key);
deltaMarked.insert(key, v);
}
}
toc("step4");
tic("step5");
// 5. Mark all cliques that involve marked variables \Theta_{J} and all their ancestors.
list<Symbol> affectedKeys;
if (markedRelin.size()>0) {
// mark all cliques that involve marked variables
affectedKeys = markedRelin; // add all marked
tic("fluid-find_all");
find_all(this->root(), affectedKeys, markedRelin); // add other cliques that have the marked ones in the separator
affectedKeys.sort(); // remove duplicates
affectedKeys.unique();
toc("fluid-find_all");
}
// merge with markedKeys // merge with markedKeys
markedKeys.splice(markedKeys.begin(), markedRelin, markedRelin.begin(), markedRelin.end()); markedKeys.splice(markedKeys.begin(), affectedKeys, affectedKeys.begin(), affectedKeys.end());
markedKeys.sort(); // remove duplicates markedKeys.sort(); // remove duplicates
markedKeys.unique(); markedKeys.unique();
} toc("step5");
toc("step4");
tic("step5"); }
// 5. Update linearization point for marked variables: \Theta_{J}:=\Theta_{J}+\Delta_{J}.
tic("step6");
// 6. Update linearization point for marked variables: \Theta_{J}:=\Theta_{J}+\Delta_{J}.
if (deltaMarked.size()>0) { if (deltaMarked.size()>0) {
theta_ = theta_.expmap(deltaMarked); theta_ = theta_.expmap(deltaMarked);
} }
toc("step5");
#ifndef SEPARATE_STEPS
tic("step6");
// 6. Linearize new factors
boost::shared_ptr<GaussianFactorGraph> linearFactors = newFactors.linearize(theta_);
toc("step6"); toc("step6");
#ifndef SEPARATE_STEPS
tic("step7"); tic("step7");
// 7. Redo top of Bayes tree // 7. Linearize new factors
recalculate(markedKeys, &(*linearFactors)); boost::shared_ptr<GaussianFactorGraph> linearFactors = newFactors.linearize(theta_);
toc("step7"); toc("step7");
tic("step8");
// 8. Redo top of Bayes tree
recalculate(markedKeys, &(*linearFactors));
toc("step8");
#else #else
recalculate(markedKeys); recalculate(markedKeys);
#endif #endif
tic("step8"); tic("step9");
// 8. Solve // 9. Solve
delta_ = optimize2(*this, wildfire_threshold); delta_ = optimize2(*this, wildfire_threshold);
toc("step8"); toc("step9");
toc("all"); toc("all");
tictoc_print(); // switch on/off at top of file (#if 1/#if 0) tictoc_print(); // switch on/off at top of file (#if 1/#if 0)

3
inference/ISAM2.h
View File

@ -87,13 +87,12 @@ public:
private: private:
std::list<size_t> getAffectedFactors(const std::list<Symbol>& keys) const; std::list<size_t> getAffectedFactors(const std::list<Symbol>& keys) const;
boost::shared_ptr<GaussianFactorGraph> relinearizeAffectedFactors(const std::set<Symbol>& affectedKeys) const; boost::shared_ptr<GaussianFactorGraph> relinearizeAffectedFactors(const std::list<Symbol>& affectedKeys) const;
FactorGraph<GaussianFactor> getCachedBoundaryFactors(Cliques& orphans); FactorGraph<GaussianFactor> getCachedBoundaryFactors(Cliques& orphans);
void recalculate(const std::list<Symbol>& markedKeys, const FactorGraph<GaussianFactor>* newFactors = NULL); void recalculate(const std::list<Symbol>& markedKeys, const FactorGraph<GaussianFactor>* newFactors = NULL);
void linear_update(const FactorGraph<GaussianFactor>& newFactors); void linear_update(const FactorGraph<GaussianFactor>& newFactors);
void find_all(sharedClique clique, std::list<Symbol>& keys, const std::list<Symbol>& marked); // helper function void find_all(sharedClique clique, std::list<Symbol>& keys, const std::list<Symbol>& marked); // helper function
std::list<Symbol> fluid_relinearization(double relinearize_threshold, VectorConfig& deltaMarked);
}; // ISAM2 }; // ISAM2