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

Merge branch 'feature/BAD' into feature/BAD_linearize

release/4.3a0
dellaert 2014-10-11 23:07:40 +02:00
parent 3c6f7a42e6 c9f80536c0
commit 86d3e559e6
2 changed files with 36 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp
View File

@ -45,7 +45,7 @@ TEST(ExpressionFactor, leaf) {
2, (Matrix(2, 2) << 1, 0, 0, 1), //
(Vector(2) << -3, -5));
// Create leaves
// Create leaf
Point2_ p(2);
// Concise version
@ -61,6 +61,8 @@ TEST(ExpressionFactor, leaf) {
// non-zero noise model
TEST(ExpressionFactor, model) {
SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.01));
// Create some values
Values values;
values.insert(2, Point2(3, 5));
@ -69,12 +71,36 @@ TEST(ExpressionFactor, model) {
2, (Matrix(2, 2) << 10, 0, 0, 100), //
(Vector(2) << -30, -500));
// Create leaves
// Create leaf
Point2_ p(2);
// Concise version
SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.01));
ExpressionFactor<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));
}
/* ************************************************************************* */
// Constrained noise model
TEST(ExpressionFactor, constrained) {
SharedDiagonal model = noiseModel::Constrained::MixedSigmas(Vector2(0.2,0));
// Create some values
Values values;
values.insert(2, Point2(3, 5));
vector<pair<Key, Matrix> > terms;
terms.push_back(make_pair(2, (Matrix(2, 2) << 1, 0, 0, 1)));
JacobianFactor expected(terms, (Vector(2) << -3, -5), model);
// Create leaf
Point2_ p(2);
// Concise version
ExpressionFactor<Point2> f(model, Point2(0, 0), p);
EXPECT_LONGS_EQUAL(2, f.dim());
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);

16
tests/testNonlinearFactor.cpp
View File

@ -197,8 +197,7 @@ TEST( NonlinearFactor, size )
/* ************************************************************************* */
TEST( NonlinearFactor, linearize_constraint1 )
{
Vector sigmas = (Vector(2) << 0.2, 0.0);
SharedDiagonal constraint = noiseModel::Constrained::MixedSigmas(sigmas);
SharedDiagonal constraint = noiseModel::Constrained::MixedSigmas(Vector2(0.2,0));
Point2 mu(1., -1.);
NonlinearFactorGraph::sharedFactor f0(new simulated2D::Prior(mu, constraint, X(1)));
@ -208,17 +207,16 @@ TEST( NonlinearFactor, linearize_constraint1 )
GaussianFactor::shared_ptr actual = f0->linearize(config);
// create expected
Vector b = (Vector(2) << 0., -3.);
Vector2 b(0., -3.);
JacobianFactor expected(X(1), (Matrix(2, 2) << 5.0, 0.0, 0.0, 1.0), b,
noiseModel::Constrained::MixedSigmas((Vector(2) << 1.0, 0.0)));
noiseModel::Constrained::MixedSigmas(Vector2(1,0)));
CHECK(assert_equal((const GaussianFactor&)expected, *actual));
}
/* ************************************************************************* */
TEST( NonlinearFactor, linearize_constraint2 )
{
Vector sigmas = (Vector(2) << 0.2, 0.0);
SharedDiagonal constraint = noiseModel::Constrained::MixedSigmas(sigmas);
SharedDiagonal constraint = noiseModel::Constrained::MixedSigmas(Vector2(0.2,0));
Point2 z3(1.,-1.);
simulated2D::Measurement f0(z3, constraint, X(1),L(1));
@ -229,10 +227,10 @@ TEST( NonlinearFactor, linearize_constraint2 )
GaussianFactor::shared_ptr actual = f0.linearize(config);
// create expected
Matrix A = (Matrix(2, 2) << 5.0, 0.0, 0.0, 1.0);
Vector b = (Vector(2) << -15., -3.);
Matrix2 A; A << 5.0, 0.0, 0.0, 1.0;
Vector2 b(-15., -3.);
JacobianFactor expected(X(1), -1*A, L(1), A, b,
noiseModel::Constrained::MixedSigmas((Vector(2) << 1.0, 0.0)));
noiseModel::Constrained::MixedSigmas(Vector2(1,0)));
CHECK(assert_equal((const GaussianFactor&)expected, *actual));
}