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 load_trajectories(args):
from evo.core import sync
from evo.tools import file_interface
if args.subcommand == "tum":
traj_ref = file_interface.read_tum_trajectory_file(args.ref_file)
traj_est = file_interface.read_tum_trajectory_file(args.est_file)
ref_name, est_name = args.ref_file, args.est_file
elif args.subcommand == "kitti":
traj_ref = file_interface.read_kitti_poses_file(args.ref_file)
traj_est = file_interface.read_kitti_poses_file(args.est_file)
ref_name, est_name = args.ref_file, args.est_file
elif args.subcommand == "euroc":
traj_ref = file_interface.read_euroc_csv_trajectory(args.state_gt_csv)
traj_est = file_interface.read_tum_trajectory_file(args.est_file)
ref_name, est_name = args.state_gt_csv, args.est_file
elif args.subcommand == "bag":
import os
logger.debug("Opening bag file " + args.bag)
if not os.path.exists(args.bag):
raise file_interface.FileInterfaceException(
"File doesn't exist: {}".format(args.bag))
import rosbag
bag = rosbag.Bag(args.bag, 'r')
try:
traj_ref = file_interface.read_bag_trajectory(bag, args.ref_topic)
traj_est = file_interface.read_bag_trajectory(bag, args.est_topic)
ref_name, est_name = args.ref_topic, args.est_topic
finally:
bag.close()
else:
raise KeyError("unknown sub-command: {}".format(args.subcommand))
return traj_ref, traj_est, ref_name, est_name
def load_trajectories(args):
import os
from collections import OrderedDict
from evo.tools import file_interface
trajectories = OrderedDict()
ref_traj = None
if args.subcommand == "tum":
for traj_file in args.traj_files:
if traj_file == args.ref:
continue
trajectories[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:
continue
trajectories[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:
continue
else:
trajectories[
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":
if not (args.topics or args.all_topics):
die("No topics used - specify topics or set --all_topics.")
if not os.path.exists(args.bag):
raise file_interface.FileInterfaceException(
"File doesn't exist: {}".format(args.bag))
import rosbag
bag = rosbag.Bag(args.bag)
try:
if args.all_topics:
topics = file_interface.get_supported_topics(bag)
if args.ref in topics:
topics.remove(args.ref)
if len(topics) == 0:
die("No topics of supported types: {}".format(" ".join(
file_interface.SUPPORTED_ROS_MSGS)))
else:
topics = args.topics
for topic in topics:
if topic == args.ref:
continue
trajectories[topic] = file_interface.read_bag_trajectory(
bag, topic)
if args.ref:
ref_traj = file_interface.read_bag_trajectory(bag, args.ref)
finally:
bag.close()
return trajectories, ref_traj
def load_trajectories(
args: argparse.Namespace
) -> typing.Tuple[PosePath3D, PosePath3D, str, str]:
from evo.tools import file_interface
traj_ref: typing.Union[PosePath3D, PoseTrajectory3D]
traj_est: typing.Union[PosePath3D, PoseTrajectory3D]
if args.subcommand == "tum":
traj_ref = file_interface.read_tum_trajectory_file(args.ref_file)
traj_est = file_interface.read_tum_trajectory_file(args.est_file)
ref_name, est_name = args.ref_file, args.est_file
elif args.subcommand == "kitti":
traj_ref = file_interface.read_kitti_poses_file(args.ref_file)
traj_est = file_interface.read_kitti_poses_file(args.est_file)
ref_name, est_name = args.ref_file, args.est_file
elif args.subcommand == "euroc":
traj_ref = file_interface.read_euroc_csv_trajectory(args.state_gt_csv)
traj_est = file_interface.read_tum_trajectory_file(args.est_file)
ref_name, est_name = args.state_gt_csv, args.est_file
elif args.subcommand in ("bag", "bag2"):
import os
logger.debug("Opening bag file " + args.bag)
if not os.path.exists(args.bag):
raise file_interface.FileInterfaceException(
"File doesn't exist: {}".format(args.bag))
if args.subcommand == "bag2":
from rosbags.rosbag2 import Reader as Rosbag2Reader
bag = Rosbag2Reader(args.bag) # type: ignore
else:
from rosbags.rosbag1 import Reader as Rosbag1Reader
bag = Rosbag1Reader(args.bag) # type: ignore
try:
bag.open()
traj_ref = file_interface.read_bag_trajectory(bag, args.ref_topic)
traj_est = file_interface.read_bag_trajectory(bag, args.est_topic)
ref_name, est_name = args.ref_topic, args.est_topic
finally:
bag.close()
else:
raise KeyError("unknown sub-command: {}".format(args.subcommand))
return traj_ref, traj_est, ref_name, est_name
def load_trajectories(args):
from collections import OrderedDict
from evo.tools import file_interface
trajectories = OrderedDict()
ref_traj = None
if args.subcommand == "tum":
for traj_file in args.traj_files:
if traj_file == args.ref:
continue
trajectories[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:
continue
trajectories[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:
continue
else:
trajectories[
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 in ("bag", "bag2"):
if not (args.topics or args.all_topics):
die("No topics used - specify topics or set --all_topics.")
if not os.path.exists(args.bag):
raise file_interface.FileInterfaceException(
"File doesn't exist: {}".format(args.bag))
logger.debug("Opening bag file " + args.bag)
if args.subcommand == "bag2":
from rosbags.rosbag2 import Reader as Rosbag2Reader
bag = Rosbag2Reader(args.bag)
else:
from rosbags.rosbag1 import Reader as Rosbag1Reader
bag = Rosbag1Reader(args.bag)
bag.open()
try:
if args.all_topics:
# Note: args.topics can have TF stuff here, so we add it too.
topics = args.topics
topics += natsorted(file_interface.get_supported_topics(bag))
if args.ref in topics:
topics.remove(args.ref)
if len(topics) == 0:
die("No topics of supported types: {}".format(" ".join(
file_interface.SUPPORTED_ROS_MSGS)))
else:
topics = args.topics
for topic in topics:
if topic == args.ref:
continue
trajectories[topic] = file_interface.read_bag_trajectory(
bag, topic)
if args.ref:
ref_traj = file_interface.read_bag_trajectory(bag, args.ref)
finally:
bag.close()
return trajectories, ref_traj
