Generate a 4x4 homogeneous transformation matrix from a 3D point and unit quaternion. Input: l -- tuple consisting of (stamp,tx,ty,tz,qx,qy,qz,qw) where (tx,ty,tz) is the 3D position and (qx,qy,qz,qw) is the unit quaternion. Output: matrix -- 4x4 homogeneous transform
(l)
| 41 | |
| 42 | |
| 43 | def transform44(l): |
| 44 | """ |
| 45 | Generate a 4x4 homogeneous transformation matrix from a 3D point and unit quaternion. |
| 46 | |
| 47 | Input: |
| 48 | l -- tuple consisting of (stamp,tx,ty,tz,qx,qy,qz,qw) where |
| 49 | (tx,ty,tz) is the 3D position and (qx,qy,qz,qw) is the unit quaternion. |
| 50 | |
| 51 | Output: |
| 52 | matrix -- 4x4 homogeneous transformation matrix |
| 53 | """ |
| 54 | _EPS = numpy.finfo(float).eps * 4.0 |
| 55 | # t = l[0,1:4] |
| 56 | t = [l[0, 1], l[0, 2], l[0, 3]] |
| 57 | # q = numpy.array(l[0,4:8], dtype=numpy.float64, copy=True) |
| 58 | q = [l[0, 4], l[0, 5], l[0, 6], l[0, 7]] |
| 59 | q = numpy.array(q, dtype=numpy.float64, copy=True) |
| 60 | nq = numpy.dot(q, q) |
| 61 | if nq < _EPS: |
| 62 | return numpy.array(( |
| 63 | (1.0, 0.0, 0.0, t[0]) |
| 64 | (0.0, 1.0, 0.0, t[1]) |
| 65 | (0.0, 0.0, 1.0, t[2]) |
| 66 | (0.0, 0.0, 0.0, 1.0) |
| 67 | ), dtype=numpy.float64) |
| 68 | q *= numpy.sqrt(2.0 / nq) |
| 69 | q = numpy.outer(q, q) |
| 70 | return numpy.array(( |
| 71 | (1.0 - q[1, 1] - q[2, 2], q[0, 1] - q[2, 3], q[0, 2] + q[1, 3], t[0]), |
| 72 | (q[0, 1] + q[2, 3], 1.0 - q[0, 0] - q[2, 2], q[1, 2] - q[0, 3], t[1]), |
| 73 | (q[0, 2] - q[1, 3], q[1, 2] + q[0, 3], 1.0 - q[0, 0] - q[1, 1], t[2]), |
| 74 | (0.0, 0.0, 0.0, 1.0)), dtype=numpy.float64) |
| 75 | |
| 76 | |
| 77 | if __name__ == '__main__': |