diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index 40c97cfca..7fb764129 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -324,13 +324,38 @@ struct SnavelyReprojectionError { /* ************************************************************************* */ // manifold_traits prototype +// Same style as Boost.TypeTraits +// All meta-functions below ever only declare a single type +// or a type/value/value_type #include +#include + +// is manifold +template +struct is_manifold: public false_type { +}; + +// dimension +template +struct dimension: public integral_constant { +}; + +// Fixed size Eigen::Matrix type +template +struct is_manifold > : public true_type { +}; + +template +struct dimension > : public integral_constant< + size_t, M * N> { + BOOST_STATIC_ASSERT(M!=Eigen::Dynamic && N!=Eigen::Dynamic); +}; template struct manifold_traits { typedef T type; - static const size_t dimension = T::dimension; - typedef Eigen::Matrix tangent; + static const size_t dim = dimension::value; + typedef Eigen::Matrix tangent; static tangent localCoordinates(const T& t1, const T& t2) { return t1.localCoordinates(t2); } @@ -344,8 +369,8 @@ template struct manifold_traits > { BOOST_STATIC_ASSERT(M!=Eigen::Dynamic && N!=Eigen::Dynamic); typedef Eigen::Matrix type; - static const size_t dimension = M * N; - typedef Eigen::Matrix tangent; + static const size_t dim = dimension::value; + typedef Eigen::Matrix tangent; static tangent localCoordinates(const type& t1, const type& t2) { type diff = t2 - t1; return tangent(Eigen::Map(diff.data())); @@ -358,8 +383,8 @@ struct manifold_traits > { // Test dimension traits TEST(Expression, Traits) { - EXPECT_LONGS_EQUAL(2, manifold_traits::dimension); - EXPECT_LONGS_EQUAL(8, manifold_traits::dimension); + EXPECT_LONGS_EQUAL(2, dimension::value); + EXPECT_LONGS_EQUAL(8, dimension::value); } /* ************************************************************************* */ @@ -367,10 +392,12 @@ TEST(Expression, Traits) { template Matrix numericalDerivative(boost::function h, const X& x, double delta = 1e-5) { + BOOST_STATIC_ASSERT(is_manifold::value); + BOOST_STATIC_ASSERT(is_manifold::value); Y hx = h(x); double factor = 1.0 / (2.0 * delta); - static const size_t M = manifold_traits::dimension; - static const size_t N = manifold_traits::dimension; + static const size_t M = dimension::value; + static const size_t N = dimension::value; Eigen::Matrix d; Matrix H = zeros(M, N); for (size_t j = 0; j < N; j++) { @@ -441,9 +468,9 @@ TEST(Expression, AutoDiff2) { SnavelyReprojectionError snavely; // Make arguments - Vector9 P; + Vector9 P; // zero rotation, (0,5,0) translation, focal length 1 P << 0, 0, 0, 0, 5, 0, 1, 0, 0; - Vector3 X(10, 0, -5); + Vector3 X(10, 0, -5); // negative Z-axis convention of Snavely! // Apply the mapping, to get image point b_x. Vector expected = Vector2(2, 1);