12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Updated Concurrent Batch Filter and Smoother to the latest versions

release/4.3a0
Stephen Williams 2013-08-10 17:15:20 +00:00
parent 1dc0e6192b
commit 663c598591
6 changed files with 722 additions and 687 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

724
gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp
View File

@ -15,7 +15,6 @@
* Concurrent Filtering and Smoothing interface.
* @author Stephen Williams
*/
#include <gtsam_unstable/nonlinear/ConcurrentBatchFilter.h>
#include <gtsam/nonlinear/LinearContainerFactor.h>
#include <gtsam/linear/GaussianJunctionTree.h>
@ -24,14 +23,96 @@
namespace gtsam {
/* ************************************************************************* */
void ConcurrentBatchFilter::PrintNonlinearFactor(const NonlinearFactor::shared_ptr& factor,
const std::string& indent, const KeyFormatter& keyFormatter) {
std::cout << indent;
if(factor) {
if(boost::dynamic_pointer_cast<LinearContainerFactor>(factor)) {
std::cout << "l( ";
} else {
std::cout << "f( ";
}
BOOST_FOREACH(Key key, *factor) {
std::cout << keyFormatter(key) << " ";
}
std::cout << ")" << std::endl;
} else {
std::cout << "{ NULL }" << std::endl;
}
}
/* ************************************************************************* */
void ConcurrentBatchFilter::PrintNonlinearFactorGraph(const NonlinearFactorGraph& factors,
const std::string& indent, const std::string& title, const KeyFormatter& keyFormatter) {
std::cout << indent << title << std::endl;
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, factors) {
PrintNonlinearFactor(factor, indent + " ", keyFormatter);
}
}
/* ************************************************************************* */
void ConcurrentBatchFilter::PrintNonlinearFactorGraph(const NonlinearFactorGraph& factors, const std::vector<size_t>& slots,
const std::string& indent, const std::string& title, const KeyFormatter& keyFormatter) {
std::cout << indent << title << std::endl;
BOOST_FOREACH(size_t slot, slots) {
PrintNonlinearFactor(factors.at(slot), indent + " ", keyFormatter);
}
}
/* ************************************************************************* */
void ConcurrentBatchFilter::PrintLinearFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering,
const std::string& indent, const KeyFormatter& keyFormatter) {
std::cout << indent;
if(factor) {
JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(factor);
HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast<HessianFactor>(factor);
if(jf) {
std::cout << "j( ";
} else if(hf) {
std::cout << "h( ";
} else {
std::cout << "g( ";
}
BOOST_FOREACH(Index index, *factor) {
std::cout << keyFormatter(ordering.key(index)) << " ";
}
std::cout << ")" << std::endl;
} else {
std::cout << "{ NULL }" << std::endl;
}
}
/* ************************************************************************* */
void ConcurrentBatchFilter::PrintLinearFactorGraph(const GaussianFactorGraph& factors, const Ordering& ordering,
const std::string& indent, const std::string& title, const KeyFormatter& keyFormatter) {
std::cout << indent << title << std::endl;
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) {
PrintLinearFactor(factor, ordering, indent + " ", keyFormatter);
}
}
/* ************************************************************************* */
template<>
void ConcurrentBatchFilter::PrintKeys<Values>(const Values& values, const std::string& indent, const std::string& title, const KeyFormatter& keyFormatter) {
FastList<Key> keys = values.keys();
PrintKeys(keys, indent, title, keyFormatter);
}
/* ************************************************************************* */
template<>
void ConcurrentBatchFilter::PrintKeys<NonlinearFactorGraph>(const NonlinearFactorGraph& graph, const std::string& indent, const std::string& title, const KeyFormatter& keyFormatter) {
FastSet<Key> keys = graph.keys();
PrintKeys(keys, indent, title, keyFormatter);
}
/* ************************************************************************* */
void ConcurrentBatchFilter::print(const std::string& s, const KeyFormatter& keyFormatter) const {
std::cout << s;
std::cout << " Factors:" << std::endl;
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, factors_) {
PrintNonlinearFactor(factor, " ", keyFormatter);
}
theta_.print("Values:\n");
PrintNonlinearFactorGraph(factors_, " ", "Factors:");
PrintKeys(theta_, " ", "Values:");
PrintNonlinearFactorGraph(smootherFactors_, " ", "Cached Smoother Factors:");
PrintKeys(smootherValues_, " ", "Cached Smoother Values:");
}
/* ************************************************************************* */
@ -42,7 +123,12 @@ bool ConcurrentBatchFilter::equals(const ConcurrentFilter& rhs, double tol) cons
&& theta_.equals(filter->theta_)
&& ordering_.equals(filter->ordering_)
&& delta_.equals(filter->delta_)
&& separatorValues_.equals(filter->separatorValues_);
&& separatorValues_.equals(filter->separatorValues_)
&& smootherSummarization_.equals(filter->smootherSummarization_)
&& smootherShortcut_.equals(filter->smootherShortcut_)
&& filterSummarization_.equals(filter->filterSummarization_)
&& smootherFactors_.equals(filter->smootherFactors_)
&& smootherValues_.equals(filter->smootherValues_);
}
/* ************************************************************************* */
@ -50,9 +136,16 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::update(const NonlinearFacto
gttic(update);
// const bool debug = ISDEBUG("ConcurrentBatchFilter update");
const bool debug = false;
if(debug) std::cout << "ConcurrentBatchFilter::update Begin" << std::endl;
// Create the return result meta-data
Result result;
if(debug) std::cout << "ConcurrentBatchFilter::update Augmenting System ..." << std::endl;
// Update all of the internal variables with the new information
gttic(augment_system);
// Add the new variables to theta
@ -73,11 +166,15 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::update(const NonlinearFacto
insertFactors(newFactors);
gttoc(augment_system);
if(debug) std::cout << "ConcurrentBatchFilter::update Reordering System ..." << std::endl;
// Reorder the system to ensure efficient optimization (and marginalization) performance
gttic(reorder);
reorder(keysToMove);
gttoc(reorder);
if(debug) std::cout << "ConcurrentBatchFilter::update Optimizing System ..." << std::endl;
// Optimize the factors using a modified version of L-M
gttic(optimize);
if(factors_.size() > 0) {
@ -85,11 +182,15 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::update(const NonlinearFacto
}
gttoc(optimize);
gttic(marginalize);
if(debug) std::cout << "ConcurrentBatchFilter::update Moving Separator ..." << std::endl;
gttic(move_separator);
if(keysToMove && keysToMove->size() > 0){
marginalize(*keysToMove);
moveSeparator(*keysToMove);
}
gttoc(marginalize);
gttoc(move_separator);
if(debug) std::cout << "ConcurrentBatchFilter::update End" << std::endl;
gttoc(update);
@ -105,115 +206,92 @@ void ConcurrentBatchFilter::presync() {
}
/* ************************************************************************* */
void ConcurrentBatchFilter::synchronize(const NonlinearFactorGraph& summarizedFactors, const Values& separatorValues) {
void ConcurrentBatchFilter::synchronize(const NonlinearFactorGraph& smootherSummarization, const Values& smootherSummarizationValues) {
gttic(synchronize);
// Remove the previous smoother summarization
removeFactors(smootherSummarizationSlots_);
// const bool debug = ISDEBUG("ConcurrentBatchFilter synchronize");
const bool debug = false;
// 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(debug) std::cout << "ConcurrentBatchFilter::synchronize Begin" << std::endl;
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));
}
// 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));
}
}
if(debug) { PrintNonlinearFactorGraph(smootherSummarization_, "ConcurrentBatchFilter::synchronize ", "Previous Smoother Summarization:", DefaultKeyFormatter); }
// 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;
}
#ifndef NDEBUG
std::set<Key> oldKeys = smootherSummarization_.keys();
std::set<Key> newKeys = smootherSummarization.keys();
assert(oldKeys.size() == newKeys.size());
assert(std::equal(oldKeys.begin(), oldKeys.end(), newKeys.begin()));
#endif
// 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);
// Update the smoother summarization on the old separator
smootherSummarization_ = smootherSummarization;
// 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);
}
if(debug) { PrintNonlinearFactorGraph(smootherSummarization_, "ConcurrentBatchFilter::synchronize ", "Updated Smoother Summarization:", DefaultKeyFormatter); }
// 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()) );
// Find the set of new separator keys
FastSet<Key> newSeparatorKeys;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) {
newSeparatorKeys.insert(key_value.key);
}
if(debug) { PrintKeys(newSeparatorKeys, "ConcurrentBatchFilter::synchronize ", "Current Separator Keys:"); }
// Use the shortcut to calculate an updated marginal on the current separator
{
// Combine just the shortcut and the previousSmootherSummarization
NonlinearFactorGraph graph;
graph.push_back(smootherSummarization_);
graph.push_back(smootherShortcut_);
Values values;
values.insert(smootherSummarizationValues);
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) {
if(!values.exists(key_value.key)) {
values.insert(key_value.key, key_value.value);
}
}
// Calculate the summarized factor on just the new separator keys
smootherSummarization_ = internal::calculateMarginalFactors(graph, values, newSeparatorKeys, parameters_.getEliminationFunction());
// Remove the old factors on the separator and insert new
removeFactors(separatorSummarizationSlots_);
separatorSummarizationSlots_ = insertFactors(smootherSummarization_);
// Clear the smoother shortcut
smootherShortcut_.resize(0);
}
if(debug) { PrintNonlinearFactorGraph(factors_, separatorSummarizationSlots_, "ConcurrentBatchFilter::synchronize ", "Current Separator Summarization:", DefaultKeyFormatter); }
// 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 the summarized factor on just the new separator keys (from the filter side)
// Copy all of the factors from the filter, not including the smoother summarization
NonlinearFactorGraph factors;
for(size_t slot = 0; slot < factors_.size(); ++slot) {
if(factors_.at(slot) && std::find(separatorSummarizationSlots_.begin(), separatorSummarizationSlots_.end(), slot) == separatorSummarizationSlots_.end()) {
factors.push_back(factors_.at(slot));
}
}
filterSummarization_ = internal::calculateMarginalFactors(factors, theta_, newSeparatorKeys, parameters_.getEliminationFunction());
}
if(debug) { PrintNonlinearFactorGraph(filterSummarization_, "ConcurrentBatchFilter::synchronize ", "Updated Filter Summarization:", DefaultKeyFormatter); }
if(debug) std::cout << "ConcurrentBatchFilter::synchronize End" << std::endl;
gttoc(synchronize);
}
/* ************************************************************************* */
void ConcurrentBatchFilter::getSummarizedFactors(NonlinearFactorGraph& summarizedFactors, Values& separatorValues) {
void ConcurrentBatchFilter::getSummarizedFactors(NonlinearFactorGraph& filterSummarization, Values& filterSummarizationValues) {
gttic(get_summarized_factors);
// Copy the previous calculated smoother summarization factors into the output
summarizedFactors.push_back(filterSummarization_);
separatorValues.insert(separatorValues_);
filterSummarization.push_back(filterSummarization_);
filterSummarizationValues.insert(separatorValues_);
gttoc(get_summarized_factors);
}
@ -323,6 +401,11 @@ void ConcurrentBatchFilter::reorder(const boost::optional<FastList<Key> >& keysT
ConcurrentBatchFilter::Result ConcurrentBatchFilter::optimize(const NonlinearFactorGraph& factors, Values& theta, const Ordering& ordering,
VectorValues& delta, const Values& linearValues, const LevenbergMarquardtParams& parameters) {
// const bool debug = ISDEBUG("ConcurrentBatchFilter optimize");
const bool debug = false;
if(debug) std::cout << "ConcurrentBatchFilter::optimize Begin" << std::endl;
// Create output result structure
Result result;
result.nonlinearVariables = theta.size() - linearValues.size();
@ -332,7 +415,7 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::optimize(const NonlinearFac
double lambda = parameters.lambdaInitial;
double lambdaFactor = parameters.lambdaFactor;
double lambdaUpperBound = parameters.lambdaUpperBound;
double lambdaLowerBound = 0.5 / parameters.lambdaUpperBound;
double lambdaLowerBound = 1.0e-10;
size_t maxIterations = parameters.maxIterations;
double relativeErrorTol = parameters.relativeErrorTol;
double absoluteErrorTol = parameters.absoluteErrorTol;
@ -342,6 +425,17 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::optimize(const NonlinearFac
Values evalpoint = theta.retract(delta, ordering);
result.error = factors.error(evalpoint);
// check if we're already close enough
if(result.error <= errorTol) {
if(debug) { std::cout << "Exiting, as error = " << result.error << " < " << errorTol << std::endl; }
return result;
}
if(debug) {
std::cout << "linearValues: " << linearValues.size() << std::endl;
std::cout << "Initial error: " << result.error << std::endl;
}
// Use a custom optimization loop so the linearization points can be controlled
double previousError;
VectorValues newDelta;
@ -356,16 +450,20 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::optimize(const NonlinearFac
// Keep increasing lambda until we make make progress
while(true) {
if(debug) { std::cout << "trying lambda = " << lambda << std::endl; }
// Add prior factors at the current solution
gttic(damp);
GaussianFactorGraph dampedFactorGraph(linearFactorGraph);
dampedFactorGraph.reserve(linearFactorGraph.size() + delta.size());
double sigma = 1.0 / std::sqrt(lambda);
{
// for each of the variables, add a prior at the current solution
for(size_t j=0; j<delta.size(); ++j) {
Matrix A = lambda * eye(delta[j].size());
Vector b = lambda * delta[j];
SharedDiagonal model = noiseModel::Unit::Create(delta[j].size());
Matrix A = eye(delta[j].size());
Vector b = delta[j];
SharedDiagonal model = noiseModel::Isotropic::Sigma(delta[j].size(), sigma);
GaussianFactor::shared_ptr prior(new JacobianFactor(j, A, b, model));
dampedFactorGraph.push_back(prior);
}
@ -385,6 +483,11 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::optimize(const NonlinearFac
double error = factors.error(evalpoint);
gttoc(compute_error);
if(debug) {
std::cout << "linear delta norm = " << newDelta.norm() << std::endl;
std::cout << "next error = " << error << std::endl;
}
if(error < result.error) {
// Keep this change
// Update the error value
@ -410,304 +513,189 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::optimize(const NonlinearFac
break;
} else {
// Reject this change
// Increase lambda and continue searching
lambda *= lambdaFactor;
if(lambda > lambdaUpperBound) {
if(lambda >= lambdaUpperBound) {
// The maximum lambda has been used. Print a warning and end the search.
std::cout << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << std::endl;
break;
} else {
// Increase lambda and continue searching
lambda *= lambdaFactor;
}
}
} // end while
}
gttoc(optimizer_iteration);
if(debug) { std::cout << "using lambda = " << lambda << std::endl; }
result.iterations++;
} while(result.iterations < maxIterations &&
!checkConvergence(relativeErrorTol, absoluteErrorTol, errorTol, previousError, result.error, NonlinearOptimizerParams::SILENT));
if(debug) { std::cout << "newError: " << result.error << std::endl; }
if(debug) std::cout << "ConcurrentBatchFilter::optimize End" << std::endl;
return result;
}
/* ************************************************************************* */
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
// const bool debug = ISDEBUG("ConcurrentBatchFilter moveSeparator");
const bool debug = false;
// Create the linear factor graph
GaussianFactorGraph linearFactorGraph = *factors_.linearize(theta_, ordering_);
if(debug) std::cout << "ConcurrentBatchFilter::moveSeparator Begin" << std::endl;
// Calculate the variable index
VariableIndex variableIndex(linearFactorGraph, ordering_.size());
if(debug) { PrintKeys(keysToMove, "ConcurrentBatchFilter::moveSeparator ", "Keys To Move:", DefaultKeyFormatter); }
// 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, separatorSummarizationSlots_) {
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) );
// 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
// TODO: Make this compact
if(debug) {
std::cout << "ConcurrentBatchFilter::moveSeparator Removed Factor Slots: ";
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);
}
std::cout << slot << " ";
}
std::cout << std::endl;
}
// Add the linearization point of the moved variables to the smoother cache
BOOST_FOREACH(Key key, keysToMove) {
smootherValues_.insert(key, theta_.at(key));
// 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));
}
}
// Remove the marginalized variables and factors from the filter
if(debug) {
PrintNonlinearFactorGraph(removedFactors, "ConcurrentBatchFilter::synchronize ", "Removed Factors:", DefaultKeyFormatter);
PrintNonlinearFactorGraph(smootherShortcut_, "ConcurrentBatchFilter::synchronize ", "Old Shortcut:", DefaultKeyFormatter);
PrintKeys(smootherShortcut_, "ConcurrentBatchFilter::moveSeparator ", "Old Shortcut Keys:", DefaultKeyFormatter);
PrintKeys(separatorValues_, "ConcurrentBatchFilter::moveSeparator ", "Previous Separator Keys:", DefaultKeyFormatter);
}
// Calculate the set of new separator keys: AffectedKeys + PreviousSeparatorKeys - KeysToMove
FastSet<Key> newSeparatorKeys = removedFactors.keys();
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) {
newSeparatorKeys.insert(key_value.key);
}
BOOST_FOREACH(Key key, keysToMove) {
newSeparatorKeys.erase(key);
}
if(debug) { PrintKeys(newSeparatorKeys, "ConcurrentBatchFilter::synchronize ", "New Separator Keys:", DefaultKeyFormatter); }
// Calculate the set of shortcut keys: NewSeparatorKeys + OldSeparatorKeys
FastSet<Key> shortcutKeys = newSeparatorKeys;
BOOST_FOREACH(Key key, smootherSummarization_.keys()) {
shortcutKeys.insert(key);
}
if(debug) { PrintKeys(shortcutKeys, "ConcurrentBatchFilter::moveSeparator ", "Old Shortcut Keys:", DefaultKeyFormatter); }
// Calculate a new shortcut
{
// 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();
}
// 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_);
Values values;
values.insert(smootherValues_);
values.insert(theta_);
// Calculate the summarized factor on the shortcut keys
smootherShortcut_ = internal::calculateMarginalFactors(graph, values, shortcutKeys, parameters_.getEliminationFunction());
}
if(debug) {
PrintNonlinearFactorGraph(smootherShortcut_, "ConcurrentBatchFilter::synchronize ", "New Shortcut:", DefaultKeyFormatter);
PrintNonlinearFactorGraph(smootherSummarization_, "ConcurrentBatchFilter::synchronize ", "Smoother Summarization:", DefaultKeyFormatter);
PrintNonlinearFactorGraph(factors_, separatorSummarizationSlots_, "ConcurrentBatchFilter::synchronize ", "Current Separator Summarization:", DefaultKeyFormatter);
}
// Calculate the new smoother summarization on the new separator using the shortcut
NonlinearFactorGraph separatorSummarization;
{
// Combine just the shortcut and the previousSmootherSummarization
NonlinearFactorGraph graph;
graph.push_back(smootherSummarization_);
graph.push_back(smootherShortcut_);
Values values;
values.insert(smootherValues_);
values.insert(theta_);
// Calculate the summarized factor on just the new separator
separatorSummarization = internal::calculateMarginalFactors(graph, values, newSeparatorKeys, parameters_.getEliminationFunction());
}
// Remove the previous marginal factors and insert the new marginal factors
removeFactors(separatorSummarizationSlots_);
separatorSummarizationSlots_ = insertFactors(separatorSummarization);
if(debug) { PrintNonlinearFactorGraph(factors_, separatorSummarizationSlots_, "ConcurrentBatchFilter::synchronize ", "New Separator Summarization:", DefaultKeyFormatter); }
// Update the separatorValues object (should only contain the new separator keys)
separatorValues_.clear();
BOOST_FOREACH(Key key, separatorSummarization.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));
}
// Remove marginalized keys from values (and separator)
BOOST_FOREACH(Key key, keysToMove) {
theta_.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();
}
if(debug) std::cout << "ConcurrentBatchFilter::moveSeparator End" << std::endl;
}
/* ************************************************************************* */
NonlinearFactorGraph ConcurrentBatchFilter::marginalize(const NonlinearFactorGraph& graph, const Values& values,
const Ordering& ordering, const std::set<Key>& marginalizeKeys, const GaussianFactorGraph::Eliminate& function) {
// Calculate marginal factors on the remaining variables (after marginalizing 'marginalizeKeys')
// Note: It is assumed the ordering already has these keys first
// Create the linear factor graph
GaussianFactorGraph linearFactorGraph = *graph.linearize(values, ordering);
// Construct a variable index
VariableIndex variableIndex(linearFactorGraph, ordering.size());
// Construct an elimination tree to perform sparse elimination
std::vector<EliminationForest::shared_ptr> forest( EliminationForest::Create(linearFactorGraph, variableIndex) );
// This is a forest. Only the top-most node/index of each tree needs 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, marginalizeKeys) {
indicesToEliminate.insert(ordering.at(key));
}
BOOST_FOREACH(Key key, marginalizeKeys) {
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(function);
if(gaussianFactor->size() > 0) {
LinearContainerFactor::shared_ptr marginalFactor(new LinearContainerFactor(gaussianFactor, ordering, values));
marginalFactors.push_back(marginalFactor);
}
}
return marginalFactors;
}
/* ************************************************************************* */
void ConcurrentBatchFilter::PrintNonlinearFactor(const NonlinearFactor::shared_ptr& factor,
const std::string& indent, const KeyFormatter& keyFormatter) {
std::cout << indent;
if(factor) {
if(boost::dynamic_pointer_cast<LinearContainerFactor>(factor)) {
std::cout << "l( ";
} else {
std::cout << "f( ";
}
BOOST_FOREACH(Key key, *factor) {
std::cout << keyFormatter(key) << " ";
}
std::cout << ")" << std::endl;
} else {
std::cout << "{ NULL }" << std::endl;
}
}
/* ************************************************************************* */
void ConcurrentBatchFilter::PrintLinearFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering,
const std::string& indent, const KeyFormatter& keyFormatter) {
std::cout << indent;
if(factor) {
std::cout << "g( ";
BOOST_FOREACH(Index index, *factor) {
std::cout << keyFormatter(ordering.key(index)) << " ";
}
std::cout << ")" << std::endl;
} else {
std::cout << "{ NULL }" << std::endl;
}
}
/* ************************************************************************* */
std::vector<Index> ConcurrentBatchFilter::EliminationForest::ComputeParents(const VariableIndex& structure) {
// Number of factors and variables
const size_t m = structure.nFactors();
const size_t n = structure.size();
static const Index none = std::numeric_limits<Index>::max();
// Allocate result parent vector and vector of last factor columns
std::vector<Index> parents(n, none);
std::vector<Index> prevCol(m, none);
// for column j \in 1 to n do
for (Index j = 0; j < n; j++) {
// for row i \in Struct[A*j] do
BOOST_FOREACH(const size_t i, structure[j]) {
if (prevCol[i] != none) {
Index k = prevCol[i];
// find root r of the current tree that contains k
Index r = k;
while (parents[r] != none)
r = parents[r];
if (r != j) parents[r] = j;
}
prevCol[i] = j;
}
}
return parents;
}
/* ************************************************************************* */
std::vector<ConcurrentBatchFilter::EliminationForest::shared_ptr> ConcurrentBatchFilter::EliminationForest::Create(const GaussianFactorGraph& factorGraph, const VariableIndex& structure) {
// Compute the tree structure
std::vector<Index> parents(ComputeParents(structure));
// Number of variables
const size_t n = structure.size();
static const Index none = std::numeric_limits<Index>::max();
// Create tree structure
std::vector<shared_ptr> trees(n);
for (Index k = 1; k <= n; k++) {
Index j = n - k; // Start at the last variable and loop down to 0
trees[j].reset(new EliminationForest(j)); // Create a new node on this variable
if (parents[j] != none) // If this node has a parent, add it to the parent's children
trees[parents[j]]->add(trees[j]);
}
// Hang factors in right places
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factorGraph) {
if(factor && factor->size() > 0) {
Index j = *std::min_element(factor->begin(), factor->end());
if(j < structure.size())
trees[j]->add(factor);
}
}
return trees;
}
/* ************************************************************************* */
GaussianFactor::shared_ptr ConcurrentBatchFilter::EliminationForest::eliminateRecursive(GaussianFactorGraph::Eliminate function) {
// Create the list of factors to be eliminated, initially empty, and reserve space
GaussianFactorGraph factors;
factors.reserve(this->factors_.size() + this->subTrees_.size());
// Add all factors associated with the current node
factors.push_back(this->factors_.begin(), this->factors_.end());
// for all subtrees, eliminate into Bayes net and a separator factor, added to [factors]
BOOST_FOREACH(const shared_ptr& child, subTrees_)
factors.push_back(child->eliminateRecursive(function));
// Combine all factors (from this node and from subtrees) into a joint factor
GaussianFactorGraph::EliminationResult eliminated(function(factors, 1));
return eliminated.second;
}
/* ************************************************************************* */
void ConcurrentBatchFilter::EliminationForest::removeChildrenIndices(std::set<Index>& indices, const ConcurrentBatchFilter::EliminationForest::shared_ptr& tree) {
BOOST_FOREACH(const EliminationForest::shared_ptr& child, tree->children()) {
indices.erase(child->key());
removeChildrenIndices(indices, child);
}
}
/* ************************************************************************* */
}/// namespace gtsam

