(l)
| 10 | |
| 11 | |
| 12 | def transform44(l): |
| 13 | _EPS = np.finfo(float).eps * 4.0 |
| 14 | """ |
| 15 | Generate a 4x4 homogeneous transformation matrix from a 3D point and unit quaternion. |
| 16 | |
| 17 | Input: |
| 18 | l -- tuple consisting of (stamp,tx,ty,tz,qx,qy,qz,qw) where |
| 19 | (tx,ty,tz) is the 3D position and (qx,qy,qz,qw) is the unit quaternion. |
| 20 | |
| 21 | Output: |
| 22 | matrix -- 4x4 homogeneous transformation matrix |
| 23 | """ |
| 24 | t = l[1:4] |
| 25 | q = np.array(l[4:8], dtype=np.float64, copy=True) |
| 26 | nq = np.dot(q, q) |
| 27 | if nq < _EPS: |
| 28 | return np.array(( |
| 29 | (1.0, 0.0, 0.0, t[0]) |
| 30 | (0.0, 1.0, 0.0, t[1]) |
| 31 | (0.0, 0.0, 1.0, t[2]) |
| 32 | (0.0, 0.0, 0.0, 1.0) |
| 33 | ), dtype=np.float64) |
| 34 | q *= np.sqrt(2.0 / nq) |
| 35 | q = np.outer(q, q) |
| 36 | return np.array(( |
| 37 | (1.0 - q[1, 1] - q[2, 2], q[0, 1] - q[2, 3], q[0, 2] + q[1, 3], t[0]), |
| 38 | (q[0, 1] + q[2, 3], 1.0 - q[0, 0] - q[2, 2], q[1, 2] - q[0, 3], t[1]), |
| 39 | (q[0, 2] - q[1, 3], q[1, 2] + q[0, 3], 1.0 - q[0, 0] - q[1, 1], t[2]), |
| 40 | (0.0, 0.0, 0.0, 1.0)), dtype=np.float64) |
| 41 | |
| 42 | |
| 43 | def convert_rel_to_44matrix(rot_x, rot_y, rot_z, pose): |
no outgoing calls
no test coverage detected