def read_bag_trajectory(bag_handle, topic):
"""
:param bag_handle: opened bag handle, from rosbag.Bag(...)
:param topic: trajectory topic of supported message type,
or a TF trajectory ID (e.g.: '/tf:map.base_link' )
:return: trajectory.PoseTrajectory3D
"""
from evo.tools import tf_cache
# Use TfCache instead if it's a TF transform ID.
if tf_cache.instance().check_id(topic):
return tf_cache.instance().get_trajectory(bag_handle, identifier=topic)
if not bag_handle.get_message_count(topic) > 0:
raise FileInterfaceException("no messages for topic '" + topic +
"' in bag")
msg_type = bag_handle.get_type_and_topic_info().topics[topic].msg_type
if msg_type not in SUPPORTED_ROS_MSGS:
raise FileInterfaceException(
"unsupported message type: {}".format(msg_type))
# Choose appropriate message conversion.
if msg_type == "geometry_msgs/TransformStamped":
get_xyz_quat = _get_xyz_quat_from_transform_stamped
else:
get_xyz_quat = _get_xyz_quat_from_pose_or_odometry_msg
stamps, xyz, quat = [], [], []
for topic, msg, _ in bag_handle.read_messages(topic):
# Use the header timestamps (converted to seconds).
t = msg.header.stamp
stamps.append(t.secs + (t.nsecs * 1e-9))
xyz_t, quat_t = get_xyz_quat(msg)
xyz.append(xyz_t)
quat.append(quat_t)
logger.debug("Loaded {} {} messages of topic: {}".format(
len(stamps), msg_type, topic))
generator = bag_handle.read_messages(topic)
_, first_msg, _ = next(generator)
frame_id = first_msg.header.frame_id
return PoseTrajectory3D(xyz, quat, stamps, meta={"frame_id": frame_id})
def read_euroc_csv_trajectory(file_path):
"""
parses ground truth trajectory from EuRoC MAV state estimate .csv
:param file_path: <sequence>/mav0/state_groundtruth_estimate0/data.csv
:return: trajectory.PoseTrajectory3D object
"""
raw_mat = csv_read_matrix(file_path, delim=",", comment_str="#")
error_msg = ("EuRoC MAV state ground truth must have 17 entries per row "
"and no trailing delimiter at the end of the rows (comma)")
if len(raw_mat) > 0 and len(raw_mat[0]) != 17:
raise FileInterfaceException(error_msg)
try:
mat = np.array(raw_mat).astype(float)
except ValueError:
raise FileInterfaceException(error_msg)
stamps = np.divide(mat[:, 0], 1e9) # n x 1 - nanoseconds to seconds
xyz = mat[:, 1:4] # n x 3
quat = mat[:, 4:8] # n x 4
logger.debug("Loaded {} stamps and poses from: {}".format(
len(stamps), file_path))
return PoseTrajectory3D(xyz, quat, stamps)
def read_bag_trajectory(bag_handle, topic):
"""
:param bag_handle: opened bag handle, from rosbag.Bag(...)
:param topic:
geometry_msgs/PoseStamped, geometry_msgs/PoseWithCovarianceStamped
or nav_msgs/Odometry topic
:return: trajectory.PoseTrajectory3D
"""
if not bag_handle.get_message_count(topic) > 0:
raise FileInterfaceException("no messages for topic '" + topic +
"' in bag")
msg_type = bag_handle.get_type_and_topic_info().topics[topic].msg_type
if msg_type not in {
"geometry_msgs/PoseStamped",
"geometry_msgs/PoseWithCovarianceStamped", "nav_msgs/Odometry"
}:
raise FileInterfaceException(
"unsupported message type: {}".format(msg_type))
stamps, xyz, quat = [], [], []
for topic, msg, _ in bag_handle.read_messages(topic):
# Use the header timestamps (converted to seconds).
t = msg.header.stamp
stamps.append(t.secs + (t.nsecs * 1e-9))
# Make nav_msgs/Odometry behave like geometry_msgs/PoseStamped.
while not hasattr(msg.pose, 'position') and not hasattr(
msg.pose, 'orientation'):
msg = msg.pose
xyz.append(
[msg.pose.position.x, msg.pose.position.y, msg.pose.position.z])
quat.append([
msg.pose.orientation.x, msg.pose.orientation.y,
msg.pose.orientation.z, msg.pose.orientation.w
])
quat = np.roll(quat, 1, axis=1) # shift 1 column -> w in front column
logger.debug("Loaded {} {} messages of topic: {}".format(
len(stamps), msg_type, topic))
generator = bag_handle.read_messages(topic)
_, first_msg, _ = generator.next()
frame_id = first_msg.header.frame_id
return PoseTrajectory3D(xyz, quat, stamps, meta={"frame_id": frame_id})
def read_tum_trajectory_file(file_path):
"""
parses trajectory file in TUM format (timestamp tx ty tz qx qy qz qw)
:param file_path: the trajectory file path (or file handle)
:return: trajectory.PoseTrajectory3D object
"""
raw_mat = csv_read_matrix(file_path, delim=" ", comment_str="#")
error_msg = ("TUM trajectory files must have 8 entries per row "
"and no trailing delimiter at the end of the rows (space)")
if len(raw_mat) > 0 and len(raw_mat[0]) != 8:
raise FileInterfaceException(error_msg)
try:
mat = np.array(raw_mat).astype(float)
except ValueError:
raise FileInterfaceException(error_msg)
stamps = mat[:, 0] # n x 1
xyz = mat[:, 1:4] # n x 3
quat = mat[:, 4:] # n x 4
quat = np.roll(quat, 1, axis=1) # shift 1 column -> w in front column
if not hasattr(file_path, 'read'): # if not file handle
logger.debug("Loaded {} stamps and poses from: {}".format(
len(stamps), file_path))
return PoseTrajectory3D(xyz, quat, stamps)
def read_bag_trajectory(bag_handle, topic):
"""
:param bag_handle: opened bag handle, from rosbag.Bag(...)
:param topic: geometry_msgs/PoseStamped topic
:return: trajectory.PoseTrajectory3D
"""
if not bag_handle.get_message_count(topic) > 0:
raise FileInterfaceException("no messages for topic '" + topic +
"' in bag")
stamps, xyz, quat = [], [], []
for topic, msg, t in bag_handle.read_messages(topic):
stamps.append(t.secs + (t.nsecs * 1e-9))
xyz.append(
[msg.pose.position.x, msg.pose.position.y, msg.pose.position.z])
quat.append([
msg.pose.orientation.x, msg.pose.orientation.y,
msg.pose.orientation.z, msg.pose.orientation.w
])
logging.debug("loaded " + str(len(stamps)) +
" geometry_msgs/PoseStamped messages" + " of topic: " +
topic)
quat = np.roll(quat, 1, axis=1) # shift 1 column -> w in front column
return PoseTrajectory3D(xyz, quat, stamps)
def read_bag_trajectory(bag_handle, topic):
"""
:param bag_handle: opened bag handle, from rosbag.Bag(...)
:param topic: geometry_msgs/PoseStamped topic
:return: trajectory.PoseTrajectory3D
"""
if not bag_handle.get_message_count(topic) > 0:
raise FileInterfaceException(
"no messages for topic '" + topic + "' in bag")
stamps, xyz, quat = [], [], []
for topic, msg, t in bag_handle.read_messages(topic):
stamps.append(t.secs + (t.nsecs * 1e-9))
xyz.append(
[msg.pose.position.x, msg.pose.position.y, msg.pose.position.z])
quat.append([msg.pose.orientation.x, msg.pose.orientation.y,
msg.pose.orientation.z, msg.pose.orientation.w])
quat = np.roll(quat, 1, axis=1) # shift 1 column -> w in front column
logger.debug(
"Loaded {} geometry_msgs/PoseStamped messages of topic: {}".format(
len(stamps), topic))
generator = bag_handle.read_messages(topic)
_, first_msg, _ = generator.next()
frame_id = first_msg.header.frame_id
return PoseTrajectory3D(xyz, quat, stamps, meta={"frame_id": frame_id})
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 read_bag_trajectory(reader: typing.Union[Rosbag1Reader, Rosbag2Reader],
topic: str) -> PoseTrajectory3D:
"""
:param reader: opened bag reader (rosbags.rosbag2 or rosbags.rosbag1)
:param topic: trajectory topic of supported message type,
or a TF trajectory ID (e.g.: '/tf:map.base_link' )
:return: trajectory.PoseTrajectory3D
"""
if not isinstance(reader, (Rosbag1Reader, Rosbag2Reader)):
raise FileInterfaceException(
"reader must be a rosbags.rosbags1.reader.Reader "
"or rosbags.rosbags2.reader.Reader - "
"rosbag.Bag() is not supported by evo anymore")
# TODO: Support TF also with ROS2 bags.
if tf_id.check_id(topic):
if isinstance(reader, Rosbag1Reader):
# Use TfCache instead if it's a TF transform ID.
from evo.tools import tf_cache
return tf_cache.instance().get_trajectory(reader, identifier=topic)
else:
raise FileInterfaceException(
"TF support for ROS2 bags is not implemented")
if topic not in reader.topics:
raise FileInterfaceException("no messages for topic '" + topic +
"' in bag")
msg_type = reader.topics[topic].msgtype
if msg_type not in SUPPORTED_ROS_MSGS:
raise FileInterfaceException(
"unsupported message type: {}".format(msg_type))
# Choose appropriate message conversion.
if msg_type == "geometry_msgs/msg/TransformStamped":
get_xyz_quat = _get_xyz_quat_from_transform_stamped
else:
get_xyz_quat = _get_xyz_quat_from_pose_or_odometry_msg
stamps, xyz, quat = [], [], []
connections = [c for c in reader.connections if c.topic == topic]
for connection, _, rawdata in reader.messages(
connections=connections): # type: ignore
if isinstance(reader, Rosbag1Reader):
msg = deserialize_cdr(ros1_to_cdr(rawdata, connection.msgtype),
connection.msgtype)
else:
msg = deserialize_cdr(rawdata, connection.msgtype)
# Use the header timestamps (converted to seconds).
# Note: msg/stamp is a rosbags type here, not native ROS.
t = msg.header.stamp
stamps.append(t.sec + (t.nanosec * 1e-9))
xyz_t, quat_t = get_xyz_quat(msg)
xyz.append(xyz_t)
quat.append(quat_t)
logger.debug("Loaded {} {} messages of topic: {}".format(
len(stamps), msg_type, topic))
# yapf: disable
(connection, _, rawdata) = list(reader.messages(connections=connections))[0] # type: ignore
# yapf: enable
if isinstance(reader, Rosbag1Reader):
first_msg = deserialize_cdr(ros1_to_cdr(rawdata, connection.msgtype),
connection.msgtype)
else:
first_msg = deserialize_cdr(rawdata, connection.msgtype)
frame_id = first_msg.header.frame_id
return PoseTrajectory3D(np.array(xyz), np.array(quat), np.array(stamps),
meta={"frame_id": frame_id})
def fake_trajectory(length, timestamp_distance):
return PoseTrajectory3D(poses_se3=random_se3_list(length),
timestamps=fake_timestamps(length,
timestamp_distance))