141
gtsam_unstable/nonlinear/ConcurrentBatchFilter.h
View File

@ -54,16 +54,16 @@ public:
};
/** Default constructor */
ConcurrentBatchFilter(const LevenbergMarquardtParams& parameters = LevenbergMarquardtParams(), bool relin = true) : parameters_(parameters), relin_(relin) {};
ConcurrentBatchFilter(const LevenbergMarquardtParams& parameters = LevenbergMarquardtParams()) : parameters_(parameters) {};
/** Default destructor */
virtual ~ConcurrentBatchFilter() {};
/** Implement a GTSAM standard 'print' function */
void print(const std::string& s = "Concurrent Batch Filter:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
virtual void print(const std::string& s = "Concurrent Batch Filter:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/** Check if two Concurrent Filters are equal */
bool equals(const ConcurrentFilter& rhs, double tol = 1e-9) const;
virtual bool equals(const ConcurrentFilter& rhs, double tol = 1e-9) const;
/** Access the current set of factors */
const NonlinearFactorGraph& getFactors() const {
@ -114,28 +114,11 @@ 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(),
virtual Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(),
const boost::optional<FastList<Key> >& keysToMove = boost::none);
protected:
LevenbergMarquardtParams parameters_; ///< LM parameters
bool relin_;
NonlinearFactorGraph factors_; ///< The set of all factors currently in the filter
Values theta_; ///< Current linearization point of all variables in the filter
Ordering ordering_; ///< The current ordering used to calculate the linear deltas
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
// 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
NonlinearFactorGraph smootherFactors_; ///< A temporary holding place for the set of full nonlinear factors being sent to the smoother
Values smootherValues_; ///< A temporary holding place for the linearization points of all keys being sent to the smoother
/**
* Perform any required operations before the synchronization process starts.
* Called by 'synchronize'
@ -149,7 +132,7 @@ protected:
* @param summarizedFactors The summarized factors for the filter branch
* @param rootValues The linearization points of the root clique variables
*/
virtual void getSummarizedFactors(NonlinearFactorGraph& summarizedFactors, Values& separatorValues);
virtual void getSummarizedFactors(NonlinearFactorGraph& filterSummarization, Values& filterSummarizationValues);
/**
* Populate the provided containers with factors being sent to the smoother from the filter. These
@ -166,7 +149,7 @@ protected:
*
* @param summarizedFactors An updated version of the smoother branch summarized factors
*/
virtual void synchronize(const NonlinearFactorGraph& summarizedFactors, const Values& separatorValues);
virtual void synchronize(const NonlinearFactorGraph& smootherSummarization, const Values& smootherSummarizationValues);
/**
* Perform any required operations after the synchronization process finishes.
@ -174,6 +157,25 @@ protected:
*/
virtual void postsync();
protected:
LevenbergMarquardtParams parameters_; ///< LM parameters
NonlinearFactorGraph factors_; ///< The set of all factors currently in the filter
Values theta_; ///< Current linearization point of all variables in the filter
Ordering ordering_; ///< The current ordering used to calculate the linear deltas
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> separatorSummarizationSlots_; ///< The slots in factor graph that correspond to the current smoother summarization on the current separator
// Storage for information from the Smoother
NonlinearFactorGraph smootherSummarization_; ///< 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
NonlinearFactorGraph smootherFactors_; ///< A temporary holding place for the set of full nonlinear factors being sent to the smoother
Values smootherValues_; ///< A temporary holding place for the linearization points of all keys being sent to the smoother
private:
@ -193,81 +195,54 @@ 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.
*/
static NonlinearFactorGraph marginalize(const NonlinearFactorGraph& graph, const Values& values,
const Ordering& ordering, const std::set<Key>& marginalizeKeys, const GaussianFactorGraph::Eliminate& function = EliminateQR);
/** Print just the nonlinear keys in a nonlinear factor */
static void PrintNonlinearFactor(const NonlinearFactor::shared_ptr& factor,
const std::string& indent = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter);
/** Print just the nonlinear keys in each factor for a whole Nonlinear Factor Graph */
static void PrintNonlinearFactorGraph(const NonlinearFactorGraph& factors,
const std::string& indent = "", const std::string& title = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter);
/** Print just the nonlinear keys of specific factors in a Nonlinear Factor Graph */
static void PrintNonlinearFactorGraph(const NonlinearFactorGraph& factors, const std::vector<size_t>& slots,
const std::string& indent = "", const std::string& title = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter);
/** Print just the nonlinear keys in a linear factor */
static void PrintLinearFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering,
const std::string& indent = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter);
// A custom elimination tree that supports forests and partial elimination
class EliminationForest {
public:
typedef boost::shared_ptr<EliminationForest> shared_ptr; ///< Shared pointer to this class
/** Print just the nonlinear keys in each linear factor for a whole Gaussian Factor Graph */
static void PrintLinearFactorGraph(const GaussianFactorGraph& factors, const Ordering& ordering,
const std::string& indent = "", const std::string& title = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter);
private:
typedef FastList<GaussianFactor::shared_ptr> Factors;
typedef FastList<shared_ptr> SubTrees;
typedef std::vector<GaussianConditional::shared_ptr> Conditionals;
Index key_; ///< index associated with root
Factors factors_; ///< factors associated with root
SubTrees subTrees_; ///< sub-trees
/** default constructor, private, as you should use Create below */
EliminationForest(Index key = 0) : key_(key) {}
/**
* Static internal function to build a vector of parent pointers using the
* algorithm of Gilbert et al., 2001, BIT.
*/
static std::vector<Index> ComputeParents(const VariableIndex& structure);
/** add a factor, for Create use only */
void add(const GaussianFactor::shared_ptr& factor) { factors_.push_back(factor); }
/** add a subtree, for Create use only */
void add(const shared_ptr& child) { subTrees_.push_back(child); }
public:
/** return the key associated with this tree node */
Index key() const { return key_; }
/** return the const reference of children */
const SubTrees& children() const { return subTrees_; }
/** return the const reference to the factors */
const Factors& factors() const { return factors_; }
/** Create an elimination tree from a factor graph */
static std::vector<shared_ptr> Create(const GaussianFactorGraph& factorGraph, const VariableIndex& structure);
/** Recursive routine that eliminates the factors arranged in an elimination tree */
GaussianFactor::shared_ptr eliminateRecursive(GaussianFactorGraph::Eliminate function);
/** Recursive function that helps find the top of each tree */
static void removeChildrenIndices(std::set<Index>& indices, const EliminationForest::shared_ptr& tree);
};
/** Print just the nonlinear keys contained inside a container */
template<class Container>
static void PrintKeys(const Container& keys, const std::string& indent = "",
const std::string& title = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter);
}; // ConcurrentBatchFilter
/// Implementation of PrintKeys
template<class Container>
void ConcurrentBatchFilter::PrintKeys(const Container& keys, const std::string& indent, const std::string& title, const KeyFormatter& keyFormatter) {
std::cout << indent << title;
BOOST_FOREACH(Key key, keys) {
std::cout << " " << keyFormatter(key);
}
std::cout << std::endl;
}
/// Typedef for Matlab wrapping
typedef ConcurrentBatchFilter::Result ConcurrentBatchFilterResult;

