Spaces:
Running
on
Zero
Running
on
Zero
File size: 1,967 Bytes
35e2073 |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 |
import numpy as np
import utils.utils_poses.ATE.trajectory_utils as tu
import utils.utils_poses.ATE.transformations as tf
def rotation_error(pose_error):
"""Compute rotation error
Args:
pose_error (4x4 array): relative pose error
Returns:
rot_error (float): rotation error
"""
a = pose_error[0, 0]
b = pose_error[1, 1]
c = pose_error[2, 2]
d = 0.5*(a+b+c-1.0)
rot_error = np.arccos(max(min(d, 1.0), -1.0))
return rot_error
def translation_error(pose_error):
"""Compute translation error
Args:
pose_error (4x4 array): relative pose error
Returns:
trans_error (float): translation error
"""
dx = pose_error[0, 3]
dy = pose_error[1, 3]
dz = pose_error[2, 3]
trans_error = np.sqrt(dx**2+dy**2+dz**2)
return trans_error
def compute_rpe(gt, pred):
trans_errors = []
rot_errors = []
for i in range(len(gt)-1):
gt1 = gt[i]
gt2 = gt[i+1]
gt_rel = np.linalg.inv(gt1) @ gt2
pred1 = pred[i]
pred2 = pred[i+1]
pred_rel = np.linalg.inv(pred1) @ pred2
rel_err = np.linalg.inv(gt_rel) @ pred_rel
trans_errors.append(translation_error(rel_err))
rot_errors.append(rotation_error(rel_err))
rpe_trans = np.mean(np.asarray(trans_errors))
rpe_rot = np.mean(np.asarray(rot_errors))
return rpe_trans, rpe_rot
def compute_ATE(gt, pred):
"""Compute RMSE of ATE
Args:
gt: ground-truth poses
pred: predicted poses
"""
errors = []
for i in range(len(pred)):
# cur_gt = np.linalg.inv(gt_0) @ gt[i]
cur_gt = gt[i]
gt_xyz = cur_gt[:3, 3]
# cur_pred = np.linalg.inv(pred_0) @ pred[i]
cur_pred = pred[i]
pred_xyz = cur_pred[:3, 3]
align_err = gt_xyz - pred_xyz
errors.append(np.sqrt(np.sum(align_err ** 2)))
ate = np.sqrt(np.mean(np.asarray(errors) ** 2))
return ate
|