def read_bag_trajectory(bag_handle, topic):
"""
:param bag_handle: opened bag handle, from rosbag.Bag(...)
:param topic: trajectory topic of supported message type
: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 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_robotcar_poses_file(file_path):
"""
parses pose file in KITTI format (first 3 rows of SE(3) matrix per line)
:param file_path: the trajectory file path (or file handle)
:return: trajectory.PosePath3D
"""
raw_mat = csv_read_matrix(file_path, delim=" ", comment_str="#")
error_msg = ("KITTI pose files must have 13 entries per row "
"and no trailing delimiter at the end of the rows (space)")
if len(raw_mat) > 0 and len(raw_mat[0]) != 13:
raise FileInterfaceException(error_msg)
try:
mat = np.array(raw_mat).astype(float)
except ValueError:
raise FileInterfaceException(error_msg)
# yapf: disable
stamps = np.divide(mat[:, 0], 1e6)
poses = [np.array([[r[1], r[2], r[3], r[4]],
[r[5], r[6], r[7], r[8]],
[r[9], r[10], r[11], r[12]],
[0, 0, 0, 1]]) for r in mat]
# yapf: enable
if not hasattr(file_path, 'read'): # if not file handle
logger.debug("Loaded {} poses from: {}".format(len(poses), file_path))
return PoseTrajectory3D(poses_se3=poses, timestamps=stamps)
def lookup_trajectory(
self, parent_frame: str, child_frame: str, start_time: rospy.Time,
end_time: rospy.Time,
lookup_frequency: float = SETTINGS.tf_cache_lookup_frequency
) -> PoseTrajectory3D:
"""
Look up the trajectory of a transform chain from the cache's TF buffer.
:param parent_frame, child_frame: TF transform frame IDs
:param start_time, end_time: expected start and end time of the
trajectory in the buffer
:param lookup_frequency: frequency of TF lookups between start and end
time, in Hz.
"""
stamps, xyz, quat = [], [], []
step = rospy.Duration.from_sec(1. / lookup_frequency)
# Look up the transforms of the trajectory in reverse order:
while end_time >= start_time:
try:
tf = self.buffer.lookup_transform_core(parent_frame,
child_frame, end_time)
except tf2_py.ExtrapolationException:
break
stamps.append(tf.header.stamp.to_sec())
x, q = _get_xyz_quat_from_transform_stamped(tf)
xyz.append(x)
quat.append(q)
end_time = end_time - step
# Flip the data order again for the final trajectory.
trajectory = PoseTrajectory3D(np.flipud(xyz), np.flipud(quat),
np.flipud(stamps))
trajectory.meta = {
"frame_id": parent_frame,
"child_frame_id": child_frame
}
return trajectory
def read_bag_trajectory(bag_handle, topic):
"""
:param bag_handle: opened bag handle, from rosbag.Bag(...)
:param topic: geometry_msgs/PoseStamped 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", "nav_msgs/Odometry"}:
raise FileInterfaceException(
"unsupported message type: {}".format(msg_type))
stamps, xyz, quat = [], [], []
for topic, msg, t in bag_handle.read_messages(topic):
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 merge(tracks):
"""
Merges multiple tracks into a single, timestamp-sorted one.
:param tracks: list of PoseTrajectory3D objects
:return: merged PoseTrajectory3D
"""
merged_stamps = np.concatenate([t.timestamps for t in tracks])
merged_xyz = np.concatenate([t.positions_xyz for t in tracks])
merged_length = np.concatenate([t.length for t in tracks])
merged_width = np.concatenate([t.width for t in tracks])
merged_angular_vel = np.concatenate([t.angular_vel for t in tracks])
merged_linear_vel = np.concatenate([t.linear_vel for t in tracks])
merged_angular_vel = np.concatenate([t.angular_vel for t in tracks])
merged_quat = np.concatenate([t.orientations_quat_wxyz for t in tracks])
order = merged_stamps.argsort()
merged_stamps = merged_stamps[order]
merged_xyz = merged_xyz[order]
merged_quat = merged_quat[order]
merged_linear_vel = merged_linear_vel[order]
merged_angular_vel = merged_angular_vel[order]
merged_length = merged_length[order]
merged_width = merged_width[order]
return PoseTrajectory3D(merged_xyz,
merged_quat,
merged_stamps,
linear_vel=merged_linear_vel,
angular_vel=merged_angular_vel,
length=merged_length,
width=merged_width)
def read_neolix_rtk_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 = ("NEOLIX trajectory files must have 13 entries per row "
"and no trailing delimiter at the end of the rows (space)")
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 = mat[:, 1] # n x 1
xyz = mat[:, 4:7] # n x 3
quat = mat[:, 7:11] # 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
])
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 kitti_poses_and_timestamps_to_trajectory(poses_file, timestamp_file):
pose_path = file_interface.read_kitti_poses_file(poses_file)
raw_timestamps_mat = file_interface.csv_read_matrix(timestamp_file)
error_msg = ("timestamp file must have one column of timestamps and same number of rows as the KITTI poses file")
if len(raw_timestamps_mat) > 0 and len(raw_timestamps_mat[0]) != 1 or len(raw_timestamps_mat) != pose_path.num_poses:
raise file_interface.FileInterfaceException(error_msg)
try:
timestamps_mat = np.array(raw_timestamps_mat).astype(float)
except ValueError:
raise file_interface.FileInterfaceException(error_msg)
return PoseTrajectory3D(poses_se3=pose_path.poses_se3, timestamps=timestamps_mat)
def convert_neolix_2_utm(odometry_file, utm_file, begin_timestamp=-1.0, end_timestamp=1e15):
odo_trajectory, _ = file_interface.read_neolix_trajectory_file(odometry_file)
timestamps = np.array(odo_trajectory.timestamps, dtype=float64)
valid_index = np.where((timestamps>begin_timestamp) & (timestamps < end_timestamp))
# valid_index = np.array(valid_index, dtype=int32)
# print valid_index.shape[0], valid_index.shape[1]
xyz = odo_trajectory.positions_xyz[valid_index]
quat = odo_trajectory.orientations_quat_wxyz[valid_index]
timestamps = timestamps[valid_index]
file_interface.write_tum_trajectory_file(utm_file, PoseTrajectory3D(xyz, quat, timestamps))
def test_alignment_degenerate_case(self):
length = 100
poses = [lie.random_se3()] * length
traj_1 = PoseTrajectory3D(poses_se3=poses,
timestamps=helpers.fake_timestamps(
length, 1, 0.0))
traj_2 = copy.deepcopy(traj_1)
traj_2.transform(lie.random_se3())
traj_2.scale(1.234)
self.assertNotEqual(traj_1, traj_2)
with self.assertRaises(GeometryException):
traj_1.align(traj_2)
with self.assertRaises(GeometryException):
traj_1.align(traj_2, correct_scale=True)
def read_TrackArray(bag_handle, topic, min_length):
"""
:param bag_handle: opened bag handle, from rosbag.Bag(...)
:param topic: trajectory topic of supported message type
:param min_length: minimum length of msgs that appended to the list
:return: List of trajectory.PoseTrajectory3D
"""
get_xyz_quat = _get_xyz_quat_from_pose_or_odometry_msg
get_twist = _get_twist_from_odometry_msg
# Make list of unique ids and copy msgs
ids = []
msgs = []
for topic, msg, t in bag_handle.read_messages(topic):
msgs.append(msg)
for i in range(0,len(msg.tracks)):
ids.append((msg.tracks[i].id))
frame_id = "map"
unique_ids = unique(ids)
list_tracks = []
for id in unique_ids:
stamps, xyz, quat = [], [], []
linear, angular = [], []
length, width = [], []
pose_covar, twist_covar = [], []
for msg in msgs:
for i in range(0,len(msg.tracks)):
if msg.tracks[i].id == id:
t = msg.tracks[i].odom.header.stamp
stamps.append(t.secs + (t.nsecs * 1e-9))
xyz_t, quat_t = get_xyz_quat(msg.tracks[i].odom.pose)
xyz.append(xyz_t)
quat.append(quat_t)
linear_t, angular_t = get_twist(msg.tracks[i].odom)
linear.append(linear_t)
angular.append(angular_t)
length.append(msg.tracks[i].length)
width.append(msg.tracks[i].width)
pose_covar.append(msg.tracks[i].odom.pose.covariance)
twist_covar.append(msg.tracks[i].odom.twist.covariance)
if len(stamps)>min_length:
list_tracks.append(PoseTrajectory3D(xyz, quat, stamps,\
meta={"frame_id":
frame_id},linear_vel=linear, angular_vel=angular, length =
length, width = width, twist_covariance = twist_covar))
return list_tracks
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
"""
mat = np.array(csv_read_matrix(file_path, delim=",",
comment_str="#")).astype(float)
if mat.shape[1] != 17:
raise FileInterfaceException(
"EuRoC MAV state ground truth must have 17 entries per row")
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_pose_csv_trajectory(file_path):
"""
parses trajectory file in pose format (timestamp[ns], x, y, z, qw, qx, qy, qz)
:param file_path: the trajectory file path (or file handle)
:return: trajectory.PoseTrajectory3D object
"""
mat = np.array(csv_read_matrix(file_path, delim=",",
comment_str="#")).astype(float)
if mat.shape[1] != 8:
raise FileInterfaceException(
"Pose trajectory files must have 8 entries per row")
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
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 lookup_trajectory(
self,
parent_frame: str,
child_frame: str,
start_time: rospy.Time,
end_time: rospy.Time,
lookup_frequency: float = SETTINGS.tf_cache_lookup_frequency
) -> PoseTrajectory3D:
"""
Look up the trajectory of a transform chain from the cache's TF buffer.
:param parent_frame, child_frame: TF transform frame IDs
:param start_time, end_time: expected start and end time of the
trajectory in the buffer
:param lookup_frequency: frequency of TF lookups between start and end
time, in Hz.
"""
stamps, xyz, quat = [], [], []
step = rospy.Duration.from_sec(1. / lookup_frequency)
# Static TF have zero timestamp in the buffer, which will be lower
# than the bag start time. Looking up a static TF is a valid request,
# so this should be possible.
attempt_single_static_lookup = end_time.to_sec() == 0.
# Look up the transforms of the trajectory in reverse order:
while end_time >= start_time or attempt_single_static_lookup:
try:
tf = self.buffer.lookup_transform_core(parent_frame,
child_frame, end_time)
except tf2_py.ExtrapolationException:
break
stamps.append(tf.header.stamp.to_sec())
x, q = _get_xyz_quat_from_transform_stamped(tf)
xyz.append(x)
quat.append(q)
if attempt_single_static_lookup:
break
end_time = end_time - step
# Flip the data order again for the final trajectory.
trajectory = PoseTrajectory3D(np.flipud(xyz), np.flipud(quat),
np.flipud(stamps))
trajectory.meta = {
"frame_id": parent_frame,
"child_frame_id": child_frame
}
return trajectory
def read_bag_trajectory(bag_handle, topic: str) -> PoseTrajectory3D:
"""
: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(np.array(xyz),
np.array(quat),
np.array(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
"""
mat = np.array(csv_read_matrix(file_path, delim=" ",
comment_str="#")).astype(float)
if mat.shape[1] != 8:
raise FileInterfaceException(
"TUM trajectory files must have 8 entries per row")
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_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 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 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 fake_trajectory(length, timestamp_distance):
return PoseTrajectory3D(poses_se3=random_se3_list(length),
timestamps=fake_timestamps(length,
timestamp_distance))
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})