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):
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):
from evo.tools import file_interface
trajectories = {}
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.")
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] in {
"geometry_msgs/PoseStamped",
"geometry_msgs/PoseWithCovarianceStamped",
"nav_msgs/Odometry"
} and t != args.ref
])
if len(topics) == 0:
die("No geometry_msgs/PoseStamped, "
"geometry_msgs/PoseWithCovarianceStamped or "
"nav_msgs/Odometry topics found!")
else:
topics = args.topics
for topic in topics:
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):
from evo.tools import file_interface
trajectories = {}
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.")
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] in file_interface.SUPPORTED_ROS_MSGS
and t != 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:
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 eval(ref_p_path, ref_t_path, est_p_path, est_t_path):
#pose1
ref_pose = file_interface.read_kitti_poses_file(ref_p_path) #PosePath3D
ref_time = np.loadtxt(ref_t_path) #time
assert len(ref_time) == ref_pose.num_poses
print(ref_p_path)
ref_traj = path2trajectory(ref_pose, ref_time) #PoseTrajectory3D
#pose2
est_pose = file_interface.read_kitti_poses_file(est_p_path) #PosePath3D
est_time = np.loadtxt(est_t_path) #time
assert len(est_time) == est_pose.num_poses
print(est_p_path)
est_traj = path2trajectory(est_pose, est_time) #PoseTrajectory3D
def eval(ref_p_path, ref_t_path, est_p_path, est_t_path):
#pose1
ref_pose = file_interface.read_kitti_poses_file(ref_p_path) #PosePath3D
ref_time = np.loadtxt(ref_t_path) #time
assert len(ref_time) == ref_pose.num_poses
ref_traj = path2trajectory(ref_pose, ref_time) #PoseTrajectory3D
#pose2
est_pose = file_interface.read_kitti_poses_file(est_p_path) #PosePath3D
est_time = np.loadtxt(est_t_path) #time
assert len(est_time) == est_pose.num_poses
est_traj = path2trajectory(est_pose, est_time) #PoseTrajectory3D
#print(ref_traj.get_infos(),est_traj.get_infos())
#sync-基于少量数据同步
max_diff = 0.1 # 雷达10Hz， 最大为
traj_ref, traj_est = sync.associate_trajectories(ref_traj, est_traj,
max_diff)
data = (traj_ref, traj_est)
#APE--全局评估
ape_pose_relation = metrics.PoseRelation.full_transformation
ape_statis, ape_err = get_ape_static_raw(data, ape_pose_relation)
ape_err = np.asarray(ape_err)
#print(ape_statis,ape_total)
#RPE-细节评估
rpe_relation_t = metrics.PoseRelation.translation_part
rpe_relation_r = metrics.PoseRelation.rotation_part
"""
frame 一般不涉及单位，可以进行逐帧对比 取值delta=1，不太影响评估误差数量
使用frame，1，利用最小粒度单位进行详细评估，寻找异常帧位置，从而修改代码。
meters 可以通过设置10/20/50/100 m ，会缩小评估误差数量
degrees=radians, filter_pairs_by_angle
"""
delta = 20 #评估窗口
delta_unit = metrics.Unit.meters #评估单位 [meters,frames,degrees]
all_pairs = False
rpe_statis_t, rpe_trans = get_rpe_static_raw(data, rpe_relation_t, delta,
delta_unit,
all_pairs) #translation
rpe_statis_r, rpe_rotat = get_rpe_static_raw(data, rpe_relation_r, delta,
delta_unit,
all_pairs) #rotation
#
static = np.vstack((ape_statis, rpe_statis_t, rpe_statis_r)) # 固定统计类，可合并
rpe_err = np.vstack((rpe_trans, rpe_rotat)) #数据不等长，不可合并
return static, ape_err, rpe_err
def test_write_read_integrity(self):
traj_out = helpers.fake_path(1000)
self.assertTrue(traj_out.check())
file_interface.write_kitti_poses_file(self.mock_file, traj_out)
self.mock_file.seek(0)
traj_in = file_interface.read_kitti_poses_file(self.mock_file)
self.assertIsInstance(traj_in, PosePath3D)
self.assertTrue(traj_in.check())
self.assertTrue(traj_out == traj_in)
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 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":
args.align = True
logger.info("Forcing trajectory alignment implicitly "
"(EuRoC ground truth is in IMU frame).")
logger.debug(SEP)
traj_ref = file_interface.read_euroc_csv_trajectory(args.state_gt_csv)
traj_est = file_interface.read_euroc_csv_trajectory(args.est_file)
ref_name, est_name = args.state_gt_csv, args.est_file
elif args.subcommand == "bag":
import rosbag
logger.debug("Opening bag file " + args.bag)
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))
if args.subcommand != "kitti":
logger.debug("Synchronizing trajectories...")
traj_ref, traj_est = sync.associate_trajectories(traj_ref,
traj_est,
args.t_max_diff,
args.t_offset,
first_name=ref_name,
snd_name=est_name)
return traj_ref, traj_est, ref_name, est_name
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":
args.align = True
logger.info("Forcing trajectory alignment implicitly "
"(EuRoC ground truth is in IMU frame).")
logger.debug(SEP)
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 rosbag
logger.debug("Opening bag file " + args.bag)
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))
if args.subcommand != "kitti":
logger.debug("Synchronizing trajectories...")
traj_ref, traj_est = sync.associate_trajectories(
traj_ref, traj_est, args.t_max_diff, args.t_offset,
first_name=ref_name, snd_name=est_name)
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
def main():
hvd.init()
args = parser.parse_args()
if hvd.size() > 1:
printcolor('----------- DISTRIBUTED DATA PARALLEL -----------', 'cyan')
device_id = hvd.local_rank()
torch.cuda.set_device(device_id)
if hvd.rank() == 0:
os.makedirs(args.output_dir, exist_ok=True)
depth_net = DispResnet(version='18_pt')
depth_net.load_state_dict(torch.load(args.depth_model, map_location='cpu'))
depth_net = depth_net.cuda() # move to GPU
keypoint_net = KeypointResnet()
keypoint_net.load_state_dict(
torch.load(args.keypoint_model, map_location='cpu'))
keypoint_net.cuda()
def _set_seeds(seed=42):
"""Set Python random seeding and PyTorch seeds.
Parameters
----------
seed: int, default: 42
Random number generator seeds for PyTorch and python
"""
np.random.seed(seed)
random.seed(seed)
torch.manual_seed(seed)
torch.cuda.manual_seed_all(seed)
def _worker_init_fn(worker_id):
"""Worker init fn to fix the seed of the workers"""
_set_seeds(42 + worker_id)
if args.run_evaluation:
all_trel = []
all_rrel = []
########## STACK PREDICTED POSES INTO FULL TRAJECTORY ##########
printcolor('Evaluation sequences {}'.format(args.sequence))
for seq in args.sequence:
dataset = KITTIODOMETRYDataset(
args.dataset_dir,
[seq],
back_context=0,
forward_context=1,
image_shape=(args.input_height, args.input_width),
)
trajectory_length = sum(len(imgs) for imgs in dataset.img_files)
printcolor('Evaluating SEQ {}'.format(seq))
sampler = None if not (hvd.size() > 1) else \
torch.utils.data.distributed.DistributedSampler(
dataset, num_replicas=hvd.size(), rank=hvd.rank())
dl = DataLoader(dataset,
batch_size=1,
pin_memory=False,
shuffle=False,
num_workers=48,
worker_init_fn=_worker_init_fn,
sampler=sampler)
all_poses = accumulate_pose_single_frame(
dataloader=dl,
depth_net=depth_net,
keypoint_net=keypoint_net,
trajectory_length=trajectory_length,
rank=hvd.rank())
if hvd.rank() == 0:
np.savetxt(os.path.join(args.output_dir,
str(seq) + '_ori.txt'),
all_poses.reshape(trajectory_length, -1)[:, :12])
if args.align_trajectory:
if hvd.rank() == 0:
printcolor('Computing scale alignment.')
# Load ground truth poses
ground_truth_trajectory = file_interface.read_kitti_poses_file(
os.path.join(os.path.join(args.dataset_dir, 'poses'), seq)
+ '.txt')
# Convert predicted trajectory
predicted_trajectory = PosePath3D(poses_se3=all_poses)
# Umeyama alignment with scaling only
predicted_trajectory_aligned = copy.deepcopy(
predicted_trajectory)
predicted_trajectory_aligned.align(ground_truth_trajectory,
correct_only_scale=True)
# Save aligned trajectory
file_interface.write_kitti_poses_file(
os.path.join(args.output_dir, seq) + '.txt',
predicted_trajectory_aligned)
if args.run_evaluation and hvd.rank() == 0:
pose_test_executable = "./kp3d/externals/cpp/evaluate_odometry"
p = subprocess.Popen(
[pose_test_executable, args.dataset_dir, args.output_dir, seq],
stdout=subprocess.PIPE,
stderr=subprocess.PIPE)
p.wait()
seq_output = os.path.join(args.output_dir, seq + '_stats.txt')
with open(seq_output, 'r') as f:
pose_result = f.readlines()[0].split(' ')
pose_result = [float(p) for p in pose_result]
traj_pos_error = pose_result[0]
traj_rot_error = pose_result[1]
all_trel.append(traj_pos_error)
all_rrel.append(traj_rot_error)
printcolor(
'SEQ {} -- Mean TRAJ (pos: {:.4f}, rot: {:.4f})'.format(
seq, traj_pos_error, traj_rot_error), 'green')
printcolor(all_trel)
printcolor(all_rrel)
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 run(args):
import os
import sys
import logging
import evo.algorithms.lie_algebra as lie
from evo.algorithms import trajectory
from evo.tools import file_interface, settings
from evo.tools.settings import SETTINGS
settings.configure_logging(verbose=True,
silent=args.silent,
debug=args.debug)
if args.debug:
import pprint
logging.debug(
"main_parser config:\n" +
pprint.pformat({arg: getattr(args, arg)
for arg in vars(args)}) + "\n")
logging.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:
logging.error("no geometry_msgs/PoseStamped topics found!")
sys.exit(1)
else:
topics = args.topics
if not topics:
logging.warning(
"no topics used - specify topics or use the --all_topics flag"
)
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.transform_left or args.transform_right:
tf_path = args.transform_left if args.transform_left else args.transform_right
t, xyz, quat = file_interface.load_transform_json(tf_path)
logging.debug(SEP)
logging.debug("applying transformation to the trajectories:\n" +
str(t))
if args.invert_transform:
t = lie.se3_inverse(t)
for name, traj in trajectories:
traj.transform(t, right_mul=args.transform_right)
if args.align or args.correct_scale:
if args.ref:
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.algorithms import sync
for name, traj in trajectories:
logging.debug(SEP)
ref_assoc, traj_assoc = sync.associate_trajectories(
ref_traj, traj, 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):
logging.debug(SEP)
logging.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:
if args.t_offset and traj.timestamps.shape[0] != 0:
logging.debug(SEP)
logging.info("adding time offset to " + name + ": " +
str(args.t_offset) + " (s)")
traj.timestamps += args.t_offset
print_traj_info(name, traj, args.full_check)
if (args.align or args.correct_scale) and not args.ref:
logging.debug(SEP)
logging.warning("can't align without a reference! (--ref) *grunt*")
if args.ref:
print_traj_info(args.ref, ref_traj, 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_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)
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)
plt.tight_layout()
plot_collection.add_figure("trajectories", fig_traj)
plot_collection.add_figure("xyz_view", fig_xyz)
if args.plot:
plot_collection.show()
if args.save_plot:
logging.debug(SEP)
plot_collection.export(args.save_plot,
confirm_overwrite=not args.no_warnings)
if args.serialize_plot:
logging.debug(SEP)
plot_collection.serialize(args.serialize_plot,
confirm_overwrite=not args.no_warnings)
if args.save_as_tum:
logging.debug(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:
logging.debug(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:
logging.debug(SEP)
import datetime
import rosbag
dest_bag_path = str(
datetime.datetime.now().strftime('%Y-%m-%d_%H:%M:%S.%f')) + ".bag"
logging.debug("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]
file_interface.write_bag_trajectory(bag, traj, dest_topic)
if args.ref:
dest_topic = os.path.splitext(os.path.basename(args.ref))[0]
file_interface.write_bag_trajectory(bag, ref_traj, dest_topic)
finally:
bag.close()
def test_too_few_columns_with_trailing_delim(self):
self.mock_file.write(u"1 2 3 4 5 6 7 8 9 10 11 ")
self.mock_file.seek(0)
with self.assertRaises(file_interface.FileInterfaceException):
file_interface.read_kitti_poses_file(self.mock_file)
def run(args):
import sys
from evo.algorithms import metrics
from evo.tools import file_interface, settings
# manually check bins and tols arguments to allow them to be in config files
if not args.bins or not args.tols:
logging.error(
"the following arguments are required: -b/--bins, -t/--tols")
sys.exit(1)
settings.configure_logging(args.verbose, args.silent, args.debug)
if args.debug:
import pprint
logging.debug(
"main_parser config:\n" +
pprint.pformat({arg: getattr(args, arg)
for arg in vars(args)}) + "\n")
logging.debug(SEP)
pose_relation = None
if args.pose_relation == "trans_part":
pose_relation = metrics.PoseRelation.translation_part
elif args.pose_relation == "angle_deg":
pose_relation = metrics.PoseRelation.rotation_angle_deg
elif args.pose_relation == "angle_rad":
pose_relation = metrics.PoseRelation.rotation_angle_rad
traj_ref, traj_est, stamps_est = None, None, None
ref_name, est_name = "", ""
if args.subcommand == "tum":
traj_ref, traj_est = file_interface.load_assoc_tum_trajectories(
args.ref_file,
args.est_file,
args.t_max_diff,
args.t_offset,
)
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":
args.align = True
logging.info(
"forcing trajectory alignment implicitly (EuRoC ground truth is in IMU frame)"
)
logging.debug(SEP)
traj_ref, traj_est = file_interface.load_assoc_euroc_trajectories(
args.state_gt_csv,
args.est_file,
args.t_max_diff,
args.t_offset,
)
ref_name, est_name = args.state_gt_csv, args.est_file
elif args.subcommand == "bag":
import rosbag
logging.debug("opening bag file " + args.bag)
bag = rosbag.Bag(args.bag, 'r')
try:
traj_ref, traj_est = file_interface.load_assoc_bag_trajectories(
bag,
args.ref_topic,
args.est_topic,
args.t_max_diff,
args.t_offset,
)
finally:
bag.close()
ref_name, est_name = args.ref_topic, args.est_topic
main_rpe_for_each(traj_ref,
traj_est,
pose_relation,
args.mode,
args.bins,
args.tols,
args.align,
args.correct_scale,
ref_name,
est_name,
args.plot,
args.save_plot,
args.save_results,
args.no_warnings,
serialize_plot=args.serialize_plot)
def test_trailing_delim(self):
self.mock_file.write(u"1 0 0 0.1 0 1 0 0.2 0 0 1 0.3 ")
self.mock_file.seek(0)
with self.assertRaises(file_interface.FileInterfaceException):
file_interface.read_kitti_poses_file(self.mock_file)
import copy
import numpy as np
import context
from evo.core import trajectory, geometry
from evo.core import lie_algebra as lie
from evo.tools import file_interface
# use absolute paths!
here = os.path.dirname(os.path.abspath(__file__))
tum_traj_file = os.path.join(here, "data/fr2_desk_ORB.txt")
kitti_traj_file = os.path.join(here, "data/KITTI_00_gt.txt")
ex_tum_traj = file_interface.read_tum_trajectory_file(tum_traj_file)
ex_kitti_traj = file_interface.read_kitti_poses_file(kitti_traj_file)
ex_kitti_traj_wrong = copy.deepcopy(ex_kitti_traj)
ex_kitti_traj_wrong._poses_se3 = [
np.zeros((4, 4)) for i in range(ex_kitti_traj.num_poses)
]
ex_tum_traj_wrong_quat = copy.deepcopy(ex_tum_traj)
ex_tum_traj_wrong_quat._orientations_quat_wxyz[3] = [5000, 0, 0, 0]
ex_tum_traj_wrong_stamps = copy.deepcopy(ex_tum_traj)
ex_tum_traj_wrong_stamps.timestamps = [1, 2, 3]
class TestPosePath3D(unittest.TestCase):
def test_init_wrong_args(self):
# no args
def run(args):
from evo.algorithms import metrics
from evo.tools import file_interface, settings
settings.configure_logging(args.verbose, args.silent, args.debug)
if args.debug:
import pprint
logging.debug(
"main_parser config:\n" +
pprint.pformat({arg: getattr(args, arg)
for arg in vars(args)}) + "\n")
logging.debug(SEP)
pose_relation = None
if args.pose_relation == "full":
pose_relation = metrics.PoseRelation.full_transformation
elif args.pose_relation == "rot_part":
pose_relation = metrics.PoseRelation.rotation_part
elif args.pose_relation == "trans_part":
pose_relation = metrics.PoseRelation.translation_part
elif args.pose_relation == "angle_deg":
pose_relation = metrics.PoseRelation.rotation_angle_deg
elif args.pose_relation == "angle_rad":
pose_relation = metrics.PoseRelation.rotation_angle_rad
traj_ref, traj_est, stamps_est = None, None, None
ref_name, est_name = "", ""
plot_mode = None # no plot imports unless really needed (slow)
if args.subcommand == "tum":
traj_ref, traj_est = file_interface.load_assoc_tum_trajectories(
args.ref_file,
args.est_file,
args.t_max_diff,
args.t_offset,
)
ref_name, est_name = args.ref_file, args.est_file
if args.plot or args.save_plot:
from evo.tools.plot import PlotMode
plot_mode = PlotMode.xyz if not args.plot_mode else PlotMode[
args.plot_mode]
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
if args.plot or args.save_plot:
from evo.tools.plot import PlotMode
plot_mode = PlotMode.xz if not args.plot_mode else PlotMode[
args.plot_mode]
elif args.subcommand == "euroc":
args.align = True
logging.info(
"forcing trajectory alignment implicitly (EuRoC ground truth is in IMU frame)"
)
logging.debug(SEP)
traj_ref, traj_est = file_interface.load_assoc_euroc_trajectories(
args.state_gt_csv,
args.est_file,
args.t_max_diff,
args.t_offset,
)
ref_name, est_name = args.state_gt_csv, args.est_file
if args.plot or args.save_plot:
from evo.tools.plot import PlotMode
plot_mode = PlotMode.xyz if not args.plot_mode else PlotMode[
args.plot_mode]
elif args.subcommand == "bag":
import rosbag
logging.debug("opening bag file " + args.bag)
bag = rosbag.Bag(args.bag, 'r')
try:
traj_ref, traj_est = file_interface.load_assoc_bag_trajectories(
bag,
args.ref_topic,
args.est_topic,
args.t_max_diff,
args.t_offset,
)
finally:
bag.close()
ref_name, est_name = args.ref_topic, args.est_topic
if args.plot or args.save_plot:
from evo.tools.plot import PlotMode
plot_mode = PlotMode.xy if not args.plot_mode else PlotMode[
args.plot_mode]
main_ape(traj_ref,
traj_est,
pose_relation,
args.align,
args.correct_scale,
ref_name,
est_name,
args.plot,
args.save_plot,
plot_mode,
args.save_results,
args.no_warnings,
serialize_plot=args.serialize_plot)
import logging
import sys
from evo.core import trajectory
from evo.tools import plot, file_interface
import evo.core.lie_algebra as lie
import numpy as np
import matplotlib.pyplot as plt
logging.basicConfig(format="%(message)s",
stream=sys.stdout,
level=logging.DEBUG)
traj_ref = file_interface.read_kitti_poses_file("../data/KITTI_00_gt.txt")
traj_est = file_interface.read_kitti_poses_file("../data/KITTI_00_ORB.txt")
# add artificial Sim(3) transformation
traj_est.transform(lie.se3(np.eye(3), [0, 0, 0]))
traj_est.scale(0.5)
logging.info("\nUmeyama alignment without scaling")
traj_est_aligned = trajectory.align_trajectory(traj_est, traj_ref)
logging.info("\nUmeyama alignment with scaling")
traj_est_aligned_scaled = trajectory.align_trajectory(traj_est,
traj_ref,
correct_scale=True)
logging.info("\nUmeyama alignment with scaling only")
traj_est_aligned_only_scaled = trajectory.align_trajectory(
traj_est, traj_ref, correct_only_scale=True)
along with evo. If not, see <http://www.gnu.org/licenses/>.
"""
from __future__ import print_function # Python 2.7 backwards compatibility
import unittest
import copy
import numpy as np
from evo.core import trajectory, geometry
from evo.core import lie_algebra as lie
from evo.tools import file_interface
ex_tum_traj = file_interface.read_tum_trajectory_file("data/fr2_desk_ORB.txt")
ex_kitti_traj = file_interface.read_kitti_poses_file("data/KITTI_00_gt.txt")
ex_kitti_traj_wrong = copy.deepcopy(ex_kitti_traj)
ex_kitti_traj_wrong._poses_se3 = [
np.zeros((4, 4)) for i in range(ex_kitti_traj.num_poses)
]
ex_tum_traj_wrong_quat = copy.deepcopy(ex_tum_traj)
ex_tum_traj_wrong_quat._orientations_quat_wxyz[3] = [5000, 0, 0, 0]
ex_tum_traj_wrong_stamps = copy.deepcopy(ex_tum_traj)
ex_tum_traj_wrong_stamps.timestamps = [1, 2, 3]
class TestPosePath3D(unittest.TestCase):
def test_init_wrong_args(self):
# no args
import logging
import sys
import evo.core.lie_algebra as lie
from evo.core import trajectory
from evo.tools import plot, file_interface, log
import numpy as np
import matplotlib.pyplot as plt
logger = logging.getLogger("evo")
log.configure_logging(verbose=True)
traj_ref = file_interface.read_kitti_poses_file("../test/data/KITTI_00_gt.txt")
traj_est = file_interface.read_kitti_poses_file("../test/data/KITTI_00_ORB.txt")
# add artificial Sim(3) transformation
traj_est.transform(lie.se3(np.eye(3), [0, 0, 0]))
traj_est.scale(0.5)
logger.info("\nUmeyama alignment without scaling")
traj_est_aligned = trajectory.align_trajectory(traj_est, traj_ref)
logger.info("\nUmeyama alignment with scaling")
traj_est_aligned_scaled = trajectory.align_trajectory(
traj_est, traj_ref, correct_scale=True)
logger.info("\nUmeyama alignment with scaling only")
traj_est_aligned_only_scaled = trajectory.align_trajectory(
def test_too_many_columns(self):
self.mock_file.write(u"1 2 3 4 5 6 7 8 9 10 11 12 13")
self.mock_file.seek(0)
with self.assertRaises(file_interface.FileInterfaceException):
file_interface.read_kitti_poses_file(self.mock_file)
def get_and_save_results_from_folder(folder_with_predicted_poses,category):
global args
global kitti_eval_tool
global folder_with_gt_poses
global output_folder
global t
global results
values_for_excel = []
columns_for_excel = []
type_of_statistics = 'mean'
for filename in sorted(os.listdir(folder_with_predicted_poses)):
if not(os.path.exists(os.path.join(folder_with_gt_poses, filename))):
print("file with gt poses doesn't exist for "+filename)
continue
if filename.find('.txt') == -1:
continue
seq_results = {}
seq_results['name_seq'] = filename[:filename.rfind('.')]
seq_results['category'] = category
folder_name = seq_results['category']
seq_results['metrics'] = {}
seq_results['lost'] = False
os.makedirs(os.path.join(output_folder, folder_name), exist_ok=True)
output_folder_seq = os.path.join(output_folder, folder_name, filename[:filename.rfind('.')])
os.makedirs(output_folder_seq, exist_ok=True)
if os.path.isfile(os.path.join(output_folder, folder_name,"results.txt")):
file_results_txt = open(os.path.join(output_folder, folder_name,"results.txt"), "a")
else:
file_results_txt = open(os.path.join(output_folder, folder_name,"results.txt"), "w")
file_results_txt.write("translation_error(%) rotation_error(deg/m) ATE(m) APE_translation_error_median(m) APE_rotation_error_median(deg) dst_to_trgt\n")
# -------------------------------------Getting results---------------------------------------------------
if args.gt_format == 'kitti':
traj_ref = file_interface.read_kitti_poses_file(os.path.join(folder_with_gt_poses, filename))
if args.gt_format == 'tum':
traj_ref = file_interface.read_tum_trajectory_file(os.path.join(folder_with_gt_poses, filename))
seq_results["length_of_ref_traj"] = traj_ref.path_length
end_time_gt = traj_ref.get_infos()["t_end (s)"]
if args.gt_format == 'euroc':
traj_ref = file_interface.read_euroc_csv_trajectory(os.path.join(folder_with_gt_poses, filename))
if args.result_format == 'kitti':
traj_est = file_interface.read_kitti_poses_file(os.path.join(folder_with_predicted_poses, filename))
if args.result_format == 'tum':
traj_est = file_interface.read_tum_trajectory_file(os.path.join(folder_with_predicted_poses, filename))
seq_results["length_of_estimated_traj"] = traj_est.path_length
if args.result_format == 'euroc':
traj_est = file_interface.read_euroc_csv_trajectory(os.path.join(folder_with_predicted_poses, filename))
if args.result_format == 'tum' and args.gt_format == 'tum':
seq_results["num_gt_poses"] = traj_ref.num_poses
seq_results["num_predicted_poses"] = traj_est.num_poses
traj_ref, traj_est = sync.associate_trajectories(traj_ref, traj_est, args.max_diff)
end_time_est = traj_est.get_infos()["t_end (s)"]
if (abs(end_time_est - end_time_gt) > 0.2) or (traj_est.get_infos()["t_start (s)"] > 0.2):
print('LOST in track '+filename[:filename.rfind('.')])
seq_results['lost'] = True
results.append(seq_results)
t.update(1)
continue
if args.alignment != None:
traj_est = trajectory.align_trajectory(traj_est, traj_ref, correct_scale=args.alignment.find("scale") != -1, correct_only_scale=args.alignment=="scale")
trajectory.align_trajectory_origin(traj_est, traj_ref)
data = (traj_ref, traj_est)
ape_metric_translation = metrics.APE(metrics.PoseRelation.translation_part)
ape_metric_rotation = metrics.APE(metrics.PoseRelation.rotation_angle_deg)
ape_metric_translation.process_data(data)
ape_metric_rotation.process_data(data)
ape_translation_statistics = ape_metric_translation.get_all_statistics()
ape_rotation_statistics = ape_metric_rotation.get_all_statistics()
ape_translation_statistics_plot = copy.deepcopy(ape_translation_statistics)
ape_rotation_statistics_plot = copy.deepcopy(ape_rotation_statistics)
ape_translation_statistics_plot.pop('sse')
ape_translation_statistics_plot.pop('std')
ape_translation_statistics_plot.pop('min')
ape_translation_statistics_plot.pop('max')
ape_rotation_statistics_plot.pop('sse')
ape_rotation_statistics_plot.pop('std')
ape_rotation_statistics_plot.pop('min')
ape_rotation_statistics_plot.pop('max')
kitti_trans_err, kitti_rot_err, ate = kitti_eval_tool.eval(traj_ref.poses_se3,
traj_est.poses_se3,
alignment=None)
#---------------------------------adding results to variable seq_results for excel -----------------------------
seq_results['metrics']['dist_to_trgt'] = traj_est.get_infos()['pos_end (m)'] - traj_ref.get_infos()['pos_end (m)']
seq_results['metrics']['dist_to_trgt'] = np.sum(np.array(seq_results['metrics']['dist_to_trgt'])**2)**0.5
seq_results['metrics']["Kitti trans err (%)"] = kitti_trans_err
seq_results['metrics']["Kitti rot err (deg/m)"] = kitti_rot_err
seq_results['metrics']["ATE (m)"] = ate
seq_results['metrics']["APE(trans err) median (m)"] = ape_translation_statistics["median"]
seq_results['metrics']["APE(rot err) median (deg)"] = ape_rotation_statistics["median"]
#--------------------------------------------------------------------------------------------------------
#-------------------------------------------------------------------------------------------------------
# --------------------------------printing results into console----------------------------------------------
print('Results for "'+filename+'":')
print('Kitti average translational error (%): {:.7f}'.format(kitti_trans_err))
print('Kitti average rotational error (deg/m): {:.7f}'.format(kitti_rot_err))
print('ATE (m): {:.7f}'.format(ate))
print('APE(translation error) median (m): {:.7f}'.format(ape_translation_statistics["median"]))
print('APE(rotation error) median (deg): {:.7f}'.format(ape_rotation_statistics["median"]))
print('distance to target on the last frame: {:.7f}'.format(seq_results['metrics']['dist_to_trgt']))
#------------------------------------------------------------------------------------------------------------
#---------------------------------Saving results into overall results text file------------------------------
file_results_txt.write('{:<24} '.format(filename[:filename.rfind('.')]))
file_results_txt.write('{:>7.4f} '.format(kitti_trans_err))
file_results_txt.write('{:>7.4f} '.format(kitti_rot_err))
file_results_txt.write('{:>7.4f} '.format(ate))
file_results_txt.write('{:>7.4f} '.format(ape_translation_statistics["median"]))
file_results_txt.write('{:>7.4f} '.format(ape_rotation_statistics["median"]))
file_results_txt.write('{:>7.4f}\n'.format(seq_results['metrics']['dist_to_trgt']))
#------------------------------------------------------------------------------------------------------------
# --------------------------------Saving metrics to text file for one track----------------------------------
txt_filename = filename[:filename.rfind('.')]+"_metrics.txt"
with open(os.path.join(output_folder_seq, txt_filename), "w") as txt_file:
txt_file.write('Kitti average translational error (%): {:.7f}\n'.format(kitti_trans_err))
txt_file.write('Kitti average rotational error (deg/m): {:.7f}\n'.format(kitti_rot_err))
txt_file.write('ATE (m): {:.7f}\n'.format(ate))
txt_file.write('APE(translation error) median (m): {:.7f}\n'.format(ape_translation_statistics["median"]))
txt_file.write('APE(rotation error) median (deg): {:.7f}\n'.format(ape_rotation_statistics["median"]))
txt_file.write('Distance to target on the last frame: {:.7f}\n'.format(seq_results['metrics']['dist_to_trgt']))
#---------------------------------------------------------------------------------------------------------
# ---------------------------------Saving values of errors for each frame to text file------------------------
# ------------------------------------------for translation errors----------------------------------------
txt_filename = filename[:filename.rfind('.')]+"_APE(translation)_errors.txt"
output_folder_seq_translation = os.path.join(output_folder_seq,"translation")
output_folder_seq_rotation = os.path.join(output_folder_seq,"rotation")
os.makedirs(output_folder_seq_translation, exist_ok=True)
os.makedirs(output_folder_seq_rotation, exist_ok=True)
with open(os.path.join(output_folder_seq_translation, txt_filename), "w") as txt_file:
for error in ape_metric_translation.error:
txt_file.write('{:.10f}\n'.format(error))
# -----------------------------------------for rotation degree errors--------------------------------------
txt_filename = filename[:filename.rfind('.')]+"_APE(rotation_deg)_errors.txt"
with open(os.path.join(output_folder_seq_rotation, txt_filename), "w") as txt_file:
for error in ape_metric_rotation.error:
txt_file.write('{:.10f}\n'.format(error))
#----------------------------------------------------------------------------------------------------------
# ---------------------------------------Saving plot of errors of each frame------------------------------
# ------------------------------------------for translation errors----------------------------------------
plot_collection = plot.PlotCollection("Example")
fig_1 = plt.figure(figsize=(8, 8))
plot.error_array(fig_1, ape_metric_translation.error,
name="APE", title=str(ape_metric_translation), xlabel="Index of frame", ylabel='Error')
plot_collection.add_figure("raw", fig_1)
plot_filename = filename[:filename.rfind('.')]+"_APE(translation)_errors.png"
plt.savefig(os.path.join(output_folder_seq_translation, plot_filename))
plt.close(fig_1)
# -----------------------------------------for rotation degree errors--------------------------------------
plot_collection = plot.PlotCollection("Example")
fig_1 = plt.figure(figsize=(8, 8))
plot.error_array(fig_1, ape_metric_rotation.error,
name="APE", title=str(ape_metric_rotation), xlabel="Index of frame", ylabel='Error')
plot_collection.add_figure("raw", fig_1)
plot_filename = filename[:filename.rfind('.')]+"_APE(rotation)_errors.png"
plt.savefig(os.path.join(output_folder_seq_rotation,plot_filename))
plt.close(fig_1)
#-----------------------------------------------------------------------------------------------------------
# -----------------------------------------Saving trajectory plot-------------------------------------------
# ------------------------------------------for translation errors----------------------------------------
fig_2 = plt.figure(figsize=(8, 8))
ax = plot.prepare_axis(fig_2, plot_mode)
plot.traj(ax, plot_mode, traj_ref, '--', 'gray', 'reference')
plot.traj_colormap( ax, traj_est, ape_metric_translation.error, plot_mode,
min_map=ape_translation_statistics["min"],
max_map=ape_translation_statistics["max"], title="APE translation mapped onto trajectory")
plot_collection.add_figure("traj (error)", fig_2)
plot_filename = filename[:filename.rfind('.')]+"_APE(translation)_map.png"
plt.savefig(os.path.join(output_folder_seq_translation,plot_filename))
plt.close(fig_2)
# -----------------------------------------for rotation degree errors--------------------------------------
fig_2 = plt.figure(figsize=(8, 8))
ax = plot.prepare_axis(fig_2, plot_mode)
plot.traj(ax, plot_mode, traj_ref, '--', 'gray', 'reference')
plot.traj_colormap( ax, traj_est, ape_metric_rotation.error, plot_mode,
min_map=ape_rotation_statistics["min"],
max_map=ape_rotation_statistics["max"], title="APE rotation mapped onto trajectory")
plot_collection.add_figure("traj (error)", fig_2)
plot_filename = filename[:filename.rfind('.')]+"_APE(rotation)_map.png"
plt.savefig(os.path.join(output_folder_seq_rotation,plot_filename))
plt.close(fig_2)
#-----------------------------------------------------------------------------------------------------------
print()
active_worksheet = wb['sheet1']
thin = Side(border_style="thin", color="000000")
thick = Side(border_style="thick", color="000000")
medium = Side(border_style="medium", color="000000")
font_header = Font(name='Arial',
size=10,
bold=True,
italic=False,
vertAlign=None,
underline='none',
strike=False,
color='FF000000')
font_values = Font(name='Arial',
size=10,
bold=False,
italic=False,
vertAlign=None,
underline='none',
strike=False,
color='FF000000')
active_worksheet.row_dimensions[2].height = 35
file_results_txt.close()
results.append(seq_results)
t.update(1)
def run(args):
from evo.core import metrics
from evo.tools import file_interface, log
log.configure_logging(args.verbose, args.silent, 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)
pose_relation = None
if args.pose_relation == "full":
pose_relation = metrics.PoseRelation.full_transformation
elif args.pose_relation == "rot_part":
pose_relation = metrics.PoseRelation.rotation_part
elif args.pose_relation == "trans_part":
pose_relation = metrics.PoseRelation.translation_part
elif args.pose_relation == "angle_deg":
pose_relation = metrics.PoseRelation.rotation_angle_deg
elif args.pose_relation == "angle_rad":
pose_relation = metrics.PoseRelation.rotation_angle_rad
delta_unit = None
if args.delta_unit == "f":
delta_unit = metrics.Unit.frames
elif args.delta_unit == "d":
delta_unit = metrics.Unit.degrees
elif args.delta_unit == "r":
delta_unit = metrics.Unit.radians
elif args.delta_unit == "m":
delta_unit = metrics.Unit.meters
traj_ref, traj_est, stamps_est = None, None, None
ref_name, est_name = "", ""
plot_mode = None # no plot imports unless really needed (slow)
if args.subcommand == "tum":
traj_ref, traj_est = file_interface.load_assoc_tum_trajectories(
args.ref_file,
args.est_file,
args.t_max_diff,
args.t_offset,
)
ref_name, est_name = args.ref_file, args.est_file
if args.plot or args.save_plot:
from evo.tools.plot import PlotMode
plot_mode = PlotMode.xyz if not args.plot_mode else PlotMode[
args.plot_mode]
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
if args.plot or args.save_plot:
from evo.tools.plot import PlotMode
plot_mode = PlotMode.xz if not args.plot_mode else PlotMode[
args.plot_mode]
elif args.subcommand == "euroc":
args.align = True
logger.info(
"Forcing trajectory alignment implicitly (EuRoC ground truth is in IMU frame)"
)
logger.debug(SEP)
traj_ref, traj_est = file_interface.load_assoc_euroc_trajectories(
args.state_gt_csv,
args.est_file,
args.t_max_diff,
args.t_offset,
)
ref_name, est_name = args.state_gt_csv, args.est_file
if args.plot or args.save_plot:
from evo.tools.plot import PlotMode
plot_mode = PlotMode.xyz if not args.plot_mode else PlotMode[
args.plot_mode]
elif args.subcommand == "bag":
import rosbag
logger.debug("Opening bag file {} ...".format(args.bag))
bag = rosbag.Bag(args.bag, 'r')
try:
traj_ref, traj_est = file_interface.load_assoc_bag_trajectories(
bag,
args.ref_topic,
args.est_topic,
args.t_max_diff,
args.t_offset,
)
finally:
bag.close()
ref_name, est_name = args.ref_topic, args.est_topic
if args.plot or args.save_plot:
from evo.tools.plot import PlotMode
plot_mode = PlotMode.xy if not args.plot_mode else PlotMode[
args.plot_mode]
main_rpe(
traj_ref=traj_ref,
traj_est=traj_est,
pose_relation=pose_relation,
delta=args.delta,
delta_unit=delta_unit,
rel_delta_tol=args.delta_tol,
all_pairs=args.all_pairs,
align=args.align,
correct_scale=args.correct_scale,
ref_name=ref_name,
est_name=est_name,
show_plot=args.plot,
save_plot=args.save_plot,
plot_mode=plot_mode,
save_results=args.save_results,
no_warnings=args.no_warnings,
serialize_plot=args.serialize_plot,
plot_colormap_max=args.plot_colormap_max,
plot_colormap_min=args.plot_colormap_min,
plot_colormap_max_percentile=args.plot_colormap_max_percentile,
)