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()
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