(x, y, z, roll, pitch, yaw)
| 145 | return T |
| 146 | |
| 147 | def world2cam(x, y, z, roll, pitch, yaw): |
| 148 | R = euler_to_rotation_matrix(roll, pitch, yaw) |
| 149 | QW, QX, QY, QZ = rotation_matrix_to_quaternion(R) |
| 150 | print(f"Quaternion (QW, QX, QY, QZ): {QW}, {QX}, {QY}, {QZ}") |
| 151 | |
| 152 | T = calculate_camera_position_from_world(R, np.array([x, y, z])) |
| 153 | formatted_T = [f"{t:.8f}" for t in T] |
| 154 | print(f"Camera position in camera coordinates (TX, TY, TZ): {', '.join(formatted_T)}") |
| 155 | |
| 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 | """ |
nothing calls this directly
no test coverage detected