def main():
Town = 1
SL = 0
nr_vans = 10
dist = 10
base_dir_opposite = "/home/sietse/results_carla0.9/VansOppositeRoad/"
base_dir_stuck = "/home/sietse/results_carla0.9/stuckbehindvan/20fps/"
dir_name_stat = "T{}_SL{}_s/".format(Town, SL)
dir_name_opp = "T{}_SL{}_d{}/".format(Town, SL, nr_vans)
dir_name_stuck = "T{}_SL{}_d{}/".format(Town, SL, dist)
orb_static, gt = InspectJsonFileInDir(Town, SL, base_dir_stuck,
dir_name_stat, "SLAM")
orb_opposite, gt_opp = InspectJsonFileInDir(Town, SL, base_dir_opposite,
dir_name_opp, "SLAM")
orb_stuck, gt_stuck = InspectJsonFileInDir(Town, SL, base_dir_stuck,
dir_name_stuck, "VO")
# methods = [gt]
# methods.extend(orb_static)
# methods.extend(orb_opposite)
# methods.extend(orb_stuck)
# evaluate_trajectory(methods)
static_mean, static_std = RmseRpe(orb_static, gt)
opposite_mean, opposite_std = RmseRpe(orb_opposite, gt)
stuck_mean, stuck_std = RmseRpe(orb_stuck, gt)
pdb.set_trace()
def main():
"""Describes the performance of a certain scenario in a certain starting location"""
Towns = (1, 2, 3)
ds = 15
base_dir = "/home/sietse/results_carla0.9/stuckbehindvan/20fps/"
scenario_performance_data = []
for Town in Towns:
if Town == 1:
starting_locations = (0, 27, 58)
elif Town == 2:
starting_locations = (18, 37, 78)
elif Town == 3:
starting_locations = (75, 97, 127, 132)
else:
print("Town does not exist")
return
for SL in starting_locations:
dir_name_stat = DirName(Town, SL, "static")
dir_name_dyn = DirName(Town, SL, "dynamic", ds)
orb_static, gt = InspectJsonFileInDir(Town, SL, base_dir, dir_name_stat)
orb_dynamic, gt = InspectJsonFileInDir(Town, SL, base_dir, dir_name_dyn)
# orb_static, orb_dynamic = FilterOutliersFromOrb(orb_static, orb_dynamic, Town, SL, ds)
print("Converting T{}SL{}".format(Town, SL))
Test = ScenarioLocationPerformance(ds, Town, SL, orb_static, orb_dynamic, gt)
scenario_performance_data.append(Test)
A = ScenarioPerformance(scenario_performance_data)
#A.CreateLatexTable(scenario_performance_data)
# A.SummaryPerformance()
print(A.LatexTableStaticResults)
print(A.LatexTableDynamicResults)
# print(A.LatexTableDiscussion)
pdb.set_trace()
def main_stuckbehindvan():
"""Describes the error increase wrt static for each dynamic scenario """
Towns = (1, 2)
dynamic_scenarios = (20, 15, 10)
base_dir = "/home/sietse/results_carla0.9/stuckbehindvan/20fps/"
# base_dir = "/media/svanschouwenburg/Elements/stuckbehindvan/20fps/"
scenario_performance_data = []
for Town in Towns:
if Town == 1:
starting_locations = (0, 27, 58)
elif Town == 2:
starting_locations = (37, 78)
elif Town == 3:
# starting_locations = (75, 97, 127, 132)
starting_locations = [127]
else:
print("Town does not exist")
return
for SL in starting_locations:
for ds in dynamic_scenarios:
dir_name_stat = DirName(Town, SL, "static")
dir_name_dyn = DirName(Town, SL, "dynamic", ds)
orb_static, gt = InspectJsonFileInDir(Town, SL, base_dir,
dir_name_stat, "VO")
orb_dynamic, gt = InspectJsonFileInDir(Town, SL, base_dir,
dir_name_dyn, "VO")
print("Converting T{}SL{}".format(Town, SL))
Test = ScenarioLocationPerformance(ds, Town, SL, orb_static,
orb_dynamic, gt)
scenario_performance_data.append(Test)
A = ScenarioPerformance(scenario_performance_data)
def main_stuckbehindvan():
"""Trying to proof that the corners in the trajectory cause the error to increase """
Town = 1
SL = 0
dist = 10
base_dir = "/home/sietse/results_carla0.9/stuckbehindvan/20fps/"
dir_name_stat = "T{}_SL{}_s/".format(Town, SL)
dir_name_dyn = "T{}_SL{}_d{}/".format(Town, SL, dist)
orb_static_list, gt = InspectJsonFileInDir(Town, SL, base_dir,
dir_name_stat, "VO")
orb_dynamic_list, gt = InspectJsonFileInDir(Town, SL, base_dir,
dir_name_dyn, "VO")
ev_dis = 10
i = 0
for orb_stat, orb_dyn in zip(orb_static_list, orb_dynamic_list):
stat_time, stat_trans_err, stat_rot_err = evaluate_RPE_dist(
gt, orb_stat, ev_dis)
dyn_time, dyn_trans_err, dyn_rot_err = evaluate_RPE_dist(
gt, orb_dyn, ev_dis)
plt.figure("RPE magnitude over {} m".format(ev_dis))
plt.subplot(2, 1, 1)
if i == 0:
plt.plot(stat_time, stat_trans_err, 'b-', label="static")
plt.plot(dyn_time, dyn_trans_err, 'r--', label="dynamic")
else:
plt.plot(stat_time, stat_trans_err, 'b-')
plt.plot(dyn_time, dyn_trans_err, 'r--')
plt.xlabel("time [s]", fontsize=14)
plt.ylabel("translational error [-]", fontsize=14)
plt.legend()
plt.subplot(2, 1, 2)
plt.plot(stat_time, stat_rot_err, 'b-')
plt.plot(dyn_time, dyn_rot_err, 'r--')
plt.xlabel("time [s]", fontsize=14)
plt.ylabel("rotational error [deg/m]", fontsize=14)
i += 1
plt.legend()
plt.show()
def main_oppositeroad():
Town = 1
SL = 0
nr_vans = 10
base_dir = "/home/sietse/results_carla0.9/VansOppositeRoad/"
base_dir_stat = "/home/sietse/results_carla0.9/stuckbehindvan/20fps/"
dir_name_stat = "T{}_SL{}_s/".format(Town, SL)
dir_name = "T{}_SL{}_d{}/".format(Town, SL, nr_vans)
ev_dis = 10
i = 0
orb_static_list, gt = InspectJsonFileInDir(Town, SL, base_dir_stat,
dir_name_stat, "SLAM")
orb_dynamic_list, gt = InspectJsonFileInDir(Town, SL, base_dir, dir_name,
"MC")
for orb_stat, orb_dyn in zip(orb_static_list, orb_dynamic_list):
stat_time, stat_trans_err, stat_rot_err = evaluate_RPE_dist(
gt, orb_stat, ev_dis)
dyn_time, dyn_trans_err, dyn_rot_err = evaluate_RPE_dist(
gt, orb_dyn, ev_dis)
plt.figure("RPE magnitude over {} m".format(ev_dis))
plt.subplot(2, 1, 1)
if i == 0:
plt.plot(stat_time, stat_trans_err, 'b-', label="static")
plt.plot(dyn_time, dyn_trans_err, 'r--', label="dynamic")
else:
plt.plot(stat_time, stat_trans_err, 'b-')
plt.plot(dyn_time, dyn_trans_err, 'r--')
plt.xlabel("time [s]", fontsize=14)
plt.ylabel("translational error [-]", fontsize=14)
plt.legend()
plt.subplot(2, 1, 2)
plt.plot(stat_time, stat_rot_err, 'b-')
plt.plot(dyn_time, dyn_rot_err, 'r--')
plt.xlabel("time [s]", fontsize=14)
plt.ylabel("rotational error [deg/m]", fontsize=14)
i += 1
plt.legend()
plt.show()
if Town == 1:
starting_locations = (0, 27, 58)
elif Town == 2:
starting_locations = (18, 37, 78)
elif Town == 3:
starting_locations = (75, 97, 127, 132)
else:
print("Town does not exist")
total_scenarios = len(dynamic_scenarios)+1
for index, SL in enumerate(starting_locations):
for ds in dynamic_scenarios:
dir_name_stat = DirName(Town, SL, "static")
dir_name_dyn = DirName(Town, SL, "dynamic", ds)
orb_static, gt = InspectJsonFileInDir(Town, SL, base_dir, dir_name_stat, mode)
orb_dynamic, gt = InspectJsonFileInDir(Town, SL, base_dir, dir_name_dyn, mode)
Perf = ScenarioLocationPerformance(ds, Town, SL, orb_static, orb_dynamic, gt)
orb_static_mc, gt_mc = InspectJsonFileInDir(Town, SL, base_dir, dir_name_stat, mode_mc)
orb_dynamic_mc, gt_mc = InspectJsonFileInDir(Town, SL, base_dir, dir_name_dyn, mode_mc)
Perf_mc = ScenarioLocationPerformance(ds, Town, SL, orb_static_mc, orb_dynamic_mc, gt_mc)
if ds == 20:
x.extend(["T" + str(Perf.location["Town"]) + "Nr" + str(index + 1) + "\n" + " Static",
"T" + str(Perf.location["Town"]) + "Nr" + str(index + 1) + "\n" + " Dist:" + str(
Perf.scenario["Distance"])])
avg_trans.extend([Perf.rmse_static_avg[0], Perf.rmse_dynamic_avg[0]])
avg_trans_upp.extend([Perf.rmse_static_avg[0] + 2 * Perf.rmse_static_std[0],
Perf.rmse_dynamic_avg[0] + 2 * Perf.rmse_dynamic_std[0]])
def main():
"""Produces test error bars that compares ORB VO with and without map culling method"""
base_dir = "/home/sietse/results_carla0.9/stuckbehindvan/20fps/"
dynamic_scenarios = (20, 15, 10)
Towns = (1, 2)
mode = "MC"
# for Town in Towns:
Town = 2
if Town == 1:
starting_locations = (0, 27, 58)
elif Town == 2:
starting_locations = (18, 37, 78)
elif Town == 3:
starting_locations = (75, 97, 127, 132)
else:
print("Town does not exist")
return
plt.figure("RMSE RPE trans")
plt.title("RMSE Relative Pose Error translational component")
# total number of entries
N = (len(dynamic_scenarios) + 1) * len(starting_locations)
i = 0
ind = np.arange(N)
x = []
for index, SL in enumerate(starting_locations):
avg_trans_vo = []
std_trans_vo = []
avg_rot_vo = []
std_rot_vo = []
avg_trans_mc = []
std_trans_mc = []
avg_rot_mc = []
std_rot_mc = []
for ds in dynamic_scenarios:
dir_name_stat = DirName(Town, SL, "static")
dir_name_dyn = DirName(Town, SL, "dynamic", ds)
orb_static_vo, gt_vo = InspectJsonFileInDir(
Town, SL, base_dir, dir_name_stat, "VO")
orb_dynamic_vo, gt_vo = InspectJsonFileInDir(
Town, SL, base_dir, dir_name_dyn, "VO")
Perf_vo = ScenarioLocationPerformance(ds, Town, SL, orb_static_vo,
orb_dynamic_vo, gt_vo)
orb_static_mc, gt_mc = InspectJsonFileInDir(
Town, SL, base_dir, dir_name_stat, "MC")
orb_dynamic_mc, gt_mc = InspectJsonFileInDir(
Town, SL, base_dir, dir_name_dyn, "MC")
Perf_mc = ScenarioLocationPerformance(ds, Town, SL, orb_static_mc,
orb_dynamic_mc, gt_mc)
if ds == 20:
x.extend([
"T" + str(Perf_vo.location["Town"]) + "Nr" +
str(index + 1) + "\n" + " Static", "T" +
str(Perf_vo.location["Town"]) + "Nr" + str(index + 1) +
"\n" + " Dist:" + str(Perf_vo.scenario["Distance"])
])
avg_trans_vo.extend(
[Perf_vo.rmse_static_avg[0], Perf_vo.rmse_dynamic_avg[0]])
std_trans_vo.extend(
[Perf_vo.rmse_static_std[0], Perf_vo.rmse_dynamic_std[0]])
avg_rot_vo.extend(
[Perf_vo.rmse_static_avg[1], Perf_vo.rmse_dynamic_avg[1]])
std_rot_vo.extend(
[Perf_vo.rmse_static_std[1], Perf_vo.rmse_dynamic_std[1]])
avg_trans_mc.extend(
[Perf_mc.rmse_static_avg[0], Perf_mc.rmse_dynamic_avg[0]])
std_trans_mc.extend(
[Perf_mc.rmse_static_std[0], Perf_mc.rmse_dynamic_std[0]])
avg_rot_mc.extend(
[Perf_mc.rmse_static_avg[1], Perf_mc.rmse_dynamic_avg[1]])
std_rot_mc.extend(
[Perf_mc.rmse_static_std[1], Perf_mc.rmse_dynamic_std[1]])
else:
x.append("T" + str(Perf_vo.location["Town"]) + "Nr" +
str(index + 1) + "\n" + "Dist:" +
str(Perf_vo.scenario["Distance"]))
avg_trans_vo.append(Perf_vo.rmse_dynamic_avg[0])
std_trans_vo.append(Perf_vo.rmse_dynamic_std[0])
avg_rot_vo.append(Perf_vo.rmse_dynamic_avg[1])
std_rot_vo.append(Perf_vo.rmse_dynamic_std[1])
avg_trans_mc.append(Perf_mc.rmse_dynamic_avg[0])
std_trans_mc.append(Perf_mc.rmse_dynamic_std[0])
avg_rot_mc.append(Perf_mc.rmse_dynamic_avg[1])
std_rot_mc.append(Perf_mc.rmse_dynamic_std[1])
print("avg trans vo", avg_trans_vo)
print("avg trans mc", avg_trans_mc)
print("avg rot vo", avg_rot_vo)
print("avg rot mc", avg_rot_mc)
# have to loop it otherwise you get the same color
plt.subplot(2, 1, 1)
print(i)
# plt.grid(True)
if i == (N - (len(dynamic_scenarios) + 1)):
plt.errorbar(ind[i:i + (len(dynamic_scenarios) + 1)],
np.array(avg_trans_vo),
np.array(std_trans_vo),
color='blue',
label="ORB VO")
plt.errorbar(ind[i:i + (len(dynamic_scenarios) + 1)],
np.array(avg_trans_mc),
np.array(std_trans_mc),
color='red',
label="ORB VO, no map culling")
plt.legend()
else:
plt.errorbar(ind[i:i + (len(dynamic_scenarios) + 1)],
np.array(avg_trans_vo),
np.array(std_trans_vo),
color='blue')
plt.errorbar(ind[i:i + (len(dynamic_scenarios) + 1)],
np.array(avg_trans_mc),
np.array(std_trans_mc),
color='red')
plt.subplots_adjust(hspace=0.5)
plt.subplot(2, 1, 2)
if i == (N - (len(dynamic_scenarios) + 1)):
plt.errorbar(ind[i:i + (len(dynamic_scenarios) + 1)],
np.array(avg_rot_vo),
np.array(std_rot_vo),
color='blue',
label="ORB VO")
plt.errorbar(ind[i:i + (len(dynamic_scenarios) + 1)],
np.array(avg_rot_mc),
np.array(std_rot_mc),
color='red',
label="ORB VO, no map culling")
plt.legend()
else:
plt.errorbar(ind[i:i + (len(dynamic_scenarios) + 1)],
np.array(avg_rot_vo),
np.array(std_rot_vo),
color='blue')
plt.errorbar(ind[i:i + (len(dynamic_scenarios) + 1)],
np.array(avg_rot_mc),
np.array(std_rot_mc),
color='red')
i = i + (len(dynamic_scenarios) + 1)
pdb.set_trace()
plt.subplot(2, 1, 1)
plt.xticks(ind, x, fontsize=14)
if mode == "SLAM" and Town == 1:
plt.ylim(top=0.060)
plt.xlim(right=N)
plt.title(
"Translational component root mean square error of the relative pose error",
fontsize=18)
plt.ylabel("Trans. RMSE RPE [-]", fontsize=14)
plt.subplot(2, 1, 2)
plt.xticks(ind, x, fontsize=14)
if mode == "SLAM" and Town == 1:
plt.ylim(top=0.040)
plt.xlim(right=N)
plt.title(
"Rotational component root mean square error relative pose error",
fontsize=18)
plt.ylabel("Rot. RMSE RPE [deg/m]", fontsize=14)
plt.show()
starting_locations = (0, 27, 58)
elif Town == 2:
starting_locations = (18, 37, 78)
elif Town == 3:
starting_locations = (75, 97, 127, 132)
else:
print("Town does not exist")
total_SL += len(starting_locations)
for index, SL in enumerate(starting_locations):
dir_name_stat = "T{}_SL{}_s/".format(Town, SL)
dir_name_dyn = "T{}_SL{}_d{}/".format(Town, SL, dv)
# get the static data from the stuck behind van experiment
orb_static, gt = InspectJsonFileInDir(Town, SL, base_dir_stat,
dir_name_stat, "SLAM")
orb_dynamic, gt = InspectJsonFileInDir(Town, SL, base_dir_dyn,
dir_name_dyn, "SLAM")
Perf = ScenarioLocationPerformance(dv, Town, SL, orb_static,
orb_dynamic, gt)
orb_static_mc, gt_mc = InspectJsonFileInDir(Town, SL, base_dir_stat,
dir_name_stat, "MC")
orb_dynamic_mc, gt_mc = InspectJsonFileInDir(Town, SL, base_dir_dyn,
dir_name_dyn, "MC")
Perf_mc = ScenarioLocationPerformance(dv, Town, SL, orb_static_mc,
orb_dynamic_mc, gt_mc)
x.extend([
"T" + str(Perf.location["Town"]) + "Nr" + str(index + 1) + "\n" +
" Static", "T" + str(Perf.location["Town"]) + "Nr" +
def main():
"""Test if error bars show the results instinctively"""
base_dir = "/home/sietse/results_carla0.9/stuckbehindvan/20fps/"
dynamic_scenarios = (20, 15, 10)
Towns = (1, 2)
mode = "MC"
# for Town in Towns:
Town = 1
if Town == 1:
starting_locations = (0, 27, 58)
elif Town == 2:
starting_locations = (18, 37, 78)
elif Town == 3:
starting_locations = (75, 97, 127, 132)
else:
print("Town does not exist")
return
plt.figure("RMSE RPE trans")
plt.title("RMSE Relative Pose Error translational component")
# total number of entries
N = (len(dynamic_scenarios) + 1) * len(starting_locations)
i = 0
ind = np.arange(N)
x = []
for index, SL in enumerate(starting_locations):
avg_trans = []
std_trans = []
avg_rot = []
std_rot = []
track_fail = []
loop_fail = []
for ds in dynamic_scenarios:
dir_name_stat = DirName(Town, SL, "static")
dir_name_dyn = DirName(Town, SL, "dynamic", ds)
orb_static, gt = InspectJsonFileInDir(Town, SL, base_dir,
dir_name_stat, mode)
orb_dynamic, gt = InspectJsonFileInDir(Town, SL, base_dir,
dir_name_dyn, mode)
Perf = ScenarioLocationPerformance(ds, Town, SL, orb_static,
orb_dynamic, gt)
if ds == 20:
x.extend([
"T" + str(Perf.location["Town"]) + "Nr" + str(index + 1) +
"\n" + " Static",
"T" + str(Perf.location["Town"]) + "Nr" + str(index + 1) +
"\n" + " Dist:" + str(Perf.scenario["Distance"])
])
avg_trans.extend(
[Perf.rmse_static_avg[0], Perf.rmse_dynamic_avg[0]])
std_trans.extend(
[Perf.rmse_static_std[0], Perf.rmse_dynamic_std[0]])
avg_rot.extend(
[Perf.rmse_static_avg[1], Perf.rmse_dynamic_avg[1]])
std_rot.extend(
[Perf.rmse_static_std[1], Perf.rmse_dynamic_std[1]])
track_fail.extend(
[Perf.lost_track_static, Perf.lost_track_dynamic])
loop_fail.extend(
[Perf.false_loop_static, Perf.false_loop_dynamic])
else:
x.append("T" + str(Perf.location["Town"]) + "Nr" +
str(index + 1) + "\n" + "Dist:" +
str(Perf.scenario["Distance"]))
avg_trans.append(Perf.rmse_dynamic_avg[0])
std_trans.append(Perf.rmse_dynamic_std[0])
avg_rot.append(Perf.rmse_dynamic_avg[1])
std_rot.append(Perf.rmse_dynamic_std[1])
track_fail.append(Perf.lost_track_dynamic)
loop_fail.append(Perf.false_loop_dynamic)
# have to loop it otherwise you get the same color
plt.subplot(3, 1, 1)
# plt.grid(True)
label_location = "T" + str(
Perf.location["Town"]) + "Nr" + str(index + 1)
plt.errorbar(ind[i:i + (len(dynamic_scenarios) + 1)],
np.array(avg_trans),
np.array(std_trans),
color='green',
label=label_location)
plt.subplots_adjust(hspace=0.5)
plt.legend()
plt.subplot(3, 1, 2)
label_location = "T" + str(
Perf.location["Town"]) + "Nr" + str(index + 1)
plt.errorbar(ind[i:i + (len(dynamic_scenarios) + 1)],
np.array(avg_rot),
np.array(std_rot),
color='green',
label=label_location)
plt.legend()
width = 0.35
plt.subplot(3, 1, 3)
plot_track = plt.bar(ind[i:i + (len(dynamic_scenarios) + 1)],
track_fail,
width,
color="r")
plot_loop = plt.bar(ind[i:i + (len(dynamic_scenarios) + 1)],
loop_fail,
width,
bottom=track_fail,
color="b")
i = i + (len(dynamic_scenarios) + 1)
plt.subplot(3, 1, 1)
plt.xticks(ind, x)
if mode == "SLAM" and Town == 1:
plt.ylim(top=0.060)
plt.xlim(right=N)
plt.title(
"Translational component root mean square error of the relative pose error"
)
plt.ylabel("Trans. RMSE RPE [-]")
plt.subplot(3, 1, 2)
plt.xticks(ind, x)
if mode == "SLAM" and Town == 1:
plt.ylim(top=0.040)
plt.xlim(right=N)
plt.title(
"Rotational component root mean square error relative pose error")
plt.ylabel("Rot. RMSE RPE [deg/m]")
plt.subplot(3, 1, 3)
plt.title("Ratio removed due to false loop closure or tracking failure")
plt.xticks(ind, x)
plt.ylim(top=1.1)
plt.xlim(right=N)
plt.ylabel("Ratio removed of total data [-]")
plt.legend((plot_track, plot_loop),
("tracking failure", "false loop closure"))
plt.show()
def main_stuckbehindvan():
"""Trying to proof that the corners in the trajectory cause the error to increase """
Town = 1
SL = 0
dist = 10
base_dir = "/home/sietse/results_carla0.9/stuckbehindvan/20fps/"
dir_name_stat = "T{}_SL{}_s/".format(Town, SL)
dir_name_dyn = "T{}_SL{}_d{}/".format(Town, SL, dist)
orb_static_list, gt = InspectJsonFileInDir(Town, SL, base_dir,
dir_name_stat, "VO")
orb_dynamic_list, gt = InspectJsonFileInDir(Town, SL, base_dir,
dir_name_dyn, "VO")
ev_dis = 10
i = 0
for orb_stat, orb_dyn in zip(orb_static_list, orb_dynamic_list):
stat_time, stat_trans_err, stat_rot_err = evaluate_RPE_dist(
gt, orb_stat, ev_dis)
dyn_time, dyn_trans_err, dyn_rot_err = evaluate_RPE_dist(
gt, orb_dyn, ev_dis)
plt.figure("RPE magnitude over {} m".format(ev_dis))
plt.subplot(2, 1, 1)
plt.plot(stat_time, stat_trans_err, orb_stat.plotstyle)
plt.plot(dyn_time, dyn_trans_err, orb_dyn.plotstyle)
plt.subplot(2, 1, 2)
plt.plot(stat_time, stat_rot_err, orb_stat.plotstyle)
plt.plot(dyn_time, dyn_rot_err, orb_dyn.plotstyle)
# add markers at the position where visual examples are inserted.
if i == 1:
for index, time in enumerate(dyn_time):
if time == 25.55:
imageB_t = time
imageB_trans = dyn_trans_err[index]
imageB_rot = dyn_rot_err[index]
elif time == 82.0:
imageC_t = time
imageC_trans = dyn_trans_err[index]
imageC_rot = dyn_rot_err[index]
ms = 15
mew = 3
plt.subplot(2, 1, 1)
plt.plot([imageB_t], [imageB_trans],
marker='x',
markersize=ms,
markeredgewidth=mew,
color='black')
plt.plot([imageC_t], [imageC_trans],
marker='+',
markersize=ms,
markeredgewidth=mew,
color='black')
plt.subplot(2, 1, 2)
plt.plot([imageB_t], [imageB_rot],
marker='x',
markersize=ms,
markeredgewidth=mew,
color='black')
plt.plot([imageC_t], [imageC_rot],
marker='+',
markersize=ms,
markeredgewidth=mew,
color='black')
i += 1
fs_x = 14
fs_y = 16
fs_title = 16
fs_legend = 12
plt.subplot(2, 1, 1)
plt.suptitle(
r'Town 1 Nr 1: ORB VO Relative Pose Error over time ($\Delta$=10m)',
fontsize=18,
fontweight='bold',
y=0.95)
plt.title("Translational component relative pose error",
fontsize=fs_title,
fontweight='bold')
plt.xticks(fontsize=fs_x)
plt.yticks(fontsize=fs_x)
plt.xlabel("Time [s]", fontsize=fs_y)
plt.ylabel("Translational error [m/m]", fontsize=fs_y)
plt.plot([], [], 'k-', label="Static")
plt.plot([], [],
'k--',
label="Driving behind van \ninter-vehicle distance = 10 meters")
plt.scatter([], [],
marker='x',
s=100,
color='black',
linewidths=5,
label="Visual example see image (b)")
plt.scatter([], [],
marker='+',
s=100,
color='black',
label="Visual example see image (c)")
plt.legend(fontsize=fs_legend)
plt.subplot(2, 1, 2)
plt.title("Rotational component relative pose error",
fontsize=fs_title,
fontweight='bold')
plt.xlabel("Time [s]", fontsize=fs_y)
plt.ylabel("Rotational error [deg/m]", fontsize=fs_y)
plt.xticks(fontsize=fs_x)
plt.yticks(fontsize=fs_x)
plt.show()
def main():
"""Test if error bars show the results instinctively"""
base_dir_dyn = "/home/sietse/results_carla0.9/VansOppositeRoad/"
base_dir_stat = "/home/sietse/results_carla0.9/stuckbehindvan/20fps/"
# dynamic variable (10 cars on opposite lane)
dv = 10
Towns = (1, 2)
mode = "SLAM"
N = 2 * 6
i = 0
ind = np.arange(N)
x = []
for Town in Towns:
if Town == 1:
starting_locations = (0, 27, 58)
elif Town == 2:
starting_locations = (18, 37, 78)
elif Town == 3:
starting_locations = (75, 97, 127, 132)
else:
print("Town does not exist")
return
plt.figure("RMSE RPE trans")
# total number of entries: static and non static
for index, SL in enumerate(starting_locations):
avg_trans = []
std_trans = []
avg_rot = []
std_rot = []
track_fail = []
loop_fail = []
dir_name_stat = "T{}_SL{}_s/".format(Town, SL)
dir_name_dyn = "T{}_SL{}_d{}/".format(Town, SL, dv)
# get the static data from the stuck behind van experiment
orb_static, gt = InspectJsonFileInDir(Town, SL, base_dir_stat, dir_name_stat, mode)
orb_dynamic, gt = InspectJsonFileInDir(Town, SL, base_dir_dyn, dir_name_dyn, mode)
Perf = ScenarioLocationPerformance(dv, Town, SL, orb_static, orb_dynamic, gt)
# first get the static data from that location
x.append("T" + str(Perf.location["Town"]) + "Nr" + str(index+1) + "\n" + " Static")
avg_trans.append(Perf.rmse_static_avg[0])
std_trans.append(Perf.rmse_static_std[0])
avg_rot.append(Perf.rmse_static_avg[1])
std_rot.append(Perf.rmse_static_std[1])
track_fail.append(Perf.lost_track_static)
loop_fail.append(Perf.false_loop_static)
x.append("T" + str(Perf.location["Town"]) + "Nr" + str(index+1) + "\n" + "Dynamic")
avg_trans.append(Perf.rmse_dynamic_avg[0])
std_trans.append(Perf.rmse_dynamic_std[0])
avg_rot.append(Perf.rmse_dynamic_avg[1])
std_rot.append(Perf.rmse_dynamic_std[1])
track_fail.append(Perf.lost_track_dynamic)
loop_fail.append(Perf.false_loop_dynamic)
# have to loop it otherwise you get the same color
plt.subplot(3, 1, 1)
# plt.grid(True)
label_location = "T" + str(Perf.location["Town"]) + "Nr" + str(index+1)
plt.errorbar(ind[i:i+2], np.array(avg_trans), np.array(std_trans), label=label_location)
plt.subplots_adjust(hspace=0.5)
plt.legend()
plt.subplot(3, 1, 2)
label_location = "T" + str(Perf.location["Town"]) + "Nr" + str(index + 1)
plt.errorbar(ind[i:i+2], np.array(avg_rot), np.array(std_rot), label=label_location)
plt.legend()
width = 0.35
plt.subplot(3, 1, 3)
plot_track = plt.bar(ind[i:i+2], track_fail, width, color="r")
plot_loop = plt.bar(ind[i:i+2], loop_fail, width, bottom=track_fail, color="b")
i = i + 2
plt.subplot(3, 1, 1)
plt.xticks(ind, x)
plt.xlim(right=N)
plt.title("Translational component root mean square error of the relative pose error")
plt.ylabel("Trans. RMSE RPE [-]")
plt.subplot(3, 1, 2)
plt.xticks(ind, x)
plt.xlim(right=N)
plt.title("Rotational component root mean square error relative pose error")
plt.ylabel("Rot. RMSE RPE [deg/m]")
plt.subplot(3, 1, 3)
plt.title("Ratio removed due to false loop closure or tracking failure")
plt.xticks(ind,x)
plt.ylim(top=1.1)
plt.xlim(right=N)
plt.ylabel("Ratio removed of total data [-]")
plt.legend((plot_track, plot_loop), ("tracking failure", "false loop closure"))
plt.show()
def main():
"""Use matplotlib to create an error bandwidth"""
base_dir = "/home/sietse/results_carla0.9/stuckbehindvan/20fps/"
dynamic_scenarios = (20, 15, 10)
Towns = (1,2)
mode = "SLAM"
for Town in Towns:
if Town == 1:
starting_locations = (0, 27, 58)
elif Town == 2:
starting_locations = (18, 37, 78)
elif Town == 3:
starting_locations = (75, 97, 127, 132)
else:
print("Town does not exist")
return
plt.figure("RMSE RPE trans")
plt.title("RMSE Relative Pose Error translational component")
# total number of entries
N = (len(dynamic_scenarios) + 1) * len(starting_locations)
i = 0
ind = np.arange(N)
x_scatter = [0]*5*N
for i in range(N):
x_scatter[5*i:5*i+5] = [ind[i]]*5
x = []
for index, SL in enumerate(starting_locations):
avg_trans = []
avg_trans_upp = []
avg_trans_low = []
avg_rot = []
avg_rot_upp = []
avg_rot_low = []
scatter_trans = []
scatter_rot = []
track_fail = []
loop_fail = []
for ds in dynamic_scenarios:
dir_name_stat = DirName(Town, SL, "static")
dir_name_dyn = DirName(Town, SL, "dynamic", ds)
orb_static, gt = InspectJsonFileInDir(Town, SL, base_dir, dir_name_stat, mode)
orb_dynamic, gt = InspectJsonFileInDir(Town, SL, base_dir, dir_name_dyn, mode)
Perf = ScenarioLocationPerformance(ds, Town, SL, orb_static, orb_dynamic, gt)
if ds == 20:
x.extend(["T" + str(Perf.location["Town"]) + "Nr" + str(index + 1) + "\n" + " Static",
"T" + str(Perf.location["Town"]) + "Nr" + str(index + 1) + "\n" + " Dist:" + str(
Perf.scenario["Distance"])])
avg_trans.extend([Perf.rmse_static_avg[0], Perf.rmse_dynamic_avg[0]])
avg_trans_upp.extend([Perf.rmse_static_avg[0] + 2*Perf.rmse_static_std[0],
Perf.rmse_dynamic_avg[0] + 2*Perf.rmse_dynamic_std[0]])
avg_trans_low.extend([Perf.rmse_static_avg[0] - 2 * Perf.rmse_static_std[0],
Perf.rmse_dynamic_avg[0] - 2 * Perf.rmse_dynamic_std[0]])
raw_trans_static = [raw_rmse_static[0] for raw_rmse_static in Perf.rmse_static]
raw_trans_dynamic = [raw_rmse_dynamic[0] for raw_rmse_dynamic in Perf.rmse_dynamic]
scatter_trans.extend([raw_trans_static, raw_trans_dynamic])
avg_rot.extend([Perf.rmse_static_avg[1], Perf.rmse_dynamic_avg[1]])
avg_rot_upp.extend([Perf.rmse_static_avg[1] + 2*Perf.rmse_static_std[1],
Perf.rmse_dynamic_avg[1] + 2*Perf.rmse_dynamic_std[1]])
avg_rot_low.extend([Perf.rmse_static_avg[1] - 2 * Perf.rmse_static_std[1],
Perf.rmse_dynamic_avg[1] - 2 * Perf.rmse_dynamic_std[1]])
track_fail.extend([Perf.lost_track_static, Perf.lost_track_dynamic])
loop_fail.extend([Perf.false_loop_static, Perf.false_loop_dynamic])
else:
x.append("T" + str(Perf.location["Town"]) + "Nr" + str(index + 1) + "\n" + "Dist:" + str(
Perf.scenario["Distance"]))
avg_trans.append(Perf.rmse_dynamic_avg[0])
avg_trans_upp.append(Perf.rmse_dynamic_avg[0] + 2*Perf.rmse_dynamic_std[0])
avg_trans_low.append(Perf.rmse_dynamic_avg[0] - 2 * Perf.rmse_dynamic_std[0])
raw_trans_dynamic = [raw_rmse_dynamic[0] for raw_rmse_dynamic in Perf.rmse_dynamic]
scatter_trans.append(raw_trans_dynamic)
avg_rot.append(Perf.rmse_dynamic_avg[1])
avg_rot_upp.append(Perf.rmse_dynamic_avg[1] + 2*Perf.rmse_dynamic_std[1])
avg_rot_low.append(Perf.rmse_dynamic_avg[1] - 2*Perf.rmse_dynamic_std[1])
track_fail.append(Perf.lost_track_dynamic)
loop_fail.append(Perf.false_loop_dynamic)
plt.subplot(2, 1, 1)
# plt.grid(True)
label_location = "T" + str(Perf.location["Town"]) + "Nr" + str(index + 1)
pdb.set_trace()
plt.plot(ind[i:i + (len(dynamic_scenarios) + 1)], np.array(avg_trans), color='green')
plt.fill_between(ind[i:i + (len(dynamic_scenarios) + 1)], np.array(avg_trans_low), np.array(avg_trans_upp), color='green', alpha=0.5)
# for j in range(len(scatter_trans)):
# pdb.set_trace()
# plt.scatter(np.array(x_scatter[j*5:j*5+1]), np.array(scatter_trans[j]))
plt.subplots_adjust(hspace=0.5)
plt.legend()
plt.subplot(2, 1, 2)
label_location = "T" + str(Perf.location["Town"]) + "Nr" + str(index + 1)
plt.plot(ind[i:i + (len(dynamic_scenarios) + 1)], np.array(avg_rot), color='green',
label=label_location)
plt.fill_between(ind[i:i + (len(dynamic_scenarios) + 1)], np.array(avg_rot_low), np.array(avg_rot_upp), color='green', alpha=0.5)
plt.legend()
pdb.set_trace()
# width = 0.35
# plt.subplot(3, 1, 3)
# plot_track = plt.bar(ind[i:i + (len(dynamic_scenarios) + 1)], track_fail, width, color="r")
# plot_loop = plt.bar(ind[i:i + (len(dynamic_scenarios) + 1)], loop_fail, width, bottom=track_fail, color="b")
i = i + (len(dynamic_scenarios) + 1)
plt.subplot(2, 1, 1)
plt.xticks(ind, x)
if mode == "SLAM" and Town == 1:
plt.ylim(top=0.060)
plt.xlim(right=N)
plt.title("Translational component root mean square error of the relative pose error")
plt.ylabel("Trans. RMSE RPE [-]")
plt.subplot(2, 1, 2)
plt.xticks(ind, x)
if mode == "SLAM" and Town == 1:
plt.ylim(top=0.040)
plt.xlim(right=N)
plt.title("Rotational component root mean square error relative pose error")
plt.ylabel("Rot. RMSE RPE [deg/m]")
# plt.subplot(3, 1, 3)
# plt.title("Ratio removed due to false loop closure or tracking failure")
# plt.xticks(ind, x)
# plt.ylim(top=1.1)
# plt.xlim(right=N)
# plt.ylabel("Ratio removed of total data [-]")
# plt.legend((plot_track, plot_loop), ("tracking failure", "false loop closure"))
plt.show()
"RMSE rot",
"lost track",
"false relocalization"]
Town = 1
dv = 20
SL = 0
base_dir = "/home/sietse/results_carla0.9/stuckbehindvan/20fps/"
dir_stat = "T{}_SL{}_s/".format(Town, SL)
dir_dyn = "T{}_SL{}_d{}/".format(Town, SL, dv)
SLAM_mode = "SLAM"
Scenario = "Behind van"
# read the raw data files
orb_static, gt = InspectJsonFileInDir(Town, SL, base_dir, dir_stat, SLAM_mode)
orb_dynamic, gt = InspectJsonFileInDir(Town, SL, base_dir, dir_dyn, SLAM_mode)
# get the rmse values
performance_class = ScenarioLocationPerformance(dv, Town, SL, orb_static, orb_dynamic, gt)
# put the data in pandas format
panda_data_static = []
panda_data_dynamic = []
for index in range(5):
if Town == 1 and SL == 0:
trajectory_dynamic = "T{}Nr{}\nDist: {}".format(Town, 1, dv)
trajectory_static = "T{}Nr{}\nStatic".format(Town, 1)
panda_data_static.append(["Static", np.nan, Town, SL, trajectory_static, SLAM_mode,
performance_class.rmse_static[index][0],
performance_class.rmse_static[index][1],
def main():
"""Test if error bars show the results instinctively"""
base_dir = "/home/sietse/results_carla0.9/stuckbehindvan/20fps/"
dynamic_scenarios = (20, 15, 10)
Towns = (1, 2)
mode = "SLAM"
# for Town in Towns:
Town = 2
if Town == 1:
starting_locations = (0, 27, 58)
elif Town == 2:
starting_locations = (18, 37, 78)
elif Town == 3:
starting_locations = (75, 97, 127, 132)
else:
print("Town does not exist")
return
plt.figure("RMSE RPE trans")
plt.title("RMSE Relative Pose Error translational component")
# total number of entries
N = (len(dynamic_scenarios) + 1) * len(starting_locations)
i = 0 # index for the error plots
j = 0 # index for the bar plots
ind = np.arange(N)
ind_bar = np.arange(3 * N) # for each trajectory we want 3 representations
x = []
for index, SL in enumerate(starting_locations):
avg_trans = []
std_trans = []
avg_rot = []
std_rot = []
track_fail = []
loop_fail = []
avg_trans_vo = []
std_trans_vo = []
avg_rot_vo = []
std_rot_vo = []
track_fail_vo = []
loop_fail_vo = []
avg_trans_mc = []
std_trans_mc = []
avg_rot_mc = []
std_rot_mc = []
track_fail_mc = []
loop_fail_mc = []
for ds in dynamic_scenarios:
dir_name_stat = DirName(Town, SL, "static")
dir_name_dyn = DirName(Town, SL, "dynamic", ds)
# SLAM performance
orb_static, gt = InspectJsonFileInDir(Town, SL, base_dir,
dir_name_stat, "SLAM")
orb_dynamic, gt = InspectJsonFileInDir(Town, SL, base_dir,
dir_name_dyn, "SLAM")
# VO performance
orb_static_vo, gt_vo = InspectJsonFileInDir(
Town, SL, base_dir, dir_name_stat, "VO")
orb_dynamic_vo, gt_vo = InspectJsonFileInDir(
Town, SL, base_dir, dir_name_dyn, "VO")
# MC performance
orb_static_mc, gt_mc = InspectJsonFileInDir(
Town, SL, base_dir, dir_name_stat, "MC")
orb_dynamic_mc, gt_mc = InspectJsonFileInDir(
Town, SL, base_dir, dir_name_dyn, "MC")
Perf = ScenarioLocationPerformance(ds, Town, SL, orb_static,
orb_dynamic, gt)
Perf_vo = ScenarioLocationPerformance(ds, Town, SL, orb_static_vo,
orb_dynamic_vo, gt_vo)
Perf_mc = ScenarioLocationPerformance(ds, Town, SL, orb_static_mc,
orb_dynamic_mc, gt_mc)
if ds == 20:
x.extend([
"T" + str(Perf.location["Town"]) + "Nr" + str(index + 1) +
"\n" + " Static",
"T" + str(Perf.location["Town"]) + "Nr" + str(index + 1) +
"\n" + " Dist:" + str(Perf.scenario["Distance"])
])
# get SLAM data
avg_trans.extend(
[Perf.rmse_static_avg[0], Perf.rmse_dynamic_avg[0]])
std_trans.extend(
[Perf.rmse_static_std[0], Perf.rmse_dynamic_std[0]])
avg_rot.extend(
[Perf.rmse_static_avg[1], Perf.rmse_dynamic_avg[1]])
std_rot.extend(
[Perf.rmse_static_std[1], Perf.rmse_dynamic_std[1]])
track_fail.extend(
[Perf.lost_track_static, Perf.lost_track_dynamic])
loop_fail.extend(
[Perf.false_loop_static, Perf.false_loop_dynamic])
# get VO data
avg_trans_vo.extend(
[Perf_vo.rmse_static_avg[0], Perf_vo.rmse_dynamic_avg[0]])
std_trans_vo.extend(
[Perf_vo.rmse_static_std[0], Perf_vo.rmse_dynamic_std[0]])
avg_rot_vo.extend(
[Perf_vo.rmse_static_avg[1], Perf_vo.rmse_dynamic_avg[1]])
std_rot_vo.extend(
[Perf_vo.rmse_static_std[1], Perf_vo.rmse_dynamic_std[1]])
track_fail_vo.extend(
[Perf_vo.lost_track_static, Perf_vo.lost_track_dynamic])
loop_fail_vo.extend(
[Perf_vo.false_loop_static, Perf_vo.false_loop_dynamic])
# get MC data
avg_trans_mc.extend(
[Perf_mc.rmse_static_avg[0], Perf_mc.rmse_dynamic_avg[0]])
std_trans_mc.extend(
[Perf_mc.rmse_static_std[0], Perf_mc.rmse_dynamic_std[0]])
avg_rot_mc.extend(
[Perf_mc.rmse_static_avg[1], Perf_mc.rmse_dynamic_avg[1]])
std_rot_mc.extend(
[Perf_mc.rmse_static_std[1], Perf_mc.rmse_dynamic_std[1]])
track_fail_mc.extend(
[Perf_mc.lost_track_static, Perf_mc.lost_track_dynamic])
loop_fail_mc.extend(
[Perf_mc.false_loop_static, Perf_mc.false_loop_dynamic])
else:
x.append("T" + str(Perf.location["Town"]) + "Nr" +
str(index + 1) + "\n" + "Dist:" +
str(Perf.scenario["Distance"]))
# get SLAM data
avg_trans.append(Perf.rmse_dynamic_avg[0])
std_trans.append(Perf.rmse_dynamic_std[0])
avg_rot.append(Perf.rmse_dynamic_avg[1])
std_rot.append(Perf.rmse_dynamic_std[1])
track_fail.append(Perf.lost_track_dynamic)
loop_fail.append(Perf.false_loop_dynamic)
# get VO data
avg_trans_vo.append(Perf_vo.rmse_dynamic_avg[0])
std_trans_vo.append(Perf_vo.rmse_dynamic_std[0])
avg_rot_vo.append(Perf_vo.rmse_dynamic_avg[1])
std_rot_vo.append(Perf_vo.rmse_dynamic_std[1])
track_fail_vo.append(Perf_vo.lost_track_dynamic)
loop_fail_vo.append(Perf_vo.false_loop_dynamic)
# get MC data
avg_trans_mc.append(Perf_mc.rmse_dynamic_avg[0])
std_trans_mc.append(Perf_mc.rmse_dynamic_std[0])
avg_rot_mc.append(Perf_mc.rmse_dynamic_avg[1])
std_rot_mc.append(Perf_mc.rmse_dynamic_std[1])
track_fail_mc.append(Perf_mc.lost_track_dynamic)
loop_fail_mc.append(Perf_mc.false_loop_dynamic)
# have to loop it otherwise you get the same color
plt.subplot(3, 1, 1)
# plt.grid(True)
plot_vo_trans = plt.errorbar(ind[i:i + (len(dynamic_scenarios) + 1)],
np.array(avg_trans_vo),
np.array(std_trans_vo),
color='blue',
label="VO")
plot_mc_trans = plt.errorbar(ind[i:i + (len(dynamic_scenarios) + 1)],
np.array(avg_trans_mc),
np.array(std_trans_mc),
color='red',
label="MC")
plot_slam_trans = plt.errorbar(ind[i:i + (len(dynamic_scenarios) + 1)],
np.array(avg_trans),
np.array(std_trans),
color='lime',
label="SLAM")
plt.subplots_adjust(hspace=0.5)
plt.subplot(3, 1, 2)
plot_vo_rot = plt.errorbar(ind[i:i + (len(dynamic_scenarios) + 1)],
np.array(avg_rot_vo),
np.array(std_rot_vo),
color='blue',
label="VO")
plot_mc_rot = plt.errorbar(ind[i:i + (len(dynamic_scenarios) + 1)],
np.array(avg_rot_mc),
np.array(std_rot_mc),
color='red',
label="MC")
plot_slam_rot = plt.errorbar(ind[i:i + (len(dynamic_scenarios) + 1)],
np.array(avg_rot),
np.array(std_rot),
color='lime',
label="SLAM")
width = 0.3
tf_color = "black"
lc_color = "silver"
plt.subplot(3, 1, 3)
plt.bar(ind_bar[j:j + 3 * 4:3], track_fail, width, color=tf_color)
plt.bar(ind_bar[j:j + 3 * 4:3],
loop_fail,
width,
bottom=track_fail,
color=lc_color)
plt.bar(ind_bar[j + 1:j + 1 + 3 * 4:3],
track_fail_vo,
width,
color=tf_color)
plt.bar(ind_bar[j + 1:j + 1 + 3 * 4:3],
loop_fail_vo,
width,
bottom=track_fail_vo,
color=lc_color)
plot_tf = plt.bar(ind_bar[j + 2:j + 2 + 3 * 4:3],
track_fail_mc,
width,
color=tf_color)
plot_lc = plt.bar(ind_bar[j + 2:j + 2 + 3 * 4:3],
loop_fail_mc,
width,
bottom=track_fail_mc,
color=lc_color)
# plot_track = plt.bar(ind_bar[j:j+(len(dynamic_scenarios)+1):3], track_fail, width, color="r")
# plot_loop = plt.bar(ind_bar[j:j+(len(dynamic_scenarios)+1):3], loop_fail, width, bottom=track_fail, color="b")
# pdb.set_trace()
i = i + (len(dynamic_scenarios) + 1)
j = j + 3 * (len(dynamic_scenarios) + 1)
plt.subplot(3, 1, 1)
plt.xticks(ind, x)
plt.xlim(right=N)
plt.title(
"Translational component root mean square error of the relative pose error"
)
plt.ylabel("Trans. RMSE RPE [-]")
plt.legend([plot_slam_trans, plot_vo_trans, plot_mc_trans],
["SLAM", "VO", "MC"])
plt.subplot(3, 1, 2)
plt.xticks(ind, x)
plt.xlim(right=N)
plt.title(
"Rotational component root mean square error relative pose error")
plt.ylabel("Rot. RMSE RPE [deg/m]")
plt.legend([plot_slam_rot, plot_vo_rot, plot_mc_rot], ["SLAM", "VO", "MC"])
plt.subplot(3, 1, 3)
plt.title("Ratio removed due to false loop closure or tracking failure")
bar_ticks = ["" for i in range(3 * N)]
for index, label in enumerate(x):
new_index = index * 3
bar_ticks[new_index] = label + "\n SLAM"
try:
bar_ticks[new_index + 1] = "VO"
bar_ticks[new_index + 2] = "MC"
except IndexError:
continue
plt.xticks(ind_bar, bar_ticks)
plt.legend([plot_tf, plot_lc],
["Tracking failure", "False relocalization"])
# plt.xticks(ind_bar[0::3], x)
plt.ylim(top=1.1)
plt.xlim(right=3 * N)
plt.ylabel("Ratio removed of total data [-]")
# plt.legend((plot_track, plot_loop), ("tracking failure", "false loop closure"))
plt.show()