def draw_point_cloud input_points canvasSize 500 space 240 diameter 10

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
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