diff --git a/include/core/math/SE2.h b/include/core/math/SE2.h index a051a63e3989f4f0363fc96a8ed462fd1b407cea..ffd8ac8725e49ea921b32140042eb0936cf5d467 100644 --- a/include/core/math/SE2.h +++ b/include/core/math/SE2.h @@ -36,6 +36,12 @@ inline Eigen::Matrix<T, 2, 2> exp(const T theta) namespace SE2{ +template<class T> +inline Eigen::Matrix<T, 2, 2> skew(const T& t) +{ + return (Eigen::Matrix<T, 2, 2>() << (T) 0, -t, t, (T) 0).finished(); +} + /** \brief Compute [1]_x * t = (-t.y ; t.x) */ template<class D>