Commit a8f17bf5 authored by jcarpent's avatar jcarpent
Browse files

[C++] Rewrite isApprox using correct Eigen isApprox

parent c2bc2f56
// //
// Copyright (c) 2015 CNRS // Copyright (c) 2015-2016 CNRS
// //
// This file is part of Pinocchio // This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it // Pinocchio is free software: you can redistribute it
...@@ -109,9 +109,9 @@ namespace se3 ...@@ -109,9 +109,9 @@ namespace se3
return derived().__equal__(other); return derived().__equal__(other);
} }
bool isApprox (const Derived_t & other) const bool isApprox (const Derived_t & other, const Scalar_t & prec = Eigen::NumTraits<Scalar_t>::dummy_precision()) const
{ {
return derived().isApprox_impl(other); return derived().isApprox_impl(other, prec);
} }
friend std::ostream & operator << (std::ostream & os,const SE3Base<Derived> & X) friend std::ostream & operator << (std::ostream & os,const SE3Base<Derived> & X)
...@@ -279,12 +279,9 @@ namespace se3 ...@@ -279,12 +279,9 @@ namespace se3
return (rotation_impl() == m2.rotation() && translation_impl() == m2.translation()); return (rotation_impl() == m2.rotation() && translation_impl() == m2.translation());
} }
bool isApprox_impl( const SE3Tpl & m2 ) const bool isApprox_impl (const SE3Tpl & m2, const Scalar_t & prec = Eigen::NumTraits<Scalar_t>::dummy_precision()) const
{ {
Matrix4 diff( toHomogeneousMatrix_impl() - return rot.isApprox(m2.rot, prec) && trans.isApprox(m2.trans, prec);
m2.toHomogeneousMatrix_impl());
return (diff.isMuchSmallerThan(toHomogeneousMatrix_impl(), 1e-14)
&& diff.isMuchSmallerThan(m2.toHomogeneousMatrix_impl(), 1e-14) );
} }
public: public:
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment