From 663c5985910f242bfc42b0878526844f951412ef Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Sat, 10 Aug 2013 17:15:20 +0000 Subject: [PATCH] Updated Concurrent Batch Filter and Smoother to the latest versions --- .../nonlinear/ConcurrentBatchFilter.cpp | 724 +++++++++--------- .../nonlinear/ConcurrentBatchFilter.h | 141 ++-- .../nonlinear/ConcurrentBatchSmoother.cpp | 189 ++--- .../nonlinear/ConcurrentBatchSmoother.h | 87 +-- .../ConcurrentFilteringAndSmoothing.cpp | 227 +++++- .../ConcurrentFilteringAndSmoothing.h | 41 +- 6 files changed, 722 insertions(+), 687 deletions(-) diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp index 1e1e39e10..74551f8e4 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp @@ -15,7 +15,6 @@ * Concurrent Filtering and Smoothing interface. * @author Stephen Williams */ - #include #include #include @@ -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(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& 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(factor); + HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast(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(const Values& values, const std::string& indent, const std::string& title, const KeyFormatter& keyFormatter) { + FastList keys = values.keys(); + PrintKeys(keys, indent, title, keyFormatter); +} + +/* ************************************************************************* */ +template<> +void ConcurrentBatchFilter::PrintKeys(const NonlinearFactorGraph& graph, const std::string& indent, const std::string& title, const KeyFormatter& keyFormatter) { + FastSet 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 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 oldKeys = smootherSummarization_.keys(); + std::set 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 filterKeys; - std::set separatorKeys; - std::set smootherKeys; - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, theta_) { - filterKeys.insert(key_value.key); - } - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { - separatorKeys.insert(key_value.key); - filterKeys.erase(key_value.key); - } - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, smootherValues_) { - smootherKeys.insert(key_value.key); - } + 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 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 >& 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& keysToMove) { - // In order to marginalize out the selected variables, the factors involved in those variables - // must be identified and removed. Also, the effect of those removed factors on the - // remaining variables needs to be accounted for. This will be done with linear container factors - // from the result of a partial elimination. This function removes the marginalized factors and - // adds the linearized factors back in. +void ConcurrentBatchFilter::moveSeparator(const FastList& keysToMove) { + // In order to move the separator, we need to calculate the marginal information on the new + // separator from all of the factors on the smoother side (both the factors actually in the + // smoother and the ones to be transitioned to the smoother but stored in the filter). + // This is exactly the same operation that is performed by a fixed-lag smoother, calculating + // a marginal factor from the variables outside the smoother window. + // + // However, for the concurrent system, we would like to calculate this marginal in a particular + // way, such that an intermediate term is produced that provides a "shortcut" between the old + // separator (as defined in the smoother) and the new separator. This will allow us to quickly + // update the new separator with changes at the old separator (from the smoother) + // TODO: This is currently not very efficient: multiple calls to graph.keys(), etc. - // Calculate marginal factors on the remaining variables (after marginalizing 'keyToMove') - // Note: It is assumed the ordering already has these keys first +// 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 removedFactorSlots; + // Identify all of the new factors to be sent to the smoother (any factor involving keysToMove) + std::vector removedFactorSlots; + VariableIndex variableIndex(*factors_.symbolic(ordering_), theta_.size()); BOOST_FOREACH(Key key, keysToMove) { const FastList& slots = variableIndex[ordering_.at(key)]; - removedFactorSlots.insert(slots.begin(), slots.end()); + removedFactorSlots.insert(removedFactorSlots.end(), slots.begin(), slots.end()); + } + // Sort and remove duplicates + std::sort(removedFactorSlots.begin(), removedFactorSlots.end()); + removedFactorSlots.erase(std::unique(removedFactorSlots.begin(), removedFactorSlots.end()), removedFactorSlots.end()); + // Remove any linear/marginal factor that made it into the set + BOOST_FOREACH(size_t index, separatorSummarizationSlots_) { + removedFactorSlots.erase(std::remove(removedFactorSlots.begin(), removedFactorSlots.end(), index), removedFactorSlots.end()); } - // Construct an elimination tree to perform sparse elimination - std::vector forest( EliminationForest::Create(linearFactorGraph, variableIndex) ); - - // This is a tree. Only the top-most nodes/indices need to be eliminated; all of the children will be eliminated automatically - // Find the subset of nodes/keys that must be eliminated - std::set indicesToEliminate; - BOOST_FOREACH(Key key, keysToMove) { - indicesToEliminate.insert(ordering_.at(key)); - } - BOOST_FOREACH(Key key, keysToMove) { - EliminationForest::removeChildrenIndices(indicesToEliminate, forest.at(ordering_.at(key))); - } - - // Eliminate each top-most key, returning a Gaussian Factor on some of the remaining variables - // Convert the marginal factors into Linear Container Factors - // Add the marginal factor variables to the separator - NonlinearFactorGraph marginalFactors; - BOOST_FOREACH(Index index, indicesToEliminate) { - GaussianFactor::shared_ptr gaussianFactor = forest.at(index)->eliminateRecursive(parameters_.getEliminationFunction()); - if(gaussianFactor->size() > 0) { - LinearContainerFactor::shared_ptr marginalFactor(new LinearContainerFactor(gaussianFactor, ordering_, theta_)); - marginalFactors.push_back(marginalFactor); - // Add the keys associated with the marginal factor to the separator values - BOOST_FOREACH(Key key, *marginalFactor) { - if(!separatorValues_.exists(key)) { - separatorValues_.insert(key, theta_.at(key)); - } - } - } - } - std::vector marginalSlots = insertFactors(marginalFactors); - - - // Cache marginalized variables and factors for later transmission to the smoother - { - // Add the new marginal factors to the list of smootherSeparatorFactors. In essence, we have just moved the separator - smootherSummarizationSlots_.insert(smootherSummarizationSlots_.end(), marginalSlots.begin(), marginalSlots.end()); - - // Move the marginalized factors from the filter to the smoother (holding area) - // Note: Be careful to only move nonlinear factors and not any marginals that may also need to be removed + // TODO: Make this compact + if(debug) { + std::cout << "ConcurrentBatchFilter::moveSeparator Removed Factor Slots: "; BOOST_FOREACH(size_t slot, removedFactorSlots) { - std::vector::iterator iter = std::find(smootherSummarizationSlots_.begin(), smootherSummarizationSlots_.end(), slot); - if(iter == smootherSummarizationSlots_.end()) { - // This is a real nonlinear factor. Add it to the smoother factor cache. - smootherFactors_.push_back(factors_.at(slot)); - } else { - // This is a marginal factor that was removed and replaced by a new marginal factor. Remove this slot from the separator factor list. - smootherSummarizationSlots_.erase(iter); - } + 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 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 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 slots(removedFactorSlots.begin(), removedFactorSlots.end()); - removeFactors(slots); - - // Remove marginalized keys from values (and separator) - BOOST_FOREACH(Key key, keysToMove) { - theta_.erase(key); - if(separatorValues_.exists(key)) { - separatorValues_.erase(key); - } - } - - // Permute the ordering such that the removed keys are at the end. - // This is a prerequisite for removing them from several structures - std::vector toBack; - BOOST_FOREACH(Key key, keysToMove) { - toBack.push_back(ordering_.at(key)); - } - Permutation forwardPermutation = Permutation::PushToBack(toBack, ordering_.size()); - ordering_.permuteInPlace(forwardPermutation); - delta_.permuteInPlace(forwardPermutation); - - // Remove marginalized keys from the ordering and delta - for(size_t i = 0; i < keysToMove.size(); ++i) { - ordering_.pop_back(); - delta_.pop_back(); - } + // 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 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& 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 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 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(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 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::max(); - - // Allocate result parent vector and vector of last factor columns - std::vector parents(n, none); - std::vector 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::Create(const GaussianFactorGraph& factorGraph, const VariableIndex& structure) { - // Compute the tree structure - std::vector parents(ComputeParents(structure)); - - // Number of variables - const size_t n = structure.size(); - - static const Index none = std::numeric_limits::max(); - - // Create tree structure - std::vector 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& 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 diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h index 9a7690786..b0d8ab546 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h @@ -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 >& 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 availableSlots_; ///< The set of available factor graph slots caused by deleting factors - Values separatorValues_; ///< The linearization points of the separator variables. These should not be updated during optimization. - std::vector smootherSummarizationSlots_; ///< The slots in factor graph that correspond to the current smoother summarization factors - - // 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 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 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 >& keysToMove = boost::none); + /** Marginalize out the set of requested variables from the filter, caching them for the smoother + * This effectively moves the separator. + * + * @param keysToMove The set of keys to move from the filter to the smoother + */ + void moveSeparator(const FastList& keysToMove); + /** Use a modified version of L-M to update the linearization point and delta */ static Result optimize(const NonlinearFactorGraph& factors, Values& theta, const Ordering& ordering, VectorValues& delta, const Values& linearValues, const LevenbergMarquardtParams& parameters); - /** Marginalize out the set of requested variables from the filter, caching them for the smoother - * This effectively moves the separator. - */ - void marginalize(const FastList& keysToMove); - - /** Marginalize out the set of requested variables from the filter, caching them for the smoother - * This effectively moves the separator. - */ - static NonlinearFactorGraph marginalize(const NonlinearFactorGraph& graph, const Values& values, - const Ordering& ordering, const std::set& 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& 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 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 Factors; - typedef FastList SubTrees; - typedef std::vector 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 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 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& indices, const EliminationForest::shared_ptr& tree); - }; + /** Print just the nonlinear keys contained inside a container */ + template + static void PrintKeys(const Container& keys, const std::string& indent = "", + const std::string& title = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter); }; // ConcurrentBatchFilter +/// Implementation of PrintKeys +template +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; diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp index 1e9d4a2a9..87e20cb13 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp @@ -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= 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 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 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 separatorKeys; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { - indicesToEliminate.erase(ordering_.at(key_value.key)); - } - std::vector 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 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::max(); - - // Allocate result parent vector and vector of last factor columns - std::vector parents(n, none); - std::vector 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::Create(const GaussianFactorGraph& factorGraph, const VariableIndex& structure) { - // Compute the tree structure - std::vector parents(ComputeParents(structure)); - - // Number of variables - const size_t n = structure.size(); - - static const Index none = std::numeric_limits::max(); - - // Create tree structure - std::vector 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& 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 diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h index f54c1f60d..8c6ea7dda 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h @@ -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 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 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 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 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 shared_ptr; ///< Shared pointer to this class - - private: - typedef FastList Factors; - typedef FastList SubTrees; - typedef std::vector 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 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 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& indices, const EliminationForest::shared_ptr& tree); - }; - }; // ConcurrentBatchSmoother /// Typedef for Matlab wrapping diff --git a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp index 40a1de802..92a57bb5c 100644 --- a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp @@ -19,10 +19,12 @@ // \callgraph -#include "ConcurrentFilteringAndSmoothing.h" +#include +#include 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 shared_ptr; ///< Shared pointer to this class + +private: + typedef FastList Factors; + typedef FastList SubTrees; + typedef std::vector 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 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 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& indices, const EliminationForest::shared_ptr& tree); +}; + +/* ************************************************************************* */ +std::vector 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::max(); + + // Allocate result parent vector and vector of last factor columns + std::vector parents(n, none); + std::vector 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::Create(const GaussianFactorGraph& factorGraph, const VariableIndex& structure) { + // Compute the tree structure + std::vector parents(ComputeParents(structure)); + + // Number of variables + const size_t n = structure.size(); + + static const Index none = std::numeric_limits::max(); + + // Create tree structure + std::vector 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& 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& remainingKeys, const GaussianFactorGraph::Eliminate& eliminateFunction) { + + // Calculate the set of RootKeys = AllKeys \Intersect RemainingKeys + FastSet rootKeys; + FastSet 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 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 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 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 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 diff --git a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h index 71fe45b47..32b6b5231 100644 --- a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h +++ b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h @@ -39,22 +39,18 @@ class GTSAM_UNSTABLE_EXPORT ConcurrentFilter { public: typedef boost::shared_ptr 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 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& remainingKeys, const GaussianFactorGraph::Eliminate& eliminateFunction); + +} + }/// namespace gtsam