Commit 43c756bf by Justin Carpentier

### geometry: fix isApprox

parent de6ee5bc
 ... ... @@ -27,8 +27,16 @@ namespace eigenpy { AngleAxisVisitor::expose(); } static inline bool isApprox(const AngleAxis & self, const AngleAxis & other, const Scalar & prec = Eigen::NumTraits::dummy_precision()) { return self.isApprox(other,prec); } }; BOOST_PYTHON_FUNCTION_OVERLOADS(isApproxAngleAxis_overload,call::isApprox,2,3) template class AngleAxisVisitor : public bp::def_visitor< AngleAxisVisitor > ... ... @@ -71,11 +79,11 @@ namespace eigenpy bp::arg("Sets *this from a 3x3 rotation matrix."),bp::return_self<>()) .def("toRotationMatrix",&AngleAxis::toRotationMatrix,"Constructs and returns an equivalent 3x3 rotation matrix.") .def("matrix",&AngleAxis::matrix,"Returns an equivalent rotation matrix.") .def("isApprox",(bool (AngleAxis::*)(const AngleAxis &))&AngleAxis::isApprox, "Returns true if *this is approximately equal to other.") .def("isApprox",(bool (AngleAxis::*)(const AngleAxis &, const Scalar prec))&AngleAxis::isApprox, bp::args("other","prec"), "Returns true if *this is approximately equal to other, within the precision determined by prec.") .def("isApprox", &call::isApprox, isApproxAngleAxis_overload(bp::args("other","prec"), "Returns true if *this is approximately equal to other, within the precision determined by prec.")) /* --- Operators --- */ .def(bp::self * bp::other()) ... ...
 ... ... @@ -39,8 +39,16 @@ namespace eigenpy { QuaternionVisitor::expose(); } static inline bool isApprox(const Quaternion & self, const Quaternion & other, const Scalar & prec = Eigen::NumTraits::dummy_precision()) { return self.isApprox(other,prec); } }; BOOST_PYTHON_FUNCTION_OVERLOADS(isApproxQuaternion_overload,call::isApprox,2,3) template class QuaternionVisitor : public bp::def_visitor< QuaternionVisitor > ... ... @@ -62,13 +70,22 @@ namespace eigenpy { cl .def(bp::init<>("Default constructor")) .def(bp::init((bp::arg("Vec4: a 4D vector representing quaternion coefficients")),"Initialize from a vector 4D.")) .def(bp::init((bp::arg("R: a rotation matrix")),"Initialize from rotation matrix.")) .def(bp::init((bp::arg("aa: angle axis")),"Initialize from an angle axis.")) .def(bp::init((bp::arg("quaternion")),"Copy constructor.")) .def(bp::init((bp::arg("vec4")), "Initialize from a vector 4D.\n" "\tvec4 : a 4D vector representing quaternion coefficients in the order xyzw.")) .def(bp::init((bp::arg("R")), "Initialize from rotation matrix.\n" "\tR : a rotation matrix 3x3.")) .def(bp::init((bp::arg("aa")), "Initialize from an angle axis.\n" "\taa: angle axis object.")) .def(bp::init((bp::arg("quat")), "Copy constructor.\n" "\tquat: a quaternion.")) .def("__init__",bp::make_constructor(&QuaternionVisitor::FromTwoVectors, bp::default_call_policies(), (bp::arg("u: a 3D vector"),bp::arg("v: a 3D vector"))),"Initialize from two vectors u andv") (bp::arg("u: a 3D vector"),bp::arg("v: a 3D vector"))), "Initialize from two vectors u and v") .def(bp::init ((bp::arg("w"),bp::arg("x"),bp::arg("y"),bp::arg("z")), "Initialize from coefficients.\n\n" ... ... @@ -88,14 +105,10 @@ namespace eigenpy &QuaternionVisitor::getCoeff<3>, &QuaternionVisitor::setCoeff<3>,"The w coefficient.") // .def("isApprox",(bool (Quaternion::*)(const Quaternion &))&Quaternion::template isApprox, // "Returns true if *this is approximately equal to other.") // .def("isApprox",(bool (Quaternion::*)(const Quaternion &, const Scalar prec))&Quaternion::template isApprox, // "Returns true if *this is approximately equal to other, within the precision determined by prec..") .def("isApprox",(bool (*)(const Quaternion &))&isApprox, "Returns true if *this is approximately equal to other.") .def("isApprox",(bool (*)(const Quaternion &, const Scalar prec))&isApprox, "Returns true if *this is approximately equal to other, within the precision determined by prec..") .def("isApprox", &call::isApprox, isApproxQuaternion_overload(bp::args("other","prec"), "Returns true if *this is approximately equal to other, within the precision determined by prec.")) /* --- Methods --- */ .def("coeffs",(const Vector4 & (Quaternion::*)()const)&Quaternion::coeffs, ... ... @@ -103,7 +116,7 @@ namespace eigenpy .def("matrix",&Quaternion::matrix,"Returns an equivalent 3x3 rotation matrix. Similar to toRotationMatrix.") .def("toRotationMatrix",&Quaternion::toRotationMatrix,"Returns an equivalent 3x3 rotation matrix.") .def("setFromTwoVectors",&setFromTwoVectors,((bp::arg("a"),bp::arg("b"))),"Set *this to be the quaternion which transform a into b through a rotation." .def("setFromTwoVectors",&setFromTwoVectors,((bp::arg("a"),bp::arg("b"))),"Set *this to be the quaternion which transforms a into b through a rotation." ,bp::return_self<>()) .def("conjugate",&Quaternion::conjugate,"Returns the conjugated quaternion. The conjugate of a quaternion represents the opposite rotation.") .def("inverse",&Quaternion::inverse,"Returns the quaternion describing the inverse rotation.") ... ... @@ -142,13 +155,11 @@ namespace eigenpy // "Returns the quaternion which transform a into b through a rotation.") .def("FromTwoVectors",&FromTwoVectors, bp::args("a","b"), "Returns the quaternion which transform a into b through a rotation.", "Returns the quaternion which transforms a into b through a rotation.", bp::return_value_policy()) .staticmethod("FromTwoVectors") .def("Identity",&Quaternion::Identity,"Returns a quaternion representing an identity rotation.") .staticmethod("Identity") ; } private: ... ... @@ -171,12 +182,6 @@ namespace eigenpy Quaternion* q(new Quaternion); q->setFromTwoVectors(u,v); return q; } static bool isApprox(const Quaternion & self, const Quaternion & other, const Scalar prec = Eigen::NumTraits::dummy_precision) { return self.isApprox(other,prec); } static bool __eq__(const Quaternion & u, const Quaternion & v) { ... ...
 from __future__ import print_function from eigenpy import * from geometry import * import numpy as np from numpy import cos,sin ... ... @@ -22,6 +23,8 @@ r = AngleAxis(q) q2 = Quaternion(r) assert(q==q) assert(isapprox(q.coeffs(),q2.coeffs())) assert(q2.isApprox(q2)) assert(q2.isApprox(q2,1e-2)) Rq = q.matrix() Rr = r.matrix() ... ... @@ -45,6 +48,8 @@ if verbose: print("Rx(.1) = \n\n",r.matrix(),"\n") assert( isapprox(r.matrix()[2,2],cos(r.angle))) assert( isapprox(r.axis,np.matrix("1;0;0")) ) assert( isapprox(r.angle,0.1) ) assert(r.isApprox(r)) assert(r.isApprox(r,1e-2)) r.axis = np.matrix([0,1,0],np.double).T assert( isapprox(r.matrix()[0,0],cos(r.angle))) ... ...
Markdown is supported
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