(R1, R2)
| 7 | import tqdm |
| 8 | |
| 9 | def rotation_angle(R1, R2): |
| 10 | # R1 and R2 are 3x3 rotation matrices |
| 11 | R = R1.T @ R2 |
| 12 | # Numerical stability: clamp values into [-1,1] |
| 13 | val = (np.trace(R) - 1) / 2 |
| 14 | val = np.clip(val, -1.0, 1.0) |
| 15 | angle_rad = np.arccos(val) |
| 16 | angle_deg = np.degrees(angle_rad) # Convert radians to degrees |
| 17 | return angle_deg |
| 18 | def extrinsic_distance(extrinsic1, extrinsic2, lambda_t=1.0): |
| 19 | R1, t1 = extrinsic1[:3, :3], extrinsic1[:3, 3] |
| 20 | R2, t2 = extrinsic2[:3, :3], extrinsic2[:3, 3] |
no outgoing calls
no test coverage detected