diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index c4420bb7d..63390ec1f 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -95,7 +95,9 @@ struct dimension : public std::integral_constant { template<> struct zero { - static double value() { return 0;} + static double value() { + return 0; + } }; // Fixed size Eigen::Matrix type @@ -118,24 +120,22 @@ struct dimension template struct dimension > : public Dynamic { - BOOST_STATIC_ASSERT(M!=Eigen::Dynamic); }; template struct dimension > : public Dynamic { - BOOST_STATIC_ASSERT(N!=Eigen::Dynamic); }; template struct dimension > : public std::integral_constant< int, M * N> { - BOOST_STATIC_ASSERT(M!=Eigen::Dynamic && N!=Eigen::Dynamic); }; template struct zero > : public std::integral_constant< int, M * N> { - BOOST_STATIC_ASSERT(M!=Eigen::Dynamic && N!=Eigen::Dynamic); + BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic), + "traits::zero is only supported for fixed-size matrices"); static Eigen::Matrix value() { return Eigen::Matrix::Zero(); } @@ -206,6 +206,8 @@ template struct DefaultChart > { typedef Eigen::Matrix T; typedef Eigen::Matrix::value, 1> vector; + BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic), + "DefaultChart has not been implemented yet for dynamically sized matrices"); T t_; DefaultChart(const T& t) : t_(t) { @@ -221,6 +223,23 @@ struct DefaultChart > { } }; +// Dynamically sized Vector +template<> +struct DefaultChart { + typedef Vector T; + typedef T vector; + T t_; + DefaultChart(const T& t) : + t_(t) { + } + vector apply(const T& other) { + return other - t_; + } + T retract(const vector& d) { + return t_ + d; + } +}; + /** * Old Concept check class for Manifold types * Requires a mapping between a linear tangent space and the underlying