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

Added wide test

release/4.3a0
dellaert 2014-11-29 13:08:10 +01:00
parent 18a8de1f46
commit 7989a8c0dc
1 changed files with 35 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -76,10 +76,13 @@ TEST(ExpressionFactor, Model) {
// Concise version // Concise version
ExpressionFactor<Point2> f(model, Point2(0, 0), p); ExpressionFactor<Point2> f(model, Point2(0, 0), p);
// Check values and derivatives
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));
EXPECT_CORRECT_FACTOR_JACOBIANS(f, values, 1e-5, 1e-5); // another way
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -124,6 +127,38 @@ TEST(ExpressionFactor, Unary) {
EXPECT( assert_equal(expected, *jf, 1e-9)); EXPECT( assert_equal(expected, *jf, 1e-9));
} }
/* ************************************************************************* */
// Unary(Leaf)) and Unary(Unary(Leaf)))
// wide version (not handled in fixed-size pipeline)
typedef Eigen::Matrix<double,9,3> Matrix93;
Vector9 wide(const Point3& p, boost::optional<Matrix93&> H) {
Vector9 v;
v << p.vector(), p.vector(), p.vector();
if (H) *H << eye(3), eye(3), eye(3);
return v;
}
typedef Eigen::Matrix<double,9,9> Matrix9;
Vector9 id9(const Vector9& v, boost::optional<Matrix9&> H) {
if (H) *H = Matrix9::Identity();
return v;
}
TEST(ExpressionFactor, Wide) {
// Create some values
Values values;
values.insert(2, Point3(0, 0, 1));
Point3_ point(2);
Vector9 measured;
Expression<Vector9> expression(wide,point);
SharedNoiseModel model = noiseModel::Unit::Create(9);
ExpressionFactor<Vector9> f1(model, measured, expression);
EXPECT_CORRECT_FACTOR_JACOBIANS(f1, values, 1e-5, 1e-9);
Expression<Vector9> expression2(id9,expression);
ExpressionFactor<Vector9> f2(model, measured, expression2);
EXPECT_CORRECT_FACTOR_JACOBIANS(f2, values, 1e-5, 1e-9);
}
/* ************************************************************************* */ /* ************************************************************************* */
static Point2 myUncal(const Cal3_S2& K, const Point2& p, static Point2 myUncal(const Cal3_S2& K, const Point2& p,
boost::optional<Matrix25&> Dcal, boost::optional<Matrix2&> Dp) { boost::optional<Matrix25&> Dcal, boost::optional<Matrix2&> Dp) {