~wecacuee/mrol/mrol-dev

« back to all changes in this revision

Viewing changes to mrol_mapping/poseutil.py

  • Committer: Vikas Dhiman
  • Date: 2012-05-22 15:26:38 UTC
  • Revision ID: wecacuee@gmail.com-20120522152638-j6srlkgrpdq4kdfe
Added support for userdata in terms of colored voxels. Added benchmark test and corresponding data

Show diffs side-by-side

added added

removed removed

Lines of Context:
63
63
        pose_return = Pose3D()
64
64
        return transformed, pose_return
65
65
 
 
66
    assert xyzs.shape[1] == 3, '3 dimensional points only'
 
67
 
66
68
    # TODO is the return pose_return really necessary?
67
69
    poseobj = _to_poseobj(pose)
68
70
 
263
265
        trans = np.vstack([trans, [0, 0, 0, 1]])
264
266
        return trans.astype(np.single)
265
267
 
 
268
    def getQuaternion(self):
 
269
        return quaternion_from_matrix(self.getMatrix())
 
270
 
266
271
    def setMatrix(self, mat):
267
272
        """ sets the pose according to the 4x4 transform matrix """
268
273
        self.rotx = np.arctan2(mat[2, 1], mat[2, 2]);
350
355
        vert_rot = 0
351
356
    poses = np.hstack([poses_lin,poses_ang])
352
357
    return poses
 
358
 
 
359
# Copied from rospy's tf.transformations
 
360
 
 
361
import numpy
 
362
import math
 
363
def quaternion_from_matrix(matrix):
 
364
    """Return quaternion from rotation matrix.
 
365
 
 
366
    >>> R = rotation_matrix(0.123, (1, 2, 3))
 
367
    >>> q = quaternion_from_matrix(R)
 
368
    >>> numpy.allclose(q, [0.0164262, 0.0328524, 0.0492786, 0.9981095])
 
369
    True
 
370
 
 
371
    """
 
372
    q = numpy.empty((4, ), dtype=numpy.float64)
 
373
    M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:4, :4]
 
374
    t = numpy.trace(M)
 
375
    if t > M[3, 3]:
 
376
        q[3] = t
 
377
        q[2] = M[1, 0] - M[0, 1]
 
378
        q[1] = M[0, 2] - M[2, 0]
 
379
        q[0] = M[2, 1] - M[1, 2]
 
380
    else:
 
381
        i, j, k = 0, 1, 2
 
382
        if M[1, 1] > M[0, 0]:
 
383
            i, j, k = 1, 2, 0
 
384
        if M[2, 2] > M[i, i]:
 
385
            i, j, k = 2, 0, 1
 
386
        t = M[i, i] - (M[j, j] + M[k, k]) + M[3, 3]
 
387
        q[i] = t
 
388
        q[j] = M[i, j] + M[j, i]
 
389
        q[k] = M[k, i] + M[i, k]
 
390
        q[3] = M[k, j] - M[j, k]
 
391
    q *= 0.5 / math.sqrt(t * M[3, 3])
 
392
    return q