diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp index 9eafd8a96..666dddd05 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp @@ -17,24 +17,20 @@ */ #include -#include #include #include -#include #include #include #include #include #include #include -#include #include #include using namespace std; using namespace gtsam; -namespace { // Set up initial pose, odometry difference, loop closure difference, and initialization errors const Pose3 poseInitial; @@ -49,7 +45,7 @@ const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas(Vector_(6, 0.25, 0 // Create a derived class to allow testing protected member functions class ConcurrentBatchFilterTester : public ConcurrentBatchFilter { public: - ConcurrentBatchFilterTester(const LevenbergMarquardtParams& parameters, double lag) : ConcurrentBatchFilter(parameters, lag) { }; + ConcurrentBatchFilterTester(const LevenbergMarquardtParams& parameters) : ConcurrentBatchFilter(parameters) { }; virtual ~ConcurrentBatchFilterTester() { }; // Add accessors to the protected members @@ -103,7 +99,7 @@ bool hessian_equal(const NonlinearFactorGraph& expected, const NonlinearFactorGr } ///* ************************************************************************* */ -void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFilter::KeyTimestampMap& timestamps, size_t index1 = 0, size_t index2 = 1) { +void CreateFactors(NonlinearFactorGraph& graph, Values& theta, size_t index1 = 0, size_t index2 = 1) { // Calculate all poses Pose3 poses[20]; @@ -128,7 +124,6 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi graph.add(PriorFactor(keys[0], poses[0], noisePrior)); // Add new variables theta.insert(keys[0], poses[0].compose(poseError)); - timestamps[keys[0]] = double(0); break; } case 1: @@ -138,7 +133,6 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi graph.add(BetweenFactor(keys[0], keys[1], poseDelta, noiseOdometery)); // Add new variables theta.insert(keys[1], poses[1].compose(poseError)); - timestamps[keys[1]] = double(1); break; } case 2: @@ -155,9 +149,7 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi graph.add(BetweenFactor(keys[2], keys[3], poseDelta, noiseOdometery)); // Add new variables theta.insert(keys[2], poses[2].compose(poseError)); - timestamps[keys[2]] = double(2); theta.insert(keys[3], poses[3].compose(poseError)); - timestamps[keys[3]] = double(3); break; } case 4: @@ -171,7 +163,6 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi graph.add(BetweenFactor(keys[3], keys[5], poseDelta, noiseOdometery)); // Add new variables theta.insert(keys[5], poses[5].compose(poseError)); - timestamps[keys[5]] = double(5); break; } case 6: @@ -184,7 +175,6 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi graph.add(BetweenFactor(keys[5], keys[6], poseDelta, noiseOdometery)); // Add new variables theta.insert(keys[6], poses[6].compose(poseError)); - timestamps[keys[6]] = double(6); break; } case 7: @@ -197,9 +187,7 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi graph.add(BetweenFactor(keys[6], keys[7], poseDelta, noiseOdometery)); // Add new variables theta.insert(keys[4], poses[4].compose(poseError)); - timestamps[keys[4]] = double(4); theta.insert(keys[7], poses[7].compose(poseError)); - timestamps[keys[7]] = double(7); break; } case 8: @@ -218,9 +206,7 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi graph.add(BetweenFactor(keys[8], keys[9], poseDelta, noiseOdometery)); // Add new variables theta.insert(keys[8], poses[8].compose(poseError)); - timestamps[keys[8]] = double(8); theta.insert(keys[9], poses[9].compose(poseError)); - timestamps[keys[9]] = double(9); break; } case 10: @@ -230,7 +216,6 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi graph.add(BetweenFactor(keys[9], keys[10], poseDelta, noiseOdometery)); // Add new variables theta.insert(keys[10], poses[10].compose(poseError)); - timestamps[keys[10]] = double(10); break; } case 11: @@ -240,7 +225,6 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi graph.add(BetweenFactor(keys[10], keys[11], poseDelta, noiseOdometery)); // Add new variables theta.insert(keys[11], poses[11].compose(poseError)); - timestamps[keys[11]] = double(11); break; } case 12: @@ -253,7 +237,6 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi graph.add(BetweenFactor(keys[9], keys[12], poseDelta, noiseOdometery)); // Add new variables theta.insert(keys[12], poses[12].compose(poseError)); - timestamps[keys[12]] = double(12); break; } @@ -271,7 +254,6 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi graph.add(BetweenFactor(keys[12], keys[13], poseDelta, noiseOdometery)); // Add new variables theta.insert(keys[13], poses[13].compose(poseError)); - timestamps[keys[13]] = double(13); break; } case 14: @@ -284,7 +266,6 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi graph.add(BetweenFactor(keys[13], keys[14], poseDelta, noiseOdometery)); // Add new variables theta.insert(keys[14], poses[14].compose(poseError)); - timestamps[keys[14]] = double(14); break; } case 15: @@ -303,9 +284,7 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi graph.add(BetweenFactor(keys[15], keys[16], poseDelta, noiseOdometery)); // Add new variables theta.insert(keys[15], poses[15].compose(poseError)); - timestamps[keys[15]] = double(15); theta.insert(keys[16], poses[16].compose(poseError)); - timestamps[keys[16]] = double(16); break; } case 17: @@ -315,7 +294,6 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi graph.add(BetweenFactor(keys[16], keys[17], poseDelta, noiseOdometery)); // Add new variables theta.insert(keys[17], poses[17].compose(poseError)); - timestamps[keys[17]] = double(17); break; } case 18: @@ -325,7 +303,6 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi graph.add(BetweenFactor(keys[17], keys[18], poseDelta, noiseOdometery)); // Add new variables theta.insert(keys[18], poses[18].compose(poseError)); - timestamps[keys[18]] = double(18); break; } case 19: @@ -338,7 +315,6 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi graph.add(BetweenFactor(keys[16], keys[19], poseDelta, noiseOdometery)); // Add new variables theta.insert(keys[19], poses[19].compose(poseError)); - timestamps[keys[19]] = double(19); break; } @@ -349,7 +325,7 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi } /* ************************************************************************* */ -Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, const Values& rootValues = Values()) { +Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, const Values& separatorValues = Values()) { // Create an L-M optimizer LevenbergMarquardtParams parameters; @@ -361,9 +337,9 @@ Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, con double currentError; do { // Force variables associated with root keys to keep the same linearization point - if(rootValues.size() > 0) { + if(separatorValues.size() > 0) { // Put the old values of the root keys back into the optimizer state - optimizer.state().values.update(rootValues); + optimizer.state().values.update(separatorValues); // Update the error value with the new theta optimizer.state().error = graph.error(optimizer.state().values); } @@ -380,60 +356,60 @@ 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 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 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+" "); - } -} - -} - -/* ************************************************************************* */ -TEST_UNSAFE( ConcurrentBatchFilter, update_Batch ) +TEST_UNSAFE( 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,23 +417,22 @@ TEST_UNSAFE( ConcurrentBatchFilter, update_Batch ) // Create a set of optimizer parameters LevenbergMarquardtParams parameters; - double lag = 4.0; // Create a Concurrent Batch Filter - ConcurrentBatchFilter filter(parameters, lag); + ConcurrentBatchFilter filter(parameters); // Create containers to keep the full graph Values fullTheta; NonlinearFactorGraph fullGraph; - ConcurrentBatchFilter::KeyTimestampMap fullTimestamps; // Create all factors - CreateFactors(fullGraph, fullTheta, fullTimestamps, 0, 20); + CreateFactors(fullGraph, fullTheta, 0, 20); // Optimize with Concurrent Batch Filter - filter.update(fullGraph, fullTheta, fullTimestamps); + filter.update(fullGraph, fullTheta, boost::none); Values actual = filter.calculateEstimate(); + // Optimize with L-M Values expected = BatchOptimize(fullGraph, fullTheta); @@ -465,8 +440,50 @@ 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_Incremental ) +TEST_UNSAFE( 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 @@ -474,10 +491,9 @@ TEST_UNSAFE( ConcurrentBatchFilter, update_Incremental ) // Create a set of optimizer parameters LevenbergMarquardtParams parameters; - double lag = 4.0; // Create a Concurrent Batch Filter - ConcurrentBatchFilter filter(parameters, lag); + ConcurrentBatchFilter filter(parameters); // Create containers to keep the full graph Values fullTheta; @@ -488,13 +504,12 @@ TEST_UNSAFE( ConcurrentBatchFilter, update_Incremental ) // Create containers to keep the new factors Values newTheta; NonlinearFactorGraph newGraph; - ConcurrentBatchFilter::KeyTimestampMap newTimestamps; // Create factors - CreateFactors(newGraph, newTheta, newTimestamps, i, i+1); + CreateFactors(newGraph, newTheta, i, i+1); // Add these entries to the filter - filter.update(newGraph, newTheta, newTimestamps); + filter.update(newGraph, newTheta, boost::none); Values actual = filter.calculateEstimate(); // Add these entries to the full batch version @@ -509,278 +524,330 @@ TEST_UNSAFE( ConcurrentBatchFilter, update_Incremental ) } -/* ************************************************************************* */ -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, 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)); +// } +// +//} - // 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; - constraints[Symbol('X', 16)] = 1; - constraints[Symbol('X', 17)] = 1; - constraints[Symbol('X', 18)] = 1; - constraints[Symbol('X', 19)] = 1; - ordering = *fullGraph.orderingCOLAMDConstrained(fullTheta, constraints); - // Linearize into a Gaussian Factor Graph - linearGraph = *fullGraph.linearize(fullTheta, 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)); - - - - - - // 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()) { - 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); - } - - - // 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))); - - // 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); - } - - // 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))); - - - - // Start the synchronization process - actualSmootherFactors.resize(0); actualFilterSumarization.resize(0); - actualSmootherValues.clear(); actualRootValues.clear(); - filter.presync(); - filter.synchronize(smootherSummarization); - 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-3)); - CHECK(hessian_equal(expectedFilterSumarization, actualFilterSumarization, updatedTheta, 1e-8)); - CHECK(assert_equal(expectedRootValues, actualRootValues, 1e-4)); -} +///* ************************************************************************* */ +//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; +// 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; +// constraints[Symbol('X', 16)] = 1; +// constraints[Symbol('X', 17)] = 1; +// constraints[Symbol('X', 18)] = 1; +// constraints[Symbol('X', 19)] = 1; +// ordering = *fullGraph.orderingCOLAMDConstrained(fullTheta, constraints); +// // Linearize into a Gaussian Factor Graph +// linearGraph = *fullGraph.linearize(fullTheta, 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)); +// +// +// +// +// +// // 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()) { +// 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); +// } +// +// +// // 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))); +// +// // 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); +// } +// +// // 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))); +// +// +// +// // Start the synchronization process +// actualSmootherFactors.resize(0); actualFilterSumarization.resize(0); +// actualSmootherValues.clear(); actualRootValues.clear(); +// filter.presync(); +// filter.synchronize(smootherSummarization); +// 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-3)); +// CHECK(hessian_equal(expectedFilterSumarization, actualFilterSumarization, updatedTheta, 1e-8)); +// CHECK(assert_equal(expectedRootValues, actualRootValues, 1e-4)); +//} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp index cef79dbd0..5c30b1db7 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp @@ -17,25 +17,20 @@ */ #include -#include #include #include -#include #include #include #include #include #include #include -#include #include #include using namespace std; using namespace gtsam; -namespace { - // Set up initial pose, odometry difference, loop closure difference, and initialization errors const Pose3 poseInitial; const Pose3 poseOdometry( Rot3::RzRyRx(Vector_(3, 0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) ); @@ -336,7 +331,7 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, size_t index1 = 0 } /* ************************************************************************* */ -Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, const Values& rootValues = Values()) { +Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, const Values& separatorValues = Values()) { // Create an L-M optimizer LevenbergMarquardtParams parameters; @@ -348,9 +343,9 @@ Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, con double currentError; do { // Force variables associated with root keys to keep the same linearization point - if(rootValues.size() > 0) { + if(separatorValues.size() > 0) { // Put the old values of the root keys back into the optimizer state - optimizer.state().values.update(rootValues); + optimizer.state().values.update(separatorValues); // Update the error value with the new theta optimizer.state().error = graph.error(optimizer.state().values); } @@ -367,60 +362,60 @@ 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 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 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+" "); - } -} - -} - -/* ************************************************************************* */ -TEST_UNSAFE( ConcurrentBatchSmoother, update_Batch ) +TEST_UNSAFE( 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 @@ -451,7 +446,7 @@ TEST_UNSAFE( ConcurrentBatchSmoother, update_Batch ) } /* ************************************************************************* */ -TEST_UNSAFE( ConcurrentBatchSmoother, update_Incremental ) +TEST_UNSAFE( 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 @@ -492,220 +487,220 @@ 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 +///* ************************************************************************* */ +//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)); - - - - // 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. - -} +// +// +// +// // 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. +// +//} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);}