(
pred_traj, gt_traj=None, title="", filename="", align=True, correct_scale=True
)
| 368 | |
| 369 | |
| 370 | def plot_trajectory( |
| 371 | pred_traj, gt_traj=None, title="", filename="", align=True, correct_scale=True |
| 372 | ): |
| 373 | pred_traj = make_traj(pred_traj) |
| 374 | |
| 375 | if gt_traj is not None: |
| 376 | gt_traj = make_traj(gt_traj) |
| 377 | if pred_traj.timestamps.shape[0] == gt_traj.timestamps.shape[0]: |
| 378 | pred_traj.timestamps = gt_traj.timestamps |
| 379 | else: |
| 380 | print("WARNING", pred_traj.timestamps.shape[0], gt_traj.timestamps.shape[0]) |
| 381 | |
| 382 | gt_traj, pred_traj = sync.associate_trajectories(gt_traj, pred_traj) |
| 383 | |
| 384 | if align: |
| 385 | pred_traj.align(gt_traj, correct_scale=correct_scale) |
| 386 | |
| 387 | plot_collection = plot.PlotCollection("PlotCol") |
| 388 | fig = plt.figure(figsize=(8, 8)) |
| 389 | plot_mode = best_plotmode(gt_traj if (gt_traj is not None) else pred_traj) |
| 390 | ax = plot.prepare_axis(fig, plot_mode) |
| 391 | ax.set_title(title) |
| 392 | if gt_traj is not None: |
| 393 | plot.traj(ax, plot_mode, gt_traj, "--", "gray", "Ground Truth") |
| 394 | plot.traj(ax, plot_mode, pred_traj, "-", "blue", "Predicted") |
| 395 | plot_collection.add_figure("traj_error", fig) |
| 396 | plot_collection.export(filename, confirm_overwrite=False) |
| 397 | plt.close(fig=fig) |
| 398 | print(f"Saved trajectory to {filename.replace('.png','')}_traj_error.png") |
| 399 | |
| 400 | |
| 401 | def save_trajectory_tum_format(traj, filename): |
no test coverage detected