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)