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

Small data structure change in ISAM2::getAffectedFactors for a speedup

release/4.3a0
Richard Roberts 2013-08-14 19:28:45 +00:00
parent 36b3e214b0
commit f9eb5b6e2c
2 changed files with 5 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
gtsam/nonlinear/ISAM2.cpp
View File

@ -173,25 +173,18 @@ bool ISAM2::equals(const ISAM2& other, double tol) const {
}
/* ************************************************************************* */
FastList<size_t> ISAM2::getAffectedFactors(const FastList<Key>& keys) const {
FastSet<size_t> ISAM2::getAffectedFactors(const FastList<Key>& keys) const {
static const bool debug = false;
if(debug) cout << "Getting affected factors for ";
if(debug) { BOOST_FOREACH(const Key key, keys) { cout << key << " "; } }
if(debug) cout << endl;
NonlinearFactorGraph allAffected;
FastList<size_t> indices;
FastSet<size_t> indices;
BOOST_FOREACH(const Key key, keys) {
// const list<size_t> l = nonlinearFactors_.factors(key);
// indices.insert(indices.begin(), l.begin(), l.end());
const VariableIndex::Factors& factors(variableIndex_[key]);
BOOST_FOREACH(size_t factor, factors) {
if(debug) cout << "Variable " << key << " affects factor " << factor << endl;
indices.push_back(factor);
}
indices.insert(factors.begin(), factors.end());
}
indices.sort();
indices.unique();
if(debug) cout << "Affected factors are: ";
if(debug) { BOOST_FOREACH(const size_t index, indices) { cout << index << " "; } }
if(debug) cout << endl;
@ -206,7 +199,7 @@ GaussianFactorGraph::shared_ptr
ISAM2::relinearizeAffectedFactors(const FastList<Key>& affectedKeys, const FastSet<Key>& relinKeys) const
{
gttic(getAffectedFactors);
FastList<size_t> candidates = getAffectedFactors(affectedKeys);
FastSet<size_t> candidates = getAffectedFactors(affectedKeys);
gttoc(getAffectedFactors);
NonlinearFactorGraph nonlinearAffectedFactors;

2
gtsam/nonlinear/ISAM2.h
View File

@ -624,7 +624,7 @@ public:
private:
FastList<size_t> getAffectedFactors(const FastList<Key>& keys) const;
FastSet<size_t> getAffectedFactors(const FastList<Key>& keys) const;
GaussianFactorGraph::shared_ptr relinearizeAffectedFactors(const FastList<Key>& affectedKeys, const FastSet<Key>& relinKeys) const;
GaussianFactorGraph getCachedBoundaryFactors(Cliques& orphans);