Convert Euler angles (roll, pitch, yaw) to rotation matrix.
(roll, pitch, yaw)
| 73 | return camera_position, rpy |
| 74 | |
| 75 | def euler_to_rotation_matrix(roll, pitch, yaw): |
| 76 | """ |
| 77 | Convert Euler angles (roll, pitch, yaw) to rotation matrix. |
| 78 | """ |
| 79 | roll = np.radians(roll) |
| 80 | pitch = np.radians(pitch) |
| 81 | yaw = np.radians(yaw) |
| 82 | |
| 83 | R_x = np.array([ |
| 84 | [1, 0, 0], |
| 85 | [0, cos(roll), -sin(roll)], |
| 86 | [0, sin(roll), cos(roll)] |
| 87 | ]) |
| 88 | |
| 89 | R_y = np.array([ |
| 90 | [cos(pitch), 0, sin(pitch)], |
| 91 | [0, 1, 0], |
| 92 | [-sin(pitch), 0, cos(pitch)] |
| 93 | ]) |
| 94 | |
| 95 | R_z = np.array([ |
| 96 | [cos(yaw), -sin(yaw), 0], |
| 97 | [sin(yaw), cos(yaw), 0], |
| 98 | [0, 0, 1] |
| 99 | ]) |
| 100 | |
| 101 | R = np.dot(R_z, np.dot(R_y, R_x)) |
| 102 | return R |
| 103 | |
| 104 | def rotation_matrix_to_quaternion(R): |
| 105 | """ |
no outgoing calls
no test coverage detected