diff --git a/src/spatial/se3.hpp b/src/spatial/se3.hpp index 10cf1a7ecd40ad75e44b2b6c6cb68de2e8a99b9d..eee1bf9cdb460c0d6aa77e92f94c742fce8bda19 100644 --- a/src/spatial/se3.hpp +++ b/src/spatial/se3.hpp @@ -277,6 +277,22 @@ namespace se3 return d.se3ActionInverse(*this); } + template<typename EigenDerived> + typename EigenDerived::PlainObject actOnEigenObject(const Eigen::MatrixBase<EigenDerived> & p) + { return EigenDerived::PlainObject(rot*p+trans); } + + template<typename MapDerived> + Vector3 actOnEigenObject(const Eigen::MapBase<MapDerived> & p) + { return Vector3(rot*p+trans); } + + template<typename EigenDerived> + typename EigenDerived::PlainObject actInvOnEigenObject(const Eigen::MatrixBase<EigenDerived> & p) + { return EigenDerived::PlainObject(rot.transpose()*(p-trans)); } + + template<typename MapDerived> + Vector3 actInvOnEigenObject(const Eigen::MapBase<MapDerived> & p) + { return Vector3(rot.transpose()*(p-trans)); } + Vector3 act_impl (const Vector3& p) const { return Vector3(rot*p+trans); } Vector3 actInv_impl(const Vector3& p) const { return Vector3(rot.transpose()*(p-trans)); }