Support relative time axis for speeds (#709)

`start_timestamp` support like for the xyz/rpy plot was missing.

Fixes: #707

evo/main_traj.py

@@ -358,7 +358,8 @@ def run(args):
                                 style=SETTINGS.plot_reference_linestyle,
                                 color=SETTINGS.plot_reference_color,
                                 alpha=SETTINGS.plot_reference_alpha,
-                                label=short_traj_name)
+                                label=short_traj_name,
+                                start_timestamp=start_time)
                 except trajectory.TrajectoryException as error:
                     logger.error(
                         f"Can't plot speeds of {short_traj_name}: {error}")
@@ -411,7 +412,8 @@ def run(args):
                                 style=SETTINGS.plot_trajectory_linestyle,
                                 color=color,
                                 alpha=SETTINGS.plot_trajectory_alpha,
-                                label=short_traj_name)
+                                label=short_traj_name,
+                                start_timestamp=start_time)
                 except trajectory.TrajectoryException as error:
                     logger.error(
                         f"Can't plot speeds of {short_traj_name}: {error}")

evo/tools/plot.py

@@ -652,7 +652,8 @@ def traj_rpy(axarr: np.ndarray, traj: trajectory.PosePath3D, style: str = '-',
 
 
 def speeds(ax: Axes, traj: trajectory.PoseTrajectory3D, style: str = '-',
-           color="black", label: str = "", alpha: float = 1.):
+           color="black", label: str = "", alpha: float = 1.,
+           start_timestamp: typing.Optional[float] = None):
     """
     Plots the speed between poses of a trajectory.
     Note that a speed value is shown at the timestamp of the newer pose.
@@ -662,10 +663,16 @@ def speeds(ax: Axes, traj: trajectory.PoseTrajectory3D, style: str = '-',
     :param color: matplotlib color
     :param label: label (for legend)
     :param alpha: alpha value for transparency
+    :param start_timestamp: optional start time of the reference
+                            (for x-axis alignment)
     """
     if not isinstance(traj, trajectory.PoseTrajectory3D):
         raise PlotException("speeds can only be plotted with trajectories")
-    ax.plot(traj.timestamps[1:], traj.speeds, style, color=color, alpha=alpha,
+    if start_timestamp:
+        timestamps = traj.timestamps - start_timestamp
+    else:
+        timestamps = traj.timestamps
+    ax.plot(timestamps[1:], traj.speeds, style, color=color, alpha=alpha,
             label=label)
     ax.set_xlabel("$t$ (s)")
     ax.set_ylabel("$v$ (m/s)")