189
gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp
View File

@ -265,19 +265,17 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() {
result.nonlinearVariables = theta_.size() - separatorValues_.size();
result.linearVariables = separatorValues_.size();
// Set optimization parameters
// Pull out parameters we'll use
const NonlinearOptimizerParams::Verbosity nloVerbosity = parameters_.verbosity;
const LevenbergMarquardtParams::VerbosityLM lmVerbosity = parameters_.verbosityLM;
double lambda = parameters_.lambdaInitial;
double lambdaFactor = parameters_.lambdaFactor;
double lambdaUpperBound = parameters_.lambdaUpperBound;
double lambdaLowerBound = 0.5 / parameters_.lambdaUpperBound;
size_t maxIterations = parameters_.maxIterations;
double relativeErrorTol = parameters_.relativeErrorTol;
double absoluteErrorTol = parameters_.absoluteErrorTol;
double errorTol = parameters_.errorTol;
// Create a Values that holds the current evaluation point
Values evalpoint = theta_.retract(delta_, ordering_);
result.error = factors_.error(evalpoint);
if(result.error < parameters_.errorTol) {
return result;
}
// Use a custom optimization loop so the linearization points can be controlled
double previousError;
@ -293,6 +291,9 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() {
// Keep increasing lambda until we make make progress
while(true) {
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
std::cout << "trying lambda = " << lambda << std::endl;
// Add prior factors at the current solution
gttic(damp);
GaussianFactorGraph dampedFactorGraph(linearFactorGraph);
@ -300,16 +301,18 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() {
{
// for each of the variables, add a prior at the current solution
for(size_t j=0; j<delta_.size(); ++j) {
Matrix A = lambda * eye(delta_[j].size());
Vector b = lambda * delta_[j];
SharedDiagonal model = noiseModel::Unit::Create(delta_[j].size());
Matrix A = eye(delta_[j].size());
Vector b = delta_[j];
SharedDiagonal model = noiseModel::Isotropic::Sigma(delta_[j].size(), 1.0 / std::sqrt(lambda));
GaussianFactor::shared_ptr prior(new JacobianFactor(j, A, b, model));
dampedFactorGraph.push_back(prior);
}
}
gttoc(damp);
if (lmVerbosity >= LevenbergMarquardtParams::DAMPED)
dampedFactorGraph.print("damped");
result.lambdas++;
gttic(solve);
// Solve Damped Gaussian Factor Graph
newDelta = GaussianJunctionTree(dampedFactorGraph).optimize(parameters_.getEliminationFunction());
@ -317,11 +320,19 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() {
evalpoint = theta_.retract(newDelta, ordering_);
gttoc(solve);
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
std::cout << "linear delta norm = " << newDelta.norm() << std::endl;
if (lmVerbosity >= LevenbergMarquardtParams::TRYDELTA)
newDelta.print("delta");
// Evaluate the new error
gttic(compute_error);
double error = factors_.error(evalpoint);
gttoc(compute_error);
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
std::cout << "next error = " << error << std::endl;
if(error < result.error) {
// Keep this change
// Update the error value
@ -339,29 +350,29 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() {
}
}
// Decrease lambda for next time
lambda /= lambdaFactor;
if(lambda < lambdaLowerBound) {
lambda = lambdaLowerBound;
}
lambda /= parameters_.lambdaFactor;
// End this lambda search iteration
break;
} else {
// Reject this change
// Increase lambda and continue searching
lambda *= lambdaFactor;
if(lambda > lambdaUpperBound) {
if(lambda >= parameters_.lambdaUpperBound) {
// The maximum lambda has been used. Print a warning and end the search.
std::cout << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << std::endl;
break;
}
} else {
// Increase lambda and continue searching
lambda *= parameters_.lambdaFactor;
}
} // end while
}
gttoc(optimizer_iteration);
if (lmVerbosity >= LevenbergMarquardtParams::LAMBDA)
std::cout << "using lambda = " << lambda << std::endl;
result.iterations++;
} while(result.iterations < maxIterations &&
!checkConvergence(relativeErrorTol, absoluteErrorTol, errorTol, previousError, result.error, NonlinearOptimizerParams::SILENT));
} while(result.iterations < parameters_.maxIterations &&
!checkConvergence(parameters_.relativeErrorTol, parameters_.absoluteErrorTol, parameters_.errorTol, previousError, result.error, NonlinearOptimizerParams::SILENT));
return result;
}
@ -374,43 +385,20 @@ void ConcurrentBatchSmoother::updateSmootherSummarization() {
// These marginal factors will be cached for later transmission to the filter using
// linear container factors
// Clear out any existing smoother summarized factors
smootherSummarization_.resize(0);
// Reorder the system so that the separator keys are eliminated last
// TODO: This is currently being done twice: here and in 'update'. Fix it.
reorder();
// Create the linear factor graph
GaussianFactorGraph linearFactorGraph = *factors_.linearize(theta_, ordering_);
// Construct an elimination tree to perform sparse elimination
std::vector<EliminationForest::shared_ptr> forest( EliminationForest::Create(linearFactorGraph, variableIndex_) );
// This is a forest. Only the top-most node/index of each tree needs 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(const Values::ConstKeyValuePair& key_value, theta_) {
indicesToEliminate.insert(ordering_.at(key_value.key));
// Create a nonlinear factor graph without the filter summarization factors
NonlinearFactorGraph graph(factors_);
BOOST_FOREACH(size_t slot, filterSummarizationSlots_) {
graph.remove(slot);
}
// Get the set of separator keys
gtsam::FastSet<Key> separatorKeys;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) {
indicesToEliminate.erase(ordering_.at(key_value.key));
}
std::vector<Index> indices(indicesToEliminate.begin(), indicesToEliminate.end());
BOOST_FOREACH(Index index, indices) {
EliminationForest::removeChildrenIndices(indicesToEliminate, forest.at(index));
}
// Eliminate each top-most key, returning a Gaussian Factor on some of the remaining variables
// Convert the marginal factors into Linear Container Factors and store
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_));
smootherSummarization_.push_back(marginalFactor);
}
separatorKeys.insert(key_value.key);
}
// Calculate the marginal factors on the separator
smootherSummarization_ = internal::calculateMarginalFactors(graph, theta_, separatorKeys, parameters_.getEliminationFunction());
}
/* ************************************************************************* */
@ -446,95 +434,4 @@ void ConcurrentBatchSmoother::PrintLinearFactor(const GaussianFactor::shared_ptr
}
/* ************************************************************************* */
std::vector<Index> ConcurrentBatchSmoother::EliminationForest::ComputeParents(const VariableIndex& structure) {
// Number of factors and variables
const size_t m = structure.nFactors();
const size_t n = structure.size();
static const Index none = std::numeric_limits<Index>::max();
// Allocate result parent vector and vector of last factor columns
std::vector<Index> parents(n, none);
std::vector<Index> prevCol(m, none);
// for column j \in 1 to n do
for (Index j = 0; j < n; j++) {
// for row i \in Struct[A*j] do
BOOST_FOREACH(const size_t i, structure[j]) {
if (prevCol[i] != none) {
Index k = prevCol[i];
// find root r of the current tree that contains k
Index r = k;
while (parents[r] != none)
r = parents[r];
if (r != j) parents[r] = j;
}
prevCol[i] = j;
}
}
return parents;
}
/* ************************************************************************* */
std::vector<ConcurrentBatchSmoother::EliminationForest::shared_ptr> ConcurrentBatchSmoother::EliminationForest::Create(const GaussianFactorGraph& factorGraph, const VariableIndex& structure) {
// Compute the tree structure
std::vector<Index> parents(ComputeParents(structure));
// Number of variables
const size_t n = structure.size();
static const Index none = std::numeric_limits<Index>::max();
// Create tree structure
std::vector<shared_ptr> trees(n);
for (Index k = 1; k <= n; k++) {
Index j = n - k; // Start at the last variable and loop down to 0
trees[j].reset(new EliminationForest(j)); // Create a new node on this variable
if (parents[j] != none) // If this node has a parent, add it to the parent's children
trees[parents[j]]->add(trees[j]);
}
// Hang factors in right places
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factorGraph) {
if(factor && factor->size() > 0) {
Index j = *std::min_element(factor->begin(), factor->end());
if(j < structure.size())
trees[j]->add(factor);
}
}
return trees;
}
/* ************************************************************************* */
GaussianFactor::shared_ptr ConcurrentBatchSmoother::EliminationForest::eliminateRecursive(GaussianFactorGraph::Eliminate function) {
// Create the list of factors to be eliminated, initially empty, and reserve space
GaussianFactorGraph factors;
factors.reserve(this->factors_.size() + this->subTrees_.size());
// Add all factors associated with the current node
factors.push_back(this->factors_.begin(), this->factors_.end());
// for all subtrees, eliminate into Bayes net and a separator factor, added to [factors]
BOOST_FOREACH(const shared_ptr& child, subTrees_)
factors.push_back(child->eliminateRecursive(function));
// Combine all factors (from this node and from subtrees) into a joint factor
GaussianFactorGraph::EliminationResult eliminated(function(factors, 1));
return eliminated.second;
}
/* ************************************************************************* */
void ConcurrentBatchSmoother::EliminationForest::removeChildrenIndices(std::set<Index>& indices, const ConcurrentBatchSmoother::EliminationForest::shared_ptr& tree) {
BOOST_FOREACH(const EliminationForest::shared_ptr& child, tree->children()) {
indices.erase(child->key());
removeChildrenIndices(indices, child);
}
}
/* ************************************************************************* */
}/// namespace gtsam

