def graph_realization_before_measurements(self, key_num, fig_num=0, show_prev=False, show_data=False):

        fig = plt.figure(fig_num)
        ax = fig.gca()
        plt.gca()
        plt.rcParams.update({'font.size': 16})

        for realization in self.belief_matrix:
            pose_exp = self.belief_matrix[realization].result.atPose3(self.belief_matrix[realization].X(key_num))
            if show_data is True:
                print(pose_exp)
            pose_cov = self.belief_matrix[realization].marginals.marginalCovariance(
                self.belief_matrix[realization]
                .X(key_num))
            pose_cov_2x2 = pose_cov[3:5, 3:5]
            plotterdaac2d.plotPose2(fig_num, pose_exp)
            pose_cov_rotated = plotterdaac2d.rotate_covariance(pose_exp, pose_cov_2x2)
            color_tuple = ((1 - np.exp(self.belief_matrix[realization].logWeight)) * 0.7,
                           (1 - np.exp(self.belief_matrix[realization].logWeight)) * 0.7,
                           np.exp(self.belief_matrix[realization].logWeight))
            alpha_check = np.exp(self.belief_matrix[realization].logWeight) * 0.3 + 0.3
            plotterdaac2d.plot_ellipse(ax, [pose_exp.x(), pose_exp.y()], pose_cov_rotated, color=color_tuple,
                                       alpha=alpha_check)

            if show_prev is True:
                pose_exp_prev = self.belief_matrix[realization].result.atPose3(self.belief_matrix[realization].X(key_num-1))
                pose_cov_prev = self.belief_matrix[realization].marginals.marginalCovariance(
                    self.belief_matrix[realization]
                        .X(key_num-1))
                pose_cov_2x2_prev = pose_cov_prev[3:5, 3:5]
                plotterdaac2d.plotPose2(fig_num, pose_exp_prev)
                if show_data is True:
                    print(pose_exp_prev)



            # ax.text(pose_exp.x(), pose_exp.y(), 'X' + str(key_num))# + ', ' + str(np.exp(self.belief_matrix[realization].logWeight)))

        #plt.title('Multiple realizations of pose X' + str(key_num) + ', before measurements')
        plt.tight_layout()
        plt.show()
