def euler_to_rot(psi, theta, phi):
M1 = eu.euler2mat(phi)
M2 = eu.euler2mat(0, theta)
M3 = eu.euler2mat(psi)
return np.dot(M3, np.dot(M2, M1))
def draw_point_cloud(input_points, canvasSize=500, space=200, diameter=15,
xrot=0, yrot=0, zrot=0, switch_xyz=[0,1,2], normalize=True):
""" 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
"""
image = np.zeros((canvasSize, canvasSize))
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))
disk[i, j] = 1
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 = canvasSize/2 + (x*space)
yc = canvasSize/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 = image / np.max(image)
return image
def draw_point_cloud(input_points, canvasSize=500, space=200, diameter=25,
xrot=0, yrot=0, zrot=0, switch_xyz=[0,1,2], normalize=True):
""" 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
"""
image = np.zeros((canvasSize, canvasSize))
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 = canvasSize/2 + (x*space)
yc = canvasSize/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 = image / np.max(image)
return image
def draw_point_cloud_topdown(input_points, canvasSize=800, radius=1,
zrot=0, switch_xyz=[0,1,2],
xlim=(0.0,70.0), ylim=(-50.0,50.0), zlim=(-5.0,10.0), background_color=(255,255,255)):
""" Render point cloud to image with alpha channel.
Input:
points: Nx3 numpy array (+z is up direction)
Output:
colorized image as numpy array of size canvasSizexcanvasSize
"""
# create mask
input_points_mask = np.logical_and.reduce(((input_points[:,0] > xlim[0]), (input_points[:,0] < xlim[1]), \
(input_points[:,1] > ylim[0]), (input_points[:,1] < ylim[1]), \
(input_points[:,2] > zlim[0]), (input_points[:,2] < zlim[1])))
# filter out
input_points = input_points[input_points_mask]
# hue range
huerange = (240, 0) # green to red
# image plane
image = np.zeros((canvasSize, canvasSize, 3), dtype=np.uint8)
if input_points is None or input_points.shape[0] == 0:
return image
# x in point-cloud to image y coordinate
def pcx_to_imgy(pcx):
m2px = (xlim[1]-xlim[0]) / float(canvasSize)
imgy = canvasSize - int(pcx / m2px)
return imgy
# y in point-cloud to image x coordinate
def pcy_to_imgx(pcy):
m2px = (ylim[1]-ylim[0]) / float(canvasSize)
imgx = int((float(canvasSize) / 2.0) - (pcy / m2px))
return imgx
points = input_points
M = euler2mat(zrot, 0, 0)
points = (np.dot(M, points.transpose())).transpose()
# go through each point
for pt in points:
imgx = pcy_to_imgx(pt[1])
imgy = pcx_to_imgy(pt[0])
# draw circle
ztohue = (zlim[1] - zlim[0]) / (huerange[0] - huerange[1])
hue = huerange[0] - int((pt[2] - zlim[0]) / ztohue)
cv2.circle(image, (imgx, imgy), radius=radius, color=(hue,255,128), thickness=-1)
# convert to BGR
image = cv2.cvtColor(image, cv2.COLOR_HSV2BGR)
image[np.where(np.all(image == [0,0,0], axis=-1))] = background_color
# scale between 0.0 and 1.0
image = image.astype(np.float32) / 255.0
return image
def convert_rel_to_44matrix(rot_x, rot_y, rot_z, pose):
R_pred = euler2mat(rot_x, rot_y, rot_z)
rotated_pose = np.dot(R_pred, pose[0:3])
DEGREE_2_RADIUS = np.pi / 180.0
pred_quat = euler2quat(z=pose[5] * DEGREE_2_RADIUS,
y=pose[4] * DEGREE_2_RADIUS,
x=pose[3] * DEGREE_2_RADIUS)
pred_transform_t = transform44([
0, rotated_pose[0], rotated_pose[1], rotated_pose[2], pred_quat[1],
pred_quat[2], pred_quat[3], pred_quat[0]
])
return pred_transform_t
def train_one_epoch(sess, ops, gmm, train_writer, trainset_loader, epoch_num):
is_training = True
train_enum = enumerate(trainset_loader, 0)
train_num_batchs = len(trainset_loader)
total_seen = 0
loss_sum = 0
for batch_idx, data in train_enum:
current_data = data[0]
target = tuple(t.data.numpy() for t in data[1:-1])
current_normals = target[0]
n_effective_points = np.squeeze(data[-1])
if INSERT_ROTATIONS:
angles = 2 * np.pi * np.random.randn(3)
R = np.transpose(
eulerangles.euler2mat(z=angles[0], y=angles[1], x=angles[2]))
rotated_data = np.zeros(current_data.shape, dtype=np.float32)
rotated_normals = np.zeros(current_normals.shape, dtype=np.float32)
for k in xrange(current_data.shape[0]):
shape_pc = current_data[k, ...]
normal_pc = current_normals[k, ...]
rotated_data[k, ...] = np.dot(shape_pc.reshape((-1, 3)), R)
rotated_normals[k, ...] = np.dot(normal_pc, R)
current_data = rotated_data
current_normals = rotated_normals
feed_dict = {
ops['points_pl']: current_data,
ops['normal_gt_pl']: current_normals,
ops['n_effective_points']: n_effective_points,
ops['w_pl']: gmm.weights_,
ops['mu_pl']: gmm.means_,
ops['sigma_pl']: np.sqrt(gmm.covariances_),
ops['is_training_pl']: is_training,
}
summary, step, _, loss_val = sess.run(
[ops['merged'], ops['step'], ops['train_op'], ops['loss']],
feed_dict=feed_dict)
train_writer.add_summary(summary, step)
total_seen += BATCH_SIZE
loss_sum += loss_val
print('epoch %d, [%d/%d] %s loss: %f' %
(epoch_num, batch_idx, train_num_batchs - 1, green('train'),
loss_val))
log_string('mean loss: %f' % (loss_sum / float(train_num_batchs)))
def transform3D2D(M, rt):
fx = 1000.
fy = 1000.
n = len(M)
Rmat = EL.euler2mat(rt[0], rt[1], rt[2])
MR = M.dot(Rmat)
ts = np.tile(rt[3::], (n, 1))
p3d = MR + ts
px = p3d[:,
0] / p3d[:,
2] #it is normalized by the fx and fy as described in page 12.
py = p3d[:, 1] / p3d[:, 2]
p2d = np.vstack((px, py)).reshape((len(px) * 2, 1), order='F')
p2d += np.random.normal(0, 4.0, len(p2d)).reshape(
(len(p2d), 1)) / fx #add \sigma=4 white noise
return p2d
def test_transform():
# Test Parameter
x, y, z, a, b, g = 1, 0, 0, math.pi / 2, 0, 0
print(x, y, z, a, b, g)
# Construct Original V
V = np.array([1, 0, 0])
# V = np.array([1, 0, 0]).reshape(3,1)
# Test func Transform
rotz, roty, rotx = math.pi / 2, 0, 0
Mr = eu.euler2mat(rotz, roty, rotx)
Mt = [0, 0, 0]
print(transform(V, Mt, Mr))
# Test func Transform6para
print(transform6para(V, 0, 0, 0, -math.pi / 2))
def transform6para(V, transx = 0, transy = 0, transz = 0, rotz = 0, roty = 0, rotx = 0): Mt = [transx, transy, transz] Mr = eu.euler2mat(rotz, roty, rotx) return np.dot(Mr, V + Mt)
def draw_point_cloud(input_points, quality, canvasSize=500, space=200, diameter=3, add_diameter=5,
xrot=0, yrot=0, zrot=0, switch_xyz=[0, 1, 2], normalize=True, color=True, percentage=1.0):
""" Render point cloud to image with alpha channel.
Input:
points: Nx3 numpy array (+y is up direction)
quality: (N,) numpy array (layer output)
Output:
gray image as numpy array of size canvasSizexcanvasSize
"""
image = np.ones((canvasSize, canvasSize, 3))
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
# 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])
#
# ### change quality to RGB value
# quality = quality[zorder]
if np.max(quality[:]) > np.min(quality[:]):
quality[:] = (quality[:] - np.min(quality[:])) / (np.max(quality[:] - np.min(quality[:])))
else:
color = False # all the point qualities are equal, no need to colorize
# if np.max(quality) == np.min(quality):
# color = False
# if np.max(quality) != 0:
# quality = quality / np.max(quality)
# else:
# color = False # all the point qualities are equal, no need to colorize
intensity = np.zeros((input_points.shape[0], 3))
leftNum = int((quality.shape[0]) * percentage)
quality_sorted = sorted(quality, reverse=True)
thresholdVal = quality_sorted[leftNum-1]
for i in range(quality.shape[0]):
if color == True: # render point color according to quality value
if quality[i] < thresholdVal:
quality[i] = 2.0 # black color
if quality[i] >= thresholdVal and thresholdVal < 1.0:
quality[i] = ((quality[i] - thresholdVal) / (1.0 - thresholdVal))
if quality[i] >= thresholdVal and thresholdVal == 1.0:
quality[i] = 1.0
else:
quality[i] = 2.0 # do not render point color
# 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]
## render important points with larger radius
diameter_large = diameter + add_diameter
radius_large = (diameter_large - 1) / 2.0
disk_large = np.zeros((diameter_large, diameter_large))
for i in range(diameter_large):
for j in range(diameter_large):
if (i - radius_large) * (i - radius_large) + (j - radius_large) * (
j - radius_large) <= radius_large * radius_large:
disk_large[i, j] = np.exp((-(i - radius_large) ** 2 - (j - radius_large) ** 2) / (radius_large ** 2))
mask_large = np.argwhere(disk_large > 0)
dx_large = mask_large[:, 0]
dy_large = mask_large[:, 1]
dv_large = disk_large[disk_large > 0]
for i in range(points.shape[0]):
j = points.shape[0] - i - 1
x = points[j, 0]
y = points[j, 1]
xc = canvasSize / 2 + (x * space)
yc = canvasSize / 2 + (y * space)
xc = int(np.round(xc))
yc = int(np.round(yc))
# transferred value = [(y2-y1)/(x2-x1)]*x+(x2*y1-x1*y2)/(x2-x1)
if quality[j] < 0.125:
intensity[j, 2] = ((1.0 - 0.5) / (0.125 - 0.0)) * quality[j] + (0.125 * 0.5 - 0.0 * 1.0) / (0.125 - 0.0)
elif quality[j] < 0.375 and quality[j] >= 0.125:
intensity[j, 2] = 1.0
intensity[j, 1] = ((1.0 - 0.0) / (0.375 - 0.125)) * quality[j] + (0.375 * 0.0 - 0.125 * 1.0) / (0.375 - 0.125)
elif quality[j] >= 0.375 and quality[j] < 0.625:
intensity[j, 2] = ((0.0 - 1.0) / (0.625 - 0.375)) * quality[j] + (0.625 * 1.0 - 0.375 * 0.0) / (0.625 - 0.375)
intensity[j, 1] = 1.0
intensity[j, 0] = ((1.0 - 0.0) / (0.625 - 0.375)) * quality[j] + (0.625 * 0.0 - 0.375 * 1.0) / (0.625 - 0.375)
elif quality[j] >= 0.625 and quality[j] < 0.875:
intensity[j, 1] = ((0.0 - 1.0) / (0.875 - 0.625)) * quality[j] + (0.875 * 1.0 - 0.625 * 0.0) / (0.875 - 0.625)
intensity[j, 0] = 1.0
elif quality[j] >= 0.875 and quality[j] <= 1.0:
intensity[j, 0] = ((0.5 - 1.0) / (1.0 - 0.875)) * quality[j] + (1.0 * 1.0 - 0.875 * 0.5) / (1.0 - 0.875)
if quality[j] >= 0.625 and quality[j] <= 1.0:
px = dx_large + xc
py = dy_large + yc
dv_large[:] = intensity[j, 0]
image[px, py, 0] = dv_large
dv_large[:] = intensity[j, 1]
image[px, py, 1] = dv_large
dv_large[:] = intensity[j, 2]
image[px, py, 2] = dv_large
else:
px = dx + xc
py = dy + yc
dv[:] = intensity[j, 0]
image[px, py, 0] = dv
dv[:] = intensity[j, 1]
image[px, py, 1] = dv
dv[:] = intensity[j, 2]
image[px, py, 2] = dv
# image = image / np.max(image)
# 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[mask] = 1.0
return image
def set_euler(self, _euler):
x, y, z = _euler
self.rotation = euler.euler2mat(z, x, y)
def draw_point_cloud_w_bbox_id(input_points, label_dict_list, canvasSize=800, radius=1,
xlim=(0.0, 70.0), ylim=(-50.0,50.0), zlim=(-5.0,10.0), background_color=(255,255,255), text_color=(0,0,0)):
""" Render point cloud to image and draw bounding box.
Input:
input_points: Nx3 numpy array (+z is up direction)
Output:
colorized image as numpy array of size canvasSizexcanvasSize
"""
# create mask
input_points_mask = np.logical_and.reduce(((input_points[:,0] > xlim[0]), (input_points[:,0] < xlim[1]), \
(input_points[:,1] > ylim[0]), (input_points[:,1] < ylim[1]), \
(input_points[:,2] > zlim[0]), (input_points[:,2] < zlim[1])))
# filter out
input_points = input_points[input_points_mask]
# hue range
huerange = (240, 0) # green to red
# image plane
image = np.zeros((canvasSize, canvasSize, 3), dtype=np.uint8)
if input_points is None or input_points.shape[0] == 0:
return image
# x in point-cloud to image y coordinate
def pcx_to_imgy(pcx):
m2px = (xlim[1]-xlim[0]) / float(canvasSize)
imgy = canvasSize - int(pcx / m2px)
return imgy
# y in point-cloud to image x coordinate
def pcy_to_imgx(pcy):
m2px = (ylim[1]-ylim[0]) / float(canvasSize)
imgx = int((float(canvasSize) / 2.0) - (pcy / m2px))
return imgx
# go through each point
for pt in input_points:
imgx = pcy_to_imgx(pt[1])
imgy = pcx_to_imgy(pt[0])
# draw circle
ztohue = (zlim[1] - zlim[0]) / (huerange[0] - huerange[1])
hue = huerange[0] - int((pt[2] - zlim[0]) / ztohue)
cv2.circle(image, (imgx, imgy), radius=radius, color=(hue,255,255), thickness=-1)
# convert to BGR
image = cv2.cvtColor(image, cv2.COLOR_HSV2BGR)
image[np.where(np.all(image == [0,0,0], axis=-1))] = background_color
# draw bounding boxes
for label_dict in label_dict_list:
x,y,z,l,w,h,yaw = label_dict['x'],label_dict['y'],label_dict['z'],label_dict['l'],label_dict['w'],label_dict['h'],label_dict['yaw']
yaw = np.pi/2.0 + yaw
# compute corners in birds-eye view
corners = []
corners.append([ l/2, -w/2, 0.0])
corners.append([ l/2, w/2, 0.0])
corners.append([-l/2, w/2, 0.0])
corners.append([-l/2, -w/2, 0.0])
corners = np.asarray(corners, dtype=np.float32)
# rotate input_points
M = euler2mat(yaw, 0, 0)
corners = (np.dot(M, corners.transpose())).transpose()
corners = corners + [x, y, z]
# corners in pixel
corners_px = []
for corner in corners:
corners_px.append([pcy_to_imgx(corner[1]), pcx_to_imgy(corner[0])])
corners_px = np.asarray(corners_px, dtype=np.int)
# draw bounding box
cv2.line(image, (corners_px[0,0], corners_px[0,1]), (corners_px[1,0], corners_px[1,1]), color=(0,0,0), thickness=6)
cv2.line(image, (corners_px[0,0], corners_px[0,1]), (corners_px[1,0], corners_px[1,1]), color=(255,0,0), thickness=2)
cv2.line(image, (corners_px[1,0], corners_px[1,1]), (corners_px[2,0], corners_px[2,1]), color=(255,0,0), thickness=2)
cv2.line(image, (corners_px[2,0], corners_px[2,1]), (corners_px[3,0], corners_px[3,1]), color=(255,0,0), thickness=2)
cv2.line(image, (corners_px[3,0], corners_px[3,1]), (corners_px[0,0], corners_px[0,1]), color=(255,0,0), thickness=2)
# get top-left coordinates
tl = (np.min(corners_px[:,0]), np.min(corners_px[:,1]))
# write object id and class
cv2.putText(image, '{} {:.1f}% | id {}'.format(label_dict['class'], label_dict['conf']*100.0, label_dict['id']),
(tl[0],tl[1]-5),
cv2.FONT_HERSHEY_SIMPLEX,
0.6,
color=text_color,
thickness=2,
lineType=2)
# scale between 0.0 and 1.0
image = image.astype(np.float32) / 255.0
return image
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):
""" 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 = np.round(xc)
yc = np.round(yc)
if np.isnan(xc):
xc = 0
else:
xc = int(xc)
if np.isnan(yc):
yc = 0
else:
yc = int(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
