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

calculating the number of nonzero entries in R corresponding to the Bayes tree

release/4.3a0
Michael Kaess 2010-09-06 20:34:59 +00:00
parent d850e2837c
commit e86c9465bb
3 changed files with 33 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
inference/ISAM2-inl.h
View File

@ -221,12 +221,14 @@ namespace gtsam {
int maxClique = 0;
double avgClique = 0;
int numCliques = 0;
int nnzR = 0;
if (counter>0) { // cannot call on empty tree
GaussianISAM2_P::CliqueData cdata = this->getCliqueData();
GaussianISAM2_P::CliqueStats cstats = cdata.getStats();
maxClique = cstats.maxConditionalSize;
avgClique = cstats.avgConditionalSize;
numCliques = cdata.conditionalSizes.size();
nnzR = calculate_nnz(this->root());
}
counter++;
#endif
@ -267,7 +269,7 @@ namespace gtsam {
#ifdef PRINT_STATS
cout << "linear: #newKeys: " << newKeys.size() << " #affectedVariables: " << affectedKeys.size()
<< " #affectedFactors: " << factors.size() << " maxCliqueSize: " << maxClique
<< " avgCliqueSize: " << avgClique << " #Cliques: " << numCliques << endl;
<< " avgCliqueSize: " << avgClique << " #Cliques: " << numCliques << " nnzR: " << nnzR << endl;
#endif
toc("linear_lookup1");

27
slam/GaussianISAM2.cpp
View File

@ -53,4 +53,31 @@ VectorConfig optimize2(const GaussianISAM2_P& bayesTree, double threshold) {
return result;
}
/* ************************************************************************* */
void nnz_internal(const GaussianISAM2::sharedClique& clique, int& result) {
// go through the conditionals of this clique
GaussianISAM2::Clique::const_reverse_iterator it;
for (it = clique->rbegin(); it!=clique->rend(); it++) {
GaussianConditional::shared_ptr cg = *it;
int dimSep = 0;
for (GaussianConditional::const_iterator matrix_it = cg->parentsBegin(); matrix_it != cg->parentsEnd(); matrix_it++) {
dimSep += matrix_it->second.size2();
}
int dimR = cg->dim();
result += (dimR+1)*dimR/2 + dimSep*dimR;
}
// traverse the children
BOOST_FOREACH(const GaussianISAM2::sharedClique& child, clique->children_) {
nnz_internal(child, result);
}
}
/* ************************************************************************* */
int calculate_nnz(const GaussianISAM2::sharedClique& clique) {
int result = 0;
// starting from the root, add up entries of frontal and conditional matrices of each conditional
nnz_internal(clique, result);
return result;
}
} /// namespace gtsam

3
slam/GaussianISAM2.h
View File

@ -35,4 +35,7 @@ namespace gtsam {
// optimize the BayesTree, starting from the root
VectorConfig optimize2(const GaussianISAM2_P& bayesTree, double threshold = 0.);
// 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);
}/// namespace gtsam