def test_relative_se3(self):
a = lie.random_se3()
b = lie.random_se3()
self.assertTrue(lie.is_se3(a) and lie.is_se3(b))
a_to_b = lie.relative_se3(a, b)
self.assertTrue(lie.is_se3(a_to_b))
b_from_a = a.dot(a_to_b)
self.assertTrue(np.allclose(b_from_a, b))
def test_relative_se3_rot(self):
a = lie.random_se3()
b = lie.random_se3()
self.assertTrue(lie.is_se3(a) and lie.is_se3(b))
a_to_b_rot = lie.relative_se3_rot(a, b)
self.assertTrue(lie.is_so3(a_to_b_rot))
b_rot_from_a_rot = a[:3, :3].dot(a_to_b_rot)
self.assertTrue(np.allclose(b_rot_from_a_rot, b[:3, :3]))
def test_is_se3(self):
p = np.array([[1, 0, 0, 1],
[0, 0, -1, 2],
[0, 1, 0, 3],
[0, 0, 0, 1]])
self.assertTrue(lie.is_se3(p))
p_false = np.array([[1, 0, 0, 1],
[0, 0, -4223, 2],
[0, 111, 0, 3],
[0, 0, 0, 1]])
self.assertFalse(lie.is_se3(p_false))
def transform(self, t, right_mul=False, propagate=False):
"""
apply a left or right multiplicative SE(3) transformation to the whole path
:param t: a valid SE(3) matrix
:param right_mul: whether to apply it right-multiplicative or not
:param propagate: whether to propagate drift with RHS transformations
"""
if not lie.is_se3(t):
raise TrajectoryException(
"transformation is not a valid SE(3) matrix")
if right_mul and not propagate:
# Transform each pose individually.
self._poses_se3 = [np.dot(p, t) for p in self.poses_se3]
elif right_mul and propagate:
# Transform each pose and propagate resulting drift to the next.
ids = np.arange(0, self.num_poses, 1)
rel_poses = [
lie.relative_se3(self.poses_se3[i], self.poses_se3[j]).dot(t)
for i, j in zip(ids, ids[1:])
]
self._poses_se3 = [self.poses_se3[0]]
for i, j in zip(ids[:-1], ids):
self._poses_se3.append(self._poses_se3[j].dot(rel_poses[i]))
else:
self._poses_se3 = [np.dot(t, p) for p in self.poses_se3]
self._positions_xyz, self._orientations_quat_wxyz \
= se3_poses_to_xyz_quat_wxyz(self.poses_se3)
def transform(self, t, right_mul=False):
"""
apply a left or right multiplicative SE(3) transformation to the whole path
:param t: a valid SE(3) matrix
:param right_mul: whether to apply it right-multiplicative or not
"""
if not lie.is_se3(t):
raise TrajectoryException("transformation is not a valid SE(3) matrix")
if right_mul:
self._poses_se3 = [np.dot(p, t) for p in self.poses_se3]
else:
self._poses_se3 = [np.dot(t, p) for p in self.poses_se3]
self._positions_xyz, self._orientations_quat_wxyz \
= se3_poses_to_xyz_quat_wxyz(self.poses_se3)
def transform(self, t, right_mul=False):
"""
apply a left or right multiplicative SE(3) transformation to the whole path
:param t: a valid SE(3) matrix
:param right_mul: whether to apply it right-multiplicative or not
"""
if not lie.is_se3(t):
raise TrajectoryException("transformation is not a valid SE(3) matrix")
if right_mul:
self._poses_se3 = [np.dot(p, t) for p in self.poses_se3]
else:
self._poses_se3 = [np.dot(t, p) for p in self.poses_se3]
self._positions_xyz, self._orientations_quat_wxyz \
= se3_poses_to_xyz_quat_wxyz(self.poses_se3)
def check(self):
"""
checks if the data is valid
:return: True/False, dictionary with some detailed infos
"""
same_len = self.positions_xyz.shape[0] \
== self.orientations_quat_wxyz.shape[0] \
== len(self.poses_se3)
se3_valid = all([lie.is_se3(p) for p in self.poses_se3])
norms = np.linalg.norm(self.orientations_quat_wxyz, axis=1)
quat_normed = np.allclose(norms, np.ones(norms.shape))
valid = same_len and se3_valid and quat_normed
details = {
"array shapes": "ok" if same_len else "invalid (lists must have same length)",
"SE(3) conform": "yes" if se3_valid else "no (poses are not valid SE(3) matrices)",
"quaternions": "ok" if quat_normed else "invalid (must be unit quaternions)"
}
return valid, details
def check(self):
"""
checks if the data is valid
:return: True/False, dictionary with some detailed infos
"""
same_len = self.positions_xyz.shape[0] \
== self.orientations_quat_wxyz.shape[0] \
== len(self.poses_se3)
se3_valid = all([lie.is_se3(p) for p in self.poses_se3])
norms = np.linalg.norm(self.orientations_quat_wxyz, axis=1)
quat_normed = np.allclose(norms, np.ones(norms.shape))
valid = same_len and se3_valid and quat_normed
details = {
"array shapes": "ok" if same_len else "invalid (lists must have same length)",
"SE(3) conform": "yes" if se3_valid else "no (poses are not valid SE(3) matrices)",
"quaternions": "ok" if quat_normed else "invalid (must be unit quaternions)"
}
return valid, details
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,
local_logfile=args.logfile)
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,
propagate=args.propagate_transform)
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.n_to_align != -1 and not (args.align or args.correct_scale):
die("--n_to_align is useless without --align or/and --correct_scale")
if args.sync or args.align or args.correct_scale or args.align_origin:
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)
if args.align_origin:
logger.debug(SEP)
logger.debug("Aligning {}'s origin to reference.".format(name))
trajectories[name] = trajectory.align_trajectory_origin(
trajectories[name], ref_traj_tmp)
print_compact_name = not args.subcommand == "bag"
for name, traj in trajectories.items():
print_traj_info(name, traj, args.verbose, args.full_check,
print_compact_name)
if args.ref:
print_traj_info(args.ref, ref_traj, args.verbose, args.full_check,
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)
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)
plot.draw_coordinate_axes(ax_traj, ref_traj, plot_mode,
SETTINGS.plot_axis_marker_scale)
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)
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)
if args.ros_map_yaml:
plot.ros_map(ax_traj, args.ros_map_yaml, plot_mode)
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.items():
if cmap_colors is None:
color = next(ax_traj._get_lines.prop_cycler)['color']
else:
color = next(cmap_colors)
if print_compact_name:
short_traj_name = os.path.splitext(os.path.basename(name))[0]
else:
short_traj_name = name
if SETTINGS.plot_usetex:
short_traj_name = short_traj_name.replace("_", "\\_")
plot.traj(ax_traj,
plot_mode,
traj,
SETTINGS.plot_trajectory_linestyle,
color,
short_traj_name,
alpha=SETTINGS.plot_trajectory_alpha)
plot.draw_coordinate_axes(ax_traj, traj, plot_mode,
SETTINGS.plot_axis_marker_scale)
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,
SETTINGS.plot_trajectory_linestyle,
color,
short_traj_name,
alpha=SETTINGS.plot_trajectory_alpha,
start_timestamp=start_time)
plot.traj_rpy(axarr_rpy,
traj,
SETTINGS.plot_trajectory_linestyle,
color,
short_traj_name,
alpha=SETTINGS.plot_trajectory_alpha,
start_timestamp=start_time)
if not SETTINGS.plot_usetex:
fig_rpy.text(0.,
0.005,
"euler_angle_sequence: {}".format(
SETTINGS.euler_angle_sequence),
fontsize=6)
plot_collection.add_figure("trajectories", fig_traj)
plot_collection.add_figure("xyz_view", fig_xyz)
plot_collection.add_figure("rpy_view", fig_rpy)
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()
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 = 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:
logger.error("No geometry_msgs/PoseStamped topics found!")
sys.exit(1)
else:
topics = args.topics
if not topics:
logger.warning(
"No topics used - specify topics or set --all_topics.")
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.merge:
if args.subcommand == "kitti":
raise TypeError(
"can't merge KITTI files - but you can append them with 'cat'")
if len(trajectories) == 0:
raise RuntimeError("no trajectories to merge (excluding --ref)")
merged_stamps = trajectories[0][1].timestamps
merged_xyz = trajectories[0][1].positions_xyz
merged_quat = trajectories[0][1].orientations_quat_wxyz
for _, traj in trajectories[1:]:
merged_stamps = np.concatenate((merged_stamps, traj.timestamps))
merged_xyz = np.concatenate((merged_xyz, traj.positions_xyz))
merged_quat = np.concatenate(
(merged_quat, traj.orientations_quat_wxyz))
order = merged_stamps.argsort()
merged_stamps = merged_stamps[order]
merged_xyz = merged_xyz[order]
merged_quat = merged_quat[order]
trajectories = [("merged_trajectory",
PoseTrajectory3D(merged_xyz, merged_quat,
merged_stamps))]
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
t, xyz, quat = file_interface.load_transform_json(tf_path)
logger.debug(SEP)
if not lie.is_se3(t):
logger.warning("Not a valid SE(3) transformation!")
if args.invert_transform:
t = lie.se3_inverse(t)
logger.debug("Applying a {}-multiplicative transformation:\n{}".format(
tf_type, t))
for name, traj in trajectories:
traj.transform(t, right_mul=args.transform_right)
if args.t_offset:
logger.debug(SEP)
for name, traj in trajectories:
if type(traj) is trajectory.PosePath3D:
logger.warning(
"{} doesn't have timestamps - can't add time offset.".
format(name))
else:
logger.info("Adding time offset to {}: {} (s)".format(
name, args.t_offset))
traj.timestamps += args.t_offset
if args.align or args.correct_scale:
if not args.ref:
logger.debug(SEP)
logger.warning("Can't align without a reference! (--ref) *grunt*")
else:
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.core import sync
for name, traj in trajectories:
logger.debug(SEP)
ref_assoc, traj_assoc = sync.associate_trajectories(
ref_traj,
traj,
max_diff=args.t_max_diff,
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):
logger.debug(SEP)
logger.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:
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))
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)
plot.traj_rpy(axarr_rpy,
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)
plot.traj_rpy(axarr_rpy,
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)
plot_collection.add_figure("rpy_view", fig_rpy)
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:
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:
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:
logger.info(SEP)
import datetime
import rosbag
dest_bag_path = str(
datetime.datetime.now().strftime('%Y-%m-%d_%H:%M:%S.%f')) + ".bag"
logger.info("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]
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()
def test_se3_inverse(self):
p = lie.random_se3()
p_inv = lie.se3_inverse(p)
self.assertTrue(lie.is_se3(p_inv))
self.assertTrue(np.allclose(p_inv.dot(p), np.eye(4)))
def test_random_se3(self):
self.assertTrue(lie.is_se3(lie.random_se3()))
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()
def run(args):
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
log.configure_logging(verbose=args.verbose,
silent=args.silent,
debug=args.debug,
local_logfile=args.logfile)
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.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.n_to_align != -1 and not (args.align or args.correct_scale):
die("--n_to_align is useless without --align or/and --correct_scale")
# TODO: this is fugly, but is a quick solution for remembering each synced
# reference when plotting pose correspondences later...
synced = (args.subcommand == "kitti" and ref_traj) or any(
(args.sync, args.align, args.correct_scale, args.align_origin))
synced_refs = {}
if synced:
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].align(ref_traj_tmp,
correct_scale=args.correct_scale,
correct_only_scale=args.correct_scale
and not args.align,
n=args.n_to_align)
if args.align_origin:
logger.debug(SEP)
logger.debug("Aligning {}'s origin to reference.".format(name))
trajectories[name].align_origin(ref_traj_tmp)
if SETTINGS.plot_pose_correspondences:
synced_refs[name] = ref_traj_tmp
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,
propagate=args.propagate_transform)
for name, traj in trajectories.items():
print_traj_info(to_compact_name(name, args), traj, args.verbose,
args.full_check)
if args.ref:
print_traj_info(to_compact_name(args.ref, args), ref_traj,
args.verbose, args.full_check)
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 = to_compact_name(args.ref, args,
SETTINGS.plot_usetex)
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)
plot.draw_coordinate_axes(
ax_traj, ref_traj, plot_mode,
SETTINGS.plot_reference_axis_marker_scale)
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_timestamp=start_time)
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_timestamp=start_time)
if args.ros_map_yaml:
plot.ros_map(ax_traj, args.ros_map_yaml, plot_mode)
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.items():
if cmap_colors is None:
color = next(ax_traj._get_lines.prop_cycler)['color']
else:
color = next(cmap_colors)
short_traj_name = to_compact_name(name, args, SETTINGS.plot_usetex)
plot.traj(ax_traj,
plot_mode,
traj,
SETTINGS.plot_trajectory_linestyle,
color,
short_traj_name,
alpha=SETTINGS.plot_trajectory_alpha)
plot.draw_coordinate_axes(ax_traj, traj, plot_mode,
SETTINGS.plot_axis_marker_scale)
if ref_traj and synced and SETTINGS.plot_pose_correspondences:
plot.draw_correspondence_edges(
ax_traj,
traj,
synced_refs[name],
plot_mode,
color=color,
style=SETTINGS.plot_pose_correspondences_linestyle,
alpha=SETTINGS.plot_trajectory_alpha)
plot.traj_xyz(axarr_xyz,
traj,
SETTINGS.plot_trajectory_linestyle,
color,
short_traj_name,
alpha=SETTINGS.plot_trajectory_alpha,
start_timestamp=start_time)
plot.traj_rpy(axarr_rpy,
traj,
SETTINGS.plot_trajectory_linestyle,
color,
short_traj_name,
alpha=SETTINGS.plot_trajectory_alpha,
start_timestamp=start_time)
if not SETTINGS.plot_usetex:
fig_rpy.text(0.,
0.005,
"euler_angle_sequence: {}".format(
SETTINGS.euler_angle_sequence),
fontsize=6)
plot_collection.add_figure("trajectories", fig_traj)
plot_collection.add_figure("xyz_view", fig_xyz)
plot_collection.add_figure("rpy_view", fig_rpy)
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 = to_filestem(name, args) + ".tum"
file_interface.write_tum_trajectory_file(
dest, traj, confirm_overwrite=not args.no_warnings)
if args.ref:
dest = to_filestem(args.ref, args) + ".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 = to_filestem(name, args) + ".kitti"
file_interface.write_kitti_poses_file(
dest, traj, confirm_overwrite=not args.no_warnings)
if args.ref:
dest = to_filestem(args.ref, args) + ".kitti"
file_interface.write_kitti_poses_file(
dest, ref_traj, confirm_overwrite=not args.no_warnings)
if args.save_as_bag or args.save_as_bag2:
from rosbags.rosbag1 import Writer as Rosbag1Writer
from rosbags.rosbag2 import Writer as Rosbag2Writer
writers = []
if args.save_as_bag:
dest_bag_path = str(
datetime.datetime.now().strftime("%Y-%m-%d-%H-%M-%S")) + ".bag"
writers.append(Rosbag1Writer(dest_bag_path))
if args.save_as_bag2:
dest_bag_path = str(
datetime.datetime.now().strftime("%Y-%m-%d-%H-%M-%S"))
writers.append(Rosbag2Writer(dest_bag_path))
for writer in writers:
logger.info(SEP)
logger.info("Saving trajectories to " + str(writer.path) + "...")
try:
writer.open()
for name, traj in trajectories.items():
dest_topic = to_topic_name(name, args)
frame_id = traj.meta[
"frame_id"] if "frame_id" in traj.meta else ""
file_interface.write_bag_trajectory(
writer, traj, dest_topic, frame_id)
if args.ref:
dest_topic = to_topic_name(args.ref, args)
frame_id = ref_traj.meta[
"frame_id"] if "frame_id" in ref_traj.meta else ""
file_interface.write_bag_trajectory(
writer, ref_traj, dest_topic, frame_id)
finally:
writer.close()
if args.save_table:
from evo.tools import pandas_bridge
logger.debug(SEP)
df = pandas_bridge.trajectories_stats_to_df(trajectories)
pandas_bridge.save_df_as_table(df,
args.save_table,
confirm_overwrite=not args.no_warnings)
