Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
def plot(args, result, traj_ref, traj_est, traj_ref_full=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, 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")
print("loading plot modules")
from evo.tools import plot
import matplotlib.pyplot as plt
print("plotting")
plot_collection = plot.PlotCollection("Example")
# metric values
fig_1 = plt.figure(figsize=(8, 8))
plot.error_array(fig_1, ape_metric.error, statistics=ape_statistics,
name="APE", title=str(ape_metric))
plot_collection.add_figure("raw", fig_1)
# trajectory colormapped with error
fig_2 = plt.figure(figsize=(8, 8))
plot_mode = plot.PlotMode.xy
ax = plot.prepare_axis(fig_2, plot_mode)
plot.traj(ax, plot_mode, traj_ref, '--', 'gray', 'reference')
plot.traj_colormap(
ax, traj_est, ape_metric.error, plot_mode, min_map=ape_statistics["min"],
max_map=ape_statistics["max"], title="APE mapped onto trajectory")
plot_collection.add_figure("traj (error)", fig_2)
# trajectory colormapped with speed
fig_3 = plt.figure(figsize=(8, 8))
plot_mode = plot.PlotMode.xy
ax = plot.prepare_axis(fig_3, plot_mode)
speeds = [
trajectory.calc_speed(traj_est.positions_xyz[i],
traj_est.positions_xyz[i + 1],
traj_est.timestamps[i], traj_est.timestamps[i + 1])
for i in range(len(traj_est.positions_xyz) - 1)
def prepare_axis(fig, plot_mode=PlotMode.xy, subplot_arg=111):
"""
prepares an axis according to the plot mode (for trajectory plotting)
:param fig: matplotlib figure object
:param plot_mode: PlotMode
:param subplot_arg: optional if using subplots - the subplot id (e.g. '122')
:return: the matplotlib axis
"""
if plot_mode == PlotMode.xyz:
ax = fig.add_subplot(subplot_arg, projection="3d")
else:
ax = fig.add_subplot(subplot_arg)
ax.axis("equal")
if plot_mode in {PlotMode.xy, PlotMode.xz, PlotMode.xyz}:
xlabel = "$x$ (m)"
elif plot_mode in {PlotMode.yz, PlotMode.yx}:
xlabel = "$y$ (m)"
else:
xlabel = "$z$ (m)"
if plot_mode in {PlotMode.xy, PlotMode.zy, PlotMode.xyz}:
ylabel = "$y$ (m)"
elif plot_mode in {PlotMode.zx, PlotMode.yx}:
ylabel = "$x$ (m)"
else:
ylabel = "$z$ (m)"
ax.set_xlabel(xlabel)
ax.set_ylabel(ylabel)
if plot_mode == PlotMode.xyz:
ax.set_zlabel('$z$ (m)')
if SETTINGS.plot_invert_xaxis:
"""
Inserts an image of an 2D ROS map into the plot axis.
See: http://wiki.ros.org/map_server#Map_format
:param ax: 2D matplotlib axes
:param plot_mode: a 2D PlotMode
:param yaml_path: yaml file that contains the metadata of the map image
:param cmap: color map used to map scalar data to colors
:param mask_unknown_value: uint8 value that represents unknown cells.
If specified, these cells will be masked out.
If set to None or False, nothing will be masked.
"""
import yaml
if isinstance(ax, Axes3D):
raise PlotException("ros_map can't be drawn into a 3D axis")
if plot_mode in {PlotMode.xz, PlotMode.yz, PlotMode.zx, PlotMode.zy}:
# Image lies in xy / yx plane, nothing to see here.
return
x_idx, y_idx, _ = plot_mode_to_idx(plot_mode)
with open(yaml_path) as f:
metadata = yaml.safe_load(f)
# Load map image, mask unknown cells if desired.
image_path = metadata["image"]
if not os.path.isabs(image_path):
image_path = os.path.join(os.path.dirname(yaml_path), image_path)
image = plt.imread(image_path)
if mask_unknown_value:
mask_unknown_value = np.uint8(mask_unknown_value)
image = np.ma.masked_where(image == mask_unknown_value, image)
print_compact_name)
if args.plot or args.save_plot or args.serialize_plot:
import numpy as np
from evo.tools import plot
import matplotlib.pyplot as plt
import matplotlib.cm as cm
plot_collection = plot.PlotCollection("evo_traj - trajectory plot")
fig_xyz, axarr_xyz = plt.subplots(3, sharex="col", figsize=tuple(
SETTINGS.plot_figsize))
fig_rpy, axarr_rpy = plt.subplots(3, sharex="col", figsize=tuple(
SETTINGS.plot_figsize))
fig_traj = plt.figure(figsize=tuple(SETTINGS.plot_figsize))
plot_mode = plot.PlotMode[args.plot_mode]
ax_traj = plot.prepare_axis(fig_traj, plot_mode)
# for x-axis alignment starting from 0 with --plot_relative_time
start_time = None
if args.ref:
if isinstance(ref_traj, trajectory.PoseTrajectory3D) \
and args.plot_relative_time:
start_time = ref_traj.timestamps[0]
short_traj_name = os.path.splitext(os.path.basename(args.ref))[0]
if SETTINGS.plot_usetex:
short_traj_name = short_traj_name.replace("_", "\\_")
plot.traj(ax_traj, plot_mode, ref_traj,
style=SETTINGS.plot_reference_linestyle,
color=SETTINGS.plot_reference_color,
elif plot_mode == PlotMode.xz:
x_idx = 0
y_idx = 2
elif plot_mode == PlotMode.yx:
x_idx = 1
y_idx = 0
elif plot_mode == PlotMode.yz:
x_idx = 1
y_idx = 2
elif plot_mode == PlotMode.zx:
x_idx = 2
y_idx = 0
elif plot_mode == PlotMode.zy:
x_idx = 2
y_idx = 1
z_idx = 2 if plot_mode == PlotMode.xyz else None
return x_idx, y_idx, z_idx
traj_est.transform(lie.se3(np.eye(3), [0, 0, 0]))
traj_est.scale(0.5)
logger.info("\nUmeyama alignment without scaling")
traj_est_aligned = trajectory.align_trajectory(traj_est, traj_ref)
logger.info("\nUmeyama alignment with scaling")
traj_est_aligned_scaled = trajectory.align_trajectory(traj_est, traj_ref,
correct_scale=True)
logger.info("\nUmeyama alignment with scaling only")
traj_est_aligned_only_scaled = trajectory.align_trajectory(
traj_est, traj_ref, correct_only_scale=True)
fig = plt.figure(figsize=(8, 8))
plot_mode = plot.PlotMode.xz
ax = plot.prepare_axis(fig, plot_mode, subplot_arg=221)
plot.traj(ax, plot_mode, traj_ref, '--', 'gray')
plot.traj(ax, plot_mode, traj_est, '-', 'blue')
fig.axes.append(ax)
plt.title('not aligned')
ax = plot.prepare_axis(fig, plot_mode, subplot_arg=222)
plot.traj(ax, plot_mode, traj_ref, '--', 'gray')
plot.traj(ax, plot_mode, traj_est_aligned, '-', 'blue')
fig.axes.append(ax)
plt.title('$\mathrm{SE}(3)$ alignment')
ax = plot.prepare_axis(fig, plot_mode, subplot_arg=223)
plot.traj(ax, plot_mode, traj_ref, '--', 'gray')
plot.traj(ax, plot_mode, traj_est_aligned_scaled, '-', 'blue')
:param plot_mode: PlotMode
:param subplot_arg: optional if using subplots - the subplot id (e.g. '122')
:return: the matplotlib axis
"""
if plot_mode == PlotMode.xyz:
ax = fig.add_subplot(subplot_arg, projection="3d")
else:
ax = fig.add_subplot(subplot_arg)
ax.axis("equal")
if plot_mode in {PlotMode.xy, PlotMode.xz, PlotMode.xyz}:
xlabel = "$x$ (m)"
elif plot_mode in {PlotMode.yz, PlotMode.yx}:
xlabel = "$y$ (m)"
else:
xlabel = "$z$ (m)"
if plot_mode in {PlotMode.xy, PlotMode.zy, PlotMode.xyz}:
ylabel = "$y$ (m)"
elif plot_mode in {PlotMode.zx, PlotMode.yx}:
ylabel = "$x$ (m)"
else:
ylabel = "$z$ (m)"
ax.set_xlabel(xlabel)
ax.set_ylabel(ylabel)
if plot_mode == PlotMode.xyz:
ax.set_zlabel('$z$ (m)')
if SETTINGS.plot_invert_xaxis:
plt.gca().invert_xaxis()
if SETTINGS.plot_invert_yaxis:
plt.gca().invert_yaxis()
return ax
def plot_mode_to_idx(plot_mode):
if plot_mode == PlotMode.xy or plot_mode == PlotMode.xyz:
x_idx = 0
y_idx = 1
elif plot_mode == PlotMode.xz:
x_idx = 0
y_idx = 2
elif plot_mode == PlotMode.yx:
x_idx = 1
y_idx = 0
elif plot_mode == PlotMode.yz:
x_idx = 1
y_idx = 2
elif plot_mode == PlotMode.zx:
x_idx = 2
y_idx = 0
elif plot_mode == PlotMode.zy:
x_idx = 2
y_idx = 1
z_idx = 2 if plot_mode == PlotMode.xyz else None
return x_idx, y_idx, z_idx