diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index 69056e6ef..f7b61120a 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -108,7 +108,7 @@ public: virtual boost::shared_ptr linearize(const Values& x) const { // Only linearize if the factor is active - if (!this->active(x)) + if (!active(x)) return boost::shared_ptr(); // Create a writeable JacobianFactor in advance @@ -128,11 +128,13 @@ public: // Evaluate error to get Jacobians and RHS vector b T value = expression_.value(x, map); // <<< Reverse AD happens here ! - Ab(size()).col(0) = -measurement_.localCoordinates(value); + Vector b(-measurement_.localCoordinates(value)); - // Whiten the corresponding system now - // TODO ! this->noiseModel_->WhitenSystem(Ab); + // Whiten the corresponding system + // Note the Ab.matrix() includes uninitialized RHS, but this beats taking it apart + noiseModel_->WhitenSystem(Ab.matrix(),b); + Ab(size()).col(0) = b; return factor; } }; diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index 23c2fa1ce..fa2e5cadb 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -65,41 +65,41 @@ TEST(ExpressionFactor, Leaf) { 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 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 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)); -// -// // 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 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 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 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)); + + // 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 gf2 = f.linearize(values); + EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9)); +} /* ************************************************************************* */ // Unary(Leaf))