Modified Concurrent Filter to calculate marginals using a "shortcut"
that allows constant-time updates during synchronization. Still need to test implementation.release/4.3a0
Stephen Williams
parent
48fc15552a
commit
5f371a4e55
|
|
@ -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<Key, int> constraints;
|
||||
std::set<Key> 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<Key, int> 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<Key> filterKeys;
|
||||
std::set<Key> separatorKeys;
|
||||
std::set<Key> 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<Key, int> constraints;
|
||||
std::set<Key> 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<Key, int> 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<Key> filterKeys;
|
||||
// std::set<Key> separatorKeys;
|
||||
// std::set<Key> 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<Key>& 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<Key>& 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<size_t> removedFactorSlots;
|
||||
// Identify all of the new factors to be sent to the smoother (any factor involving keysToMove)
|
||||
std::vector<size_t> removedFactorSlots;
|
||||
VariableIndex variableIndex(*factors_.symbolic(ordering_), theta_.size());
|
||||
BOOST_FOREACH(Key key, keysToMove) {
|
||||
const FastList<size_t>& 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<EliminationForest::shared_ptr> 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<Index> 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<Key> 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<Key> oldSeparatorKeys = previousSmootherSummarization_.keys();
|
||||
BOOST_FOREACH(Key key, newSeparatorKeys) {
|
||||
oldSeparatorKeys.erase(key);
|
||||
}
|
||||
|
||||
// Calculate the set of OtherKeys = AllKeys - OldSeparator - NewSeparator
|
||||
FastSet<Key> 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<Key, int> 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<size_t> 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<size_t>::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<size_t> 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<Index> 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<Index> 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<size_t> marginalSlots = insertFactors(marginalFactors);
|
||||
//
|
||||
// // text
|
||||
// // Use the variable Index to mark the factors that will be marginalized
|
||||
// std::set<size_t> removedFactorSlots;
|
||||
// BOOST_FOREACH(Key key, keysToMove) {
|
||||
// const FastList<size_t>& 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<size_t> removedFactorSlots;
|
||||
// BOOST_FOREACH(Key key, keysToMove) {
|
||||
// const FastList<size_t>& slots = variableIndex[ordering_.at(key)];
|
||||
// removedFactorSlots.insert(slots.begin(), slots.end());
|
||||
// }
|
||||
//
|
||||
// // Construct an elimination tree to perform sparse elimination
|
||||
// std::vector<EliminationForest::shared_ptr> 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<Index> 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<size_t> 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<size_t>::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<size_t> 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<Index> 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();
|
||||
// }
|
||||
// }
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -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<FastList<Key> >& keysToMove = boost::none);
|
||||
|
|
@ -129,7 +129,11 @@ protected:
|
|||
VectorValues delta_; ///< The current set of linear deltas from the linearization point
|
||||
std::queue<size_t> 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<size_t> smootherSummarizationSlots_; ///< The slots in factor graph that correspond to the current smoother summarization factors
|
||||
std::vector<size_t> 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<FastList<Key> >& 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<Key>& 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<Key>& keysToMove);
|
||||
|
||||
/** Marginalize out the set of requested variables from the filter, caching them for the smoother
|
||||
* This effectively moves the separator.
|
||||
*/
|
||||
|
|
|
|||
Loading…
Reference in New Issue