diff --git a/gtsam_unstable/nonlinear/tests/testBADFactor.cpp b/gtsam_unstable/nonlinear/tests/testBADFactor.cpp index 44a7eab3f..635a19235 100644 --- a/gtsam_unstable/nonlinear/tests/testBADFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testBADFactor.cpp @@ -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 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)); +} + +/* ************************************************************************* */ +// 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 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)); +} + /* ************************************************************************* */ // Unary(Leaf)) TEST(BADFactor, test) {