From 872ef5fe057ffbd7093188a2860194b08a3dc4ff Mon Sep 17 00:00:00 2001 From: Michael Kaess Date: Sun, 12 Sep 2010 15:43:24 +0000 Subject: [PATCH] added statistics for evaluation, disabled timing --- inference/ISAM2-inl.h | 29 ++++++++++++++++------------- inference/ISAM2.h | 5 ++--- slam/GaussianISAM2.cpp | 11 +++++++---- slam/GaussianISAM2.h | 3 ++- 4 files changed, 27 insertions(+), 21 deletions(-) diff --git a/inference/ISAM2-inl.h b/inference/ISAM2-inl.h index 980d67d1f..af1ed01b2 100644 --- a/inference/ISAM2-inl.h +++ b/inference/ISAM2-inl.h @@ -19,7 +19,7 @@ using namespace boost::assign; #include -#if 1 // timing - note: adds some time when applied in inner loops +#if 0 // timing - note: adds some time when applied in inner loops #include // simple class for accumulating execution timing information by name class Timing { @@ -310,8 +310,11 @@ boost::shared_ptr > ISAM2::recalculate(const li // insert conditionals back in, straight into the topless bayesTree typename BayesNet::const_reverse_iterator rit; - for ( rit=bayesNet->rbegin(); rit != bayesNet->rend(); ++rit ) + for ( rit=bayesNet->rbegin(); rit != bayesNet->rend(); ++rit ) { this->insert(*rit, index); + } + + lastNnzTop = calculate_nnz(this->root()); // Save number of affectedCliques lastAffectedCliqueCount = this->size(); @@ -319,7 +322,7 @@ boost::shared_ptr > ISAM2::recalculate(const li // 4. Insert the orphans back into the new Bayes tree. - tic("re-orphans"); + tic("re-orphan"); // add orphans to the bottom of the new tree BOOST_FOREACH(sharedClique orphan, orphans) { Symbol parentRepresentative = findParentClique(orphan->separator_, index); @@ -327,7 +330,7 @@ boost::shared_ptr > ISAM2::recalculate(const li parent->children_ += orphan; orphan->parent_ = parent; // set new parent! } - toc("re-orphans"); + toc("re-orphan"); // Output: BayesTree(this) @@ -375,9 +378,8 @@ void ISAM2::update( lastAffectedFactorCount = 0; lastAffectedCliqueCount = 0; lastAffectedMarkedCount = 0; - lastNonlinearMarkedCount = 0; - lastNonlinearAffectedVariableCount = 0; - lastNonlinearAffectedFactorCount = 0; + lastBacksubVariableCount = 0; + lastNnzTop = 0; tic("all"); @@ -405,7 +407,7 @@ void ISAM2::update( #endif VectorConfig deltaMarked; - if (relinearize && count%10 == 0) { // todo: every n steps + if (relinearize) { // && count%10 == 0) { // todo: every n steps tic("step4"); // 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. list markedRelin; @@ -462,11 +464,12 @@ void ISAM2::update( tic("step9"); // 9. Solve -// if (wildfire_threshold<=0.) { -// delta_ = *(optimize2(this->root())); -// } else { - optimize2(this->root(), wildfire_threshold, *replacedKeys, delta_); // modifies delta_ -// } + if (wildfire_threshold<=0.) { + delta_ = *(optimize2(this->root())); + lastBacksubVariableCount = theta_.size(); + } else { + lastBacksubVariableCount = optimize2(this->root(), wildfire_threshold, *replacedKeys, delta_); // modifies delta_ + } toc("step9"); toc("all"); diff --git a/inference/ISAM2.h b/inference/ISAM2.h index a83b3042e..6e79a7cbf 100644 --- a/inference/ISAM2.h +++ b/inference/ISAM2.h @@ -80,9 +80,8 @@ public: size_t lastAffectedFactorCount; size_t lastAffectedCliqueCount; size_t lastAffectedMarkedCount; - size_t lastNonlinearMarkedCount; - size_t lastNonlinearAffectedVariableCount; - size_t lastNonlinearAffectedFactorCount; + size_t lastBacksubVariableCount; + size_t lastNnzTop; private: diff --git a/slam/GaussianISAM2.cpp b/slam/GaussianISAM2.cpp index 70905d1a1..5ebb48eb8 100644 --- a/slam/GaussianISAM2.cpp +++ b/slam/GaussianISAM2.cpp @@ -19,7 +19,7 @@ namespace gtsam { /* ************************************************************************* */ void optimize2(const GaussianISAM2::sharedClique& clique, double threshold, - set& changed, const set& replaced, VectorConfig& delta) { + set& changed, const set& replaced, VectorConfig& delta, int& count) { // 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 @@ -47,6 +47,7 @@ void optimize2(const GaussianISAM2::sharedClique& clique, double threshold, if (found) { // Solve for that variable Vector d = cg->solve(delta); + count++; // 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 @@ -81,7 +82,7 @@ void optimize2(const GaussianISAM2::sharedClique& clique, double threshold, } if (process_children) { BOOST_FOREACH(const GaussianISAM2::sharedClique& child, clique->children_) { - optimize2(child, threshold, changed, replaced, delta); + optimize2(child, threshold, changed, replaced, delta, count); } } } @@ -112,10 +113,12 @@ boost::shared_ptr optimize2(const GaussianISAM2::sharedClique& roo } /* ************************************************************************* */ -void optimize2(const GaussianISAM2::sharedClique& root, double threshold, const set& keys, VectorConfig& delta) { +int optimize2(const GaussianISAM2::sharedClique& root, double threshold, const set& keys, VectorConfig& delta) { set changed; + int count = 0; // starting from the root, call optimize on each conditional - optimize2(root, threshold, changed, keys, delta); + optimize2(root, threshold, changed, keys, delta, count); + return count; } /* ************************************************************************* */ diff --git a/slam/GaussianISAM2.h b/slam/GaussianISAM2.h index d288dce5d..d55fd61c5 100644 --- a/slam/GaussianISAM2.h +++ b/slam/GaussianISAM2.h @@ -29,7 +29,8 @@ namespace gtsam { // 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, + // returns the number of variables that were solved for + int optimize2(const GaussianISAM2::sharedClique& root, double threshold, const std::set& replaced, VectorConfig& delta); // calculate the number of non-zero entries for the tree starting at clique (use root for complete matrix)