def filter_pairs_by_speed(poses, timestamps, speed, tol):
"""
filters pairs in a list of SE(3) poses by the linear speed of the motion in between them
:param poses: list of SE(3) poses
:param timestamps: list of timestamps corresponding to the poses
:param speed: in m/s
:param tol: tolerance to accept or reject velocities, in m/s
:return: list of index tuples of the filtered pairs
"""
positions = [pose[:3, 3] for pose in poses]
speeds = [trajectory.calc_speed(positions[i], positions[i + 1],
timestamps[i], timestamps[i + 1])
for i in range(len(positions) - 1)]
id_pairs = [(i, i+1) for i, v in enumerate(speeds) if speed - tol <= v <= speed + tol]
return id_pairs
def filter_pairs_by_speed(poses, timestamps, speed, tol):
"""
filters pairs in a list of SE(3) poses by the linear speed of the motion in between them
:param poses: list of SE(3) poses
:param timestamps: list of timestamps corresponding to the poses
:param speed: in m/s
:param tol: tolerance to accept or reject velocities, in m/s
:return: list of index tuples of the filtered pairs
"""
positions = [pose[:3, 3] for pose in poses]
speeds = [trajectory.calc_speed(positions[i], positions[i + 1],
timestamps[i], timestamps[i + 1])
for i in range(len(positions) - 1)]
id_pairs = [(i, i+1) for i, v in enumerate(speeds) if speed - tol <= v <= speed + tol]
return id_pairs
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)]
speeds.append(0)
plot.traj(ax, plot_mode, traj_ref, '--', 'gray', 'reference')
plot.traj_colormap(ax, traj_est, speeds, plot_mode,
min_map=min(speeds), max_map=max(speeds),
title="speed mapped onto trajectory")
fig_3.axes.append(ax)
plot_collection.add_figure("traj (speed)", fig_3)
plot_collection.show()
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)
]
speeds.append(0)
plot.traj(ax, plot_mode, traj_ref, '--', 'gray', 'reference')
plot.traj_colormap(ax,
traj_est,
speeds,
plot_mode,
min_map=min(speeds),
max_map=max(speeds),
title="speed mapped onto trajectory")
fig_3.axes.append(ax)
plot_collection.add_figure("traj (speed)", fig_3)
def run(args):
import os
import sys
import numpy as np
import evo.core.lie_algebra as lie
from evo.core import trajectory
from evo.core.trajectory import PoseTrajectory3D
from evo.tools import file_interface, log
from evo.tools.settings import SETTINGS
log.configure_logging(verbose=args.verbose,
silent=args.silent,
debug=args.debug)
if args.debug:
import pprint
logger.debug(
"main_parser config:\n" +
pprint.pformat({arg: getattr(args, arg)
for arg in vars(args)}) + "\n")
logger.debug(SEP)
trajectories, ref_traj = load_trajectories(args)
if args.merge:
if args.subcommand == "kitti":
die("Can't merge KITTI files.")
if len(trajectories) == 0:
die("No trajectories to merge (excluding --ref).")
trajectories = {
"merged_trajectory": trajectory.merge(trajectories.values())
}
if args.transform_left or args.transform_right:
tf_type = "left" if args.transform_left else "right"
tf_path = args.transform_left \
if args.transform_left else args.transform_right
transform = file_interface.load_transform_json(tf_path)
logger.debug(SEP)
if not lie.is_se3(transform):
logger.warning("Not a valid SE(3) transformation!")
if args.invert_transform:
transform = lie.se3_inverse(transform)
logger.debug("Applying a {}-multiplicative transformation:\n{}".format(
tf_type, transform))
for traj in trajectories.values():
traj.transform(transform, right_mul=args.transform_right)
if args.t_offset:
logger.debug(SEP)
for name, traj in trajectories.items():
if type(traj) is trajectory.PosePath3D:
die("{} doesn't have timestamps - can't add time offset.".
format(name))
logger.info("Adding time offset to {}: {} (s)".format(
name, args.t_offset))
traj.timestamps += args.t_offset
if args.sync or args.align or args.correct_scale:
from evo.core import sync
if not args.ref:
logger.debug(SEP)
die("Can't align or sync without a reference! (--ref) *grunt*")
for name, traj in trajectories.items():
if args.subcommand == "kitti":
ref_traj_tmp = ref_traj
else:
logger.debug(SEP)
ref_traj_tmp, trajectories[name] = sync.associate_trajectories(
ref_traj,
traj,
max_diff=args.t_max_diff,
first_name="reference",
snd_name=name)
if args.align or args.correct_scale:
logger.debug(SEP)
logger.debug("Aligning {} to reference.".format(name))
trajectories[name] = trajectory.align_trajectory(
trajectories[name],
ref_traj_tmp,
correct_scale=args.correct_scale,
correct_only_scale=args.correct_scale and not args.align,
n=args.n_to_align)
for name, traj in trajectories.items():
print_traj_info(name, traj, args.verbose, args.full_check)
if args.ref:
print_traj_info(args.ref, ref_traj, args.verbose, args.full_check)
if args.plot or args.save_plot or args.serialize_plot:
from evo.tools.plot import PlotMode
plot_mode = PlotMode.xyz if not args.plot_mode else PlotMode[
args.plot_mode]
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))
ax_traj = plot.prepare_axis(fig_traj, plot_mode)
pltstart = 0
pltend = traj.num_poses
if args.plotstart:
pltstart = args.plotstart
if args.plotend != -1:
pltend = args.plotend
if args.ref:
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,
label=short_traj_name,
alpha=SETTINGS.plot_reference_alpha,
start=pltstart,
end=pltend)
plot.traj_xyz(axarr_xyz,
ref_traj,
style=SETTINGS.plot_reference_linestyle,
color=SETTINGS.plot_reference_color,
label=short_traj_name,
alpha=SETTINGS.plot_reference_alpha,
start=pltstart,
end=pltend)
plot.traj_rpy(axarr_rpy,
ref_traj,
style=SETTINGS.plot_reference_linestyle,
color=SETTINGS.plot_reference_color,
label=short_traj_name,
alpha=SETTINGS.plot_reference_alpha,
start=pltstart,
end=pltend)
cmap_colors = None
if SETTINGS.plot_multi_cmap.lower() != "none":
cmap = getattr(cm, SETTINGS.plot_multi_cmap)
cmap_colors = iter(cmap(np.linspace(0, 1, len(trajectories))))
fig_3 = plt.figure(figsize=tuple(SETTINGS.plot_figsize))
#plot_mode = plot.PlotMode.xz
ax = plot.prepare_axis(fig_3, plot_mode)
fig_3.axes.append(ax)
for name, traj in trajectories.items():
num = traj.positions_xyz.shape[0]
if pltstart >= num:
print(name, "plotstart > len!", num)
pltstart = 0
if pltend != -1 and (pltend > num or pltend < pltstart):
print(name, "plotend > len!", num)
pltend = traj.num_poses
pltstart = int(pltstart)
pltend = int(pltend)
if cmap_colors is None:
color = next(ax_traj._get_lines.prop_cycler)['color']
else:
color = next(cmap_colors)
short_traj_name = os.path.splitext(os.path.basename(name))[0]
if SETTINGS.plot_usetex:
short_traj_name = short_traj_name.replace("_", "\\_")
plot.traj(ax_traj,
plot_mode,
traj,
'-',
color,
short_traj_name,
start=pltstart,
end=pltend)
if args.ref and isinstance(ref_traj, trajectory.PoseTrajectory3D):
start_time = ref_traj.timestamps[0]
else:
start_time = None
plot.traj_xyz(axarr_xyz,
traj,
'-',
color,
short_traj_name,
start_timestamp=start_time,
start=pltstart,
end=pltend)
plot.traj_rpy(axarr_rpy,
traj,
'-',
color,
short_traj_name,
start_timestamp=start_time,
start=pltstart,
end=pltend)
speeds = [
trajectory.calc_speed(traj.positions_xyz[i],
traj.positions_xyz[i + 1],
traj.timestamps[i],
traj.timestamps[i + 1])
for i in range(pltstart, pltend - 1)
]
speeds.append(0)
#plot.traj(ax, plot_mode, traj, '--', 'gray', 'reference')
plot.traj_colormap(ax,
traj,
speeds,
plot_mode,
min_map=min(speeds),
max_map=max(max(speeds), 10),
title="speed mapped onto trajectory",
start=pltstart,
end=pltend)
plot_collection.add_figure("trajectories", fig_traj)
plot_collection.add_figure("xyz_view", fig_xyz)
plot_collection.add_figure("rpy_view", fig_rpy)
plot_collection.add_figure("traj (speed)", fig_3)
if args.plot:
plot_collection.show()
if args.save_plot:
logger.info(SEP)
plot_collection.export(args.save_plot,
confirm_overwrite=not args.no_warnings)
if args.serialize_plot:
logger.info(SEP)
plot_collection.serialize(args.serialize_plot,
confirm_overwrite=not args.no_warnings)
if args.save_as_tum:
logger.info(SEP)
for name, traj in trajectories.items():
dest = os.path.splitext(os.path.basename(name))[0] + ".tum"
file_interface.write_tum_trajectory_file(
dest, traj, confirm_overwrite=not args.no_warnings)
if args.ref:
dest = os.path.splitext(os.path.basename(args.ref))[0] + ".tum"
file_interface.write_tum_trajectory_file(
dest, ref_traj, confirm_overwrite=not args.no_warnings)
if args.save_as_kitti:
logger.info(SEP)
for name, traj in trajectories.items():
dest = os.path.splitext(os.path.basename(name))[0] + ".kitti"
file_interface.write_kitti_poses_file(
dest, traj, confirm_overwrite=not args.no_warnings)
if args.ref:
dest = os.path.splitext(os.path.basename(args.ref))[0] + ".kitti"
file_interface.write_kitti_poses_file(
dest, ref_traj, confirm_overwrite=not args.no_warnings)
if args.save_as_bag:
import datetime
import rosbag
dest_bag_path = str(
datetime.datetime.now().strftime("%Y-%m-%d-%H-%M-%S")) + ".bag"
logger.info(SEP)
logger.info("Saving trajectories to " + dest_bag_path + "...")
bag = rosbag.Bag(dest_bag_path, 'w')
try:
for name, traj in trajectories.items():
dest_topic = os.path.splitext(os.path.basename(name))[0]
frame_id = traj.meta[
"frame_id"] if "frame_id" in traj.meta else ""
file_interface.write_bag_trajectory(bag, traj, dest_topic,
frame_id)
if args.ref:
dest_topic = os.path.splitext(os.path.basename(args.ref))[0]
frame_id = ref_traj.meta[
"frame_id"] if "frame_id" in ref_traj.meta else ""
file_interface.write_bag_trajectory(bag, ref_traj, dest_topic,
frame_id)
finally:
bag.close()
