def print_update_stats(self, plot_data=True):
print('\n---------- UPDATE STATS -----------')
current_gt = self.robot.get_gt_pose()
gt_index = self.mapper.to_map(*current_gt)
argmax_bel = get_max(self.bel)
current_belief = self.mapper.from_map(*argmax_bel[0])
pos_error = np.array(current_gt) - np.array(current_belief)
# Print prob as a string to prevent rounding
print("GT index : ", gt_index)
print("Bel index : ", argmax_bel[0], "with prob = ",
str(argmax_bel[1])[:9])
print("Bel_bar prob at index = ", self.bel_bar[argmax_bel[0]])
print("\nGT : ({:.3f}, {:.3f}, {:.3f})".format(*current_gt))
print("Belief : ({:.3f}, {:.3f}, {:.3f})".format(*current_belief))
print("POS ERROR : ({:.3f}, {:.3f}, {:.3f})".format(*pos_error))
# Plot data
if (plot_data == True):
self.plotter.plot_point(current_belief[0], current_belief[1], BEL)
print('---------- UPDATE STATS -----------')
return pos_error
def print_prediction_stats(self, plot_data=True):
print('\n---------- PREDICTION STATS -----------')
current_odom = self.robot.get_pose()
current_gt = self.robot.get_gt_pose()
gt_index = self.mapper.to_map(*current_gt)
argmax_bel_bar = get_max(self.bel_bar)
current_prior_belief = self.mapper.from_map(*argmax_bel_bar[0])
pos_error = np.array(current_gt) - np.array(current_prior_belief)
# Print prob as a string to prevent rounding
print("GT index : ", gt_index)
print("Prior Bel index : ", argmax_bel_bar[0], "with prob = ",
str(argmax_bel_bar[1])[:9])
print("POS ERROR : ({:.3f}, {:.3f}, {:.3f})".format(*pos_error))
# Plot data
if (plot_data == True):
self.plotter.plot_point(current_gt[0], current_gt[1], GT)
self.plotter.plot_point(current_odom[0], current_odom[1], ODOM)
self.plotter.visualize_prior_bel()
print('---------- PREDICTION STATS -----------')
return pos_error