12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

In progress fixing filter/smoother unit tests

release/4.3a0
Luca Carlone 2013-08-19 21:33:13 +00:00
parent 46ab10fb06
commit 568335d21f
2 changed files with 6 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam_unstable/nonlinear/CMakeLists.txt
View File

@ -9,7 +9,7 @@ ${gtsam_unstable-default})
# Exclude tests that don't work # Exclude tests that don't work
set (nonlinear_excluded_tests #"") 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" #"${CMAKE_CURRENT_SOURCE_DIR}/tests/testIncrementalFixedLagSmoother.cpp"
) )

28
gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp
View File

@ -31,6 +31,9 @@
#include <gtsam/base/TestableAssertions.h> #include <gtsam/base/TestableAssertions.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <tbb/tbb.h>
tbb::task_scheduler_init init(1);
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
@ -105,10 +108,6 @@ NonlinearFactorGraph CalculateMarginals(const NonlinearFactorGraph& factorGraph,
LinearContainerForGaussianMarginals.push_back(LinearContainerFactor(factor, linPoint)); 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; return LinearContainerForGaussianMarginals;
} }
@ -431,16 +430,8 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_1 )
partialGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery)); partialGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
partialGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery)); partialGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
Values partialValues; GaussianFactorGraph linearGraph = *partialGraph.linearize(newValues);
// 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 = *factorGraph.linearize(linPoint);
// TODO: Correct?
GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(keysToMove.begin(), keysToMove.end()), EliminateCholesky).second; GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(keysToMove.begin(), keysToMove.end()), EliminateCholesky).second;
NonlinearFactorGraph expectedGraph; NonlinearFactorGraph expectedGraph;
@ -523,17 +514,8 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 )
partialGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery)); partialGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
partialGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery)); partialGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
Values partialValues; GaussianFactorGraph linearGraph = *partialGraph.linearize(newValues);
// 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 = *factorGraph.linearize(linPoint);
// TODO: correct?
GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(keysToMove.begin(), keysToMove.end()), EliminateCholesky).second; GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(keysToMove.begin(), keysToMove.end()), EliminateCholesky).second;
NonlinearFactorGraph expectedGraph; NonlinearFactorGraph expectedGraph;