def calc_euroc_seq_errors(est_traj, gt_traj):
gt_traj_synced, est_traj_synced = sync.associate_trajectories(
gt_traj, est_traj, max_diff=0.01)
est_traj_aligned = trajectory.align_trajectory(est_traj_synced,
gt_traj_synced,
correct_scale=False,
correct_only_scale=False)
pose_relation = metrics.PoseRelation.translation_part
ape_metric = metrics.APE(pose_relation)
ape_metric.process_data((
gt_traj_synced,
est_traj_aligned,
))
ape_stat = ape_metric.get_statistic(metrics.StatisticsType.rmse)
# ape_metric = metrics.RPE(pose_relation)
# ape_metric.process_data((gt_traj_synced, est_traj_aligned,))
# ape_stat = ape_metric.get_statistic(metrics.StatisticsType.rmse)
# fig = plt.figure()
# traj_by_label = {
# "estimate (not aligned)": est_traj,
# "estimate (aligned)": est_traj_aligned,
# "reference": gt_traj
# }
# plot.trajectories(fig, traj_by_label, plot.PlotMode.xyz)
# plt.show()
return ape_metric, ape_stat
def test_association(self):
traj_1 = helpers.fake_trajectory(10, 0.1)
traj_2 = helpers.fake_trajectory(100, 0.01)
traj_1_sync, traj_2_sync = sync.associate_trajectories(traj_1, traj_2)
self.assertEqual(traj_1_sync.num_poses, traj_2_sync.num_poses)
self.assertNotEqual(traj_2.num_poses, traj_2_sync.num_poses)
self.assertEqual(traj_2_sync.num_poses, 10)
def stats_to_latex_table(traj_ref, segments, idx, table):
"""Associate segments of an object trajectory as given by a DATMO system
with the object's reference trajectory
:traj_ref: Reference trajectory
:segments: All the segments of the robot trajectory
:table: Latex table that the statistics get added to
"""
whole = trajectory.merge(segments)
traj_ref, traj_est = sync.associate_trajectories(traj_ref,
whole,
max_diff=0.01)
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(traj_est.get_infos())
table.add_row((
idx + 1,
round(ape_statistics["rmse"], 3),
round(ape_statistics["mean"], 3),
round(ape_statistics["median"], 3),
round(ape_statistics["std"], 3),
round(ape_statistics["min"], 3),
round(ape_statistics["max"], 3),
round(ape_statistics["sse"], 3),
))
table.add_hline
def run(args):
import evo.common_ape_rpe as common
from evo import EvoException
from evo.core import sync
from evo.tools import file_interface, log
from evo.tools.settings import SETTINGS
log.configure_logging(args.verbose, args.silent, args.debug,
local_logfile=args.logfile)
if args.debug:
from pprint import pformat
parser_str = pformat({arg: getattr(args, arg) for arg in vars(args)})
logger.debug("main_parser config:\n{}".format(parser_str))
logger.debug(SEP)
if (args.plot or args.save_plot or args.serialize_plot) and args.all_pairs:
raise EvoException(
"all_pairs mode cannot be used with plotting functions")
traj_ref, traj_est, ref_name, est_name = common.load_trajectories(args)
pose_relation = common.get_pose_relation(args)
delta_unit = common.get_delta_unit(args)
traj_ref_full = None
if args.plot_full_ref:
import copy
traj_ref_full = copy.deepcopy(traj_ref)
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)
result = 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,
align_origin=args.align_origin,
ref_name=ref_name,
est_name=est_name,
)
if args.plot or args.save_plot or args.serialize_plot:
common.plot(args, result, traj_ref, result.trajectories[est_name],
traj_ref_full=traj_ref_full)
if args.save_results:
logger.debug(SEP)
if not SETTINGS.save_traj_in_zip:
del result.trajectories[ref_name]
del result.trajectories[est_name]
file_interface.save_res_file(args.save_results, result,
confirm_overwrite=not args.no_warnings)
def run(args: argparse.Namespace) -> None:
log.configure_logging(args.verbose,
args.silent,
args.debug,
local_logfile=args.logfile)
if args.debug:
from pprint import pformat
parser_str = pformat({arg: getattr(args, arg) for arg in vars(args)})
logger.debug("main_parser config:\n{}".format(parser_str))
logger.debug(SEP)
traj_ref, traj_est, ref_name, est_name = common.load_trajectories(args)
traj_ref_full = None
if args.plot_full_ref:
import copy
traj_ref_full = copy.deepcopy(traj_ref)
if isinstance(traj_ref, PoseTrajectory3D) and isinstance(
traj_est, PoseTrajectory3D):
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)
pose_relation = common.get_pose_relation(args)
result = ape(
traj_ref=traj_ref,
traj_est=traj_est,
pose_relation=pose_relation,
align=args.align,
correct_scale=args.correct_scale,
n_to_align=args.n_to_align,
align_origin=args.align_origin,
ref_name=ref_name,
est_name=est_name,
)
if args.plot or args.save_plot or args.serialize_plot:
common.plot_result(args,
result,
traj_ref,
result.trajectories[est_name],
traj_ref_full=traj_ref_full)
if args.save_results:
logger.debug(SEP)
if not SETTINGS.save_traj_in_zip:
del result.trajectories[ref_name]
del result.trajectories[est_name]
file_interface.save_res_file(args.save_results,
result,
confirm_overwrite=not args.no_warnings)
def run(args: argparse.Namespace) -> None:
log.configure_logging(args.verbose,
args.silent,
args.debug,
local_logfile=args.logfile)
if args.debug:
from pprint import pformat
parser_str = pformat({arg: getattr(args, arg) for arg in vars(args)})
logger.debug("main_parser config:\n{}".format(parser_str))
logger.debug(SEP)
traj_ref, traj_est, ref_name, est_name = common.load_trajectories(args)
traj_ref_full = None
if args.plot_full_ref:
import copy
traj_ref_full = copy.deepcopy(traj_ref)
if args.flip_xy:
print("flip xy", args.flip_xy)
flip_rotation = transformations.rotation_matrix(math.pi / 2, [0, 0, 1])
print('transformation matrix,', flip_rotation)
traj_est.transform_rotation_only(flip_rotation)
if isinstance(traj_ref, PoseTrajectory3D) and isinstance(
traj_est, PoseTrajectory3D):
logger.debug(SEP)
if args.t_start or args.t_end:
if args.t_start:
logger.info("Using time range start: {}s".format(args.t_start))
if args.t_end:
logger.info("Using time range end: {}s".format(args.t_end))
traj_ref.reduce_to_time_range(args.t_start, args.t_end)
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)
pose_relation = common.get_pose_relation(args)
result = segment_ape(traj_ref=traj_ref,
traj_est=traj_est,
pose_relation=pose_relation,
align=args.align,
correct_scale=args.correct_scale,
n_to_align=args.n_to_align,
align_origin=args.align_origin,
align_odom=args.align_odom,
segment_length=args.seg,
ref_name=ref_name,
est_name=est_name,
plot=args.plot)
def ape(traj_ref,
traj_est,
pose_relation,
align=False,
correct_scale=False,
align_origin=False,
ref_name="reference",
est_name="estimate"):
traj_ref, traj_est = sync.associate_trajectories(traj_ref, traj_est)
# Align the trajectories.
only_scale = correct_scale and not align
if align or correct_scale:
# logger.debug(SEP)
traj_est = trajectory.align_trajectory(traj_est, traj_ref,
correct_scale, only_scale)
elif align_origin:
# logger.debug(SEP)
traj_est = trajectory.align_trajectory_origin(traj_est, traj_ref)
# Calculate APE.
# logger.debug(SEP)
data = (traj_ref, traj_est)
ape_metric = metrics.APE(pose_relation)
ape_metric.process_data(data)
title = str(ape_metric)
if align and not correct_scale:
title += "\n(with SE(3) Umeyama alignment)"
elif align and correct_scale:
title += "\n(with Sim(3) Umeyama alignment)"
elif only_scale:
title += "\n(scale corrected)"
elif align_origin:
title += "\n(with origin alignment)"
else:
title += "\n(not aligned)"
ape_result = ape_metric.get_result(ref_name, est_name)
ape_result.info["title"] = title
# logger.debug(SEP)
# logger.info(ape_result.pretty_str())
ape_result.add_trajectory(ref_name, traj_ref)
ape_result.add_trajectory(est_name, traj_est)
if isinstance(traj_est, trajectory.PoseTrajectory3D):
seconds_from_start = [
t - traj_est.timestamps[0] for t in traj_est.timestamps
]
ape_result.add_np_array("seconds_from_start", seconds_from_start)
ape_result.add_np_array("timestamps", traj_est.timestamps)
return ape_result
def align_trajectory_by_vicon_whole(trj_est, trj_vicon):
trj_vicon_sync, trj_est_sync = sync.associate_trajectories(
trj_vicon, trj_est)
trj_copy = copy.deepcopy(
trj_est_sync
) # otherwise np arrays will be references and mess up stuff
r_a, t_a, s = geometry.umeyama_alignment(trj_copy.positions_xyz.T,
trj_vicon_sync.positions_xyz.T,
False)
res = lie.se3(r_a, t_a)
return res
def evaluate_results(structvio_res_path, data_folder):
ape_res = []
trj_est = read_structvio_trajectory_file(structvio_res_path)
name = os.path.basename(data_folder)
print(name)
vicon_path = os.path.join(data_folder, 'vicon.txt')
arcode_a_path = os.path.join(data_folder, name + '-ArUco-a.txt')
arcode_b_path = os.path.join(data_folder, name + '-ArUco-b.txt')
if (os.path.exists(vicon_path)):
print("using Vicon ground truth:")
print(vicon_path)
#read the ground truth
trj_vicon = read_tum_trajectory_file(vicon_path)
res = align_trajectory_by_vicon(trj_est, trj_vicon)
trj_est.transform(res[0])
trj1, trj2 = sync.associate_trajectories(trj_vicon, trj_est)
ape_res = ape(trj1, trj2, PoseRelation.translation_part)
elif os.path.exists(arcode_a_path) and os.path.exists(arcode_b_path):
#read the ground truth
print("using ArUco ground truth:")
print(arcode_a_path)
print(arcode_b_path)
trj_ar = read_arcode_trajectory_files(data_folder)
if trj_ar[1].timestamps[0] > trj_est.timestamps[-1]:
print('Incomplete trajectory!')
exit(-1)
se3 = align_trajectory_by_arcode(trj_est, trj_ar)
trj_est.transform(se3)
trj_ar_merge = merge_arcode_trajeoctries(trj_ar[0], trj_ar[1])
trj1, trj2 = sync.associate_trajectories(trj_ar_merge, trj_est)
ape_res = ape(trj1, trj2, PoseRelation.translation_part)
else:
print('cannot find any ground truth files!')
exit(-1)
return ape_res
def associate_segments_common_frame(traj, tracks, distance):
"""Associate segments of an object trajectory as given by a DATMO system
with the object's reference trajectory
:traj: Reference trajectory
:tracks: All the tracks that got produced by the DATMO system
:localization: The trajectory of the self-localization
:returns: segments: The tracks that match to the reference trajectory
:returns: traj_ref: The part of the reference trajectory that matches with
tracks
"""
matches = []
for tr in tracks: # Find the best matching tracks to the object trajectory
traj_ref, traj_est = sync.associate_trajectories(traj,
tr,
max_diff=0.1)
# print("calculating APE for track of length", len(tr.timestamps))
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(ape_statistics)
mismatch = ape_statistics['mean']
# print(mismatch)
tuple = [
traj_est, mismatch,
traj_est.get_infos()['t_start (s)'], traj_ref
]
matches.append(tuple)
matches.sort(key=lambda x: x[2])
segments_track = [] #The parts of the trajectory are added to this list
segments_refer = [
] #The parts of the reference trajectory are added to this list
for m in matches:
if m[1] < distance: # if the mismatch is smaller than 1
# print(m[1],distance)
# print(m[0].get_statistics()['v_avg (m/s)'])
segments_track.append(m[0])
segments_refer.append(m[3])
# print(m[0].get_infos()['t_start (s)'],m[0].get_infos()["path length (m)"])
# print(m[0].get_statistics()['v_avg (m/s)'])
if len(segments_track) == 0:
print("No matching segments")
traj_ref = trajectory.merge(segments_refer)
# print(traj_ref.length)
return segments_track, traj_ref
def run(args):
import evo.common_ape_rpe as common
from evo.core import sync
from evo.tools import file_interface, log
from evo.tools.settings import SETTINGS
log.configure_logging(args.verbose,
args.silent,
args.debug,
local_logfile=args.logfile)
if args.debug:
from pprint import pformat
parser_str = pformat({arg: getattr(args, arg) for arg in vars(args)})
logger.debug("main_parser config:\n{}".format(parser_str))
logger.debug(SEP)
traj_ref_list, traj_est_list, ref_name, est_name = common.load_trajectories_multi(
args)
for i in range(len(traj_ref_list)):
traj_ref_full = None
if args.plot_full_ref:
import copy
traj_ref_full = copy.deepcopy(traj_ref_list[i])
if args.subcommand != "kitti":
logger.debug("Synchronizing trajectories...")
traj_ref_list[i], traj_est_list[i] = sync.associate_trajectories(
traj_ref_list[i],
traj_est_list[i],
args.t_max_diff,
args.t_offset,
first_name=ref_name[i],
snd_name=est_name[i])
pose_relation = common.get_pose_relation(args)
result = ape(
traj_ref_list=traj_ref_list,
traj_est_list=traj_est_list,
pose_relation=pose_relation,
align=args.align,
correct_scale=args.correct_scale,
align_origin=args.align_origin,
ref_name=ref_name,
est_name=est_name,
)
if args.plot or args.save_plot or args.serialize_plot:
common.plot_multi(args, result, traj_ref_list, traj_est_list)
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 three_plots(ref, est, table, name):
"""Generates plots and statistics table into Report
:ref: PoseTrajectory3D object that is used as reference
:est: PoseTrajectory3D object that is plotted against reference
:table: Tabular object that is generated by Tabular('c c')
:name: String that is used as name for file and table entry
:returns: translation of reference against estimation
"""
ref, est = sync.associate_trajectories(ref, est)
est, rot, tra, s = trajectory.align_trajectory(est,
ref,
correct_scale=False,
return_parameters=True)
data = (ref, est)
ape_metric = metrics.APE(metrics.PoseRelation.translation_part)
ape_metric.process_data(data)
ape_statistics = ape_metric.get_all_statistics()
# [ Localization ]
fig, axarr = plt.subplots(3) #sharex=True)
fig.suptitle('Localization', fontsize=30)
fig.tight_layout()
plot.traj_xyyaw(axarr, est, '-', 'red', 'estimation', 1, ref.timestamps[0])
plot.traj_xyyaw(axarr, ref, '-', 'gray', 'original')
fig.subplots_adjust(hspace=0.2)
plt.waitforbuttonpress(0)
plt.savefig("/home/kostas/results/latest/" + name + ".png",
format='png',
bbox_inches='tight')
plt.close(fig)
table.add_row((
name,
round(ape_statistics["rmse"], 3),
round(ape_statistics["mean"], 3),
round(ape_statistics["median"], 3),
round(ape_statistics["std"], 3),
round(ape_statistics["min"], 3),
round(ape_statistics["max"], 3),
round(ape_statistics["sse"], 3),
))
table.add_hline
def load_assoc_euroc_trajectories(ref_file,
est_file,
max_diff=0.01,
offset_2=0.0,
invert=False):
"""
parses ground truth trajectory from EuRoC MAV state estimate .csv and estimated TUM trajectory
and returns the data with associated (matching) timestamps according to the time parameters
:param ref_file: ground truth: <sequence>/mav0/state_groundtruth_estimate0/data.csv
:param est_file: estimated TUM trajectory file
:param max_diff: max. allowed absolute time difference for associating
:param offset_2: optional time offset of second timestamp vector
:param invert: set to True to match from longer list to short (default: short to longer list)
:return: trajectory.PoseTrajectory3D objects traj_ref and traj_est
"""
traj_ref = read_euroc_csv_trajectory(ref_file)
traj_est = read_tum_trajectory_file(est_file)
return sync.associate_trajectories(traj_ref, traj_est, max_diff, offset_2,
invert, ref_file, est_file)
def load_assoc_tum_trajectories(ref_file,
est_file,
max_diff=0.01,
offset_2=0.0,
invert=False):
"""
parses two trajectory files in TUM format (timestamp tx ty tz qx qy qz qw)
and returns the data with associated (matching) timestamps according to the time parameters
:param ref_file: first trajectory file
:param est_file: second trajectory file
:param max_diff: max. allowed absolute time difference for associating
:param offset_2: optional time offset of second timestamp vector
:param invert: set to True to match from longer list to short (default: short to longer list)
:return: trajectory.PoseTrajectory3D objects traj_ref and traj_est
"""
traj_ref = read_tum_trajectory_file(ref_file)
traj_est = read_tum_trajectory_file(est_file)
return sync.associate_trajectories(traj_ref, traj_est, max_diff, offset_2,
invert, ref_file, est_file)
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 load_assoc_bag_trajectories(bag_handle,
ref_topic,
est_topic,
max_diff=0.01,
offset_2=0.0,
invert=False):
"""
reads trajectory data from a ROS bag file with two geometry_msgs/PoseStamped topics
and returns the data with associated (matching) timestamps according to the time parameters
:param bag_handle: opened bag handle, from rosbag.Bag(...)
:param ref_topic: first geometry_msgs/PoseStamped topic
:param est_topic: second geometry_msgs/PoseStamped topic
:param max_diff: max. allowed absolute time difference for associating
:param offset_2: optional time offset of second timestamp vector
:param invert: set to True to match from longer list to short (default: short to longer list)
:return: trajectory.PoseTrajectory3D objects traj_ref and traj_est
"""
traj_ref = read_bag_trajectory(bag_handle, ref_topic)
traj_est = read_bag_trajectory(bag_handle, est_topic)
return sync.associate_trajectories(traj_ref, traj_est, max_diff, offset_2,
invert, ref_topic, est_topic)
def compare_using_APE(ref_file, est_file, use_aligned_trajectories=False):
"""
Compare two files using EVO API. Using the APE metric.
:param ref_file:
:param est_file:
:param use_aligned_trajectories: True to align before comparing. False to leave original data as it is.
:return:
"""
# Load trajectories
traj_ref = file_interface.read_tum_trajectory_file(ref_file)
traj_est = file_interface.read_tum_trajectory_file(est_file)
# Sinchronize trajectories by timestamps
max_diff = 0.01
traj_ref, traj_est = sync.associate_trajectories(traj_ref, traj_est, max_diff)
# -------------EVO_APE-------------
# Settings
pose_relation = metrics.PoseRelation.translation_part
use_aligned_trajectories = False # OPTION -va on the scripts. Is related to Uleyamas alignment
# The aligned trajectories can be used if we want it to
if use_aligned_trajectories:
# Align trajectories with Uleyamas algorithm
try:
traj_est_aligned = trajectory.align_trajectory(traj_est, traj_ref, correct_scale=False,
correct_only_scale=False)
data = (traj_ref, traj_est_aligned)
except GeometryException:
print("Couldnt align with Uleyamas algorithm...")
data = (traj_ref, traj_est)
else:
data = (traj_ref, traj_est)
ape_metric = metrics.APE(pose_relation) # APE with only pose is in reality ATE (Absolute trajectory error) instead of APE (abs. pose error)
ape_metric.process_data(data)
# Get all stadistics in a dict
ape_stats = ape_metric.get_all_statistics()
return ape_stats
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 ape(traj_ref,
traj_est,
pose_relation,
align=False,
correct_scale=False,
align_origin=False,
ref_name="reference",
est_name="estimate"):
''' Based on main_ape.py
'''
traj_ref, traj_est = sync.associate_trajectories(traj_ref, traj_est)
# Align the trajectories.
only_scale = correct_scale and not align
if align or correct_scale:
traj_est = trajectory.align_trajectory(traj_est, traj_ref,
correct_scale, only_scale)
elif align_origin:
traj_est = trajectory.align_trajectory_origin(traj_est, traj_ref)
# Calculate APE.
data = (traj_ref, traj_est)
ape_metric = APE(pose_relation)
ape_metric.process_data(data)
ape_result = ape_metric.get_result(ref_name, est_name)
ape_result.add_trajectory(ref_name, traj_ref)
ape_result.add_trajectory(est_name, traj_est)
if isinstance(traj_est, trajectory.PoseTrajectory3D):
seconds_from_start = [
t - traj_est.timestamps[0] for t in traj_est.timestamps
]
ape_result.add_np_array("seconds_from_start", seconds_from_start)
ape_result.add_np_array("timestamps", traj_est.timestamps)
return ape_result
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()
print("Left cam calibration matrix: ")
print(body_T_leftCam)
# %%
gt_df = gt_df[~gt_df.index.duplicated()]
# %%
# Generate some trajectories for later plots
# Convert to evo trajectory objects
traj_ref_unassociated = pandas_bridge.df_to_trajectory(gt_df)
# Use the mono ransac file as estimated trajectory.
traj_est_unassociated = pandas_bridge.df_to_trajectory(mono_df)
# Associate the trajectories
traj_ref_abs, traj_est_rel = sync.associate_trajectories(
traj_ref_unassociated, traj_est_unassociated)
traj_ref_rel = convert_abs_traj_to_rel_traj(traj_ref_abs, up_to_scale=False)
# Transform the relative gt trajectory from body to left camera frame
traj_ref_cam_rel = convert_rel_traj_from_body_to_cam(traj_ref_rel,
body_T_leftCam)
# Remove the first timestamp; we don't have relative pose at first gt timestamp
traj_est_rel = trajectory.PoseTrajectory3D(
traj_est_rel._positions_xyz[1:],
traj_est_rel._orientations_quat_wxyz[1:],
traj_est_rel.timestamps[1:],
)
print("traj_ref_rel: ", traj_ref_rel)
def associate_segments(traj, tracks):
"""Associate segments of an object trajectory as given by a DATMO system
with the object's reference trajectory
:traj: Reference trajectory
:tracks: All the tracks that got produced by the DATMO system
:localization: The trajectory of the self-localization
:returns: segments: The tracks that match to the reference trajectory
:returns: traj_ref: The part of the reference trajectory that matches with
tracks
"""
matches = []
for tr in tracks: # Find the best matching tracks to the object trajectory
traj_ref, traj_est = sync.associate_trajectories(traj,
tr,
max_diff=0.01)
traj_est, rot, tra, _ = trajectory.align_trajectory(
traj_est, traj_ref, correct_scale=False, return_parameters=True)
# print("calculating APE for track of length", len(tr.timestamps))
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()
tra_dif = (tra - loc_tra)
# print(tra)
abs_tra_dif = abs((tra - loc_tra)[0]) + abs((tra - loc_tra)[1])
translation = abs(tra[0]) + abs(tra[1])
rot_dif = (rot - loc_rot)
abs_rot_dif = 0
for i in range(0, len(rot_dif)): abs_rot_dif += abs(rot_dif[i][0])+ abs(rot_dif[i][1]) +\
abs(rot_dif[i][2])
# print(abs_tra_dif,abs_rot_dif)
mismatch = abs_tra_dif + abs_rot_dif
tuple = [traj_est, mismatch, traj_est.get_infos()['t_start (s)']]
matches.append(tuple)
matches.sort(key=lambda x: x[2])
segments = [] #The parts of the trajectory are added to this list
for m in matches:
# print(m[1])
if m[1] < 0.1: # if the mismatch is smaller than 1
# print(m[0].get_statistics()['v_avg (m/s)'])
segments.append(m[0])
# print(m[0].get_infos()['t_start (s)'],m[0].get_infos()["path length (m)"])
# print(m[0].get_statistics()['v_avg (m/s)'])
if len(segments) == 0:
print("No matching segments")
whole = trajectory.merge(segments)
traj_ref, traj_est = sync.associate_trajectories(traj,
whole,
max_diff=0.01)
traj_est, rot, tra, _ = trajectory.align_trajectory(traj_est,
traj_ref,
correct_scale=False,
return_parameters=True)
# print(traj_est.get_infos())
return segments, traj_ref, translation
#!/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
print("loading trajectories")
traj_ref = file_interface.read_tum_trajectory_file("../../test/data/fr2_desk_groundtruth.txt")
traj_est = file_interface.read_tum_trajectory_file("../../test/data/fr2_desk_ORB.txt")
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"])
print("loading plot modules")
from evo.tools import plot
import matplotlib.pyplot as plt
print("plotting")
plot_collection = plot.PlotCollection("Example")
# metric values
fig_1 = plt.figure(figsize=(8, 8))
def test_no_matches_due_to_offset(self):
traj_1 = helpers.fake_trajectory(10, 0.1, start_time=0.)
traj_2 = helpers.fake_trajectory(10, 0.1, start_time=2.)
with self.assertRaises(sync.SyncException):
sync.associate_trajectories(traj_1, traj_2)
def test_wrong_type(self):
path_1 = helpers.fake_path(10)
path_2 = helpers.fake_path(10)
with self.assertRaises(sync.SyncException):
sync.associate_trajectories(path_1, path_2)
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)
#!/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
print("loading trajectories")
traj_ref = file_interface.read_tum_trajectory_file(
"../../test/data/fr2_desk_groundtruth.txt")
traj_est = file_interface.read_tum_trajectory_file(
"../../test/data/fr2_desk_ORB.txt")
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"])
print("loading plot modules")
from evo.tools import plot
import matplotlib.pyplot as plt
print("plotting")
plot_collection = plot.PlotCollection("Example")
# %%
gt_df = gt_df[~gt_df.index.duplicated()]
# %%
# Convert the gt relative-pose DataFrame to a trajectory object.
traj_ref_complete = pandas_bridge.df_to_trajectory(gt_df)
# Use the backend poses as trajectory.
traj_est_unaligned = pandas_bridge.df_to_trajectory(output_poses_df)
discard_n_start_poses = 0
discard_n_end_poses = 0
# Associate the data.
traj_est = copy.deepcopy(traj_est_unaligned)
traj_ref, traj_est = sync.associate_trajectories(traj_ref_complete, traj_est)
traj_est = trajectory.align_trajectory(
traj_est,
traj_ref,
correct_scale=False,
discard_n_start_poses=int(discard_n_start_poses),
discard_n_end_poses=int(discard_n_end_poses),
)
print("traj_ref: ", traj_ref)
print("traj_est: ", traj_est)
# %%
# plot ground truth trajectory with pose
plot_mode = plot.PlotMode.xy
fig = plt.figure()
def run_analysis(self,
traj_ref_path,
traj_vio_path,
traj_pgo_path,
segments,
dataset_name="",
discard_n_start_poses=0,
discard_n_end_poses=0):
""" Analyze data from a set of trajectory csv files.
Args:
traj_ref_path: string representing filepath of the reference (ground-truth) trajectory.
traj_vio_path: string representing filepath of the vio estimated trajectory.
traj_pgo_path: string representing filepath of the pgo estimated trajectory.
segments: list of segments for RPE calculation, defined in the experiments yaml file.
dataset_name: string representing the dataset's name
discard_n_start_poses: int representing number of poses to discard from start of analysis.
discard_n_end_poses: int representing the number of poses to discard from end of analysis.
"""
import copy
# Mind that traj_est_pgo might be None
traj_ref, traj_est_vio, traj_est_pgo = self.read_traj_files(
traj_ref_path, traj_vio_path, traj_pgo_path)
# We copy to distinguish from the pgo version that may be created
traj_ref_vio = copy.deepcopy(traj_ref)
# Register and align trajectories:
evt.print_purple("Registering and aligning trajectories")
traj_ref_vio, traj_est_vio = sync.associate_trajectories(
traj_ref_vio, traj_est_vio)
traj_est_vio = trajectory.align_trajectory(
traj_est_vio,
traj_ref_vio,
correct_scale=False,
discard_n_start_poses=int(discard_n_start_poses),
discard_n_end_poses=int(discard_n_end_poses))
# We do the same for the PGO trajectory if needed:
traj_ref_pgo = None
if traj_est_pgo is not None:
traj_ref_pgo = copy.deepcopy(traj_ref)
traj_ref_pgo, traj_est_pgo = sync.associate_trajectories(
traj_ref_pgo, traj_est_pgo)
traj_est_pgo = trajectory.align_trajectory(
traj_est_pgo,
traj_ref_pgo,
correct_scale=False,
discard_n_start_poses=int(discard_n_start_poses),
discard_n_end_poses=int(discard_n_end_poses))
# We need to pick the lowest num_poses before doing any computation:
num_of_poses = traj_est_vio.num_poses
if traj_est_pgo is not None:
num_of_poses = min(num_of_poses, traj_est_pgo.num_poses)
traj_est_pgo.reduce_to_ids(
range(int(discard_n_start_poses),
int(num_of_poses - discard_n_end_poses), 1))
traj_ref_pgo.reduce_to_ids(
range(int(discard_n_start_poses),
int(num_of_poses - discard_n_end_poses), 1))
traj_est_vio.reduce_to_ids(
range(int(discard_n_start_poses),
int(num_of_poses - discard_n_end_poses), 1))
traj_ref_vio.reduce_to_ids(
range(int(discard_n_start_poses),
int(num_of_poses - discard_n_end_poses), 1))
# Calculate all metrics:
(ape_metric_vio, rpe_metric_trans_vio, rpe_metric_rot_vio,
results_vio) = self.process_trajectory_data(traj_ref_vio,
traj_est_vio, segments,
True)
# We do the same for the pgo trajectory if needed:
ape_metric_pgo = None
rpe_metric_trans_pgo = None
rpe_metric_rot_pgo = None
results_pgo = None
if traj_est_pgo is not None:
(ape_metric_pgo, rpe_metric_trans_pgo, rpe_metric_rot_pgo,
results_pgo) = self.process_trajectory_data(
traj_ref_pgo, traj_est_pgo, segments, False)
# Generate plots for return:
plot_collection = None
if self.display_plots or self.save_plots:
evt.print_green("Plotting:")
log.info(dataset_name)
plot_collection = plot.PlotCollection("Example")
if traj_est_pgo is not None:
# APE Metric Plot:
plot_collection.add_figure(
"PGO_APE_translation",
plot_metric(ape_metric_pgo, "PGO + VIO APE Translation"))
# Trajectory Colormapped with ATE Plot:
plot_collection.add_figure(
"PGO_APE_translation_trajectory_error",
plot_traj_colormap_ape(
ape_metric_pgo, traj_ref_pgo, traj_est_vio,
traj_est_pgo, "PGO + VIO ATE Mapped Onto Trajectory"))
# RPE Translation Metric Plot:
plot_collection.add_figure(
"PGO_RPE_translation",
plot_metric(rpe_metric_trans_pgo,
"PGO + VIO RPE Translation"))
# Trajectory Colormapped with RTE Plot:
plot_collection.add_figure(
"PGO_RPE_translation_trajectory_error",
plot_traj_colormap_rpe(
rpe_metric_trans_pgo, traj_ref_pgo, traj_est_vio,
traj_est_pgo,
"PGO + VIO RPE Translation Error Mapped Onto Trajectory"
))
# RPE Rotation Metric Plot:
plot_collection.add_figure(
"PGO_RPE_Rotation",
plot_metric(rpe_metric_rot_pgo, "PGO + VIO RPE Rotation"))
# Trajectory Colormapped with RTE Plot:
plot_collection.add_figure(
"PGO_RPE_rotation_trajectory_error",
plot_traj_colormap_rpe(
rpe_metric_rot_pgo, traj_ref_pgo, traj_est_vio,
traj_est_pgo,
"PGO + VIO RPE Rotation Error Mapped Onto Trajectory"))
# Plot VIO results
plot_collection.add_figure(
"VIO_APE_translation",
plot_metric(ape_metric_vio, "VIO APE Translation"))
plot_collection.add_figure(
"VIO_APE_translation_trajectory_error",
plot_traj_colormap_ape(ape_metric_vio, traj_ref_vio,
traj_est_vio, None,
"VIO ATE Mapped Onto Trajectory"))
plot_collection.add_figure(
"VIO_RPE_translation",
plot_metric(rpe_metric_trans_vio, "VIO RPE Translation"))
plot_collection.add_figure(
"VIO_RPE_translation_trajectory_error",
plot_traj_colormap_rpe(
rpe_metric_trans_vio, traj_ref_vio, traj_est_vio, None,
"VIO RPE Translation Error Mapped Onto Trajectory"))
plot_collection.add_figure(
"VIO_RPE_Rotation",
plot_metric(rpe_metric_rot_vio, "VIO RPE Rotation"))
plot_collection.add_figure(
"VIO_RPE_rotation_trajectory_error",
plot_traj_colormap_rpe(
rpe_metric_rot_vio, traj_ref_vio, traj_est_vio, None,
"VIO RPE Rotation Error Mapped Onto Trajectory"))
return [plot_collection, results_vio, results_pgo]
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,
local_logfile=args.logfile)
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 = load_trajectories(args)
if args.merge:
if args.subcommand == "kitti":
die("Can't merge KITTI files.")
if len(trajectories) == 0:
die("No trajectories to merge (excluding --ref).")
trajectories = {
"merged_trajectory": trajectory.merge(trajectories.values())
}
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
transform = file_interface.load_transform_json(tf_path)
logger.debug(SEP)
if not lie.is_se3(transform):
logger.warning("Not a valid SE(3) transformation!")
if args.invert_transform:
transform = lie.se3_inverse(transform)
logger.debug("Applying a {}-multiplicative transformation:\n{}".format(
tf_type, transform))
for traj in trajectories.values():
traj.transform(transform,
right_mul=args.transform_right,
propagate=args.propagate_transform)
if args.t_offset:
logger.debug(SEP)
for name, traj in trajectories.items():
if type(traj) is trajectory.PosePath3D:
die("{} doesn't have timestamps - can't add time offset.".
format(name))
logger.info("Adding time offset to {}: {} (s)".format(
name, args.t_offset))
traj.timestamps += args.t_offset
if args.n_to_align != -1 and not (args.align or args.correct_scale):
die("--n_to_align is useless without --align or/and --correct_scale")
if args.sync or args.align or args.correct_scale or args.align_origin:
from evo.core import sync
if not args.ref:
logger.debug(SEP)
die("Can't align or sync without a reference! (--ref) *grunt*")
for name, traj in trajectories.items():
if args.subcommand == "kitti":
ref_traj_tmp = ref_traj
else:
logger.debug(SEP)
ref_traj_tmp, trajectories[name] = sync.associate_trajectories(
ref_traj,
traj,
max_diff=args.t_max_diff,
first_name="reference",
snd_name=name)
if args.align or args.correct_scale:
logger.debug(SEP)
logger.debug("Aligning {} to reference.".format(name))
trajectories[name] = trajectory.align_trajectory(
trajectories[name],
ref_traj_tmp,
correct_scale=args.correct_scale,
correct_only_scale=args.correct_scale and not args.align,
n=args.n_to_align)
if args.align_origin:
logger.debug(SEP)
logger.debug("Aligning {}'s origin to reference.".format(name))
trajectories[name] = trajectory.align_trajectory_origin(
trajectories[name], ref_traj_tmp)
print_compact_name = not args.subcommand == "bag"
for name, traj in trajectories.items():
print_traj_info(name, traj, args.verbose, args.full_check,
print_compact_name)
if args.ref:
print_traj_info(args.ref, ref_traj, args.verbose, args.full_check,
print_compact_name)
if args.plot or args.save_plot or args.serialize_plot:
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))
plot_mode = plot.PlotMode[args.plot_mode]
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,
style=SETTINGS.plot_reference_linestyle,
color=SETTINGS.plot_reference_color,
label=short_traj_name,
alpha=SETTINGS.plot_reference_alpha)
plot.draw_coordinate_axes(ax_traj, ref_traj, plot_mode,
SETTINGS.plot_axis_marker_scale)
plot.traj_xyz(axarr_xyz,
ref_traj,
style=SETTINGS.plot_reference_linestyle,
color=SETTINGS.plot_reference_color,
label=short_traj_name,
alpha=SETTINGS.plot_reference_alpha)
plot.traj_rpy(axarr_rpy,
ref_traj,
style=SETTINGS.plot_reference_linestyle,
color=SETTINGS.plot_reference_color,
label=short_traj_name,
alpha=SETTINGS.plot_reference_alpha)
if args.ros_map_yaml:
plot.ros_map(ax_traj, args.ros_map_yaml, plot_mode)
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.items():
if cmap_colors is None:
color = next(ax_traj._get_lines.prop_cycler)['color']
else:
color = next(cmap_colors)
if print_compact_name:
short_traj_name = os.path.splitext(os.path.basename(name))[0]
else:
short_traj_name = name
if SETTINGS.plot_usetex:
short_traj_name = short_traj_name.replace("_", "\\_")
plot.traj(ax_traj,
plot_mode,
traj,
SETTINGS.plot_trajectory_linestyle,
color,
short_traj_name,
alpha=SETTINGS.plot_trajectory_alpha)
plot.draw_coordinate_axes(ax_traj, traj, plot_mode,
SETTINGS.plot_axis_marker_scale)
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,
SETTINGS.plot_trajectory_linestyle,
color,
short_traj_name,
alpha=SETTINGS.plot_trajectory_alpha,
start_timestamp=start_time)
plot.traj_rpy(axarr_rpy,
traj,
SETTINGS.plot_trajectory_linestyle,
color,
short_traj_name,
alpha=SETTINGS.plot_trajectory_alpha,
start_timestamp=start_time)
if not SETTINGS.plot_usetex:
fig_rpy.text(0.,
0.005,
"euler_angle_sequence: {}".format(
SETTINGS.euler_angle_sequence),
fontsize=6)
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.items():
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.items():
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:
import datetime
import rosbag
dest_bag_path = str(
datetime.datetime.now().strftime("%Y-%m-%d-%H-%M-%S")) + ".bag"
logger.info(SEP)
logger.info("Saving trajectories to " + dest_bag_path + "...")
bag = rosbag.Bag(dest_bag_path, 'w')
try:
for name, traj in trajectories.items():
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 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 = load_trajectories(args)
if args.merge:
if args.subcommand == "kitti":
die("Can't merge KITTI files.")
if len(trajectories) == 0:
die("No trajectories to merge (excluding --ref).")
trajectories = {
"merged_trajectory": trajectory.merge(trajectories.values())
}
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
transform = file_interface.load_transform_json(tf_path)
logger.debug(SEP)
if not lie.is_se3(transform):
logger.warning("Not a valid SE(3) transformation!")
if args.invert_transform:
transform = lie.se3_inverse(transform)
logger.debug("Applying a {}-multiplicative transformation:\n{}".format(
tf_type, transform))
for traj in trajectories.values():
traj.transform(transform, right_mul=args.transform_right)
if args.t_offset:
logger.debug(SEP)
for name, traj in trajectories.items():
if type(traj) is trajectory.PosePath3D:
die("{} doesn't have timestamps - can't add time offset.".
format(name))
logger.info("Adding time offset to {}: {} (s)".format(
name, args.t_offset))
traj.timestamps += args.t_offset
if args.sync or args.align or args.correct_scale:
from evo.core import sync
if not args.ref:
logger.debug(SEP)
die("Can't align or sync without a reference! (--ref) *grunt*")
for name, traj in trajectories.items():
if args.subcommand == "kitti":
ref_traj_tmp = ref_traj
else:
logger.debug(SEP)
ref_traj_tmp, trajectories[name] = sync.associate_trajectories(
ref_traj,
traj,
max_diff=args.t_max_diff,
first_name="reference",
snd_name=name)
if args.align or args.correct_scale:
logger.debug(SEP)
logger.debug("Aligning {} to reference.".format(name))
trajectories[name] = trajectory.align_trajectory(
trajectories[name],
ref_traj_tmp,
correct_scale=args.correct_scale,
correct_only_scale=args.correct_scale and not args.align,
n=args.n_to_align)
for name, traj in trajectories.items():
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))
ax_traj = plot.prepare_axis(fig_traj, plot_mode)
pltstart = 0
pltend = traj.num_poses
if args.plotstart:
pltstart = args.plotstart
if args.plotend != -1:
pltend = args.plotend
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,
style=SETTINGS.plot_reference_linestyle,
color=SETTINGS.plot_reference_color,
label=short_traj_name,
alpha=SETTINGS.plot_reference_alpha,
start=pltstart,
end=pltend)
plot.traj_xyz(axarr_xyz,
ref_traj,
style=SETTINGS.plot_reference_linestyle,
color=SETTINGS.plot_reference_color,
label=short_traj_name,
alpha=SETTINGS.plot_reference_alpha,
start=pltstart,
end=pltend)
plot.traj_rpy(axarr_rpy,
ref_traj,
style=SETTINGS.plot_reference_linestyle,
color=SETTINGS.plot_reference_color,
label=short_traj_name,
alpha=SETTINGS.plot_reference_alpha,
start=pltstart,
end=pltend)
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))))
fig_3 = plt.figure(figsize=tuple(SETTINGS.plot_figsize))
#plot_mode = plot.PlotMode.xz
ax = plot.prepare_axis(fig_3, plot_mode)
fig_3.axes.append(ax)
for name, traj in trajectories.items():
num = traj.positions_xyz.shape[0]
if pltstart >= num:
print(name, "plotstart > len!", num)
pltstart = 0
if pltend != -1 and (pltend > num or pltend < pltstart):
print(name, "plotend > len!", num)
pltend = traj.num_poses
pltstart = int(pltstart)
pltend = int(pltend)
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,
start=pltstart,
end=pltend)
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,
start=pltstart,
end=pltend)
plot.traj_rpy(axarr_rpy,
traj,
'-',
color,
short_traj_name,
start_timestamp=start_time,
start=pltstart,
end=pltend)
speeds = [
trajectory.calc_speed(traj.positions_xyz[i],
traj.positions_xyz[i + 1],
traj.timestamps[i],
traj.timestamps[i + 1])
for i in range(pltstart, pltend - 1)
]
speeds.append(0)
#plot.traj(ax, plot_mode, traj, '--', 'gray', 'reference')
plot.traj_colormap(ax,
traj,
speeds,
plot_mode,
min_map=min(speeds),
max_map=max(max(speeds), 10),
title="speed mapped onto trajectory",
start=pltstart,
end=pltend)
plot_collection.add_figure("trajectories", fig_traj)
plot_collection.add_figure("xyz_view", fig_xyz)
plot_collection.add_figure("rpy_view", fig_rpy)
plot_collection.add_figure("traj (speed)", fig_3)
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.items():
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.items():
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:
import datetime
import rosbag
dest_bag_path = str(
datetime.datetime.now().strftime("%Y-%m-%d-%H-%M-%S")) + ".bag"
logger.info(SEP)
logger.info("Saving trajectories to " + dest_bag_path + "...")
bag = rosbag.Bag(dest_bag_path, 'w')
try:
for name, traj in trajectories.items():
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()
