diff --git a/inference/ISAM2-inl.h b/inference/ISAM2-inl.h index 6bbae8314..4e4e28b50 100644 --- a/inference/ISAM2-inl.h +++ b/inference/ISAM2-inl.h @@ -19,7 +19,7 @@ using namespace boost::assign; #include -#if 1 // timing +#if 1 // timing - note: adds some time when applied in inner loops #include // simple class for accumulating execution timing information by name class Timing { @@ -236,10 +236,10 @@ namespace gtsam { // 1. Remove top of Bayes tree and convert to a factor graph: // (a) For each affected variable, remove the corresponding clique and all parents up to the root. // (b) Store orphaned sub-trees \BayesTree_{O} of removed cliques. - const list newKeys = newFactors.keys(); + const list markedKeys = newFactors.keys(); Cliques orphans; BayesNet affectedBayesNet; - this->removeTop(newKeys, affectedBayesNet, orphans); + this->removeTop(markedKeys, affectedBayesNet, orphans); // FactorGraph factors(affectedBayesNet); // bug was here: we cannot reuse the original factors, because then the cached factors get messed up @@ -258,16 +258,15 @@ namespace gtsam { list tmp = affectedBayesNet.ordering(); affectedKeys.insert(tmp.begin(), tmp.end()); - // Save number of affected variables - lastAffectedVariableCount = affectedKeys.size(); - FactorGraph factors(*relinearizeAffectedFactors(affectedKeys)); // todo: no need to relinearize here, should have cached linearized factors - // Save number of affected factors + lastAffectedMarkedCount = markedKeys.size(); + lastAffectedVariableCount = affectedKeys.size(); lastAffectedFactorCount = factors.size(); - // output for generating figures + #ifdef PRINT_STATS - cout << "linear: #newKeys: " << newKeys.size() << " #affectedVariables: " << affectedKeys.size() + // output for generating figures + cout << "linear: #markedKeys: " << markedKeys.size() << " #affectedVariables: " << affectedKeys.size() << " #affectedFactors: " << factors.size() << " maxCliqueSize: " << maxClique << " avgCliqueSize: " << avgClique << " #Cliques: " << numCliques << " nnzR: " << nnzR << endl; #endif @@ -287,10 +286,10 @@ namespace gtsam { // 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]) // create an ordering for the new and contaminated factors - // newKeys are passed in: those variables will be forced to the end in the ordering - set newKeysSet; - newKeysSet.insert(newKeys.begin(), newKeys.end()); - Ordering ordering = factors.getConstrainedOrdering(newKeysSet); // intelligent ordering + // markedKeys are passed in: those variables will be forced to the end in the ordering + set markedKeysSet; + markedKeysSet.insert(markedKeys.begin(), markedKeys.end()); + Ordering ordering = factors.getConstrainedOrdering(markedKeysSet); // intelligent ordering // Ordering ordering = factors.getOrdering(); // original ordering, yields in bad performance // eliminate into a Bayes net @@ -401,7 +400,11 @@ namespace gtsam { factors = relinearizeAffectedFactors(affectedKeys); toc("nonlin_relin"); - cout << "nonlinear: #marked: " << marked.size() << " #affected: " << affectedKeys.size() << " #factors: " << factors->size() << endl; + lastNonlinearMarkedCount = marked.size(); + lastNonlinearAffectedVariableCount = affectedKeys.size(); + lastNonlinearAffectedFactorCount = factors->size(); + +// cout << "nonlinear: #marked: " << marked.size() << " #affected: " << affectedKeys.size() << " #factors: " << factors->size() << endl; // add the cached intermediate results from the boundary of the orphans ... FactorGraph cachedBoundary = getCachedBoundaryFactors(orphans); @@ -438,6 +441,14 @@ namespace gtsam { const NonlinearFactorGraph& newFactors, const Config& newTheta, double wildfire_threshold, double relinearize_threshold, bool relinearize) { + lastAffectedVariableCount = 0; + lastAffectedFactorCount = 0; + lastAffectedCliqueCount = 0; + lastAffectedMarkedCount = 0; + lastNonlinearMarkedCount = 0; + lastNonlinearAffectedVariableCount = 0; + lastNonlinearAffectedFactorCount = 0; + tic("all"); tic("step1"); @@ -473,7 +484,7 @@ namespace gtsam { toc("step6"); // todo: not part of algorithm in paper: linearization point and delta_ do not fit... have to update delta again -//todo delta_ = optimize2(*this, wildfire_threshold); + delta_ = optimize2(*this, wildfire_threshold); toc("all"); diff --git a/inference/ISAM2.h b/inference/ISAM2.h index 3b1d2875e..d842d4d0c 100644 --- a/inference/ISAM2.h +++ b/inference/ISAM2.h @@ -79,6 +79,10 @@ namespace gtsam { size_t lastAffectedVariableCount; size_t lastAffectedFactorCount; size_t lastAffectedCliqueCount; + size_t lastAffectedMarkedCount; + size_t lastNonlinearMarkedCount; + size_t lastNonlinearAffectedVariableCount; + size_t lastNonlinearAffectedFactorCount; private: diff --git a/slam/GaussianISAM2.cpp b/slam/GaussianISAM2.cpp index 67396eb29..fdb65b671 100644 --- a/slam/GaussianISAM2.cpp +++ b/slam/GaussianISAM2.cpp @@ -18,38 +18,89 @@ template class ISAM2; namespace gtsam { /* ************************************************************************* */ -void optimize2(const GaussianISAM2::sharedClique& clique, double threshold, VectorConfig& result) { +void optimize2(const GaussianISAM2::sharedClique& clique, double threshold, set& changed, VectorConfig& result) { + // 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 bool process_children = false; + // 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); // Solve for that variable - if (max(abs(x)) >= threshold) { - process_children = true; - } - result.insert(cg->key(), x); // store result in partial solution - } + + // 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) { + found = false; + BOOST_FOREACH(const Symbol& key, cg->parents()) { + if (changed.find(key)!=changed.end()) { + found = true; + } + } + } + if (found) { + // Solve for that variable + Vector x = cg->solve(result); + 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; + } + } + } if (process_children) { BOOST_FOREACH(const GaussianISAM2::sharedClique& child, clique->children_) { - optimize2(child, threshold, result); + optimize2(child, threshold, changed, result); } } } +/* ************************************************************************* */ +// fast version without threshold +void optimize2(const GaussianISAM2::sharedClique& clique, VectorConfig& result) { + // 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); + // store result in partial solution + result.insert(cg->key(), x); + } + BOOST_FOREACH(const GaussianISAM2::sharedClique& child, clique->children_) { + optimize2(child, result); + } +} + /* ************************************************************************* */ VectorConfig optimize2(const GaussianISAM2& bayesTree, double threshold) { VectorConfig result; + set changed; // starting from the root, call optimize on each conditional - optimize2(bayesTree.root(), threshold, result); + if (threshold<=0.) { + optimize2(bayesTree.root(), result); + } else { + optimize2(bayesTree.root(), threshold, changed, result); + } return result; } /* ************************************************************************* */ VectorConfig optimize2(const GaussianISAM2_P& bayesTree, double threshold) { VectorConfig result; + set changed; // starting from the root, call optimize on each conditional - optimize2(bayesTree.root(), threshold, result); + if (threshold<=0.) { + optimize2(bayesTree.root(), result); + } else { + optimize2(bayesTree.root(), threshold, changed, result); + } return result; } @@ -64,8 +115,8 @@ void nnz_internal(const GaussianISAM2::sharedClique& clique, int& result) { dimSep += matrix_it->second.size2(); } int dimR = cg->dim(); - result += (dimR+1)*dimR/2 + dimSep*dimR; - } + result += ((dimR+1)*dimR)/2 + dimSep*dimR; + } // traverse the children BOOST_FOREACH(const GaussianISAM2::sharedClique& child, clique->children_) { nnz_internal(child, result); diff --git a/slam/GaussianISAM2.h b/slam/GaussianISAM2.h index b8b52bd28..c430b5a6e 100644 --- a/slam/GaussianISAM2.h +++ b/slam/GaussianISAM2.h @@ -18,9 +18,6 @@ namespace gtsam { typedef ISAM2 GaussianISAM2; - // recursively optimize this conditional and all subtrees - void optimize2(const GaussianISAM2::sharedClique& clique, double threshold, VectorConfig& result); - // optimize the BayesTree, starting from the root VectorConfig optimize2(const GaussianISAM2& bayesTree, double threshold = 0.);