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 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
def __init__(self, ds, Town, SL, slam_stat, slam_dyn, gt):
# description of scenario
self.scenario = {"Scenario": "Stuck behind van", "Distance": ds}
self.location = {"Town": Town, "Location": SL}
# contains the list of all crs objects static
self.slam_stat = []
# contains list of all crs objects dynamic
self.slam_dyn = []
# raw RPE dist data in a dict form. Everything is here to make a plot that explains everything
self.raw_rpe_stat = []
self.raw_rpe_dyn = []
# filtered RPE dist data in dict form. All failed tracking and false loop closure are removed
self.filtered_rpe_stat = []
self.filtered_rpe_dyn = []
# Root mean square error translational and rotational dynamic and static of each filtered sequence
self.rmse_static = []
self.rmse_dynamic = []
# ratio that lost track of location
self.lost_track_static = ()
self.lost_track_dynamic = ()
# ratio that had false loop closure
# NOTE: THIS DETECTS ANY LOOP CLOSURES, NOT FALSE LOOP CLOSURES
self.false_loop_static = ()
self.false_loop_dynamic = ()
# total ratio that is filtered out due to false loop closure or lost track
self.ratio_filtered_static = ()
self.ratio_filtered_dynamic = ()
# average and variance root mean square error translational and rotational component, static and dynamic
self.rmse_static_avg = ()
self.rmse_static_std = ()
self.rmse_dynamic_avg = ()
self.rmse_dynamic_std = ()
# Percentage increase error dynamic over static
self.static_vs_dynamic_avg = ()
self.static_vs_dynamic_std = ()
# save the crf data in lists
for orb in slam_stat:
self.slam_stat.append(orb)
for orb in slam_dyn:
self.slam_dyn.append(orb)
# get the RPE performance from this data
self.SaveRpeData(gt)
# find and filter all data that loose track during orb estimation
filter_index_stat, filter_index_dyn = self.FilterSuccesfullTracking(gt)
tracked_raw_rpe_stat = [
self.raw_rpe_stat[index] for index in filter_index_stat
]
tracked_raw_rpe_dyn = [
self.raw_rpe_dyn[index] for index in filter_index_dyn
]
tracked_slam_stat = [
self.slam_stat[index] for index in filter_index_stat
]
tracked_slam_dyn = [self.slam_dyn[index] for index in filter_index_dyn]
# find and filter all data that have false loop closure and store data in class
correct_loop_dyn = self.FilterFalseLoopClosure(tracked_slam_dyn)
self.ratio_filtered_dynamic = 1.0 - float(
len(correct_loop_dyn)) / float(len(self.slam_dyn))
self.false_loop_dynamic = self.ratio_filtered_dynamic - self.lost_track_dynamic
correct_loop_stat = self.FilterFalseLoopClosure(tracked_slam_stat)
self.ratio_filtered_static = 1.0 - float(
len(correct_loop_stat)) / float(len(self.slam_stat))
self.false_loop_static = self.ratio_filtered_static - self.lost_track_static
self.filtered_rpe_stat = [
tracked_raw_rpe_stat[index] for index in correct_loop_stat
]
self.filtered_rpe_dyn = [
tracked_raw_rpe_dyn[index] for index in correct_loop_dyn
]
# calculate Root Mean Square Error of the filtered data
for filtered_rpe_data in self.filtered_rpe_stat:
trans_errors = filtered_rpe_data["RPE_trans"]
rot_errors = filtered_rpe_data["RPE_rot"]
orb_rmse = (calc_rmse(trans_errors), calc_rmse(rot_errors))
self.rmse_static.append(orb_rmse)
for filtered_rpe_data in self.filtered_rpe_dyn:
trans_errors = filtered_rpe_data["RPE_trans"]
rot_errors = filtered_rpe_data["RPE_rot"]
orb_rmse = (calc_rmse(trans_errors), calc_rmse(rot_errors))
self.rmse_dynamic.append(orb_rmse)
# calculate average performance of the filtered data
self.CalculateAverageRMSE()
# compare static performance vs dynamic performance
self.CompareAvgStaticVsDynamic()
