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
#!/usr/bin/env python
from __future__ import print_function
print("loading required evo modules")
from evo.core import trajectory, sync, metrics
from evo.tools import file_interface
import rosbag
print("checking if bag has readable topics")
bag_handle = rosbag.Bag(
"/media/devavratjivani/New Volume/Ubuntu18_ExtendedStorage/results_slam/bags_stereo/2021-03-25-15-57-38.bag",
'r')
print(file_interface.get_supported_topics(bag_handle))
print("loading trajectories")
traj_ref = file_interface.read_bag_trajectory(bag_handle, "/tf:map.cam00")
traj_est = file_interface.read_bag_trajectory(bag_handle,
"/tf:map.camera_link")
print("registering and aligning trajectories")
traj_ref, traj_est = sync.associate_trajectories(traj_ref, traj_est)
traj_est = trajectory.align_trajectory(traj_est, traj_ref, correct_scale=False)
print("calculating APE")
data = (traj_ref, traj_est)
ape_metric = metrics.APE(metrics.PoseRelation.translation_part)
ape_metric.process_data(data)
ape_statistics = ape_metric.get_all_statistics()
print("mean:", ape_statistics["mean"])
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
