Convert rotation matrix to Euler angles (roll, pitch, yaw).
(R)
| 43 | return camera_position_world |
| 44 | |
| 45 | def rotation_matrix_to_euler_angles(R): |
| 46 | """ |
| 47 | Convert rotation matrix to Euler angles (roll, pitch, yaw). |
| 48 | """ |
| 49 | pitch = asin(-R[2, 0]) |
| 50 | if abs(R[2, 0]) != 1: |
| 51 | roll = atan2(R[2, 1], R[2, 2]) |
| 52 | yaw = atan2(R[1, 0], R[0, 0]) |
| 53 | else: |
| 54 | roll = atan2(-R[1, 2], R[1, 1]) |
| 55 | yaw = 0 |
| 56 | |
| 57 | roll = degrees(roll) |
| 58 | pitch = degrees(pitch) |
| 59 | yaw = degrees(yaw) |
| 60 | |
| 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) |
no outgoing calls
no test coverage detected