From 5cd020080bef4ca753a0378c23b7c433741593e5 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Wed, 10 Apr 2013 12:47:55 +0000 Subject: [PATCH] Added marginalization code and unit tests to ConcurrentBatchFilter --- .../nonlinear/ConcurrentBatchFilter.cpp | 230 ++++++++++++++++-- .../nonlinear/ConcurrentBatchFilter.h | 78 +++++- .../tests/testConcurrentBatchFilter.cpp | 185 +++++++------- 3 files changed, 364 insertions(+), 129 deletions(-) diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp index 7026a625c..4c8e68bbe 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp @@ -88,14 +88,7 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::update(const NonlinearFacto gttic(marginalize); if(keysToMove && keysToMove->size() > 0){ -// // Generate a permutation that will put the factor graph into the proper order -// std::set activeKeys; -// std::set marginalizableKeys; -// calculateKeySets(activeKeys, marginalizableKeys, registeredSensors_, ordering_, keyTimestampMap_, currentTimestamp, parameters_.smootherLag); -// permuteOrdering(ordering_, factors_, marginalizableKeys, activeKeys); -// -// // Marginalize out inactive states (and remove from ordering/values) -// marginalize(factors_, theta_, ordering_, keyTimestampMap_, linearValues_, marginalizableKeys); + marginalize(*keysToMove); } gttoc(marginalize); @@ -175,16 +168,13 @@ std::vector ConcurrentBatchFilter::insertFactors(const NonlinearFactorGr slots.push_back(slot); } - // Augment the Variable Index - variableIndex_.augment(*factors.symbolic(ordering_)); - gttoc(insert_factors); return slots; } /* ************************************************************************* */ -void ConcurrentBatchFilter::removeFactors(const std::vector& slots) { +void ConcurrentBatchFilter::removeFactors(const std::set& slots) { gttic(remove_factors); @@ -201,19 +191,18 @@ void ConcurrentBatchFilter::removeFactors(const std::vector& slots) { availableSlots_.push(slot); } - // Remove references to this factor from the VariableIndex - variableIndex_.remove(slots, factors); - gttoc(remove_factors); } /* ************************************************************************* */ void ConcurrentBatchFilter::reorder(const boost::optional >& keysToMove) { - // COLAMD groups will be used to place marginalize keys in Group 0, active keys in Group 1, and separator keys in Group 2 + // Recalculate the variable index + variableIndex_ = VariableIndex(*factors_.symbolic(ordering_)); + + // COLAMD groups will be used to place marginalize keys in Group 0, and everything else in Group 1 int group0 = 0; - int group1 = group0 + (keysToMove ? 1 : 0); - int group2 = group1 + 1; + int group1 = keysToMove ? 1 : 0; // Initialize all variables to group1 std::vector cmember(variableIndex_.size(), group1); @@ -225,13 +214,6 @@ void ConcurrentBatchFilter::reorder(const boost::optional >& keysT } } - // Set all of the separator keys to Group2 - if(separatorValues_.size() > 0) { - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { - cmember[ordering_.at(key_value.key)] = group2; - } - } - // Generate the permutation Permutation forwardPermutation = *inference::PermutationCOLAMD_(variableIndex_, cmember); @@ -349,6 +331,114 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::optimize() { return result; } +/* ************************************************************************* */ +void ConcurrentBatchFilter::marginalize(const FastList& 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. + + std::set removedFactorSlots; + std::vector marginalSlots; + + // Calculate marginal factors on the remaining variables (after marginalizing 'keyToMove') + // 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)]; + removedFactorSlots.insert(slots.begin(), slots.end()); + } + + // Create the linear factor graph + gtsam::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) { + indicesToEliminate.insert(ordering_.at(key)); + } + BOOST_FOREACH(gtsam::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(gtsam::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) { + if(!separatorValues_.exists(key)) { + separatorValues_.insert(key, theta_.at(key)); + } + } + } + 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 they may also need to be removed + 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); + } + } + + // Add the linearization point of the moved variables to the smoother cache + BOOST_FOREACH(gtsam::Key key, keysToMove) { + smootherValues_.insert(key, theta_.at(key)); + } + } + + // Remove the marginalized variables and factors from the filter + { + // Remove marginalized factors from the factor graph + removeFactors(removedFactorSlots); + + // Remove marginalized keys from values (and separator) + BOOST_FOREACH(gtsam::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(gtsam::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, variableIndex, and delta + for(size_t i = 0; i < keysToMove.size(); ++i) { + ordering_.pop_back(); + delta_.pop_back(); + } + } +} /* ************************************************************************* */ void ConcurrentBatchFilter::PrintNonlinearFactor(const NonlinearFactor::shared_ptr& factor, @@ -384,6 +474,96 @@ void ConcurrentBatchFilter::PrintLinearFactor(const GaussianFactor::shared_ptr& } } +/* ************************************************************************* */ +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 gtsam::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 (gtsam::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]; + // find root r of the current tree that contains k + gtsam::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 gtsam::GaussianFactorGraph& factorGraph, const gtsam::VariableIndex& structure) { + // Compute the tree structure + std::vector parents(ComputeParents(structure)); + + // Number of variables + const size_t n = structure.size(); + + static const gtsam::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 + 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) { + if(factor && factor->size() > 0) { + gtsam::Index j = *std::min_element(factor->begin(), factor->end()); + if(j < structure.size()) + trees[j]->add(factor); + } + } + + return trees; +} + +/* ************************************************************************* */ +ConcurrentBatchFilter::EliminationForest::sharedFactor ConcurrentBatchFilter::EliminationForest::eliminateRecursive(Eliminate function) { + + // Create the list of factors to be eliminated, initially empty, and reserve space + gtsam::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 + gtsam::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); + } +} + ///* ************************************************************************* */ //void ConcurrentBatchFilter::synchronize(const NonlinearFactorGraph& summarizedFactors) { // diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h index 481563e17..2564b1d8c 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h @@ -76,16 +76,11 @@ public: return delta_; } - /** Access the nonlinear variable index */ - const VariableIndex& getVariableIndex() const { - return variableIndex_; - } - /** Compute the current best estimate of all variables and return a full Values structure. * If only a single variable is needed, it may be faster to call calculateEstimate(const KEY&). */ Values calculateEstimate() const { - return getLinearizationPoint().retract(getDelta(), getOrdering()); + return theta_.retract(delta_, ordering_); } /** Compute the current best estimate of a single variable. This is generally faster than @@ -95,9 +90,9 @@ public: */ template VALUE calculateEstimate(Key key) const { - const Index index = getOrdering().at(key); - const Vector delta = getDelta().at(index); - return getLinearizationPoint().at(key).retract(delta); + const Index index = ordering_.at(key); + const Vector delta = delta_.at(index); + return theta_.at(key).retract(delta); } /** @@ -122,7 +117,7 @@ protected: 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 - VariableIndex variableIndex_; ///< The current variable index, which allows efficient factor lookup by variable + VariableIndex variableIndex_; ///< The current variable index, which allows efficient factor lookup by variable. Note: after marginalization, this is left in an inconsistent state 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 @@ -183,8 +178,8 @@ private: /** Remove factors from the graph by slot index * * @param slots The slots in the factor graph that should be deleted - * */ - void removeFactors(const std::vector& slots); + */ + void removeFactors(const std::set& slots); /** Use colamd to update into an efficient ordering */ void reorder(const boost::optional >& keysToMove = boost::none); @@ -192,6 +187,11 @@ private: /** Use a modified version of L-M to update the linearization point and delta */ Result optimize(); + /** 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); + /** 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); @@ -200,6 +200,60 @@ 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 + 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; + + gtsam::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) {} + + /** + * 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); + + /** add a factor, for Create use only */ + void add(const sharedFactor& 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 */ + gtsam::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 gtsam::GaussianFactorGraph& factorGraph, const gtsam::VariableIndex& structure); + + /** Recursive routine that eliminates the factors arranged in an elimination tree */ + sharedFactor eliminateRecursive(Eliminate function); + + /** Recursive function that helps find the top of each tree */ + static void removeChildrenIndices(std::set& indices, const EliminationForest::shared_ptr& tree); + }; + }; // ConcurrentBatchFilter }/// namespace gtsam diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp index 666dddd05..45892200f 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp @@ -440,47 +440,47 @@ TEST_UNSAFE( ConcurrentBatchFilter, update_batch ) CHECK(assert_equal(expected, actual, 1e-4)); } -///* ************************************************************************* */ -//TEST_UNSAFE( ConcurrentBatchFilter, update_batch_with_marginalization ) -//{ -// // Test the 'update' function of the ConcurrentBatchFilter in a nonlinear environment. -// // Thus, a full L-M optimization and the ConcurrentBatchFilter results should be identical -// // This tests adds all of the factors to the filter at once (i.e. batch) -// -// // Create a set of optimizer parameters -// LevenbergMarquardtParams parameters; -// -// // Create a Concurrent Batch Filter -// ConcurrentBatchFilter filter(parameters); -// -// // Create containers to keep the full graph -// Values fullTheta; -// NonlinearFactorGraph fullGraph; -// -// // Create all factors -// CreateFactors(fullGraph, fullTheta, 0, 20); -// -// // Create the set of key to marginalize out -// FastList marginalizeKeys; -// for(size_t j = 0; j < 15; ++j) { -// marginalizeKeys.push_back(Symbol('X', j)); -// } -// -// // Optimize with Concurrent Batch Filter -// filter.update(fullGraph, fullTheta, marginalizeKeys); -// Values actual = filter.calculateEstimate(); -// -// -// // Optimize with L-M -// Values expected = BatchOptimize(fullGraph, fullTheta); -// // Remove the marginalized keys -// for(size_t j = 0; j < 15; ++j) { -// expected.erase(Symbol('X', j)); -// } -// -// // Check smoother versus batch -// CHECK(assert_equal(expected, actual, 1e-4)); -//} +/* ************************************************************************* */ +TEST_UNSAFE( ConcurrentBatchFilter, update_batch_with_marginalization ) +{ + // Test the 'update' function of the ConcurrentBatchFilter in a nonlinear environment. + // Thus, a full L-M optimization and the ConcurrentBatchFilter results should be identical + // This tests adds all of the factors to the filter at once (i.e. batch) + + // Create a set of optimizer parameters + LevenbergMarquardtParams parameters; + + // Create a Concurrent Batch Filter + ConcurrentBatchFilter filter(parameters); + + // Create containers to keep the full graph + Values fullTheta; + NonlinearFactorGraph fullGraph; + + // Create all factors + CreateFactors(fullGraph, fullTheta, 0, 20); + + // Create the set of key to marginalize out + FastList marginalizeKeys; + for(size_t j = 0; j < 15; ++j) { + marginalizeKeys.push_back(Symbol('X', j)); + } + + // Optimize with Concurrent Batch Filter + filter.update(fullGraph, fullTheta, marginalizeKeys); + Values actual = filter.calculateEstimate(); + + + // Optimize with L-M + Values expected = BatchOptimize(fullGraph, fullTheta); + // Remove the marginalized keys + for(size_t j = 0; j < 15; ++j) { + expected.erase(Symbol('X', j)); + } + + // Check smoother versus batch + CHECK(assert_equal(expected, actual, 1e-4)); +} /* ************************************************************************* */ TEST_UNSAFE( ConcurrentBatchFilter, update_incremental ) @@ -524,57 +524,58 @@ TEST_UNSAFE( ConcurrentBatchFilter, update_incremental ) } -///* ************************************************************************* */ -//TEST_UNSAFE( ConcurrentBatchFilter, update_incremental_with_marginalization ) -//{ -// // Test the 'update' function of the ConcurrentBatchFilter in a nonlinear environment. -// // Thus, a full L-M optimization and the ConcurrentBatchFilter results should be identical -// // This tests adds the factors to the filter as they are created (i.e. incrementally) -// -// // Create a set of optimizer parameters -// LevenbergMarquardtParams parameters; -// -// // Create a Concurrent Batch Filter -// ConcurrentBatchFilter filter(parameters); -// -// // Create containers to keep the full graph -// Values fullTheta; -// NonlinearFactorGraph fullGraph; -// -// // Add odometry from time 0 to time 10 -// for(size_t i = 0; i < 20; ++i) { -// // Create containers to keep the new factors -// Values newTheta; -// NonlinearFactorGraph newGraph; -// -// // Create factors -// CreateFactors(newGraph, newTheta, i, i+1); -// -// // Create the set of factors to marginalize -// FastList marginalizeKeys; -// if(i >= 4) { -// marginalizeKeys.push_back(Symbol('X', i-4)); -// } -// -// // Add these entries to the filter -// filter.update(newGraph, newTheta, marginalizeKeys); -// Values actual = filter.calculateEstimate(); -// -// // Add these entries to the full batch version -// fullGraph.push_back(newGraph); -// fullTheta.insert(newTheta); -// Values expected = BatchOptimize(fullGraph, fullTheta); -// fullTheta = expected; -// // Remove marginalized keys -// for(int j = 0; j < (int)i - 4; ++j) { -// expected.erase(Symbol('X', j)); -// } -// -// // Compare filter solution with full batch -// CHECK(assert_equal(expected, actual, 1e-4)); -// } -// -//} +/* ************************************************************************* */ +TEST_UNSAFE( ConcurrentBatchFilter, update_incremental_with_marginalization ) +{ + // Test the 'update' function of the ConcurrentBatchFilter in a nonlinear environment. + // Thus, a full L-M optimization and the ConcurrentBatchFilter results should be identical + // This tests adds the factors to the filter as they are created (i.e. incrementally) + + // Create a set of optimizer parameters + LevenbergMarquardtParams parameters; + + // Create a Concurrent Batch Filter + ConcurrentBatchFilter filter(parameters); + + // Create containers to keep the full graph + Values fullTheta; + NonlinearFactorGraph fullGraph; + + // Add odometry from time 0 to time 10 + for(size_t i = 0; i < 20; ++i) { + + // Create containers to keep the new factors + Values newTheta; + NonlinearFactorGraph newGraph; + + // Create factors + CreateFactors(newGraph, newTheta, i, i+1); + + // Create the set of factors to marginalize + FastList marginalizeKeys; + if(i >= 5) { + marginalizeKeys.push_back(Symbol('X', i-5)); + } + + // Add these entries to the filter + filter.update(newGraph, newTheta, marginalizeKeys); + Values actual = filter.calculateEstimate(); + + // Add these entries to the full batch version + fullGraph.push_back(newGraph); + fullTheta.insert(newTheta); + Values expected = BatchOptimize(fullGraph, fullTheta); + fullTheta = expected; + // Remove marginalized keys + for(int j = (int)i - 5; j >= 0; --j) { + expected.erase(Symbol('X', j)); + } + + // Compare filter solution with full batch + CHECK(assert_equal(expected, actual, 1e-4)); + } + +} ///* ************************************************************************* */ //TEST_UNSAFE( ConcurrentBatchFilter, synchronize )