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

Added a constraint model

release/4.3a0
dellaert 2014-10-11 23:07:23 +02:00
parent e46a8b05eb
commit c9f80536c0
1 changed files with 29 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -45,7 +45,7 @@ TEST(ExpressionFactor, leaf) {
2, (Matrix(2, 2) << 1, 0, 0, 1), // 2, (Matrix(2, 2) << 1, 0, 0, 1), //
(Vector(2) << -3, -5)); (Vector(2) << -3, -5));
// Create leaves // Create leaf
Point2_ p(2); Point2_ p(2);
// Concise version // Concise version
@ -61,6 +61,8 @@ TEST(ExpressionFactor, leaf) {
// non-zero noise model // non-zero noise model
TEST(ExpressionFactor, model) { TEST(ExpressionFactor, model) {
SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.01));
// Create some values // Create some values
Values values; Values values;
values.insert(2, Point2(3, 5)); values.insert(2, Point2(3, 5));
@ -69,12 +71,36 @@ TEST(ExpressionFactor, model) {
2, (Matrix(2, 2) << 10, 0, 0, 100), // 2, (Matrix(2, 2) << 10, 0, 0, 100), //
(Vector(2) << -30, -500)); (Vector(2) << -30, -500));
// Create leaves // Create leaf
Point2_ p(2); Point2_ p(2);
// Concise version // Concise version
SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.01)); ExpressionFactor<Point2> f(model, Point2(0, 0), p);
EXPECT_LONGS_EQUAL(2, f.dim());
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf);
EXPECT( assert_equal(expected, *jf, 1e-9));
}
/* ************************************************************************* */
// Constrained noise model
TEST(ExpressionFactor, constrained) {
SharedDiagonal model = noiseModel::Constrained::MixedSigmas(Vector2(0.2,0));
// Create some values
Values values;
values.insert(2, Point2(3, 5));
vector<pair<Key, Matrix> > 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);
// Concise version
ExpressionFactor<Point2> f(model, Point2(0, 0), p); ExpressionFactor<Point2> f(model, Point2(0, 0), p);
EXPECT_LONGS_EQUAL(2, f.dim()); EXPECT_LONGS_EQUAL(2, f.dim());
boost::shared_ptr<GaussianFactor> gf = f.linearize(values); boost::shared_ptr<GaussianFactor> gf = f.linearize(values);