ATE Metric Calculation Error #140
Description
def align(model, data):
"""Align two trajectories using the method of Horn (closed-form).
Args:
model -- first trajectory (3xn)
data -- second trajectory (3xn)
Returns:
rot -- rotation matrix (3x3)
trans -- translation vector (3x1)
trans_error -- translational error per point (1xn)
"""
np.set_printoptions(precision=3, suppress=True)
model_zerocentered = model - model.mean(1).reshape((3, -1))
data_zerocentered = data - data.mean(1).reshape((3, -1))
W = np.zeros((3, 3))
for column in range(model.shape[1]):
W += np.outer(model_zerocentered[:, column], data_zerocentered[:, column])
U, d, Vh = np.linalg.linalg.svd(W.transpose())
S = np.matrix(np.identity(3))
if np.linalg.det(U) * np.linalg.det(Vh) < 0:
S[2, 2] = -1
rot = U * S * Vh
trans = data.mean(1).reshape((3, -1)) - rot * model.mean(1).reshape((3, -1))
model_aligned = rot * model + trans
alignment_error = model_aligned - data
trans_error = np.sqrt(np.sum(np.multiply(alignment_error, alignment_error), 0)).A[0]
return rot, trans, trans_error
def evaluate_ate(gt_traj, est_traj):
"""
Input :
gt_traj: list of 4x4 matrices
est_traj: list of 4x4 matrices
len(gt_traj) == len(est_traj)
"""
gt_traj_pts = [gt_traj[idx][:3, 3] for idx in range(len(gt_traj))]
est_traj_pts = [est_traj[idx][:3, 3] for idx in range(len(est_traj))]
gt_traj_pts = torch.stack(gt_traj_pts).detach().cpu().numpy().T
est_traj_pts = torch.stack(est_traj_pts).detach().cpu().numpy().T
_, _, trans_error = align(gt_traj_pts, est_traj_pts)
avg_trans_error = trans_error.mean()
return avg_trans_error _, _, trans_error = align(gt_traj_pts, est_traj_pts) should be _, _, trans_error = align(est_traj_pts, gt_traj_pts)?