def draw_point_cloud(input_points, canvasSize=500, space=240, diameter=10,
xrot=0, yrot=0, zrot=0, switch_xyz=[0, 1, 2], normalize=True, n_points_red=None):
""" Render point cloud to image with alpha channel.
Input:
points: Nx3 numpy array (+y is up direction)
Output:
gray image as numpy array of size canvasSizexcanvasSize
"""
canvasSizeX = canvasSize
canvasSizeY = canvasSize
image = np.zeros((canvasSizeX, canvasSizeY))
if input_points is None or input_points.shape[0] == 0:
return image
points = input_points[:, switch_xyz]
M = euler2mat(zrot, yrot, xrot)
points = (np.dot(M, points.transpose())).transpose()
# Normalize the point cloud
# We normalize scale to fit points in a unit sphere
if normalize:
centroid = np.mean(points, axis=0)
points -= centroid
furthest_distance = np.max(np.sqrt(np.sum(abs(points) ** 2, axis=-1)))
points /= furthest_distance
# Pre-compute the Gaussian disk
radius = (diameter - 1) / 2.0
disk = np.zeros((diameter, diameter))
for i in range(diameter):
for j in range(diameter):
if (i - radius) * (i - radius) + (j - radius) * (j - radius) <= radius * radius:
disk[i, j] = np.exp((-(i - radius) ** 2 - (j - radius) ** 2) / (radius ** 2))
mask = np.argwhere(disk > 0)
dx = mask[:, 0]
dy = mask[:, 1]
dv = disk[disk > 0]
# Order points by z-buffer
zorder = np.argsort(points[:, 2])
points = points[zorder, :]
points[:, 2] = (points[:, 2] - np.min(points[:, 2])) / (np.max(points[:, 2] - np.min(points[:, 2])))
max_depth = np.max(points[:, 2])
for i in range(points.shape[0]):
j = points.shape[0] - i - 1
x = points[j, 0]
y = points[j, 1]
xc = canvasSizeX / 2 + (x * space)
yc = canvasSizeY / 2 + (y * space)
xc = int(np.round(xc))
yc = int(np.round(yc))
px = dx + xc
py = dy + yc
# image[px, py] = image[px, py] * 0.7 + dv * (max_depth - points[j, 2]) * 0.3
image[px, py] = image[px, py] * 0.7 + dv * 0.3
val = np.max(image)
val = np.percentile(image, 99.9)
image = image / val
mask = image == 0
image[image > 1.0] = 1.0
image = 1.0 - image
# image = np.expand_dims(image, axis=-1)
# image = np.concatenate((image*0.3+0.7,np.ones_like(image), np.ones_like(image)), axis=2)
# image = colors.hsv_to_rgb(image)
image[mask] = 1.0
return image
# def plot_point_cloud(vis, points):
# visdom
def point_cloud_three_views(points, diameter=5, n_points_red=None):
""" input points Nx3 numpy array (+y is up direction).
return an numpy array gray image of size 500x1500. """
# +y is up direction
# xrot is azimuth
# yrot is in-plane
# zrot is elevation
# img1 = draw_point_cloud(points, xrot=90/180.0*np.pi, yrot=0/180.0*np.pi, zrot=0/180.0*np.pi,diameter=diameter)
# img2 = draw_point_cloud(points, xrot=180/180.0*np.pi, yrot=0/180.0*np.pi, zrot=0/180.0*np.pi,diameter=diameter)
# img3 = draw_point_cloud(points, xrot=0/180.0*np.pi, yrot=-90/180.0*np.pi, zrot=0/180.0*np.pi,diameter=diameter)
# image_large = np.concatenate([img1, img2, img3], 1)
img1 = draw_point_cloud(points, zrot=110 / 180.0 * np.pi, xrot=135 / 180.0 * np.pi, yrot=0 / 180.0 * np.pi,
diameter=diameter, n_points_red=n_points_red)
img2 = draw_point_cloud(points, zrot=70 / 180.0 * np.pi, xrot=135 / 180.0 * np.pi, yrot=0 / 180.0 * np.pi,
diameter=diameter, n_points_red=n_points_red)
img3 = draw_point_cloud(points, zrot=180.0 / 180.0 * np.pi, xrot=90 / 180.0 * np.pi, yrot=0 / 180.0 * np.pi,
diameter=diameter, n_points_red=n_points_red)
image_large = np.concatenate([img1, img2, img3], 1)
return image_large
def plot_3d_point_cloud(x, y, z, show_axis=False, in_u_sphere=False, marker='.', s=8, alpha=.8,
figsize=(15, 15), elev=10, azim=240, axis=None, title=None, *args, **kwargs):
if axis is None:
fig = plt.figure(figsize=figsize)
ax = fig.add_subplot(111, projection='3d')
else:
ax = axis
fig = axis
if title is not None:
plt.title(title)
sc = ax.scatter(x, y, z, marker=marker, s=s, alpha=alpha, *args, **kwargs)
ax.view_init(elev=elev, azim=azim)
if in_u_sphere:
ax.set_xlim3d(-0.5, 0.5)
ax.set_ylim3d(-0.5, 0.5)
ax.set_zlim3d(-0.5, 0.5)
else:
miv = 0.7 * np.min([np.min(x), np.min(y), np.min(z)]) # Multiply with 0.7 to squeeze free-space.
mav = 0.7 * np.max([np.max(x), np.max(y), np.max(z)])
ax.set_xlim(miv, mav)
ax.set_ylim(miv, mav)
ax.set_zlim(miv, mav)
plt.tight_layout()
if not show_axis:
plt.axis('off')
if 'c' in kwargs:
plt.colorbar(sc)
return fig