def run(args):
import evo.common_ape_rpe as common
from evo.tools import file_interface, log
from evo.tools.settings import SETTINGS
log.configure_logging(args.verbose, args.silent, args.debug)
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)
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,
ref_name=ref_name,
est_name=est_name,
)
if args.plot or args.save_plot or args.serialize_plot:
common.plot(
args, result,
result.trajectories[ref_name],
result.trajectories[est_name])
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):
import evo.common_ape_rpe as common
from evo.tools import file_interface, log
from evo.tools.settings import SETTINGS
log.configure_logging(args.verbose, args.silent, args.debug)
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)
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,
ref_name=ref_name,
est_name=est_name,
)
if args.plot or args.save_plot or args.serialize_plot:
common.plot(args, result, result.trajectories[ref_name],
result.trajectories[est_name])
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)
#!/usr/bin/env python from __future__ import print_function import argparse import copy from evo.tools import log log.configure_logging() import numpy as np import os from tqdm import tqdm import openpyxl from openpyxl import Workbook from openpyxl.styles import Border, Side, PatternFill, Font, GradientFill, Alignment from openpyxl.utils.dataframe import dataframe_to_rows from openpyxl.utils import get_column_letter import pandas as pd from evo.tools import plot from evo.tools.plot import PlotMode from evo.tools.settings import SETTINGS from evo.core.metrics import PoseRelation, Unit from evo.core import metrics from evo.core import sync from evo.core import trajectory # temporarily override some package settings SETTINGS.plot_figsize = [6, 6] SETTINGS.plot_split = True SETTINGS.plot_usetex = False
def run(args):
from evo.core import metrics
from evo.tools import file_interface, log
log.configure_logging(args.verbose, args.silent, args.debug)
if args.debug:
import pprint
logger.debug(
"main_parser config:\n" +
pprint.pformat({arg: getattr(args, arg)
for arg in vars(args)}) + "\n")
logger.debug(SEP)
pose_relation = None
if args.pose_relation == "full":
pose_relation = metrics.PoseRelation.full_transformation
elif args.pose_relation == "rot_part":
pose_relation = metrics.PoseRelation.rotation_part
elif args.pose_relation == "trans_part":
pose_relation = metrics.PoseRelation.translation_part
elif args.pose_relation == "angle_deg":
pose_relation = metrics.PoseRelation.rotation_angle_deg
elif args.pose_relation == "angle_rad":
pose_relation = metrics.PoseRelation.rotation_angle_rad
traj_ref, traj_est, stamps_est = None, None, None
ref_name, est_name = "", ""
plot_mode = None # no plot imports unless really needed (slow)
if args.subcommand == "tum":
traj_ref, traj_est = file_interface.load_assoc_tum_trajectories(
args.ref_file,
args.est_file,
args.t_max_diff,
args.t_offset,
)
ref_name, est_name = args.ref_file, args.est_file
if args.plot or args.save_plot:
from evo.tools.plot import PlotMode
plot_mode = PlotMode.xyz if not args.plot_mode else PlotMode[
args.plot_mode]
elif args.subcommand == "kitti":
traj_ref = file_interface.read_kitti_poses_file(args.ref_file)
traj_est = file_interface.read_kitti_poses_file(args.est_file)
ref_name, est_name = args.ref_file, args.est_file
if args.plot or args.save_plot:
from evo.tools.plot import PlotMode
plot_mode = PlotMode.xz if not args.plot_mode else PlotMode[
args.plot_mode]
elif args.subcommand == "euroc":
args.align = True
logger.info(
"forcing trajectory alignment implicitly (EuRoC ground truth is in IMU frame)"
)
logger.debug(SEP)
traj_ref, traj_est = file_interface.load_assoc_euroc_trajectories(
args.state_gt_csv,
args.est_file,
args.t_max_diff,
args.t_offset,
)
ref_name, est_name = args.state_gt_csv, args.est_file
if args.plot or args.save_plot:
from evo.tools.plot import PlotMode
plot_mode = PlotMode.xyz if not args.plot_mode else PlotMode[
args.plot_mode]
elif args.subcommand == "bag":
import rosbag
logger.debug("opening bag file " + args.bag)
bag = rosbag.Bag(args.bag, 'r')
try:
traj_ref, traj_est = file_interface.load_assoc_bag_trajectories(
bag,
args.ref_topic,
args.est_topic,
args.t_max_diff,
args.t_offset,
)
finally:
bag.close()
ref_name, est_name = args.ref_topic, args.est_topic
if args.plot or args.save_plot:
from evo.tools.plot import PlotMode
plot_mode = PlotMode.xy if not args.plot_mode else PlotMode[
args.plot_mode]
main_ape(
traj_ref=traj_ref,
traj_est=traj_est,
pose_relation=pose_relation,
align=args.align,
correct_scale=args.correct_scale,
ref_name=ref_name,
est_name=est_name,
show_plot=args.plot,
save_plot=args.save_plot,
plot_mode=plot_mode,
save_results=args.save_results,
no_warnings=args.no_warnings,
serialize_plot=args.serialize_plot,
plot_colormap_max=args.plot_colormap_max,
plot_colormap_min=args.plot_colormap_min,
plot_colormap_max_percentile=args.plot_colormap_max_percentile,
)
def run(args):
import sys
import pandas as pd
from evo.tools import file_interface, 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)
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 = pd.DataFrame()
for result_file in args.result_files:
result = file_interface.load_res_file(result_file)
name = result_file if args.use_filenames else None
df = pd.concat([df, pandas_bridge.result_to_df(result, name)],
axis="columns")
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]
first_file = args.result_files[0]
if not args.no_warnings:
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(
"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)
logger.info("\n{}\n\n{}\n".format(
first_title, df.loc["stats"].T.to_string(line_width=80)))
if args.save_table:
logger.debug(SEP)
if args.no_warnings or user.check_and_confirm_overwrite(
args.save_table):
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))
if SETTINGS.table_export_transpose:
data = data.T
if SETTINGS.table_export_format == "excel":
writer = pd.ExcelWriter(args.save_table)
data.to_excel(writer)
writer.save()
writer.close()
else:
getattr(data,
"to_" + SETTINGS.table_export_format)(args.save_table)
logger.debug("{} table saved to: {}".format(
SETTINGS.table_export_format, args.save_table))
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
title = first_title
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]
plot_collection = plot.PlotCollection(title)
# raw value plot
fig_raw = plt.figure(figsize=figsize)
# handle NaNs from concat() above
error_df.interpolate(method="index").plot(ax=fig_raw.gca(),
colormap=colormap,
style=linestyles,
title=first_title)
plt.xlabel(index_label)
plt.ylabel(metric_label)
plt.legend(frameon=True)
plot_collection.add_figure("raw", fig_raw)
# statistics plot
fig_stats = plt.figure(figsize=figsize)
exclude = df.loc["stats"].index.isin(["sse"]) # don't plot sse
df.loc["stats"][~exclude].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)
ax = sns.boxplot(x=raw_tidy["estimate"],
y=raw_tidy[metric_label],
ax=fig_box.gca())
# ax.set_xticklabels(labels=[item.get_text() for item in ax.get_xticklabels()], rotation=30)
plot_collection.add_figure("box_plot", fig_box)
# 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)
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()
def run(args: argparse.Namespace) -> None:
print('using this main_ape')
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)
print('align_odom: ', args.align_odom)
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,
align_odom=args.align_odom,
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)
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 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)
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,
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 main():
import argparse
import argcomplete
basic_desc = "experimental tool for opening a serialized PlotCollection"
lic = "(c) evo authors"
main_parser = argparse.ArgumentParser(description="%s %s" %
(basic_desc, lic))
main_parser.add_argument("in_file",
help="path to a serialized plot_collection")
main_parser.add_argument("-t",
"--title",
help="custom title (default: file name)")
main_parser.add_argument("--save_plot",
help="path to save plot",
default=None)
main_parser.add_argument("--serialize_plot",
help="path to re-serialize PlotCollection",
default=None)
main_parser.add_argument("--to_html",
help="convert to html (requires mpld3 library)",
action="store_true")
main_parser.add_argument("--no_warnings",
help="no warnings requiring user confirmation",
action="store_true")
argcomplete.autocomplete(main_parser)
args = main_parser.parse_args()
from evo.tools import log, plot, user
log.configure_logging(verbose=True)
if not args.title:
title = os.path.basename(args.in_file)
else:
title = args.title
if not args.no_warnings:
logger.warning(
"This tool is experimental and not guranteed to work.\nOnly works "
"if the same plot settings are used as for serialization.\n"
"If not, try: evo_config show/set \n" + SEP)
plot_collection = plot.PlotCollection(title, deserialize=args.in_file)
logger.debug("Deserialized PlotCollection: " + str(plot_collection))
plot_collection.show()
if args.serialize_plot:
logger.debug(SEP)
plot_collection.serialize(args.serialize_plot,
confirm_overwrite=not args.no_warnings)
if args.save_plot:
logger.debug(SEP)
plot_collection.export(args.save_plot,
confirm_overwrite=not args.no_warnings)
if args.to_html:
import mpld3
logger.debug(SEP + "\nhtml export\n")
for name, fig in plot_collection.figures.items():
html = mpld3.fig_to_html(fig)
out = name + ".html"
with open(out, 'w') as f:
logger.debug(out)
f.write(html)
if not args.no_warnings:
logger.debug(SEP)
if user.confirm("Save changes & overwrite original file " +
args.in_file + "? (y/n)"):
plot_collection.serialize(args.in_file, confirm_overwrite=False)
def main():
import argcomplete
basic_desc = "crappy configuration tool"
lic = "(c) [email protected]"
shared_parser = argparse.ArgumentParser(add_help=False)
shared_parser.add_argument("--no_color", help="don't color output", action="store_true")
main_parser = argparse.ArgumentParser(description="%s %s" % (basic_desc, lic))
sub_parsers = main_parser.add_subparsers(dest="subcommand")
sub_parsers.required = True
show_parser = sub_parsers.add_parser("show", description="show configuration - %s" % lic,
parents=[shared_parser])
show_parser.add_argument("config",
help="optional config file to display (default: package settings)",
nargs='?')
show_parser.add_argument("--brief", help="show only the .json data",
action="store_true")
set_parser = sub_parsers.add_parser("set", description=SET_HELP, parents=[shared_parser],
formatter_class=argparse.RawTextHelpFormatter)
set_parser.add_argument("params", choices=list(DEFAULT_SETTINGS_DICT.keys()),
nargs=argparse.REMAINDER, help="parameters to set")
set_parser.add_argument("-c", "--config",
help="optional config file (default: package settings)", default=None)
set_parser.add_argument("-m", "--merge",
help="other config file to merge in (priority)", default=None)
set_parser.add_argument("--soft", help="do a soft-merge (no overwriting)", action="store_true")
gen_parser = sub_parsers.add_parser("generate", description=GENERATE_HELP,
parents=[shared_parser],
formatter_class=argparse.RawTextHelpFormatter)
gen_parser.add_argument("-o", "--out", help="path for config file to generate")
reset_parser = sub_parsers.add_parser("reset", description="reset package settings - %s" % lic,
parents=[shared_parser])
reset_parser.add_argument("-y", help="acknowledge automatically", action="store_true")
argcomplete.autocomplete(main_parser)
if len(sys.argv) > 1 and sys.argv[1] == "set":
args, other_args = main_parser.parse_known_args()
other_args = [arg for arg in sys.argv[2:] if not arg.startswith('-')]
else:
args, other_args = main_parser.parse_known_args()
log.configure_logging()
colorama.init()
config = settings.DEFAULT_PATH
if hasattr(args, "config"):
if args.config:
config = args.config
if args.subcommand == "show":
if not args.brief and not args.config:
style = Style.BRIGHT if not args.no_color else Style.NORMAL
doc_str = "\n".join("{0}{1}{2}:\n{3}\n".format(style, k, Style.RESET_ALL, v[1])
for k, v in sorted(DEFAULT_SETTINGS_DICT_DOC.items()))
logger.info(doc_str)
logger.info("{0}\n{1}\n{0}".format(SEP, config))
show(config, colored=not args.no_color)
if config == settings.DEFAULT_PATH and not args.brief:
logger.info(SEP + "\nSee text above for parameter descriptions.")
elif args.subcommand == "set":
if not os.access(config, os.W_OK):
logger.error("No permission to modify " + config)
sys.exit()
if other_args or args.merge:
logger.info("{0}\nOld configuration:\n{0}".format(SEP))
show(config, colored=not args.no_color)
try:
set_cfg(config, other_args)
except ConfigError as e:
logger.error(e)
sys.exit(1)
if args.merge:
merge_json_union(config, args.merge, args.soft)
logger.info(SEP + "\nNew configuration:\n" + SEP)
show(config, colored=not args.no_color)
else:
logger.error("No configuration parameters given (see --help).")
elif args.subcommand == "generate":
if other_args:
logger.info("{0}\nParsed by argparse:\n{1}\n"
"{0}\nWARNING:\n"
"Make sure you use the 'long-style' -- options (e.g. --plot) if possible\n"
"and no combined short '-' flags, (e.g. -vp)\n{0}".format(SEP, other_args))
data = generate(other_args)
log_info_dict_json(data, colored=not args.no_color)
if args.out and user.check_and_confirm_overwrite(args.out):
with open(args.out, 'w') as out:
out.write(json.dumps(data, indent=4, sort_keys=True))
elif not args.out:
logger.warning("\n(-o | --out) not specified - saving nothing")
else:
logger.error("No command line arguments given (see --help)")
elif args.subcommand == "reset":
if not os.access(config, os.W_OK):
logger.error("No permission to modify" + config)
sys.exit()
if args.y or user.confirm("Reset the package settings to the default settings? (y/n)"):
settings.reset()
logger.info("{0}\nPackage settings after reset:\n{0}".format(SEP))
show(settings.DEFAULT_PATH, colored=not args.no_color)
def run(args):
import sys
from evo.core import metrics
from evo.tools import file_interface, log
# manually check bins and tols arguments to allow them to be in config files
if not args.bins or not args.tols:
logger.error("The following arguments are required: -b/--bins, -t/--tols")
sys.exit(1)
log.configure_logging(args.verbose, args.silent, args.debug)
if args.debug:
import pprint
logger.debug("main_parser config:\n"
+ pprint.pformat({arg: getattr(args, arg) for arg in vars(args)}) + "\n")
logger.debug(SEP)
pose_relation = None
if args.pose_relation == "trans_part":
pose_relation = metrics.PoseRelation.translation_part
elif args.pose_relation == "angle_deg":
pose_relation = metrics.PoseRelation.rotation_angle_deg
elif args.pose_relation == "angle_rad":
pose_relation = metrics.PoseRelation.rotation_angle_rad
traj_ref, traj_est, stamps_est = None, None, None
ref_name, est_name = "", ""
if args.subcommand == "tum":
traj_ref, traj_est = file_interface.load_assoc_tum_trajectories(
args.ref_file,
args.est_file,
args.t_max_diff,
args.t_offset,
)
ref_name, est_name = args.ref_file, args.est_file
elif args.subcommand == "kitti":
traj_ref = file_interface.read_kitti_poses_file(args.ref_file)
traj_est = file_interface.read_kitti_poses_file(args.est_file)
ref_name, est_name = args.ref_file, args.est_file
elif args.subcommand == "euroc":
args.align = True
logger.info("Forcing trajectory alignment implicitly (EuRoC ground truth is in IMU frame)")
logger.debug(SEP)
traj_ref, traj_est = file_interface.load_assoc_euroc_trajectories(
args.state_gt_csv,
args.est_file,
args.t_max_diff,
args.t_offset,
)
ref_name, est_name = args.state_gt_csv, args.est_file
elif args.subcommand == "bag":
import rosbag
logger.debug("Opening bag file {} ...".format(args.bag))
bag = rosbag.Bag(args.bag, 'r')
try:
traj_ref, traj_est = file_interface.load_assoc_bag_trajectories(
bag,
args.ref_topic,
args.est_topic,
args.t_max_diff,
args.t_offset,
)
finally:
bag.close()
ref_name, est_name = args.ref_topic, args.est_topic
main_rpe_for_each(traj_ref, traj_est, pose_relation, args.mode, args.bins, args.tols,
args.align, args.correct_scale, ref_name, est_name,
args.plot, args.save_plot, args.save_results, args.no_warnings,
serialize_plot=args.serialize_plot)
You should have received a copy of the GNU General Public License
along with evo. If not, see <http://www.gnu.org/licenses/>.
"""
import logging
import sys
import evo.core.lie_algebra as lie
from evo.core import trajectory
from evo.tools import plot, file_interface, log
import numpy as np
import matplotlib.pyplot as plt
logger = logging.getLogger("evo")
log.configure_logging(verbose=True)
traj_ref = file_interface.read_kitti_poses_file("../test/data/KITTI_00_gt.txt")
traj_est = file_interface.read_kitti_poses_file(
"../test/data/KITTI_00_ORB.txt")
# add artificial Sim(3) transformation
traj_est.transform(lie.se3(np.eye(3), [0, 0, 0]))
traj_est.scale(0.5)
logger.info("\nUmeyama alignment without scaling")
traj_est_aligned = trajectory.align_trajectory(traj_est, traj_ref)
logger.info("\nUmeyama alignment with scaling")
traj_est_aligned_scaled = trajectory.align_trajectory(traj_est,
traj_ref,
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.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
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)
for name, traj in trajectories.items():
print_traj_info(to_compact_name(name, args), traj, args.verbose,
args.full_check)
if args.ref:
print_traj_info(to_compact_name(args.ref, args), ref_traj,
args.verbose, args.full_check)
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 = to_compact_name(args.ref, args,
SETTINGS.plot_usetex)
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_reference_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)
short_traj_name = to_compact_name(name, args, SETTINGS.plot_usetex)
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 or args.save_as_bag2:
from rosbags.rosbag1 import Writer as Rosbag1Writer
from rosbags.rosbag2 import Writer as Rosbag2Writer
writers = []
if args.save_as_bag:
dest_bag_path = str(
datetime.datetime.now().strftime("%Y-%m-%d-%H-%M-%S")) + ".bag"
writers.append(Rosbag1Writer(dest_bag_path))
if args.save_as_bag2:
dest_bag_path = str(
datetime.datetime.now().strftime("%Y-%m-%d-%H-%M-%S"))
writers.append(Rosbag2Writer(dest_bag_path))
for writer in writers:
logger.info(SEP)
logger.info("Saving trajectories to " + str(writer.path) + "...")
try:
writer.open()
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(
writer, 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(
writer, ref_traj, dest_topic, frame_id)
finally:
writer.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 main():
import argcomplete
basic_desc = "crappy configuration tool"
lic = "(c) [email protected]"
shared_parser = argparse.ArgumentParser(add_help=False)
shared_parser.add_argument("--no_color",
help="don't color output",
action="store_true")
main_parser = argparse.ArgumentParser(description="%s %s" %
(basic_desc, lic))
sub_parsers = main_parser.add_subparsers(dest="subcommand")
sub_parsers.required = True
show_parser = sub_parsers.add_parser(
"show",
description="show configuration - %s" % lic,
parents=[shared_parser])
show_parser.add_argument(
"config",
help="optional config file to display (default: package settings)",
nargs='?')
show_parser.add_argument("--brief",
help="show only the .json data",
action="store_true")
set_parser = sub_parsers.add_parser(
"set",
description=SET_HELP,
parents=[shared_parser],
formatter_class=argparse.RawTextHelpFormatter)
set_parser.add_argument("params",
choices=list(DEFAULT_SETTINGS_DICT.keys()),
nargs=argparse.REMAINDER,
help="parameters to set")
set_parser.add_argument(
"-c",
"--config",
help="optional config file (default: package settings)",
default=None)
set_parser.add_argument("-m",
"--merge",
help="other config file to merge in (priority)",
default=None)
set_parser.add_argument("--soft",
help="do a soft-merge (no overwriting)",
action="store_true")
gen_parser = sub_parsers.add_parser(
"generate",
description=GENERATE_HELP,
parents=[shared_parser],
formatter_class=argparse.RawTextHelpFormatter)
gen_parser.add_argument("-o",
"--out",
help="path for config file to generate")
reset_parser = sub_parsers.add_parser(
"reset",
description="reset package settings - %s" % lic,
parents=[shared_parser])
reset_parser.add_argument("-y",
help="acknowledge automatically",
action="store_true")
argcomplete.autocomplete(main_parser)
if len(sys.argv) > 1 and sys.argv[1] == "set":
args, other_args = main_parser.parse_known_args()
other_args = [arg for arg in sys.argv[2:] if not arg.startswith('-')]
else:
args, other_args = main_parser.parse_known_args()
log.configure_logging()
colorama.init()
config = settings.DEFAULT_PATH
if hasattr(args, "config"):
if args.config:
config = args.config
if args.subcommand == "show":
if not args.brief and not args.config:
style = Style.BRIGHT if not args.no_color else Style.NORMAL
doc_str = "\n".join(
"{0}{1}{2}:\n{3}\n".format(style, k, Style.RESET_ALL, v[1])
for k, v in sorted(DEFAULT_SETTINGS_DICT_DOC.items()))
logger.info(doc_str)
logger.info("{0}\n{1}\n{0}".format(SEP, config))
show(config, colored=not args.no_color)
if config == settings.DEFAULT_PATH and not args.brief:
logger.info(SEP + "\nSee text above for parameter descriptions.")
elif args.subcommand == "set":
if not os.access(config, os.W_OK):
logger.error("No permission to modify " + config)
sys.exit()
if other_args or args.merge:
logger.info("{0}\nOld configuration:\n{0}".format(SEP))
show(config, colored=not args.no_color)
try:
set_cfg(config, other_args)
except ConfigError as e:
logger.error(e)
sys.exit(1)
if args.merge:
merge_json_union(config, args.merge, args.soft)
logger.info(SEP + "\nNew configuration:\n" + SEP)
show(config, colored=not args.no_color)
else:
logger.error("No configuration parameters given (see --help).")
elif args.subcommand == "generate":
if other_args:
logger.info(
"{0}\nParsed by argparse:\n{1}\n"
"{0}\nWARNING:\nMake sure you use the 'long-style' -- options "
"(e.g. --plot) if possible\nand no combined short '-' flags, "
"(e.g. -vp)\n{0}".format(SEP, other_args))
data = generate(other_args)
log_info_dict_json(data, colored=not args.no_color)
if args.out and user.check_and_confirm_overwrite(args.out):
with open(args.out, 'w') as out:
out.write(json.dumps(data, indent=4, sort_keys=True))
elif not args.out:
logger.warning("\n(-o | --out) not specified - saving nothing")
else:
logger.error("No command line arguments given (see --help)")
elif args.subcommand == "reset":
if not os.access(config, os.W_OK):
logger.error("No permission to modify" + config)
sys.exit()
if args.y or user.confirm(
"Reset the package settings to the default settings? (y/n)"):
settings.reset()
logger.info("{0}\nPackage settings after reset:\n{0}".format(SEP))
show(settings.DEFAULT_PATH, colored=not args.no_color)
def main():
import sys
import argparse
import argcomplete
main_parser = argparse.ArgumentParser()
shared_parser = argparse.ArgumentParser(add_help=False)
sub_parsers = main_parser.add_subparsers(dest="subcommand")
sub_parsers.required = True
pkg_parser = sub_parsers.add_parser(
"pkg",
description="show infos of the package",
parents=[shared_parser])
pkg_parser.add_argument("--info",
help="show the package description",
action="store_true")
pkg_parser.add_argument("--version",
help="print the package version",
action="store_true")
pkg_parser.add_argument("--pyversion",
help="print the Python version",
action="store_true")
pkg_parser.add_argument("--license",
help="print the package license",
action="store_true")
pkg_parser.add_argument("--location",
help="print the package path",
action="store_true")
pkg_parser.add_argument("--logfile",
help="print the logfile path",
action="store_true")
pkg_parser.add_argument("--open_log",
help="open the package logfile",
action="store_true")
pkg_parser.add_argument("--clear_log",
help="clear package logfile",
action="store_true")
cat_parser = sub_parsers.add_parser(
"cat_log",
description="pipe stdin to evo logfile"
" or print logfile to stdout (if no stdin)",
parents=[shared_parser])
cat_parser.add_argument("-l",
"--loglevel",
help="loglevel of the message",
default="info",
choices=["error", "warning", "info", "debug"])
cat_parser.add_argument("-m",
"--message",
help="explicit message instead of pipe")
cat_parser.add_argument("-s",
"--source",
help="source name to use for the log message")
cat_parser.add_argument("--clear_log",
help="clear logfile before exiting",
action="store_true")
argcomplete.autocomplete(main_parser)
if len(sys.argv[1:]) == 0:
sys.argv.extend(["pkg", "--info"]) # cheap trick because YOLO
args = main_parser.parse_args()
line_end = "\n" if sys.stdout.isatty() else ""
if args.subcommand == "pkg":
if not len(sys.argv) > 2:
pkg_parser.print_help()
sys.exit(1)
if args.license:
print(
open(os.path.join(settings.PACKAGE_BASE_PATH,
"LICENSE")).read())
if args.info:
main_parser.print_usage()
print(DESC)
if args.version:
here = os.path.dirname(os.path.abspath(__file__))
version_path = os.path.join(here, "version")
print(" ".join(open(version_path).read().splitlines()),
end=line_end)
if args.pyversion:
import platform as pf
print(pf.python_version(), end=line_end)
if args.location:
print(settings.PACKAGE_BASE_PATH, end=line_end)
if args.logfile or args.open_log:
if not os.path.exists(settings.DEFAULT_LOGFILE_PATH):
print("no logfile found - run: evo_config set logfile_enabled",
end=line_end)
sys.exit(1)
print(settings.DEFAULT_LOGFILE_PATH, end=line_end)
if args.open_log:
import webbrowser
webbrowser.open(settings.DEFAULT_LOGFILE_PATH)
if args.clear_log:
from evo.tools import user
if user.confirm("clear logfile? (y/n)"):
open(settings.DEFAULT_LOGFILE_PATH, mode='w')
elif args.subcommand == "cat_log":
if os.name == "nt":
print("cat_log feature not available on Windows")
sys.exit(1)
if not args.message and sys.stdin.isatty():
if not os.path.exists(settings.DEFAULT_LOGFILE_PATH):
print("no logfile found - run: evo_config set logfile_enabled",
end=line_end)
else:
print(open(settings.DEFAULT_LOGFILE_PATH).read(), end="")
elif not settings.SETTINGS.logfile_enabled:
print("logfile disabled", end=line_end)
sys.exit(1)
else:
import logging
logger = logging.getLogger(__name__)
from evo.tools import log
file_fmt = log.DEFAULT_LONG_FMT
if args.source:
file_fmt = file_fmt.replace(
"%(module)s.%(funcName)s():%(lineno)s", args.source)
log.configure_logging(silent=True, file_fmt=file_fmt)
if not args.message:
msg = sys.stdin.read()
else:
msg = args.message
getattr(logger, args.loglevel)(msg)
if args.clear_log:
open(settings.DEFAULT_LOGFILE_PATH, mode='w')
def run(args):
import os
import sys
import numpy as np
import evo.core.lie_algebra as lie
from evo.core import trajectory
from evo.core.trajectory import PoseTrajectory3D
from evo.tools import file_interface, log
from evo.tools.settings import SETTINGS
log.configure_logging(verbose=args.verbose,
silent=args.silent,
debug=args.debug)
if args.debug:
import pprint
logger.debug(
"main_parser config:\n" +
pprint.pformat({arg: getattr(args, arg)
for arg in vars(args)}) + "\n")
logger.debug(SEP)
trajectories = []
ref_traj = None
if args.subcommand == "tum":
for traj_file in args.traj_files:
if traj_file != args.ref:
trajectories.append(
(traj_file,
file_interface.read_tum_trajectory_file(traj_file)))
if args.ref:
ref_traj = file_interface.read_tum_trajectory_file(args.ref)
elif args.subcommand == "kitti":
for pose_file in args.pose_files:
if pose_file != args.ref:
trajectories.append(
(pose_file,
file_interface.read_kitti_poses_file(pose_file)))
if args.ref:
ref_traj = file_interface.read_kitti_poses_file(args.ref)
elif args.subcommand == "euroc":
for csv_file in args.state_gt_csv:
if csv_file != args.ref:
trajectories.append(
(csv_file,
file_interface.read_euroc_csv_trajectory(csv_file)))
if args.ref:
ref_traj = file_interface.read_euroc_csv_trajectory(args.ref)
elif args.subcommand == "bag":
import rosbag
bag = rosbag.Bag(args.bag)
try:
if args.all_topics:
topic_info = bag.get_type_and_topic_info()
topics = sorted([
t for t in topic_info[1].keys()
if topic_info[1][t][0] == "geometry_msgs/PoseStamped"
and t != args.ref
])
if len(topics) == 0:
logger.error("No geometry_msgs/PoseStamped topics found!")
sys.exit(1)
else:
topics = args.topics
if not topics:
logger.warning(
"No topics used - specify topics or set --all_topics.")
sys.exit(1)
for topic in topics:
trajectories.append(
(topic, file_interface.read_bag_trajectory(bag, topic)))
if args.ref:
ref_traj = file_interface.read_bag_trajectory(bag, args.ref)
finally:
bag.close()
else:
raise RuntimeError("unsupported subcommand: " + args.subcommand)
if args.merge:
if args.subcommand == "kitti":
raise TypeError(
"can't merge KITTI files - but you can append them with 'cat'")
if len(trajectories) == 0:
raise RuntimeError("no trajectories to merge (excluding --ref)")
merged_stamps = trajectories[0][1].timestamps
merged_xyz = trajectories[0][1].positions_xyz
merged_quat = trajectories[0][1].orientations_quat_wxyz
for _, traj in trajectories[1:]:
merged_stamps = np.concatenate((merged_stamps, traj.timestamps))
merged_xyz = np.concatenate((merged_xyz, traj.positions_xyz))
merged_quat = np.concatenate(
(merged_quat, traj.orientations_quat_wxyz))
order = merged_stamps.argsort()
merged_stamps = merged_stamps[order]
merged_xyz = merged_xyz[order]
merged_quat = merged_quat[order]
trajectories = [("merged_trajectory",
PoseTrajectory3D(merged_xyz, merged_quat,
merged_stamps))]
if args.transform_left or args.transform_right:
tf_type = "left" if args.transform_left else "right"
tf_path = args.transform_left if args.transform_left else args.transform_right
t, xyz, quat = file_interface.load_transform_json(tf_path)
logger.debug(SEP)
if not lie.is_se3(t):
logger.warning("Not a valid SE(3) transformation!")
if args.invert_transform:
t = lie.se3_inverse(t)
logger.debug("Applying a {}-multiplicative transformation:\n{}".format(
tf_type, t))
for name, traj in trajectories:
traj.transform(t, right_mul=args.transform_right)
if args.t_offset:
logger.debug(SEP)
for name, traj in trajectories:
if type(traj) is trajectory.PosePath3D:
logger.warning(
"{} doesn't have timestamps - can't add time offset.".
format(name))
else:
logger.info("Adding time offset to {}: {} (s)".format(
name, args.t_offset))
traj.timestamps += args.t_offset
if args.align or args.correct_scale:
if not args.ref:
logger.debug(SEP)
logger.warning("Can't align without a reference! (--ref) *grunt*")
else:
if args.subcommand == "kitti":
traj_tmp, ref_traj_tmp = trajectories, [
ref_traj for n, t in trajectories
]
else:
traj_tmp, ref_traj_tmp = [], []
from evo.core import sync
for name, traj in trajectories:
logger.debug(SEP)
ref_assoc, traj_assoc = sync.associate_trajectories(
ref_traj,
traj,
max_diff=args.t_max_diff,
first_name="ref",
snd_name=name)
ref_traj_tmp.append(ref_assoc)
traj_tmp.append((name, traj_assoc))
trajectories = traj_tmp
correct_only_scale = args.correct_scale and not args.align
trajectories_new = []
for nt, ref_assoc in zip(trajectories, ref_traj_tmp):
logger.debug(SEP)
logger.debug("Aligning " + nt[0] + " to " + args.ref + "...")
trajectories_new.append(
(nt[0],
trajectory.align_trajectory(nt[1], ref_assoc,
args.correct_scale,
correct_only_scale,
args.n_to_align)))
trajectories = trajectories_new
for name, traj in trajectories:
print_traj_info(name, traj, args.verbose, args.full_check)
if args.ref:
print_traj_info(args.ref, ref_traj, args.verbose, args.full_check)
if args.plot or args.save_plot or args.serialize_plot:
from evo.tools.plot import PlotMode
plot_mode = PlotMode.xyz if not args.plot_mode else PlotMode[
args.plot_mode]
import numpy as np
from evo.tools import plot
import matplotlib.pyplot as plt
import matplotlib.cm as cm
plot_collection = plot.PlotCollection("evo_traj - trajectory plot")
fig_xyz, axarr_xyz = plt.subplots(3,
sharex="col",
figsize=tuple(SETTINGS.plot_figsize))
fig_rpy, axarr_rpy = plt.subplots(3,
sharex="col",
figsize=tuple(SETTINGS.plot_figsize))
fig_traj = plt.figure(figsize=tuple(SETTINGS.plot_figsize))
if (args.align or args.correct_scale) and not args.ref:
plt.xkcd(scale=2, randomness=4)
fig_traj.suptitle("what if --ref?")
fig_xyz.suptitle("what if --ref?")
ax_traj = plot.prepare_axis(fig_traj, plot_mode)
if args.ref:
short_traj_name = os.path.splitext(os.path.basename(args.ref))[0]
if SETTINGS.plot_usetex:
short_traj_name = short_traj_name.replace("_", "\\_")
plot.traj(ax_traj,
plot_mode,
ref_traj,
'--',
'grey',
short_traj_name,
alpha=0 if SETTINGS.plot_hideref else 1)
plot.traj_xyz(axarr_xyz,
ref_traj,
'--',
'grey',
short_traj_name,
alpha=0 if SETTINGS.plot_hideref else 1)
plot.traj_rpy(axarr_rpy,
ref_traj,
'--',
'grey',
short_traj_name,
alpha=0 if SETTINGS.plot_hideref else 1)
cmap_colors = None
if SETTINGS.plot_multi_cmap.lower() != "none":
cmap = getattr(cm, SETTINGS.plot_multi_cmap)
cmap_colors = iter(cmap(np.linspace(0, 1, len(trajectories))))
for name, traj in trajectories:
if cmap_colors is None:
color = next(ax_traj._get_lines.prop_cycler)['color']
else:
color = next(cmap_colors)
short_traj_name = os.path.splitext(os.path.basename(name))[0]
if SETTINGS.plot_usetex:
short_traj_name = short_traj_name.replace("_", "\\_")
plot.traj(ax_traj, plot_mode, traj, '-', color, short_traj_name)
if args.ref and isinstance(ref_traj, trajectory.PoseTrajectory3D):
start_time = ref_traj.timestamps[0]
else:
start_time = None
plot.traj_xyz(axarr_xyz,
traj,
'-',
color,
short_traj_name,
start_timestamp=start_time)
plot.traj_rpy(axarr_rpy,
traj,
'-',
color,
short_traj_name,
start_timestamp=start_time)
plt.tight_layout()
plot_collection.add_figure("trajectories", fig_traj)
plot_collection.add_figure("xyz_view", fig_xyz)
plot_collection.add_figure("rpy_view", fig_rpy)
if args.plot:
plot_collection.show()
if args.save_plot:
logger.info(SEP)
plot_collection.export(args.save_plot,
confirm_overwrite=not args.no_warnings)
if args.serialize_plot:
logger.info(SEP)
plot_collection.serialize(args.serialize_plot,
confirm_overwrite=not args.no_warnings)
if args.save_as_tum:
logger.info(SEP)
for name, traj in trajectories:
dest = os.path.splitext(os.path.basename(name))[0] + ".tum"
file_interface.write_tum_trajectory_file(
dest, traj, confirm_overwrite=not args.no_warnings)
if args.ref:
dest = os.path.splitext(os.path.basename(args.ref))[0] + ".tum"
file_interface.write_tum_trajectory_file(
dest, ref_traj, confirm_overwrite=not args.no_warnings)
if args.save_as_kitti:
logger.info(SEP)
for name, traj in trajectories:
dest = os.path.splitext(os.path.basename(name))[0] + ".kitti"
file_interface.write_kitti_poses_file(
dest, traj, confirm_overwrite=not args.no_warnings)
if args.ref:
dest = os.path.splitext(os.path.basename(args.ref))[0] + ".kitti"
file_interface.write_kitti_poses_file(
dest, ref_traj, confirm_overwrite=not args.no_warnings)
if args.save_as_bag:
logger.info(SEP)
import datetime
import rosbag
dest_bag_path = str(
datetime.datetime.now().strftime('%Y-%m-%d_%H:%M:%S.%f')) + ".bag"
logger.info("Saving trajectories to " + dest_bag_path + "...")
bag = rosbag.Bag(dest_bag_path, 'w')
try:
for name, traj in trajectories:
dest_topic = os.path.splitext(os.path.basename(name))[0]
frame_id = traj.meta[
"frame_id"] if "frame_id" in traj.meta else ""
file_interface.write_bag_trajectory(bag, traj, dest_topic,
frame_id)
if args.ref:
dest_topic = os.path.splitext(os.path.basename(args.ref))[0]
frame_id = ref_traj.meta[
"frame_id"] if "frame_id" in ref_traj.meta else ""
file_interface.write_bag_trajectory(bag, ref_traj, dest_topic,
frame_id)
finally:
bag.close()
def run(args):
import 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)
if (args.plot or args.save_plot or args.serialize_plot) and args.all_pairs:
raise NotImplementedError(
"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_full if args.plot_full_ref else traj_ref,
result.trajectories[est_name])
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):
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()
You should have received a copy of the GNU General Public License
along with evo. If not, see <http://www.gnu.org/licenses/>.
"""
import logging
import sys
import evo.core.lie_algebra as lie
from evo.core import trajectory
from evo.tools import plot, file_interface, log
import numpy as np
import matplotlib.pyplot as plt
logger = logging.getLogger("evo")
log.configure_logging(verbose=True)
traj_ref = file_interface.read_kitti_poses_file("../test/data/KITTI_00_gt.txt")
traj_est = file_interface.read_kitti_poses_file("../test/data/KITTI_00_ORB.txt")
# add artificial Sim(3) transformation
traj_est.transform(lie.se3(np.eye(3), [0, 0, 0]))
traj_est.scale(0.5)
logger.info("\nUmeyama alignment without scaling")
traj_est_aligned = trajectory.align_trajectory(traj_est, traj_ref)
logger.info("\nUmeyama alignment with scaling")
traj_est_aligned_scaled = trajectory.align_trajectory(
traj_est, traj_ref, correct_scale=True)