From 1e5f9c742d49d99c23fa5b582e3c02754b19758a Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Wed, 22 May 2013 00:05:03 +0000 Subject: [PATCH] Revert "Modified Concurrent Filter to calculate marginals using a "shortcut" that allows constant-time updates during synchronization. Still need to test implementation." This reverts commit f24a4f4668006cfe9a3eeb1658b7df03c74490d5. --- .../nonlinear/ConcurrentBatchFilter.cpp | 621 ++++++------------ .../nonlinear/ConcurrentBatchFilter.h | 20 +- 2 files changed, 191 insertions(+), 450 deletions(-) diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp index d8dcd6c14..1e1e39e10 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp @@ -85,11 +85,11 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::update(const NonlinearFacto } gttoc(optimize); - gttic(move_separator); + gttic(marginalize); if(keysToMove && keysToMove->size() > 0){ - moveSeparator(*keysToMove); + marginalize(*keysToMove); } - gttoc(move_separator); + gttoc(marginalize); gttoc(update); @@ -109,169 +109,100 @@ void ConcurrentBatchFilter::synchronize(const NonlinearFactorGraph& summarizedFa gttic(synchronize); - // Update the smoother summarization on the old separator - previousSmootherSummarization_ = summarizedFactors; + // Remove the previous smoother summarization + removeFactors(smootherSummarizationSlots_); - // Use the shortcut to calculate an updated marginal on the current separator - { - // Combine the new smoother summarization and the existing shortcut - NonlinearFactorGraph graph; - graph.push_back(previousSmootherSummarization_); - graph.push_back(smootherShortcut_); + // Create a factor graph containing the new smoother summarization, the factors to be sent to the smoother, + // and all of the filter factors. This is the set of factors on the filter side since the smoother started + // its previous update cycle. + NonlinearFactorGraph graph; + graph.push_back(factors_); + graph.push_back(smootherFactors_); + graph.push_back(summarizedFactors); + Values values; + values.insert(theta_); + values.insert(smootherValues_); + values.update(separatorValues); // ensure the smoother summarized factors are linearized around the values in the smoother - // Extract the values needed for just this graph - Values values; - BOOST_FOREACH(Key key, graph.keys()) { - values.insert(key, theta_.at(key)); - } - - // Calculate the ordering: [OldSeparator NewSeparator] - // And determine the set of variables to be marginalized out - std::map constraints; - std::set marginalizeKeys; - int group = 0; - if(values.size() > 0) { - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, values) { - constraints[key_value.key] = group; - marginalizeKeys.insert(key_value.key); - } - ++group; - } - if(separatorValues_.size() > 0) { - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { - constraints[key_value.key] = group; - marginalizeKeys.erase(key_value.key); - } - } - Ordering ordering = *graph.orderingCOLAMDConstrained(values, constraints); - - // Calculate the marginal on the new separator (from the smoother side) - NonlinearFactorGraph marginals = marginalize(graph, values, ordering, marginalizeKeys, parameters_.getEliminationFunction()); - - // Remove old summarization and insert new - removeFactors(currentSmootherSummarizationSlots_); - currentSmootherSummarizationSlots_ = insertFactors(marginals); - } - - // Calculate the marginal on the new separator from the filter factors - // Note: This could also be done during each filter update so it would simply be available - { - // Calculate an ordering that places the new separator at the root - // And determine the set of variables to be marginalized out - std::map constraints; - std::set marginalizeKeys; - int group = 0; - if(theta_.size() > 0) { + if(factors_.size() > 0) { + // Perform an optional optimization on the to-be-sent-to-the-smoother factors + if(relin_) { + // Create ordering and delta + Ordering ordering = *graph.orderingCOLAMD(values); + VectorValues delta = values.zeroVectors(ordering); + // Optimize this graph using a modified version of L-M + optimize(graph, values, ordering, delta, separatorValues, parameters_); + // Update filter theta and delta BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, theta_) { - constraints[key_value.key] = group; - marginalizeKeys.insert(key_value.key); + theta_.update(key_value.key, values.at(key_value.key)); + delta_.at(ordering_.at(key_value.key)) = delta.at(ordering.at(key_value.key)); } - ++group; - } - if(separatorValues_.size() > 0) { + // Update the fixed linearization points (since they just changed) BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { - constraints[key_value.key] = group; - marginalizeKeys.erase(key_value.key); + separatorValues_.update(key_value.key, values.at(key_value.key)); } } - Ordering ordering = *factors_.orderingCOLAMDConstrained(theta_, constraints); - // Calculate the marginal on the new separator (from the filter side) - filterSummarization_ = marginalize(factors_, theta_, ordering, marginalizeKeys, parameters_.getEliminationFunction()); + // Create separate ordering constraints that force either the filter keys or the smoother keys to the front + typedef std::map OrderingConstraints; + OrderingConstraints filterConstraints; + OrderingConstraints smootherConstraints; + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, theta_) { /// the filter keys + filterConstraints[key_value.key] = 0; + smootherConstraints[key_value.key] = 1; + } + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, smootherValues_) { /// the smoother keys + filterConstraints[key_value.key] = 1; + smootherConstraints[key_value.key] = 0; + } + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { /// the *new* separator keys + filterConstraints[key_value.key] = 2; + smootherConstraints[key_value.key] = 2; + } + + // Generate separate orderings that place the filter keys or the smoother keys first + // TODO: This is convenient, but it recalculates the variable index each time + Ordering filterOrdering = *graph.orderingCOLAMDConstrained(values, filterConstraints); + Ordering smootherOrdering = *graph.orderingCOLAMDConstrained(values, smootherConstraints); + + // Extract the set of filter keys and smoother keys + std::set filterKeys; + std::set separatorKeys; + std::set smootherKeys; + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, theta_) { + filterKeys.insert(key_value.key); + } + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { + separatorKeys.insert(key_value.key); + filterKeys.erase(key_value.key); + } + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, smootherValues_) { + smootherKeys.insert(key_value.key); + } + + // Calculate the marginal on the new separator from the filter factors. This is performed by marginalizing out + // all of the filter variables that are not part of the new separator. This filter summarization will then be + // sent to the smoother. + filterSummarization_ = marginalize(graph, values, filterOrdering, filterKeys, parameters_.getEliminationFunction()); + // The filter summarization should also include any nonlinear factors that involve only the separator variables. + // Otherwise the smoother will be missing this information + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, factors_) { + if(factor) { + NonlinearFactor::const_iterator key = factor->begin(); + while((key != factor->end()) && (std::binary_search(separatorKeys.begin(), separatorKeys.end(), *key))) { + ++key; + } + if(key == factor->end()) { + filterSummarization_.push_back(factor); + } + } + } + + // Calculate the marginal on the new separator from the smoother factors. This is performed by marginalizing out + // all of the smoother variables that are not part of the new separator. This smoother summarization will be + // stored locally for use in the filter + smootherSummarizationSlots_ = insertFactors( marginalize(graph, values, smootherOrdering, smootherKeys, parameters_.getEliminationFunction()) ); } - -// // Create a factor graph containing the new smoother summarization, the factors to be sent to the smoother, -// // and all of the filter factors. This is the set of factors on the filter side since the smoother started -// // its previous update cycle. -// NonlinearFactorGraph graph; -// graph.push_back(factors_); -// graph.push_back(smootherFactors_); -// graph.push_back(summarizedFactors); -// Values values; -// values.insert(theta_); -// values.insert(smootherValues_); -// values.update(separatorValues); // ensure the smoother summarized factors are linearized around the values in the smoother -// -// if(factors_.size() > 0) { -// // Perform an optional optimization on the to-be-sent-to-the-smoother factors -// if(true) { -// // Create ordering and delta -// Ordering ordering = *graph.orderingCOLAMD(values); -// VectorValues delta = values.zeroVectors(ordering); -// // Optimize this graph using a modified version of L-M -// optimize(graph, values, ordering, delta, separatorValues, parameters_); -// // Update filter theta and delta -// BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, theta_) { -// theta_.update(key_value.key, values.at(key_value.key)); -// delta_.at(ordering_.at(key_value.key)) = delta.at(ordering.at(key_value.key)); -// } -// // Update the fixed linearization points (since they just changed) -// BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { -// separatorValues_.update(key_value.key, values.at(key_value.key)); -// } -// } -// -// // Create separate ordering constraints that force either the filter keys or the smoother keys to the front -// typedef std::map OrderingConstraints; -// OrderingConstraints filterConstraints; -// OrderingConstraints smootherConstraints; -// BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, theta_) { /// the filter keys -// filterConstraints[key_value.key] = 0; -// smootherConstraints[key_value.key] = 1; -// } -// BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, smootherValues_) { /// the smoother keys -// filterConstraints[key_value.key] = 1; -// smootherConstraints[key_value.key] = 0; -// } -// BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { /// the *new* separator keys -// filterConstraints[key_value.key] = 2; -// smootherConstraints[key_value.key] = 2; -// } -// -// // Generate separate orderings that place the filter keys or the smoother keys first -// // TODO: This is convenient, but it recalculates the variable index each time -// Ordering filterOrdering = *graph.orderingCOLAMDConstrained(values, filterConstraints); -// Ordering smootherOrdering = *graph.orderingCOLAMDConstrained(values, smootherConstraints); -// -// // Extract the set of filter keys and smoother keys -// std::set filterKeys; -// std::set separatorKeys; -// std::set smootherKeys; -// BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, theta_) { -// filterKeys.insert(key_value.key); -// } -// BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { -// separatorKeys.insert(key_value.key); -// filterKeys.erase(key_value.key); -// } -// BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, smootherValues_) { -// smootherKeys.insert(key_value.key); -// } -// -// // Calculate the marginal on the new separator from the filter factors. This is performed by marginalizing out -// // all of the filter variables that are not part of the new separator. This filter summarization will then be -// // sent to the smoother. -// filterSummarization_ = marginalize(graph, values, filterOrdering, filterKeys, parameters_.getEliminationFunction()); -// // The filter summarization should also include any nonlinear factors that involve only the separator variables. -// // Otherwise the smoother will be missing this information -// BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, factors_) { -// if(factor) { -// NonlinearFactor::const_iterator key = factor->begin(); -// while((key != factor->end()) && (std::binary_search(separatorKeys.begin(), separatorKeys.end(), *key))) { -// ++key; -// } -// if(key == factor->end()) { -// filterSummarization_.push_back(factor); -// } -// } -// } -// -// // Calculate the marginal on the new separator from the smoother factors. This is performed by marginalizing out -// // all of the smoother variables that are not part of the new separator. This smoother summarization will be -// // stored locally for use in the filter -// smootherSummarizationSlots_ = insertFactors( marginalize(graph, values, smootherOrdering, smootherKeys, parameters_.getEliminationFunction()) ); -// } - gttoc(synchronize); } @@ -499,301 +430,117 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::optimize(const NonlinearFac } /* ************************************************************************* */ -void ConcurrentBatchFilter::moveSeparator(const FastList& keysToMove) { - // In order to move the separator, we need to calculate the marginal information on the new - // separator from all of the factors on the smoother side (both the factors actually in the - // smoother and the ones to be transitioned to the smoother but stored in the filter). - // This is exactly the same operation that is performed by a fixed-lag smoother, calculating - // a marginal factor from the variables outside the smoother window. - // - // However, for the concurrent system, we would like to calculate this marginal in a particular - // way, such that an intermediate term is produced that provides a "shortcut" between the old - // separator (as defined in the smoother) and the new separator. This will allow us to quickly - // update the new separator with changes at the old separator (from the smoother) - - // TODO: This is currently not very efficient: multiple calls to graph.keys(), etc. +void ConcurrentBatchFilter::marginalize(const FastList& keysToMove) { + // In order to marginalize out the selected variables, the factors involved in those variables + // must be identified and removed. Also, the effect of those removed factors on the + // remaining variables needs to be accounted for. This will be done with linear container factors + // from the result of a partial elimination. This function removes the marginalized factors and + // adds the linearized factors back in. - // Identify all of the new factors to be sent to the smoother (any factor involving keysToMove) - std::vector removedFactorSlots; - VariableIndex variableIndex(*factors_.symbolic(ordering_), theta_.size()); + // Calculate marginal factors on the remaining variables (after marginalizing 'keyToMove') + // Note: It is assumed the ordering already has these keys first + + // Create the linear factor graph + GaussianFactorGraph linearFactorGraph = *factors_.linearize(theta_, ordering_); + + // Calculate the variable index + VariableIndex variableIndex(linearFactorGraph, ordering_.size()); + + // Use the variable Index to mark the factors that will be marginalized + std::set removedFactorSlots; BOOST_FOREACH(Key key, keysToMove) { const FastList& slots = variableIndex[ordering_.at(key)]; - removedFactorSlots.insert(removedFactorSlots.end(), slots.begin(), slots.end()); - } - // Sort and remove duplicates - std::sort(removedFactorSlots.begin(), removedFactorSlots.end()); - removedFactorSlots.erase(std::unique(removedFactorSlots.begin(), removedFactorSlots.end()), removedFactorSlots.end()); - // Remove any linear,marginal factor that made it into the set - BOOST_FOREACH(size_t index, currentSmootherSummarizationSlots_) { - removedFactorSlots.erase(std::remove(removedFactorSlots.begin(), removedFactorSlots.end(), index), removedFactorSlots.end()); + removedFactorSlots.insert(slots.begin(), slots.end()); } - // Add these factors to a factor graph - NonlinearFactorGraph removedFactors; - BOOST_FOREACH(size_t slot, removedFactorSlots) { - if(factors_.at(slot)) { - removedFactors.push_back(factors_.at(slot)); - } - } + // Construct an elimination tree to perform sparse elimination + std::vector forest( EliminationForest::Create(linearFactorGraph, variableIndex) ); - // Combine the previous shortcut factor with all of the new factors being sent to the smoother - NonlinearFactorGraph graph; - graph.push_back(removedFactors); - graph.push_back(smootherShortcut_); - - // Extract just the values needed for the current marginalization - // and the set all keys - Values values; - BOOST_FOREACH(Key key, graph.keys()) { - values.insert(key, theta_.at(key)); - } - - // Calculate the set of new separator keys (AllAffectedKeys - KeysToMove) - FastSet newSeparatorKeys = removedFactors.keys(); + // This is a tree. Only the top-most nodes/indices need to be eliminated; all of the children will be eliminated automatically + // Find the subset of nodes/keys that must be eliminated + std::set indicesToEliminate; BOOST_FOREACH(Key key, keysToMove) { - newSeparatorKeys.erase(key); + indicesToEliminate.insert(ordering_.at(key)); } - - // Calculate the set of old set of separator keys from the previous summarization factors - FastSet oldSeparatorKeys = previousSmootherSummarization_.keys(); - BOOST_FOREACH(Key key, newSeparatorKeys) { - oldSeparatorKeys.erase(key); - } - - // Calculate the set of OtherKeys = AllKeys - OldSeparator - NewSeparator - FastSet otherKeys(graph.keys()); - BOOST_FOREACH(Key key, oldSeparatorKeys) { - otherKeys.erase(key); - } - BOOST_FOREACH(Key key, newSeparatorKeys) { - otherKeys.erase(key); - } - - // Calculate the ordering: [Others OldSeparator NewSeparator] - typedef std::map OrderingConstraints; - OrderingConstraints constraints; - int group = 0; - if(otherKeys.size() > 0) { - BOOST_FOREACH(Key key, otherKeys) { - constraints[key] = group; - } - ++group; - } - if(oldSeparatorKeys.size() > 0) { - BOOST_FOREACH(Key key, oldSeparatorKeys) { - constraints[key] = group; - } - ++group; - } - if(newSeparatorKeys.size() > 0) { - BOOST_FOREACH(Key key, newSeparatorKeys) { - constraints[key] = group; - } - } - Ordering ordering = *graph.orderingCOLAMDConstrained(values, constraints); - - // Calculate the new shortcut marginal on the OldSeparator + NewSeparator - smootherShortcut_ = marginalize(graph, values, ordering, otherKeys, parameters_.getEliminationFunction()); - - // Combine the old smoother summarization and the new shortcut - graph = NonlinearFactorGraph(); - graph.push_back(previousSmootherSummarization_); - graph.push_back(smootherShortcut_); - - // Extract the values needed for just this graph - values = Values(); - BOOST_FOREACH(Key key, graph.keys()) { - values.insert(key, theta_.at(key)); - } - - // Calculate the ordering: [OldSeparator NewSeparator] - constraints = OrderingConstraints(); - group = 0; - if(oldSeparatorKeys.size() > 0) { - BOOST_FOREACH(Key key, oldSeparatorKeys) { - constraints[key] = group; - } - ++group; - } - if(newSeparatorKeys.size() > 0) { - BOOST_FOREACH(Key key, newSeparatorKeys) { - constraints[key] = group; - } - } - ordering = *graph.orderingCOLAMDConstrained(values, constraints); - - // Calculate the marginal on the new separator - NonlinearFactorGraph marginals = marginalize(graph, values, ordering, oldSeparatorKeys, parameters_.getEliminationFunction()); - - // Remove the previous marginal factors and insert the new marginal factors - removeFactors(currentSmootherSummarizationSlots_); - currentSmootherSummarizationSlots_ = insertFactors(marginals); - - // Update the separatorValues object (should only contain the new separator keys) - separatorValues_.clear(); - BOOST_FOREACH(Key key, marginals.keys()) { - separatorValues_.insert(key, theta_.at(key)); - } - - // Remove the marginalized factors and add them to the smoother cache - smootherFactors_.push_back(removedFactors); - removeFactors(removedFactorSlots); - - // Add the linearization point of the moved variables to the smoother cache BOOST_FOREACH(Key key, keysToMove) { - smootherValues_.insert(key, theta_.at(key)); + EliminationForest::removeChildrenIndices(indicesToEliminate, forest.at(ordering_.at(key))); } - // Remove marginalized keys from values (and separator) - BOOST_FOREACH(Key key, keysToMove) { - theta_.erase(key); + // Eliminate each top-most key, returning a Gaussian Factor on some of the remaining variables + // Convert the marginal factors into Linear Container Factors + // Add the marginal factor variables to the separator + NonlinearFactorGraph marginalFactors; + BOOST_FOREACH(Index index, indicesToEliminate) { + GaussianFactor::shared_ptr gaussianFactor = forest.at(index)->eliminateRecursive(parameters_.getEliminationFunction()); + if(gaussianFactor->size() > 0) { + LinearContainerFactor::shared_ptr marginalFactor(new LinearContainerFactor(gaussianFactor, ordering_, theta_)); + marginalFactors.push_back(marginalFactor); + // Add the keys associated with the marginal factor to the separator values + BOOST_FOREACH(Key key, *marginalFactor) { + if(!separatorValues_.exists(key)) { + separatorValues_.insert(key, theta_.at(key)); + } + } + } + } + std::vector marginalSlots = insertFactors(marginalFactors); + + + // Cache marginalized variables and factors for later transmission to the smoother + { + // Add the new marginal factors to the list of smootherSeparatorFactors. In essence, we have just moved the separator + smootherSummarizationSlots_.insert(smootherSummarizationSlots_.end(), marginalSlots.begin(), marginalSlots.end()); + + // Move the marginalized factors from the filter to the smoother (holding area) + // Note: Be careful to only move nonlinear factors and not any marginals that may also need to be removed + BOOST_FOREACH(size_t slot, removedFactorSlots) { + std::vector::iterator iter = std::find(smootherSummarizationSlots_.begin(), smootherSummarizationSlots_.end(), slot); + if(iter == smootherSummarizationSlots_.end()) { + // This is a real nonlinear factor. Add it to the smoother factor cache. + smootherFactors_.push_back(factors_.at(slot)); + } else { + // This is a marginal factor that was removed and replaced by a new marginal factor. Remove this slot from the separator factor list. + smootherSummarizationSlots_.erase(iter); + } + } + + // Add the linearization point of the moved variables to the smoother cache + BOOST_FOREACH(Key key, keysToMove) { + smootherValues_.insert(key, theta_.at(key)); + } } - // Permute the ordering such that the removed keys are at the end. - // This is a prerequisite for removing them from several structures - std::vector toBack; - BOOST_FOREACH(Key key, keysToMove) { - toBack.push_back(ordering_.at(key)); + // Remove the marginalized variables and factors from the filter + { + // Remove marginalized factors from the factor graph + std::vector slots(removedFactorSlots.begin(), removedFactorSlots.end()); + removeFactors(slots); + + // Remove marginalized keys from values (and separator) + BOOST_FOREACH(Key key, keysToMove) { + theta_.erase(key); + if(separatorValues_.exists(key)) { + separatorValues_.erase(key); + } + } + + // Permute the ordering such that the removed keys are at the end. + // This is a prerequisite for removing them from several structures + std::vector toBack; + BOOST_FOREACH(Key key, keysToMove) { + toBack.push_back(ordering_.at(key)); + } + Permutation forwardPermutation = Permutation::PushToBack(toBack, ordering_.size()); + ordering_.permuteInPlace(forwardPermutation); + delta_.permuteInPlace(forwardPermutation); + + // Remove marginalized keys from the ordering and delta + for(size_t i = 0; i < keysToMove.size(); ++i) { + ordering_.pop_back(); + delta_.pop_back(); + } } - Permutation forwardPermutation = Permutation::PushToBack(toBack, ordering_.size()); - ordering_.permuteInPlace(forwardPermutation); - delta_.permuteInPlace(forwardPermutation); - - // Remove marginalized keys from the ordering and delta - for(size_t i = 0; i < keysToMove.size(); ++i) { - ordering_.pop_back(); - delta_.pop_back(); - } - - -// // Calculate marginal factors on the remaining variables (after marginalizing 'keyToMove') -// // Note: It is assumed the ordering already has these keys first -// -// // text -// NonlinearFactorGraph marginalFactors = marginalize(factors_, theta_, ordering_, keysToMove, parameters_.getEliminationFunction()); -// -// // text -// BOOST_FOREACH(const NonlinearFactor::shared_ptr& marginalFactor, marginalFactors) { -// BOOST_FOREACH(Key key, *marginalFactor) { -// if(!separatorValues_.exists(key)) { -// separatorValues_.insert(key, theta_.at(key)); -// } -// } -// } -// std::vector marginalSlots = insertFactors(marginalFactors); -// -// // text -// // Use the variable Index to mark the factors that will be marginalized -// std::set removedFactorSlots; -// BOOST_FOREACH(Key key, keysToMove) { -// const FastList& slots = variableIndex[ordering_.at(key)]; -// removedFactorSlots.insert(slots.begin(), slots.end()); -// } -// -// -// -// -// // Create the linear factor graph -// GaussianFactorGraph linearFactorGraph = *factors_.linearize(theta_, ordering_); -// -// // Calculate the variable index -// VariableIndex variableIndex(linearFactorGraph, ordering_.size()); -// -// // Use the variable Index to mark the factors that will be marginalized -// std::set removedFactorSlots; -// BOOST_FOREACH(Key key, keysToMove) { -// const FastList& slots = variableIndex[ordering_.at(key)]; -// removedFactorSlots.insert(slots.begin(), slots.end()); -// } -// -// // Construct an elimination tree to perform sparse elimination -// std::vector forest( EliminationForest::Create(linearFactorGraph, variableIndex) ); -// -// // This is a tree. Only the top-most nodes/indices need to be eliminated; all of the children will be eliminated automatically -// // Find the subset of nodes/keys that must be eliminated -// std::set indicesToEliminate; -// BOOST_FOREACH(Key key, keysToMove) { -// indicesToEliminate.insert(ordering_.at(key)); -// } -// BOOST_FOREACH(Key key, keysToMove) { -// EliminationForest::removeChildrenIndices(indicesToEliminate, forest.at(ordering_.at(key))); -// } -// -// // Eliminate each top-most key, returning a Gaussian Factor on some of the remaining variables -// // Convert the marginal factors into Linear Container Factors -// // Add the marginal factor variables to the separator -// NonlinearFactorGraph marginalFactors; -// BOOST_FOREACH(Index index, indicesToEliminate) { -// GaussianFactor::shared_ptr gaussianFactor = forest.at(index)->eliminateRecursive(parameters_.getEliminationFunction()); -// if(gaussianFactor->size() > 0) { -// LinearContainerFactor::shared_ptr marginalFactor(new LinearContainerFactor(gaussianFactor, ordering_, theta_)); -// marginalFactors.push_back(marginalFactor); -// // Add the keys associated with the marginal factor to the separator values -// BOOST_FOREACH(Key key, *marginalFactor) { -// if(!separatorValues_.exists(key)) { -// separatorValues_.insert(key, theta_.at(key)); -// } -// } -// } -// } -// std::vector marginalSlots = insertFactors(marginalFactors); -// -// -// // Cache marginalized variables and factors for later transmission to the smoother -// { -// // Add the new marginal factors to the list of smootherSeparatorFactors. In essence, we have just moved the separator -// smootherSummarizationSlots_.insert(smootherSummarizationSlots_.end(), marginalSlots.begin(), marginalSlots.end()); -// -// // Move the marginalized factors from the filter to the smoother (holding area) -// // Note: Be careful to only move nonlinear factors and not any marginals that may also need to be removed -// BOOST_FOREACH(size_t slot, removedFactorSlots) { -// std::vector::iterator iter = std::find(smootherSummarizationSlots_.begin(), smootherSummarizationSlots_.end(), slot); -// if(iter == smootherSummarizationSlots_.end()) { -// // This is a real nonlinear factor. Add it to the smoother factor cache. -// smootherFactors_.push_back(factors_.at(slot)); -// } else { -// // This is a marginal factor that was removed and replaced by a new marginal factor. Remove this slot from the smoother summarization list. -// smootherSummarizationSlots_.erase(iter); -// } -// } -// -// // Add the linearization point of the moved variables to the smoother cache -// BOOST_FOREACH(Key key, keysToMove) { -// smootherValues_.insert(key, theta_.at(key)); -// } -// } -// -// // Remove the marginalized variables and factors from the filter -// { -// // Remove marginalized factors from the factor graph -// std::vector slots(removedFactorSlots.begin(), removedFactorSlots.end()); -// removeFactors(slots); -// -// // Remove marginalized keys from values (and separator) -// BOOST_FOREACH(Key key, keysToMove) { -// theta_.erase(key); -// if(separatorValues_.exists(key)) { -// separatorValues_.erase(key); -// } -// } -// -// // Permute the ordering such that the removed keys are at the end. -// // This is a prerequisite for removing them from several structures -// std::vector toBack; -// BOOST_FOREACH(Key key, keysToMove) { -// toBack.push_back(ordering_.at(key)); -// } -// Permutation forwardPermutation = Permutation::PushToBack(toBack, ordering_.size()); -// ordering_.permuteInPlace(forwardPermutation); -// delta_.permuteInPlace(forwardPermutation); -// -// // Remove marginalized keys from the ordering and delta -// for(size_t i = 0; i < keysToMove.size(); ++i) { -// ordering_.pop_back(); -// delta_.pop_back(); -// } -// } } /* ************************************************************************* */ diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h index 7feee4d82..9a7690786 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h @@ -114,7 +114,7 @@ public: * @param newTheta Initialization points for new variables to be added to the filter * You must include here all new variables occurring in newFactors that were not already * in the filter. - * @param keysToMove An optional set of keys to move from the filter to the smoother + * @param keysToMove An optional set of keys to remove from the filter and */ Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), const boost::optional >& keysToMove = boost::none); @@ -129,11 +129,7 @@ protected: VectorValues delta_; ///< The current set of linear deltas from the linearization point std::queue availableSlots_; ///< The set of available factor graph slots caused by deleting factors Values separatorValues_; ///< The linearization points of the separator variables. These should not be updated during optimization. - std::vector currentSmootherSummarizationSlots_; ///< The slots in factor graph that correspond to the current smoother summarization on the current separator - - // Storage for information from the Smoother - NonlinearFactorGraph previousSmootherSummarization_; ///< The smoother summarization on the old separator sent by the smoother during the last synchronization - NonlinearFactorGraph smootherShortcut_; ///< A set of conditional factors from the old separator to the current separator (recursively calculated during each filter update) + std::vector smootherSummarizationSlots_; ///< The slots in factor graph that correspond to the current smoother summarization factors // Storage for information to be sent to the smoother NonlinearFactorGraph filterSummarization_; ///< A temporary holding place for calculated filter summarization factors to be sent to the smoother @@ -197,17 +193,15 @@ private: /** Use colamd to update into an efficient ordering */ void reorder(const boost::optional >& keysToMove = boost::none); - /** Marginalize out the set of requested variables from the filter, caching them for the smoother - * This effectively moves the separator. - * - * @param keysToMove The set of keys to move from the filter to the smoother - */ - void moveSeparator(const FastList& keysToMove); - /** Use a modified version of L-M to update the linearization point and delta */ static Result optimize(const NonlinearFactorGraph& factors, Values& theta, const Ordering& ordering, VectorValues& delta, const Values& linearValues, const LevenbergMarquardtParams& parameters); + /** Marginalize out the set of requested variables from the filter, caching them for the smoother + * This effectively moves the separator. + */ + void marginalize(const FastList& keysToMove); + /** Marginalize out the set of requested variables from the filter, caching them for the smoother * This effectively moves the separator. */