87
gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h
View File

@ -60,10 +60,10 @@ public:
virtual ~ConcurrentBatchSmoother() {};
/** Implement a GTSAM standard 'print' function */
void print(const std::string& s = "Concurrent Batch Smoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
virtual void print(const std::string& s = "Concurrent Batch Smoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/** Check if two Concurrent Smoothers are equal */
bool equals(const ConcurrentSmoother& rhs, double tol = 1e-9) const;
virtual bool equals(const ConcurrentSmoother& rhs, double tol = 1e-9) const;
/** Access the current set of factors */
const NonlinearFactorGraph& getFactors() const {
@ -118,23 +118,7 @@ public:
* in the smoother). There must not be any variables here that do not occur in newFactors,
* and additionally, variables that were already in the system must not be included here.
*/
Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values());
protected:
LevenbergMarquardtParams parameters_; ///< LM parameters
NonlinearFactorGraph factors_; ///< The set of all factors currently in the smoother
Values theta_; ///< Current linearization point of all variables in the smoother
Ordering ordering_; ///< The current ordering used to calculate the linear deltas
VectorValues delta_; ///< The current set of linear deltas from the linearization point
VariableIndex variableIndex_; ///< The current variable index, which allows efficient factor lookup by variable
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> filterSummarizationSlots_; ///< The slots in factor graph that correspond to the current filter summarization factors
// Storage for information to be sent to the filter
NonlinearFactorGraph smootherSummarization_; ///< A temporary holding place for calculated smoother summarization
virtual Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values());
/**
* Perform any required operations before the synchronization process starts.
@ -168,6 +152,21 @@ protected:
*/
virtual void postsync();
protected:
LevenbergMarquardtParams parameters_; ///< LM parameters
NonlinearFactorGraph factors_; ///< The set of all factors currently in the smoother
Values theta_; ///< Current linearization point of all variables in the smoother
Ordering ordering_; ///< The current ordering used to calculate the linear deltas
VectorValues delta_; ///< The current set of linear deltas from the linearization point
VariableIndex variableIndex_; ///< The current variable index, which allows efficient factor lookup by variable
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> filterSummarizationSlots_; ///< The slots in factor graph that correspond to the current filter summarization factors
// Storage for information to be sent to the filter
NonlinearFactorGraph smootherSummarization_; ///< A temporary holding place for calculated smoother summarization
private:
/** Augment the graph with new factors
@ -200,56 +199,6 @@ private:
static void PrintLinearFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering,
const std::string& indent = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter);
// A custom elimination tree that supports forests and partial elimination
class EliminationForest {
public:
typedef boost::shared_ptr<EliminationForest> shared_ptr; ///< Shared pointer to this class
private:
typedef FastList<GaussianFactor::shared_ptr> Factors;
typedef FastList<shared_ptr> SubTrees;
typedef std::vector<GaussianConditional::shared_ptr> Conditionals;
Index key_; ///< index associated with root
Factors factors_; ///< factors associated with root
SubTrees subTrees_; ///< sub-trees
/** default constructor, private, as you should use Create below */
EliminationForest(Index key = 0) : key_(key) {}
/**
* Static internal function to build a vector of parent pointers using the
* algorithm of Gilbert et al., 2001, BIT.
*/
static std::vector<Index> ComputeParents(const VariableIndex& structure);
/** add a factor, for Create use only */
void add(const GaussianFactor::shared_ptr& factor) { factors_.push_back(factor); }
/** add a subtree, for Create use only */
void add(const shared_ptr& child) { subTrees_.push_back(child); }
public:
/** return the key associated with this tree node */
Index key() const { return key_; }
/** return the const reference of children */
const SubTrees& children() const { return subTrees_; }
/** return the const reference to the factors */
const Factors& factors() const { return factors_; }
/** Create an elimination tree from a factor graph */
static std::vector<shared_ptr> Create(const GaussianFactorGraph& factorGraph, const VariableIndex& structure);
/** Recursive routine that eliminates the factors arranged in an elimination tree */
GaussianFactor::shared_ptr eliminateRecursive(GaussianFactorGraph::Eliminate function);
/** Recursive function that helps find the top of each tree */
static void removeChildrenIndices(std::set<Index>& indices, const EliminationForest::shared_ptr& tree);
};
}; // ConcurrentBatchSmoother
/// Typedef for Matlab wrapping

