def get_plot_phi_d(self, ground_truth=None, bgcolor=dtu.ColorConstants.RGB_DUCKIETOWN_YELLOW): figure_args = dict(facecolor=dtu.matplotlib_01_from_rgb(bgcolor)) a = CreateImageFromPylab(dpi=120, figure_args=figure_args) gh = self.grid_helper with a as pylab: grid_helper_plot_field(gh, self.belief, pylab) grid_helper_annotate_axes(gh, pylab) estimate = self.get_estimate() if ground_truth is not None: ground_truth_location = \ self._localization_template.coords_from_pose(ground_truth) grid_helper_mark_point(gh, pylab, ground_truth_location, color='green', markersize=4) grid_helper_mark_point(gh, pylab, estimate, color='magenta', markersize=10) s = '' s += "status = %s" % self.get_status() for name, spec in zip(gh._names, gh._specs): convert = lambda x: '%.2f %s' % (convert_unit( x, spec.units, spec.units_display), spec.units_display) s += '\n' s += "\nest %s = %s" % (name, convert(estimate[name])) if ground_truth is not None: s += "\ntrue %s = %s" % ( name, convert(ground_truth_location[name])) err = np.abs(ground_truth_location[name] - estimate[name]) s += '\nabs err = %s' % (convert(err)) cell = spec.resolution percent = 100.0 / cell * err s += '\nrel err = %.1f %% of cell' % (percent) s += '\n true = green dot' s += '\n' s += "\nentropy = %.4f" % self.get_entropy() s += "\nmax = %.4f" % self.belief.max() s += "\nmin = %.4f" % self.belief.min() pylab.annotate(s, xy=(0.7, 0.45), xycoords='figure fraction') grid_helper_set_axes(gh, pylab) return a.get_bgr()
def get_plot_phi_d( self, ground_truth=None, bgcolor: dtu.RGBColor8 = dtu.ColorConstants.RGB_DUCKIETOWN_YELLOW ): facecolor = dtu.matplotlib_01_from_rgb(bgcolor) figure_args = dict(facecolor=facecolor) a = dtu.CreateImageFromPylab(dpi=120, figure_args=figure_args) gh = self.grid_helper with a as pylab: grid_helper_plot_field(gh, self.belief, pylab) grid_helper_annotate_axes(gh, pylab) estimate = self.get_estimate() if ground_truth is not None: ground_truth_location = self._localization_template.coords_from_pose(ground_truth) grid_helper_mark_point(gh, pylab, ground_truth_location, color="green", markersize=4) grid_helper_mark_point(gh, pylab, estimate, color="magenta", markersize=10) s = "" s += f"status = {self.get_status()}" for name, spec in zip(gh._names, gh._specs): convert = lambda x: "%.2f %s" % ( convert_unit(x, spec.units, spec.units_display), spec.units_display, ) s += "\n" s += f"\nest {name} = {convert(estimate[name])}" if ground_truth is not None: s += f"\ntrue {name} = {convert(ground_truth_location[name])}" err = np.abs(ground_truth_location[name] - estimate[name]) s += f"\nabs err = {convert(err)}" cell = spec.resolution percent = 100.0 / cell * err s += f"\nrel err = {percent:.1f} % of cell" s += "\n true = green dot" s += "\n" s += f"\nentropy = {self.get_entropy():.4f}" s += f"\nmax = {self.belief.max():.4f}" s += f"\nmin = {self.belief.min():.4f}" pylab.annotate(s, xy=(0.7, 0.45), xycoords="figure fraction") grid_helper_set_axes(gh, pylab) return a.get_bgr()