From 5f371a4e55072ff73910467558b6cfe9be910482 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Tue, 21 May 2013 21:07:43 +0000 Subject: [PATCH] Modified Concurrent Filter to calculate marginals using a "shortcut" that allows constant-time updates during synchronization. Still need to test implementation. --- .../nonlinear/ConcurrentBatchFilter.cpp | 621 ++++++++++++------ .../nonlinear/ConcurrentBatchFilter.h | 20 +- 2 files changed, 450 insertions(+), 191 deletions(-) diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp index 1e1e39e10..d8dcd6c14 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(marginalize); + gttic(move_separator); if(keysToMove && keysToMove->size() > 0){ - marginalize(*keysToMove); + moveSeparator(*keysToMove); } - gttoc(marginalize); + gttoc(move_separator); gttoc(update); @@ -109,100 +109,169 @@ void ConcurrentBatchFilter::synchronize(const NonlinearFactorGraph& summarizedFa gttic(synchronize); - // Remove the previous smoother summarization - removeFactors(smootherSummarizationSlots_); + // Update the smoother summarization on the old separator + previousSmootherSummarization_ = summarizedFactors; - // 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 + // 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_); - 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_) { - theta_.update(key_value.key, values.at(key_value.key)); - delta_.at(ordering_.at(key_value.key)) = delta.at(ordering.at(key_value.key)); + // 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); } - // Update the fixed linearization points (since they just changed) + ++group; + } + if(separatorValues_.size() > 0) { BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { - separatorValues_.update(key_value.key, values.at(key_value.key)); + constraints[key_value.key] = group; + marginalizeKeys.erase(key_value.key); } } + Ordering ordering = *graph.orderingCOLAMDConstrained(values, constraints); - // 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; - } + // Calculate the marginal on the new separator (from the smoother side) + NonlinearFactorGraph marginals = marginalize(graph, values, ordering, marginalizeKeys, parameters_.getEliminationFunction()); - // 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()) ); + // 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) { + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, theta_) { + 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 = *factors_.orderingCOLAMDConstrained(theta_, constraints); + + // Calculate the marginal on the new separator (from the filter side) + filterSummarization_ = marginalize(factors_, theta_, ordering, marginalizeKeys, 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); } @@ -430,117 +499,301 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::optimize(const NonlinearFac } /* ************************************************************************* */ -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. +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. - // 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; + // 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()); BOOST_FOREACH(Key key, keysToMove) { const FastList& slots = variableIndex[ordering_.at(key)]; - removedFactorSlots.insert(slots.begin(), slots.end()); + 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()); } - // Construct an elimination tree to perform sparse elimination - std::vector forest( EliminationForest::Create(linearFactorGraph, variableIndex) ); + // 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)); + } + } - // 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; + // 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(); BOOST_FOREACH(Key key, keysToMove) { - indicesToEliminate.insert(ordering_.at(key)); + newSeparatorKeys.erase(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) { - EliminationForest::removeChildrenIndices(indicesToEliminate, forest.at(ordering_.at(key))); + smootherValues_.insert(key, theta_.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 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)); - } + // Remove marginalized keys from values (and separator) + BOOST_FOREACH(Key key, keysToMove) { + theta_.erase(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(); - } + // 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(); + } + + +// // 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 9a7690786..7feee4d82 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 remove from the filter and + * @param keysToMove An optional set of keys to move from the filter to the smoother */ Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), const boost::optional >& keysToMove = boost::none); @@ -129,7 +129,11 @@ 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 smootherSummarizationSlots_; ///< The slots in factor graph that correspond to the current smoother summarization factors + 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) // 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 @@ -193,15 +197,17 @@ 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. */