227
gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp
View File

@ -19,10 +19,12 @@
// \callgraph
#include "ConcurrentFilteringAndSmoothing.h"
#include <gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h>
#include <gtsam/nonlinear/LinearContainerFactor.h>
namespace gtsam {
/* ************************************************************************* */
void synchronize(ConcurrentFilter& filter, ConcurrentSmoother& smoother) {
NonlinearFactorGraph smootherFactors, filterSumarization, smootherSummarization;
@ -46,4 +48,227 @@ void synchronize(ConcurrentFilter& filter, ConcurrentSmoother& smoother) {
smoother.postsync();
}
namespace internal {
// TODO: Remove this and replace with the standard Elimination Tree once GTSAM 3.0 is released and supports forests
// A custom elimination tree that supports forests and partial elimination
class EliminationForest {
public:
typedef boost::shared_ptr<EliminationForest> shared_ptr; ///< Shared pointer to this class
private:
typedef FastList<GaussianFactor::shared_ptr> Factors;
typedef FastList<shared_ptr> SubTrees;
typedef std::vector<GaussianConditional::shared_ptr> Conditionals;
Index key_; ///< index associated with root
Factors factors_; ///< factors associated with root
SubTrees subTrees_; ///< sub-trees
/** default constructor, private, as you should use Create below */
EliminationForest(Index key = 0) : key_(key) {}
/**
* Static internal function to build a vector of parent pointers using the
* algorithm of Gilbert et al., 2001, BIT.
*/
static std::vector<Index> ComputeParents(const VariableIndex& structure);
/** add a factor, for Create use only */
void add(const GaussianFactor::shared_ptr& factor) { factors_.push_back(factor); }
/** add a subtree, for Create use only */
void add(const shared_ptr& child) { subTrees_.push_back(child); }
public:
/** return the key associated with this tree node */
Index key() const { return key_; }
/** return the const reference of children */
const SubTrees& children() const { return subTrees_; }
/** return the const reference to the factors */
const Factors& factors() const { return factors_; }
/** Create an elimination tree from a factor graph */
static std::vector<shared_ptr> Create(const GaussianFactorGraph& factorGraph, const VariableIndex& structure);
/** Recursive routine that eliminates the factors arranged in an elimination tree */
GaussianFactor::shared_ptr eliminateRecursive(GaussianFactorGraph::Eliminate function);
/** Recursive function that helps find the top of each tree */
static void removeChildrenIndices(std::set<Index>& indices, const EliminationForest::shared_ptr& tree);
};
/* ************************************************************************* */
std::vector<Index> EliminationForest::ComputeParents(const VariableIndex& structure) {
// Number of factors and variables
const size_t m = structure.nFactors();
const size_t n = structure.size();
static const Index none = std::numeric_limits<Index>::max();
// Allocate result parent vector and vector of last factor columns
std::vector<Index> parents(n, none);
std::vector<Index> prevCol(m, none);
// for column j \in 1 to n do
for (Index j = 0; j < n; j++) {
// for row i \in Struct[A*j] do
BOOST_FOREACH(const size_t i, structure[j]) {
if (prevCol[i] != none) {
Index k = prevCol[i];
// find root r of the current tree that contains k
Index r = k;
while (parents[r] != none)
r = parents[r];
if (r != j) parents[r] = j;
}
prevCol[i] = j;
}
}
return parents;
}
/* ************************************************************************* */
std::vector<EliminationForest::shared_ptr> EliminationForest::Create(const GaussianFactorGraph& factorGraph, const VariableIndex& structure) {
// Compute the tree structure
std::vector<Index> parents(ComputeParents(structure));
// Number of variables
const size_t n = structure.size();
static const Index none = std::numeric_limits<Index>::max();
// Create tree structure
std::vector<shared_ptr> trees(n);
for (Index k = 1; k <= n; k++) {
Index j = n - k; // Start at the last variable and loop down to 0
trees[j].reset(new EliminationForest(j)); // Create a new node on this variable
if (parents[j] != none) // If this node has a parent, add it to the parent's children
trees[parents[j]]->add(trees[j]);
}
// Hang factors in right places
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factorGraph) {
if(factor && factor->size() > 0) {
Index j = *std::min_element(factor->begin(), factor->end());
if(j < structure.size())
trees[j]->add(factor);
}
}
return trees;
}
/* ************************************************************************* */
GaussianFactor::shared_ptr EliminationForest::eliminateRecursive(GaussianFactorGraph::Eliminate function) {
// Create the list of factors to be eliminated, initially empty, and reserve space
GaussianFactorGraph factors;
factors.reserve(this->factors_.size() + this->subTrees_.size());
// Add all factors associated with the current node
factors.push_back(this->factors_.begin(), this->factors_.end());
// for all subtrees, eliminate into Bayes net and a separator factor, added to [factors]
BOOST_FOREACH(const shared_ptr& child, subTrees_)
factors.push_back(child->eliminateRecursive(function));
// Combine all factors (from this node and from subtrees) into a joint factor
GaussianFactorGraph::EliminationResult eliminated(function(factors, 1));
return eliminated.second;
}
/* ************************************************************************* */
void EliminationForest::removeChildrenIndices(std::set<Index>& indices, const EliminationForest::shared_ptr& tree) {
BOOST_FOREACH(const EliminationForest::shared_ptr& child, tree->children()) {
indices.erase(child->key());
removeChildrenIndices(indices, child);
}
}
/* ************************************************************************* */
NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph& graph, const Values& theta,
const FastSet<Key>& remainingKeys, const GaussianFactorGraph::Eliminate& eliminateFunction) {
// Calculate the set of RootKeys = AllKeys \Intersect RemainingKeys
FastSet<Key> rootKeys;
FastSet<Key> allKeys(graph.keys());
std::set_intersection(allKeys.begin(), allKeys.end(), remainingKeys.begin(), remainingKeys.end(), std::inserter(rootKeys, rootKeys.end()));
// Calculate the set of MarginalizeKeys = AllKeys - RemainingKeys
FastSet<Key> marginalizeKeys;
std::set_difference(allKeys.begin(), allKeys.end(), remainingKeys.begin(), remainingKeys.end(), std::inserter(marginalizeKeys, marginalizeKeys.end()));
if(marginalizeKeys.size() == 0) {
// There are no keys to marginalize. Simply return the input factors
return graph;
} else {
// Create a subset of theta that only contains the required keys
Values values;
BOOST_FOREACH(Key key, allKeys) {
values.insert(key, theta.at(key));
}
// Calculate the ordering: [Others Root]
std::map<Key, int> constraints;
BOOST_FOREACH(Key key, marginalizeKeys) {
constraints[key] = 0;
}
BOOST_FOREACH(Key key, rootKeys) {
constraints[key] = 1;
}
Ordering ordering = *graph.orderingCOLAMDConstrained(values, constraints);
// Create the linear factor graph
GaussianFactorGraph linearFactorGraph = *graph.linearize(values, ordering);
// Construct a variable index
VariableIndex variableIndex(linearFactorGraph, ordering.size());
// Construct an elimination tree to perform sparse elimination
std::vector<EliminationForest::shared_ptr> forest( EliminationForest::Create(linearFactorGraph, variableIndex) );
// This is a forest. Only the top-most node/index of each tree needs 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, marginalizeKeys) {
indicesToEliminate.insert(ordering.at(key));
}
BOOST_FOREACH(Key key, marginalizeKeys) {
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
NonlinearFactorGraph marginalFactors;
BOOST_FOREACH(Index index, indicesToEliminate) {
GaussianFactor::shared_ptr gaussianFactor = forest.at(index)->eliminateRecursive(eliminateFunction);
if(gaussianFactor->size() > 0) {
LinearContainerFactor::shared_ptr marginalFactor(new LinearContainerFactor(gaussianFactor, ordering, values));
marginalFactors.push_back(marginalFactor);
}
}
// Also add any remaining factors that were unaffected by marginalizing out the selected variables.
// These are part of the marginal on the remaining variables as well.
BOOST_FOREACH(Key key, rootKeys) {
BOOST_FOREACH(const GaussianFactor::shared_ptr& gaussianFactor, forest.at(ordering.at(key))->factors()) {
LinearContainerFactor::shared_ptr marginalFactor(new LinearContainerFactor(gaussianFactor, ordering, values));
marginalFactors.push_back(marginalFactor);
}
}
return marginalFactors;
}
}
/* ************************************************************************* */
}/// namespace internal
/* ************************************************************************* */
}/// namespace gtsam

