def test_non_matching_stats_keys(self):
r1 = result.Result()
r1.add_stats({"bla": 1., "blub": 2.})
r2 = result.Result()
r2.add_stats({"foo": 1., "bar": 2.})
with self.assertRaises(result.ResultException):
result.merge_results([r1, r2])
def test_non_matching_np_arrays_keys(self):
r1 = result.Result()
r1.add_np_array("test", np.array([]))
r1.add_np_array("test_2", np.array([]))
r2 = result.Result()
r2.add_np_array("test", np.array([]))
with self.assertRaises(result.ResultException):
result.merge_results([r1, r2])
def test_merge_strategy_average(self):
r1 = result.Result()
r1.add_np_array("test", np.array([1., 2., 3.]))
r1.add_stats({"bla": 1., "blub": 2.})
r2 = result.Result()
r2.add_np_array("test", np.array([0., 0., 0.]))
r2.add_stats({"bla": 0., "blub": 0.})
merged = result.merge_results([r1, r2])
self.assertTrue(
np.array_equal(merged.np_arrays["test"], np.array([0.5, 1., 1.5])))
self.assertEqual(merged.stats, {"bla": 0.5, "blub": 1.})
def load_res_file(zip_path, load_trajectories: bool = False) -> result.Result:
"""
load contents of a result .zip file saved with save_res_file(...)
:param zip_path: path to zip file
:param load_trajectories: set to True to load also the (backup) trajectories
:return: evo.core.result.Result instance
"""
logger.debug("Loading result from {} ...".format(zip_path))
result_obj = result.Result()
with zipfile.ZipFile(zip_path, mode='r') as archive:
file_list = archive.namelist()
if not {"info.json", "stats.json"} <= set(file_list):
raise FileInterfaceException(
"{} is not a valid result file".format(zip_path))
result_obj.info = json.loads(archive.read("info.json").decode("utf-8"))
result_obj.stats = json.loads(
archive.read("stats.json").decode("utf-8"))
# Compatibility: previous evo versions wrote .npz, although it was .npy
# In any case, np.load() supports both file formats.
np_files = [f for f in file_list if f.endswith((".npy", ".npz"))]
for filename in np_files:
with io.BytesIO(archive.read(filename)) as array_buffer:
array = np.load(array_buffer)
name = os.path.splitext(os.path.basename(filename))[0]
result_obj.add_np_array(name, array)
if load_trajectories:
tum_files = [f for f in file_list if f.endswith(".tum")]
for filename in tum_files:
with io.TextIOWrapper(archive.open(filename,
mode='r')) as traj_buffer:
traj = read_tum_trajectory_file(traj_buffer)
name = os.path.splitext(os.path.basename(filename))[0]
result_obj.add_trajectory(name, traj)
kitti_files = [f for f in file_list if f.endswith(".kitti")]
for filename in kitti_files:
with io.TextIOWrapper(archive.open(filename,
mode='r')) as path_buffer:
path = read_kitti_poses_file(path_buffer)
name = os.path.splitext(os.path.basename(filename))[0]
result_obj.add_trajectory(name, path)
return result_obj
def load_res_file(zip_path, load_trajectories=False):
"""
load contents of a result .zip file saved with save_res_file(...)
:param zip_path: path to zip file
:param load_trajectories: set to True to load also the (backup) trajectories
:return: evo.core.result.Result instance
"""
logging.debug("loading result from {} ...".format(zip_path))
result_obj = result.Result()
with zipfile.ZipFile(zip_path, mode='r') as archive:
file_list = archive.namelist()
if not {"error_array.npz", "info.json", "stats.json"
} <= set(file_list):
logging.warning(
"{} has incorrect zip file structure for evo_res".format(
zip_path))
npz_files = [n for n in archive.namelist() if n.endswith(".npz")]
for filename in npz_files:
with io.BytesIO(archive.read(filename)) as f:
array = np.load(f)
name = os.path.splitext(os.path.basename(filename))[0]
result_obj.add_np_array(name, array)
if load_trajectories:
tum_files = [n for n in archive.namelist() if n.endswith(".tum")]
for filename in tum_files:
with io.TextIOWrapper(archive.open(filename, mode='r')) as f:
traj = read_tum_trajectory_file(f)
name = os.path.splitext(os.path.basename(filename))[0]
result_obj.add_trajectory(name, traj)
kitti_files = [
n for n in archive.namelist() if n.endswith(".kitti")
]
for filename in kitti_files:
with io.TextIOWrapper(archive.open(filename, mode='r')) as f:
traj = read_kitti_poses_file(f)
name = os.path.splitext(os.path.basename(filename))[0]
result_obj.add_trajectory(name, traj)
result_obj.info = json.loads(archive.read("info.json").decode("utf-8"))
result_obj.stats = json.loads(
archive.read("stats.json").decode("utf-8"))
return result_obj
def main_rpe_for_each(traj_ref, traj_est, pose_relation, mode, bins, rel_tols,
align=False, correct_scale=False, ref_name="", est_name="",
show_plot=False, save_plot=None, save_results=None, no_warnings=False,
serialize_plot=None):
from evo.core import metrics, result
from evo.core import filters
from evo.core import trajectory
from evo.tools import file_interface
from evo.tools.settings import SETTINGS
if not bins or not rel_tols:
raise RuntimeError("bins and tolerances must have more than one element")
if len(bins) != len(rel_tols):
raise RuntimeError("bins and tolerances must have the same number of elements")
if mode in {"speed", "angular_speed"} and traj_est is trajectory.PosePath3D:
raise RuntimeError("timestamps are required for mode: " + mode)
bin_unit = None
if mode == "speed":
bin_unit = metrics.VelUnit.meters_per_sec
elif mode == "path":
bin_unit = metrics.Unit.meters
elif mode == "angle":
bin_unit = metrics.Unit.degrees
elif mode == "angular_speed":
bin_unit = metrics.VelUnit.degrees_per_sec
rpe_unit = None
if pose_relation is metrics.PoseRelation.translation_part:
rpe_unit = metrics.Unit.meters
elif pose_relation is metrics.PoseRelation.rotation_angle_deg:
rpe_unit = metrics.Unit.degrees
elif pose_relation is metrics.PoseRelation.rotation_angle_rad:
rpe_unit = metrics.Unit.radians
correct_only_scale = correct_scale and not align
if align or correct_scale:
logger.debug(SEP)
if correct_only_scale:
logger.debug("Correcting scale...")
else:
logger.debug("Aligning using Umeyama's method..."
+ (" (with scale correction)" if correct_scale else ""))
traj_est = trajectory.align_trajectory(traj_est, traj_ref, correct_scale,
correct_only_scale)
results = []
for bin, rel_tol, in zip(bins, rel_tols):
logger.debug(SEP)
logger.info(
"Calculating RPE for each sub-sequence of " + str(bin) + " (" + bin_unit.value + ")")
tol = bin * rel_tol
id_pairs = []
if mode == "path":
id_pairs = filters.filter_pairs_by_path(traj_ref.poses_se3, bin, tol, all_pairs=True)
elif mode == "angle":
id_pairs = filters.filter_pairs_by_angle(traj_ref.poses_se3, bin, tol, degrees=True)
elif mode == "speed":
id_pairs = filters.filter_pairs_by_speed(traj_ref.poses_se3, traj_ref.timestamps,
bin, tol)
elif mode == "angular_speed":
id_pairs = filters.filter_pairs_by_angular_speed(traj_ref.poses_se3,
traj_ref.timestamps, bin, tol, True)
if len(id_pairs) == 0:
raise RuntimeError("bin " + str(bin) + " (" + str(bin_unit.value) + ") "
+ "produced empty index list - try other values")
# calculate RPE with all IDs (delta 1 frames)
data = (traj_ref, traj_est)
# the delta here has nothing to do with the bin - 1f delta just to use all poses of the bin
rpe_metric = metrics.RPE(pose_relation, delta=1, delta_unit=metrics.Unit.frames,
all_pairs=True)
rpe_metric.process_data(data, id_pairs)
mean = rpe_metric.get_statistic(metrics.StatisticsType.mean)
results.append(mean)
if SETTINGS.plot_usetex:
mode.replace("_", "\_")
title = "mean RPE w.r.t. " + pose_relation.value + "\nfor different " + mode + " sub-sequences"
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 correct_only_scale:
title += "\n(scale corrected)"
else:
title += "\n(not aligned)"
rpe_for_each_result = result.Result()
rpe_for_each_result.add_info({
"label": "RPE ({})".format(rpe_unit.value),
"est_name": est_name,
"ref_name": ref_name,
"title": title,
"xlabel": "{} sub-sequences ({})".format(mode, bin_unit.value)
})
rpe_for_each_result.add_stats({bin: result for bin, result in zip(bins, results)})
# TODO use a more suitable name than seconds
rpe_for_each_result.add_np_array("seconds_from_start", bins)
rpe_for_each_result.add_np_array("error_array", results)
logger.debug(SEP)
logger.info(rpe_for_each_result.pretty_str())
if show_plot or save_plot or serialize_plot:
from evo.tools import plot
import matplotlib.pyplot as plt
plot_collection = plot.PlotCollection(title)
fig = plt.figure(figsize=(SETTINGS.plot_figsize[0], SETTINGS.plot_figsize[1]))
plot.error_array(fig, results, x_array=bins,
name="mean RPE" + (" (" + rpe_unit.value + ")") if rpe_unit else "",
marker="o", title=title,
xlabel=mode + " sub-sequences " + " (" + bin_unit.value + ")")
# info text
if SETTINGS.plot_info_text and est_name and ref_name:
ax = fig.gca()
ax.text(0, -0.12, "estimate: " + est_name + "\nreference: " + ref_name,
transform=ax.transAxes, fontsize=8, color="gray")
plt.title(title)
plot_collection.add_figure("raw", fig)
if show_plot:
plot_collection.show()
if save_plot:
plot_collection.export(save_plot, confirm_overwrite=not no_warnings)
if serialize_plot:
logger.debug(SEP)
plot_collection.serialize(serialize_plot, confirm_overwrite=not no_warnings)
if save_results:
logger.debug(SEP)
if SETTINGS.save_traj_in_zip:
rpe_for_each_result.add_trajectory("traj_ref", traj_ref)
rpe_for_each_result.add_trajectory("traj_est", traj_est)
file_interface.save_res_file(save_results, rpe_for_each_result,
confirm_overwrite=not no_warnings)
return rpe_for_each_result
