-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathtest.py
113 lines (86 loc) · 3.78 KB
/
test.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
import matplotlib.pyplot as plt
from evo.tools import file_interface
from evo.tools import plot
from evo.core.sync import associate_trajectories
import camera_plot
def test_compare_two_trajactories(traj_01_, traj_gt_, marker_scale=0.005, show_camera_indices=False):
a = plot.PlotCollection("test")
fig = plt.figure()
plot_mode = plot.PlotMode["xyz"]
ax = plot.prepare_axis(fig, plot_mode)
plot.traj(ax, plot_mode, traj_01_,
style="-",
color="r",
label="method 01",
alpha=0.5)
plot.traj(ax, plot_mode, traj_gt_,
style="--",
color="gray",
label="GT",
alpha=0.5)
if show_camera_indices:
camera_plot.draw_camera_indices(ax, traj_01_, color="r")
camera_plot.draw_camera_indices(ax, traj_gt_, color="gray")
# marker_scale is the size of the pose pyramids
camera_plot.compare_two_trajactories(
ax, traj_gt_, traj_01_, marker_scale=marker_scale)
# This is an alternative to the pyramids
# plot.draw_coordinate_axes(ax, traj_01_, plot_mode, marker_scale)
# plot.draw_coordinate_axes(ax, traj_gt_, plot_mode, marker_scale)
# disable axis grid
plt.axis('off')
a.add_figure("compare two trajactories", fig)
a.show()
def test_compare_three_trajactories(traj_01_, traj_02_, traj_gt_, marker_scale=0.005, show_camera_indices=False):
a = plot.PlotCollection("test")
fig = plt.figure()
plot_mode = plot.PlotMode["xyz"]
ax = plot.prepare_axis(fig, plot_mode)
plot.traj(ax, plot_mode, traj_01_,
style="-",
color="r",
label="method 01",
alpha=0.5)
plot.traj(ax, plot_mode, traj_02_,
style="-",
color="b",
label="method 2",
alpha=0.5)
plot.traj(ax, plot_mode, traj_gt_,
style="--",
color="gray",
label="GT",
alpha=0.5)
if show_camera_indices:
camera_plot.draw_camera_indices(ax, traj_01_, color="r")
camera_plot.draw_camera_indices(ax, traj_02_, color="b")
camera_plot.draw_camera_indices(ax, traj_gt_, color="gray")
# This is an alternative to the pyramids
# plot.draw_coordinate_axes(ax, traj_01_, plot_mode, marker_scale)
# plot.draw_coordinate_axes(ax, traj_02_, plot_mode, marker_scale)
# plot.draw_coordinate_axes(ax, traj_gt_, plot_mode, marker_scale)
# marker_scale is the size of the pose pyramids
camera_plot.draw_pyramids(ax, traj_01_, marker_scale=marker_scale, color='r')
camera_plot.draw_pyramids(ax, traj_02_, marker_scale=marker_scale, color='b')
camera_plot.draw_pyramids(ax, traj_gt_, marker_scale=marker_scale, color='gray')
# disable axis grid
plt.axis('off')
a.add_figure("compare three trajactories", fig)
a.show()
def main():
FILE_GT = "./test/data/freiburg1_xyz-groundtruth.txt"
FILE_01 = "./test/data/freiburg1_xyz-rgbdslam.txt"
FILE_02 = "./test/data/freiburg1_xyz-ORB_kf_mono.txt"
traj_gt = file_interface.read_tum_trajectory_file(FILE_GT)
traj_01 = file_interface.read_tum_trajectory_file(FILE_01)
traj_02 = file_interface.read_tum_trajectory_file(FILE_02)
gt_synced01, traj_01_synced = associate_trajectories(
traj_gt, traj_01, max_diff=0.1)
gt_synced02, traj_02_synced = associate_trajectories(
traj_gt, traj_02, max_diff=0.1)
traj_01_synced.align(gt_synced01, correct_scale=True)
traj_02_synced.align(gt_synced02, correct_scale=True)
test_compare_two_trajactories(traj_01_synced, gt_synced01, marker_scale=0.005, show_camera_indices=False)
test_compare_three_trajactories(traj_01_synced, traj_02_synced, traj_gt, marker_scale=0.005, show_camera_indices=False)
if __name__ == "__main__":
main()