Input : gt_traj: list of 4x4 matrices est_traj: list of 4x4 matrices len(gt_traj) == len(est_traj)
(gt_traj, est_traj)
| 58 | |
| 59 | |
| 60 | def evaluate_ate(gt_traj, est_traj): |
| 61 | """ |
| 62 | Input : |
| 63 | gt_traj: list of 4x4 matrices |
| 64 | est_traj: list of 4x4 matrices |
| 65 | len(gt_traj) == len(est_traj) |
| 66 | """ |
| 67 | gt_traj_pts = [gt_traj[idx][:3,3] for idx in range(len(gt_traj))] |
| 68 | est_traj_pts = [est_traj[idx][:3,3] for idx in range(len(est_traj))] |
| 69 | |
| 70 | gt_traj_pts = torch.stack(gt_traj_pts).detach().cpu().numpy().T |
| 71 | est_traj_pts = torch.stack(est_traj_pts).detach().cpu().numpy().T |
| 72 | |
| 73 | _, _, trans_error = align(gt_traj_pts, est_traj_pts) |
| 74 | |
| 75 | avg_trans_error = trans_error.mean() |
| 76 | |
| 77 | return avg_trans_error |
| 78 | |
| 79 | def report_progress(params, data, i, progress_bar, iter_time_idx, sil_thres, every_i=1, qual_every_i=1, |
| 80 | tracking=False, mapping=False, online_time_idx=None): |
no test coverage detected