diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp index 56857ef44..5be512ea8 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp @@ -115,32 +115,77 @@ void ConcurrentBatchFilter::synchronize(const NonlinearFactorGraph& summarizedFa // 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 + NonlinearFactorGraph graph; + graph.push_back(factors_); + graph.push_back(smootherFactors_); + graph.push_back(summarizedFactors); + Values values; + values.insert(theta_); + values.insert(smootherValues_); + values.update(separatorValues); // ensure the smoother summarized factors are linearized around the values in the smoother // Optimize this graph using a modified version of L-M // TODO: + // Create separate ordering constraints that force either the filter keys or the smoother keys to the front + typedef std::map OrderingConstraints; + OrderingConstraints filterConstraints; + OrderingConstraints smootherConstraints; + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, theta_) { /// the filter keys + filterConstraints[key_value.key] = 0; + smootherConstraints[key_value.key] = 1; + } + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, smootherValues_) { /// the smoother keys + filterConstraints[key_value.key] = 1; + smootherConstraints[key_value.key] = 0; + } + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { /// the *new* separator keys + filterConstraints[key_value.key] = 2; + smootherConstraints[key_value.key] = 2; + } + + // Generate separate orderings that place the filter keys or the smoother keys first + // TODO: This is convenient, but it recalculates the variable index each time + Ordering filterOrdering = *graph.orderingCOLAMDConstrained(values, filterConstraints); + Ordering smootherOrdering = *graph.orderingCOLAMDConstrained(values, smootherConstraints); + + // Extract the set of filter keys and smoother keys + std::set filterKeys; + std::set separatorKeys; + std::set smootherKeys; + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, theta_) { + filterKeys.insert(key_value.key); + } + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { + separatorKeys.insert(key_value.key); + filterKeys.erase(key_value.key); + } + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, smootherValues_) { + smootherKeys.insert(key_value.key); + } + // 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()); + filterSummarization_ = marginalize(graph, values, filterOrdering, filterKeys, parameters_.getEliminationFunction()); + // The filter summarization should also include any nonlinear factors that involve only the separator variables. + // Otherwise the smoother will be missing this information + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, factors_) { + if(factor) { + NonlinearFactor::const_iterator key = factor->begin(); + while((key != factor->end()) && (std::binary_search(separatorKeys.begin(), separatorKeys.end(), *key))) { + ++key; + } + if(key == factor->end()) { + filterSummarization_.push_back(factor); + } + } + } // Calculate the marginal on the new separator from the smoother factors. This is performed by marginalizing out // all of the smoother variables that are not part of the new separator. This smoother summarization will be // stored locally for use in the filter - Ordering smootherOrdering; - std::vector smootherKeys; - smootherSummarizationSlots_ = insertFactors( marginalize(filterGraph, filterValues, smootherOrdering, smootherKeys, parameters_.getEliminationFunction()) ); - + smootherSummarizationSlots_ = insertFactors( marginalize(graph, values, smootherOrdering, smootherKeys, parameters_.getEliminationFunction()) ); gttoc(synchronize); } @@ -174,6 +219,10 @@ void ConcurrentBatchFilter::postsync() { gttic(postsync); + // Clear out the factors and values that were just sent to the smoother + smootherFactors_.resize(0); + smootherValues_.clear(); + gttoc(postsync); } @@ -475,7 +524,7 @@ 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) { + const Ordering& ordering, const std::set& marginalizeKeys, const GaussianFactorGraph::Eliminate& function) { // This function returns marginal factors (in the form of LinearContainerFactors) that result from // marginalizing out the selected variables. diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h index fa2986054..d1127b327 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h @@ -196,7 +196,7 @@ private: * 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); + const Ordering& ordering, const std::set& marginalizeKeys, const GaussianFactorGraph::Eliminate& function = EliminateQR); /** Print just the nonlinear keys in a nonlinear factor */ static void PrintNonlinearFactor(const NonlinearFactor::shared_ptr& factor, diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp index f36ff31ec..dc00da846 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp @@ -19,12 +19,16 @@ #include #include #include +#include #include #include +#include #include #include #include #include +#include +#include #include #include @@ -59,12 +63,27 @@ public: /* ************************************************************************* */ bool hessian_equal(const NonlinearFactorGraph& expected, const NonlinearFactorGraph& actual, const Values& theta, double tol = 1e-9) { + // Verify the set of keys in both graphs are the same FastSet expectedKeys = expected.keys(); FastSet actualKeys = actual.keys(); + bool keys_equal = (expectedKeys.size() == actualKeys.size()) && std::equal(expectedKeys.begin(), expectedKeys.end(), actualKeys.begin()); + if(!keys_equal) { + std::cout << "Hessian Keys not equal:" << std::endl; + + std::cout << "Expected Keys: "; + BOOST_FOREACH(Key key, expectedKeys) { + std::cout << DefaultKeyFormatter(key) << " "; + } + std::cout << std::endl; + + std::cout << "Actual Keys: "; + BOOST_FOREACH(Key key, actualKeys) { + std::cout << DefaultKeyFormatter(key) << " "; + } + std::cout << std::endl; - // Verify the set of keys in both graphs are the same - if(expectedKeys.size() != actualKeys.size() || !std::equal(expectedKeys.begin(), expectedKeys.end(), actualKeys.begin())) return false; + } // Create an ordering Ordering ordering; @@ -356,60 +375,64 @@ 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+" "); -// } -//} -// -//} +/* ************************************************************************* */ +NonlinearFactorGraph MarginalFactors(const NonlinearFactorGraph& factors, const Values& values, const std::set& remainingKeys) { + + // Create an ordering with the remaining variable last + std::map constraints; + BOOST_FOREACH(Key key, remainingKeys) { + constraints[key] = 1; + } + Ordering ordering = *factors.orderingCOLAMDConstrained(values, constraints); + + // Convert the remaining keys into indices + std::vector remainingIndices; + BOOST_FOREACH(Key key, remainingKeys) { + remainingIndices.push_back(ordering.at(key)); + } + + // Solve for the Gaussian marginal factors + GaussianSequentialSolver gss(*factors.linearize(values, ordering), true); + GaussianFactorGraph linearMarginals = *gss.jointFactorGraph(remainingIndices); + + // Convert to LinearContainFactors + return LinearContainerFactor::convertLinearGraph(linearMarginals, ordering, values); +} /* ************************************************************************* */ -TEST_UNSAFE( ConcurrentBatchFilter, update_batch ) +template +void FindFactorsWithAny(const CONTAINER& 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); + } + } + +} + +/* ************************************************************************* */ +template +void FindFactorsWithOnly(const CONTAINER& 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( ConcurrentBatchFilter, update_batch ) { // Test the 'update' function of the ConcurrentBatchFilter in a nonlinear environment. // Thus, a full L-M optimization and the ConcurrentBatchFilter results should be identical @@ -441,7 +464,7 @@ TEST_UNSAFE( ConcurrentBatchFilter, update_batch ) } /* ************************************************************************* */ -TEST_UNSAFE( ConcurrentBatchFilter, update_batch_with_marginalization ) +TEST( 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 @@ -483,7 +506,7 @@ TEST_UNSAFE( ConcurrentBatchFilter, update_batch_with_marginalization ) } /* ************************************************************************* */ -TEST_UNSAFE( ConcurrentBatchFilter, update_incremental ) +TEST( ConcurrentBatchFilter, update_incremental ) { // Test the 'update' function of the ConcurrentBatchFilter in a nonlinear environment. // Thus, a full L-M optimization and the ConcurrentBatchFilter results should be identical @@ -525,7 +548,7 @@ TEST_UNSAFE( ConcurrentBatchFilter, update_incremental ) } /* ************************************************************************* */ -TEST_UNSAFE( ConcurrentBatchFilter, update_incremental_with_marginalization ) +TEST( 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 @@ -577,151 +600,227 @@ TEST_UNSAFE( ConcurrentBatchFilter, update_incremental_with_marginalization ) } -///* ************************************************************************* */ -//TEST_UNSAFE( ConcurrentBatchFilter, synchronize ) -//{ -// // Test the 'synchronize' function of the ConcurrentBatchFilter in a nonlinear environment. -// // The filter is operating on a known tree structure, so the factors and summarization can -// // be predicted for testing purposes +/* ************************************************************************* */ +TEST_UNSAFE( ConcurrentBatchFilter, synchronize ) +{ + // Test the 'synchronize' function of the ConcurrentBatchFilter in a nonlinear environment. + // The filter 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 Filter + ConcurrentBatchFilterTester filter(parameters); + + // Create containers to keep the full graph + Values newTheta, fullTheta; + NonlinearFactorGraph newGraph, fullGraph; + + // Create factors from t=0 to t=12 + // BAYES TREE: + // P( X7 X9 X12 ) + // P( X10 | X9 ) + // P( X11 | X10 ) + // P( X8 | X9 ) + // P( X6 | X7 X9 ) + // P( X3 X5 | X6 ) + // P( X2 | X3 ) + // P( X1 | X3 ) + // P( X0 | X1 ) + // P( X4 | X7 ) + CreateFactors(newGraph, newTheta, 0, 13); + fullTheta.insert(newTheta); + fullGraph.push_back(newGraph); + Values optimalTheta = BatchOptimize(fullGraph, fullTheta); + + // Optimize with Concurrent Batch Filter + FastList marginalizeKeys; + marginalizeKeys.push_back(Symbol('X', 0)); + marginalizeKeys.push_back(Symbol('X', 1)); + marginalizeKeys.push_back(Symbol('X', 2)); + marginalizeKeys.push_back(Symbol('X', 3)); + marginalizeKeys.push_back(Symbol('X', 4)); + marginalizeKeys.push_back(Symbol('X', 5)); + marginalizeKeys.push_back(Symbol('X', 6)); + filter.update(newGraph, newTheta, marginalizeKeys); + + // Extract the nonlinear factors that should be sent to the smoother + NonlinearFactorGraph expectedSmootherFactors; + FindFactorsWithAny(marginalizeKeys, fullGraph, expectedSmootherFactors); + + // Extract smoother values + Values expectedSmootherValues; + BOOST_FOREACH(Key key, marginalizeKeys) { + expectedSmootherValues.insert(key, optimalTheta.at(key)); + } + + // Extract the filter summarized factors on the separator + // ( as defined by the smoother branch separators {X7,X9} and {X7} ) + std::set separatorKeys; + separatorKeys.insert(Symbol('X', 7)); + separatorKeys.insert(Symbol('X', 9)); + + // Marginal factors remaining after marginalizing out all non-separator keys from the filter factors + std::set filterKeys; + filterKeys.insert(Symbol('X', 7)); + filterKeys.insert(Symbol('X', 8)); + filterKeys.insert(Symbol('X', 9)); + filterKeys.insert(Symbol('X', 10)); + filterKeys.insert(Symbol('X', 11)); + filterKeys.insert(Symbol('X', 12)); + NonlinearFactorGraph filterFactors; + FindFactorsWithOnly(filterKeys, fullGraph, filterFactors); + Values filterValues; + BOOST_FOREACH(Key key, filterKeys) { + filterValues.insert(key, optimalTheta.at(key)); + } + NonlinearFactorGraph expectedFilterSumarization = MarginalFactors(filterFactors, filterValues, separatorKeys); + + // Extract the new separator values + Values expectedSeparatorValues; + BOOST_FOREACH(Key key, separatorKeys) { + expectedSeparatorValues.insert(key, optimalTheta.at(key)); + } + + // Start the synchronization process + NonlinearFactorGraph actualSmootherFactors, actualFilterSumarization, smootherSummarization; + Values actualSmootherValues, actualSeparatorValues, smootherSeparatorValues; + filter.presync(); + filter.synchronize(smootherSummarization, smootherSeparatorValues); // Supplying an empty factor graph here + filter.getSmootherFactors(actualSmootherFactors, actualSmootherValues); + filter.getSummarizedFactors(actualFilterSumarization, actualSeparatorValues); + filter.postsync(); + + // Compare filter sync variables versus the expected + CHECK(hessian_equal(expectedSmootherFactors, actualSmootherFactors, optimalTheta, 1e-8)); + CHECK(assert_equal(expectedSmootherValues, actualSmootherValues, 1e-4)); + CHECK(hessian_equal(expectedFilterSumarization, actualFilterSumarization, optimalTheta, 1e-9)); + CHECK(assert_equal(expectedSeparatorValues, actualSeparatorValues, 1e-4)); + + + + + + // Calculate the quantities that would be coming from the smoother for the next iteration + expectedSmootherValues.insert(expectedSeparatorValues); + smootherSummarization = MarginalFactors(expectedSmootherFactors, expectedSmootherValues, separatorKeys); + smootherSeparatorValues = expectedSeparatorValues; + + // Now add additional factors to the filter and re-sync + // BAYES TREE: + // P( X14 X16 X19 ) + // P( X17 | X16 ) + // P( X18 | X17 ) + // P( X15 | X16 ) + // P( X13 | X14 X16 ) + // P( X11 | X13 X14 ) + // P( X10 | X11 X13 ) + // P( X12 | X10 X13 ) + // P( X9 | X12 X10 ) + // P( X7 | X9 X12 ) + // P( X6 | X7 X9 ) + // P( X3 X5 | X6 ) + // P( X2 | X3 ) + // P( X1 | X3 ) + // P( X0 | X1 ) + // P( X4 | X7 ) + // P( X8 | X9 ) + newGraph.resize(0); newTheta.clear(); + CreateFactors(newGraph, newTheta, 13, 20); + fullTheta.insert(newTheta); + fullGraph.push_back(newGraph); + optimalTheta = BatchOptimize(fullGraph, fullTheta); + + // Optimize with Concurrent Batch Filter + marginalizeKeys.clear(); + marginalizeKeys.push_back(Symbol('X', 7)); + marginalizeKeys.push_back(Symbol('X', 8)); + marginalizeKeys.push_back(Symbol('X', 9)); + marginalizeKeys.push_back(Symbol('X', 10)); + marginalizeKeys.push_back(Symbol('X', 11)); + marginalizeKeys.push_back(Symbol('X', 12)); + marginalizeKeys.push_back(Symbol('X', 13)); + filter.update(newGraph, newTheta, marginalizeKeys); + + // Extract the nonlinear factors that should be sent to the smoother + NonlinearFactorGraph newSmootherFactors; + FindFactorsWithAny(marginalizeKeys, fullGraph, newSmootherFactors); + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, expectedSmootherFactors) { + NonlinearFactorGraph::iterator iter = std::find(newSmootherFactors.begin(), newSmootherFactors.end(), factor); + if(iter != newSmootherFactors.end()) { + newSmootherFactors.remove(iter - newSmootherFactors.begin()); + } + } + expectedSmootherFactors.resize(0); + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, newSmootherFactors) { + if(factor) { + expectedSmootherFactors.push_back(factor); + } + } + + // Extract smoother values + expectedSmootherValues.clear(); + BOOST_FOREACH(Key key, marginalizeKeys) { + expectedSmootherValues.insert(key, optimalTheta.at(key)); + } + + // Extract the filter summarized factors on the separator + // ( as defined by the smoother branch separators {X14,X16} ) + separatorKeys.clear(); + separatorKeys.insert(Symbol('X', 14)); + separatorKeys.insert(Symbol('X', 16)); + + // Marginal factors remaining after marginalizing out all non-separator keys from the filter factors + filterKeys.clear(); + filterKeys.insert(Symbol('X', 14)); + filterKeys.insert(Symbol('X', 15)); + filterKeys.insert(Symbol('X', 16)); + filterKeys.insert(Symbol('X', 17)); + filterKeys.insert(Symbol('X', 18)); + filterKeys.insert(Symbol('X', 19)); + filterFactors.resize(0); + FindFactorsWithOnly(filterKeys, fullGraph, filterFactors); + filterValues.clear(); + BOOST_FOREACH(Key key, filterKeys) { + filterValues.insert(key, optimalTheta.at(key)); + } + expectedFilterSumarization = MarginalFactors(filterFactors, filterValues, separatorKeys); + + // Extract the new separator values + expectedSeparatorValues.clear(); + BOOST_FOREACH(Key key, separatorKeys) { + expectedSeparatorValues.insert(key, optimalTheta.at(key)); + } + + + // Start the synchronization process + actualSmootherFactors.resize(0); actualFilterSumarization.resize(0); + actualSmootherValues.clear(); actualSeparatorValues.clear(); + smootherSeparatorValues = expectedSeparatorValues; + filter.presync(); + filter.synchronize(smootherSummarization, smootherSeparatorValues); + filter.getSmootherFactors(actualSmootherFactors, actualSmootherValues); + filter.getSummarizedFactors(actualFilterSumarization, actualSeparatorValues); + filter.postsync(); + + + + // Compare filter sync variables versus the expected + CHECK(hessian_equal(expectedSmootherFactors, actualSmootherFactors, optimalTheta, 1e-8)); + CHECK(assert_equal(expectedSmootherValues, actualSmootherValues, 1e-3)); + CHECK(hessian_equal(expectedFilterSumarization, actualFilterSumarization, optimalTheta, 1e-8)); + CHECK(assert_equal(expectedSeparatorValues, actualSeparatorValues, 1e-4)); +// +// // -// // Create a set of optimizer parameters -// LevenbergMarquardtParams parameters; -// double lag = 4.0; // -// // Create a Concurrent Batch Filter -// ConcurrentBatchFilterTester filter(parameters, lag); // -// // Create containers to keep the full graph -// Values newTheta, fullTheta; -// NonlinearFactorGraph newGraph, fullGraph; -// ConcurrentBatchFilter::KeyTimestampMap newTimestamps; // -// // Create all factors -// CreateFactors(newGraph, newTheta, newTimestamps, 0, 13); -// fullTheta.insert(newTheta); -// fullGraph.push_back(newGraph); // -// // Optimize with Concurrent Batch Filter -// filter.update(newGraph, newTheta, newTimestamps); -// Values updatedTheta = filter.calculateEstimate(); // // // Eliminate the factor graph into a Bayes Tree to create the summarized factors -// // Create Ordering -// std::map constraints; -// constraints[Symbol('X', 8)] = 1; -// constraints[Symbol('X', 9)] = 1; -// constraints[Symbol('X', 10)] = 1; -// constraints[Symbol('X', 11)] = 1; -// constraints[Symbol('X', 12)] = 1; -// Ordering ordering = *fullGraph.orderingCOLAMDConstrained(fullTheta, constraints); -// // Linearize into a Gaussian Factor Graph -// GaussianFactorGraph linearGraph = *fullGraph.linearize(fullTheta, ordering); -// // Eliminate into a Bayes Net with iSAM2-type cliques -// JunctionTree jt(linearGraph); -// ISAM2Clique::shared_ptr root = jt.eliminate(EliminateQR); -// BayesTree bayesTree; -// bayesTree.insert(root); // -// // std::cout << "BAYES TREE:" << std::endl; -// // SymbolicPrintTree(root, ordering.invert(), " "); -// -// // BAYES TREE: -// // P( X7 X9 X12 ) -// // P( X10 | X9 ) -// // P( X11 | X10 ) -// // P( X8 | X9 ) -// // P( X6 | X7 X9 ) -// // P( X3 X5 | X6 ) -// // P( X2 | X3 ) -// // P( X1 | X3 ) -// // P( X0 | X1 ) -// // P( X4 | X7 ) -// -// // Extract the nonlinear factors that should be sent to the smoother -// std::vector smootherKeys; -// smootherKeys.push_back(Symbol('X', 0)); -// smootherKeys.push_back(Symbol('X', 1)); -// smootherKeys.push_back(Symbol('X', 2)); -// smootherKeys.push_back(Symbol('X', 3)); -// smootherKeys.push_back(Symbol('X', 4)); -// smootherKeys.push_back(Symbol('X', 5)); -// smootherKeys.push_back(Symbol('X', 6)); -// NonlinearFactorGraph expectedSmootherFactors; -// BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, fullGraph) { -// if(std::find_first_of(factor->begin(), factor->end(), smootherKeys.begin(), smootherKeys.end()) != factor->end()) { -// expectedSmootherFactors.push_back(factor); -// } -// } -// -// // Extract smoother values -// Values expectedSmootherValues; -// expectedSmootherValues.insert(Symbol('X', 0), updatedTheta.at(Symbol('X', 0))); -// expectedSmootherValues.insert(Symbol('X', 1), updatedTheta.at(Symbol('X', 1))); -// expectedSmootherValues.insert(Symbol('X', 2), updatedTheta.at(Symbol('X', 2))); -// expectedSmootherValues.insert(Symbol('X', 3), updatedTheta.at(Symbol('X', 3))); -// expectedSmootherValues.insert(Symbol('X', 4), updatedTheta.at(Symbol('X', 4))); -// expectedSmootherValues.insert(Symbol('X', 5), updatedTheta.at(Symbol('X', 5))); -// expectedSmootherValues.insert(Symbol('X', 6), updatedTheta.at(Symbol('X', 6))); -// -// // Extract the filter summarized factors -// // Cached factors that represent the filter side (i.e. the X8 and X10 clique) -// NonlinearFactorGraph expectedFilterSumarization; -// expectedFilterSumarization.add(LinearizedJacobianFactor(boost::static_pointer_cast(bayesTree.nodes().at(ordering.at(Symbol('X', 8)))->cachedFactor()), ordering, fullTheta)); -// expectedFilterSumarization.add(LinearizedJacobianFactor(boost::static_pointer_cast(bayesTree.nodes().at(ordering.at(Symbol('X', 10)))->cachedFactor()), ordering, fullTheta)); -// // And any factors that involve only the root (X7, X9, X12) -// std::vector rootKeys; -// rootKeys.push_back(Symbol('X', 7)); -// rootKeys.push_back(Symbol('X', 9)); -// rootKeys.push_back(Symbol('X', 12)); -// BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, fullGraph) { -// size_t count = 0; -// BOOST_FOREACH(Key key, *factor) { -// if(std::binary_search(rootKeys.begin(), rootKeys.end(), key)) ++count; -// } -// if(count == factor->size()) expectedFilterSumarization.push_back(factor); -// } -// -// // Extract the new root values -// Values expectedRootValues; -// expectedRootValues.insert(Symbol('X', 7), updatedTheta.at(Symbol('X', 7))); -// expectedRootValues.insert(Symbol('X', 9), updatedTheta.at(Symbol('X', 9))); -// expectedRootValues.insert(Symbol('X', 12), updatedTheta.at(Symbol('X', 12))); -// -// -// -// // Start the synchronization process -// NonlinearFactorGraph actualSmootherFactors, actualFilterSumarization, smootherSummarization; -// Values actualSmootherValues, actualRootValues; -// filter.presync(); -// filter.synchronize(smootherSummarization); // Supplying an empty factor graph here -// filter.getSmootherFactors(actualSmootherFactors, actualSmootherValues); -// filter.getSummarizedFactors(actualFilterSumarization, actualRootValues); -// filter.postsync(); -// -// -// -// // Compare filter sync variables versus the expected -// CHECK(hessian_equal(expectedSmootherFactors, actualSmootherFactors, updatedTheta, 1e-8)); -// CHECK(assert_equal(expectedSmootherValues, actualSmootherValues, 1e-4)); -// CHECK(hessian_equal(expectedFilterSumarization, actualFilterSumarization, updatedTheta, 1e-9)); -// CHECK(assert_equal(expectedRootValues, actualRootValues, 1e-4)); -// -// -// -// -// -// -// // Now add additional factors to the filter and re-sync -// newGraph.resize(0); newTheta.clear(); newTimestamps.clear(); -// CreateFactors(newGraph, newTheta, newTimestamps, 13, 20); -// fullTheta.insert(newTheta); -// fullGraph.push_back(newGraph); -// -// // Optimize with Concurrent Batch Filter -// filter.update(newGraph, newTheta, newTimestamps); -// updatedTheta = filter.calculateEstimate(); -// -// // Eliminate the factor graph into a Bayes Tree to create the summarized factors // // Create Ordering // constraints.clear(); // constraints[Symbol('X', 15)] = 1; @@ -729,126 +828,78 @@ TEST_UNSAFE( ConcurrentBatchFilter, update_incremental_with_marginalization ) // constraints[Symbol('X', 17)] = 1; // constraints[Symbol('X', 18)] = 1; // constraints[Symbol('X', 19)] = 1; -// ordering = *fullGraph.orderingCOLAMDConstrained(fullTheta, constraints); +// ordering = *fullGraph.orderingCOLAMDConstrained(optimalTheta, constraints); // // Linearize into a Gaussian Factor Graph -// linearGraph = *fullGraph.linearize(fullTheta, ordering); +// linearGraph = *fullGraph.linearize(optimalTheta, ordering); // // Eliminate into a Bayes Net with iSAM2-type cliques // jt = JunctionTree(linearGraph); // root = jt.eliminate(EliminateQR); // bayesTree = BayesTree(); // bayesTree.insert(root); // -// // std::cout << "BAYES TREE:" << std::endl; -// // SymbolicPrintTree(root, ordering.invert(), " "); -// -// // BAYES TREE: -// // P( X14 X16 X19 ) -// // P( X17 | X16 ) -// // P( X18 | X17 ) -// // P( X15 | X16 ) -// // P( X13 | X14 X16 ) -// // P( X11 | X13 X14 ) -// // P( X10 | X11 X13 ) -// // P( X12 | X10 X13 ) -// // P( X9 | X12 X10 ) -// // P( X7 | X9 X12 ) -// // P( X6 | X7 X9 ) -// // P( X3 X5 | X6 ) -// // P( X2 | X3 ) -// // P( X1 | X3 ) -// // P( X0 | X1 ) -// // P( X4 | X7 ) -// // P( X8 | X9 ) // // // Extract the cached factors for X4 and X6. These are the new smoother summarized factors -// // TODO: I'm concerned about the linearization point used to create these factors. It may need to be the updated lin points? // smootherSummarization.resize(0); -// smootherSummarization.add(LinearizedJacobianFactor(boost::static_pointer_cast(bayesTree.nodes().at(ordering.at(Symbol('X', 4)))->cachedFactor()), ordering, fullTheta)); -// smootherSummarization.add(LinearizedJacobianFactor(boost::static_pointer_cast(bayesTree.nodes().at(ordering.at(Symbol('X', 6)))->cachedFactor()), ordering, fullTheta)); -// -// -// -// +// smootherSummarization.add(LinearContainerFactor(bayesTree.nodes().at(ordering.at(Symbol('X', 4)))->cachedFactor(), ordering, optimalTheta)); +// smootherSummarization.add(LinearContainerFactor(bayesTree.nodes().at(ordering.at(Symbol('X', 6)))->cachedFactor(), ordering, optimalTheta)); // // // Extract the nonlinear factors that should be sent to the smoother -// smootherKeys.clear(); -// smootherKeys.push_back(Symbol('X', 8)); -// smootherKeys.push_back(Symbol('X', 10)); -// smootherKeys.push_back(Symbol('X', 11)); -// smootherKeys.push_back(Symbol('X', 13)); // expectedSmootherFactors.resize(0); // BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, fullGraph) { -// if(std::find_first_of(factor->begin(), factor->end(), smootherKeys.begin(), smootherKeys.end()) != factor->end()) { +// if(std::find_first_of(factor->begin(), factor->end(), marginalizeKeys.begin(), marginalizeKeys.end()) != factor->end()) { // expectedSmootherFactors.push_back(factor); // } // } -// // And any factors that involve only the old root (X7, X9, X12) -// rootKeys.clear(); -// rootKeys.push_back(Symbol('X', 7)); -// rootKeys.push_back(Symbol('X', 9)); -// rootKeys.push_back(Symbol('X', 12)); -// BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, fullGraph) { -// size_t count = 0; -// BOOST_FOREACH(Key key, *factor) { -// if(std::binary_search(rootKeys.begin(), rootKeys.end(), key)) ++count; -// } -// if(count == factor->size()) expectedSmootherFactors.push_back(factor); -// } +// // And any factors that involve only the old separator (X7, X9) +// separatorKeys.clear(); +// separatorKeys.insert(Symbol('X', 7)); +// separatorKeys.insert(Symbol('X', 9)); +// FindFactorsWithOnly(separatorKeys, fullGraph, expectedSmootherFactors); // // // // Extract smoother Values // expectedSmootherValues.clear(); -// expectedSmootherValues.insert(Symbol('X', 7), updatedTheta.at(Symbol('X', 7))); -// expectedSmootherValues.insert(Symbol('X', 8), updatedTheta.at(Symbol('X', 8))); -// expectedSmootherValues.insert(Symbol('X', 9), updatedTheta.at(Symbol('X', 9))); -// expectedSmootherValues.insert(Symbol('X', 10), updatedTheta.at(Symbol('X', 10))); -// expectedSmootherValues.insert(Symbol('X', 11), updatedTheta.at(Symbol('X', 11))); -// expectedSmootherValues.insert(Symbol('X', 12), updatedTheta.at(Symbol('X', 12))); -// expectedSmootherValues.insert(Symbol('X', 13), updatedTheta.at(Symbol('X', 13))); +// BOOST_FOREACH(Key key, marginalizeKeys) { +// expectedSmootherValues.insert(key, optimalTheta.at(key)); +// } // // // Extract the filter summarized factors // // Cached factors that represent the filter side (i.e. the X15 and X17 clique) // expectedFilterSumarization.resize(0); -// expectedFilterSumarization.add(LinearizedJacobianFactor(boost::static_pointer_cast(bayesTree.nodes().at(ordering.at(Symbol('X', 15)))->cachedFactor()), ordering, fullTheta)); -// expectedFilterSumarization.add(LinearizedJacobianFactor(boost::static_pointer_cast(bayesTree.nodes().at(ordering.at(Symbol('X', 17)))->cachedFactor()), ordering, fullTheta)); -// // And any factors that involve only the root (X14, X16, X17) -// rootKeys.clear(); -// rootKeys.push_back(Symbol('X', 14)); -// rootKeys.push_back(Symbol('X', 16)); -// rootKeys.push_back(Symbol('X', 19)); -// BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, fullGraph) { -// size_t count = 0; -// BOOST_FOREACH(Key key, *factor) { -// if(std::binary_search(rootKeys.begin(), rootKeys.end(), key)) ++count; -// } -// if(count == factor->size()) expectedFilterSumarization.push_back(factor); -// } +// expectedFilterSumarization.add(LinearContainerFactor(bayesTree.nodes().at(ordering.at(Symbol('X', 15)))->cachedFactor(), ordering, optimalTheta)); +// expectedFilterSumarization.add(LinearContainerFactor(bayesTree.nodes().at(ordering.at(Symbol('X', 17)))->cachedFactor(), ordering, optimalTheta)); +// // And any factors that involve only the new separator (X14, X16) +// separatorKeys.clear(); +// separatorKeys.insert(Symbol('X', 14)); +// separatorKeys.insert(Symbol('X', 16)); +// FindFactorsWithOnly(separatorKeys, fullGraph, expectedFilterSumarization); // // // Extract the new root keys -// expectedRootValues.clear(); -// expectedRootValues.insert(Symbol('X', 14), updatedTheta.at(Symbol('X', 14))); -// expectedRootValues.insert(Symbol('X', 16), updatedTheta.at(Symbol('X', 16))); -// expectedRootValues.insert(Symbol('X', 19), updatedTheta.at(Symbol('X', 19))); +// expectedSeparatorValues.clear(); +// BOOST_FOREACH(Key key, separatorKeys) { +// expectedSeparatorValues.insert(key, optimalTheta.at(key)); +// } // // // // // Start the synchronization process // actualSmootherFactors.resize(0); actualFilterSumarization.resize(0); -// actualSmootherValues.clear(); actualRootValues.clear(); +// actualSmootherValues.clear(); actualSeparatorValues.clear(); +// smootherSeparatorValues = expectedSeparatorValues; // filter.presync(); -// filter.synchronize(smootherSummarization); +// filter.synchronize(smootherSummarization, smootherSeparatorValues); // filter.getSmootherFactors(actualSmootherFactors, actualSmootherValues); -// filter.getSummarizedFactors(actualFilterSumarization, actualRootValues); +// filter.getSummarizedFactors(actualFilterSumarization, actualSeparatorValues); // filter.postsync(); // // // // // Compare filter sync variables versus the expected -// CHECK(hessian_equal(expectedSmootherFactors, actualSmootherFactors, updatedTheta, 1e-8)); +// CHECK(hessian_equal(expectedSmootherFactors, actualSmootherFactors, optimalTheta, 1e-8)); // CHECK(assert_equal(expectedSmootherValues, actualSmootherValues, 1e-3)); -// CHECK(hessian_equal(expectedFilterSumarization, actualFilterSumarization, updatedTheta, 1e-8)); -// CHECK(assert_equal(expectedRootValues, actualRootValues, 1e-4)); -//} +// CHECK(hessian_equal(expectedFilterSumarization, actualFilterSumarization, optimalTheta, 1e-8)); +// CHECK(assert_equal(expectedSeparatorValues, actualSeparatorValues, 1e-4)); +} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);}