import matplotlib.pyplot as plt
import numpy as np
debug_text_displays = True
[docs]def axisEqual3D(ax):
"""
In a 3D plot, equalizes all axes to same scale
"""
extents = np.array([getattr(ax, 'get_{}lim'.format(dim))() for dim in 'xyz'])
sz = extents[:, 1] - extents[:, 0]
centers = np.mean(extents, axis=1)
maxsize = max(abs(sz))
r = maxsize / 2
for ctr, dim in zip(centers, 'xyz'):
getattr(ax, 'set_{}lim'.format(dim))(ctr - r, ctr + r)
[docs]def plot_coord_sys(rmat, tvec, ax, scale=10, text=None):
"""
Given a pose, plots the basis of that coordinate system in the given matplotlib axis
In the plot, the x axis is blue, the y axis is orange, and the z axis is green
Parameters
----------
rmat : array_like, shape (3, 3)
Rotation Matrix. Columns represent the basis of the coordinate system
tvec : array_like, , shape (3,)
Origin of the coordinate system
ax : mpl_toolkits.mplot3d.axes3d.Axes3D
matplotlib axis
scale : int or float, optional
Size of the vcoordinate system vectors
text : str, optional
Label of the coordinate system. If ``None`` (default), no text is added.
"""
s_rmat = scale * rmat
# TODO: these should be drawn in the order of distance to the virtual camera
# Drawing them in this order by default can cause the debug outputs to look incorrect
# due to occlusions
ax.plot3D([tvec[0][0], tvec[0][0] + s_rmat[0][0]], [tvec[1][0], tvec[1][0] + s_rmat[1][0]], [tvec[2][0], tvec[2][0] + s_rmat[2][0]], 'b')
ax.plot3D([tvec[0][0], tvec[0][0] + s_rmat[0][1]], [tvec[1][0], tvec[1][0] + s_rmat[1][1]], [tvec[2][0], tvec[2][0] + s_rmat[2][1]], 'orange')
ax.plot3D([tvec[0][0], tvec[0][0] + s_rmat[0][2]], [tvec[1][0], tvec[1][0] + s_rmat[1][2]], [tvec[2][0], tvec[2][0] + s_rmat[2][2]], 'g')
if (text is not None) and debug_text_displays:
ax.text(tvec[0][0], tvec[1][0], tvec[2][0], text)
[docs]def show_image_locations(img, img_locations, fig_name, scale=5, c='w'):
"""
Displays an image with the given image locations indicated with a marker of the
given color (white by default)
"""
plt.figure(fig_name)
plt.imshow(img)
plt.scatter(img_locations[:, 0], img_locations[:, 1], s=scale, c=c)
plt.savefig(str(fig_name) + '.png')
plt.close(fig_name)
[docs]def plot_pts_and_norms(pts_and_norms, ax, scale=5, c='r'):
"""Helper function to plot points with normals
Parameters
----------
pts_and_norms : list of dict
Each dictionary has keys 'tvec' and 'norm' tvec refers to the translation vector
from the origin to the point norm refers to the point's normal vector
ax : mpl_toolkits.mplot3d.axes3d.Axes3D
matplotlib axis
scale : int or float, optional
Size of the normal vectors
c : str, optional
Color of points and normals
"""
for pt_and_norm in pts_and_norms:
# Calculate translation vector relative to camera
tvec = pt_and_norm['tvec'][:, 0]
# Plot point
ax.scatter([tvec[0]], [tvec[1]], [tvec[2]], marker='o', c='r', s=scale*2)
# Calculate scaled normal vector in camera frame
s_norm = scale * pt_and_norm['norm'][:, 0]
# Plot normal vector
ax.plot3D([tvec[0], tvec[0] + s_norm[0]],
[tvec[1], tvec[1] + s_norm[1]],
[tvec[2], tvec[2] + s_norm[2]],
c=c)
[docs]def show_pts_and_norms(rmat, tvec, pts_and_norms, ax=None, c='r', texts=(None, None)):
"""
Helper function to plot transformation from the origin to given coordinate system,
and show a set of points. Originally intended to plot the tgts frame and the targets
with their normals
Parameters
----------
rmat : array_like, shape (3, 3)
Basis of the coordinate system to be shown
tvec : array_like, shape (3,)
Origin of coordinate system to be shown
pts_and_norms : list of dict
Each dictionary has keys 'tvec' and 'norm' tvec refers to the translation vector
from the origin to the point norm refers to the point's normal vector. Points
and normals are in the coordinate frame of the origin
ax : matplotlib.Axes, optional
matplotlib axis. If not provided, a figure and 3D axes are created.
scale : int or float
Size of the normal vectors
c : str, optional
Color of points and normals
texts : tuple, optional
Labels for the origin and transformed coordinate system.
"""
# If no axis was given, make a new one
if ax is None:
fig = plt.figure('Points & Normals')
ax = fig.gca(projection='3d')
# Plot the camera frame, the vector from the camera frame to the tgts frame
# and the tgts frame
plot_coord_sys(np.eye(3), (0, 0, 0), ax, text=texts[0])
ax.plot3D([0, tvec[0]], [0, tvec[1]], [0, tvec[2]], 'black')
plot_coord_sys(rmat, tvec, ax, text=texts[1])
# Plot the targets and their normals
plot_pts_and_norms(pts_and_norms, ax, c=c)
axisEqual3D(ax)
plt.savefig('unfound_visibles.png')
plt.show()
plt.close()
[docs]def show_projection_matching(img, proj_pts, matching_points, num_matches=None,
name='', bonus_pt=None, scale=10., ax=None):
"""Show the projected target matching.
Projected points are labeled in red. Matching points are labeled in white. Line
connecting point to match is drawn in black
Parameters
----------
img : array_like
Display image
proj_pts : array_like, shape (n, 2)
:rojected locations of 3D points
matching_points : array_like, shape (n, 2)
Image location of point
name : str, optinoal
Figure name prefix
bonus_pt : array_like, optional
Optional input. Point to be shown in blue
scale : int or float, optional
Scale of point labels
"""
if num_matches is None:
num_matches = len(proj_pts)
plt.figure(name + '_Matched Points')
plt.imshow(img)
plt.scatter(proj_pts[:, 0], proj_pts[:, 1], s=scale, c='r')
plt.scatter(matching_points[:, 0], matching_points[:, 1], s=scale/4, c='w')
for i, data in enumerate(zip(proj_pts, matching_points)):
if i >= num_matches:
break
proj, match = data
plt.plot([proj[0], match[0]], [proj[1], match[1]], color='black')
if bonus_pt is not None:
plt.scatter([bonus_pt[0]], [bonus_pt[1]], s=scale, c='b')
plt.savefig(name + '_Matched_Points.png', dpi=400)
plt.close()