(QW, QX, QY, QZ, TX, TY, TZ)
| 61 | return roll, pitch, yaw |
| 62 | |
| 63 | def cam2world(QW, QX, QY, QZ, TX, TY, TZ): |
| 64 | camera_position = calculate_camera_position(QW, QX, QY, QZ, TX, TY, TZ) |
| 65 | formatted_position = [f"{x:.8f}" for x in camera_position] |
| 66 | print(f"Camera position in world coordinates: {', '.join(formatted_position)}") |
| 67 | |
| 68 | R = quaternion_to_rotation_matrix(QW, QX, QY, QZ) |
| 69 | roll, pitch, yaw = rotation_matrix_to_euler_angles(R) |
| 70 | print(f"Camera's orientation (Roll, Pitch, Yaw): {roll}, {pitch}, {yaw}") |
| 71 | rpy = [roll, pitch, yaw] |
| 72 | |
| 73 | return camera_position, rpy |
| 74 | |
| 75 | def euler_to_rotation_matrix(roll, pitch, yaw): |
| 76 | """ |
nothing calls this directly
no test coverage detected