diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp index a973dd42a..ede077665 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp @@ -42,7 +42,6 @@ bool ConcurrentBatchSmoother::equals(const ConcurrentSmoother& rhs, double tol) && theta_.equals(smoother->theta_) && ordering_.equals(smoother->ordering_) && delta_.equals(smoother->delta_) - && variableIndex_.equals(smoother->variableIndex_) && separatorValues_.equals(smoother->separatorValues_); } @@ -56,22 +55,24 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::update(const NonlinearF // Update all of the internal variables with the new information gttic(augment_system); - // Add the new variables to theta - theta_.insert(newTheta); - // Add new variables to the end of the ordering - std::vector dims; - dims.reserve(newTheta.size()); - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) { - ordering_.push_back(key_value.key); - dims.push_back(key_value.value.dim()); + { + // Add the new variables to theta + theta_.insert(newTheta); + // Add new variables to the end of the ordering + std::vector dims; + dims.reserve(newTheta.size()); + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) { + ordering_.push_back(key_value.key); + dims.push_back(key_value.value.dim()); + } + // Augment Delta + delta_.append(dims); + for(size_t i = delta_.size() - dims.size(); i < delta_.size(); ++i) { + delta_[i].setZero(); + } + // Add the new factors to the graph, updating the variable index + insertFactors(newFactors); } - // Augment Delta - delta_.append(dims); - for(size_t i = delta_.size() - dims.size(); i < delta_.size(); ++i) { - delta_[i].setZero(); - } - // Add the new factors to the graph, updating the variable index - insertFactors(newFactors); gttoc(augment_system); // Reorder the system to ensure efficient optimization (and marginalization) performance @@ -87,181 +88,17 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::update(const NonlinearF gttoc(optimize); + // Moved presync code into the update function. Generally, only one call to smoother.update(*) is performed + // between synchronizations, so no extra work is being done. This also allows the presync code to be performed + // while the filter is still running (instead of during the synchronization when the filter is paused) gttic(presync); + if(separatorValues_.size() > 0) { + updateSmootherSummarization(); + } gttoc(presync); gttoc(update); - - - - - - - - - - -// gttic(update); -// -// // Create result structure -// Result result; -// -// gttic(augment_system); -// // Add the new factors to the graph -// BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, newFactors) { -// insertFactor(factor); -// } -// // Add the new variables to theta -// theta_.insert(newTheta); -// gttoc(augment_system); -// -// // Optimize the graph, updating theta -// gttic(optimize); -// if(graph_.size() > 0){ -// optimize(); -// -// // TODO: fill in the results structure properly -// result.iterations = 0; -// result.nonlinearVariables = theta_.size() - separatorValues_.size(); -// result.linearVariables = separatorValues_.size(); -// result.error = 0; -// } -// gttoc(optimize); -// -// // Move all of the Pre-Sync code to the end of the update. This allows the smoother to perform these -// // calculations while the filter is still running -// gttic(presync); -// // Calculate and store the information passed up to the separator. This requires: -// // 1) Calculate an ordering that forces the separator variables to be in the root -// // 2) Eliminate all of the variables except the root. This produces one or more -// // linear, marginal factors on the separator variables. -// // 3) Convert the marginal factors into nonlinear ones using the 'LinearContainerFactor' -// -// if(separatorValues_.size() > 0) { -// // Force variables associated with root keys to keep the same linearization point -// gttic(enforce_consistency); -// Values linpoint; -// linpoint.insert(theta_); -// linpoint.insert(separatorValues_); -// -////linpoint.print("ConcurrentBatchSmoother::presync() LinPoint:\n"); -// -// gttoc(enforce_consistency); -// -// // Calculate a root-constrained ordering -// gttic(compute_ordering); -// std::map constraints; -// BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { -// constraints[key_value.key] = 1; -// } -// Ordering ordering = *graph_.orderingCOLAMDConstrained(linpoint, constraints); -// gttoc(compute_ordering); -// -// // Create a Bayes Tree using iSAM2 cliques -// gttic(create_bayes_tree); -// JunctionTree jt(*graph_.linearize(linpoint, ordering)); -// ISAM2Clique::shared_ptr root = jt.eliminate(parameters_.getEliminationFunction()); -// BayesTree bayesTree; -// bayesTree.insert(root); -// gttoc(create_bayes_tree); -// -////ordering.print("ConcurrentBatchSmoother::presync() Ordering:\n"); -//std::cout << "ConcurrentBatchSmoother::presync() Root Keys: "; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { std::cout << DefaultKeyFormatter(key_value.key) << " "; } std::cout << std::endl; -//std::cout << "ConcurrentBatchSmoother::presync() Bayes Tree:" << std::endl; -////SymbolicPrintTree(root, ordering, " "); -// -// // Extract the marginal factors from the smoother -// // For any non-filter factor that involves a root variable, -// // calculate its marginal on the root variables using the -// // current linearization point. -// -// // Find all of the smoother branches as the children of root cliques that are not also root cliques -// gttic(find_smoother_branches); -// std::set rootCliques; -// std::set smootherBranches; -// BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { -// const ISAM2Clique::shared_ptr& clique = bayesTree.nodes().at(ordering.at(key_value.key)); -// if(clique) { -// rootCliques.insert(clique); -// smootherBranches.insert(clique->children().begin(), clique->children().end()); -// } -// } -// BOOST_FOREACH(const ISAM2Clique::shared_ptr& rootClique, rootCliques) { -// smootherBranches.erase(rootClique); -// } -// gttoc(find_smoother_branches); -// -// // Extract the cached factors on the root cliques from the smoother branches -// gttic(extract_cached_factors); -// GaussianFactorGraph cachedFactors; -// BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, smootherBranches) { -// cachedFactors.push_back(clique->cachedFactor()); -// } -// gttoc(extract_cached_factors); -// -//std::cout << "ConcurrentBatchSmoother::presync() Cached Factors Before:" << std::endl; -//BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, cachedFactors) { -// std::cout << " g( "; -// BOOST_FOREACH(Index index, factor->keys()) { -// std::cout << DefaultKeyFormatter(ordering.key(index)) << " "; -// } -// std::cout << ")" << std::endl; -//} -// -// // Marginalize out any additional (non-root) variables -// gttic(marginalize_extra_variables); -// // The rootKeys have been ordered last, so their linear indices will be { linpoint.size()-rootKeys.size() :: linpoint.size()-1 } -// Index minRootIndex = linpoint.size() - separatorValues_.size(); -// // Calculate the set of keys to be marginalized -// FastSet cachedIndices = cachedFactors.keys(); -// std::vector marginalizeIndices; -// std::remove_copy_if(cachedIndices.begin(), cachedIndices.end(), std::back_inserter(marginalizeIndices), boost::lambda::_1 >= minRootIndex); -// -//std::cout << "ConcurrentBatchSmoother::presync() Marginalize Keys: "; -//BOOST_FOREACH(Index index, marginalizeIndices) { std::cout << DefaultKeyFormatter(ordering.key(index)) << " "; } -//std::cout << std::endl; -// -// // If non-root-keys are present, marginalize them out -// if(marginalizeIndices.size() > 0) { -// // Eliminate the extra variables, stored the remaining factors back into the 'cachedFactors' graph -// GaussianConditional::shared_ptr conditional; -// boost::tie(conditional, cachedFactors) = cachedFactors.eliminate(marginalizeIndices, parameters_.getEliminationFunction()); -// } -// gttoc(marginalize_extra_variables); -// -//std::cout << "ConcurrentBatchSmoother::presync() Cached Factors After:" << std::endl; -//BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, cachedFactors) { -// std::cout << " g( "; -// BOOST_FOREACH(Index index, factor->keys()) { -// std::cout << DefaultKeyFormatter(ordering.key(index)) << " "; -// } -// std::cout << ")" << std::endl; -//} -// -// // Convert factors into 'Linearized' nonlinear factors -// gttic(store_cached_factors); -// smootherSummarization_.resize(0); -// BOOST_FOREACH(const GaussianFactor::shared_ptr& gaussianFactor, cachedFactors) { -// LinearContainerFactor::shared_ptr factor(new LinearContainerFactor(gaussianFactor, ordering, linpoint)); -// smootherSummarization_.push_back(factor); -// } -// gttoc(store_cached_factors); -// -//std::cout << "ConcurrentBatchSmoother::presync() Smoother Summarization:" << std::endl; -//BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, smootherSummarization_) { -// std::cout << " f( "; -// BOOST_FOREACH(Key key, factor->keys()) { -// std::cout << DefaultKeyFormatter(key) << " "; -// } -// std::cout << ")" << std::endl; -//} -// -// } -// gttoc(presync); -// -// gttoc(update); - return result; } @@ -366,9 +203,6 @@ std::vector ConcurrentBatchSmoother::insertFactors(const NonlinearFactor slots.push_back(slot); } - // Augment the Variable Index - variableIndex_.augment(*factors.symbolic(ordering_)); - gttoc(insert_factors); return slots; @@ -392,15 +226,15 @@ void ConcurrentBatchSmoother::removeFactors(const std::vector& slots) { availableSlots_.push(slot); } - // Remove references to this factor from the VariableIndex - variableIndex_.remove(slots, factors); - gttoc(remove_factors); } /* ************************************************************************* */ void ConcurrentBatchSmoother::reorder() { + // Recalculate the variable index + variableIndex_ = VariableIndex(*factors_.symbolic(ordering_)); + // Initialize all variables to group0 std::vector cmember(variableIndex_.size(), 0); @@ -529,6 +363,48 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() { return result; } +/* ************************************************************************* */ +void ConcurrentBatchSmoother::updateSmootherSummarization() { + + // The smoother summarization factors are the resulting marginal factors on the separator + // variables that result from marginalizing out all of the other variables + // These marginal factors will be cached for later transmission to the filter using + // linear container factors + // Note: This assumes the ordering already has the separator variables eliminated last + + // Clear out any existing smoother summarized factors + smootherSummarization_.resize(0); + + // 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 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)); + } + 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(gtsam::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); + } + +} + /* ************************************************************************* */ void ConcurrentBatchSmoother::PrintNonlinearFactor(const NonlinearFactor::shared_ptr& factor, const std::string& indent, const KeyFormatter& keyFormatter) { std::cout << indent; @@ -561,6 +437,96 @@ 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 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 ConcurrentBatchSmoother::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; +} + +/* ************************************************************************* */ +ConcurrentBatchSmoother::EliminationForest::sharedFactor ConcurrentBatchSmoother::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 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); + } +} + diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h index 98674465c..57b0195b6 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.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); } /** @@ -164,6 +159,8 @@ protected: */ virtual void postsync(); +private: + /** Augment the graph with new factors * * @param factors The factors to add to the graph @@ -183,15 +180,71 @@ protected: /** Use a modified version of L-M to update the linearization point and delta */ Result optimize(); -private: - /** Some printing functions for debugging */ + /** Calculate the smoother marginal factors on the separator variables */ + void updateSmootherSummarization(); + /** 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 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 + 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); + }; + }; // ConcurrentBatchSmoother }/// namespace gtsam diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp index 5c30b1db7..d1c0e79df 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp @@ -19,12 +19,15 @@ #include #include #include +#include #include #include +#include #include #include #include #include +#include #include #include @@ -362,60 +365,38 @@ Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, con return optimizer.values(); } -///* ************************************************************************* */ -//void FindFactorsWithAny(const std::set& keys, const NonlinearFactorGraph& sourceFactors, NonlinearFactorGraph& destinationFactors) { -// -// BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, sourceFactors) { -// NonlinearFactor::const_iterator key = factor->begin(); -// while((key != factor->end()) && (!std::binary_search(keys.begin(), keys.end(), *key))) { -// ++key; -// } -// if(key != factor->end()) { -// destinationFactors.push_back(factor); -// } -// } -// -//} -// -///* ************************************************************************* */ -//void FindFactorsWithOnly(const std::set& keys, const NonlinearFactorGraph& sourceFactors, NonlinearFactorGraph& destinationFactors) { -// -// BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, sourceFactors) { -// NonlinearFactor::const_iterator key = factor->begin(); -// while((key != factor->end()) && (std::binary_search(keys.begin(), keys.end(), *key))) { -// ++key; -// } -// if(key == factor->end()) { -// destinationFactors.push_back(factor); -// } -// } -// -//} -// -///* ************************************************************************* */ -//typedef BayesTree::sharedClique Clique; -//void SymbolicPrintTree(const Clique& clique, const Ordering& ordering, const std::string indent = "") { -// std::cout << indent << "P( "; -// BOOST_FOREACH(Index index, clique->conditional()->frontals()){ -// std::cout << DefaultKeyFormatter(ordering.key(index)) << " "; -// } -// if(clique->conditional()->nrParents() > 0) { -// std::cout << "| "; -// } -// BOOST_FOREACH(Index index, clique->conditional()->parents()){ -// std::cout << DefaultKeyFormatter(ordering.key(index)) << " "; -// } -// std::cout << ")" << std::endl; -// -// BOOST_FOREACH(const Clique& child, clique->children()) { -// SymbolicPrintTree(child, ordering, indent+" "); -// } -//} -// -//} +/* ************************************************************************* */ +void FindFactorsWithAny(const std::set& keys, const NonlinearFactorGraph& sourceFactors, NonlinearFactorGraph& destinationFactors) { + + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, sourceFactors) { + NonlinearFactor::const_iterator key = factor->begin(); + while((key != factor->end()) && (!std::binary_search(keys.begin(), keys.end(), *key))) { + ++key; + } + if(key != factor->end()) { + destinationFactors.push_back(factor); + } + } + +} /* ************************************************************************* */ -TEST_UNSAFE( ConcurrentBatchSmoother, update_batch ) +void FindFactorsWithOnly(const std::set& keys, const NonlinearFactorGraph& sourceFactors, NonlinearFactorGraph& destinationFactors) { + + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, sourceFactors) { + NonlinearFactor::const_iterator key = factor->begin(); + while((key != factor->end()) && (std::binary_search(keys.begin(), keys.end(), *key))) { + ++key; + } + if(key == factor->end()) { + destinationFactors.push_back(factor); + } + } + +} + +/* ************************************************************************* */ +TEST( ConcurrentBatchSmoother, update_batch ) { // Test the 'update' function of the ConcurrentBatchSmoother in a nonlinear environment. // Thus, a full L-M optimization and the ConcurrentBatchSmoother results should be identical @@ -446,7 +427,7 @@ TEST_UNSAFE( ConcurrentBatchSmoother, update_batch ) } /* ************************************************************************* */ -TEST_UNSAFE( ConcurrentBatchSmoother, update_incremental ) +TEST( ConcurrentBatchSmoother, update_incremental ) { // Test the 'update' function of the ConcurrentBatchSmoother in a nonlinear environment. // Thus, a full L-M optimization and the ConcurrentBatchSmoother results should be identical @@ -487,220 +468,223 @@ TEST_UNSAFE( ConcurrentBatchSmoother, update_incremental ) } -///* ************************************************************************* */ -//TEST_UNSAFE( ConcurrentBatchSmoother, synchronize ) -//{ -// // Test the 'synchronize' function of the ConcurrentBatchSmoother in a nonlinear environment. -// // The smoother is operating on a known tree structure, so the factors and summarization can -// // be predicted for testing purposes -// -// // Create a set of optimizer parameters -// LevenbergMarquardtParams parameters; -// -// // Create a Concurrent Batch Smoother -// ConcurrentBatchSmootherTester smoother(parameters); -// -// // Create containers to keep the full graph -// Values fullTheta; -// NonlinearFactorGraph fullGraph; -// -// // Create factors for times 0 - 12 -// // When eliminated with ordering (X2 X0 X1 X4 X5 X3 X6 X8 X11 X10 X7 X9 X12)augmentedHessian -// // ... this Bayes Tree is produced: -// // Bayes Tree: -// // P( X7 X9 X12 ) -// // P( X10 | X9 ) -// // P( X11 | X10 ) -// // P( X8 | X9 ) -// // P( X6 | X7 X9 ) -// // P( X5 X3 | X6 ) -// // P( X1 | X3 ) -// // P( X0 | X1 ) -// // P( X2 | X3 ) -// // P( X4 | X7 ) -// // We then produce the inputs necessary for the 'synchronize' function. -// // The smoother is branches X4 and X6, the filter is branches X8 and X10, and the root is (X7 X9 X12) -// CreateFactors(fullGraph, fullTheta, 0, 13); -// -// // Optimize the full graph -// Values optimalTheta = BatchOptimize(fullGraph, fullTheta); -// -// // Re-eliminate to create the Bayes Tree -// Ordering ordering; -// ordering.push_back(Symbol('X', 2)); -// ordering.push_back(Symbol('X', 0)); -// ordering.push_back(Symbol('X', 1)); -// ordering.push_back(Symbol('X', 4)); -// ordering.push_back(Symbol('X', 5)); -// ordering.push_back(Symbol('X', 3)); -// ordering.push_back(Symbol('X', 6)); -// ordering.push_back(Symbol('X', 8)); -// ordering.push_back(Symbol('X', 11)); -// ordering.push_back(Symbol('X', 10)); -// ordering.push_back(Symbol('X', 7)); -// ordering.push_back(Symbol('X', 9)); -// ordering.push_back(Symbol('X', 12)); -// Values linpoint; -// linpoint.insert(optimalTheta); -// GaussianFactorGraph linearGraph = *fullGraph.linearize(linpoint, ordering); -// JunctionTree jt(linearGraph); -// ISAM2Clique::shared_ptr root = jt.eliminate(EliminateQR); -// BayesTree bayesTree; -// bayesTree.insert(root); -// -// // Extract the values for the smoother keys. This consists of the branches: X4 and X6 -// // Extract the non-root values from the initial values to test the smoother optimization -// Values smootherValues; -// smootherValues.insert(Symbol('X', 0), fullTheta.at(Symbol('X', 0))); -// smootherValues.insert(Symbol('X', 1), fullTheta.at(Symbol('X', 1))); -// smootherValues.insert(Symbol('X', 2), fullTheta.at(Symbol('X', 2))); -// smootherValues.insert(Symbol('X', 3), fullTheta.at(Symbol('X', 3))); -// smootherValues.insert(Symbol('X', 4), fullTheta.at(Symbol('X', 4))); -// smootherValues.insert(Symbol('X', 5), fullTheta.at(Symbol('X', 5))); -// smootherValues.insert(Symbol('X', 6), fullTheta.at(Symbol('X', 6))); -// -// // Extract the optimal root values -// Values rootValues; -// rootValues.insert(Symbol('X', 7), optimalTheta.at(Symbol('X', 7))); -// rootValues.insert(Symbol('X', 9), optimalTheta.at(Symbol('X', 9))); -// rootValues.insert(Symbol('X', 12), optimalTheta.at(Symbol('X', 12))); -// -// // Extract the nonlinear smoother factors as any factor with a non-root smoother key -// std::set smootherKeys; -// smootherKeys.insert(Symbol('X', 0)); -// smootherKeys.insert(Symbol('X', 1)); -// smootherKeys.insert(Symbol('X', 2)); -// smootherKeys.insert(Symbol('X', 3)); -// smootherKeys.insert(Symbol('X', 4)); -// smootherKeys.insert(Symbol('X', 5)); -// smootherKeys.insert(Symbol('X', 6)); -// NonlinearFactorGraph smootherFactors; -// FindFactorsWithAny(smootherKeys, fullGraph, smootherFactors); -// -// // Extract the filter summarized factors. This consists of the linear cached factors from -// // the filter branches X8 and X10, as well as any nonlinear factor that involves only root keys -// NonlinearFactorGraph filterSummarization; -// filterSummarization.add(LinearizedJacobianFactor(boost::static_pointer_cast(bayesTree.nodes().at(ordering.at(Symbol('X', 8)))->cachedFactor()), ordering, linpoint)); -// filterSummarization.add(LinearizedJacobianFactor(boost::static_pointer_cast(bayesTree.nodes().at(ordering.at(Symbol('X', 10)))->cachedFactor()), ordering, linpoint)); -// std::set rootKeys; -// rootKeys.insert(Symbol('X', 7)); -// rootKeys.insert(Symbol('X', 9)); -// rootKeys.insert(Symbol('X', 12)); -// FindFactorsWithOnly(rootKeys, fullGraph, filterSummarization); -// -// -// -// // Perform the synchronization procedure -// NonlinearFactorGraph actualSmootherSummarization; -// smoother.presync(); -// smoother.getSummarizedFactors(actualSmootherSummarization); -// smoother.synchronize(smootherFactors, smootherValues, filterSummarization, rootValues); -// smoother.postsync(); -// -// // Verify the returned smoother values is empty in the first iteration -// NonlinearFactorGraph expectedSmootherSummarization; -// CHECK(assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1e-4)); -// -// -// -// // Perform a full update of the smoother. Since the root values/summarized filter factors were -// // created at the optimal values, the smoother should be identical to the batch optimization -// smoother.update(); -// Values actualSmootherTheta = smoother.calculateEstimate(); -// -// // Create the expected values as the optimal set -// Values expectedSmootherTheta; -// expectedSmootherTheta.insert(Symbol('X', 0), optimalTheta.at(Symbol('X', 0))); -// expectedSmootherTheta.insert(Symbol('X', 1), optimalTheta.at(Symbol('X', 1))); -// expectedSmootherTheta.insert(Symbol('X', 2), optimalTheta.at(Symbol('X', 2))); -// expectedSmootherTheta.insert(Symbol('X', 3), optimalTheta.at(Symbol('X', 3))); -// expectedSmootherTheta.insert(Symbol('X', 4), optimalTheta.at(Symbol('X', 4))); -// expectedSmootherTheta.insert(Symbol('X', 5), optimalTheta.at(Symbol('X', 5))); -// expectedSmootherTheta.insert(Symbol('X', 6), optimalTheta.at(Symbol('X', 6))); -// -// // Compare filter solution with full batch -// CHECK(assert_equal(expectedSmootherTheta, actualSmootherTheta, 1e-4)); -// -// -// -// // Add a loop closure factor to the smoother and re-check. Since the filter -// // factors were created at the optimal linpoint, and since the new loop closure -// // does not involve filter keys, the smoother should still yeild the optimal solution -// // The new Bayes Tree is: -// // Bayes Tree: -// // P( X7 X9 X12 ) -// // P( X10 | X9 ) -// // P( X11 | X10 ) -// // P( X8 | X9 ) -// // P( X6 | X7 X9 ) -// // P( X4 | X6 X7 ) -// // P( X3 X5 | X4 X6 ) -// // P( X2 | X3 ) -// // P( X1 | X3 X4 ) -// // P( X0 | X1 ) -// Pose3 poseDelta = fullTheta.at(Symbol('X', 1)).between(fullTheta.at(Symbol('X', 4))); -// NonlinearFactor::shared_ptr loopClosure = NonlinearFactor::shared_ptr(new BetweenFactor(Symbol('X', 1), Symbol('X', 4), poseDelta, noiseOdometery)); -// fullGraph.push_back(loopClosure); -// optimalTheta = BatchOptimize(fullGraph, fullTheta, rootValues); -// -// // Recreate the Bayes Tree -// linpoint.clear(); -// linpoint.insert(optimalTheta); -// linpoint.update(rootValues); -// linearGraph = *fullGraph.linearize(linpoint, ordering); -// jt = JunctionTree(linearGraph); -// root = jt.eliminate(EliminateQR); -// bayesTree = BayesTree(); -// bayesTree.insert(root); -// -// // Add the loop closure to the smoother -// NonlinearFactorGraph newFactors; -// newFactors.push_back(loopClosure); -// smoother.update(newFactors); -// actualSmootherTheta = smoother.calculateEstimate(); -// -// // Create the expected values as the optimal set -// expectedSmootherTheta.clear(); -// expectedSmootherTheta.insert(Symbol('X', 0), optimalTheta.at(Symbol('X', 0))); -// expectedSmootherTheta.insert(Symbol('X', 1), optimalTheta.at(Symbol('X', 1))); -// expectedSmootherTheta.insert(Symbol('X', 2), optimalTheta.at(Symbol('X', 2))); -// expectedSmootherTheta.insert(Symbol('X', 3), optimalTheta.at(Symbol('X', 3))); -// expectedSmootherTheta.insert(Symbol('X', 4), optimalTheta.at(Symbol('X', 4))); -// expectedSmootherTheta.insert(Symbol('X', 5), optimalTheta.at(Symbol('X', 5))); -// expectedSmootherTheta.insert(Symbol('X', 6), optimalTheta.at(Symbol('X', 6))); -// -// // Compare filter solution with full batch -// // TODO: Check This -//// CHECK(assert_equal(expectedSmootherTheta, actualSmootherTheta, 1e-4)); -// -// -// -// // Now perform a second synchronization to test the smoother-calculated summarization -// actualSmootherSummarization.resize(0); -// smootherFactors.resize(0); -// smootherValues.clear(); -// smoother.presync(); -// smoother.getSummarizedFactors(actualSmootherSummarization); -// smoother.synchronize(smootherFactors, smootherValues, filterSummarization, rootValues); -// smoother.postsync(); -// -// // Extract the expected smoother summarization from the Bayes Tree -// // The smoother branches after the addition of the loop closure is only X6 -// expectedSmootherSummarization.resize(0); -// JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(bayesTree.nodes().at(ordering.at(Symbol('X', 6)))->cachedFactor()); -// LinearizedJacobianFactor::shared_ptr ljf(new LinearizedJacobianFactor(jf, ordering, linpoint)); -// expectedSmootherSummarization.push_back(ljf); -// -// // Compare smoother factors with the expected factors by computing the hessian information matrix -// // TODO: Check This -//// CHECK(hessian_equal(expectedSmootherSummarization, actualSmootherSummarization, linpoint, 1e-4)); -// -// -// -// // TODO: Modify the second synchronization so that the filter sends an additional set of factors. -// // I'm not sure what additional code this will exercise, but just for good measure. -// -//} +/* ************************************************************************* */ +TEST_UNSAFE( ConcurrentBatchSmoother, synchronize ) +{ + // Test the 'synchronize' function of the ConcurrentBatchSmoother in a nonlinear environment. + // The smoother is operating on a known tree structure, so the factors and summarization can + // be predicted for testing purposes + + // Create a set of optimizer parameters + LevenbergMarquardtParams parameters; + + // Create a Concurrent Batch Smoother + ConcurrentBatchSmootherTester smoother(parameters); + + // Create containers to keep the full graph + Values fullTheta; + NonlinearFactorGraph fullGraph; + + // Create factors for times 0 - 12 + // When eliminated with ordering (X2 X0 X1 X4 X5 X3 X6 X8 X11 X10 X7 X9 X12)augmentedHessian + // ... this Bayes Tree is produced: + // Bayes Tree: + // P( X7 X9 X12 ) + // P( X10 | X9 ) + // P( X11 | X10 ) + // P( X8 | X9 ) + // P( X6 | X7 X9 ) + // P( X5 X3 | X6 ) + // P( X1 | X3 ) + // P( X0 | X1 ) + // P( X2 | X3 ) + // P( X4 | X7 ) + // We then produce the inputs necessary for the 'synchronize' function. + // The smoother is branches X4 and X6, the filter is branches X8 and X10, and the root is (X7 X9 X12) + CreateFactors(fullGraph, fullTheta, 0, 13); + + // Optimize the full graph + Values optimalTheta = BatchOptimize(fullGraph, fullTheta); + + // Re-eliminate to create the Bayes Tree + Ordering ordering; + ordering.push_back(Symbol('X', 2)); + ordering.push_back(Symbol('X', 0)); + ordering.push_back(Symbol('X', 1)); + ordering.push_back(Symbol('X', 4)); + ordering.push_back(Symbol('X', 5)); + ordering.push_back(Symbol('X', 3)); + ordering.push_back(Symbol('X', 6)); + ordering.push_back(Symbol('X', 8)); + ordering.push_back(Symbol('X', 11)); + ordering.push_back(Symbol('X', 10)); + ordering.push_back(Symbol('X', 7)); + ordering.push_back(Symbol('X', 9)); + ordering.push_back(Symbol('X', 12)); + Values linpoint; + linpoint.insert(optimalTheta); + GaussianFactorGraph linearGraph = *fullGraph.linearize(linpoint, ordering); + JunctionTree jt(linearGraph); + ISAM2Clique::shared_ptr root = jt.eliminate(EliminateQR); + BayesTree bayesTree; + bayesTree.insert(root); + + // Extract the values for the smoother keys. This consists of the branches: X4 and X6 + // Extract the non-root values from the initial values to test the smoother optimization + Values smootherValues; + smootherValues.insert(Symbol('X', 0), fullTheta.at(Symbol('X', 0))); + smootherValues.insert(Symbol('X', 1), fullTheta.at(Symbol('X', 1))); + smootherValues.insert(Symbol('X', 2), fullTheta.at(Symbol('X', 2))); + smootherValues.insert(Symbol('X', 3), fullTheta.at(Symbol('X', 3))); + smootherValues.insert(Symbol('X', 4), fullTheta.at(Symbol('X', 4))); + smootherValues.insert(Symbol('X', 5), fullTheta.at(Symbol('X', 5))); + smootherValues.insert(Symbol('X', 6), fullTheta.at(Symbol('X', 6))); + + // Extract the optimal root values + Values rootValues; + rootValues.insert(Symbol('X', 7), optimalTheta.at(Symbol('X', 7))); + rootValues.insert(Symbol('X', 9), optimalTheta.at(Symbol('X', 9))); + rootValues.insert(Symbol('X', 12), optimalTheta.at(Symbol('X', 12))); + + // Extract the nonlinear smoother factors as any factor with a non-root smoother key + std::set smootherKeys; + smootherKeys.insert(Symbol('X', 0)); + smootherKeys.insert(Symbol('X', 1)); + smootherKeys.insert(Symbol('X', 2)); + smootherKeys.insert(Symbol('X', 3)); + smootherKeys.insert(Symbol('X', 4)); + smootherKeys.insert(Symbol('X', 5)); + smootherKeys.insert(Symbol('X', 6)); + NonlinearFactorGraph smootherFactors; + FindFactorsWithAny(smootherKeys, fullGraph, smootherFactors); + + // Extract the filter summarized factors. This consists of the linear cached factors from + // the filter branches X8 and X10, as well as any nonlinear factor that involves only root keys + NonlinearFactorGraph filterSummarization; + filterSummarization.add(LinearContainerFactor(bayesTree.nodes().at(ordering.at(Symbol('X', 8)))->cachedFactor(), ordering, linpoint)); + filterSummarization.add(LinearContainerFactor(bayesTree.nodes().at(ordering.at(Symbol('X', 10)))->cachedFactor(), ordering, linpoint)); + std::set rootKeys; + rootKeys.insert(Symbol('X', 7)); + rootKeys.insert(Symbol('X', 9)); + rootKeys.insert(Symbol('X', 12)); + FindFactorsWithOnly(rootKeys, fullGraph, filterSummarization); + + + + // Perform the synchronization procedure + NonlinearFactorGraph actualSmootherSummarization; + smoother.presync(); + smoother.getSummarizedFactors(actualSmootherSummarization); + smoother.synchronize(smootherFactors, smootherValues, filterSummarization, rootValues); + smoother.postsync(); + + // Verify the returned smoother values is empty in the first iteration + NonlinearFactorGraph expectedSmootherSummarization; + CHECK(assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1e-4)); + + + + // Perform a full update of the smoother. Since the root values/summarized filter factors were + // created at the optimal values, the smoother should be identical to the batch optimization + smoother.update(); + Values actualSmootherTheta = smoother.calculateEstimate(); + + // Create the expected values from the optimal set + Values expectedSmootherTheta; + expectedSmootherTheta.insert(Symbol('X', 0), optimalTheta.at(Symbol('X', 0))); + expectedSmootherTheta.insert(Symbol('X', 1), optimalTheta.at(Symbol('X', 1))); + expectedSmootherTheta.insert(Symbol('X', 2), optimalTheta.at(Symbol('X', 2))); + expectedSmootherTheta.insert(Symbol('X', 3), optimalTheta.at(Symbol('X', 3))); + expectedSmootherTheta.insert(Symbol('X', 4), optimalTheta.at(Symbol('X', 4))); + expectedSmootherTheta.insert(Symbol('X', 5), optimalTheta.at(Symbol('X', 5))); + expectedSmootherTheta.insert(Symbol('X', 6), optimalTheta.at(Symbol('X', 6))); + expectedSmootherTheta.insert(Symbol('X', 7), optimalTheta.at(Symbol('X', 7))); + expectedSmootherTheta.insert(Symbol('X', 9), optimalTheta.at(Symbol('X', 9))); + expectedSmootherTheta.insert(Symbol('X', 12), optimalTheta.at(Symbol('X', 12))); + + // Compare filter solution with full batch + CHECK(assert_equal(expectedSmootherTheta, actualSmootherTheta, 1e-4)); + + + + // Add a loop closure factor to the smoother and re-check. Since the filter + // factors were created at the optimal linpoint, and since the new loop closure + // does not involve filter keys, the smoother should still yield the optimal solution + // The new Bayes Tree is: + // Bayes Tree: + // P( X7 X9 X12 ) + // P( X10 | X9 ) + // P( X11 | X10 ) + // P( X8 | X9 ) + // P( X6 | X7 X9 ) + // P( X4 | X6 X7 ) + // P( X3 X5 | X4 X6 ) + // P( X2 | X3 ) + // P( X1 | X3 X4 ) + // P( X0 | X1 ) + Pose3 poseDelta = fullTheta.at(Symbol('X', 1)).between(fullTheta.at(Symbol('X', 4))); + NonlinearFactor::shared_ptr loopClosure = NonlinearFactor::shared_ptr(new BetweenFactor(Symbol('X', 1), Symbol('X', 4), poseDelta, noiseOdometery)); + fullGraph.push_back(loopClosure); + optimalTheta = BatchOptimize(fullGraph, fullTheta, rootValues); + + // Recreate the Bayes Tree + linpoint.clear(); + linpoint.insert(optimalTheta); + linpoint.update(rootValues); + linearGraph = *fullGraph.linearize(linpoint, ordering); + jt = JunctionTree(linearGraph); + root = jt.eliminate(EliminateQR); + bayesTree = BayesTree(); + bayesTree.insert(root); + + // Add the loop closure to the smoother + NonlinearFactorGraph newFactors; + newFactors.push_back(loopClosure); + smoother.update(newFactors); + actualSmootherTheta = smoother.calculateEstimate(); + + // Create the expected values as the optimal set + expectedSmootherTheta.clear(); + expectedSmootherTheta.insert(Symbol('X', 0), optimalTheta.at(Symbol('X', 0))); + expectedSmootherTheta.insert(Symbol('X', 1), optimalTheta.at(Symbol('X', 1))); + expectedSmootherTheta.insert(Symbol('X', 2), optimalTheta.at(Symbol('X', 2))); + expectedSmootherTheta.insert(Symbol('X', 3), optimalTheta.at(Symbol('X', 3))); + expectedSmootherTheta.insert(Symbol('X', 4), optimalTheta.at(Symbol('X', 4))); + expectedSmootherTheta.insert(Symbol('X', 5), optimalTheta.at(Symbol('X', 5))); + expectedSmootherTheta.insert(Symbol('X', 6), optimalTheta.at(Symbol('X', 6))); + expectedSmootherTheta.insert(Symbol('X', 7), optimalTheta.at(Symbol('X', 7))); + expectedSmootherTheta.insert(Symbol('X', 9), optimalTheta.at(Symbol('X', 9))); + expectedSmootherTheta.insert(Symbol('X', 12), optimalTheta.at(Symbol('X', 12))); + + // Compare filter solution with full batch + CHECK(assert_equal(expectedSmootherTheta, actualSmootherTheta, 1e-4)); + + + + // Now perform a second synchronization to test the smoother-calculated summarization + actualSmootherSummarization.resize(0); + smootherFactors.resize(0); + smootherValues.clear(); + smoother.presync(); + smoother.getSummarizedFactors(actualSmootherSummarization); + smoother.synchronize(smootherFactors, smootherValues, filterSummarization, rootValues); + smoother.postsync(); + + // Extract the expected smoother summarization from the Bayes Tree + // The smoother branches after the addition of the loop closure is only X6 + expectedSmootherSummarization.resize(0); + LinearContainerFactor::shared_ptr factor(new LinearContainerFactor(bayesTree.nodes().at(ordering.at(Symbol('X', 6)))->cachedFactor(), ordering, linpoint)); + expectedSmootherSummarization.push_back(factor); + + // Compare smoother factors with the expected factors by computing the hessian information matrix + CHECK(hessian_equal(expectedSmootherSummarization, actualSmootherSummarization, linpoint, 1e-4)); + + + + // TODO: Modify the second synchronization so that the filter sends an additional set of factors. + // I'm not sure what additional code this will exercise, but just for good measure. + +} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);}