diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index b1a16d2a3..5a678c0e3 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -82,12 +82,7 @@ public: noiseModel::Constrained::shared_ptr constrained = // boost::dynamic_pointer_cast(this->noiseModel_); if (constrained) { - // Create a factor of reduced row dimension d_ - size_t d_ = b.size() - constrained->dim(); - Vector zero_ = zero(d_); - Vector b_ = concatVectors(2, &b, &zero_); - noiseModel::Constrained::shared_ptr model = constrained->unit(d_); - return boost::make_shared(terms, b_, model); + return boost::make_shared(terms, b, constrained->unit()); } else return boost::make_shared(terms, b); } diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index 02d9b4e9d..eb4f1e52e 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include @@ -33,80 +34,68 @@ using namespace gtsam; Point2 measured(-17, 30); SharedNoiseModel model = noiseModel::Unit::Create(2); +namespace leaf { +// Create some values +struct MyValues: public Values { + MyValues() { + insert(2, Point2(3, 5)); + } +} values; + +// Create leaf +Point2_ p(2); +} + /* ************************************************************************* */ // Leaf TEST(ExpressionFactor, leaf) { + using namespace 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 leaf - Point2_ p(2); + // Create old-style factor to create expected value and derivatives + PriorFactor old(2, Point2(0, 0), model); // Concise version ExpressionFactor f(model, Point2(0, 0), p); + EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9); 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)); + boost::shared_ptr gf2 = f.linearize(values); + EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9)); } /* ************************************************************************* */ // non-zero noise model TEST(ExpressionFactor, model) { + using namespace leaf; SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.01)); - // 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 leaf - Point2_ p(2); + // Create old-style factor to create expected value and derivatives + PriorFactor old(2, Point2(0, 0), model); // Concise version ExpressionFactor f(model, Point2(0, 0), p); + EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9); 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)); + boost::shared_ptr gf2 = f.linearize(values); + EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9)); } /* ************************************************************************* */ // Constrained noise model TEST(ExpressionFactor, constrained) { + using namespace leaf; - SharedDiagonal model = noiseModel::Constrained::MixedSigmas(Vector2(0.2,0)); + 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); + // Create old-style factor to create expected value and derivatives + PriorFactor old(2, Point2(0, 0), model); // Concise version ExpressionFactor f(model, Point2(0, 0), p); + EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9); 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)); + boost::shared_ptr gf2 = f.linearize(values); + EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9)); } /* ************************************************************************* */