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

Non-trivial noise models now correctly handled (at a small performance penalty, due to malloc of Vector b).

release/4.3a0
dellaert 2014-11-02 14:37:52 +01:00
parent 8a6d8bfc82
commit d2f56b13ed
2 changed files with 41 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam_unstable/nonlinear/ExpressionFactor.h
View File

@ -108,7 +108,7 @@ public:
virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const { virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const {
// Only linearize if the factor is active // Only linearize if the factor is active
if (!this->active(x)) if (!active(x))
return boost::shared_ptr<JacobianFactor>(); return boost::shared_ptr<JacobianFactor>();
// Create a writeable JacobianFactor in advance // Create a writeable JacobianFactor in advance
@ -128,11 +128,13 @@ public:
// Evaluate error to get Jacobians and RHS vector b // Evaluate error to get Jacobians and RHS vector b
T value = expression_.value(x, map); // <<< Reverse AD happens here ! 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 // Whiten the corresponding system
// TODO ! this->noiseModel_->WhitenSystem(Ab); // 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; return factor;
} }
}; };

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

@ -65,41 +65,41 @@ TEST(ExpressionFactor, Leaf) {
EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9)); EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9));
} }
///* ************************************************************************* */ /* ************************************************************************* */
//// non-zero noise model // non-zero noise model
//TEST(ExpressionFactor, Model) { TEST(ExpressionFactor, Model) {
// using namespace leaf; using namespace leaf;
//
// SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.01)); SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.01));
//
// // Create old-style factor to create expected value and derivatives // Create old-style factor to create expected value and derivatives
// PriorFactor<Point2> old(2, Point2(0, 0), model); PriorFactor<Point2> old(2, Point2(0, 0), model);
//
// // Concise version // Concise version
// ExpressionFactor<Point2> f(model, Point2(0, 0), p); ExpressionFactor<Point2> f(model, Point2(0, 0), p);
// EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9); EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9);
// EXPECT_LONGS_EQUAL(2, f.dim()); EXPECT_LONGS_EQUAL(2, f.dim());
// boost::shared_ptr<GaussianFactor> gf2 = f.linearize(values); boost::shared_ptr<GaussianFactor> gf2 = f.linearize(values);
// EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9)); EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9));
//} }
//
///* ************************************************************************* */ /* ************************************************************************* */
//// Constrained noise model // Constrained noise model
//TEST(ExpressionFactor, Constrained) { TEST(ExpressionFactor, Constrained) {
// using namespace leaf; using namespace leaf;
//
// SharedDiagonal model = noiseModel::Constrained::MixedSigmas(Vector2(0.2, 0)); SharedDiagonal model = noiseModel::Constrained::MixedSigmas(Vector2(0.2, 0));
//
// // Create old-style factor to create expected value and derivatives // Create old-style factor to create expected value and derivatives
// PriorFactor<Point2> old(2, Point2(0, 0), model); PriorFactor<Point2> old(2, Point2(0, 0), model);
//
// // Concise version // Concise version
// ExpressionFactor<Point2> f(model, Point2(0, 0), p); ExpressionFactor<Point2> f(model, Point2(0, 0), p);
// EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9); EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9);
// EXPECT_LONGS_EQUAL(2, f.dim()); EXPECT_LONGS_EQUAL(2, f.dim());
// boost::shared_ptr<GaussianFactor> gf2 = f.linearize(values); boost::shared_ptr<GaussianFactor> gf2 = f.linearize(values);
// EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9)); EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9));
//} }
/* ************************************************************************* */ /* ************************************************************************* */
// Unary(Leaf)) // Unary(Leaf))