← Back to branch summary

~mrol-dev/mrol/trunk

« back to all changes in this revision

Viewing changes to mrol_mapping/poseutil.py

  • Committer: Vikas Dhiman
  • Date: 2012-05-14 16:28:10 UTC
  • Revision ID: wecacuee@gmail.com-20120514162810-gqqevqvisr4qyi5r
Backtracing must be working fine now.

Show diffs side-by-side

added added

removed removed

Lines of Context:
mrol_mapping/poseutil.py
63
63
        pose_return = Pose3D()
64
64
        return transformed, pose_return
65
65
 
66
 
    assert xyzs.shape[1] == 3, '3 dimensional points only'
67
 
 
68
66
    # TODO is the return pose_return really necessary?
69
67
    poseobj = _to_poseobj(pose)
70
68
 
265
263
        trans = np.vstack([trans, [0, 0, 0, 1]])
266
264
        return trans.astype(np.single)
267
265
 
268
 
    def getQuaternion(self):
269
 
        return quaternion_from_matrix(self.getMatrix())
270
 
 
271
266
    def setMatrix(self, mat):
272
267
        """ sets the pose according to the 4x4 transform matrix """
273
268
        self.rotx = np.arctan2(mat[2, 1], mat[2, 2]);
355
350
        vert_rot = 0
356
351
    poses = np.hstack([poses_lin,poses_ang])
357
352
    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