Compute rotation error Args: pose_error (4x4 array): relative pose error Returns: rot_error (float): rotation error
(pose_error)
| 11 | import utils.utils_poses.ATE.trajectory_utils as tu |
| 12 | import utils.utils_poses.ATE.transformations as tf |
| 13 | def rotation_error(pose_error): |
| 14 | """Compute rotation error |
| 15 | Args: |
| 16 | pose_error (4x4 array): relative pose error |
| 17 | Returns: |
| 18 | rot_error (float): rotation error |
| 19 | """ |
| 20 | a = pose_error[0, 0] |
| 21 | b = pose_error[1, 1] |
| 22 | c = pose_error[2, 2] |
| 23 | d = 0.5*(a+b+c-1.0) |
| 24 | rot_error = np.arccos(max(min(d, 1.0), -1.0)) |
| 25 | return rot_error |
| 26 | |
| 27 | def translation_error(pose_error): |
| 28 | """Compute translation error |