1 from __future__
import print_function
5 from numpy
import cos,sin
10 if issubclass(a.__class__,np.ndarray)
and issubclass(b.__class__,np.ndarray):
11 return np.allclose(a,b,epsilon)
13 return abs(a-b)<epsilon
16 q = Quaternion(1,2,3,4)
18 assert(
isapprox(np.linalg.norm(q.coeffs()),q.norm()))
19 assert(
isapprox(np.linalg.norm(q.coeffs()),1))
21 v = np.array([0.5,-0.5,0.5,0.5])
28 assert(
isapprox(q.coeffs(),q2.coeffs()))
29 assert(q2.isApprox(q2))
30 assert(q2.isApprox(q2,1e-2))
34 assert(
isapprox(Rq.dot(Rq.T),np.eye(3)))
38 assert(q.isApprox(qR))
39 assert(
isapprox(q.coeffs(),qR.coeffs()))
41 assert(
isapprox(qR[3],1./np.sqrt(30)))
44 print(
"Error, this message should not appear.")
45 except RuntimeError
as e:
46 if verbose: print(
"As expected, catched exception: ",e)
49 r = AngleAxis(.1,np.array([1,0,0],np.double))
50 if verbose: print(
"Rx(.1) = \n\n",r.matrix(),
"\n")
51 assert(
isapprox(r.matrix()[2,2],cos(r.angle)))
52 assert(
isapprox(r.axis,np.array([1.,0,0])) )
55 assert(r.isApprox(r,1e-2))
57 r.axis = np.array([0,1,0],np.double).T
58 assert(
isapprox(r.matrix()[0,0],cos(r.angle)))
64 r2 = AngleAxis(np.dot(R,R))
65 assert(
isapprox(r2.angle,r.angle*2) )
69 assert(ro.__class__ == AngleAxis)
71 assert( res==r.angle )
74 assert(qo.__class__ == Quaternion)
76 assert(q.norm() == res)
Eigen::AngleAxisd testOutAngleAxis()
def isapprox(a, b, epsilon=1e-6)
Eigen::Quaterniond testOutQuaternion()
double testInAngleAxis(Eigen::AngleAxisd aa)
double testInQuaternion(Eigen::Quaterniond q)