(
pred_traj, gt_traj=None, title="", filename="", align=True, correct_scale=True
)
| 331 | |
| 332 | |
| 333 | def plot_trajectory( |
| 334 | pred_traj, gt_traj=None, title="", filename="", align=True, correct_scale=True |
| 335 | ): |
| 336 | pred_traj = make_traj(pred_traj) |
| 337 | |
| 338 | if gt_traj is not None: |
| 339 | gt_traj = make_traj(gt_traj) |
| 340 | if pred_traj.timestamps.shape[0] == gt_traj.timestamps.shape[0]: |
| 341 | pred_traj.timestamps = gt_traj.timestamps |
| 342 | else: |
| 343 | print("WARNING", pred_traj.timestamps.shape[0], gt_traj.timestamps.shape[0]) |
| 344 | |
| 345 | gt_traj, pred_traj = sync.associate_trajectories(gt_traj, pred_traj) |
| 346 | |
| 347 | if align: |
| 348 | pred_traj.align(gt_traj, correct_scale=correct_scale) |
| 349 | |
| 350 | plot_collection = plot.PlotCollection("PlotCol") |
| 351 | fig = plt.figure(figsize=(8, 8)) |
| 352 | plot_mode = best_plotmode(gt_traj if (gt_traj is not None) else pred_traj) |
| 353 | ax = plot.prepare_axis(fig, plot_mode) |
| 354 | ax.set_title(title) |
| 355 | if gt_traj is not None: |
| 356 | plot.traj(ax, plot_mode, gt_traj, "--", "gray", "Ground Truth") |
| 357 | plot.traj(ax, plot_mode, pred_traj, "-", "blue", "Predicted") |
| 358 | plot_collection.add_figure("traj_error", fig) |
| 359 | plot_collection.export(filename, confirm_overwrite=False) |
| 360 | plt.close(fig=fig) |
| 361 | print(f"Saved trajectory to {filename.replace('.png','')}_traj_error.png") |
| 362 | |
| 363 | |
| 364 | def save_trajectory_tum_format(traj, filename): |
no test coverage detected