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

Added unit test for NonlinearISAM with a simple markov chain demo

release/4.3a0
Alex Cunningham 2011-01-20 15:42:11 +00:00
parent 9ac20eea8a
commit 4367a245bd
2 changed files with 63 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
tests/Makefile.am
View File

@ -19,6 +19,7 @@ check_PROGRAMS += testNonlinearEquality testNonlinearFactor testNonlinearFactorG
check_PROGRAMS += testNonlinearOptimizer
check_PROGRAMS += testSymbolicBayesNet testSymbolicFactorGraph
check_PROGRAMS += testTupleValues
check_PROGRAMS += testNonlinearISAM
# only if serialization is available
if ENABLE_SERIALIZATION

62
tests/testNonlinearISAM.cpp Normal file
View File

@ -0,0 +1,62 @@
/**
* @file testNonlinearISAM
* @author Alex Cunningham
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/linear/Sampler.h>
#include <gtsam/nonlinear/NonlinearISAM-inl.h>
#include <gtsam/slam/planarSLAM.h>
using namespace gtsam;
using namespace planarSLAM;
typedef NonlinearISAM<Values> PlanarISAM;
const double tol=1e-5;
/* ************************************************************************* */
TEST(testNonlinearISAM, markov_chain ) {
int reorder_interval = 2;
PlanarISAM isam(reorder_interval);
SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(3, 3.0, 3.0, 0.5));
Sampler sampler(model, 42u);
// create initial graph
PoseKey key(0);
Pose2 cur_pose; // start at origin
Graph start_factors;
start_factors.addPoseConstraint(key, cur_pose);
Values init;
Values expected;
init.insert(key, cur_pose);
expected.insert(key, cur_pose);
isam.update(start_factors, init);
// loop for a period of time to verify memory usage
size_t nrPoses = 21;
Pose2 z(1.0, 2.0, 0.1);
for (size_t i=1; i<=nrPoses; ++i) {
Graph new_factors;
PoseKey key1(i-1), key2(i);
new_factors.addOdometry(key1, key2, z, model);
Values new_init;
cur_pose = cur_pose.compose(z);
new_init.insert(key2, cur_pose.expmap(sampler.sample()));
expected.insert(key2, cur_pose);
isam.update(new_factors, new_init);
}
// verify values - all but the last one should be very close
Values actual = isam.estimate();
for (size_t i=0; i<21; ++i) {
PoseKey cur_key(i);
EXPECT(assert_equal(expected[cur_key], actual[cur_key], tol));
}
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */