From 04d595dec1b0d4af82b3937a0afe68f8331b1598 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Thu, 11 Apr 2013 12:43:42 +0000 Subject: [PATCH] Started re-implementing the synchronization functions for the Concurrent Filter --- .../nonlinear/ConcurrentBatchFilter.cpp | 138 ++++++++++++++---- .../nonlinear/ConcurrentBatchFilter.h | 38 +++-- .../tests/testConcurrentBatchFilter.cpp | 2 +- 3 files changed, 130 insertions(+), 48 deletions(-) diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp index 9e7393c13..56857ef44 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp @@ -105,10 +105,43 @@ void ConcurrentBatchFilter::presync() { } /* ************************************************************************* */ -void ConcurrentBatchFilter::synchronize(const NonlinearFactorGraph& summarizedFactors) { +void ConcurrentBatchFilter::synchronize(const NonlinearFactorGraph& summarizedFactors, const Values& separatorValues) { gttic(synchronize); + // Remove the previous smoother summarization + removeFactors(smootherSummarizationSlots_); + + // 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 filterGraph; + filterGraph.push_back(factors_); + filterGraph.push_back(smootherFactors_); + filterGraph.push_back(summarizedFactors); + Values filterValues; + filterValues.insert(theta_); + filterValues.insert(smootherValues_); + filterValues.update(separatorValues); // ensure the smoother summarized factors are linearized around the values in the smoother + + // Optimize this graph using a modified version of L-M + // TODO: + + // 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. + Ordering filterOrdering; + std::vector filterKeys; + filterSummarization_ = marginalize(filterGraph, filterValues, filterOrdering, filterKeys, parameters_.getEliminationFunction()); + + // 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 + Ordering smootherOrdering; + std::vector smootherKeys; + smootherSummarizationSlots_ = insertFactors( marginalize(filterGraph, filterValues, smootherOrdering, smootherKeys, parameters_.getEliminationFunction()) ); + + gttoc(synchronize); } @@ -173,7 +206,7 @@ std::vector ConcurrentBatchFilter::insertFactors(const NonlinearFactorGr } /* ************************************************************************* */ -void ConcurrentBatchFilter::removeFactors(const std::set& slots) { +void ConcurrentBatchFilter::removeFactors(const std::vector& slots) { gttic(remove_factors); @@ -345,24 +378,24 @@ void ConcurrentBatchFilter::marginalize(const FastList& keysToMove) { // Note: It is assumed the ordering already has these keys first { // Use the variable Index to mark the factors that will be marginalized - BOOST_FOREACH(gtsam::Key key, keysToMove) { - const gtsam::FastList& slots = variableIndex_[ordering_.at(key)]; + BOOST_FOREACH(Key key, keysToMove) { + const FastList& slots = variableIndex_[ordering_.at(key)]; removedFactorSlots.insert(slots.begin(), slots.end()); } // Create the linear factor graph - gtsam::GaussianFactorGraph linearFactorGraph = *factors_.linearize(theta_, ordering_); + GaussianFactorGraph linearFactorGraph = *factors_.linearize(theta_, ordering_); // 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(gtsam::Key key, keysToMove) { + std::set indicesToEliminate; + BOOST_FOREACH(Key key, keysToMove) { indicesToEliminate.insert(ordering_.at(key)); } - BOOST_FOREACH(gtsam::Key key, keysToMove) { + BOOST_FOREACH(Key key, keysToMove) { EliminationForest::removeChildrenIndices(indicesToEliminate, forest.at(ordering_.at(key))); } @@ -370,12 +403,12 @@ void ConcurrentBatchFilter::marginalize(const FastList& keysToMove) { // Convert the marginal factors into Linear Container Factors // Add the marginal factor variables to the separator NonlinearFactorGraph marginalFactors; - BOOST_FOREACH(gtsam::Index index, indicesToEliminate) { + BOOST_FOREACH(Index index, indicesToEliminate) { GaussianFactor::shared_ptr gaussianFactor = forest.at(index)->eliminateRecursive(parameters_.getEliminationFunction()); 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(gtsam::Key key, *marginalFactor) { + BOOST_FOREACH(Key key, *marginalFactor) { if(!separatorValues_.exists(key)) { separatorValues_.insert(key, theta_.at(key)); } @@ -403,7 +436,7 @@ void ConcurrentBatchFilter::marginalize(const FastList& keysToMove) { } // Add the linearization point of the moved variables to the smoother cache - BOOST_FOREACH(gtsam::Key key, keysToMove) { + BOOST_FOREACH(Key key, keysToMove) { smootherValues_.insert(key, theta_.at(key)); } } @@ -411,10 +444,11 @@ void ConcurrentBatchFilter::marginalize(const FastList& keysToMove) { // Remove the marginalized variables and factors from the filter { // Remove marginalized factors from the factor graph - removeFactors(removedFactorSlots); + std::vector slots(removedFactorSlots.begin(), removedFactorSlots.end()); + removeFactors(slots); // Remove marginalized keys from values (and separator) - BOOST_FOREACH(gtsam::Key key, keysToMove) { + BOOST_FOREACH(Key key, keysToMove) { theta_.erase(key); if(separatorValues_.exists(key)) { separatorValues_.erase(key); @@ -423,8 +457,8 @@ void ConcurrentBatchFilter::marginalize(const FastList& keysToMove) { // 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(gtsam::Key key, keysToMove) { + std::vector toBack; + BOOST_FOREACH(Key key, keysToMove) { toBack.push_back(ordering_.at(key)); } Permutation forwardPermutation = Permutation::PushToBack(toBack, ordering_.size()); @@ -439,6 +473,48 @@ void ConcurrentBatchFilter::marginalize(const FastList& keysToMove) { } } +/* ************************************************************************* */ +NonlinearFactorGraph ConcurrentBatchFilter::marginalize(const NonlinearFactorGraph& graph, const Values& values, + const Ordering& ordering, const std::vector& marginalizeKeys, const GaussianFactorGraph::Eliminate& function) { + + // This function returns marginal factors (in the form of LinearContainerFactors) that result from + // marginalizing out the selected variables. + + // 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); + + // 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); + 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) { @@ -479,20 +555,20 @@ std::vector ConcurrentBatchFilter::EliminationForest::ComputeParents(cons const size_t m = structure.nFactors(); const size_t n = structure.size(); - static const gtsam::Index none = std::numeric_limits::max(); + 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); + std::vector parents(n, none); + std::vector prevCol(m, none); // for column j \in 1 to n do - for (gtsam::Index j = 0; j < n; j++) { + 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) { - gtsam::Index k = prevCol[i]; + Index k = prevCol[i]; // find root r of the current tree that contains k - gtsam::Index r = k; + Index r = k; while (parents[r] != none) r = parents[r]; if (r != j) parents[r] = j; @@ -505,28 +581,28 @@ std::vector ConcurrentBatchFilter::EliminationForest::ComputeParents(cons } /* ************************************************************************* */ -std::vector ConcurrentBatchFilter::EliminationForest::Create(const gtsam::GaussianFactorGraph& factorGraph, const gtsam::VariableIndex& structure) { +std::vector ConcurrentBatchFilter::EliminationForest::Create(const GaussianFactorGraph& factorGraph, const VariableIndex& structure) { // Compute the tree structure - std::vector parents(ComputeParents(structure)); + std::vector parents(ComputeParents(structure)); // Number of variables const size_t n = structure.size(); - static const gtsam::Index none = std::numeric_limits::max(); + static const Index none = std::numeric_limits::max(); // Create tree structure std::vector trees(n); - for (gtsam::Index k = 1; k <= n; k++) { - gtsam::Index j = n - k; // Start at the last variable and loop down to 0 + 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 sharedFactor& factor, factorGraph) { + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factorGraph) { if(factor && factor->size() > 0) { - gtsam::Index j = *std::min_element(factor->begin(), factor->end()); + Index j = *std::min_element(factor->begin(), factor->end()); if(j < structure.size()) trees[j]->add(factor); } @@ -536,10 +612,10 @@ std::vector ConcurrentBatc } /* ************************************************************************* */ -ConcurrentBatchFilter::EliminationForest::sharedFactor ConcurrentBatchFilter::EliminationForest::eliminateRecursive(Eliminate function) { +GaussianFactor::shared_ptr ConcurrentBatchFilter::EliminationForest::eliminateRecursive(GaussianFactorGraph::Eliminate function) { // Create the list of factors to be eliminated, initially empty, and reserve space - gtsam::GaussianFactorGraph factors; + GaussianFactorGraph factors; factors.reserve(this->factors_.size() + this->subTrees_.size()); // Add all factors associated with the current node @@ -550,7 +626,7 @@ ConcurrentBatchFilter::EliminationForest::sharedFactor ConcurrentBatchFilter::El factors.push_back(child->eliminateRecursive(function)); // Combine all factors (from this node and from subtrees) into a joint factor - gtsam::GaussianFactorGraph::EliminationResult eliminated(function(factors, 1)); + GaussianFactorGraph::EliminationResult eliminated(function(factors, 1)); return eliminated.second; } diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h index 2564b1d8c..fa2986054 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h @@ -157,7 +157,7 @@ protected: * * @param summarizedFactors An updated version of the smoother branch summarized factors */ - virtual void synchronize(const NonlinearFactorGraph& summarizedFactors); + virtual void synchronize(const NonlinearFactorGraph& summarizedFactors, const Values& separatorValues); /** * Perform any required operations after the synchronization process finishes. @@ -179,7 +179,7 @@ private: * * @param slots The slots in the factor graph that should be deleted */ - void removeFactors(const std::set& slots); + void removeFactors(const std::vector& slots); /** Use colamd to update into an efficient ordering */ void reorder(const boost::optional >& keysToMove = boost::none); @@ -192,6 +192,12 @@ private: */ 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::vector& 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); @@ -204,31 +210,31 @@ private: class EliminationForest { public: typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class - typedef gtsam::GaussianFactor Factor; ///< The factor Type - typedef Factor::shared_ptr sharedFactor; ///< Shared pointer to a factor - typedef gtsam::BayesNet BayesNet; ///< The BayesNet - typedef gtsam::GaussianFactorGraph::Eliminate Eliminate; ///< The eliminate subroutine +// typedef GaussianFactor Factor; ///< The factor Type +// typedef Factor::shared_ptr sharedFactor; ///< Shared pointer to a factor +// typedef BayesNet BayesNet; ///< The BayesNet +// typedef GaussianFactorGraph::Eliminate Eliminate; ///< The eliminate subroutine private: - typedef gtsam::FastList Factors; - typedef gtsam::FastList SubTrees; - typedef std::vector Conditionals; + typedef FastList Factors; + typedef FastList SubTrees; + typedef std::vector Conditionals; - gtsam::Index key_; ///< index associated with root + 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(gtsam::Index key = 0) : key_(key) {} + 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 gtsam::VariableIndex& structure); + static std::vector ComputeParents(const VariableIndex& structure); /** add a factor, for Create use only */ - void add(const sharedFactor& factor) { factors_.push_back(factor); } + 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); } @@ -236,7 +242,7 @@ private: public: /** return the key associated with this tree node */ - gtsam::Index key() const { return key_; } + Index key() const { return key_; } /** return the const reference of children */ const SubTrees& children() const { return subTrees_; } @@ -245,10 +251,10 @@ private: const Factors& factors() const { return factors_; } /** Create an elimination tree from a factor graph */ - static std::vector Create(const gtsam::GaussianFactorGraph& factorGraph, const gtsam::VariableIndex& structure); + static std::vector Create(const GaussianFactorGraph& factorGraph, const VariableIndex& structure); /** Recursive routine that eliminates the factors arranged in an elimination tree */ - sharedFactor eliminateRecursive(Eliminate function); + 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); diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp index 45892200f..f36ff31ec 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp @@ -52,7 +52,7 @@ public: void presync() { ConcurrentBatchFilter::presync(); }; void getSummarizedFactors(NonlinearFactorGraph& summarizedFactors, Values& rootValues) { ConcurrentBatchFilter::getSummarizedFactors(summarizedFactors, rootValues); }; void getSmootherFactors(NonlinearFactorGraph& smootherFactors, Values& smootherValues) { ConcurrentBatchFilter::getSmootherFactors(smootherFactors, smootherValues); }; - void synchronize(const NonlinearFactorGraph& summarizedFactors) { ConcurrentBatchFilter::synchronize(summarizedFactors); }; + void synchronize(const NonlinearFactorGraph& summarizedFactors, const Values& separatorValues) { ConcurrentBatchFilter::synchronize(summarizedFactors, separatorValues); }; void postsync() { ConcurrentBatchFilter::postsync(); }; };