def get_point_cloud(self, source, img_idx, image_shape=None): """ Gets the points from the point cloud for a particular image, keeping only the points within the area extents, and takes a slice between self._ground_filter_offset and self._offset_distance above the ground plane Args: source: point cloud source, e.g. 'lidar' img_idx: An integer sample image index, e.g. 123 or 500 image_shape: image dimensions (h, w), only required when source is 'lidar' or 'depth' Returns: The set of points in the shape (N, 3) """ if source == 'lidar': # wavedata wants im_size in (w, h) order im_size = [image_shape[1], image_shape[0]] point_cloud = obj_utils.get_lidar_point_cloud( img_idx, self.dataset.calib_dir, self.dataset.velo_dir, im_size=im_size) else: raise ValueError("Invalid source {}".format(source)) return point_cloud
def get_nan_point_cloud(perspect_dir, idx): calib_dir = perspect_dir + '/calib' velo_dir = perspect_dir + '/velodyne' all_points = obj_utils.get_lidar_point_cloud(idx, calib_dir, velo_dir) # Remove nan points nan_mask = ~np.any(np.isnan(all_points), axis=1) point_cloud = all_points[nan_mask].T return point_cloud
def load_samples(self, indices): sample_dicts = [] for sample_idx in indices: sample = self.sample_list[sample_idx] sample_name = sample.name if self.has_labels: obj_labels = obj_utils.read_labels(self.label_dir, int(sample_name)) label_classes, label_boxes_3d, label_boxes_2d = self.parse_obj_labels( obj_labels, self.label_map) else: obj_labels = None label_classes = np.zeros(1) label_boxes_2d = np.zeros((1, 4)) label_boxes_3d = np.zeros((1, 7)) # image cv_bgr_image = cv2.imread(self.get_rbg_image_path( int(sample_name))) rgb_image = cv_bgr_image[..., ::-1] im_shape = rgb_image.shape[0:2] image_input = rgb_image # calibration stereo_calib_p2 = calib_utils.read_calibration( self.calib_dir, int(sample_name)).p2 # point cloud # just project point to camera frame and then keep point in front of image point_cloud = obj_utils.get_lidar_point_cloud(int(sample_name), self.calib_dir, self.velo_dir, im_size=im_shape) ################################# # Data Augmentation ################################# if kitti_aug.AUG_FLIPPING in sample.augs: pass if kitti_aug.AUG_PCA_JITTER in sample.augs: pass sample_dict = { constants.KEY_IMAGE_INPUT: image_input, constants.KEY_POINT_CLOUD: point_cloud, constants.KEY_LABEL_CLASSES: label_classes, constants.KEY_LABEL_BOXES_2D: label_boxes_2d, constants.KEY_LABEL_BOXES_3D: label_boxes_3d, constants.KEY_STEREO_CALIB_P2: stereo_calib_p2 } sample_dicts.append(sample_dict) return sample_dicts
def estimate_ground_planes(base_dir, dataset_config, plane_method=0, specific_idx=-1): velo_dir = base_dir + 'velodyne' plane_dir = base_dir + 'planes' if plane_method == 1: plane_dir = plane_dir + '_ransac' ground_points_dir = base_dir + 'ground_points' grid_points_dir = base_dir + 'ground_points_grid' calib_dir = base_dir + 'calib' files = os.listdir(velo_dir) num_files = len(files) file_idx = 0 #For checking ground planes dataset = DatasetBuilder.build_kitti_dataset(dataset_config, use_defaults=False) kitti_utils = dataset.kitti_utils if not os.path.exists(plane_dir): os.makedirs(plane_dir) #Estimate each idx for file in files: filepath = velo_dir + '/' + file idx = int(os.path.splitext(file)[0]) if idx < cfg.MIN_IDX or idx > cfg.MAX_IDX: continue if specific_idx != -1: idx = specific_idx planes_file = plane_dir + '/%06d.txt' % idx logging.debug("Index: %d", idx) lidar_point_cloud = obj_utils.get_lidar_point_cloud( idx, calib_dir, velo_dir) # Reshape points into N x [x, y, z] point_cloud = np.array(lidar_point_cloud).transpose().reshape( (-1, 3)).T ground_points_failed = False if plane_method == use_ground_points: s = loadGroundPointsFromFile(idx, ground_points_dir, grid_points_dir) if s.shape[0] < 4: logging.debug("Not enough points at idx: %d", idx) ground_points_failed = True else: m = estimate(s) a, b, c, d = m plane = loadKittiPlane(m) #ground_points_failed = checkBadSlices(point_cloud, plane, kitti_utils) ransac_failed = False if plane_method == ransac or ground_points_failed: logging.debug("PC shape: {}".format(point_cloud.shape)) points = point_cloud.T all_points_near = points[(points[:, 0] > -3.0) & (points[:, 0] < 3.0) & (points[:, 1] > -3.0) & (points[:, 1] < 0.0) & (points[:, 2] < 20.0) & (points[:, 2] > 2.0)] n = all_points_near.shape[0] logging.debug("Number of points near: %d", n) if n < 3: ransac_failed = True else: max_iterations = 100 goal_inliers = n * 0.5 m, b = run_ransac(all_points_near, lambda x, y: is_inlier(x, y, 0.2), 3, goal_inliers, max_iterations) a, b, c, d = m if plane_method == moose or ransac_failed: plane_coeffs = estimate_plane_coeffs(point_cloud.T) with open(planes_file, 'w+') as f: f.write('# Plane\nWidth 4\nHeight 1\n') if (plane_method == ransac and not ransac_failed) or \ (plane_method == use_ground_points and not ground_points_failed): coeff_string = '%.6e %.6e %.6e %.6e' % (a, b, c, d) else: coeff_string = '%.6e %.6e %.6e %.6e' % ( plane_coeffs[0], plane_coeffs[1], plane_coeffs[2], plane_coeffs[3]) f.write(coeff_string) sys.stdout.write("\rGenerating plane {} / {}".format( file_idx + 1, num_files)) sys.stdout.flush() file_idx = file_idx + 1 if testing and idx == 2 or specific_idx != -1: quit()
def is_plausible(obj, idx, detector_id, det_idx): # Can only deny plausibility in the fov of the ego-vehicle sensors # Filter objects to be in the sensor range, return True if out of range/fov filtered_obj = p_utils.filter_labels([obj]) if len(filtered_obj) == 0: return True # Check if object is more than MAX_LIDAR_DIST away # If yes then it is plausible since our sensors don't go that far obj_pos = np.asanyarray(obj.t) obj_dist = np.sqrt(np.dot(obj_pos, obj_pos.T)) if obj_dist >= cfg.MAX_LIDAR_DIST: return True # If it is truncated by more than half, return True if np.abs(obj.t[2]) < np.abs(obj.t[0]): return True # If the center of the object is above our center, return True (only 2 degrees up) if obj.t[1] - (obj.h / 2) < 0: return True # TODO - Do we want to check this or is it better to only check frustum? # This may allow untrustworthy entities to put ground points in box # Check if points in 3D box first, if yes it is plausible # points_dict = points_3d.load_points_in_3d_boxes(idx, const.ego_id()) # points_in_box = points_dict[detector_id, det_idx] # if points_in_box > 0: # return True # Load point cloud velo_dir = cfg.DATASET_DIR + '/velodyne' calib_dir = cfg.DATASET_DIR + '/calib' pc = obj_utils.get_lidar_point_cloud(idx, calib_dir, velo_dir) # Vis regular PC # vis_utils.vis_pc(pc, [obj]) # Obtain unit vector to object unit_vec_forward = obj_pos / np.linalg.norm(obj_pos) # The size of the frustum square at the object's distance # We want to ensure we are within the object's bounding box half_size = min(obj.l / 4.0, obj.w / 4.0) # camera position is origin cam_pos = np.array([0, 0, 0]) # Calculate the up and right unit vectors for the frustum y_up = (obj_pos[0]**2 + obj_pos[2]**2) / obj_pos[1] vec_down = np.array([obj_pos[0], y_up, obj_pos[2]]) unit_vec_down = vec_down / np.linalg.norm(vec_down) unit_vec_right = np.cross(unit_vec_down, unit_vec_forward) # KITTI labels are at bottom center of 3D bounding box obj_center = obj_pos - np.array([0, obj.h / 2, 0]) # Obtain 2D box for computing frustum boundary planes top_left = -(half_size * unit_vec_down) - (half_size * unit_vec_right) + obj_center top_right = -(half_size * unit_vec_down) + (half_size * unit_vec_right) + obj_center bot_left = (half_size * unit_vec_down) - (half_size * unit_vec_right) + obj_center bot_right = (half_size * unit_vec_down) + (half_size * unit_vec_right) + obj_center # Compute the 4 boundary planes plane_top = get_plane_params(cam_pos, top_left, top_right) plane_bot = get_plane_params(cam_pos, bot_left, bot_right) plane_right = get_plane_params(cam_pos, bot_right, top_right) plane_left = get_plane_params(cam_pos, bot_left, top_left) # Extend pc with ones for dot product with planes shape = pc.shape pc = pc.T new_pc = np.ones((pc.shape[0], pc.shape[1] + 1)) new_pc[:, :-1] = pc pc = new_pc # Filter the points for each plane pc = pc[np.dot(pc, plane_top) < 0] pc = pc[np.dot(pc, plane_bot) > 0] pc = pc[np.dot(pc, plane_right) > 0] pc = pc[np.dot(pc, plane_left) < 0] # If there are no points left then the object is not plausible # Otherwise LiDAR should've hit something on or before the object if pc.shape[0] == 0: return False # Remove extended column pc = pc[:, 0:3] # To visualize the resulting points # vis_utils.vis_pc(pc.T, [obj]) # Save the current number of points before distance based culling num_points = pc.shape[0] # Compute if remaining points are closer or further than the object centre point_distances = np.sqrt(np.sum(np.multiply(pc, pc), axis=1)) # Added in a safety factor of 0.25 (sometimes pedestrians are positioned poorly) pc_closer = pc[point_distances < (obj_dist + 0.25)] num_closer_points = pc_closer.shape[0] # To visualize the resulting points with frustum lines # print(pc_closer.shape) # if pc_closer.shape[0] > 0: # vis_utils.vis_pc(pc_closer.T, [obj], frustum_points=[top_left, top_right, bot_right, bot_left]) # Return True if > 10% of points are closer if num_closer_points / float(num_points) > 0.1: return True return False
def pad_sample(self, sample): sample = super().pad_sample(sample) image_path = sample[constants.KEY_IMAGE_PATH] sample_name = self.get_sample_name_from_path(image_path) pc = obj_utils.get_lidar_point_cloud(int(sample_name), self.calib_dir, self.velo_dir, im_size=None, min_intensity=None) p2 = sample[constants.KEY_STEREO_CALIB_P2] h, w = sample[constants.KEY_IMAGE_INFO].astype(np.int)[:2] label_boxes_3d = sample[constants.KEY_LABEL_BOXES_3D] num_instances = sample[constants.KEY_NUM_INSTANCES] depth = np.zeros((h, w), dtype=np.float32) # 0 refers to bg mask = np.zeros((h, w), dtype=np.int64) for i in range(num_instances): label_box_3d = label_boxes_3d[i] instance_pc = pointcloud_utils.box_3d_filter(label_box_3d, pc[:3]) # draw depth instance_depth = pointcloud_utils.cam_pts_to_rect_depth( instance_pc[:3, :], K=p2[:3, :3], h=h, w=w) mask[instance_depth > 0] = i + 1 depth = depth + instance_depth # import matplotlib.pyplot as plt # plt.imshow(mask) # plt.show() sample[constants.KEY_LABEL_DEPTHMAP] = depth[None] sample[constants.KEY_LABEL_INSTANCES_MASK] = mask[None] # boxes_3d = sample[constants.KEY_LABEL_BOXES_3D] # boxes_2d_proj = sample[constants.KEY_LABEL_BOXES_2D] # p2 = sample[constants.KEY_STEREO_CALIB_P2] # cls_orients = [] # reg_orients = [] # dims = np.stack( # [boxes_3d[:, 4], boxes_3d[:, 5], boxes_3d[:, 3]], axis=-1) # for i in range(boxes_3d.shape[0]): # target = {} # target['ry'] = boxes_3d[i, -1] # target['dimension'] = dims[i] # target['location'] = boxes_3d[i, :3] # corners_xy, points_3d = compute_box_3d(target, p2, True) # # some labels for estimating orientation # left_side_points_2d = corners_xy[[0, 3]] # right_side_points_2d = corners_xy[[1, 2]] # left_side_points_3d = points_3d.T[[0, 3]] # right_side_points_3d = points_3d.T[[1, 2]] # # which one is visible # mid_left_points_3d = left_side_points_3d.mean(axis=0) # mid_right_points_3d = right_side_points_3d.mean(axis=0) # # K*T # KT = p2[:, -1] # K = p2[:3, :3] # T = np.dot(np.linalg.inv(K), KT) # C = -T # mid_left_dist = np.linalg.norm((C - mid_left_points_3d)) # mid_right_dist = np.linalg.norm((C - mid_right_points_3d)) # if mid_left_dist > mid_right_dist: # visible_side = right_side_points_2d # else: # visible_side = left_side_points_2d # cls_orient, reg_orient = truncate_box(boxes_2d_proj[i], # visible_side) # cls_orient = modify_cls_orient(cls_orient, left_side_points_2d, # right_side_points_2d) # cls_orients.append(cls_orient) # reg_orients.append(reg_orient) # sample['cls_orient'] = np.stack(cls_orients, axis=0).astype(np.int32) # sample['reg_orient'] = np.stack(reg_orients, axis=0).astype(np.float32) sample[constants.KEY_LABEL_CLASSES] = torch.from_numpy( sample[constants.KEY_LABEL_CLASSES]).long() return sample
def main(): """ Visualization of anchor filtering using 3D integral images """ anchor_colour_scheme = { "Car": (0, 255, 0), # Green "Pedestrian": (255, 150, 50), # Orange "Cyclist": (150, 50, 100), # Purple "DontCare": (255, 0, 0), # Red "Anchor": (0, 0, 255), # Blue } # Create Dataset dataset = DatasetBuilder.build_kitti_dataset(DatasetBuilder.KITTI_TRAINVAL) # Options clusters, _ = dataset.get_cluster_info() sample_name = "000000" img_idx = int(sample_name) anchor_stride = [0.5, 0.5] ground_plane = obj_utils.get_road_plane(img_idx, dataset.planes_dir) anchor_3d_generator = grid_anchor_3d_generator.GridAnchor3dGenerator( anchor_3d_sizes=clusters, anchor_stride=anchor_stride) area_extents = np.array([[-40, 40], [-5, 3], [0, 70]]) # Generate anchors in box_3d format start_time = time.time() anchor_boxes_3d = anchor_3d_generator.generate(area_3d=area_extents, ground_plane=ground_plane) end_time = time.time() print("Anchors generated in {} s".format(end_time - start_time)) point_cloud = obj_utils.get_lidar_point_cloud(img_idx, dataset.calib_dir, dataset.velo_dir) offset_dist = 2.0 # Filter points within certain xyz range and offset from ground plane offset_filter = obj_utils.get_point_filter(point_cloud, area_extents, ground_plane, offset_dist) # Filter points within 0.2m of the road plane road_filter = obj_utils.get_point_filter(point_cloud, area_extents, ground_plane, 0.1) slice_filter = np.logical_xor(offset_filter, road_filter) point_cloud = point_cloud.T[slice_filter] # Generate Voxel Grid vx_grid_3d = voxel_grid.VoxelGrid() vx_grid_3d.voxelize(point_cloud, 0.1, area_extents) # Anchors in anchor format all_anchors = box_3d_encoder.box_3d_to_anchor(anchor_boxes_3d) # Filter the boxes here! start_time = time.time() empty_filter = \ anchor_filter.get_empty_anchor_filter(anchors=all_anchors, voxel_grid_3d=vx_grid_3d, density_threshold=1) anchor_boxes_3d = anchor_boxes_3d[empty_filter] end_time = time.time() print("Anchors filtered in {} s".format(end_time - start_time)) # Visualize GT boxes # Grab ground truth ground_truth_list = obj_utils.read_labels(dataset.label_dir, img_idx) # ---------- # Test Sample extraction # Visualize from here vis_utils.visualization(dataset.rgb_image_dir, img_idx) plt.show(block=False) image_path = dataset.get_rgb_image_path(sample_name) image_shape = np.array(Image.open(image_path)).shape rgb_boxes, rgb_normalized_boxes = \ anchor_projector.project_to_image_space(all_anchors, dataset, image_shape, img_idx) # Overlay boxes on images anchor_objects = [] for anchor_idx in range(len(anchor_boxes_3d)): anchor_box_3d = anchor_boxes_3d[anchor_idx] obj_label = box_3d_encoder.box_3d_to_object_label( anchor_box_3d, 'Anchor') # Append to a list for visualization in VTK later anchor_objects.append(obj_label) for idx in range(len(ground_truth_list)): ground_truth_obj = ground_truth_list[idx] # Append to a list for visualization in VTK later anchor_objects.append(ground_truth_obj) # Create VtkAxes axes = vtk.vtkAxesActor() axes.SetTotalLength(5, 5, 5) # Create VtkBoxes for boxes vtk_boxes = VtkBoxes() vtk_boxes.set_objects(anchor_objects, anchor_colour_scheme) vtk_point_cloud = VtkPointCloud() vtk_point_cloud.set_points(point_cloud) vtk_voxel_grid = VtkVoxelGrid() vtk_voxel_grid.set_voxels(vx_grid_3d) # Create Voxel Grid Renderer in bottom half vtk_renderer = vtk.vtkRenderer() vtk_renderer.AddActor(vtk_boxes.vtk_actor) # vtk_renderer.AddActor(vtk_point_cloud.vtk_actor) vtk_renderer.AddActor(vtk_voxel_grid.vtk_actor) vtk_renderer.AddActor(axes) vtk_renderer.SetBackground(0.2, 0.3, 0.4) # Setup Camera current_cam = vtk_renderer.GetActiveCamera() current_cam.Pitch(170.0) current_cam.Roll(180.0) # Zooms out to fit all points on screen vtk_renderer.ResetCamera() # Zoom in slightly current_cam.Zoom(2.5) # Reset the clipping range to show all points vtk_renderer.ResetCameraClippingRange() # Setup Render Window vtk_render_window = vtk.vtkRenderWindow() vtk_render_window.SetWindowName("Anchors") vtk_render_window.SetSize(900, 500) vtk_render_window.AddRenderer(vtk_renderer) # Setup custom interactor style, which handles mouse and key events vtk_render_window_interactor = vtk.vtkRenderWindowInteractor() vtk_render_window_interactor.SetRenderWindow(vtk_render_window) vtk_render_window_interactor.SetInteractorStyle( vtk.vtkInteractorStyleTrackballCamera()) # Render in VTK vtk_render_window.Render() vtk_render_window_interactor.Start() # Blocking
def main(): # Setting Paths cam = 2 # dataset_dir = '/media/bradenhurl/hd/gta/object/' data_set = 'training' dataset_dir = os.path.expanduser('~') + '/wavedata-dev/demos/gta' #dataset_dir = os.path.expanduser('~') + '/Kitti/object/' dataset_dir = os.path.expanduser( '~') + '/GTAData/TruPercept/object_tru_percept8/' #Set to true to see predictions (results) from all perspectives use_results = True altPerspective = False perspID = 48133 perspStr = '%07d' % perspID altPerspect_dir = os.path.join(dataset_dir, data_set + '/alt_perspective/') if altPerspective: data_set = data_set + '/alt_perspective/' + perspStr fromWiseWindows = False useEVE = False if fromWiseWindows: data_set = 'object' if useEVE: dataset_dir = '/media/bradenhurl/hd/data/eve/' else: dataset_dir = '/media/bradenhurl/hd/data/' image_dir = os.path.join(dataset_dir, data_set) + '/image_2' velo_dir = os.path.join(dataset_dir, data_set) + '/velodyne' calib_dir = os.path.join(dataset_dir, data_set) + '/calib' if use_results: label_dir = os.path.join(dataset_dir, data_set) + '/predictions' else: label_dir = os.path.join(dataset_dir, data_set) + '/label_2' base_dir = os.path.join(dataset_dir, data_set) comparePCs = False if comparePCs: velo_dir2 = os.path.join(dataset_dir, data_set) + '/velodyne' tracking = False if tracking: seq_idx = 1 data_set = '%04d' % seq_idx dataset_dir = '/media/bradenhurl/hd/GTAData/August-01/tracking' image_dir = os.path.join(dataset_dir, 'images', data_set) label_dir = os.path.join(dataset_dir, 'labels', data_set) velo_dir = os.path.join(dataset_dir, 'velodyne', data_set) calib_dir = os.path.join(dataset_dir, 'training', 'calib', '0000') #Used for visualizing inferences #label_dir = '/media/bradenhurl/hd/avod/avod/data/outputs/pyramid_people_gta_40k' #label_dir = label_dir + '/predictions/kitti_predictions_3d/test/0.02/154000/data/' closeView = False pitch = 170 pointSize = 3 zoom = 1 if closeView: pitch = 180.5 pointSize = 3 zoom = 35 image_list = os.listdir(image_dir) fulcrum_of_points = True use_intensity = False img_idx = 2 print('=== Loading image: {:06d}.png ==='.format(img_idx)) print(image_dir) image = cv2.imread(image_dir + '/{:06d}.png'.format(img_idx)) image_shape = (image.shape[1], image.shape[0]) if use_intensity: point_cloud, intensity = obj_utils.get_lidar_point_cloud( img_idx, calib_dir, velo_dir, ret_i=use_intensity) else: point_cloud = obj_utils.get_lidar_point_cloud(img_idx, calib_dir, velo_dir, im_size=image_shape) if comparePCs: point_cloud2 = obj_utils.get_lidar_point_cloud(img_idx, calib_dir, velo_dir2, im_size=image_shape) point_cloud = np.hstack((point_cloud, point_cloud2)) # Reshape points into N x [x, y, z] all_points = np.array(point_cloud).transpose().reshape((-1, 3)) # Define Fixed Sizes for the voxel grid x_min = -85 x_max = 85 y_min = -5 y_max = 5 z_min = 3 z_max = 85 x_min = min(point_cloud[0]) x_max = max(point_cloud[0]) y_min = min(point_cloud[1]) y_max = max(point_cloud[1]) #z_min = min(point_cloud[2]) z_max = max(point_cloud[2]) # Filter points within certain xyz range area_filter = (point_cloud[0] > x_min) & (point_cloud[0] < x_max) & \ (point_cloud[1] > y_min) & (point_cloud[1] < y_max) & \ (point_cloud[2] > z_min) & (point_cloud[2] < z_max) all_points = all_points[area_filter] #point_colours = np.zeros(point_cloud.shape[1],0) #print(point_colours.shape) if fulcrum_of_points: # Get point colours point_colours = vis_utils.project_img_to_point_cloud( all_points, image, calib_dir, img_idx) print("Point colours shape: ", point_colours.shape) print("Sample 0 of colour: ", point_colours[0]) elif use_intensity: adjusted = intensity == 65535 intensity = intensity > 0 intensity = np.expand_dims(intensity, -1) point_colours = np.hstack( (intensity * 255, intensity * 255 - adjusted * 255, intensity * 255 - adjusted * 255)) print("Intensity shape:", point_colours.shape) print("Intensity sample: ", point_colours[0]) # Create Voxel Grid voxel_grid = VoxelGrid() voxel_grid_extents = [[x_min, x_max], [y_min, y_max], [z_min, z_max]] print(voxel_grid_extents) start_time = time.time() voxel_grid.voxelize(all_points, 0.2, voxel_grid_extents) end_time = time.time() print("Voxelized in {} s".format(end_time - start_time)) # Get bounding boxes gt_detections = obj_utils.read_labels(label_dir, img_idx, results=use_results) if gt_detections is None: gt_detections = [] #perspective_utils.to_world(gt_detections, base_dir, img_idx) #perspective_utils.to_perspective(gt_detections, base_dir, img_idx) for entity_str in os.listdir(altPerspect_dir): if os.path.isdir(os.path.join(altPerspect_dir, entity_str)): perspect_detections = perspective_utils.get_detections( base_dir, altPerspect_dir, img_idx, perspID, entity_str, results=use_results) if perspect_detections != None: if use_results: stripped_detections = trust_utils.strip_objs( perspect_detections) gt_detections = gt_detections + stripped_detections else: gt_detections = gt_detections + perspect_detections # Create VtkPointCloud for visualization vtk_point_cloud = VtkPointCloud() if fulcrum_of_points or use_intensity: vtk_point_cloud.set_points(all_points, point_colours) else: vtk_point_cloud.set_points(all_points) vtk_point_cloud.vtk_actor.GetProperty().SetPointSize(pointSize) # Create VtkVoxelGrid for visualization vtk_voxel_grid = VtkVoxelGrid() vtk_voxel_grid.set_voxels(voxel_grid) COLOUR_SCHEME_PAPER = { "Car": (0, 0, 255), # Blue "Pedestrian": (255, 0, 0), # Red "Bus": (0, 0, 255), #Blue "Cyclist": (150, 50, 100), # Purple "Van": (255, 150, 150), # Peach "Person_sitting": (150, 200, 255), # Sky Blue "Truck": (0, 0, 255), # Light Grey "Tram": (150, 150, 150), # Grey "Misc": (100, 100, 100), # Dark Grey "DontCare": (255, 255, 255), # White } # Create VtkBoxes for boxes vtk_boxes = VtkBoxes() vtk_boxes.set_objects(gt_detections, COLOUR_SCHEME_PAPER) #vtk_boxes.COLOUR_SCHEME_KITTI) # Create Axes axes = vtk.vtkAxesActor() axes.SetTotalLength(5, 5, 5) # Create Voxel Grid Renderer in bottom half vtk_renderer = vtk.vtkRenderer() vtk_renderer.AddActor(vtk_point_cloud.vtk_actor) vtk_renderer.AddActor(vtk_voxel_grid.vtk_actor) vtk_renderer.AddActor(vtk_boxes.vtk_actor) #vtk_renderer.AddActor(axes) vtk_renderer.SetBackground(0.2, 0.3, 0.4) # Setup Camera current_cam = vtk_renderer.GetActiveCamera() current_cam.Pitch(pitch) current_cam.Roll(180.0) # Zooms out to fit all points on screen vtk_renderer.ResetCamera() # Zoom in slightly current_cam.Zoom(zoom) # Reset the clipping range to show all points vtk_renderer.ResetCameraClippingRange() # Setup Render Window vtk_render_window = vtk.vtkRenderWindow() vtk_render_window.SetWindowName( "Point Cloud and Voxel Grid, Image {}".format(img_idx)) vtk_render_window.SetSize(1920, 1080) vtk_render_window.AddRenderer(vtk_renderer) # Setup custom interactor style, which handles mouse and key events vtk_render_window_interactor = vtk.vtkRenderWindowInteractor() vtk_render_window_interactor.SetRenderWindow(vtk_render_window) # Add custom interactor to toggle actor visibilities vtk_render_window_interactor.SetInteractorStyle( vis_utils.ToggleActorsInteractorStyle([ vtk_point_cloud.vtk_actor, vtk_voxel_grid.vtk_actor, vtk_boxes.vtk_actor, ])) # Show image image = cv2.imread(image_dir + "/%06d.png" % img_idx) cv2.imshow("Press any key to continue", image) cv2.waitKey() # Render in VTK vtk_render_window.Render() vtk_render_window_interactor.Start() # Blocking
def visualize_objects_in_pointcloud(objects, COLOUR_SCHEME, dataset_dir, img_idx, fulcrum_of_points, use_intensity, receive_from_perspective, compare_pcs=False, show_3d_point_count=False, show_orientation=cfg.VISUALIZE_ORIENTATION, final_results=False, show_score=False, compare_with_gt=False, show_image=True, _text_positions=None, _text_labels=None): image_dir = os.path.join(dataset_dir, 'image_2') velo_dir = os.path.join(dataset_dir, 'velodyne') calib_dir = os.path.join(dataset_dir, 'calib') if compare_pcs: fulcrum_of_points = False print('=== Loading image: {:06d}.png ==='.format(img_idx)) print(image_dir) image = cv2.imread(image_dir + '/{:06d}.png'.format(img_idx)) image_shape = (image.shape[1], image.shape[0]) if use_intensity: point_cloud,intensity = obj_utils.get_lidar_point_cloud(img_idx, calib_dir, velo_dir, ret_i=use_intensity) else: point_cloud = obj_utils.get_lidar_point_cloud(img_idx, calib_dir, velo_dir, im_size=image_shape) if compare_pcs: receive_persp_dir = os.path.join(altPerspect_dir, '{:07d}'.format(receive_from_perspective)) velo_dir2 = os.path.join(receive_persp_dir, 'velodyne') print(velo_dir2) if not os.path.isdir(velo_dir2): print("Error: cannot find velo_dir2: ", velo_dir2) exit() point_cloud2 = obj_utils.get_lidar_point_cloud(img_idx, calib_dir, velo_dir2, im_size=image_shape) #Set to true to display point clouds in world coordinates (for debugging) display_in_world=False if display_in_world: point_cloud = perspective_utils.pc_to_world(point_cloud.T, receive_persp_dir, img_idx) point_cloud2 = perspective_utils.pc_to_world(point_cloud2.T, dataset_dir, img_idx) point_cloud = np.hstack((point_cloud.T, point_cloud2.T)) else: point_cloud2 = perspective_utils.pc_persp_transform(point_cloud2.T, receive_persp_dir, dataset_dir, img_idx) point_cloud = np.hstack((point_cloud, point_cloud2.T)) # Reshape points into N x [x, y, z] all_points = np.array(point_cloud).transpose().reshape((-1, 3)) # Define Fixed Sizes for the voxel grid x_min = -85 x_max = 85 y_min = -5 y_max = 5 z_min = 3 z_max = 85 # Comment these out to filter points by area x_min = min(point_cloud[0]) x_max = max(point_cloud[0]) y_min = min(point_cloud[1]) y_max = max(point_cloud[1]) z_min = min(point_cloud[2]) z_max = max(point_cloud[2]) # Filter points within certain xyz range area_filter = (point_cloud[0] > x_min) & (point_cloud[0] < x_max) & \ (point_cloud[1] > y_min) & (point_cloud[1] < y_max) & \ (point_cloud[2] > z_min) & (point_cloud[2] < z_max) all_points = all_points[area_filter] point_colours = None if fulcrum_of_points: # Get point colours point_colours = vis_utils.project_img_to_point_cloud(all_points, image, calib_dir, img_idx) elif use_intensity: adjusted = intensity == 65535 intensity = intensity > 0 intensity = np.expand_dims(intensity,-1) point_colours = np.hstack((intensity*255,intensity*255-adjusted*255,intensity*255-adjusted*255)) # Create Voxel Grid voxel_grid = VoxelGrid() voxel_grid_extents = [[x_min, x_max], [y_min, y_max], [z_min, z_max]] print(voxel_grid_extents) start_time = time.time() voxel_grid.voxelize(all_points, 0.2, voxel_grid_extents) end_time = time.time() print("Voxelized in {} s".format(end_time - start_time)) # Some settings for the initial camera view and point size closeView = False pitch = 170 pointSize = 2 zoom = 1 if closeView: pitch = 180.5 pointSize = 3 zoom = 35 # Create VtkPointCloud for visualization vtk_point_cloud = VtkPointCloud() if point_colours is not None: vtk_point_cloud.set_points(all_points, point_colours) else: vtk_point_cloud.set_points(all_points) vtk_point_cloud.vtk_actor.GetProperty().SetPointSize(pointSize) # Create VtkVoxelGrid for visualization vtk_voxel_grid = VtkVoxelGrid() vtk_voxel_grid.set_voxels(voxel_grid) # Create VtkBoxes for boxes vtk_boxes = VtkBoxes() vtk_boxes.set_objects(objects, COLOUR_SCHEME, show_orientation)#vtk_boxes.COLOUR_SCHEME_KITTI) # Create Axes axes = vtk.vtkAxesActor() axes.SetTotalLength(5, 5, 5) # Create Voxel Grid Renderer in bottom half vtk_renderer = vtk.vtkRenderer() vtk_renderer.AddActor(vtk_point_cloud.vtk_actor) vtk_renderer.AddActor(vtk_voxel_grid.vtk_actor) vtk_renderer.AddActor(vtk_boxes.vtk_actor) vtk_renderer.AddActor(axes) if _text_positions is not None: vtk_text_labels = VtkTextLabels() vtk_text_labels.set_text_labels(_text_positions, _text_labels) vtk_renderer.AddActor(vtk_text_labels.vtk_actor) vtk_renderer.SetBackground(0.2, 0.3, 0.4) # Setup Camera current_cam = vtk_renderer.GetActiveCamera() current_cam.Pitch(pitch) current_cam.Roll(180.0) # Zooms out to fit all points on screen vtk_renderer.ResetCamera() # Zoom in slightly current_cam.Zoom(zoom) # Zoom/navigate to the desired camera view then exit # Three lines will be output. Paste these here # Above forward view current_cam.SetPosition(7.512679241328601, -312.20497623371926, -130.38469206536766) current_cam.SetViewUp(-0.01952407393317445, -0.44874501090739727, 0.893446543293314) current_cam.SetFocalPoint(11.624950999358777, 14.835920755080867, 33.965665867613836) # Top down view of synchronization current_cam.SetPosition(28.384757950371405, -125.46190537888288, 63.60263366961189) current_cam.SetViewUp(-0.02456679343399302, 0.0030507437719906913, 0.9996935358512673) current_cam.SetFocalPoint(27.042134804730317, 15.654378427929846, 63.13899801247614) current_cam.SetPosition(30.3869590224831, -50.28910856489952, 60.097631136698965) current_cam.SetViewUp(-0.0237472244952177, -0.06015048799392083, 0.997906803325274) current_cam.SetFocalPoint(27.06695416156647, 15.347824332314035, 63.97499987548391) # current_cam.SetPosition(14.391008769593322, -120.06549828061613, -1.567028749253062) # current_cam.SetViewUp(-0.02238762832327178, -0.1049057307562059, 0.9942301452644481) # current_cam.SetFocalPoint(10.601112314728102, 20.237110061924664, 13.151596441968126) # # Top down view of whole detection area # current_cam.SetPosition(11.168659642537031, -151.97163016078756, 17.590894639193227) # current_cam.SetViewUp(-0.02238762832327178, -0.1049057307562059, 0.9942301452644481) # current_cam.SetFocalPoint(6.5828849321501055, 17.79452593368671, 35.400431120570865) # Top down view of scenario current_cam.SetPosition(2.075612197299923, -76.19063612245675, 5.948366424752178) current_cam.SetViewUp(-0.02238762832327178, -0.1049057307562059, 0.9942301452644481) current_cam.SetFocalPoint(-0.5129380758134061, 19.637933198314016, 16.00138547483155) # Reset the clipping range to show all points vtk_renderer.ResetCameraClippingRange() # Setup Render Window vtk_render_window = vtk.vtkRenderWindow() vtk_render_window.SetWindowName( "Point Cloud and Voxel Grid, Image {}".format(img_idx)) vtk_render_window.SetSize(1920, 1080) vtk_render_window.AddRenderer(vtk_renderer) # Setup custom interactor style, which handles mouse and key events vtk_render_window_interactor = vtk.vtkRenderWindowInteractor() vtk_render_window_interactor.SetRenderWindow(vtk_render_window) # Add custom interactor to toggle actor visibilities vtk_render_window_interactor.SetInteractorStyle( vis_utils.ToggleActorsInteractorStyle([ vtk_point_cloud.vtk_actor, vtk_voxel_grid.vtk_actor, vtk_boxes.vtk_actor, ])) # Show image if show_image: image = cv2.imread(image_dir + "/%06d.png" % img_idx) cv2.imshow("Press any key to continue", image) cv2.waitKey() # Render in VTK vtk_render_window.Render() vtk_render_window_interactor.Start() # Blocking # vtk_render_window_interactor.Initialize() # Non-Blocking # Obtain camera positional information for repeatable views print("current_cam.SetPosition{}".format(current_cam.GetPosition())) print("current_cam.SetViewUp{}".format(current_cam.GetViewUp())) print("current_cam.SetFocalPoint{}".format(current_cam.GetFocalPoint()))