From 05280140668b56f731660cbbb424b12fa665c77d Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Thu, 25 Jan 2024 11:05:04 -0800 Subject: [PATCH] added graph vio velocities to plotting --- .../scripts/multipose_plotter.py | 9 +++--- .../scripts/multivector3d_plotter.py | 4 +-- .../scripts/plot_conversions.py | 20 ++++++++---- .../scripts/plot_utilities.py | 2 ++ .../scripts/vio_results_plotter.py | 32 ++++++------------- 5 files changed, 30 insertions(+), 37 deletions(-) diff --git a/tools/localization_analysis/scripts/multipose_plotter.py b/tools/localization_analysis/scripts/multipose_plotter.py index 9e9c5b454c..5763b8a1c1 100644 --- a/tools/localization_analysis/scripts/multipose_plotter.py +++ b/tools/localization_analysis/scripts/multipose_plotter.py @@ -37,11 +37,10 @@ def radians_to_degrees(angles): # Pass poses to plot using the add_poses function. # Optionally also plot individual plots for each position or orientation axis (x/y/z or r/p/y) if individual plots is true. class MultiPosePlotter: - def __init__(self, xlabel, ylabel, title, individual_plots = False): + def __init__(self, xlabel, ylabel, title, individual_plots = True): self.xlabel = xlabel self.ylabel = ylabel self.title = title - self.individual_plots = individual_plots self.orientations_plotter = MultiVector3dPlotter(xlabel, ylabel, title, individual_plots) self.positions_plotter = MultiVector3dPlotter(xlabel, ylabel, title, individual_plots) @@ -135,6 +134,6 @@ def add_pose_orientations( # Plot each of the added pose values. Plots positions and orientations in different plots. Optionally plot individual axes on seperate plots # if individual_plots set to True. - def plot(self, pdf, individual_plots=True): - self.positions_plotter.plot(pdf, individual_plots) - self.orientations_plotter.plot(pdf, individual_plots) + def plot(self, pdf): + self.positions_plotter.plot(pdf) + self.orientations_plotter.plot(pdf) diff --git a/tools/localization_analysis/scripts/multivector3d_plotter.py b/tools/localization_analysis/scripts/multivector3d_plotter.py index d1301b23a8..065011c25a 100644 --- a/tools/localization_analysis/scripts/multivector3d_plotter.py +++ b/tools/localization_analysis/scripts/multivector3d_plotter.py @@ -45,7 +45,7 @@ def add(self, vector3d_plotter): # Plot each of the vector3d values. Optionally plot individual axes on seperate plots # if individual_plots set to True. - def plot(self, pdf, individual_plots=True): + def plot(self, pdf): plt.figure() for vector3d_plotter in self.vector3d_plotters: vector3d_plotter.plot_xyz() @@ -56,7 +56,7 @@ def plot(self, pdf, individual_plots=True): pdf.savefig() plt.close() - if individual_plots: + if self.individual_plots: self.plot_xs(pdf) self.plot_ys(pdf) self.plot_zs(pdf) diff --git a/tools/localization_analysis/scripts/plot_conversions.py b/tools/localization_analysis/scripts/plot_conversions.py index c97a3f6cf8..81e3a16e48 100644 --- a/tools/localization_analysis/scripts/plot_conversions.py +++ b/tools/localization_analysis/scripts/plot_conversions.py @@ -18,9 +18,7 @@ # under the License. import numpy as np -import pose_with_covariance -import timestamped_pose_with_covariance -import timestamped_velocity_with_covariance +from vector3d_plotter import Vector3dPlotter import scipy.spatial.transform # Return list of 3 lists, one each for x, y, z values in poses @@ -40,10 +38,11 @@ def ypr_vectors_from_poses(poses): return [ys, ps, rs] # Return list of 3 lists, one each for x, y, z values in velocities -def xyz_vectors_from_velocities(velocities): - xs = [velocity.x for velocity in velocities] - xs = [velocity.y for velocity in velocities] - xs = [velocity.z for velocity in velocities] +def xyz_velocity_vectors_from_graph_vio_states(graph_vio_states): + # TODO: Do this more efficiently + xs = [state.velocity_with_covariance.x for state in graph_vio_states] + ys = [state.velocity_with_covariance.y for state in graph_vio_states] + zs = [state.velocity_with_covariance.z for state in graph_vio_states] return [xs, ys, zs] # Return list of times for given timestamped objects @@ -79,3 +78,10 @@ def optical_flow_feature_counts_from_graph_vio_states(graph_vio_states): # Return list of number of optical flow factors from graph vio states def optical_flow_factor_counts_from_graph_vio_states(graph_vio_states): return [graph_vio_state.num_detected_of_features for graph_vio_state in graph_vio_states] + + +def velocity_plotter_from_graph_vio_states(graph_vio_states): + xs, ys, zs = xyz_velocity_vectors_from_graph_vio_states(graph_vio_states) + times = times_from_timestamped_objects(graph_vio_states) + return Vector3dPlotter("Graph VIO Velocity", times, xs, ys, zs, ['X', 'Y', 'Z']) + diff --git a/tools/localization_analysis/scripts/plot_utilities.py b/tools/localization_analysis/scripts/plot_utilities.py index 2c87426569..1bc666e551 100755 --- a/tools/localization_analysis/scripts/plot_utilities.py +++ b/tools/localization_analysis/scripts/plot_utilities.py @@ -30,6 +30,8 @@ from matplotlib.backends.backend_pdf import PdfPages import vector3d_plotter +# TODO: remove this file.... + # Plot velocities over time def plot_velocities_vs_time( pdf, diff --git a/tools/localization_analysis/scripts/vio_results_plotter.py b/tools/localization_analysis/scripts/vio_results_plotter.py index 346543fb2b..0153c813b1 100755 --- a/tools/localization_analysis/scripts/vio_results_plotter.py +++ b/tools/localization_analysis/scripts/vio_results_plotter.py @@ -27,7 +27,9 @@ import sys import message_reader -import multipose_plotter +from multipose_plotter import MultiPosePlotter +from multivector3d_plotter import MultiVector3dPlotter +import plot_conversions from timestamped_pose import TimestampedPose #import plotting_utilities @@ -46,7 +48,7 @@ def plot_vio_results( groundtruth_poses, graph_vio_states, ): - poses_plotter = multipose_plotter.MultiPosePlotter("Time (s)", "Position (m)", "Graph vs. Groundtruth Position", True) + poses_plotter = MultiPosePlotter("Time (s)", "Position (m)", "Graph vs. Groundtruth Position", True) poses_plotter.add_poses( "Groundtruth Poses", groundtruth_poses, @@ -73,27 +75,11 @@ def plot_vio_results( # ) poses_plotter.plot(pdf) - # orientations -# orientation_plotter = vector3d_plotter.Vector3dPlotter( -# "Time (s)", "Orientation (deg)", "Graph vs. Groundtruth Orientation", True -# ) -# orientation_plotter.add_pose_orientation( -# groundtruth_poses, -# linestyle="None", -# marker="o", -# markeredgewidth=0.1, -# markersize=1.5, -# ) -# if ar_tag_poses.times: -# orientation_plotter.add_pose_orientation( -# ar_tag_poses, -# linestyle="None", -# marker="x", -# markeredgewidth=0.1, -# markersize=1.5, -# ) -# orientation_plotter.add_pose_orientation(graph_localization_states) -# orientation_plotter.plot(pdf) + velocities_plotter = MultiVector3dPlotter("Time (s)", "Velocity (m/s)", "Graph VIO Velocities", True) + graph_vio_velocity_plotter = plot_conversions.velocity_plotter_from_graph_vio_states(graph_vio_states) + velocities_plotter.add(graph_vio_velocity_plotter) + velocities_plotter.plot(pdf) + # # # Imu Augmented Loc vs. Loc # position_plotter = vector3d_plotter.Vector3dPlotter(