示例#1
0
    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()