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