| 227 | |
| 228 | |
| 229 | def eval_metrics(pred_traj, gt_traj=None, seq="", filename="", sample_stride=1, verbose=False): |
| 230 | |
| 231 | if sample_stride > 1: |
| 232 | pred_traj[0] = pred_traj[0][::sample_stride] |
| 233 | pred_traj[1] = pred_traj[1][::sample_stride] |
| 234 | if gt_traj is not None: |
| 235 | updated_gt_traj = [] |
| 236 | updated_gt_traj.append(gt_traj[0][::sample_stride]) |
| 237 | updated_gt_traj.append(gt_traj[1][::sample_stride]) |
| 238 | gt_traj = updated_gt_traj |
| 239 | |
| 240 | pred_traj = make_traj(pred_traj) |
| 241 | |
| 242 | if gt_traj is not None: |
| 243 | gt_traj = make_traj(gt_traj) |
| 244 | |
| 245 | if pred_traj.timestamps.shape[0] == gt_traj.timestamps.shape[0]: |
| 246 | pred_traj.timestamps = gt_traj.timestamps |
| 247 | else: |
| 248 | print(pred_traj.timestamps.shape[0], gt_traj.timestamps.shape[0]) |
| 249 | |
| 250 | gt_traj, pred_traj = sync.associate_trajectories(gt_traj, pred_traj) |
| 251 | |
| 252 | # ATE |
| 253 | traj_ref = gt_traj |
| 254 | traj_est = pred_traj |
| 255 | |
| 256 | ate_result = main_ape.ape( |
| 257 | traj_ref, |
| 258 | traj_est, |
| 259 | est_name="traj", |
| 260 | pose_relation=PoseRelation.translation_part, |
| 261 | align=True, |
| 262 | correct_scale=True, |
| 263 | ) |
| 264 | |
| 265 | ate = ate_result.stats["rmse"] |
| 266 | # print(ate_result.np_arrays['error_array']) |
| 267 | # exit() |
| 268 | |
| 269 | # RPE rotation and translation |
| 270 | delta_list = [1] |
| 271 | rpe_rots, rpe_transs = [], [] |
| 272 | for delta in delta_list: |
| 273 | rpe_rots_result = main_rpe.rpe( |
| 274 | traj_ref, |
| 275 | traj_est, |
| 276 | est_name="traj", |
| 277 | pose_relation=PoseRelation.rotation_angle_deg, |
| 278 | align=True, |
| 279 | correct_scale=True, |
| 280 | delta=delta, |
| 281 | delta_unit=Unit.frames, |
| 282 | rel_delta_tol=0.01, |
| 283 | all_pairs=True, |
| 284 | ) |
| 285 | |
| 286 | rot = rpe_rots_result.stats["rmse"] |