Exemple #2
0
    def display_graph(self, fig_num=0, plt_size=10):

        # Figure ID
        fig = plt.figure(fig_num)
        ax = fig.gca()
        plt.gca()

        if self.resultsFlag is False:
            self.optimize()
        plotterdaac2d.plot2DPoints(fig_num, self.result, 'rx')

        # Plot cameras
        idx = 0
        while self.result.exists(self.X(idx)):

            pose_x = self.result.atPose3(self.X(idx))
            plotterdaac2d.plotPose2(fig_num, pose_x, 0.3)

            # Annotate the point
            ax.text(pose_x.x(), pose_x.y(), 'X' + str(idx))

            # Plot covariance ellipse
            mu, cov = plotterdaac2d.extract_mu_cov_2d(self.result,
                                                      self.marginals,
                                                      self.X(idx))

            # Rotate covariance matrix
            # cov_rotated = plotterdaac2d.rotate_covariance(pose_x, cov)
            angles = pose_x.rotation().matrix()
            psi = np.arctan2(angles[1, 0], angles[0, 0])
            gRp = np.array([[np.cos(psi), -np.sin(psi)],
                            [np.sin(psi), np.cos(psi)]])
            multi1 = np.matmul(gRp, cov)
            cov_rotated = np.matmul(multi1, np.transpose(gRp))

            # Plot rotated cov ellipse
            plotterdaac2d.plot_ellipse(ax, mu, cov_rotated)

            idx += 1

        # Plot objects
        idx = 1
        # self.result.print()
        while self.result.exists(self.XO(idx)):

            pose_xo = self.result.atPose3(self.XO(idx))
            plotterdaac2d.plotPoint2(fig_num, pose_xo.translation(), 'bo')

            # Annotate the point
            ax.text(pose_xo.x(), pose_xo.y(), 'O' + str(idx))

            # Plot covariance ellipse
            mu, cov = plotterdaac2d.extract_mu_cov_2d(self.result,
                                                      self.marginals,
                                                      self.XO(idx))

            # Rotate covariance matrix
            angles = pose_xo.rotation().matrix()
            psi = np.arctan2(angles[1, 0], angles[0, 0])
            gRp = np.array([[np.cos(psi), -np.sin(psi)],
                            [np.sin(psi), np.cos(psi)]])
            multi1 = np.matmul(gRp, cov)
            cov_rotated = np.matmul(multi1, np.transpose(gRp))

            # Plot rotated cov ellipse
            plotterdaac2d.plot_ellipse(ax, mu, cov_rotated)

            idx += 1

        plt.title('Figure number: ' + str(fig_num) +
                  '. \nData association realization: ' +
                  str(self.daRealization) + '. \nClass realization: ' +
                  str(self.classRealization) + '.')
        ax.set_xlabel('X axis [m]')
        ax.set_ylabel('Y axis [m]')
        # ax.set_zlabel('Z axis [m]')

        # draw
        plt.show()
    def graph_realizations_in_one(self, key_num, fig_num=0, show_obj=False,
                                  show_weights=False, show_plot=True, plt_save_name=None):

        if show_weights is False:
            fig, ax = plt.subplots(1, sharex=False)
            ax_first = ax
        else:
            ax = list()
            fig= plt.figure(fig_num)
            grid = plt.GridSpec(1, 2)
            ax.append(fig.add_subplot(grid[0, 0]))
            ax.append(fig.add_subplot(grid[0, 1]))
            ax_first = ax[0]
        plt.gca()
        plt.rcParams.update({'font.size': 16})

        #fig = plt.figure(fig_num)
        #ax = fig.gca()
        #plt.gca()

        for realization in self.belief_matrix:
            self.belief_matrix[realization].optimize()

        #beta_realizations = self.compute_beta_only_realizations()
        #beta_weights = self.compute_beta_weights()

        idx = 0

        for realization in self.belief_matrix:


            marginals = self.belief_matrix[realization].marginals_after
            #marginals = gtsam.Marginals(self.belief_matrix[realization].graph, self.belief_matrix[realization].result)
            pose_exp = self.belief_matrix[realization].result.atPose3(self.belief_matrix[realization].X(key_num))
            pose_cov = marginals.marginalCovariance(
                self.belief_matrix[realization]
                .X(key_num))
            pose_cov_2x2 = pose_cov[3:5, 3:5]

            weight = np.exp(self.belief_matrix[realization].logWeight)

            plotterdaac2d.plotPose2(fig_num, pose_exp, ax=ax_first)
            #pose_cov_rotated = pose_cov_2x2
            pose_cov_rotated = self.belief_matrix[realization].rotate_cov_6x6(pose_cov, pose_exp.rotation().matrix())
            #pose_cov_rotated = plotterdaac2d.rotate_covariance(pose_exp, pose_cov_2x2)
            color_tuple = ((1 - weight) * 0.7,
                           (1 - weight) * 0.7,
                           (1 - weight) * 0.7)
            # Adjusting ellipse alpha values based on if classification is turned on or not.
            if self.cls_enable is False:
                line_thickness = weight* 3.3 + 0.2
            else:
                line_thickness = weight * 3.3 + 0.2
            if show_weights is False:
                plotterdaac2d.plot_ellipse(ax, [pose_exp.x(), pose_exp.y()], pose_cov_rotated[3:5,3:5], color=None,
                                           edgecolor='r',
                                           alpha=1, enlarge=3,
                                           linewidth=line_thickness)
                #ax.text(pose_exp.x(), pose_exp.y(), 'R' + str(idx+1))
            else:
                plotterdaac2d.plot_ellipse(ax[0], [pose_exp.x(), pose_exp.y()], pose_cov_rotated[3:5,3:5], color=None,
                                           edgecolor='r',
                                           alpha=1, enlarge=3,
                                           linewidth=line_thickness)
                #ax[0].text(pose_exp.x(), pose_exp.y(), 'R' + str(idx+1))

            if show_obj is True:
                for obj in self.obj_realization:
                    pose_obj = self.belief_matrix[realization].result.atPose3(self.belief_matrix[realization].XO(obj))
                    pose_obj_cov = self.belief_matrix[realization].marginals_after.marginalCovariance\
                        (self.belief_matrix[realization].XO(obj))
                    pose_obj_cov_2x2 = pose_obj_cov[3:5, 3:5]
                    plotterdaac2d.plotPoint2(fig_num, pose_obj, 'k.', ax=ax_first)
                    #pose_obj_cov_rotated = pose_obj_cov_2x2
                    pose_obj_cov_rotated = self.belief_matrix[realization].rotate_cov_6x6(pose_obj_cov,
                                                                                          pose_obj.rotation().matrix())
                    color_tuple = ((1 - np.exp(self.belief_matrix[realization].logWeight)) * 0.7,
                                (1 - np.exp(self.belief_matrix[realization].logWeight)) * 0.7,
                                (1 - np.exp(self.belief_matrix[realization].logWeight)) * 0.7)
                    line_thickness = np.exp(self.belief_matrix[realization].logWeight) * 2.5 + 0.5


                    if show_weights is False:
                        plotterdaac2d.plot_ellipse(ax, [pose_obj.x(), pose_obj.y()], pose_obj_cov_rotated[3:5,3:5],
                                                   color=None,
                                                   edgecolor=(0.5,0.5,0.5),
                                                   alpha=1, enlarge=3,
                                                   linewidth=line_thickness)
                    else:
                        plotterdaac2d.plot_ellipse(ax[0], [pose_obj.x(), pose_obj.y()], pose_obj_cov_rotated[3:5,3:5],
                                                   color=None,
                                                   edgecolor=(0.5,0.5,0.5),
                                                   alpha=1, enlarge=3,
                                                   linewidth=line_thickness)

            idx += 1

        if show_weights is True:

            # Plot class realization probability bar
            cls_weights = self.compute_cls_weights()
            cls_weights_list = np.zeros(self.number_of_objects)
            x_labels = list()
            for realization in cls_weights:
                for idx in range(self.number_of_objects):
                    if realization[idx][1] == 1:
                        cls_weights_list[idx] += cls_weights[realization]
                    x_labels.append('Object ' + str(realization[idx][0]))
            plotterdaac2d.plot_weight_bars(ax[1], cls_weights_list, labels=x_labels, log_weight_flag=False)
            x = np.arange(self.number_of_objects)
            ax[1].set_xticks(x)
            ax[1].set_xticklabels(x_labels)

            # Rotate labes
            for tick in ax[1].get_xticklabels():
                tick.set_rotation(90)

            #ax[2].set_title('Probability of class 1, all objects')

        #ax_first.set_title('Multiple realizations of pose X' + str(key_num) + ', after measurements')
        if show_plot is True:
            plt.tight_layout()
            if plt_save_name is not None:
                plt.savefig(plt_save_name + '.eps', format='eps')
            plt.show()

        return ax
    def display_graph(self,
                      fig_num=0,
                      plt_size=10,
                      plot_line=False,
                      display_title=True,
                      show_plot=True,
                      color='r',
                      ax=None,
                      enlarge=1):

        # Figure ID
        if ax is None:
            fig = plt.figure(fig_num)
            ax = fig.gca()
        plt.gca()

        if self.resultsFlag is False:
            self.optimize()
        plotterdaac2d.plot2DPoints(fig_num, self.result, 'rx')

        # Plot cameras
        if plot_line is False:
            idx = 0
            while self.result.exists(self.X(idx)):

                pose_x = self.result.atPose3(self.X(idx))
                plotterdaac2d.plotPose2(fig_num, pose_x, 0.3)

                # Annotate the point
                ax.text(pose_x.x(), pose_x.y(), 'X' + str(idx))

                # Plot covariance ellipse
                mu, cov = plotterdaac2d.extract_mu_cov_2d(
                    self.result, self.marginals_after, self.X(idx))

                # Rotate covariance matrix
                # cov_rotated = plotterdaac2d.rotate_covariance(pose_x, cov)
                angles = pose_x.rotation().matrix()
                psi = np.arctan2(angles[1, 0], angles[0, 0])
                gRp = np.array([[np.cos(psi), -np.sin(psi)],
                                [np.sin(psi), np.cos(psi)]])
                multi1 = np.matmul(gRp, cov)
                cov_rotated = np.matmul(multi1, np.transpose(gRp))

                # Plot rotated cov ellipse
                plotterdaac2d.plot_ellipse(ax,
                                           mu,
                                           cov_rotated,
                                           enlarge=enlarge)

                idx += 1

        else:
            idx = 0
            pose_x_collector = list()
            pose_xo_collector = list()
            while self.result.exists(self.X(idx)):

                pose_x_collector.append(self.result.atPose3(self.X(idx)))

                idx += 1

            plotterdaac2d.GT_plotter_line(pose_x_collector, [],
                                          fig_num=0,
                                          ax=ax,
                                          show_plot=False,
                                          plt_save_name=None,
                                          pause_time=None,
                                          red_point=None,
                                          jpg_save=False,
                                          color=color,
                                          alpha=0.7)

        # Plot objects
        # self.result.print()
        for idx in self.object_lambda_dict:

            pose_xo = self.result.atPose3(self.XO(idx))
            pose_xo_cov = self.marginals_after.marginalCovariance(self.XO(idx))
            plotterdaac2d.plotPoint2(fig_num,
                                     pose_xo.translation(),
                                     'go',
                                     ax=ax)

            # Annotate the point
            ax.text(pose_xo.x(), pose_xo.y(), 'O' + str(idx))

            # Plot covariance ellipse
            mu, cov = plotterdaac2d.extract_mu_cov_2d(self.result,
                                                      self.marginals_after,
                                                      self.XO(idx))

            # Rotate covariance matrix
            angles = pose_xo.rotation().matrix()
            psi = np.arctan2(angles[1, 0], angles[0, 0])
            gRp = np.array([[np.cos(psi), -np.sin(psi)],
                            [np.sin(psi), np.cos(psi)]])
            multi1 = np.matmul(gRp, cov)
            cov_rotated = np.matmul(multi1, np.transpose(gRp))

            # Plot rotated cov ellipse
            plotterdaac2d.plot_ellipse(ax, mu, cov_rotated, enlarge=enlarge)

        if display_title is True:
            plt.title('Figure number: ' + str(fig_num) +
                      '. \nData association realization: ' +
                      str(self.daRealization))
        ax.set_xlabel('X axis [m]')
        ax.set_ylabel('Y axis [m]')
        # ax.set_zlabel('Z axis [m]')

        # draw
        plt.tight_layout()
        if show_plot is True:
            plt.show()

        return ax