def run(args):
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
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")
# TODO: this is fugly, but is a quick solution for remembering each synced
# reference when plotting pose correspondences later...
synced = (args.subcommand == "kitti" and ref_traj) or any(
(args.sync, args.align, args.correct_scale, args.align_origin))
synced_refs = {}
if synced:
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].align(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].align_origin(ref_traj_tmp)
if SETTINGS.plot_pose_correspondences:
synced_refs[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)
# for x-axis alignment starting from 0 with --plot_relative_time
start_time = None
if args.ref:
if isinstance(ref_traj, trajectory.PoseTrajectory3D) \
and args.plot_relative_time:
start_time = ref_traj.timestamps[0]
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,
start_timestamp=start_time)
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_timestamp=start_time)
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 ref_traj and synced and SETTINGS.plot_pose_correspondences:
plot.draw_correspondence_edges(
ax_traj,
traj,
synced_refs[name],
plot_mode,
color=color,
style=SETTINGS.plot_pose_correspondences_linestyle,
alpha=SETTINGS.plot_trajectory_alpha)
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 = to_filestem(name, args) + ".tum"
file_interface.write_tum_trajectory_file(
dest, traj, confirm_overwrite=not args.no_warnings)
if args.ref:
dest = to_filestem(args.ref, args) + ".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 = to_filestem(name, args) + ".kitti"
file_interface.write_kitti_poses_file(
dest, traj, confirm_overwrite=not args.no_warnings)
if args.ref:
dest = to_filestem(args.ref, args) + ".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 = to_topic_name(name, args)
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 = to_topic_name(args.ref, args)
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()
if args.save_table:
from evo.tools import pandas_bridge
logger.debug(SEP)
df = pandas_bridge.trajectories_stats_to_df(trajectories)
pandas_bridge.save_df_as_table(df,
args.save_table,
confirm_overwrite=not args.no_warnings)
def run(args):
import sys
import pandas as pd
from evo.tools import log, user, settings, pandas_bridge
from evo.tools.settings import SETTINGS
pd.options.display.width = 80
pd.options.display.max_colwidth = 20
log.configure_logging(args.verbose,
args.silent,
args.debug,
local_logfile=args.logfile)
if args.debug:
import pprint
arg_dict = {arg: getattr(args, arg) for arg in vars(args)}
logger.debug("main_parser config:\n{}\n".format(
pprint.pformat(arg_dict)))
df = load_results_as_dataframe(args.result_files, args.use_filenames,
args.merge)
keys = df.columns.values.tolist()
if SETTINGS.plot_usetex:
keys = [key.replace("_", "\\_") for key in keys]
df.columns = keys
duplicates = [x for x in keys if keys.count(x) > 1]
if duplicates:
logger.error("Values of 'est_name' must be unique - duplicates: {}\n"
"Try using the --use_filenames option to use filenames "
"for labeling instead.".format(", ".join(duplicates)))
sys.exit(1)
# derive a common index type if possible - preferably timestamps
common_index = None
time_indices = ["timestamps", "seconds_from_start", "sec_from_start"]
if args.use_rel_time:
del time_indices[0]
for idx in time_indices:
if idx not in df.loc["np_arrays"].index:
continue
if df.loc["np_arrays", idx].isnull().values.any():
continue
else:
common_index = idx
break
# build error_df (raw values) according to common_index
if common_index is None:
# use a non-timestamp index
error_df = pd.DataFrame(df.loc["np_arrays", "error_array"].tolist(),
index=keys).T
else:
error_df = pd.DataFrame()
for key in keys:
new_error_df = pd.DataFrame(
{key: df.loc["np_arrays", "error_array"][key]},
index=df.loc["np_arrays", common_index][key])
duplicates = new_error_df.index.duplicated(keep="first")
if any(duplicates):
logger.warning(
"duplicate indices in error array of {} - "
"keeping only first occurrence of duplicates".format(key))
new_error_df = new_error_df[~duplicates]
error_df = pd.concat([error_df, new_error_df], axis=1)
# check titles
first_title = df.loc["info", "title"][0] if not args.ignore_title else ""
first_file = args.result_files[0]
if not args.no_warnings and not args.ignore_title:
checks = df.loc["info", "title"] != first_title
for i, differs in enumerate(checks):
if not differs:
continue
else:
mismatching_title = df.loc["info", "title"][i]
mismatching_file = args.result_files[i]
logger.debug(SEP)
logger.warning(
CONFLICT_TEMPLATE.format(first_file, first_title,
mismatching_title,
mismatching_file))
if not user.confirm(
"You can use --ignore_title to just aggregate data.\n"
"Go on anyway? - enter 'y' or any other key to exit"):
sys.exit()
logger.debug(SEP)
logger.debug("Aggregated dataframe:\n{}".format(
df.to_string(line_width=80)))
# show a statistics overview
logger.debug(SEP)
if not args.ignore_title:
logger.info("\n" + first_title + "\n\n")
logger.info(df.loc["stats"].T.to_string(line_width=80) + "\n")
if args.save_table:
logger.debug(SEP)
if SETTINGS.table_export_data.lower() == "error_array":
data = error_df
elif SETTINGS.table_export_data.lower() in ("info", "stats"):
data = df.loc[SETTINGS.table_export_data.lower()]
else:
raise ValueError("unsupported export data specifier: {}".format(
SETTINGS.table_export_data))
pandas_bridge.save_df_as_table(data,
args.save_table,
confirm_overwrite=not args.no_warnings)
if args.plot or args.save_plot or args.serialize_plot:
# check if data has NaN "holes" due to different indices
inconsistent = error_df.isnull().values.any()
if inconsistent and common_index != "timestamps" and not args.no_warnings:
logger.debug(SEP)
logger.warning("Data lengths/indices are not consistent, "
"raw value plot might not be correctly aligned")
from evo.tools import plot
import matplotlib.pyplot as plt
import seaborn as sns
import math
# use default plot settings
figsize = (SETTINGS.plot_figsize[0], SETTINGS.plot_figsize[1])
use_cmap = SETTINGS.plot_multi_cmap.lower() != "none"
colormap = SETTINGS.plot_multi_cmap if use_cmap else None
linestyles = ["-o" for x in args.result_files
] if args.plot_markers else None
# labels according to first dataset
if "xlabel" in df.loc["info"].index and not df.loc[
"info", "xlabel"].isnull().values.any():
index_label = df.loc["info", "xlabel"][0]
else:
index_label = "$t$ (s)" if common_index else "index"
metric_label = df.loc["info", "label"][0]
print
plot_collection = plot.PlotCollection(first_title)
# raw value plot
fig_raw = plt.figure(figsize=figsize)
# handle NaNs from concat() above
error_df.interpolate(method="index", limit_area="inside").plot(
ax=fig_raw.gca(),
colormap=colormap,
style=linestyles,
title=first_title,
alpha=SETTINGS.plot_trajectory_alpha)
plt.xlabel(index_label)
plt.ylabel(metric_label)
plt.legend(frameon=True)
plot_collection.add_figure("raw", fig_raw)
# statistics plot
if SETTINGS.plot_statistics:
fig_stats = plt.figure(figsize=figsize)
include = df.loc["stats"].index.isin(SETTINGS.plot_statistics)
if any(include):
df.loc["stats"][include].plot(kind="barh",
ax=fig_stats.gca(),
colormap=colormap,
stacked=False)
plt.xlabel(metric_label)
plt.legend(frameon=True)
plot_collection.add_figure("stats", fig_stats)
# grid of distribution plots
raw_tidy = pd.melt(error_df,
value_vars=list(error_df.columns.values),
var_name="estimate",
value_name=metric_label)
col_wrap = 2 if len(args.result_files) <= 2 else math.ceil(
len(args.result_files) / 2.0)
dist_grid = sns.FacetGrid(raw_tidy, col="estimate", col_wrap=col_wrap)
# TODO: see issue #98
import warnings
with warnings.catch_warnings():
warnings.simplefilter("ignore")
dist_grid.map(sns.distplot, metric_label) # fits=stats.gamma
plot_collection.add_figure("histogram", dist_grid.fig)
################### box plot ##############################
fig_box = plt.figure(figsize=figsize)
algorithm = str(raw_tidy["estimate"])
mono_pal = {
"tx2/vins-mono": "lightpink",
"tx2/alvio": "coral",
"tx2/rovio": "lightskyblue",
"../d1": "white",
"nx/vins-mono": "lightpink",
"nx/alvio": "coral",
"nx/rovio": "lightskyblue",
"../d2": "white",
"xavier/vins-mono": "lightpink",
"xavier/alvio": "coral",
"xavier/rovio": "lightskyblue"
}
tmp_pal = {"tx2/vins-fusion-gpu": "mediumpurple"}
stereo_pal = {
"tx2/orb2": "mediumseagreen",
"tx2/vins-fusion-gpu": "mediumpurple",
"tx2/msckf-vio": "khaki",
"tx2/kimera": "indianred",
"../d1": "white",
"nx/vins-fusion": "plum",
"nx/orb2": "mediumseagreen",
"nx/vins-fusion-imu": "hotpink",
"nx/vins-fusion-gpu": "mediumpurple",
"nx/msckf-vio": "khaki",
"nx/kimera": "indianred",
"../d2": "white",
"xavier/vins-fusion": "plum",
"xavier/orb2": "mediumseagreen",
"xavier/vins-fusion-imu": "hotpink",
"xavier/vins-fusion-gpu": "mediumpurple",
"xavier/msckf-vio": "khaki",
"xavier/kimera": "indianred"
}
# print(algorithm)
print("called")
ax = sns.boxplot(x=raw_tidy["estimate"],
y=raw_tidy[metric_label],
ax=fig_box.gca(),
palette=stereo_pal,
sym='') #color="blue")
ax.set_xticklabels(
labels=[item.get_text() for item in ax.get_xticklabels()],
rotation=30)
plot_collection.add_figure("box_plot", fig_box)
################### box plot ##############################
# violin plot
fig_violin = plt.figure(figsize=figsize)
ax = sns.violinplot(x=raw_tidy["estimate"],
y=raw_tidy[metric_label],
ax=fig_violin.gca())
# ax.set_xticklabels(labels=[item.get_text() for item in ax.get_xticklabels()], rotation=30)
plot_collection.add_figure("violin_histogram", fig_violin)
if args.plot:
plot_collection.show()
if args.save_plot:
logger.debug(SEP)
plot_collection.export(args.save_plot,
confirm_overwrite=not args.no_warnings)
if args.serialize_plot:
logger.debug(SEP)
plot_collection.serialize(args.serialize_plot,
confirm_overwrite=not args.no_warnings)
