diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index b6bef00b8..02d9b4e9d 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -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 f(model, Point2(0, 0), p); + EXPECT_LONGS_EQUAL(2, f.dim()); + boost::shared_ptr gf = f.linearize(values); + boost::shared_ptr jf = // + boost::dynamic_pointer_cast(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 > 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 f(model, Point2(0, 0), p); EXPECT_LONGS_EQUAL(2, f.dim()); boost::shared_ptr gf = f.linearize(values); diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 390257f02..739fe52fb 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -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)); }