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))