Skip to content
Snippets Groups Projects
Verified Commit 43c756bf authored by Justin Carpentier's avatar Justin Carpentier
Browse files

geometry: fix isApprox

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