From 634a4c5ef9c3f69b25a9c79ced18daea7e3d5bfd Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Thu, 11 Apr 2013 12:42:45 +0000 Subject: [PATCH] Updated Concurrent Smoother for changes in the base class synchronization --- .../nonlinear/ConcurrentBatchSmoother.cpp | 43 ++++++++++--------- .../nonlinear/ConcurrentBatchSmoother.h | 26 +++++------ .../tests/testConcurrentBatchSmoother.cpp | 10 +++-- 3 files changed, 40 insertions(+), 39 deletions(-) diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp index 6ba9a3bd0..4bb9bb0bf 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp @@ -111,13 +111,16 @@ void ConcurrentBatchSmoother::presync() { } /* ************************************************************************* */ -void ConcurrentBatchSmoother::getSummarizedFactors(NonlinearFactorGraph& summarizedFactors) { +void ConcurrentBatchSmoother::getSummarizedFactors(NonlinearFactorGraph& summarizedFactors, Values& separatorValues) { gttic(get_summarized_factors); // Copy the previous calculated smoother summarization factors into the output summarizedFactors.push_back(smootherSummarization_); + // Copy the separator values into the output + separatorValues.insert(separatorValues_); + gttoc(get_summarized_factors); } @@ -376,14 +379,14 @@ void ConcurrentBatchSmoother::updateSmootherSummarization() { smootherSummarization_.resize(0); // 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 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; + std::set indicesToEliminate; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, theta_) { indicesToEliminate.insert(ordering_.at(key_value.key)); } @@ -397,7 +400,7 @@ void ConcurrentBatchSmoother::updateSmootherSummarization() { // 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(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_)); smootherSummarization_.push_back(marginalFactor); @@ -443,20 +446,20 @@ std::vector ConcurrentBatchSmoother::EliminationForest::ComputeParents(co 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; @@ -469,28 +472,28 @@ std::vector ConcurrentBatchSmoother::EliminationForest::ComputeParents(co } /* ************************************************************************* */ -std::vector ConcurrentBatchSmoother::EliminationForest::Create(const gtsam::GaussianFactorGraph& factorGraph, const gtsam::VariableIndex& structure) { +std::vector ConcurrentBatchSmoother::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); } @@ -500,10 +503,10 @@ std::vector ConcurrentBa } /* ************************************************************************* */ -ConcurrentBatchSmoother::EliminationForest::sharedFactor ConcurrentBatchSmoother::EliminationForest::eliminateRecursive(Eliminate function) { +GaussianFactor::shared_ptr ConcurrentBatchSmoother::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 @@ -514,7 +517,7 @@ ConcurrentBatchSmoother::EliminationForest::sharedFactor ConcurrentBatchSmoother 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/ConcurrentBatchSmoother.h b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h index 57b0195b6..2b33c8b2d 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h @@ -139,7 +139,7 @@ protected: * * @param summarizedFactors The summarized factors for the filter branch */ - virtual void getSummarizedFactors(NonlinearFactorGraph& summarizedFactors); + virtual void getSummarizedFactors(NonlinearFactorGraph& summarizedFactors, Values& separatorValues); /** * Apply the new smoother factors sent by the filter, and the updated version of the filter @@ -195,31 +195,27 @@ 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 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); } @@ -227,7 +223,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_; } @@ -236,10 +232,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/testConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp index d1c0e79df..1a92dec0e 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp @@ -54,8 +54,8 @@ public: void presync() { ConcurrentBatchSmoother::presync(); }; - void getSummarizedFactors(NonlinearFactorGraph& summarizedFactors) { - ConcurrentBatchSmoother::getSummarizedFactors(summarizedFactors); + void getSummarizedFactors(NonlinearFactorGraph& summarizedFactors, Values& separatorValues) { + ConcurrentBatchSmoother::getSummarizedFactors(summarizedFactors, separatorValues); }; void synchronize(const NonlinearFactorGraph& smootherFactors, const Values& smootherValues, const NonlinearFactorGraph& summarizedFactors, const Values& rootValues) { ConcurrentBatchSmoother::synchronize(smootherFactors, smootherValues, summarizedFactors, rootValues); @@ -573,8 +573,9 @@ TEST_UNSAFE( ConcurrentBatchSmoother, synchronize ) // Perform the synchronization procedure NonlinearFactorGraph actualSmootherSummarization; + Values actualSeparatorValues; smoother.presync(); - smoother.getSummarizedFactors(actualSmootherSummarization); + smoother.getSummarizedFactors(actualSmootherSummarization, actualSeparatorValues); smoother.synchronize(smootherFactors, smootherValues, filterSummarization, rootValues); smoother.postsync(); @@ -663,10 +664,11 @@ TEST_UNSAFE( ConcurrentBatchSmoother, synchronize ) // Now perform a second synchronization to test the smoother-calculated summarization actualSmootherSummarization.resize(0); + actualSeparatorValues.clear(); smootherFactors.resize(0); smootherValues.clear(); smoother.presync(); - smoother.getSummarizedFactors(actualSmootherSummarization); + smoother.getSummarizedFactors(actualSmootherSummarization, actualSeparatorValues); smoother.synchronize(smootherFactors, smootherValues, filterSummarization, rootValues); smoother.postsync();