def pcl_vis(clouds): if isinstance(clouds, list): for cloud in clouds: vis = pcl.Visualizer() vis.addPointCloud(cloud) # if normal is not None: # vis.addPointCloudNormals(cloud, cloud_nm) # else: vis.addCoordinateSystem() vis.spin() else: vis = pcl.Visualizer() vis.addPointCloud(clouds) vis.addCoordinateSystem() vis.spin()
def test_color_handlers(self): viewer = pcl.Visualizer() cloud = pcl.PointCloud(np.random.rand(100, 4).astype('f4')) viewer.addPointCloud(cloud, field="y", id="cloud1") cloud = pcl.PointCloud(np.random.rand(100, 4).astype('f4')) viewer.addPointCloud(cloud, color=[0.8, 0.2, 0], id="cloud2") cloud = pcl.PointCloud(np.random.rand(100, 4).astype('f4')) viewer.addPointCloud( cloud, color_handler=lambda: np.random.rand(100, 4) * 255, id="cloud3") cloud = np.random.rand(100, 4).astype('f4') cloud[:, 3] *= 20 # create label fields cloud = pcl.create_xyzl(cloud) viewer.addPointCloud(cloud, field="label", id="cloud4") cloud = np.random.rand(100, 6).astype('f4') cloud[:, 3:6] *= 256 # create rgb fields cloud = pcl.create_xyzrgb(cloud) viewer.addPointCloud(cloud, field="rgb", id="cloud5") cloud = np.random.rand(100, 7).astype('f4') cloud[:, 3:7] *= 256 # create rgb fields cloud = pcl.create_xyzrgba(cloud) viewer.addPointCloud(cloud, field="rgba", id="cloud6") viewer.spinOnce(time=2000) viewer.close()
def test_add_callback(self): def key_callback(event): print(event) def mouse_callback(event): print(event) def picking_callback(event): print(event) def area_callback(event): print(event) cloud = pcl.PointCloud(np.random.rand(100, 4).astype('f4')) viewer = pcl.Visualizer("Testing mouse and key events") viewer.addPointCloud(cloud) connection = viewer.registerKeyboardCallback(key_callback) assert connection.connected() connection = viewer.registerMouseCallback(mouse_callback) assert connection.connected() connection = viewer.registerPointPickingCallback(picking_callback) assert connection.connected() connection = viewer.registerAreaPickingCallback(area_callback) assert connection.connected() viewer.spinOnce(time=2000) viewer.close()
def test_python_color_handler(self): viewer = pcl.Visualizer() cloud = pcl.PointCloud(np.random.rand(1000, 4).astype('f4')) def rgb_handler(cloud_ref): r = (cloud_ref.xyz[:, [0]] * 255) g = (cloud_ref.xyz[:, [1]] * 255) b = (cloud_ref.xyz[:, [2]] * 255) return np.hstack([r, g, b]).astype('u1') viewer.addPointCloud(cloud, color_handler=rgb_handler, id="cloud1") cloud = pcl.PointCloud(np.random.rand(1000, 4).astype('f4')) def rgba_handler(cloud_ref): r = (cloud_ref.xyz[:, [0]] * 255) g = (cloud_ref.xyz[:, [1]] * 255) b = (cloud_ref.xyz[:, [2]] * 255) a = np.full((len(cloud), 1), 200) return np.hstack([r, g, b, a]).astype('u1') viewer.addPointCloud(cloud, color_handler=rgba_handler, id="cloud2") viewer.spinOnce(time=2000) viewer.close()
def test_add_normal(self): viewer = pcl.Visualizer() cloud_data = np.random.rand(100, 6) cloud_data[:, 3:] = np.clip(cloud_data[:, 3:] * 128 + 128, 0, 256) cloud = pcl.create_xyzrgb(cloud_data) normals = pcl.create_normal(np.random.rand(100, 4)) viewer.addPointCloud(cloud) viewer.addPointCloudNormals(cloud, normals, level=2, scale=0.1, id="cloudn") viewer.spinOnce(time=2000) viewer.close()
def visualize_pcl(xyz, rgb=None, intensity=None, normal=None, filename=None, single_batch=False, tag=''): """Inputs are tensors of shape B*C*N """ ## 1. tensor to np array B = xyz.shape[0] xyz_np = xyz.cpu().numpy().swapaxes(1, 2) if rgb is not None: rgb_np = rgb.cpu().numpy().swapaxes(1, 2) * 255 xyz_rgb = np.concatenate((xyz_np, rgb_np), axis=2) elif intensity is not None: intensity_np = intensity.cpu().numpy().swapaxes(1, 2) xyz_inten = np.concatenate((xyz_np, intensity_np), axis=2) if normal is not None: normal_np = normal.cpu().numpy().swapaxes(1, 2) ## 2. np array to pcl cloud objects ## 3. create visualize window for ib in range(B): if rgb is not None: cloud = pcl.create_xyzrgb(xyz_rgb[ib]) elif intensity is not None: cloud = pcl.create_xyzi(xyz_inten[ib]) else: cloud = pcl.create_xyz(xyz_np[ib]) if normal is not None: cloud_nm = pcl.create_normal(normal_np[ib]) cloud = cloud.append_fields(cloud_nm) # print(cloud.to_ndarray()) if filename is None: vis = pcl.Visualizer() if normal is not None: vis.addPointCloudNormals(cloud, cloud_nm) else: vis.addPointCloud(cloud) vis.addCoordinateSystem() vis.spin() else: if single_batch: pcl.io.save_pcd('{}{}.pcd'.format(filename, tag), cloud) # if normal is not None: # pcl.io.save_pcd('{}{}_normal.pcd'.format(filename, tag), cloud_nm) else: pcl.io.save_pcd('{}{}_{}.pcd'.format(filename, tag, ib), cloud)
def test_color_handlers(self): viewer = pcl.Visualizer() cloud = pcl.PointCloud(np.random.rand(100, 4).astype('f4')) viewer.addPointCloud(cloud, field="y", id="cloud1") cloud = pcl.PointCloud(np.random.rand(100, 4).astype('f4')) viewer.addPointCloud(cloud, color=[0.8,0.2,0], id="cloud2") cloud = pcl.PointCloud(np.random.rand(100, 4).astype('f4')) viewer.addPointCloud(cloud, color_handler=lambda: np.random.rand(100, 4)*255, id="cloud3") viewer.spinOnce(time=2000) viewer.close()
def pcl_load_viewer_fromfile(filename=None): """accept a .cam file generated by pcl, generate a pcl.Visualizer""" viewer = pcl.Visualizer() viewer.addCoordinateSystem() # optional if filename is not None: pos, view, focal, win_size = pcl_load_viewpoint(filename) viewer.setCameraPosition( pos, view, focal ) #https://github.com/PointCloudLibrary/pcl/blob/000a762bb84781a119ee0cd4ba6bdfee59325fc3/visualization/src/interactor_style.cpp # viewer.setCameraClipDistances(0.0001,5.0) viewer.setSize(win_size[0], win_size[1]) return viewer
def test_load_pointcloud(self): cloud1 = pcl.PointCloud(np.random.rand(100, 4).astype('f4')) cloud2 = pcl.PointCloud(np.random.rand(100, 4).astype('f4')) viewer = pcl.Visualizer() viewer.addPointCloud(cloud1) viewer.addCoordinateSystem() viewer.removeCoordinateSystem() v1 = viewer.createViewPort(0, 0, 0.5, 1) v2 = viewer.createViewPort(0.5, 0, 1, 1) viewer.addPointCloud(cloud1, viewport=v1, id="cloud1") viewer.addPointCloud(cloud2, viewport=v2, id="cloud2") viewer.spinOnce(time=1000) viewer.close()
def test_ground_truth_visualizer_pcl(self): idx = selection or random.randint(0, len(self.oloader)) lidar = random.choice(self.oloader.VALID_LIDAR_NAMES) cloud = self.oloader.lidar_data(idx, lidar) cloud = pcl.create_xyzi(cloud[:, :4]) targets = self.oloader.annotation_3dobject(idx) calib = self.oloader.calibration_data(idx) visualizer = pcl.Visualizer() visualizer.addPointCloud(cloud, field="intensity") pcl_vis(visualizer, lidar, targets, calib) visualizer.setRepresentationToWireframeForAllActors() visualizer.setWindowName( "Please check whether the gt boxes are aligned!") visualizer.spinOnce(time=5000) visualizer.close()
def test_ground_truth_visualizer_pcl(self): # this function is overrided since point cloud return from waymo is in vehicle frame idx = selection or random.randint(0, len(self.oloader)) cloud = self.oloader.lidar_data(idx, "lidar_top") cloud = pcl.create_xyzi(cloud[:, :4]) targets = self.oloader.annotation_3dobject(idx) calib = self.oloader.calibration_data(idx) visualizer = pcl.Visualizer() visualizer.addPointCloud(cloud, field="intensity") pcl_vis(visualizer, "vehicle", targets, calib) visualizer.setRepresentationToWireframeForAllActors() visualizer.setWindowName( "Please check whether the gt boxes are aligned!") visualizer.spinOnce(time=5000) visualizer.close()
def test_add_shape(self): cloud = pcl.PointCloud(np.random.rand(100, 4).astype('f4')) viewer = pcl.Visualizer() viewer.addPointCloud(cloud) viewer.addLine([-1,-1,-1], [1,1,1]) viewer.addArrow([1,-1,-1], [-1,1,1], line_color=[0.7,0,0], text_color=[0,3,0,0]) viewer.addSphere([0,0,0], 1) viewer.addText("Text", -1, 1, fontsize=100) viewer.addText3D("Text3D", [-1,1,-1], text_scale=2) viewer.spinOnce(time=500) viewer.updateSphere([0,0,0], 2) viewer.spinOnce(time=500) viewer.close()
def test_add_callback(self): def key_callback(event): print(event.KeyCode, event.KeySym) def mouse_callback(event): print(event.X, event.Y, event.Type, event.Button) def picking_callback(event): print(event.Point, event.PointIndex) def area_callback(event): print(event.PointsIndices) cloud = pcl.PointCloud(np.random.rand(100, 4).astype('f4')) viewer = pcl.Visualizer() viewer.addPointCloud(cloud) viewer.registerKeyboardCallback(key_callback) viewer.registerMouseCallback(mouse_callback) viewer.registerPointPickingCallback(picking_callback) viewer.registerAreaPickingCallback(area_callback) viewer.spinOnce(time=2000) viewer.close()
def dataset_visualize_pcl(dataset_path: Union[str, Path], dataset_type: str, scene: str, ninter_frames: int = 0): ''' Visualize tracking dataset using PCL visualizer :param dataset_path: path to the dataset root :param dataset_type: type of supported datasets. Options: {kitti-raw, nuscenes, waymo} :param scene: scene ID to be visualized ''' import pcl dataset_type = dataset_type.lower() if dataset_type == "kitti-raw": loader = KittiRawLoader(dataset_path) elif dataset_type == "nuscenes": loader = NuscenesLoader(dataset_path) else: raise ValueError("Unsupported dataset type!") state = dict(idx=1) vis = pcl.Visualizer() def render_next(key): if not (key is None or (key.KeySym == 'space' and key.keyDown())): return lidar_frame = loader.VALID_LIDAR_NAMES[0] sidx = scene, state['idx'] objs = loader.annotation_3dobject(sidx) calib = loader.calibration_data(sidx) cloud = loader.lidar_data(sidx)[:, :4] inter_lidar = loader.intermediate_data(sidx, names=lidar_frame, ninter_frames=ninter_frames) pose = loader.pose(sidx) for frame in inter_lidar: lidar_ego_rt = calib.get_extrinsic(frame_from=lidar_frame) rt = np.linalg.inv(lidar_ego_rt).dot(np.linalg.inv(pose.h**o()))\ .dot(frame.pose.h**o()).dot(lidar_ego_rt) xyz = frame.data[:, :3].dot(rt[:3, :3].T) + rt[:3, 3] cloud_frame = np.hstack([xyz, frame.data[:, [3]]]) cloud = np.vstack([cloud, cloud_frame]) vis.removeAllPointClouds() vis.removeAllShapes() vis.addPointCloud(pcl.create_xyzi(cloud[:, :4]), field="intensity") visualize_detections(vis, lidar_frame, objs, calib, id_prefix="gt", box_color="rainbow") vis.setRepresentationToWireframeForAllActors() vis.addCoordinateSystem() state['idx'] += 1 if state['idx'] >= loader.sequence_sizes[scene]: vis.close() vis.registerKeyboardCallback(render_next) render_next(None) vis.spin()
def test_point_cloud_temporal_fusion(self): # TODO: this fail for nuscenes and waymo idx = selection or random.randint(0, len(self.tloader)) lidar = self.tloader.VALID_LIDAR_NAMES[0] # load data clouds = self.tloader.lidar_data(idx, lidar) poses = self.tloader.pose(idx) targets = self.tloader.annotation_3dobject(idx) calib = self.oloader.calibration_data(idx) cloud1, cloud2 = clouds[0][:, :4], clouds[-1][:, :4] pose1, pose2 = poses[0], poses[-1] targets1, targets2 = targets[0], targets[-1] print("START", pose1) print("END", pose2) # create transforms tf = TransformSet("global") fname1, fname2 = "pose1", "pose2" tf.set_intrinsic_map_pin(fname1) tf.set_intrinsic_map_pin(fname2) tf.set_extrinsic(pose1.h**o(), fname1) tf.set_extrinsic(pose2.h**o(), fname2) # make coordinate unified in frame targets1 = calib.transform_objects(targets1, lidar) targets2 = calib.transform_objects(targets2, lidar) targets1.frame = fname1 targets2.frame = fname2 print("HOMO1", pose1.h**o()) print("HOMO2", pose2.h**o()) print(tf.get_extrinsic(fname1, fname2)) # visualize both point cloud in frame2 visualizer = pcl.Visualizer() visualizer.addPointCloud(pcl.create_xyzi( tf.transform_points(cloud1, frame_from=fname1, frame_to=fname2)), field="intensity", id="cloud1") visualizer.addPointCloud(pcl.create_xyzi(cloud2), field="intensity", id="cloud2") pcl_vis(visualizer, fname2, targets1, tf, box_color=(1, 1, 0), id_prefix="frame1") pcl_vis(visualizer, fname2, targets2, tf, box_color=(0, 1, 1), id_prefix="frame2") visualizer.setRepresentationToWireframeForAllActors() visualizer.addCoordinateSystem() visualizer.setWindowName( "Please check whether the gt boxes are aligned!") # visualizer.spinOnce(time=5000) # visualizer.close() visualizer.spin()
def test_add_callback(self, cloud, labelfile, database_name): def key_callback(event): print(event.KeyCode, event.KeySym) def mouse_callback(event): print(event.X, event.Y, event.Type, event.Button) def picking_callback(event): print(event.Point, event.PointIndex) def area_callback(event): print(event.PointsIndices) def bounding_box_apollo(para_list, line_num, r, b, g): center = np.matrix([para_list[0], para_list[1], para_list[2] ]).T * np.ones((1, 8)) size = np.matrix([[para_list[3], 0, 0], [0, para_list[4], 0], [0, 0, para_list[5]]]) vertex = center + size * np.matrix( [[.5, .5, -.5, -.5, .5, .5, -.5, -.5], [.5, -.5, -.5, .5, .5, -.5, -.5, .5], [.5, .5, .5, .5, -.5, -.5, -.5, -.5]]) for i in range(3): viewer.addLine(vertex[:, i], vertex[:, i + 1], r, b, g, id=str(line_num)) viewer.addLine(vertex[:, i + 4], vertex[:, i + 5], r, b, g, id=str(4 + line_num)) viewer.addLine(vertex[:, i], vertex[:, i + 4], r, b, g, id=str(8 + line_num)) line_num += 1 viewer.addLine(vertex[:, 3], vertex[:, 0], r, b, g, id=str(line_num)) viewer.addLine(vertex[:, 4], vertex[:, 7], r, b, g, id=str(4 + line_num)) viewer.addLine(vertex[:, 3], vertex[:, 7], r, b, g, id=str(8 + line_num)) lines = 0 viewer = pcl.Visualizer() viewer.addPointCloud(cloud) viewer.addCoordinateSystem() # addLine(self, p1, p2, double r=0.5, double g=0.5, double b=0.5, str id="line", int viewport=0) # addText(self, str text, int xpos, int ypos, int fontsize = 10, double r = 1, double g = 1, double b = 1, str id = "", int viewport = 0) file = open(labelfile, 'r') if database_name == "apollo": for obj_line in file: obj_line = obj_line.split() obj_para = [float(i) for i in obj_line[1:7]] if obj_line[0] == "vehicle": # red r = 1 g = 0 b = 0 bounding_box_apollo(obj_para, lines, r, b, g) lines += 12 elif obj_line[0] == "pedestrian": # blue r = 0 g = 1 b = 0 bounding_box_apollo(obj_para, lines, r, b, g) lines += 12 elif obj_line[0] == "cyclist": # green r = 0 g = 0 b = 1 bounding_box_apollo(obj_para, lines, r, b, g) lines += 12 else: # don't care r = 0.7 g = 0.7 b = 0.1 bounding_box_apollo(obj_para, lines, r, b, g) lines += 12 elif database_name == "kitti": pass viewer.registerKeyboardCallback(key_callback) viewer.registerMouseCallback(mouse_callback) viewer.registerPointPickingCallback(picking_callback) viewer.registerAreaPickingCallback(area_callback) viewer.spinOnce(time=2e9) viewer.close()