Determines the orientation of a 3D pose based on the alignment of its z-vector with predefined orientations. Args: pose (np.ndarray): A 4x4 NumPy array representing a 3D pose transformation matrix. Returns: (int): Index representing the closest predefined orientation:
(pose)
| 19 | |
| 20 | |
| 21 | def decide_pose(pose): |
| 22 | """ |
| 23 | Determines the orientation of a 3D pose based on the alignment of its z-vector with predefined orientations. |
| 24 | |
| 25 | Args: |
| 26 | pose (np.ndarray): A 4x4 NumPy array representing a 3D pose transformation matrix. |
| 27 | |
| 28 | Returns: |
| 29 | (int): Index representing the closest predefined orientation: |
| 30 | 0 for upright, 1 for left, 2 for upside-down, and 3 for right. |
| 31 | """ |
| 32 | |
| 33 | # pose style |
| 34 | z_vec = pose[2, :3] |
| 35 | z_orien = np.array( |
| 36 | [ |
| 37 | [0.0, -1.0, 0.0], # upright |
| 38 | [-1.0, 0.0, 0.0], # left |
| 39 | [0.0, 1.0, 0.0], # upside-down |
| 40 | [1.0, 0.0, 0.0], # right |
| 41 | ] |
| 42 | ) |
| 43 | corr = np.matmul(z_orien, z_vec) |
| 44 | corr_max = np.argmax(corr) |
| 45 | return corr_max |
| 46 | |
| 47 | |
| 48 | def rotate_pose(im, rot_index): |
nothing calls this directly
no outgoing calls
no test coverage detected