def load_assoc_euroc_trajectories(ref_file,
est_file,
max_diff=0.01,
offset_2=0.0,
invert=False):
"""
parses ground truth trajectory from EuRoC MAV state estimate .csv and estimated TUM trajectory
and returns the data with associated (matching) timestamps according to the time parameters
:param ref_file: ground truth: <sequence>/mav0/state_groundtruth_estimate0/data.csv
:param est_file: estimated TUM trajectory file
:param max_diff: max. allowed absolute time difference for associating
:param offset_2: optional time offset of second timestamp vector
:param invert: set to True to match from longer list to short (default: short to longer list)
:return: trajectory.PoseTrajectory3D objects traj_ref and traj_est
"""
traj_ref = read_euroc_csv_trajectory(ref_file)
traj_est = read_tum_trajectory_file(est_file)
return sync.associate_trajectories(traj_ref, traj_est, max_diff, offset_2,
invert, ref_file, est_file)
def load_assoc_tum_trajectories(ref_file,
est_file,
max_diff=0.01,
offset_2=0.0,
invert=False):
"""
parses two trajectory files in TUM format (timestamp tx ty tz qx qy qz qw)
and returns the data with associated (matching) timestamps according to the time parameters
:param ref_file: first trajectory file
:param est_file: second trajectory file
:param max_diff: max. allowed absolute time difference for associating
:param offset_2: optional time offset of second timestamp vector
:param invert: set to True to match from longer list to short (default: short to longer list)
:return: trajectory.PoseTrajectory3D objects traj_ref and traj_est
"""
traj_ref = read_tum_trajectory_file(ref_file)
traj_est = read_tum_trajectory_file(est_file)
return sync.associate_trajectories(traj_ref, traj_est, max_diff, offset_2,
invert, ref_file, est_file)
def load_assoc_bag_trajectories(bag_handle,
ref_topic,
est_topic,
max_diff=0.01,
offset_2=0.0,
invert=False):
"""
reads trajectory data from a ROS bag file with two geometry_msgs/PoseStamped topics
and returns the data with associated (matching) timestamps according to the time parameters
:param bag_handle: opened bag handle, from rosbag.Bag(...)
:param ref_topic: first geometry_msgs/PoseStamped topic
:param est_topic: second geometry_msgs/PoseStamped topic
:param max_diff: max. allowed absolute time difference for associating
:param offset_2: optional time offset of second timestamp vector
:param invert: set to True to match from longer list to short (default: short to longer list)
:return: trajectory.PoseTrajectory3D objects traj_ref and traj_est
"""
traj_ref = read_bag_trajectory(bag_handle, ref_topic)
traj_est = read_bag_trajectory(bag_handle, est_topic)
return sync.associate_trajectories(traj_ref, traj_est, max_diff, offset_2,
invert, ref_topic, est_topic)
def run(args):
import os
import sys
import logging
import evo.algorithms.lie_algebra as lie
from evo.algorithms import trajectory
from evo.tools import file_interface, settings
from evo.tools.settings import SETTINGS
settings.configure_logging(verbose=True,
silent=args.silent,
debug=args.debug)
if args.debug:
import pprint
logging.debug(
"main_parser config:\n" +
pprint.pformat({arg: getattr(args, arg)
for arg in vars(args)}) + "\n")
logging.debug(SEP)
trajectories = []
ref_traj = None
if args.subcommand == "tum":
for traj_file in args.traj_files:
if traj_file != args.ref:
trajectories.append(
(traj_file,
file_interface.read_tum_trajectory_file(traj_file)))
if args.ref:
ref_traj = file_interface.read_tum_trajectory_file(args.ref)
elif args.subcommand == "kitti":
for pose_file in args.pose_files:
if pose_file != args.ref:
trajectories.append(
(pose_file,
file_interface.read_kitti_poses_file(pose_file)))
if args.ref:
ref_traj = file_interface.read_kitti_poses_file(args.ref)
elif args.subcommand == "euroc":
for csv_file in args.state_gt_csv:
if csv_file != args.ref:
trajectories.append(
(csv_file,
file_interface.read_euroc_csv_trajectory(csv_file)))
if args.ref:
ref_traj = file_interface.read_euroc_csv_trajectory(args.ref)
elif args.subcommand == "bag":
import rosbag
bag = rosbag.Bag(args.bag)
try:
if args.all_topics:
topic_info = bag.get_type_and_topic_info()
topics = sorted([
t for t in topic_info[1].keys()
if topic_info[1][t][0] == "geometry_msgs/PoseStamped"
and t != args.ref
])
if len(topics) == 0:
logging.error("no geometry_msgs/PoseStamped topics found!")
sys.exit(1)
else:
topics = args.topics
if not topics:
logging.warning(
"no topics used - specify topics or use the --all_topics flag"
)
sys.exit(1)
for topic in topics:
trajectories.append(
(topic, file_interface.read_bag_trajectory(bag, topic)))
if args.ref:
ref_traj = file_interface.read_bag_trajectory(bag, args.ref)
finally:
bag.close()
else:
raise RuntimeError("unsupported subcommand: " + args.subcommand)
if args.transform_left or args.transform_right:
tf_path = args.transform_left if args.transform_left else args.transform_right
t, xyz, quat = file_interface.load_transform_json(tf_path)
logging.debug(SEP)
logging.debug("applying transformation to the trajectories:\n" +
str(t))
if args.invert_transform:
t = lie.se3_inverse(t)
for name, traj in trajectories:
traj.transform(t, right_mul=args.transform_right)
if args.align or args.correct_scale:
if args.ref:
if args.subcommand == "kitti":
traj_tmp, ref_traj_tmp = trajectories, [
ref_traj for n, t in trajectories
]
else:
traj_tmp, ref_traj_tmp = [], []
from evo.algorithms import sync
for name, traj in trajectories:
logging.debug(SEP)
ref_assoc, traj_assoc = sync.associate_trajectories(
ref_traj, traj, first_name="ref", snd_name=name)
ref_traj_tmp.append(ref_assoc)
traj_tmp.append((name, traj_assoc))
trajectories = traj_tmp
correct_only_scale = args.correct_scale and not args.align
trajectories_new = []
for nt, ref_assoc in zip(trajectories, ref_traj_tmp):
logging.debug(SEP)
logging.debug("aligning " + nt[0] + " to " + args.ref + "...")
trajectories_new.append(
(nt[0],
trajectory.align_trajectory(nt[1], ref_assoc,
args.correct_scale,
correct_only_scale,
args.n_to_align)))
trajectories = trajectories_new
for name, traj in trajectories:
if args.t_offset and traj.timestamps.shape[0] != 0:
logging.debug(SEP)
logging.info("adding time offset to " + name + ": " +
str(args.t_offset) + " (s)")
traj.timestamps += args.t_offset
print_traj_info(name, traj, args.full_check)
if (args.align or args.correct_scale) and not args.ref:
logging.debug(SEP)
logging.warning("can't align without a reference! (--ref) *grunt*")
if args.ref:
print_traj_info(args.ref, ref_traj, 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_traj = plt.figure(figsize=tuple(SETTINGS.plot_figsize))
if (args.align or args.correct_scale) and not args.ref:
plt.xkcd(scale=2, randomness=4)
fig_traj.suptitle("what if --ref?")
fig_xyz.suptitle("what if --ref?")
ax_traj = plot.prepare_axis(fig_traj, plot_mode)
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,
'--',
'grey',
short_traj_name,
alpha=0 if SETTINGS.plot_hideref else 1)
plot.traj_xyz(axarr_xyz,
ref_traj,
'--',
'grey',
short_traj_name,
alpha=0 if SETTINGS.plot_hideref else 1)
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))))
for name, traj in trajectories:
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)
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)
plt.tight_layout()
plot_collection.add_figure("trajectories", fig_traj)
plot_collection.add_figure("xyz_view", fig_xyz)
if args.plot:
plot_collection.show()
if args.save_plot:
logging.debug(SEP)
plot_collection.export(args.save_plot,
confirm_overwrite=not args.no_warnings)
if args.serialize_plot:
logging.debug(SEP)
plot_collection.serialize(args.serialize_plot,
confirm_overwrite=not args.no_warnings)
if args.save_as_tum:
logging.debug(SEP)
for name, traj in trajectories:
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:
logging.debug(SEP)
for name, traj in trajectories:
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:
logging.debug(SEP)
import datetime
import rosbag
dest_bag_path = str(
datetime.datetime.now().strftime('%Y-%m-%d_%H:%M:%S.%f')) + ".bag"
logging.debug("saving trajectories to " + dest_bag_path + "...")
bag = rosbag.Bag(dest_bag_path, 'w')
try:
for name, traj in trajectories:
dest_topic = os.path.splitext(os.path.basename(name))[0]
file_interface.write_bag_trajectory(bag, traj, dest_topic)
if args.ref:
dest_topic = os.path.splitext(os.path.basename(args.ref))[0]
file_interface.write_bag_trajectory(bag, ref_traj, dest_topic)
finally:
bag.close()
#!/usr/bin/env python
from __future__ import print_function
print("loading required evo modules")
from evo.algorithms import trajectory, sync, metrics
from evo.tools import file_interface
print("loading trajectories")
traj_ref = file_interface.read_tum_trajectory_file("../test/data/fr2_desk_groundtruth.txt")
traj_est = file_interface.read_tum_trajectory_file("../test/data/fr2_desk_ORB.txt")
print("registering trajectories")
traj_ref, traj_est = sync.associate_trajectories(traj_ref, traj_est)
traj_est = trajectory.align_trajectory(traj_est, traj_ref, correct_scale=False)
print("calculating APE")
data = (traj_ref, traj_est)
ape_metric = metrics.APE(metrics.PoseRelation.translation_part)
ape_metric.process_data(data)
ape_statistics = ape_metric.get_all_statistics()
print("mean:", ape_statistics["mean"])
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))
