(
pred_traj, gt_traj=None, title="", filename="", align=True, correct_scale=True, verbose=False
)
| 398 | |
| 399 | |
| 400 | def plot_trajectory( |
| 401 | pred_traj, gt_traj=None, title="", filename="", align=True, correct_scale=True, verbose=False |
| 402 | ): |
| 403 | pred_traj = make_traj(pred_traj) |
| 404 | |
| 405 | if gt_traj is not None: |
| 406 | gt_traj = make_traj(gt_traj) |
| 407 | if pred_traj.timestamps.shape[0] == gt_traj.timestamps.shape[0]: |
| 408 | pred_traj.timestamps = gt_traj.timestamps |
| 409 | else: |
| 410 | print("WARNING", pred_traj.timestamps.shape[0], gt_traj.timestamps.shape[0]) |
| 411 | |
| 412 | gt_traj, pred_traj = sync.associate_trajectories(gt_traj, pred_traj) |
| 413 | |
| 414 | if align: |
| 415 | pred_traj.align(gt_traj, correct_scale=correct_scale) |
| 416 | |
| 417 | plot_collection = plot.PlotCollection("PlotCol") |
| 418 | fig = plt.figure(figsize=(8, 8)) |
| 419 | plot_mode = best_plotmode(gt_traj if (gt_traj is not None) else pred_traj) |
| 420 | ax = plot.prepare_axis(fig, plot_mode) |
| 421 | ax.set_title(title) |
| 422 | if gt_traj is not None: |
| 423 | plot.traj(ax, plot_mode, gt_traj, "--", "gray", "Ground Truth") |
| 424 | plot.traj(ax, plot_mode, pred_traj, "-", "blue", "Predicted") |
| 425 | plot_collection.add_figure("traj_error", fig) |
| 426 | plot_collection.export(filename, confirm_overwrite=False) |
| 427 | plt.close(fig=fig) |
| 428 | if verbose: |
| 429 | print(f"Saved trajectory to {filename.replace('.png','')}_traj_error.png") |
| 430 | |
| 431 | |
| 432 | def save_trajectory_tum_format(traj, filename, verbose=False): |
no test coverage detected