From 6f97b73f9065772856c621ef194d8c292b27312e Mon Sep 17 00:00:00 2001
From: Valenza Florian <fvalenza@laas.fr>
Date: Tue, 2 Aug 2016 17:10:30 +0200
Subject: [PATCH] [C++] Added isApprox in Motion
---
src/spatial/motion.hpp | 8 ++++++++
1 file changed, 8 insertions(+)
diff --git a/src/spatial/motion.hpp b/src/spatial/motion.hpp
index a15b3df42..e18e34252 100644
--- a/src/spatial/motion.hpp
+++ b/src/spatial/motion.hpp
@@ -62,6 +62,9 @@ namespace se3
Derived_t operator-(const Derived_t & v2) const { return derived().__minus__(v2); }
Derived_t & operator+=(const Derived_t & v2) { return derived().__pequ__(v2); }
+ bool isApprox (const Derived_t & other, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
+ { return derived().isApprox_impl(other, prec);}
+
Derived_t se3Action(const SE3 & m) const { return derived().se3Action_impl(m); }
Derived_t se3ActionInverse(const SE3 & m) const { return derived().se3ActionInverse_impl(m); }
@@ -203,6 +206,11 @@ namespace se3
angular_impl().cross(phi.angular_impl())+linear_impl().cross(phi.linear_impl()) );
}
+ bool isApprox_impl (const MotionTpl & m2, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
+ {
+ return data.isApprox(m2.data, prec);
+ }
+
MotionTpl se3Action_impl(const SE3 & m) const
{
Vector3 Rw (static_cast<Vector3>(m.rotation() * angular_impl()));
--
GitLab