...some modules and settings for this demo:
In [ ]:
from __future__ import print_function
from evo.tools import log
log.configure_logging()
In [ ]:
from evo.tools import plot
from evo.tools.plot import PlotMode
from evo.core.metrics import PoseRelation, Unit
from evo.tools.settings import SETTINGS
# temporarily override some package settings
SETTINGS.plot_figsize = [6, 6]
SETTINGS.plot_split = True
SETTINGS.plot_usetex = False
# magic plot configuration
import matplotlib.pyplot as plt
%matplotlib inline
%matplotlib notebook
In [ ]:
# interactive widgets configuration
import ipywidgets
check_opts_ape = {"align": False, "correct_scale": False, "show_plot": True}
check_boxes_ape=[ipywidgets.Checkbox(description=desc, value=val) for desc, val in check_opts_ape.items()]
check_opts_rpe = {"align": False, "correct_scale": False, "all_pairs": False, "show_plot": True}
check_boxes_rpe=[ipywidgets.Checkbox(description=desc, value=val) for desc, val in check_opts_rpe.items()]
delta_input = ipywidgets.FloatText(value=1.0, description='delta', disabled=False, color='black')
delta_unit_selector=ipywidgets.Dropdown(
options={u.value: u for u in Unit if u is not Unit.seconds},
value=Unit.frames, description='delta_unit'
)
plotmode_selector=ipywidgets.Dropdown(
options={p.value: p for p in PlotMode},
value=PlotMode.xy, description='plot_mode'
)
pose_relation_selector=ipywidgets.Dropdown(
options={p.value: p for p in PoseRelation},
value=PoseRelation.translation_part, description='pose_relation'
)
In [ ]:
from evo.tools import file_interface
from evo.core import sync
Load KITTI files with entries of the first three rows of $\mathrm{SE}(3)$ matrices per line (no timestamps):
In [ ]:
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")
...or load a ROS bagfile with geometry_msgs/PoseStamped, geometry_msgs/TransformStamped, geometry_msgs/PoseWithCovarianceStamped or nav_msgs/Odometry topics:
In [ ]:
try:
import rosbag
bag_handle = rosbag.Bag("../test/data/ROS_example.bag")
traj_ref = file_interface.read_bag_trajectory(bag_handle, "groundtruth")
traj_est = file_interface.read_bag_trajectory(bag_handle, "ORB-SLAM")
traj_ref, traj_est = sync.associate_trajectories(traj_ref, traj_est)
except ImportError as e:
print(e) # ROS not found
... or load TUM files with 3D position and orientation quaternion per line ($x$ $y$ $z$ $q_x$ $q_y$ $q_z$ $q_w$):
In [ ]:
traj_ref = file_interface.read_tum_trajectory_file("../test/data/fr2_desk_groundtruth.txt")
traj_est = file_interface.read_tum_trajectory_file("../test/data/fr2_desk_ORB_kf_mono.txt")
traj_ref, traj_est = sync.associate_trajectories(traj_ref, traj_est)
In [ ]:
print(traj_ref)
print(traj_est)
In [ ]:
import evo.main_ape as main_ape
import evo.common_ape_rpe as common
count = 0
results = []
def callback_ape(pose_relation, align, correct_scale, plot_mode, show_plot):
global results, count
est_name="APE Test #{}".format(count)
result = main_ape.ape(traj_ref, traj_est, est_name=est_name,
pose_relation=pose_relation, align=align, correct_scale=correct_scale)
count += 1
results.append(result)
if show_plot:
fig = plt.figure()
ax = plot.prepare_axis(fig, plot_mode)
plot.traj(ax, plot_mode, traj_ref, style="--", alpha=0.5)
plot.traj_colormap(
ax, result.trajectories[est_name], result.np_arrays["error_array"], plot_mode,
min_map=result.stats["min"], max_map=result.stats["max"])
_ = ipywidgets.interact_manual(callback_ape, pose_relation=pose_relation_selector, plot_mode=plotmode_selector,
**{c.description: c.value for c in check_boxes_ape})
Run the code below, configure the parameters in the GUI and press the update button.
(uses the trajectories loaded above, alignment only useful for visualization here)
In [ ]:
import evo.main_rpe as main_rpe
count = 0
results = []
def callback_rpe(pose_relation, delta, delta_unit, all_pairs, align, correct_scale, plot_mode, show_plot):
global results, count
est_name="RPE Test #{}".format(count)
result = main_rpe.rpe(traj_ref, traj_est, est_name=est_name,
pose_relation=pose_relation, delta=delta, delta_unit=delta_unit,
all_pairs=all_pairs, align=align, correct_scale=correct_scale,
support_loop=True)
count += 1
results.append(result)
if show_plot:
fig = plt.figure()
ax = plot.prepare_axis(fig, plot_mode)
plot.traj(ax, plot_mode, traj_ref, style="--", alpha=0.5)
plot.traj_colormap(
ax, result.trajectories[est_name], result.np_arrays["error_array"], plot_mode,
min_map=result.stats["min"], max_map=result.stats["max"])
_ = ipywidgets.interact_manual(callback_rpe, pose_relation=pose_relation_selector, plot_mode=plotmode_selector,
delta=delta_input, delta_unit=delta_unit_selector,
**{c.description: c.value for c in check_boxes_rpe})
Do stuff with the result objects:
In [ ]:
import pandas as pd
from evo.tools import pandas_bridge
df = pd.DataFrame()
for result in results:
df = pd.concat((df, pandas_bridge.result_to_df(result)), axis="columns")
df
In [ ]:
df.loc["stats"]
In [ ]: