def draw_cam_frustum(ax, inverse_ext, label_string=None, aspect_ratio=4 / 3.0, scale=40, dotted=False):
"""Draw a traditional camera frustum / rectangular pyramid ting to show where
the camera is."""
# The NaN column is necessary because to plot a bunch of lines that aren't all
# contiguous
a = aspect_ratio
points = np.array([[0, a, a, -a, -a, np.NaN],
[0, 1, -1, -1, 1, np.NaN],
[0, 1, 1, 1, 1, np.NaN]])
points *= scale
# Indices to draw all the lines. Include an index of 5 to get a NaN
# which allows us to start a new line.
indices = [0, 1, 5, 0, 2, 5, 0, 3, 5, 0, 4, 5, 1, 2, 3, 4, 1]
# import pdb; pdb.set_trace()
pts_transformed = tform.apply_transform(inverse_ext, points)
pts_to_plot = np.asarray(pts_transformed[:, indices])
col = "k-" # solid line by default
if dotted:
col = 'k--'
ax.plot(pts_to_plot[0, :], pts_to_plot[1, :], pts_to_plot[2, :], col)
if label_string is not None:
# import pdb; pdb.set_trace()
px = pts_to_plot[0, 0]
py = pts_to_plot[1, 0]
pz = pts_to_plot[2, 0]
# print "plotting", label_string, " at", px, py, pz
ax.text(px, py, pz, label_string)
def draw_axes_spikes(ax, ext, scale=50):
s = scale
pts = np.array([[0, s, 0, 0],
[0, 0, s, 0],
[0, 0, 0, s]])
pts_tformed = tform.apply_transform(ext, pts)
for idx, col in zip([[0, 1], [0, 2], [0, 3]], ['r-', 'g-', 'b-']):
ax.plot(pts_tformed[0, idx], pts_tformed[1, idx], pts_tformed[2, idx], col)
def draw_feature_visible_line(ax, homog_direction, inverse_ext, label_string=None, length=2000):
"""Given some 3D axes a direction for the ray, plus the extrinsics
matrix, plot the ray in 3D"""
coords = np.hstack((np.zeros((3, 1)), length * homog_direction.reshape(3, 1)))
coords = tform.apply_transform(inverse_ext, coords)
if label_string:
ax.plot(coords[0, :], coords[1, :], coords[2, :], 'b', label=label_string)
else:
ax.plot(coords[0, :], coords[1, :], coords[2, :], 'b')
