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

New Leaf/noise tests

release/4.3a0
dellaert 2014-10-08 11:06:42 +02:00
parent d38bcf1805
commit ba9faa68b6
1 changed files with 50 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
gtsam_unstable/nonlinear/tests/testBADFactor.cpp
View File

@ -33,6 +33,56 @@ using namespace gtsam;
Point2 measured(-17, 30);
SharedNoiseModel model = noiseModel::Unit::Create(2);
/* ************************************************************************* */
// Leaf
TEST(BADFactor, leaf) {
// Create some values
Values values;
values.insert(2, Point2(3, 5));
JacobianFactor expected( //
2, (Matrix(2, 2) << 1, 0, 0, 1), //
(Vector(2) << -3, -5));
// Create leaves
Point2_ p(2);
// Try concise version
BADFactor<Point2> f(model, Point2(0, 0), p);
EXPECT_LONGS_EQUAL(2, f.dim());
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf);
EXPECT( assert_equal(expected, *jf, 1e-9));
}
/* ************************************************************************* */
// non-zero noise model
TEST(BADFactor, model) {
// Create some values
Values values;
values.insert(2, Point2(3, 5));
JacobianFactor expected( //
2, (Matrix(2, 2) << 10, 0, 0, 100), //
(Vector(2) << -30, -500));
// Create leaves
Point2_ p(2);
// Try concise version
SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.01));
BADFactor<Point2> f(model, Point2(0, 0), p);
EXPECT_LONGS_EQUAL(2, f.dim());
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf);
EXPECT( assert_equal(expected, *jf, 1e-9));
}
/* ************************************************************************* */
// Unary(Leaf))
TEST(BADFactor, test) {