diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 5e71ee22b..f3b27b138 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -96,18 +96,16 @@ void ISAM2::Impl::RemoveVariables(const FastSet& unusedKeys, const ISAM2Cli VectorValues newDeltaNewton(dims); VectorValues newDeltaGradSearch(dims); std::vector newReplacedKeys(replacedKeys.size() - unusedIndices.size()); - Base::Nodes newNodes(nodes.size()); // We still keep unused keys at the end until later in ISAM2::recalculate + Base::Nodes newNodes(replacedKeys.size() - unusedIndices.size()); for(size_t j = 0; j < dims.size(); ++j) { newDelta[j] = delta[unusedToEnd[j]]; newDeltaNewton[j] = deltaNewton[unusedToEnd[j]]; newDeltaGradSearch[j] = deltaGradSearch[unusedToEnd[j]]; newReplacedKeys[j] = replacedKeys[unusedToEnd[j]]; + newNodes[j] = nodes[unusedToEnd[j]]; } - // Permute the nodes index so the unused variables are the end - unusedToEnd.applyToCollection(newNodes, nodes); - // Swap the new data structures with the outputs of this function delta.swap(newDelta); deltaNewton.swap(newDeltaNewton); diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 779880dab..a9bd195ba 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -203,8 +203,8 @@ GaussianFactorGraph ISAM2::getCachedBoundaryFactors(Cliques& orphans) { return cachedBoundary; } -boost::shared_ptr > ISAM2::recalculate( - const FastSet& markedKeys, const FastSet& relinKeys, const FastVector& observedKeys, +boost::shared_ptr > ISAM2::recalculate(const FastSet& markedKeys, + const FastSet& relinKeys, const FastVector& observedKeys, const FastSet& unusedIndices, const boost::optional >& constrainKeys, ISAM2Result& result) { // TODO: new factors are linearized twice, the newFactors passed in are not used. @@ -249,9 +249,6 @@ boost::shared_ptr > ISAM2::recalculate( this->removeTop(markedKeys, affectedBayesNet, orphans); toc(1, "removetop"); - // Now that the top is removed, correct the size of the Nodes index - this->nodes_.resize(delta_.size()); - if(debug) affectedBayesNet.print("Removed top: "); if(debug) orphans.print("Orphans: "); @@ -269,14 +266,6 @@ boost::shared_ptr > ISAM2::recalculate( // ordering provides all keys in conditionals, there cannot be others because path to root included tic(2,"affectedKeys"); FastList affectedKeys = affectedBayesNet.ordering(); - // The removed top also contained removed variables, which will be ordered - // higher than the number of variables in the system since unused variables - // were already removed in ISAM2::update. - for(FastList::iterator key = affectedKeys.begin(); key != affectedKeys.end(); ) - if(*key >= delta_.size()) - affectedKeys.erase(key++); - else - ++key; toc(2,"affectedKeys"); boost::shared_ptr > affectedKeysSet(new FastSet()); // Will return this result @@ -430,8 +419,12 @@ boost::shared_ptr > ISAM2::recalculate( reorderingMode.constrainedKeys = FastMap(); BOOST_FOREACH(Index var, observedKeys) { reorderingMode.constrainedKeys->insert(make_pair(var, 1)); } } + FastSet affectedUsedKeys = *affectedKeysSet; // Remove unused keys from the set we pass to PartialSolve + BOOST_FOREACH(Index unused, unusedIndices) { + affectedUsedKeys.erase(unused); + } Impl::PartialSolveResult partialSolveResult = - Impl::PartialSolve(factors, *affectedKeysSet, reorderingMode, (params_.factorization == ISAM2Params::QR)); + Impl::PartialSolve(factors, affectedUsedKeys, reorderingMode, (params_.factorization == ISAM2Params::QR)); toc(2,"PartialSolve"); // We now need to permute everything according this partial reordering: the @@ -559,50 +552,25 @@ ISAM2Result ISAM2::update( // Remove removed factors from the variable index so we do not attempt to relinearize them variableIndex_.remove(removeFactorIndices, *removeFactors.symbolic(ordering_)); - // We now need to start keeping track of the marked keys involved in added or - // removed factors. - FastSet markedKeys; - - // Remove unused keys and add keys from removed factors that are still used - // in other factors to markedKeys. + // Compute unused keys and indices + FastSet unusedKeys; + FastSet unusedIndices; { - // Get keys from removed factors - FastSet removedFactorSymbKeys = removeFactors.keys(); - - // For each key, if still used in other factors, add to markedKeys to be - // recalculated, or if not used, add to unusedKeys to be removed from the - // system. Note that unusedKeys stores Key while markedKeys stores Index. - FastSet unusedKeys; - BOOST_FOREACH(Key key, removedFactorSymbKeys) { - Index index = ordering_[key]; - if(variableIndex_[index].empty()) - unusedKeys.insert(key); + // Get keys from removed factors and new factors, and compute unused keys, + // i.e., keys that are empty now and do not appear in the new factors. + FastSet removedAndEmpty; + BOOST_FOREACH(Key key, removeFactors.keys()) { + if(variableIndex_[ordering_[key]].empty()) + removedAndEmpty.insert(removedAndEmpty.end(), key); } + FastSet newFactorSymbKeys = newFactors.keys(); + std::set_difference(removedAndEmpty.begin(), removedAndEmpty.end(), + newFactorSymbKeys.begin(), newFactorSymbKeys.end(), std::inserter(unusedKeys, unusedKeys.end())); - // Delete any keys from 'unusedKeys' that are actually used by the new, incoming factors - FastSet newNonlinearKeys = newFactors.keys(); - BOOST_FOREACH(Key key, newNonlinearKeys) { - FastSet::iterator iter = unusedKeys.find(key); - if(iter != unusedKeys.end()) { - unusedKeys.erase(iter); - } + // Get indices for unused keys + BOOST_FOREACH(Key key, unusedKeys) { + unusedIndices.insert(unusedIndices.end(), ordering_[key]); } - - // Remove unused keys. We must hold on to the new nodes index for now - // instead of placing it into the tree because removeTop will need to - // update it. - Impl::RemoveVariables(unusedKeys, root_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_, - deltaReplacedMask_, ordering_, Base::nodes_, linearFactors_); - - // Mark keys that are still in the use and are also included in the removed factors - // Note: The ordering has been modified during the RemoveVariables() function call. - // Hence, we do not create this list until after that call - BOOST_FOREACH(Key key, removedFactorSymbKeys) { - Index index; - if(ordering_.tryAt(key, index)) - markedKeys.insert(index); - } - } toc(1,"push_back factors"); @@ -622,10 +590,12 @@ ISAM2Result ISAM2::update( tic(4,"gather involved keys"); // 3. Mark linear update - { - FastSet newFactorIndices = Impl::IndicesFromFactors(ordering_, newFactors); // Get keys from new factors - markedKeys.insert(newFactorIndices.begin(), newFactorIndices.end()); - } + FastSet markedKeys = Impl::IndicesFromFactors(ordering_, newFactors); // Get keys from new factors + // Also mark keys involved in removed factors + { + FastSet markedRemoveKeys = Impl::IndicesFromFactors(ordering_, removeFactors); // Get keys involved in removed factors + markedKeys.insert(markedRemoveKeys.begin(), markedRemoveKeys.end()); // Add to the overall set of marked keys + } // Observed keys for detailed results if(params_.enableDetailedResults) { @@ -636,7 +606,11 @@ ISAM2Result ISAM2::update( // NOTE: we use assign instead of the iterator constructor here because this // is a vector of size_t, so the constructor unintentionally resolves to // vector(size_t count, Index value) instead of the iterator constructor. - FastVector observedKeys; observedKeys.assign(markedKeys.begin(), markedKeys.end()); // Make a copy of these, as we'll soon add to them + FastVector observedKeys; observedKeys.reserve(markedKeys.size()); + BOOST_FOREACH(Index index, markedKeys) { + if(unusedIndices.find(index) == unusedIndices.end()) // Only add if not unused + observedKeys.push_back(index); // Make a copy of these, as we'll soon add to them + } toc(4,"gather involved keys"); // Check relinearization if we're at the nth step, or we are using a looser loop relin threshold @@ -668,15 +642,6 @@ ISAM2Result ISAM2::update( // add other cliques that have the marked ones in the separator Impl::FindAll(this->root(), markedKeys, markedRelinMask); - // FindAll might have marked some removed keys because the removed keys - // are still in the old BayesTree structure - we remove them from - // markedKeys here. Note that the limit for removing indices that we use - // (delta_.size()) is after all of the new variables - so we do not - // remove indices that were removed but then recreated by adding new - // keys. - FastSet::iterator firstRemovedIndex = markedKeys.lower_bound(delta_.size()); - markedKeys.erase(firstRemovedIndex, markedKeys.end()); - // Relin involved keys for detailed results if(params_.enableDetailedResults) { FastSet involvedRelinKeys; @@ -738,7 +703,7 @@ ISAM2Result ISAM2::update( } boost::shared_ptr > replacedKeys; if(!markedKeys.empty() || !observedKeys.empty()) - replacedKeys = recalculate(markedKeys, relinKeys, observedKeys, constrainedIndices, result); + replacedKeys = recalculate(markedKeys, relinKeys, observedKeys, unusedIndices, constrainedIndices, result); // Update replaced keys mask (accumulates until back-substitution takes place) if(replacedKeys) { @@ -746,10 +711,11 @@ ISAM2Result ISAM2::update( deltaReplacedMask_[var] = true; } } toc(9,"recalculate"); - //tic(9,"solve"); - // 9. Solve - if(debug) delta_.print("delta_: "); - //toc(9,"solve"); + // After the top of the tree has been redone and may have index gaps from + // unused keys, condense the indices to remove gaps by rearranging indices + // in all data structures. + Impl::RemoveVariables(unusedKeys, root_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_, + deltaReplacedMask_, ordering_, Base::nodes_, linearFactors_); result.cliques = this->nodes().size(); deltaDoglegUptodate_ = false; diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 1fa893819..34fc7b46f 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -516,7 +516,7 @@ private: GaussianFactorGraph getCachedBoundaryFactors(Cliques& orphans); boost::shared_ptr > recalculate(const FastSet& markedKeys, const FastSet& relinKeys, - const FastVector& observedKeys, const boost::optional >& constrainKeys, ISAM2Result& result); + const FastVector& observedKeys, const FastSet& unusedIndices, const boost::optional >& constrainKeys, ISAM2Result& result); // void linear_update(const GaussianFactorGraph& newFactors); void updateDelta(bool forceFullSolve = false) const;