diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index ab2aae1c2..e9a1b7163 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -352,8 +352,10 @@ struct is_manifold: public std::false_type { // dimension, can return Eigen::Dynamic (-1) if not known at compile time template -struct dimension: public std::integral_constant { -}; +struct dimension; +//: public std::integral_constant { +// BOOST_STATIC_ASSERT(is_manifold::value); +//}; // Chart is a map from T -> vector, retract is its inverse template @@ -373,6 +375,34 @@ private: T const & t_; }; +// double + +template<> +struct is_manifold : public true_type { +}; + +template<> +struct dimension : public integral_constant { +}; + +template<> +struct DefaultChart { + typedef Eigen::Matrix vector; + DefaultChart(double t) : + t_(t) { + } + vector apply(double other) { + vector d; + d << other - t_; + return d; + } + double retract(const vector& d) { + return t_ + d[0]; + } +private: + double t_; +}; + // Fixed size Eigen::Matrix type template @@ -404,7 +434,6 @@ struct dimension > : public integral_consta BOOST_STATIC_ASSERT(M!=Eigen::Dynamic && N!=Eigen::Dynamic); }; -// Chart is a map from T -> vector, retract is its inverse template struct DefaultChart > { typedef Eigen::Matrix T; @@ -414,10 +443,12 @@ struct DefaultChart > { } vector apply(const T& other) { T diff = other - t_; - return Eigen::Map(diff.data()); + Eigen::Map map(diff.data()); + return vector(map); } T retract(const vector& d) { - return t_ + Eigen::Map(d.data()); + Eigen::Map map(d.data()); + return t_ + map; } private: T const & t_; @@ -438,16 +469,23 @@ TEST(Expression, is_manifold) { EXPECT(!is_manifold::value); EXPECT(is_manifold::value); EXPECT(is_manifold::value); + EXPECT(is_manifold::value); + EXPECT(is_manifold::value); + EXPECT(is_manifold::value); } // dimension TEST(Expression, dimension) { EXPECT_LONGS_EQUAL(2, dimension::value); EXPECT_LONGS_EQUAL(8, dimension::value); + EXPECT_LONGS_EQUAL(1, dimension::value); + EXPECT_LONGS_EQUAL(Eigen::Dynamic, dimension::value); + EXPECT_LONGS_EQUAL(Eigen::Dynamic, dimension::value); } // charts TEST(Expression, Charts) { + DefaultChart chart1(Point2(0, 0)); EXPECT(chart1.apply(Point2(1,0))==Vector2(1,0)); EXPECT(chart1.retract(Vector2(1,0))==Point2(1,0)); @@ -455,6 +493,18 @@ TEST(Expression, Charts) { DefaultChart chart2(Vector2(0, 0)); EXPECT(chart2.apply(Vector2(1,0))==Vector2(1,0)); EXPECT(chart2.retract(Vector2(1,0))==Vector2(1,0)); + + DefaultChart chart3(0); + Eigen::Matrix v1; v1<<1; + EXPECT(chart3.apply(1)==v1); + EXPECT(chart3.retract(v1)==1); + + // Dynamic does not work yet ! +// Vector z = zero(2), v(2); +// v << 1, 0; +// DefaultChart chart4(z); +// EXPECT(chart4.apply(v)==v); +// EXPECT(chart4.retract(v)==v); } /* ************************************************************************* */