41
gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h
View File

@ -39,22 +39,18 @@ class GTSAM_UNSTABLE_EXPORT ConcurrentFilter {
public:
typedef boost::shared_ptr<ConcurrentFilter> shared_ptr;
/** Implement a standard 'print' function */
virtual void print(const std::string& s = "Concurrent Filter:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const = 0;
/** Check if two Concurrent Smoothers are equal */
virtual bool equals(const ConcurrentFilter& rhs, double tol = 1e-9) const = 0;
protected:
friend void GTSAM_UNSTABLE_EXPORT synchronize(ConcurrentFilter& filter, ConcurrentSmoother& smoother);
/** Default constructor */
ConcurrentFilter() {};
/** Default destructor */
virtual ~ConcurrentFilter() {};
/** Implement a standard 'print' function */
virtual void print(const std::string& s = "Concurrent Filter:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const = 0;
/** Check if two Concurrent Smoothers are equal */
virtual bool equals(const ConcurrentFilter& rhs, double tol = 1e-9) const = 0;
/**
* Perform any required operations before the synchronization process starts.
* Called by 'synchronize'
@ -103,22 +99,18 @@ class GTSAM_UNSTABLE_EXPORT ConcurrentSmoother {
public:
typedef boost::shared_ptr<ConcurrentSmoother> shared_ptr;
/** Implement a standard 'print' function */
virtual void print(const std::string& s = "Concurrent Smoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const = 0;
/** Check if two Concurrent Smoothers are equal */
virtual bool equals(const ConcurrentSmoother& rhs, double tol = 1e-9) const = 0;
protected:
GTSAM_UNSTABLE_EXPORT friend void synchronize(ConcurrentFilter& filter, ConcurrentSmoother& smoother);
/** Default constructor */
ConcurrentSmoother() {};
/** Default destructor */
virtual ~ConcurrentSmoother() {};
/** Implement a standard 'print' function */
virtual void print(const std::string& s = "Concurrent Smoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const = 0;
/** Check if two Concurrent Smoothers are equal */
virtual bool equals(const ConcurrentSmoother& rhs, double tol = 1e-9) const = 0;
/**
* Perform any required operations before the synchronization process starts.
* Called by 'synchronize'
@ -154,4 +146,13 @@ protected:
}; // ConcurrentSmoother
namespace internal {
/** Calculate the marginal on the specified keys, returning a set of LinearContainerFactors.
* Unlike other GTSAM functions with similar purposes, this version can operate on disconnected graphs. */
NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph& graph, const Values& theta,
const FastSet<Key>& remainingKeys, const GaussianFactorGraph::Eliminate& eliminateFunction);
}
}/// namespace gtsam