def SaveRpeData(self, gt):
"""Calculate the RPE dist for all sequences and the RMSE"""
for orb in self.slam_stat:
time_used, trans_errors, rot_errors = evaluate_RPE_dist(
gt, orb, 100)
RawRpeSingle = {
"time": time_used,
"RPE_trans": trans_errors,
"RPE_rot": rot_errors,
"plotstyle": orb.plotstyle,
"label": orb.label
}
self.raw_rpe_stat.append(RawRpeSingle)
for orb in self.slam_dyn:
time_used, trans_errors, rot_errors = evaluate_RPE_dist(
gt, orb, 100)
RawRpeSingle = {
"time": time_used,
"RPE_trans": trans_errors,
"RPE_rot": rot_errors,
"plotstyle": orb.plotstyle,
"label": orb.label
}
self.raw_rpe_dyn.append(RawRpeSingle)
def RmseRpe(ORB, gt):
"""Calculates the mean value of RMSE values of an ORB list"""
rmse_trans_list = []
rmse_rot_list = []
for orb in ORB:
t, trans, rot = evaluate_RPE_dist(gt, orb, 100)
rmse_trans = calc_rmse(trans)
rmse_rot = calc_rmse(rot)
rmse_trans_list.append(rmse_trans)
rmse_rot_list.append(rmse_rot)
rmse_trans = np.array(rmse_trans_list)
rmse_rot = np.array(rmse_rot_list)
trans_mean = rmse_trans.mean()
trans_std = rmse_trans.std()
rot_mean = rmse_rot.mean()
rot_std = rmse_rot.std()
RMSE_mean = (trans_mean, rot_mean)
RMSE_std = (trans_std, rot_std)
return RMSE_mean, RMSE_std
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()
def AvgStdRmseNoMapping(base_dir, town, starting_locations, map_estimation,
pose_estimation):
"""Input which combination results you want to have (e.g. static map, pose estimation sc 2) and starting locations.
Function outputs the avg and std rmse values"""
avg_rmse_trans_list = []
std_rmse_trans_list = []
avg_rmse_rot_list = []
std_rmse_rot_list = []
for SL in starting_locations:
if pose_estimation == "Stat":
folder = "stuckbehindvan/20fps/T{}_SL{}_s".format(town, SL)
elif pose_estimation == "Sc1":
folder = "stuckbehindvan/20fps/T{}_SL{}_d15".format(town, SL)
elif pose_estimation == "Sc2":
folder = "VansOppositeRoad/T{}_SL{}_d10".format(town, SL)
string_convention = map_estimation + pose_estimation
dir = base_dir + folder
# Collect the orb data and ground truth from json files
orb_data = []
for orb_nr in range(5):
file_name = "/T{}_SL{}_LM_{}_{}_json.txt".format(
town, SL, string_convention, orb_nr)
try:
orb = json2crf(dir, file_name)
orb_data.append(orb)
except IOError:
continue
# False relocalization happened during mapping in one of the orb data points
if SL == 0 and map_estimation == "Sc2":
orb_data.pop(0)
# trajectory is always the same so ground truth is independent of scenario
ground_truth_dir = base_dir + "stuckbehindvan/20fps/T{}_SL{}_s".format(
town, SL)
gt = json2crf(ground_truth_dir,
"/T{}_SL{}_s_gt_json.txt".format(town, SL))
rmse_trans_list = []
rmse_rot_list = []
for i, orb in enumerate(orb_data):
time_used, trans_errors, rot_errors = evaluate_RPE_dist(
gt, orb, 100)
# utilize data from a certain point in time, before that ORB SLAM may not realize that a new rosbag has started
for index, time in enumerate(time_used):
if time >= 5.0:
index_used = index
break
time_used = time_used[index_used::]
trans_errors = trans_errors[index_used::]
rot_errors = rot_errors[index_used::]
trans_rmse = calc_rmse(trans_errors)
rmse_trans_list.append(trans_rmse)
rot_rmse = calc_rmse(rot_errors)
rmse_rot_list.append(rot_rmse)
rmse_trans_all = np.array(rmse_trans_list)
avg_rmse_trans = np.mean(rmse_trans_all)
avg_rmse_trans_list.append(avg_rmse_trans)
std_rmse_trans = np.std(rmse_trans_all)
std_rmse_trans_list.append(std_rmse_trans)
rmse_rot_all = np.array(rmse_rot_list)
avg_rmse_rot = np.mean(rmse_rot_all)
avg_rmse_rot_list.append(avg_rmse_rot)
std_rmse_rot = np.std(rmse_rot_all)
std_rmse_rot_list.append(std_rmse_rot)
pdb.set_trace()
return avg_rmse_trans_list, std_rmse_trans_list, avg_rmse_rot_list, std_rmse_rot_list
def AvgStdRmseMapLoc(base_dir, town, starting_locations, pose_estimation):
"""Compare the result to localization without map """
avg_rmse_trans_list = []
std_rmse_trans_list = []
avg_rmse_rot_list = []
std_rmse_rot_list = []
for SL in starting_locations:
if pose_estimation == "Stat":
folder = "stuckbehindvan/20fps/T{}_SL{}_s".format(town, SL)
file_name = "/T{}_SL{}_s_orb_{}_json.txt"
elif pose_estimation == "Sc1":
folder = "stuckbehindvan/20fps/T{}_SL{}_d15".format(town, SL)
file_name = "/T{}_SL{}_d15_orb_{}_json.txt"
elif pose_estimation == "Sc2":
folder = "VansOppositeRoad/T{}_SL{}_d10".format(town, SL)
file_name = "/T{}_SL{}_d10_orb_{}_json.txt"
dir = base_dir + folder
# Collect the orb data and ground truth from json files
orb_data = []
for orb_nr in range(5):
try:
orb = json2crf(dir, file_name.format(town, SL, orb_nr))
orb_data.append(orb)
except IOError:
continue
# trajectory is always the same so ground truth is independent of scenario
ground_truth_dir = base_dir + "stuckbehindvan/20fps/T{}_SL{}_s".format(
town, SL)
gt = json2crf(ground_truth_dir,
"/T{}_SL{}_s_gt_json.txt".format(town, SL))
rmse_trans_list = []
rmse_rot_list = []
for i, orb in enumerate(orb_data):
time_used, trans_errors, rot_errors = evaluate_RPE_dist(
gt, orb, 100)
trans_rmse = calc_rmse(trans_errors)
rmse_trans_list.append(trans_rmse)
rot_rmse = calc_rmse(rot_errors)
rmse_rot_list.append(rot_rmse)
if i == 4 and SL == 58 and pose_estimation == "Sc2":
rmse_trans_list.pop(2)
rmse_rot_list.pop(2)
rmse_trans_all = np.array(rmse_trans_list)
avg_rmse_trans = np.mean(rmse_trans_all)
avg_rmse_trans_list.append(avg_rmse_trans)
std_rmse_trans = np.std(rmse_trans_all)
std_rmse_trans_list.append(std_rmse_trans)
rmse_rot_all = np.array(rmse_rot_list)
avg_rmse_rot = np.mean(rmse_rot_all)
avg_rmse_rot_list.append(avg_rmse_rot)
std_rmse_rot = np.std(rmse_rot_all)
std_rmse_rot_list.append(std_rmse_rot)
return avg_rmse_trans_list, std_rmse_trans_list, avg_rmse_rot_list, std_rmse_rot_list
ev_dis = 10
base_dir_stat = "/home/sietse/results_carla0.9/stuckbehindvan/20fps/"
base_dir_dyn = "/home/sietse/results_carla0.9/VansOppositeRoad/"
dir_name_stat = "T{}_SL{}_s/".format(Town, SL)
dir_name_dyn = "T{}_SL{}_d{}/".format(Town, SL, dv)
orb_static_list, gt = InspectJsonFileInDir(Town, SL, base_dir_stat, dir_name_stat, "SLAM")
orb_dynamic_list, gt = InspectJsonFileInDir(Town, SL, base_dir_dyn, dir_name_dyn, "SLAM")
orb_dynamic_list_mc, gt = InspectJsonFileInDir(Town, SL, base_dir_dyn, dir_name_dyn, "MC")
i = 0
for orb_stat, orb_dyn, orb_dyn_mc in zip(orb_static_list, orb_dynamic_list, orb_dynamic_list_mc):
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)
# dyn_time_mc, dyn_trans_err_mc, dyn_rot_err_mc = evaluate_RPE_dist(gt, orb_dyn_mc, ev_dis)
if i == 2:
for index, time in enumerate(dyn_time):
if time == 44.5:
imageB_t = time
imageB_trans = dyn_trans_err[index]
imageB_rot = dyn_rot_err[index]
elif time == 57.0:
imageC_t = time
imageC_trans = dyn_trans_err[index]
imageC_rot = dyn_rot_err[index]
evaluate_trajectory([orb_stat, orb_dyn])
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()