def plot(args, result, traj_ref, traj_est): from evo.tools import plot from evo.tools.settings import SETTINGS import matplotlib.pyplot as plt import numpy as np logger.debug(SEP) logger.debug("Plotting results... ") plot_mode = plot.PlotMode(args.plot_mode) # Plot the raw metric values. fig1 = plt.figure(figsize=SETTINGS.plot_figsize) if "seconds_from_start" in result.np_arrays: seconds_from_start = result.np_arrays["seconds_from_start"] else: seconds_from_start = None plot.error_array( fig1, result.np_arrays["error_array"], x_array=seconds_from_start, statistics={ s: result.stats[s] for s in SETTINGS.plot_statistics if s not in ("min", "max") }, name=result.info["label"], title=result.info["title"], xlabel="$t$ (s)" if seconds_from_start else "index") # Plot the values color-mapped onto the trajectory. fig2 = plt.figure(figsize=SETTINGS.plot_figsize) ax = plot.prepare_axis(fig2, plot_mode) plot.traj(ax, plot_mode, traj_ref, style=SETTINGS.plot_reference_linestyle, color=SETTINGS.plot_reference_color, label='reference', alpha=SETTINGS.plot_reference_alpha) if args.plot_colormap_min is None: args.plot_colormap_min = result.stats["min"] if args.plot_colormap_max is None: args.plot_colormap_max = result.stats["max"] if args.plot_colormap_max_percentile is not None: args.plot_colormap_max = np.percentile( result.np_arrays["error_array"], args.plot_colormap_max_percentile) plot.traj_colormap(ax, traj_est, result.np_arrays["error_array"], plot_mode, min_map=args.plot_colormap_min, max_map=args.plot_colormap_max, title="Error mapped onto trajectory") fig2.axes.append(ax) plot_collection = plot.PlotCollection(result.info["title"]) plot_collection.add_figure("raw", fig1) plot_collection.add_figure("map", fig2) if args.plot: plot_collection.show() if args.save_plot: plot_collection.export(args.save_plot, confirm_overwrite=not args.no_warnings) if args.serialize_plot: logger.debug(SEP) plot_collection.serialize(args.serialize_plot, confirm_overwrite=not args.no_warnings)
def plot_result(args: argparse.Namespace, result: Result, traj_ref: PosePath3D, traj_est: PosePath3D, traj_ref_full: typing.Optional[PosePath3D] = None) -> None: from evo.tools import plot from evo.tools.settings import SETTINGS import matplotlib.pyplot as plt import numpy as np logger.debug(SEP) logger.debug("Plotting results... ") plot_mode = plot.PlotMode(args.plot_mode) # Plot the raw metric values. fig1 = plt.figure(figsize=SETTINGS.plot_figsize) if "seconds_from_start" in result.np_arrays: seconds_from_start = result.np_arrays["seconds_from_start"] else: seconds_from_start = None plot.error_array( fig1.gca(), result.np_arrays["error_array"], x_array=seconds_from_start, statistics={ s: result.stats[s] for s in SETTINGS.plot_statistics if s not in ("min", "max") }, name=result.info["label"], title=result.info["title"], xlabel="$t$ (s)" if seconds_from_start is not None else "index") # Plot the values color-mapped onto the trajectory. fig2 = plt.figure(figsize=SETTINGS.plot_figsize) ax = plot.prepare_axis(fig2, plot_mode) if args.ros_map_yaml: plot.ros_map(ax, args.ros_map_yaml, plot_mode) plot.traj(ax, plot_mode, traj_ref_full if traj_ref_full else traj_ref, style=SETTINGS.plot_reference_linestyle, color=SETTINGS.plot_reference_color, label='reference', alpha=SETTINGS.plot_reference_alpha) plot.draw_coordinate_axes(ax, traj_ref, plot_mode, SETTINGS.plot_axis_marker_scale) if args.plot_colormap_min is None: args.plot_colormap_min = result.stats["min"] if args.plot_colormap_max is None: args.plot_colormap_max = result.stats["max"] if args.plot_colormap_max_percentile is not None: args.plot_colormap_max = np.percentile( result.np_arrays["error_array"], args.plot_colormap_max_percentile) plot.traj_colormap(ax, traj_est, result.np_arrays["error_array"], plot_mode, min_map=args.plot_colormap_min, max_map=args.plot_colormap_max, title="Error mapped onto trajectory") plot.draw_coordinate_axes(ax, traj_est, plot_mode, SETTINGS.plot_axis_marker_scale) if SETTINGS.plot_pose_correspondences: plot.draw_correspondence_edges( ax, traj_est, traj_ref, plot_mode, style=SETTINGS.plot_pose_correspondences_linestyle, color=SETTINGS.plot_reference_color, alpha=SETTINGS.plot_reference_alpha) fig2.axes.append(ax) plot_collection = plot.PlotCollection(result.info["title"]) plot_collection.add_figure("raw", fig1) plot_collection.add_figure("map", fig2) if args.plot: plot_collection.show() if args.save_plot: plot_collection.export(args.save_plot, confirm_overwrite=not args.no_warnings) if args.serialize_plot: logger.debug(SEP) plot_collection.serialize(args.serialize_plot, confirm_overwrite=not args.no_warnings)
def plot_multi(args, result, traj_ref_list, traj_est_list): from evo.tools import plot from evo.tools.settings import SETTINGS import matplotlib.pyplot as plt import numpy as np logger.debug(SEP) logger.debug("Plotting results... ") plot_mode = plot.PlotMode(args.plot_mode) figs = [] # Plot the raw metric values. error_array_comb = np.array([]) for i in range(len(result)): figs.append(plt.figure(figsize=SETTINGS.plot_figsize)) if "seconds_from_start" in result[i].np_arrays: seconds_from_start = result[i].np_arrays["seconds_from_start"] else: seconds_from_start = None plot.error_array( figs[i], result[i].np_arrays["error_array"], x_array=seconds_from_start, statistics={ s: result[i].stats[s] for s in SETTINGS.plot_statistics if s not in ("min", "max") }, name=result[i].info["label"], title=result[i].info["title"], xlabel="$t$ (s)" if seconds_from_start else "index") # Plot the values color-mapped onto the trajectory. figs.append(plt.figure(figsize=SETTINGS.plot_figsize)) ax = plot.prepare_axis(figs[-1], plot_mode) if args.ros_map_yaml: plot.ros_map(ax, args.ros_map_yaml, plot_mode) if args.plot_colormap_min is None: args.plot_colormap_min = min([result[i].stats["min"] for i in range(len(result))]) if args.plot_colormap_max is None: args.plot_colormap_max = max([result[i].stats["max"] for i in range(len(result))]) if args.plot_colormap_max_percentile is not None: args.plot_colormap_max = np.percentile( error_array_comb, args.plot_colormap_max_percentile) traj_est_list_comb = np.array([]) for i in range(len(result)): plot.traj(ax, plot_mode, traj_ref_list[i], style=SETTINGS.plot_reference_linestyle, color=multirobot_reference_color[i], label='reference'+str(i), alpha=0.8) plot.draw_coordinate_axes(ax, traj_ref_list[i], plot_mode, SETTINGS.plot_axis_marker_scale) plot.draw_coordinate_axes(ax, traj_est_list[i], plot_mode, SETTINGS.plot_axis_marker_scale) figs[-1].axes.append(ax) plot.traj_colormap_multi(ax, traj_est_list, [r.np_arrays["error_array"] for r in result], plot_mode, min_map=args.plot_colormap_min, max_map=args.plot_colormap_max, title="Error mapped onto trajectory") plot_collection = plot.PlotCollection("Multi-robot APE analysis") for i in range(len(result)): plot_collection.add_figure("raw" + str(i), figs[i]) plot_collection.add_figure("map", figs[-1]) if args.plot: plot_collection.show() if args.save_plot: plot_collection.export(args.save_plot, confirm_overwrite=not args.no_warnings) if args.serialize_plot: logger.debug(SEP) plot_collection.serialize(args.serialize_plot, confirm_overwrite=not args.no_warnings)