4
# TODO replace a transformations library?
5
# transformations.py now in lib directory
6
# This library also has random rotation pose generation
8
# TODO make all interfaces consistent and using Pose3D objects
10
# TODO need to test mat and posetuple functions properly rather than just testing that
11
# they invert one another
14
'''Convert pose tuple in form x, y, z, rotx, roty, rotz to matrix'''
15
#M = transformations.euler_matrix(pose[3], pose[4], pose[5], 'sxyz')
20
def posetuple(posemat):
21
'''Convert pose matrix 4x4 to pose tuple x, y, z, rotx, roty, rotz'''
26
# TODO combine tuplepose and posetuple into one function
28
'''Convert an array like 6 element pose representation to a tuple x, y, z, rotx, roty, rotz'''
29
return (pose[0], pose[1], pose[2], pose[3], pose[4], pose[5])
31
# TODO enable transformPoints function to take either matrix or pose tuple, and modify
32
# where this function is invoked accordingly
33
def transformPoints(xyz, posemat):
34
'''returns the transform of a list of points n by 3 by applying the
35
matrix transform described by mat'''
36
# TODO probably would be faster to transform in place
37
#M = np.vstack((xyz[:, 0:3].T, np.ones((1, xyz.shape[0]))))
38
#return np.dot(mat, M).T[:, 0:3]
40
rotated = np.dot(posemat[:3, :3], xyz.T).T
41
rotated += posemat[:3, 3]
44
# TODO combine transformPointsTuple into transformPoints
45
def transformPointsTuple(xyz, pose_tuple):
46
pose_mat = mat(pose_tuple)
47
return transformPoints(xyz, pose_mat)
49
def refineposes(poses, deltaposition, deltaorientation):
50
'''Produces a new set of poses that are around the supplied poses'''
51
# TODO this needs to be done properly for 3D
52
# TODO does not handle a single pose
54
# each pose should spawn 3 new poses for each dimension
55
# this definitely produces better results in 2D tested by testing on 2D mapping
56
# and looking at the voxel count of the final map
57
#mg = np.mgrid[-1:2, -1:2, -1:2, -1:2, -1:2, -1:2].astype(float)
58
# each pose should spawn 2 new poses for each dimension
60
mg = np.mgrid[-1:1, -1:1, -1:1, -1:1, -1:1, -1:1].astype(float)
66
# TODO remove this nasty hack for 3DOF speed
68
#mg = quantizer.uniquerows(mg, 3)
70
mg[:, 0:3] *= deltaposition
71
mg[:, 3:6] *= deltaorientation
72
#zerorows = np.zeros((6, mg.shape[1]))
73
#mg = np.vstack((mg[0:3, :]*deltaposition, zerorows, mg[3:6, :]*deltaorientation))
75
refinedposes = np.empty((poses.shape[0]*mg.shape[0], 6))
78
ptile = np.tile(p, (mg.shape[0], 1))
80
refinedposes[row:row+mg.shape[0], :] = newposes
83
# TODO remove those outside of centrepose + spreadpose
84
return quantizer.uniquerows(refinedposes, 3)
86
def uniformposes(centrepose, spreadpose, positionres, orientationres):
87
""" Produce uniform distribution of poses with positions separated by
88
positionres and orientations separated by orientationres, centered around
89
center pose and spanning +/- spreadpose """
90
# TODO this needs to be done properly for 3D poses
91
# see papers on generating uniform random 3D rotations
92
cp = np.array(centrepose.get())
93
sp = np.array(spreadpose.get())
96
# this small addition is to fix when the spreadpose is 0 still want to
97
# generate some poses rather than none
98
S = np.hstack((np.tile(positionres, 3), np.tile(orientationres, 3)))
99
pose_count = np.prod((E-B)/S)
100
assert pose_count < 1e5, 'Requested too many poses %d' % int(pose_count)
102
mg = np.mgrid[B[0]:E[0]:S[0], B[1]:E[1]:S[1], B[2]:E[2]:S[2], B[3]:E[3]:S[3], B[4]:E[4]:S[4], B[5]:E[5]:S[5]]
104
return quantizer.uniquerows(mg.T, 3)
107
""" Return an uniformly distributed random rotation matrix """
108
# TODO write a test for this somehow. NFI.
109
# make a random quaternion
110
randelements = np.random.rand(3)
111
lin1 = np.sqrt(1.0 - randelements[0])
112
lin2 = np.sqrt(randelements[0])
113
rot1 = 2*np.pi * randelements[1]
114
rot2 = 2*np.pi * randelements[2]
115
x = np.sin(rot1)*lin1
116
y = np.cos(rot1)*lin1
117
z = np.sin(rot2)*lin2
118
w = np.cos(rot2)*lin2
119
quat = np.array([x,y,z,w])
120
quat *= np.sqrt(2.0/np.dot(quat,quat)) # not sure if we need this line
121
q = np.outer(quat,quat) # assumes quat is properly normalised... hrmm
122
mat = np.array([[1.0-q[1, 1]-q[2, 2], q[0, 1]-q[2, 3], q[0, 2]+q[1, 3]],
123
[ q[0, 1]+q[2, 3], 1.0-q[0, 0]-q[2, 2], q[1, 2]-q[0, 3]],
124
[ q[0, 2]-q[1, 3], q[1, 2]+q[0, 3], 1.0-q[0, 0]-q[1, 1]]])
129
# TODO allow pose_str function to also take in a pose in matrix form
131
'''Convert 6 element pose x, y, z, rotx, roty, rotz to a string with angles
136
def check_and_transform_points(xyzs,pose):
137
'''Check the type of pose passed and perform the transformation'''
138
if type(pose) == type(Pose3D()):
140
xyzs = xyzs # do nothing
142
xyzs = transformPoints(xyzs, pose.getMatrix())
145
if pose == None or (np.array(pose) == np.array((0, 0, 0, 0, 0, 0))).all() or (np.array(pose) == np.array((0.0, 0.0, 0.0, 0.0, 0.0, 0.0))).all():
146
xyzs = xyzs # do nothing
147
pose_return = Pose3D()
149
xyzs = transformPointsTuple(xyzs, pose)
150
pose_return = Pose3D(pose)
151
return xyzs, pose_return
154
'''compute the inverse transform of that given'''
156
invPose = np.matrix(posemat).I
157
return posetuple(invPose)
159
# TODO use an array to store pose rather than self.x, self.y, ...
162
def __init__(self, X=(0, 0, 0, 0, 0, 0)):
163
""" Initialises a pose with no translation and no rotation """
164
#if type(posestr) == str:
165
# fields = posestr.split()
169
# TODO Something like below so we can assign from one Pose3D to another
170
#if type(X) == type(Pose3D()):
174
self.tol = 6 # number of decimals places to round to
177
return self.__str__()
180
return '%.4f %.4f %.4f %.4f %.4f %.4f' % (self.x, self.y, self.z, np.degrees(self.rotx), np.degrees(self.roty), np.degrees(self.rotz))
182
# __eq__ and __hash__ must be consistent
183
# TODO fix to handle equal tolerance maybe with rounding
187
return np.allclose(self.get(), o.get(), atol=0.0001)
190
X = np.around(self.get(), self.tol)
191
return hash(tuple(X))
193
def is_close(self, o, delta_pos, delta_angle_rad):
194
'''Returns true if the pose is within delta_pos position and
195
delta_angle orientation, delta_angle is in radians'''
196
# TODO will be better when the pose components are stored as a numpy
198
if abs(self.x - o.x) > delta_pos: return False
199
if abs(self.y - o.y) > delta_pos: return False
200
if abs(self.z - o.z) > delta_pos: return False
201
if abs(self.rotx - o.rotx) > delta_angle_rad: return False
202
if abs(self.roty - o.roty) > delta_angle_rad: return False
203
if abs(self.rotz - o.rotz) > delta_angle_rad: return False
208
c.setMatrix(self.getMatrix())
212
""" returns a pose which is the inverse of this pose
214
M = np.linalg.inv(self.getMatrix())
219
def set_position(self, position):
225
X = np.ravel(X) # handle lists, tuples and 6x1 and 1x6 arrays
229
self.rotx = float(X[3])
230
self.roty = float(X[4])
231
self.rotz = float(X[5])
234
return (self.x, self.y, self.z, self.rotx, self.roty, self.rotz)
236
# TODO refactor these identical functions together?
238
return (self.x, self.y, self.z, self.rotx, self.roty, self.rotz)
241
""" returns 4x4 matrix representing this pose """
242
sq = np.sin(self.rotx)
243
cq = np.cos(self.rotx)
244
Rx = [[1, 0, 0], [0, cq, -sq], [0, sq, cq]]
245
sq = np.sin(self.roty)
246
cq = np.cos(self.roty)
247
Ry = [[cq, 0, sq], [0, 1, 0], [-sq, 0, cq]]
248
sq = np.sin(self.rotz)
249
cq = np.cos(self.rotz)
250
Rz = [[cq, -sq, 0], [sq, cq, 0], [0, 0, 1]]
251
# this order is important
252
#R = np.dot(Rx, np.dot(Ry, Rz))
253
R = np.dot(Rz, np.dot(Ry, Rx))
254
T = [[self.x], [self.y], [self.z]]
255
trans = np.hstack((R, T))
256
trans = np.vstack([trans, [0, 0, 0, 1]])
257
return trans.astype(np.single)
259
def setMatrix(self, mat):
260
""" sets the pose according to the 4x4 transform matrix """
261
self.rotx = np.arctan2(mat[2, 1], mat[2, 2]);
262
self.roty = np.arcsin(-mat[2, 0]);
263
self.rotz = np.arctan2(mat[1, 0], mat[0, 0]);
268
def getCombined(self, p):
272
C.setMatrix(np.dot(A, B))
275
def transformPoints(self, xyz):
276
""" returns a transform of a list of points n by 3 by applying the
277
matrix transform described by this pose """
278
return transformPoints(xyz, self.getMatrix())
281
return Pose3D(inverse(self.getTuple()))
283
def axis_order(horz_rot, vert_rot, axis):
284
assert axis in [0,1,2], "axis must be 0, 1 or 2 (x, y or z)"
286
return ([horz_rot, vert_rot, 0])
288
return ([horz_rot, 0, vert_rot])
290
return ([0, horz_rot, vert_rot])
292
def scan_pose_list(FOVV, resV, FOVH, resH, axis=0):
293
''' Create a list of poses that cover the angular region defined by
294
the input arguments, about the defined axis'''
295
assert axis in [0,1,2], "axis must be 0, 1 or 2 (x, y or z)"
296
print "scan poses based on:"
297
print FOVV, resV, FOVH, resH
298
assert np.remainder(np.round(FOVV/resV),2) == 0, "FOVV/resV must be an even integer"
299
assert np.remainder(np.round(FOVH/resH),2) == 0, "FOVH/resH must be an even integer"
300
numV = 2*np.ceil(FOVV/resV/2)+1
301
numH = 2*np.ceil(FOVH/resH/2)+1
302
# must be odd so we have a definitive center pixel
303
if np.remainder(numV,2) != 1:
305
if np.remainder(numV,2) != 1:
308
center = np.floor(numposes/2)
309
poses_ang = np.zeros((numposes,3))
310
poses_lin = np.zeros((numposes,3))
313
for i in np.arange(-FOVH/2,0,resH):
315
for j in np.arange(0,FOVV/2+resV,resV):
317
poses_ang[count,:] = axis_order(horz_rot, vert_rot, axis)
320
# top right, center pixel
321
for i in np.arange(0,FOVH/2+resH,resH):
323
for j in np.arange(0,FOVV/2+resV,resV):
325
poses_ang[count,:] = axis_order(horz_rot, vert_rot, axis)
329
for i in np.arange(-FOVH/2,0,resH):
331
for j in np.arange(-FOVV/2,0,resV):
333
poses_ang[count,:] = axis_order(horz_rot, vert_rot, axis)
337
for i in np.arange(0,FOVH/2+resH,resH):
339
for j in np.arange(-FOVV/2,0,resV):
341
poses_ang[count,:] = axis_order(horz_rot, vert_rot, axis)
344
poses = np.hstack([poses_lin,poses_ang])
added
removed
mrol_mapping/poseutil.py
