diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp index dc00da846..79ca79378 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp @@ -811,94 +811,6 @@ TEST_UNSAFE( ConcurrentBatchFilter, synchronize ) CHECK(assert_equal(expectedSmootherValues, actualSmootherValues, 1e-3)); CHECK(hessian_equal(expectedFilterSumarization, actualFilterSumarization, optimalTheta, 1e-8)); CHECK(assert_equal(expectedSeparatorValues, actualSeparatorValues, 1e-4)); -// -// -// -// -// -// -// -// -// // 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(optimalTheta, constraints); -// // Linearize into a Gaussian Factor Graph -// 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); -// -// -// // Extract the cached factors for X4 and X6. These are the new smoother summarized factors -// smootherSummarization.resize(0); -// 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 -// expectedSmootherFactors.resize(0); -// BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, fullGraph) { -// 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 separator (X7, X9) -// separatorKeys.clear(); -// separatorKeys.insert(Symbol('X', 7)); -// separatorKeys.insert(Symbol('X', 9)); -// FindFactorsWithOnly(separatorKeys, fullGraph, expectedSmootherFactors); -// -// -// // Extract smoother Values -// expectedSmootherValues.clear(); -// 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(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 -// 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)); } /* ************************************************************************* */