From 44e369a8688cf57bba8d0c720cad288d1bc24a6e Mon Sep 17 00:00:00 2001
From: jcarpent <jcarpent@laas.fr>
Date: Fri, 18 Nov 2016 08:14:46 +0100
Subject: [PATCH] [Spatial] Add temporary operator to act ont Eigen object
---
src/spatial/se3.hpp | 16 ++++++++++++++++
1 file changed, 16 insertions(+)
diff --git a/src/spatial/se3.hpp b/src/spatial/se3.hpp
index 10cf1a7ec..eee1bf9cd 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)); }
--
GitLab