Converts world coordinates and quaternion to camera coordinates (position and orientation).
(x, y, z, QW, QX, QY, QZ)
| 156 | return QW, QX, QY, QZ, T[0], T[1], T[2] |
| 157 | |
| 158 | def world2cam_WXYZ(x, y, z, QW, QX, QY, QZ): |
| 159 | """ |
| 160 | Converts world coordinates and quaternion to camera coordinates (position and orientation). |
| 161 | """ |
| 162 | R = quaternion_to_rotation_matrix(QW, QX, QY, QZ) |
| 163 | position_world = np.array([x, y, z]) |
| 164 | camera_position = calculate_camera_position_from_world(R, position_world) |
| 165 | formatted_camera_position = [f"{t:.8f}" for t in camera_position] |
| 166 | print(f"Camera position in camera coordinates (TX, TY, TZ): {', '.join(formatted_camera_position)}") |
| 167 | |
| 168 | return camera_position |
no test coverage detected