diff --git a/inference/ISAM2-inl.h b/inference/ISAM2-inl.h index a9ee5e58d..980d67d1f 100644 --- a/inference/ISAM2-inl.h +++ b/inference/ISAM2-inl.h @@ -212,7 +212,7 @@ FactorGraph ISAM2::getCachedBoundaryFactors /* ************************************************************************* */ template -void ISAM2::recalculate(const list& markedKeys, const FactorGraph* newFactors) { +boost::shared_ptr > ISAM2::recalculate(const list& markedKeys, const FactorGraph* newFactors) { // Input: BayesTree(this), newFactors @@ -330,6 +330,10 @@ void ISAM2::recalculate(const list& markedKeys, con toc("re-orphans"); // Output: BayesTree(this) + + boost::shared_ptr > affectedKeysSet(new set()); + affectedKeysSet->insert(affectedKeys.begin(), affectedKeys.end()); + return affectedKeysSet; } /* ************************************************************************* */ @@ -450,7 +454,7 @@ void ISAM2::update( tic("step8"); // 8. Redo top of Bayes tree - recalculate(markedKeys, &(*linearFactors)); + boost::shared_ptr > replacedKeys = recalculate(markedKeys, &(*linearFactors)); toc("step8"); #else recalculate(markedKeys); @@ -458,7 +462,11 @@ void ISAM2::update( tic("step9"); // 9. Solve - delta_ = optimize2(*this, wildfire_threshold); +// if (wildfire_threshold<=0.) { +// delta_ = *(optimize2(this->root())); +// } else { + optimize2(this->root(), wildfire_threshold, *replacedKeys, delta_); // modifies delta_ +// } toc("step9"); toc("all"); diff --git a/inference/ISAM2.h b/inference/ISAM2.h index 85202cb44..a83b3042e 100644 --- a/inference/ISAM2.h +++ b/inference/ISAM2.h @@ -72,7 +72,7 @@ public: const Config calculateEstimate() const {return theta_.expmap(delta_);} // estimate based on full delta (note that this is based on the current linearization point) - const Config calculateBestEstimate() const {return theta_.expmap(optimize2(*this, 0.));} + const Config calculateBestEstimate() const {return theta_.expmap(*optimize2(this->root()));} const NonlinearFactorGraph& getFactorsUnsafe() const { return nonlinearFactors_; } @@ -90,7 +90,7 @@ private: boost::shared_ptr relinearizeAffectedFactors(const std::list& affectedKeys) const; FactorGraph getCachedBoundaryFactors(Cliques& orphans); - void recalculate(const std::list& markedKeys, const FactorGraph* newFactors = NULL); + boost::shared_ptr > recalculate(const std::list& markedKeys, const FactorGraph* newFactors = NULL); void linear_update(const FactorGraph& newFactors); void find_all(sharedClique clique, std::list& keys, const std::list& marked); // helper function diff --git a/slam/GaussianISAM2.cpp b/slam/GaussianISAM2.cpp index 83556f840..70905d1a1 100644 --- a/slam/GaussianISAM2.cpp +++ b/slam/GaussianISAM2.cpp @@ -18,7 +18,8 @@ template class ISAM2; namespace gtsam { /* ************************************************************************* */ -void optimize2(const GaussianISAM2::sharedClique& clique, double threshold, set& changed, VectorConfig& result) { +void optimize2(const GaussianISAM2::sharedClique& clique, double threshold, + set& changed, const set& replaced, VectorConfig& delta) { // 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 @@ -29,10 +30,13 @@ void optimize2(const GaussianISAM2::sharedClique& clique, double threshold, set< for (it = clique->rbegin(); it!=clique->rend(); it++) { GaussianConditional::shared_ptr cg = *it; + // is this variable part of the top of the tree that has been redone? + bool redo = (replaced.find(cg->key()) != replaced.end()); + // 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) { + if (!redo && cg->nrParents()>0) { found = false; BOOST_FOREACH(const Symbol& key, cg->parents()) { if (changed.find(key)!=changed.end()) { @@ -42,66 +46,76 @@ void optimize2(const GaussianISAM2::sharedClique& clique, double threshold, set< } if (found) { // Solve for that variable - Vector x = cg->solve(result); + Vector d = cg->solve(delta); + // 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 + // can we be sure that the subtree is not affected 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; + // we change the delta unconditionally if redo, otherwise + // conditioned on the change being above the threshold + if (!redo) { + // change is measured against the previous delta! + if (delta.contains(cg->key())) { + Vector d_old = delta[cg->key()]; + if (max(abs(d-d_old)) >= threshold) { + redo = true; + } + } else { + redo = true; // never created before, so we simply add it + } } + + // replace current entry in delta vector + if (redo) { + changed.insert(cg->key()); + if (delta.contains(cg->key())) { + delta[cg->key()] = d; // replace existing entry + } else { + delta.insert(cg->key(), d); // insert new entry + } + } + } } if (process_children) { BOOST_FOREACH(const GaussianISAM2::sharedClique& child, clique->children_) { - optimize2(child, threshold, changed, result); + optimize2(child, threshold, changed, replaced, delta); } } } /* ************************************************************************* */ // fast full version without threshold -void optimize2(const GaussianISAM2::sharedClique& clique, VectorConfig& result) { +void optimize2(const GaussianISAM2::sharedClique& clique, boost::shared_ptr delta) { // 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); + Vector d = cg->solve(*delta); // store result in partial solution - result.insert(cg->key(), x); + delta->insert(cg->key(), d); } BOOST_FOREACH(const GaussianISAM2::sharedClique& child, clique->children_) { - optimize2(child, result); + optimize2(child, delta); } } /* ************************************************************************* */ -VectorConfig optimize2(const GaussianISAM2& bayesTree, double threshold) { - VectorConfig result; +boost::shared_ptr optimize2(const GaussianISAM2::sharedClique& root) { + boost::shared_ptr delta(new VectorConfig); set changed; // starting from the root, call optimize on each conditional - if (threshold<=0.) { - optimize2(bayesTree.root(), result); - } else { - optimize2(bayesTree.root(), threshold, changed, result); - } - return result; + optimize2(root, delta); + return delta; } /* ************************************************************************* */ -VectorConfig optimize2(const GaussianISAM2_P& bayesTree, double threshold) { - VectorConfig result; +void optimize2(const GaussianISAM2::sharedClique& root, double threshold, const set& keys, VectorConfig& delta) { set changed; // starting from the root, call optimize on each conditional - if (threshold<=0.) { - optimize2(bayesTree.root(), result); - } else { - optimize2(bayesTree.root(), threshold, changed, result); - } - return result; + optimize2(root, threshold, changed, keys, delta); } /* ************************************************************************* */ diff --git a/slam/GaussianISAM2.h b/slam/GaussianISAM2.h index c430b5a6e..d288dce5d 100644 --- a/slam/GaussianISAM2.h +++ b/slam/GaussianISAM2.h @@ -17,20 +17,20 @@ namespace gtsam { typedef ISAM2 GaussianISAM2; - - // optimize the BayesTree, starting from the root - VectorConfig optimize2(const GaussianISAM2& bayesTree, double threshold = 0.); - - - // todo: copy'n'paste to avoid template hell - typedef ISAM2 GaussianISAM2_P; - // recursively optimize this conditional and all subtrees -// void optimize2(const GaussianISAM2_P::sharedClique& clique, VectorConfig& result); - // optimize the BayesTree, starting from the root - VectorConfig optimize2(const GaussianISAM2_P& bayesTree, double threshold = 0.); + boost::shared_ptr optimize2(const GaussianISAM2::sharedClique& root); + + // optimize the BayesTree, starting from the root; "replaced" needs to contain + // all variables that are contained in the top of the Bayes tree that has been + // redone; "delta" is the current solution, an offset from the linearization + // point; "threshold" is the maximum change against the PREVIOUS delta for + // 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, + 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) int calculate_nnz(const GaussianISAM2::sharedClique& clique);