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 {
// Only linearize if the factor is active
if (!this->active(x))
if (!active(x))
return boost::shared_ptr<JacobianFactor>();
// 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;
}
};

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));
}
///* ************************************************************************* */
//// 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<Point2> old(2, Point2(0, 0), model);
//
// // Concise version
// ExpressionFactor<Point2> 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<GaussianFactor> 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<Point2> old(2, Point2(0, 0), model);
//
// // Concise version
// ExpressionFactor<Point2> 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<GaussianFactor> 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<Point2> old(2, Point2(0, 0), model);
// Concise version
ExpressionFactor<Point2> 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<GaussianFactor> 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<Point2> old(2, Point2(0, 0), model);
// Concise version
ExpressionFactor<Point2> 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<GaussianFactor> gf2 = f.linearize(values);
EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9));
}
/* ************************************************************************* */
// Unary(Leaf))