Newer
Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
from geometry import *
import numpy as np
from numpy import cos,sin
verbose = True
def isapprox(a,b,epsilon=1e-6):
if a.__class__ == b.__class__ == np.ndarray:
return np.allclose(a,b,epsilon)
else:
return abs(a-b)<epsilon
# --- Quaternion ---------------------------------------------------------------
q = Quaternion(1,2,3,4)
q.normalize()
assert(isapprox(np.linalg.norm(q.coeffs()),q.norm()))
assert(isapprox(np.linalg.norm(q.coeffs()),1))
r = AngleAxis(q)
q2 = Quaternion(r)
assert(q==q2)
assert(isapprox(q.coeffs(),q2.coeffs()))
Rq = q.matrix()
Rr = r.matrix()
assert(isapprox(np.dot(Rq,Rq.T),np.eye(3)))
assert(isapprox(Rr,Rq))
qR = Quaternion(Rr)
assert(q==qR)
assert(isapprox(q.coeffs(),qR.coeffs()))
assert(isapprox(qR[3],1./np.sqrt(30)))
try:
qR[5]
print "Error, this message should not appear."
except Exception,e:
if verbose: print "As expected, catched exception: ",e.message
# --- Angle Vector ------------------------------------------------
r = AngleAxis(.1,np.array([1,0,0],np.double))
if verbose: print "Rx(.1) = \n\n",r.matrix(),"\n"
assert( isapprox(r.matrix()[2,2],cos(r.angle)))
assert( isapprox(r.axis,np.array([[1,0,0],],np.double).T) )
assert( isapprox(r.angle,0.1) )
r.axis = np.array([0,1,0],np.double)
assert( isapprox(r.matrix()[0,0],cos(r.angle)))
ri = r.inverse()
assert( isapprox(ri.angle,-.1) )
R = r.matrix()
r2 = AngleAxis(np.dot(R,R))
assert( isapprox(r2.angle,r.angle*2) )
# --- USER FUNCTIONS -----------------------------------------------------------
ro = testOutAngleAxis()
assert(ro.__class__ == AngleAxis)
res = testInAngleAxis(r)
assert( res==r.angle )
qo = testOutQuaternion()
assert(qo.__class__ == Quaternion)
res = testInQuaternion_fx(q)
assert(q.norm() == res)
try:
testInQuaternion(q)
print "Error, this message should not appear."
except:
if verbose: print "As expected, catch a Boost::python::ArgError exception."