From 568335d21fa53aab4221b2112be1312a24befcb0 Mon Sep 17 00:00:00 2001 From: Luca Carlone Date: Mon, 19 Aug 2013 21:33:13 +0000 Subject: [PATCH] In progress fixing filter/smoother unit tests --- gtsam_unstable/nonlinear/CMakeLists.txt | 2 +- .../tests/testConcurrentIncrementalFilter.cpp | 28 ++++--------------- 2 files changed, 6 insertions(+), 24 deletions(-) diff --git a/gtsam_unstable/nonlinear/CMakeLists.txt b/gtsam_unstable/nonlinear/CMakeLists.txt index 876f342b6..b4f386065 100644 --- a/gtsam_unstable/nonlinear/CMakeLists.txt +++ b/gtsam_unstable/nonlinear/CMakeLists.txt @@ -9,7 +9,7 @@ ${gtsam_unstable-default}) # Exclude tests that don't work set (nonlinear_excluded_tests #"") -"${CMAKE_CURRENT_SOURCE_DIR}/tests/testConcurrentIncrementalFilter.cpp" +#"${CMAKE_CURRENT_SOURCE_DIR}/tests/testConcurrentIncrementalFilter.cpp" #"${CMAKE_CURRENT_SOURCE_DIR}/tests/testIncrementalFixedLagSmoother.cpp" ) diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp index 6c6edd4e6..143c79d83 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp @@ -31,6 +31,9 @@ #include #include +#include +tbb::task_scheduler_init init(1); + using namespace std; using namespace gtsam; @@ -105,10 +108,6 @@ NonlinearFactorGraph CalculateMarginals(const NonlinearFactorGraph& factorGraph, LinearContainerForGaussianMarginals.push_back(LinearContainerFactor(factor, linPoint)); } - NonlinearFactorGraph LinearContainerForGaussianMarginals; - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal.second) { - LinearContainerForGaussianMarginals.push_back(LinearContainerFactor(factor, ordering, linPoint)); - } return LinearContainerForGaussianMarginals; } @@ -431,16 +430,8 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_1 ) partialGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); partialGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - Values partialValues; - // ISAM2 after the update is computing the delta, without applying them to the current lin point - partialValues.insert(1, newValues.at(1)); - partialValues.insert(2, newValues.at(2)); - partialValues.insert(3, newValues.at(3)); + GaussianFactorGraph linearGraph = *partialGraph.linearize(newValues); - - GaussianFactorGraph linearGraph = *factorGraph.linearize(linPoint); - - // TODO: Correct? GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector(keysToMove.begin(), keysToMove.end()), EliminateCholesky).second; NonlinearFactorGraph expectedGraph; @@ -523,17 +514,8 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 ) partialGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); partialGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - Values partialValues; - // ISAM2 after the update is computing the delta, without applying them to the current lin point - partialValues.insert(1, optimalValues.at(1)); - partialValues.insert(2, optimalValues.at(2)); - partialValues.insert(3, optimalValues.at(3)); + GaussianFactorGraph linearGraph = *partialGraph.linearize(newValues); - - - GaussianFactorGraph linearGraph = *factorGraph.linearize(linPoint); - - // TODO: correct? GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector(keysToMove.begin(), keysToMove.end()), EliminateCholesky).second; NonlinearFactorGraph expectedGraph;