added graph vio velocities to plotting

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)

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)

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'])
+

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,

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(