class LaserScanVis: """Class that creates and handles a visualizer for a pointcloud""" def __init__(self, scan, scan_names, label_names, offset=0, semantics=True, instances=False): self.scan = scan self.scan_names = scan_names self.label_names = label_names self.offset = offset self.semantics = semantics self.instances = instances # sanity check if not self.semantics and self.instances: print("Instances are only allowed in when semantics=True") raise ValueError self.reset() self.update_scan() def reset(self): """ Reset. """ # last key press (it should have a mutex, but visualization is not # safety critical, so let's do things wrong) self.action = "no" # no, next, back, quit are the possibilities # new canvas prepared for visualizing data self.canvas = SceneCanvas(keys='interactive', show=True) # interface (n next, b back, q quit, very simple) self.canvas.events.key_press.connect(self.key_press) self.canvas.events.draw.connect(self.draw) # grid self.grid = self.canvas.central_widget.add_grid() # laserscan part self.scan_view = vispy.scene.widgets.ViewBox(border_color='white', parent=self.canvas.scene) self.grid.add_widget(self.scan_view, 0, 0) self.scan_vis = visuals.Markers() self.scan_view.camera = 'turntable' self.scan_view.add(self.scan_vis) visuals.XYZAxis(parent=self.scan_view.scene) # add semantics if self.semantics: print("Using semantics in visualizer") self.sem_view = vispy.scene.widgets.ViewBox( border_color='white', parent=self.canvas.scene) self.grid.add_widget(self.sem_view, 0, 1) self.sem_vis = visuals.Markers() self.sem_view.camera = 'turntable' self.sem_view.add(self.sem_vis) visuals.XYZAxis(parent=self.sem_view.scene) # self.sem_view.camera.link(self.scan_view.camera) if self.instances: print("Using instances in visualizer") self.inst_view = vispy.scene.widgets.ViewBox( border_color='white', parent=self.canvas.scene) self.grid.add_widget(self.inst_view, 0, 2) self.inst_vis = visuals.Markers() self.inst_view.camera = 'turntable' self.inst_view.add(self.inst_vis) visuals.XYZAxis(parent=self.inst_view.scene) # self.inst_view.camera.link(self.scan_view.camera) # img canvas size self.multiplier = 1 self.canvas_W = 1024 self.canvas_H = 64 if self.semantics: self.multiplier += 1 if self.instances: self.multiplier += 1 # new canvas for img self.img_canvas = SceneCanvas(keys='interactive', show=True, size=(self.canvas_W, self.canvas_H * self.multiplier)) # grid self.img_grid = self.img_canvas.central_widget.add_grid() # interface (n next, b back, q quit, very simple) self.img_canvas.events.key_press.connect(self.key_press) self.img_canvas.events.draw.connect(self.draw) # add a view for the depth self.img_view = vispy.scene.widgets.ViewBox( border_color='white', parent=self.img_canvas.scene) self.img_grid.add_widget(self.img_view, 0, 0) self.img_vis = visuals.Image(cmap='viridis') self.img_view.add(self.img_vis) # add semantics if self.semantics: self.sem_img_view = vispy.scene.widgets.ViewBox( border_color='white', parent=self.img_canvas.scene) self.img_grid.add_widget(self.sem_img_view, 1, 0) self.sem_img_vis = visuals.Image(cmap='viridis') self.sem_img_view.add(self.sem_img_vis) # add instances if self.instances: self.inst_img_view = vispy.scene.widgets.ViewBox( border_color='white', parent=self.img_canvas.scene) self.img_grid.add_widget(self.inst_img_view, 2, 0) self.inst_img_vis = visuals.Image(cmap='viridis') self.inst_img_view.add(self.inst_img_vis) def get_mpl_colormap(self, cmap_name): cmap = plt.get_cmap(cmap_name) # Initialize the matplotlib color map sm = plt.cm.ScalarMappable(cmap=cmap) # Obtain linear color range color_range = sm.to_rgba(np.linspace(0, 1, 256), bytes=True)[:, 2::-1] return color_range.reshape(256, 3).astype(np.float32) / 255.0 def update_scan(self): # first open data self.scan.open_scan(self.scan_names[self.offset]) if self.semantics: self.scan.open_label(self.label_names[self.offset]) self.scan.colorize() # then change names title = "scan " + str(self.offset) + " of " + str(len(self.scan_names)) self.canvas.title = title self.img_canvas.title = title # then do all the point cloud stuff # plot scan power = 16 # print() range_data = np.copy(self.scan.unproj_range) # print(range_data.max(), range_data.min()) range_data = range_data**(1 / power) # print(range_data.max(), range_data.min()) viridis_range = ((range_data - range_data.min()) / (range_data.max() - range_data.min()) * 255).astype( np.uint8) viridis_map = self.get_mpl_colormap("viridis") viridis_colors = viridis_map[viridis_range] self.scan_vis.set_data(self.scan.points, face_color=viridis_colors[..., ::-1], edge_color=viridis_colors[..., ::-1], size=1) # plot semantics if self.semantics: self.sem_vis.set_data( self.scan.points, face_color=self.scan.sem_label_color[..., ::-1], edge_color=self.scan.sem_label_color[..., ::-1], size=1) # plot instances if self.instances: self.inst_vis.set_data( self.scan.points, face_color=self.scan.inst_label_color[..., ::-1], edge_color=self.scan.inst_label_color[..., ::-1], size=1) # now do all the range image stuff # plot range image data = np.copy(self.scan.proj_range) # print(data[data > 0].max(), data[data > 0].min()) data[data > 0] = data[data > 0]**(1 / power) data[data < 0] = data[data > 0].min() # print(data.max(), data.min()) data = (data - data[data > 0].min()) / \ (data.max() - data[data > 0].min()) # print(data.max(), data.min()) self.img_vis.set_data(data) self.img_vis.update() if self.semantics: self.sem_img_vis.set_data(self.scan.proj_sem_color[..., ::-1]) self.sem_img_vis.update() if self.instances: self.inst_img_vis.set_data(self.scan.proj_inst_color[..., ::-1]) self.inst_img_vis.update() # interface def key_press(self, event): self.canvas.events.key_press.block() self.img_canvas.events.key_press.block() if event.key == 'N': self.offset += 1 self.update_scan() elif event.key == 'B': self.offset -= 1 self.update_scan() elif event.key == 'Q' or event.key == 'Escape': self.destroy() def draw(self, event): if self.canvas.events.key_press.blocked(): self.canvas.events.key_press.unblock() if self.img_canvas.events.key_press.blocked(): self.img_canvas.events.key_press.unblock() def destroy(self): # destroy the visualization self.canvas.close() self.img_canvas.close() vispy.app.quit() def run(self): vispy.app.run()
class PointVis: """Class that creates and handles a visualizer for a pointcloud""" def __init__(self, target_pts=None, viz_dict=None, viz_point=True, viz_label=True, viz_joint=False, viz_box=False): self.viz_point = viz_point self.viz_label = viz_label self.viz_joint = viz_joint self.viz_box = viz_box self.viz_label = viz_label self.reset() self.update_scan(target_pts, viz_dict) def reset(self, sem_color_dict=None): """ Reset. """ # new canvas prepared for visualizing data self.map_color(sem_color_dict=sem_color_dict) self.canvas = SceneCanvas(keys='interactive', show=True) # grid self.grid = self.canvas.central_widget.add_grid() # laserscan part self.scan_view = vispy.scene.widgets.ViewBox(border_color='white', parent=self.canvas.scene) self.grid.add_widget(self.scan_view, 0, 0) self.scan_view.camera = 'turntable' self.scan_vis = visuals.Markers() self.scan_view.add(self.scan_vis) if self.viz_joint: self.joint_vis = visuals.Arrow(connect='segments', arrow_size=18, color='blue', width=10, arrow_type='angle_60') self.arrow_length = 10 self.scan_view.add(self.joint_vis) if self.viz_box: vertices, faces, outline = create_box(width=1, height=1, depth=1, width_segments=1, height_segments=1, depth_segments=1) vertices['color'][:, 3] = 0.2 # breakpoint() self.box = visuals.Box(vertex_colors=vertices['color'], edge_color='b') self.box.transform = STTransform(translate=[-2.5, 0, 0]) self.theta = 0 self.phi = 0 self.scan_view.add(self.box) visuals.XYZAxis(parent=self.scan_view.scene) # add nocs if self.viz_label: print("Using nocs in visualizer") self.nocs_view = vispy.scene.widgets.ViewBox( border_color='white', parent=self.canvas.scene) self.grid.add_widget(self.nocs_view, 0, 1) self.label_vis = visuals.Markers() self.nocs_view.camera = 'turntable' self.nocs_view.add(self.label_vis) visuals.XYZAxis(parent=self.nocs_view.scene) self.nocs_view.camera.link(self.scan_view.camera) def update_scan(self, points, viz_dict=None): # then change names self.canvas.title = "scan " if viz_dict is not None: # input_point_seq = viz_dict['input'] # input_index_seq = viz_dict['coord'] #(3, 150000, 2) # # npts = viz_dict['npts'] # (3,) # npts = [150000, 150000, 150000] # for i, num in enumerate(npts[:1]): # target_pts = input_point_seq[i, 0, :num, 0:3] # indices = input_index_seq[i, 0] # idxs = np.where(indices[:, 0]==200)[0] # target_pts = target_pts[idxs] # indices = indices[idxs] # y_value = [indices[100, 1], indices[800, 1], indices[1200,1]] # idys = list(np.where(indices[:, 1]==y_value[2])[0]) # + list(np.where(indices[:, 1]==y_value[1])[0]) + list(np.where(indices[:, 1]==y_value[2])[0]) # target_pts = target_pts[idys] target_pts = np.concatenate( [viz_dict['input1'], viz_dict['input2']], axis=0) gt_labels = np.concatenate([ viz_dict['label'], 4 * np.ones( (viz_dict['input2'].shape[0]), dtype=np.int32) ], axis=0) print(np.max(target_pts, axis=0).reshape(1, 3)) print(np.min(target_pts, axis=0).reshape(1, 3)) # target_pts = target_pts - (np.max(target_pts, axis=0) + np.min(target_pts, axis=0))/2 power = 16 range_data = np.copy(np.linalg.norm(target_pts, axis=1)) range_data = range_data**(1 / power) viridis_range = ((range_data - range_data.min()) / (range_data.max() - range_data.min()) * 255).astype(np.uint8) viridis_map = self.get_mpl_colormap("viridis") viridis_colors = viridis_map[viridis_range] self.scan_vis.set_data(target_pts, face_color=viridis_colors[..., ::-1], edge_color=viridis_colors[..., ::-1], size=5) # plot nocs if self.viz_label: label_colors = self.sem_color_lut[gt_labels] label_colors = label_colors.reshape((-1, 3)) self.label_vis.set_data(target_pts, face_color=label_colors[..., ::-1], edge_color=label_colors[..., ::-1], size=5) # time.sleep(15) if self.viz_joint: self.update_joints() if self.viz_box: self.update_boxes() def map_color(self, max_classes=20, sem_color_dict=None): # make semantic colors if sem_color_dict: # if I have a dict, make it max_sem_key = 0 for key, data in sem_color_dict.items(): if key + 1 > max_sem_key: max_sem_key = key + 1 self.sem_color_lut = np.zeros((max_sem_key + 100, 3), dtype=np.float32) for key, value in sem_color_dict.items(): self.sem_color_lut[key] = np.array(value, np.float32) / 255.0 else: # otherwise make random max_sem_key = max_classes self.sem_color_lut = np.random.uniform(low=0.0, high=1.0, size=(max_sem_key, 3)) # force zero to a gray-ish color self.sem_color_lut[0] = np.full((3), 0.2) self.sem_color_lut[4] = np.full((3), 0.6) self.sem_color_lut[1] = np.array([1.0, 0.0, 0.0]) self.sem_color_lut[2] = np.array([0.0, 0.0, 1.0]) def get_mpl_colormap(self, cmap_name): cmap = plt.get_cmap(cmap_name) # Initialize the matplotlib color map sm = plt.cm.ScalarMappable(cmap=cmap) # Obtain linear color range color_range = sm.to_rgba(np.linspace(0, 1, 256), bytes=True)[:, 2::-1] return color_range.reshape(256, 3).astype(np.float32) / 255.0 def update_joints(self, joints=None): # plot if joints is not None: start_coords = joints['p'].reshape(1, 3) point_towards = start_coords + joints['l'].reshape(1, 3) else: start_coords = np.array([[1, 0, 0], [-1, 0, 0]]) point_towards = np.array([[0, 0, 1], [0, 0, 1]]) direction_vectors = (start_coords - point_towards).astype(np.float32) norms = np.sqrt(np.sum(direction_vectors**2, axis=-1)) direction_vectors[:, 0] /= norms direction_vectors[:, 1] /= norms direction_vectors[:, 2] /= norms vertices = np.repeat(start_coords, 2, axis=0) vertices[::2] = vertices[::2] + ( (0.5 * self.arrow_length) * direction_vectors) vertices[1::2] = vertices[1::2] - ( (0.5 * self.arrow_length) * direction_vectors) self.joint_vis.set_data( pos=vertices, arrows=vertices.reshape((len(vertices) // 2, 6)), ) def update_boxes(self): pass def draw(self, event): if self.canvas.events.key_press.blocked(): self.canvas.events.key_press.unblock() if self.img_canvas.events.key_press.blocked(): self.img_canvas.events.key_press.unblock() def destroy(self): # destroy the visualization self.canvas.close() vispy.app.quit() def run(self): vispy.app.run()
class LaserDetVis: """Class that creates and handles a visualizer for a pointcloud""" def __init__(self, show_img=False): self.show_img = show_img self.canvas_size = (1920, 1920) self.running = True self.intensity_mode = False self.reset() def is_running(self): return self.running def key_press(self, event): raise NotImplementedError ''' Parameters: points: N x 3 or N x 4 ''' def add_points(self, points): self.points = points[:, 0:3] if points.shape[1] >= 4: intensity = points[:, 4] intensity = ((intensity - intensity.min()) / (intensity.max() - intensity.min()) * 128 + 127).astype(np.uint8) viridis_map = self.get_mpl_colormap("viridis") self.viridis_colors = viridis_map[intensity] """ Takes an object and a projection matrix (P) and projects the 3d bounding box into the image plane. Returns: corners_2d: (8,2) array in left image coord. corners_3d: (8,3) array in in rect camera coord. """ def compute_box_3d(self, obj): """ 7 -------- 4 /| /| 6 -------- 5 . | | | | . 3 -------- 0 |/ |/ 2 -------- 1 Args: boxes3d: (N, 7) [x, y, z, dx, dy, dz, heading], (x, y, z) is the box center Returns: """ boxes3d, is_numpy = check_numpy_to_torch(obj) template = boxes3d.new_tensor(( [1, 1, -1], [1, -1, -1], [-1, -1, -1], [-1, 1, -1], [1, 1, 1], [1, -1, 1], [-1, -1, 1], [-1, 1, 1], )) / 2 corners3d = boxes3d[None, 3:6].repeat(8, 1) * template[:, :] corners3d = rotate_points_along_z(corners3d.view(-1, 8, 3), boxes3d[None, 6]).view(8, 3) corners3d += boxes3d[0:3] connect = np.array([[0, 1], [1, 5], [0, 4], [4, 5], [1, 2], [0, 3], [5, 6], [4, 7], [2, 3], [2, 6], [3, 7], [6, 7]], dtype=np.int32) return corners3d if is_numpy else corners3d.numpy(), connect ''' Parameters: objs: N x 7 (x, y, z, w, l, h, yaw) ''' def add_objs(self, objs): obj_vertices = [] obj_vert_connect = [] obj_label_pos = [] for i in range(len(objs)): vertices, connect = self.compute_box_3d(objs[i]) obj_vertices.append(vertices) obj_vert_connect.append(connect + i * len(vertices)) obj_label_pos.append( (vertices[-1, 0], vertices[-1, 1], vertices[-1, 2])) obj_vertices = np.concatenate(obj_vertices, axis=0) obj_vert_connect = np.concatenate(obj_vert_connect, axis=0) return obj_vertices, obj_vert_connect, obj_label_pos ''' Parameters: data : ndarray ImageVisual data. Can be shape (M, N), (M, N, 3), or (M, N, 4). ''' def add_image(self, img): self.image = img def reset(self): """ Reset. """ # last key press (it should have a mutex, but visualization is not # safety critical, so let's do things wrong) self.action = "no" # no, next, back, quit are the possibilities # new canvas prepared for visualizing data self.canvas = SceneCanvas(keys='interactive', show=True, size=self.canvas_size) # interface (n next, b back, q quit, very simple) self.canvas.events.key_press.connect(self.key_press) self.canvas.events.draw.connect(self.draw) # laserscan part self.scan_view = vispy.scene.widgets.ViewBox(border_color='white', parent=self.canvas.scene) self.scan_view.camera = vispy.scene.TurntableCamera(elevation=30, azimuth=-90, distance=30, translate_speed=30, up='+z') # grid self.grid = self.canvas.central_widget.add_grid() self.grid.add_widget(self.scan_view) self.scan_vis = visuals.Markers(parent=self.scan_view.scene) self.scan_vis.antialias = 0 # self.scan_view.add(self.scan_vis) visuals.XYZAxis(parent=self.scan_view.scene) self.line = visuals.Line(width=1, method='gl', parent=self.scan_view.scene) self.text = visuals.Text(color='red', font_size=600, bold=True, parent=self.scan_view.scene) self.gt_line = visuals.Line(width=1000, parent=self.scan_view.scene) # self.sem_view.camera.link(self.scan_view.camera) if self.show_img: # img canvas size # new canvas for img self.img_canvas = SceneCanvas(keys='interactive', show=True, size=(1242, 375)) # grid self.img_grid = self.img_canvas.central_widget.add_grid() # interface (n next, b back, q quit, very simple) self.img_canvas.events.key_press.connect(self.key_press) self.img_canvas.events.draw.connect(self.draw) # add a view for the depth self.img_view = vispy.scene.widgets.ViewBox( border_color='white', parent=self.img_canvas.scene) self.img_grid.add_widget(self.img_view, 0, 0) self.img_vis = visuals.Image(cmap='viridis') self.img_view.add(self.img_vis) def get_mpl_colormap(self, cmap_name): cmap = plt.get_cmap(cmap_name) # Initialize the matplotlib color map sm = plt.cm.ScalarMappable(cmap=cmap) # Obtain linear color range color_range = sm.to_rgba(np.linspace(0, 1, 256), bytes=True)[:, 2::-1] return color_range.reshape(256, 3).astype(np.float32) / 255.0 def update_view(self, idx, points=None, objs=None, gt_objs=None, ref_scores=None, ref_labels=None, img=None): # then change names title = "scan " + str(idx) self.canvas.title = title # then do all the point cloud stuff # plot scan if points is not None: self.add_points(points) if self.viridis_colors is not None: self.scan_vis.set_data( self.points, face_color=self.viridis_colors[..., ::-1], edge_color=self.viridis_colors[..., ::-1], # face_color='white', # edge_color='white', size=1) else: self.scan_vis.set_data(self.points, size=1) # plot objs if objs is None: self.line.set_data() else: obj_vertices, obj_vert_connect, obj_label_pos = self.add_objs(objs) self.line.set_data(pos=obj_vertices, color='red', connect=obj_vert_connect) if ref_scores is not None and ref_labels is not None: labels = [] for i in range(len(ref_labels)): labels.append('{:.2f}'.format(ref_scores[i])) self.text.text = labels self.text.pos = obj_label_pos if gt_objs is None: self.gt_line.set_data() else: gt_obj_vertices, gt_obj_vert_connect, _ = self.add_objs(gt_objs) self.gt_line.set_data(pos=gt_obj_vertices, color='green', connect=gt_obj_vert_connect) # plot image if self.show_img: self.img_canvas.title = title if img is not None: self.add_image(img) self.img_vis.set_data(self.image) self.img_vis.update() def draw(self, event): if self.canvas.events.key_press.blocked(): self.canvas.events.key_press.unblock() if self.show_img: if self.img_canvas.events.key_press.blocked(): self.img_canvas.events.key_press.unblock() def destroy(self): # destroy the visualization self.canvas.close() if self.show_img: self.img_canvas.close() vispy.app.quit() self.running = False def run(self): vispy.app.run()
class QtViewer(QSplitter): with open(os.path.join(resources_dir, 'stylesheet.qss'), 'r') as f: raw_stylesheet = f.read() def __init__(self, viewer): super().__init__() self.pool = QThreadPool() QCoreApplication.setAttribute( Qt.AA_UseStyleSheetPropagationInWidgetStyles, True) self.viewer = viewer self.dims = QtDims(self.viewer.dims) self.controls = QtControls(self.viewer) self.layers = QtLayerList(self.viewer.layers) self.layerButtons = QtLayerButtons(self.viewer) self.viewerButtons = QtViewerButtons(self.viewer) self.console = QtConsole({'viewer': self.viewer}) layerList = QWidget() layerList.setObjectName('layerList') layerListLayout = QVBoxLayout() layerListLayout.addWidget(self.layerButtons) layerListLayout.addWidget(self.layers) layerListLayout.addWidget(self.viewerButtons) layerListLayout.setContentsMargins(8, 4, 8, 6) layerList.setLayout(layerListLayout) self.dockLayerList = QtViewerDockWidget( self, layerList, name='layer list', area='left', allowed_areas=['left', 'right'], ) self.dockLayerControls = QtViewerDockWidget( self, self.controls, name='layer controls', area='left', allowed_areas=['left', 'right'], ) self.dockConsole = QtViewerDockWidget( self, self.console, name='console', area='bottom', allowed_areas=['top', 'bottom'], shortcut='Ctrl+Shift+C', ) self.dockConsole.setVisible(False) self.dockLayerControls.visibilityChanged.connect(self._constrain_width) self.dockLayerList.setMaximumWidth(258) self.dockLayerList.setMinimumWidth(258) self.aboutKeybindings = QtAboutKeybindings(self.viewer) self.aboutKeybindings.hide() # This dictionary holds the corresponding vispy visual for each layer self.layer_to_visual = {} if self.console.shell is not None: self.viewerButtons.consoleButton.clicked.connect( lambda: self.toggle_console()) else: self.viewerButtons.consoleButton.setEnabled(False) self.canvas = SceneCanvas(keys=None, vsync=True) self.canvas.events.ignore_callback_errors = False self.canvas.events.draw.connect(self.dims.enable_play) self.canvas.native.setMinimumSize(QSize(200, 200)) self.canvas.context.set_depth_func('lequal') self.canvas.connect(self.on_mouse_move) self.canvas.connect(self.on_mouse_press) self.canvas.connect(self.on_mouse_release) self.canvas.connect(self.on_key_press) self.canvas.connect(self.on_key_release) self.canvas.connect(self.on_draw) self.view = self.canvas.central_widget.add_view() self._update_camera() main_widget = QWidget() main_layout = QVBoxLayout() main_layout.setContentsMargins(10, 22, 10, 2) main_layout.addWidget(self.canvas.native) main_layout.addWidget(self.dims) main_layout.setSpacing(10) main_widget.setLayout(main_layout) self.setOrientation(Qt.Vertical) self.addWidget(main_widget) self._last_visited_dir = str(Path.home()) self._cursors = { 'disabled': QCursor( QPixmap(':/icons/cursor/cursor_disabled.png').scaled(20, 20)), 'cross': Qt.CrossCursor, 'forbidden': Qt.ForbiddenCursor, 'pointing': Qt.PointingHandCursor, 'standard': QCursor(), } self._update_palette(viewer.palette) self._key_release_generators = {} self.viewer.events.interactive.connect(self._on_interactive) self.viewer.events.cursor.connect(self._on_cursor) self.viewer.events.reset_view.connect(self._on_reset_view) self.viewer.events.palette.connect( lambda event: self._update_palette(event.palette)) self.viewer.layers.events.reordered.connect(self._reorder_layers) self.viewer.layers.events.added.connect(self._add_layer) self.viewer.layers.events.removed.connect(self._remove_layer) self.viewer.dims.events.camera.connect( lambda event: self._update_camera()) # stop any animations whenever the layers change self.viewer.events.layers_change.connect(lambda x: self.dims.stop()) self.setAcceptDrops(True) def _constrain_width(self, event): # allow the layer controls to be wider, only if floated if self.dockLayerControls.isFloating(): self.controls.setMaximumWidth(700) else: self.controls.setMaximumWidth(220) def _add_layer(self, event): """When a layer is added, set its parent and order.""" layers = event.source layer = event.item vispy_layer = create_vispy_visual(layer) vispy_layer.camera = self.view.camera vispy_layer.node.parent = self.view.scene vispy_layer.order = len(layers) self.layer_to_visual[layer] = vispy_layer def _remove_layer(self, event): """When a layer is removed, remove its parent.""" layer = event.item vispy_layer = self.layer_to_visual[layer] vispy_layer.node.transforms = ChainTransform() vispy_layer.node.parent = None del self.layer_to_visual[layer] def _reorder_layers(self, event): """When the list is reordered, propagate changes to draw order.""" for i, layer in enumerate(self.viewer.layers): vispy_layer = self.layer_to_visual[layer] vispy_layer.order = i self.canvas._draw_order.clear() self.canvas.update() def _update_camera(self): if self.viewer.dims.ndisplay == 3: # Set a 3D camera if not isinstance(self.view.camera, ArcballCamera): self.view.camera = ArcballCamera(name="ArcballCamera", fov=0) # flip y-axis to have correct alignment # self.view.camera.flip = (0, 1, 0) self.view.camera.viewbox_key_event = viewbox_key_event self.viewer.reset_view() else: # Set 2D camera if not isinstance(self.view.camera, PanZoomCamera): self.view.camera = PanZoomCamera(aspect=1, name="PanZoomCamera") # flip y-axis to have correct alignment self.view.camera.flip = (0, 1, 0) self.view.camera.viewbox_key_event = viewbox_key_event self.viewer.reset_view() def screenshot(self): """Take currently displayed screen and convert to an image array. Returns ------- image : array Numpy array of type ubyte and shape (h, w, 4). Index [0, 0] is the upper-left corner of the rendered region. """ img = self.canvas.native.grabFramebuffer() return QImg2array(img) def _open_images(self): """Add image files from the menubar.""" filenames, _ = QFileDialog.getOpenFileNames( parent=self, caption='Select image(s)...', directory=self._last_visited_dir, # home dir by default ) if (filenames != []) and (filenames is not None): self._add_files(filenames) def _open_folder(self): """Add a folder of files from the menubar.""" folder = QFileDialog.getExistingDirectory( parent=self, caption='Select folder...', directory=self._last_visited_dir, # home dir by default ) if folder not in {'', None}: self._add_files([folder]) def _add_files(self, filenames): """Add an image layer to the viewer. If multiple images are selected, they are stacked along the 0th axis. Parameters ------- filenames : list List of filenames to be opened """ if len(filenames) > 0: self.viewer.add_image(path=filenames) self._last_visited_dir = os.path.dirname(filenames[0]) def _on_interactive(self, event): self.view.interactive = self.viewer.interactive def _on_cursor(self, event): cursor = self.viewer.cursor size = self.viewer.cursor_size if cursor == 'square': if size < 10 or size > 300: q_cursor = self._cursors['cross'] else: q_cursor = QCursor( QPixmap(':/icons/cursor/cursor_square.png').scaledToHeight( size)) else: q_cursor = self._cursors[cursor] self.canvas.native.setCursor(q_cursor) def _on_reset_view(self, event): if isinstance(self.view.camera, ArcballCamera): quat = self.view.camera._quaternion.create_from_axis_angle( *event.quaternion) self.view.camera._quaternion = quat self.view.camera.center = event.center self.view.camera.scale_factor = event.scale_factor else: # Assumes default camera has the same properties as PanZoomCamera self.view.camera.rect = event.rect def _update_palette(self, palette): # template and apply the primary stylesheet themed_stylesheet = template(self.raw_stylesheet, **palette) self.console.style_sheet = themed_stylesheet self.console.syntax_style = palette['syntax_style'] bracket_color = QtGui.QColor(*str_to_rgb(palette['highlight'])) self.console._bracket_matcher.format.setBackground(bracket_color) self.setStyleSheet(themed_stylesheet) self.aboutKeybindings.setStyleSheet(themed_stylesheet) self.canvas.bgcolor = palette['canvas'] def toggle_console(self): """Toggle console visible and not visible.""" viz = not self.dockConsole.isVisible() # modulate visibility at the dock widget level as console is docakable self.dockConsole.setVisible(viz) if self.dockConsole.isFloating(): self.dockConsole.setFloating(True) self.viewerButtons.consoleButton.setProperty( 'expanded', self.dockConsole.isVisible()) self.viewerButtons.consoleButton.style().unpolish( self.viewerButtons.consoleButton) self.viewerButtons.consoleButton.style().polish( self.viewerButtons.consoleButton) def on_mouse_press(self, event): """Called whenever mouse pressed in canvas. """ if event.pos is None: return event = ReadOnlyWrapper(event) mouse_press_callbacks(self.viewer, event) layer = self.viewer.active_layer if layer is not None: # Line bellow needed until layer mouse callbacks are refactored self.layer_to_visual[layer].on_mouse_press(event) mouse_press_callbacks(layer, event) def on_mouse_move(self, event): """Called whenever mouse moves over canvas. """ if event.pos is None: return mouse_move_callbacks(self.viewer, event) layer = self.viewer.active_layer if layer is not None: # Line bellow needed until layer mouse callbacks are refactored self.layer_to_visual[layer].on_mouse_move(event) mouse_move_callbacks(layer, event) def on_mouse_release(self, event): """Called whenever mouse released in canvas. """ mouse_release_callbacks(self.viewer, event) layer = self.viewer.active_layer if layer is not None: # Line bellow needed until layer mouse callbacks are refactored self.layer_to_visual[layer].on_mouse_release(event) mouse_release_callbacks(layer, event) def on_key_press(self, event): """Called whenever key pressed in canvas. """ if (event.native is not None and event.native.isAutoRepeat() and event.key.name not in ['Up', 'Down', 'Left', 'Right' ]) or event.key is None: # pass is no key is present or if key is held down, unless the # key being held down is one of the navigation keys return comb = components_to_key_combo(event.key.name, event.modifiers) layer = self.viewer.active_layer if layer is not None and comb in layer.keymap: parent = layer elif comb in self.viewer.keymap: parent = self.viewer else: return func = parent.keymap[comb] gen = func(parent) if inspect.isgeneratorfunction(func): try: next(gen) except StopIteration: # only one statement pass else: self._key_release_generators[event.key] = gen def on_key_release(self, event): """Called whenever key released in canvas. """ try: next(self._key_release_generators[event.key]) except (KeyError, StopIteration): pass def on_draw(self, event): """Called whenever drawn in canvas. Called for all layers, not just top """ for visual in self.layer_to_visual.values(): visual.on_draw(event) def keyPressEvent(self, event): self.canvas._backend._keyEvent(self.canvas.events.key_press, event) event.accept() def keyReleaseEvent(self, event): self.canvas._backend._keyEvent(self.canvas.events.key_release, event) event.accept() def dragEnterEvent(self, event): if event.mimeData().hasUrls(): event.accept() else: event.ignore() def dropEvent(self, event): """Add local files and web URLS with drag and drop.""" filenames = [] for url in event.mimeData().urls(): if url.isLocalFile(): filenames.append(url.toLocalFile()) else: filenames.append(url.toString()) self._add_files(filenames) def closeEvent(self, event): if self.pool.activeThreadCount() > 0: self.pool.clear() event.accept() def shutdown(self): self.pool.clear() self.canvas.close() self.console.shutdown()
class LaserScanVis: """Class that creates and handles a visualizer for a pointcloud""" def __init__(self, scan, scan_names, label_names, offset=0, semantics=True, bboxes_names=None, use_bbox_measurements=False, bboxes_labels_names=None, roi_filter=False, instances=False): self.scan = scan self.scan_names = scan_names self.label_names = label_names self.offset = offset self.semantics = semantics self.bboxes_names = bboxes_names self.use_bbox_measurements = use_bbox_measurements self.bboxes_labels_names = bboxes_labels_names self.roi_filter = roi_filter self.instances = instances # sanity check if not self.semantics and self.instances: print("Instances are only allowed in when semantics=True") raise ValueError self.reset() self.update_scan() def reset(self): """ Reset. """ # last key press (it should have a mutex, but visualization is not # safety critical, so let's do things wrong) self.action = "no" # no, next, back, quit are the possibilities # new canvas prepared for visualizing data self.canvas = SceneCanvas(keys='interactive', show=True) # interface (n next, b back, q quit, very simple) self.canvas.events.key_press.connect(self.key_press) self.canvas.events.draw.connect(self.draw) # grid self.grid = self.canvas.central_widget.add_grid() # laserscan part self.scan_view = vispy.scene.widgets.ViewBox(border_color='white', parent=self.canvas.scene) self.grid.add_widget(self.scan_view, 0, 0) self.scan_vis = visuals.Markers() self.scan_view.camera = 'turntable' self.scan_view.add(self.scan_vis) visuals.XYZAxis(parent=self.scan_view.scene) # add semantics if self.semantics: print("Using semantics in visualizer") self.sem_view = vispy.scene.widgets.ViewBox( border_color='white', parent=self.canvas.scene) self.grid.add_widget(self.sem_view, 0, 1) self.sem_vis = visuals.Markers() self.sem_view.camera = 'turntable' self.sem_view.add(self.sem_vis) visuals.XYZAxis(parent=self.sem_view.scene) self.sem_view.camera.link(self.scan_view.camera) if self.instances: print("Using instances in visualizer") self.inst_view = vispy.scene.widgets.ViewBox( border_color='white', parent=self.canvas.scene) self.grid.add_widget(self.inst_view, 0, 2) self.inst_vis = visuals.Markers() self.inst_view.camera = 'turntable' self.inst_view.add(self.inst_vis) visuals.XYZAxis(parent=self.inst_view.scene) # self.inst_view.camera.link(self.scan_view.camera) # img canvas size self.multiplier = 1 self.canvas_W = 1024 self.canvas_H = 64 if self.semantics: self.multiplier += 1 if self.instances: self.multiplier += 1 # new canvas for img self.img_canvas = SceneCanvas(keys='interactive', show=True, size=(self.canvas_W, self.canvas_H * self.multiplier)) # grid self.img_grid = self.img_canvas.central_widget.add_grid() # interface (n next, b back, q quit, very simple) self.img_canvas.events.key_press.connect(self.key_press) self.img_canvas.events.draw.connect(self.draw) # add a view for the depth self.img_view = vispy.scene.widgets.ViewBox( border_color='white', parent=self.img_canvas.scene) self.img_grid.add_widget(self.img_view, 0, 0) self.img_vis = visuals.Image(cmap='viridis') self.img_view.add(self.img_vis) # add semantics if self.semantics: self.sem_img_view = vispy.scene.widgets.ViewBox( border_color='white', parent=self.img_canvas.scene) self.img_grid.add_widget(self.sem_img_view, 1, 0) self.sem_img_vis = visuals.Image(cmap='viridis') self.sem_img_view.add(self.sem_img_vis) # add instances if self.instances: self.inst_img_view = vispy.scene.widgets.ViewBox( border_color='white', parent=self.img_canvas.scene) self.img_grid.add_widget(self.inst_img_view, 2, 0) self.inst_img_vis = visuals.Image(cmap='viridis') self.inst_img_view.add(self.inst_img_vis) def roi_filter_(self, pointcloud, colors, x_roi, y_roi, z_roi): min_x, max_x = x_roi min_y, max_y = y_roi min_z, max_z = z_roi for pcloud, i in zip(self.scan.points, range(len(self.scan.sem_label_color))): if ((pcloud[0] > 0) & (pcloud[0] < max_x) & (pcloud[1] > min_y) & (pcloud[1] < max_y) & (pcloud[2] > min_z) & (pcloud[2] < max_z)): pointcloud.append(np.array(pcloud)) colors.append(np.array(self.scan.sem_label_color[i])) else: pointcloud.append(np.array(pcloud)) colors.append(np.array((0.5, 0.5, 0.5))) def get_mpl_colormap(self, cmap_name): cmap = plt.get_cmap(cmap_name) # Initialize the matplotlib color map sm = plt.cm.ScalarMappable(cmap=cmap) # Obtain linear color range color_range = sm.to_rgba(np.linspace(0, 1, 256), bytes=True)[:, 2::-1] return color_range.reshape(256, 3).astype(np.float32) / 255.0 def update_scan(self): # first open data self.scan.open_scan(self.scan_names[self.offset]) if self.semantics: self.scan.open_label(self.label_names[self.offset]) self.scan.colorize() if self.bboxes_names: self.scan.open_bbox(self.bboxes_names[self.offset], self.use_bbox_measurements) if self.bboxes_labels_names: self.scan.open_bbox_labels(self.bboxes_labels_names[self.offset]) # then change names title = "scan " + str(self.offset) + " of " + str(len(self.scan_names)) self.canvas.title = title self.img_canvas.title = title # then do all the point cloud stuff # plot scan power = 16 # print() range_data = np.copy(self.scan.unproj_range) # print(range_data.max(), range_data.min()) range_data = range_data**(1 / power) # print(range_data.max(), range_data.min()) viridis_range = ((range_data - range_data.min()) / (range_data.max() - range_data.min()) * 255).astype( np.uint8) viridis_map = self.get_mpl_colormap("viridis") viridis_colors = viridis_map[viridis_range] self.scan_vis.set_data(self.scan.points, face_color=viridis_colors[..., ::-1], edge_color=viridis_colors[..., ::-1], size=1) # plot semantics if self.semantics: colors = [] pointcloud = [] if self.roi_filter: self.roi_filter_(pointcloud, colors, [0, 45], [-14, 14], [-2, 1]) else: for pcloud, i in zip(self.scan.points, range(len(self.scan.sem_label_color))): pointcloud.append(np.array(pcloud)) colors.append(np.array(self.scan.sem_label_color[i])) self.sem_view.add(self.sem_vis) self.sem_vis.set_data(np.array(pointcloud), face_color=np.array(colors), edge_color=np.array(colors), size=1) # plot instances if self.instances: self.inst_vis.set_data( self.scan.points, face_color=self.scan.inst_label_color[..., ::-1], edge_color=self.scan.inst_label_color[..., ::-1], size=1) # plot draw_clusters if self.bboxes_names and self.scan.bboxes: self.sem_vis.set_data() self.sem_vis.update() color = (0, 1, 1, 0.6) edge_color = (0, 0.05, 1) global bboxes, labels bboxes = [] labels = [] for bbox in self.scan.bboxes: width = bbox[0] depth = bbox[1] height = bbox[2] bboxes.append( vispy.scene.visuals.Box(width=width, height=height, depth=depth, color=color, edge_color=edge_color, parent=self.sem_view.scene)) for cluster, i in zip(bboxes, range(len(self.scan.bboxes))): bbox = self.scan.bboxes[i] center = bbox[3] angle = bbox[4] cluster.transform = vispy.visuals.transforms.MatrixTransform() cluster.transform.rotate(-angle, (0, 0, 1)) cluster.transform.translate(center) if self.bboxes_labels_names: for i in range(len(self.scan.bbox_labels)): bbox = self.scan.bboxes[i] center = bbox[3] #labels.append(vispy.scene.visuals.Text(text = self.scan.bbox_labels[i], parent = self.sem_view.scene, color = self.scan.bbox_label_color[i], bold=True)) labels.append( vispy.scene.visuals.Text(text=self.scan.bbox_labels[i], parent=self.sem_view.scene, color="red", bold=True)) labels[i].pos = center[0], center[1], center[2] + 1 labels[i].font_size = 600 # now do all the range image stuff # plot range image data = np.copy(self.scan.proj_range) # print(data[data > 0].max(), data[data > 0].min()) data[data > 0] = data[data > 0]**(1 / power) data[data < 0] = data[data > 0].min() # print(data.max(), data.min()) data = (data - data[data > 0].min()) / \ (data.max() - data[data > 0].min()) # print(data.max(), data.min()) self.img_vis.set_data(data) self.img_vis.update() if self.semantics: self.sem_img_vis.set_data(self.scan.proj_sem_color[..., ::-1]) self.sem_img_vis.update() if self.instances: self.inst_img_vis.set_data(self.scan.proj_inst_color[..., ::-1]) self.inst_img_vis.update() # interface def key_press(self, event): self.canvas.events.key_press.block() self.img_canvas.events.key_press.block() if event.key == 'N': for bbox in bboxes: bbox.parent = None for label in labels: label.parent = None self.offset += 1 self.update_scan() elif event.key == 'B': for bbox in bboxes: bbox.parent = None for label in labels: label.parent = None self.offset -= 1 self.update_scan() elif event.key == 'Q' or event.key == 'Escape': self.destroy() def draw(self, event): if self.canvas.events.key_press.blocked(): self.canvas.events.key_press.unblock() if self.img_canvas.events.key_press.blocked(): self.img_canvas.events.key_press.unblock() def destroy(self): # destroy the visualization self.canvas.close() self.img_canvas.close() vispy.app.quit() def run(self): vispy.app.run()
class ScannetVis(QWidget): """Class that creates and handles a visualizer for a pointcloud""" def __init__(self, scan, rgb_names, depth_names, pose_names, offset=0, skip_im=10, mesh_plot=True, parent=None): super(ScannetVis, self).__init__(parent=parent) self.scan = scan self.rgb_names = rgb_names self.depth_names = depth_names self.pose_names = pose_names self.offset = offset self.offset_prev = offset self.skip_im = skip_im self.mesh_plot = mesh_plot self.keyboard_inputs = None self.total = len(self.rgb_names) self.checkBox_list = [] self.checkBox_with_3D = [] self.reset() self.initUI() self.update_scan() def initUI(self): self.setStyleSheet("background-color: white;") self.principalLayout = QHBoxLayout(self) ''' left left Frame : RGB with yolact & depth frame ''' self.left2Frame = QFrame(self) self.left2Frame.setFrameShape(QFrame.StyledPanel) self.left2Frame.setFrameShadow(QFrame.Raised) self.vertical2Layout = QVBoxLayout(self.left2Frame) # self.vertical2Layout.setSpacing(0) self.principalLayout.addWidget(self.left2Frame) # self.vertical2_1Layout = QVBoxLayout(self.left2Frame) # self.vertical2Layout.addWidget(self.left2Frame) # add rgb depth self.img_canvas.create_native() self.img_canvas.native.setMinimumSize(320, 480) self.vertical2Layout.addWidget(self.img_canvas.native) ''' left Frame : 3D reconstructed Scene ''' self.leftFrame = QFrame(self) self.leftFrame.setFrameShape(QFrame.StyledPanel) self.leftFrame.setFrameShadow(QFrame.Raised) self.verticalLayout = QVBoxLayout(self.leftFrame) # self.verticalLayout.setSpacing(0) self.principalLayout.addWidget(self.leftFrame) self.canvas.create_native() self.canvas.native.setMinimumSize(640, 480) self.verticalLayout.addWidget(self.canvas.native) ''' left center Frame : 3D Scene graph''' self.SGFrame = QFrame(self) self.SGFrame.setFrameShape(QFrame.StyledPanel) self.SGFrame.setFrameShadow(QFrame.Raised) # self.verticalSGLayout = QVBoxLayout(self.SGFrame) # self.verticalLayout.setSpacing(0) self.principalLayout.addWidget(self.SGFrame) self.scene_graph_canvas.create_native() self.scene_graph_canvas.native.setMinimumSize(640, 480) self.verticalLayout.addWidget(self.scene_graph_canvas.native) ''' center Frame : control pannel ''' self.keyFrame = QFrame(self) self.keyFrame.setFrameShape(QFrame.StyledPanel) self.keyFrame.setFrameShadow(QFrame.Raised) self.keysverticalLayout = QVBoxLayout(self.keyFrame) self.label1 = QLabel( "To navigate: " "\n n: next (next scan) " "\n s: start (start processing sequential rgb-d images)" "\n p: pause (pause processing)" "\n q: quit (exit program)" "\n\n To control 3D view: " "\n LMB: orbits the view around its center point" "\n RMB or scroll: change scale_factor (i.e. zoom level)" "\n SHIFT + LMB: translate the center point" "\n SHIFT + RMB: change FOV") self.label2 = QLabel("To find specific objects in 3D Space : ") # self.keysverticalLayout.addWidget(self.label1) # self.keysverticalLayout.addWidget(self.label2) self.vertical2Layout.addWidget(self.label1) self.vertical2Layout.addWidget(self.label2) self.le = QLineEdit(self) self.vertical2Layout.addWidget(self.le) self.spb = QPushButton('search', self) self.vertical2Layout.addWidget(self.spb) self.spb.clicked.connect(self.search_button_click) self.cpb = QPushButton('clear', self) self.vertical2Layout.addWidget(self.cpb) self.cpb.clicked.connect(self.clear_button_click) self.verticalLayoutR = QVBoxLayout() self.verticalLayoutR.addWidget(self.keyFrame) self.verticalLayoutR.setContentsMargins(0, 0, 0, 0) self.verticalLayoutR.setSpacing(0) self.principalLayout.addLayout(self.verticalLayoutR) ''' Right Frame : result images of searched objects ''' self.rightFrame = QFrame(self) self.rightFrame.setFrameShape(QFrame.StyledPanel) self.rightFrame.setFrameShadow(QFrame.Raised) self.verticalLayoutRight = QVBoxLayout(self.rightFrame) self.verticalLayoutRight.setContentsMargins(0, 0, 0, 0) self.verticalLayoutRight.setSpacing(0) self.principalLayout.addWidget(self.rightFrame) self.setLayout(self.principalLayout) self.setWindowTitle('Searching objects') self.setGeometry(300, 300, 300, 200) self.show() def reset(self): """ Reset. """ # last key press (it should have a mutex, but visualization is not # safety critical, so let's do things wrong) self.action = "no" # no, next, back, quit are the possibilities ''' 3D points cloud or mesh SceneCanvas ''' # new canvas prepared for visualizing data self.canvas = SceneCanvas(keys='interactive', show=True) # interface (n next, b back, q quit, very simple) self.canvas.events.key_press.connect(self.key_press) self.canvas.events.draw.connect(self.draw) # grid self.grid = self.canvas.central_widget.add_grid() # add point cloud views self.scan_view = vispy.scene.widgets.ViewBox(border_color='white', parent=self.canvas.scene) self.grid.add_widget(self.scan_view, 0, 0) # Camera location settings self.scene_cam = vispy.scene.cameras.BaseCamera() # self.scene_cam.center = (-10, -10, 10) # self.scan_view.add(self.scene_cam) # self.scene_cam.pre_transform.set_range() canvas2 = vispy.app.Canvas() w = QMainWindow() widget = QWidget() w.setCentralWidget(widget) widget.setLayout(QVBoxLayout()) widget.layout().addWidget(canvas2.native) widget.layout().addWidget(QPushButton()) w.show() self.scan_vis = visuals.Mesh() self.scan_vis_mean = visuals.Line() self.scan_vis_cam = visuals.Line() self.scan_bbox_3d = visuals.Line() self.label_vis = visuals.Text() self.scan_view.add(self.scan_vis) self.scan_view.add(self.scan_vis_mean) self.scan_view.add(self.scan_vis_cam) self.scan_view.add(self.scan_bbox_3d) self.scan_view.add(self.label_vis) self.scan_view.camera = 'arcball' self.tr = self.scan_vis.transforms.get_transform(map_from='visual', map_to='canvas') # self.scan_view.camera = self.scene_cam # self.scan_view.camera = 'arcball' , 'turntable' # self.scan_view.camera.transform.rotate(90, (0,1,0)) visuals.XYZAxis(parent=self.scan_view.scene) ''' 2D images SceneCanvas ''' # img canvas size self.canvas_W = 320 self.canvas_H = 280 self.multiplier = 2 ''' new canvas for RGB & Depth img ''' self.img_canvas = SceneCanvas(keys='interactive', show=True, size=(self.canvas_W, self.canvas_H * self.multiplier)) self.img_grid = self.img_canvas.central_widget.add_grid() # interface (n next, s start, p pause, q quit, ) self.img_canvas.events.key_press.connect(self.key_press) self.img_canvas.events.draw.connect(self.draw) # add rgb views self.rgb_img_raw_view = vispy.scene.widgets.ViewBox( border_color='white', parent=self.img_canvas.scene) self.img_grid.add_widget(self.rgb_img_raw_view, 0, 0) self.rgb_img_raw_vis = visuals.Image(cmap='viridis') self.rgb_img_raw_view.add(self.rgb_img_raw_vis) # add a view for the depth self.depth_img_view = vispy.scene.widgets.ViewBox( border_color='white', parent=self.img_canvas.scene) self.img_grid.add_widget(self.depth_img_view, 1, 0) self.depth_img_vis = visuals.Image(cmap='viridis') self.depth_img_view.add(self.depth_img_vis) ''' new canvas for 3D scene graph img ''' self.scene_graph_canvas = SceneCanvas(keys='interactive', show=True, size=(640, 480)) self.scene_graph_grid = self.scene_graph_canvas.central_widget.add_grid( ) self.scene_graph_canvas.events.key_press.connect(self.key_press) self.scene_graph_canvas.events.draw.connect(self.draw) # add a view for 3D scene graphs self.scene_graph_view = vispy.scene.widgets.ViewBox( border_color='white', parent=self.scene_graph_canvas.scene) self.scene_graph_grid.add_widget(self.scene_graph_view, 0, 0) self.scene_graph_vis = visuals.Image(cmap='viridis') self.scene_graph_view.add(self.scene_graph_vis) def get_mpl_colormap(self, cmap_name): cmap = plt.get_cmap(cmap_name) # Initialize the matplotlib color map sm = plt.cm.ScalarMappable(cmap=cmap) # Obtain linear color range color_range = sm.to_rgba(np.linspace(0, 1, 256), bytes=True)[:, 2::-1] return color_range.reshape(256, 3).astype(np.float32) / 255.0 def update_yolact(self): title = "scan " + str(self.offset) # draw color & depth image self.img_canvas.title = title _, _, _, _ = self.scan.open_scan(self.rgb_names[self.offset], self.depth_names[self.offset], self.pose_names[self.offset], self.offset, recon=False) text_str = 'Frame %d ' % (self.offset) font_face = cv2.FONT_HERSHEY_DUPLEX font_scale = 0.6 font_thickness = 1 text_w, text_h = cv2.getTextSize(text_str, font_face, font_scale, font_thickness)[0] masked_img = self.scan.masked_img.copy() masked_img = cv2.resize(masked_img, (320, 240), interpolation=cv2.INTER_AREA) x1, y1 = 0, 0 text_pt = (x1, y1 + 15) text_color = [255, 255, 255] color = [0, 0, 0] cv2.rectangle(masked_img, (x1, y1), (x1 + text_w, y1 + text_h + 4), color, -1) cv2.putText(masked_img, text_str, text_pt, font_face, font_scale, text_color, font_thickness, cv2.LINE_AA) self.rgb_img_raw_vis.set_data(masked_img) self.rgb_img_raw_vis.update() depth_img = cv2.resize(self.scan.depth_im.copy(), (320, 240), interpolation=cv2.INTER_AREA) self.depth_img_vis.set_data(depth_img) self.depth_img_vis.update() def update_3d_recon(self): title = "scan " + str(self.offset) if (self.offset % self.skip_im == 0): start_time = time.time() verts, faces, norms, colors = self.scan.open_scan( self.rgb_names[self.offset], self.depth_names[self.offset], self.pose_names[self.offset], self.offset, recon=True) self.verts, self.faces, self.norms, self.colors = verts, faces, norms, colors self.canvas.title = title self.scan_vis.set_data(vertices=verts, faces=faces, vertex_colors=colors / 255.) self.scan_vis.update() #if self.scan.num_dets_to_consider > 0 and not self.scan.use_gpu: if self.scan.num_dets_to_consider > 0 and self.scan.tsdf_vol.debug_same_node_detector: self.mean_pose = np.array(self.scan.tsdf_vol.mask_centers) self.scan_vis_mean.set_data(self.mean_pose, color='red', width=3, connect='strip') self.scan_vis_mean.update() # find object's position and visualize self.label_vis.text = self.scan.tsdf_vol.class_label self.label_vis.pos = self.mean_pose self.label_vis.font_size = int(40) self.cam_frustum = np.array(self.scan.tsdf_vol.cam_frustum) self.scan_vis_cam.set_data(self.cam_frustum, color='blue', width=3, connect=self.scan.tsdf_vol.cam_connect) self.scan_vis_cam.update() if ('camera' in self.label_vis.text): self.label_vis.text.pop() self.label_vis.pos = self.label_vis.pos[:-1, :] self.label_vis.text += self.scan.tsdf_vol.cam_label self.label_vis.pos = np.append(self.label_vis.pos, self.scan.tsdf_vol.cam_centers, axis=0) # Draw Scene graph images generated_scene_graph_file = os.path.join( self.scan.tsdf_vol.scene_graph_path, 'scene_graph' + str(self.offset) + '.png') if os.path.exists(generated_scene_graph_file): print('Draw scene graph{}'.format(self.offset)) sg_img = cv2.cvtColor(cv2.imread(generated_scene_graph_file), cv2.COLOR_BGR2RGB) self.sg_img = cv2.resize(sg_img, (640, 480), interpolation=cv2.INTER_AREA) self.scene_graph_vis.set_data(self.sg_img) self.scene_graph_vis.update() print("--- %s seconds of %d to %d images---" % (time.time() - start_time, self.offset - self.skip_im + 1, self.offset)) print("--- fps : {} ---".format(self.skip_im / (time.time() - start_time))) def update_scan(self): # update_yolact images self.update_yolact() # Reconstruct 3D Scene and detect same nodes or not self.update_3d_recon() def update_seq_scan(self): if self.canvas.events.key_press.blocked(): self.canvas.events.key_press.unblock() if self.img_canvas.events.key_press.blocked(): self.img_canvas.events.key_press.unblock() if self.scene_graph_canvas.events.key_press.blocked(): self.scene_graph_canvas.events.key_press.unblock() if (self.start): self.offset += 1 self.update_yolact() self.update_3d_recon() self.canvas.scene.update() self.img_canvas.scene.update() self.scene_graph_canvas.update() self.canvas.on_draw(None) self.img_canvas.on_draw(None) self.scene_graph_canvas.on_draw(None) # interface def key_press(self, event): self.keyboard_inputs = event.key if event.key == 'N': self.offset += 1 if self.offset >= self.total: self.offset = 0 self.update_scan() elif event.key == 'S': # Start to process RGB-D sequences self.start = True self.timer1 = vispy.app.Timer(0.033, connect=self.on_timer1, start=True) self.timer2 = vispy.app.Timer(0.033, connect=self.on_timer2, start=True) elif event.key == 'P': # Pause to process RGB sequences self.start = False elif event.key == 'U': # test when updated draw function self.canvas.scene.update() self.img_canvas.scene.update() self.scene_graph_canvas.update() elif event.key == 'Q' or event.key == 'Escape': self.destroy() def on_timer1(self, event): # self.update_seq_scan() if (self.start): self.offset += 1 self.update_yolact() def on_timer2(self, event): if (self.start): # self.offset += 1 self.update_3d_recon() def search_button_click(self): print('searching object : {}'.format(self.le.text())) objects_dict = self.scan.tsdf_vol.node_data is_obj_exist = [] self.clear_searched_items(self.verticalLayoutRight) for key, val in objects_dict.items(): if (val['class'] == self.le.text()): print('find {}'.format(self.le.text())) thumbnail_path = os.path.join( self.scan.tsdf_vol.bbox_path, 'thumbnail_' + str(key) + '_' + str(int(objects_dict[str(key)]['detection_cnt'] / 2)) + '.png') cv2_img = cv2.cvtColor(cv2.imread(thumbnail_path), cv2.COLOR_BGR2RGB) image = QImage(cv2_img.data, cv2_img.shape[1], cv2_img.shape[0], cv2_img.strides[0], QImage.Format_RGB888) image_frame = QLabel() image_frame.setPixmap(QPixmap.fromImage(image)) self.verticalLayoutRight.addWidget(image_frame) checkBox = QCheckBox(val['class'] + str(key)) self.checkBox_list += [[checkBox, val['class'], str(key)]] scan_bbox_3d = visuals.Line() self.checkBox_with_3D += [scan_bbox_3d] self.scan_view.add(scan_bbox_3d) checkBox.stateChanged.connect(self.checkBoxState) # searched_obj = QLabel(val['class'] + str(key)) self.verticalLayoutRight.addWidget(checkBox) is_obj_exist += [True] if (not is_obj_exist): searched_obj = QLabel("Nothing was found!") self.verticalLayoutRight.addWidget(searched_obj) else: searched_obj = QLabel( "Check box if you want to find objects in 3D Scene.") self.verticalLayoutRight.addWidget(searched_obj) def clear_button_click(self): print('clear previous searched object') self.clear_searched_items(self.verticalLayoutRight) def clear_searched_items(self, layout): # reset rearching results widget while layout.count() > 0: item = layout.takeAt(0) if not item: continue w = item.widget() if w: w.deleteLater() # reset visuals.Line for 3D BBox of searched objects for i, check in enumerate(self.checkBox_list): self.checkBox_with_3D[i].parent = None self.checkBox_with_3D[i] = visuals.Line() self.scan_view.add(self.checkBox_with_3D[i]) self.checkBox_list = [] self.checkBox_with_3D = [] def checkBoxState(self): # checkBox_list is composed of [QcheckBox, class_name, class_3D_ID] for i, check in enumerate(self.checkBox_list): if check[0].isChecked() == True: print('checked!!!') # Find 3D BBox in 3D Scene Canvas\ bbox_3d = np.array(self.scan.tsdf_vol.bbox_3ds[check[2]]) bbox_connect = np.array([[0, 1], [1, 2], [2, 3], [3, 0], [4, 5], [5, 6], [6, 7], [7, 4], [0, 4], [1, 5], [2, 6], [3, 7]]) self.checkBox_with_3D[i].set_data(bbox_3d, color='green', width=3, connect=bbox_connect) else: self.checkBox_with_3D[i].parent = None self.checkBox_with_3D[i] = visuals.Line() self.scan_view.add(self.checkBox_with_3D[i]) def draw(self, event): # print('draw states!!') # print('event key: {}'.format(self.keyboard_inputs)) if self.canvas.events.key_press.blocked(): self.canvas.events.key_press.unblock() if self.img_canvas.events.key_press.blocked(): self.img_canvas.events.key_press.unblock() if self.scene_graph_canvas.events.key_press.blocked(): self.scene_graph_canvas.events.key_press.unblock() if self.keyboard_inputs == 'P': # Pause to process RGB sequences self.start = False # if self.keyboard_inputs == 'S': # self.update_seq_scan() def destroy(self): # destroy the visualization self.canvas.close() self.img_canvas.close() self.scene_graph_canvas.close() vispy.app.quit() def run(self): vispy.app.use_app(backend_name="PyQt5", call_reuse=True) vispy.app.run()
class LaserScanVis(): """Class that creates and handles a visualizer for a pointcloud""" def __init__(self, W, H, show_mesh=False, show_diff=False, show_range=False, show_remissions=False, show_target=True, show_label=True): self.W = W self.H = H self.point_size = 3 self.frame = 0 self.nframes = 0 self.view_mode = 'label' self.show_label = show_label self.show_target = show_target self.show_mesh = show_mesh self.show_remissions = show_remissions self.show_diff = show_diff self.show_range = show_range self.reset() def reset(self): """ Reset. """ # last key press (it should have a mutex, but visualization is not # safety critical, so let's do things wrong) self.action = "no" # no, next, back, quit are the possibilities # 3D canvas self.scan_canvas = SceneCanvas(keys='interactive', show=True, title='', size=(1600, 600), bgcolor='white') self.scan_canvas.events.key_press.connect(self.key_press) self.grid_view = self.scan_canvas.central_widget.add_grid() # source laserscan 3D self.scan_view = vispy.scene.widgets.ViewBox( border_color='white', parent=self.scan_canvas.scene) self.scan_vis = visuals.Markers() self.scan_view.camera = 'turntable' self.scan_view.add(self.scan_vis) visuals.XYZAxis(parent=self.scan_view.scene) self.grid_view.add_widget(self.scan_view, 0, 0) # target laserscan 3D if self.show_target is True: self.back_view = vispy.scene.widgets.ViewBox( border_color='white', parent=self.scan_canvas.scene) self.back_vis = visuals.Markers() self.back_view.camera = 'turntable' self.back_view.camera.link(self.scan_view.camera) self.back_view.add(self.back_vis) visuals.XYZAxis(parent=self.back_view.scene) self.grid_view.add_widget(self.back_view, 0, 1) # self.grid_view.padding = 6 # Set height of images h = 1 if self.show_range is True: h += 1 if self.show_remissions is True: h += 1 # source canvas 2D source_canvas_title = 'Source ' + str(self.H[0]) + 'x' + str(self.W[0]) self.source_canvas = SceneCanvas(keys='interactive', show=True, title=source_canvas_title, size=(self.W[0], h * self.H[0])) self.source_canvas.events.key_press.connect(self.key_press) self.source_view = self.source_canvas.central_widget.add_grid() source_grid_idx = 0 # Add label image if self.show_label: self.img_view = vispy.scene.widgets.ViewBox( border_color='white', parent=self.source_canvas.scene) self.img_vis = visuals.Image(cmap='viridis') self.img_view.add(self.img_vis) self.source_view.add_widget(self.img_view, source_grid_idx, 0) source_grid_idx += 1 # target canvas 2D if self.show_target: target_canvas_title = 'Target ' + str(self.H[1]) + 'x' + str( self.W[1]) self.target_canvas = SceneCanvas(keys='interactive', show=True, title=target_canvas_title, size=(self.W[1], h * self.H[1])) self.target_canvas.events.key_press.connect(self.key_press) self.target_view = self.target_canvas.central_widget.add_grid() target_grid_idx = 0 # Add label image if self.show_label: self.test_view = vispy.scene.widgets.ViewBox( border_color='white', parent=self.target_canvas.scene) self.test_vis = visuals.Image(cmap='viridis') self.test_view.add(self.test_vis) self.target_view.add_widget(self.test_view, target_grid_idx, 0) target_grid_idx += 1 if self.show_range: # Add source range image self.range_view_source = vispy.scene.widgets.ViewBox( border_color='white', parent=self.source_canvas.scene) self.range_image_source = visuals.Image() self.range_view_source.add(self.range_image_source) self.source_view.add_widget(self.range_view_source, source_grid_idx, 0) source_grid_idx += 1 if self.show_target: self.range_view_target = vispy.scene.widgets.ViewBox( border_color='white', parent=self.target_canvas.scene) self.range_image_target = visuals.Image(cmap='viridis') self.range_view_target.add(self.range_image_target) self.target_view.add_widget(self.range_view_target, target_grid_idx, 0) target_grid_idx += 1 if self.show_remissions: # Add source remissions image self.remissions_view_source = vispy.scene.widgets.ViewBox( border_color='white', parent=self.source_canvas.scene) self.remissions_image_source = visuals.Image() self.remissions_view_source.add(self.remissions_image_source) self.source_view.add_widget(self.remissions_view_source, source_grid_idx, 0) source_grid_idx += 1 # Add target remissions image if self.show_target: self.remissions_view_target = vispy.scene.widgets.ViewBox( border_color='white', parent=self.target_canvas.scene) self.remissions_image_target = visuals.Image(cmap='viridis') self.remissions_view_target.add(self.remissions_image_target) self.target_view.add_widget(self.remissions_view_target, target_grid_idx, 0) target_grid_idx += 1 # 2D canvas for showing difference in range, labels and remissions if self.show_diff: self.diff_canvas = SceneCanvas(keys='interactive', show=True, title='Difference Range Image', size=(self.W[1], self.H[1] * h)) self.diff_canvas.events.key_press.connect(self.key_press) self.diff_view = self.diff_canvas.central_widget.add_grid() grid_idx = 0 # Add label difference if self.show_label: self.diff_view_label = vispy.scene.widgets.ViewBox( border_color='white', parent=self.diff_canvas.scene) self.diff_image_label = visuals.Image(cmap='viridis') self.diff_view_label.add(self.diff_image_label) self.diff_view.add_widget(self.diff_view_label, grid_idx, 0) grid_idx += 1 # Add range difference if self.show_range: self.diff_view_depth = vispy.scene.widgets.ViewBox( border_color='white', parent=self.diff_canvas.scene) self.diff_image_depth = visuals.Image() self.diff_view_depth.add(self.diff_image_depth) self.diff_view.add_widget(self.diff_view_depth, grid_idx, 0) grid_idx += 1 # Add remissions difference if self.show_remissions: self.diff_view_remissions = vispy.scene.widgets.ViewBox( border_color='white', parent=self.diff_canvas.scene) self.diff_image_remissions = visuals.Image() self.diff_view_remissions.add(self.diff_image_remissions) self.diff_view.add_widget(self.diff_view_remissions, grid_idx, 0) grid_idx += 1 if self.show_mesh: self.mesh_view = vispy.scene.widgets.ViewBox( border_color='white', parent=self.scan_canvas.scene) self.mesh_vis = visuals.Mesh(shading=None) self.mesh_view.camera = 'turntable' self.mesh_view.camera.link(self.scan_view.camera) self.mesh_view.add(self.mesh_vis) visuals.XYZAxis(parent=self.mesh_view.scene) self.grid_view.add_widget(self.mesh_view, 0, 2) def set_title(self): self.scan_canvas.title = 'Frame %d of %d' % (self.frame + 1, self.nframes) def set_source_scan(self, scan): """ Set single raw scan (2D only) """ # plot 2D images if self.show_label: self.img_vis.set_data(scan.proj_color[..., ::-1]) self.img_vis.update() if self.show_range: data = convert_range(scan.proj_range) self.range_image_source.set_data(data) if self.show_remissions: data = scan.proj_remissions # data = convert_range(scan.proj_remissions) self.remissions_image_source.set_data(data) def set_data(self, scan_source, scan_target, verts=None, verts_colors=None, faces=None, W=None, H=None): """ Set both source and target scans (2D and mesh in 3D) """ self.set_title() if self.show_target: self.set_target_3d(scan_target) if self.show_label: self.img_vis.set_data(scan_source.proj_color[..., ::-1]) if self.show_target: self.test_vis.set_data(scan_target.proj_color[..., ::-1]) if self.show_range: source_range = scan_source.proj_range # print("source", source_range.max(), source_range.min(), # source_range[source_range>=0].mean()) # source_range = self.convert_ranges(scan_source.proj_range) self.range_image_source.set_data(source_range) self.range_image_source.update() if self.show_target: target_range = scan_target.proj_range # target_data = self.convert_range(target_range) # print("target", target_data.max(), target_data.min(), # target_data.mean()) target_data = target_range self.range_image_target.set_data(target_data) self.range_image_target.update() if self.show_remissions: source_data = np.copy(scan_source.proj_remissions) source_data[source_data == -1] = 0 # print("source", source_data.max(), source_data.min(), # source_data[source_data >= 0].mean()) self.remissions_image_source.set_data(source_data) self.remissions_image_source.update() if self.show_target: target_data = scan_target.proj_remissions # print("source", target_data.max(), target_data.min(), # target_data[target_data >= 0].mean()) self.remissions_image_target.set_data(target_data) self.remissions_image_target.update() if self.show_mesh: self.mesh_vis.set_data(vertices=verts, vertex_colors=verts_colors[..., ::-1], faces=faces) self.mesh_vis.update() def set_diff(self, label_diff, range_diff, remissions_diff, m_iou, m_acc, MSE): """ Update difference images """ if self.show_label: self.diff_image_label.set_data(label_diff[..., ::-1]) self.diff_image_label.update() if self.show_range: data = convert_range(range_diff) self.diff_image_depth.set_data(data) self.diff_image_depth.set_data(range_diff) self.diff_image_depth.update() if self.show_remissions: self.diff_image_remissions.set_data(remissions_diff) self.diff_image_remissions.update() self.diff_canvas.title = \ 'IoU %5.2f%%, Acc %5.2f%%, MSE %f' % (m_iou * 100.0, m_acc * 100, MSE) def set_mesh(self, verts, verts_colors, faces): if self.show_mesh: self.mesh_vis.set_data(vertices=verts, vertex_colors=verts_colors, faces=faces) self.mesh_vis.update() def set_source_3d(self, scan_source): points = scan_source.points if self.view_mode == 'label': colors = scan_source.label_color else: if self.view_mode == 'range': range_data = np.copy(scan_source.unproj_range.reshape(-1)) power = 2 range_data = range_data**(1 / power) viridis_range = ((range_data - range_data.min()) / (range_data.max() - range_data.min()) * 255).astype(np.uint8) elif self.view_mode == 'rem': range_data = np.copy(scan_source.remissions.reshape(-1)) viridis_range = (range_data * 255).astype(np.uint8) viridis_map = get_mpl_colormap("viridis") colors = viridis_map[viridis_range] self.scan_vis.set_data(points, face_color=colors[..., ::-1], edge_color=colors[..., ::-1], size=self.point_size) self.scan_vis.update() def set_target_3d(self, scan_target): points = scan_target.back_points if self.view_mode == 'label': colors = scan_target.label_color else: if self.view_mode == 'range': range_data = np.copy(scan_target.proj_range.reshape(-1)) power = 2 range_data = range_data**(1 / power) viridis_range = ((range_data - range_data.min()) / (range_data.max() - range_data.min()) * 255).astype(np.uint8) elif self.view_mode == 'rem': range_data = np.copy(scan_target.proj_remissions.reshape(-1)) viridis_range = (range_data * 255).astype(np.uint8) viridis_map = get_mpl_colormap("viridis") colors = viridis_map[viridis_range] self.back_vis.set_data(points, face_color=colors[..., ::-1], edge_color=colors[..., ::-1], size=self.point_size) self.back_vis.update() # interface def key_press(self, event): if event.key == 'N': self.action = 'next' elif event.key == 'B': self.action = 'back' elif event.key == 'Q' or event.key == 'Escape': self.destroy() self.action = 'quit' elif event.key == '1': self.action = 'change' self.view_mode = 'label' elif event.key == '2': self.action = 'change' self.view_mode = 'range' elif event.key == '3': self.action = 'change' self.view_mode = 'rem' def get_action(self, timeout=0): # return action and void it to avoid reentry vispy.app.use_app().sleep(timeout) ret = self.action self.action = 'no' return ret def destroy(self): # destroy the visualization self.source_canvas.events.key_press.disconnect() self.source_canvas.close() if self.show_target: self.target_canvas.events.key_press.disconnect() self.target_canvas.close() if self.show_diff: self.diff_canvas.events.key_press.disconnect() self.diff_canvas.close() vispy.app.quit()
class LaserScanExtract: """Class that creates and handles a visualizer for a pointcloud""" def __init__(self, scan, scan_names, label_names, offset=0, semantics=True, instances=False, classid=51): self.scan = scan self.scan_names = scan_names self.label_names = label_names self.offset = offset self.total = len(self.scan_names) self.semantics = semantics self.instances = instances self.classid = classid # sanity check if not self.semantics and self.instances: print("Instances are only allowed in when semantics=True") raise ValueError # self.reset() self.update_scan() def reset(self): """ Reset. """ # last key press (it should have a mutex, but visualization is not # safety critical, so let's do things wrong) self.action = "no" # no, next, back, quit are the possibilities # new canvas prepared for visualizing data self.canvas = SceneCanvas(keys='interactive', show=True) # interface (n next, b back, q quit, very simple) self.canvas.events.key_press.connect(self.key_press) self.canvas.events.draw.connect(self.draw) # grid self.grid = self.canvas.central_widget.add_grid() # laserscan part self.scan_view = vispy.scene.widgets.ViewBox(border_color='white', parent=self.canvas.scene) self.grid.add_widget(self.scan_view, 0, 0) self.scan_vis = visuals.Markers() self.scan_view.camera = 'turntable' self.scan_view.add(self.scan_vis) visuals.XYZAxis(parent=self.scan_view.scene) # add semantics if self.semantics: print("Using semantics in visualizer") self.sem_view = vispy.scene.widgets.ViewBox( border_color='white', parent=self.canvas.scene) self.grid.add_widget(self.sem_view, 0, 1) self.sem_vis = visuals.Markers() self.sem_view.camera = 'turntable' self.sem_view.add(self.sem_vis) visuals.XYZAxis(parent=self.sem_view.scene) # self.sem_view.camera.link(self.scan_view.camera) if self.instances: print("Using instances in visualizer") self.inst_view = vispy.scene.widgets.ViewBox( border_color='white', parent=self.canvas.scene) self.grid.add_widget(self.inst_view, 0, 2) self.inst_vis = visuals.Markers() self.inst_view.camera = 'turntable' self.inst_view.add(self.inst_vis) visuals.XYZAxis(parent=self.inst_view.scene) # self.inst_view.camera.link(self.scan_view.camera) # img canvas size self.multiplier = 1 self.canvas_W = 1024 self.canvas_H = 64 if self.semantics: self.multiplier += 1 if self.instances: self.multiplier += 1 # new canvas for img self.img_canvas = SceneCanvas(keys='interactive', show=True, size=(self.canvas_W, self.canvas_H * self.multiplier)) # grid self.img_grid = self.img_canvas.central_widget.add_grid() # interface (n next, b back, q quit, very simple) self.img_canvas.events.key_press.connect(self.key_press) self.img_canvas.events.draw.connect(self.draw) # add a view for the depth self.img_view = vispy.scene.widgets.ViewBox( border_color='white', parent=self.img_canvas.scene) self.img_grid.add_widget(self.img_view, 0, 0) self.img_vis = visuals.Image(cmap='viridis') self.img_view.add(self.img_vis) # add semantics if self.semantics: self.sem_img_view = vispy.scene.widgets.ViewBox( border_color='white', parent=self.img_canvas.scene) self.img_grid.add_widget(self.sem_img_view, 1, 0) self.sem_img_vis = visuals.Image(cmap='viridis') self.sem_img_view.add(self.sem_img_vis) # add instances if self.instances: self.inst_img_view = vispy.scene.widgets.ViewBox( border_color='white', parent=self.img_canvas.scene) self.img_grid.add_widget(self.inst_img_view, 2, 0) self.inst_img_vis = visuals.Image(cmap='viridis') self.inst_img_view.add(self.inst_img_vis) def get_mpl_colormap(self, cmap_name): cmap = plt.get_cmap(cmap_name) # Initialize the matplotlib color map sm = plt.cm.ScalarMappable(cmap=cmap) # Obtain linear color range color_range = sm.to_rgba(np.linspace(0, 1, 256), bytes=True)[:, 2::-1] return color_range.reshape(256, 3).astype(np.float32) / 255.0 def update_scan(self): count = 0 for offset, name in enumerate(self.scan_names): self.scan.open_scan(self.scan_names[offset]) if self.semantics: self.scan.open_label(self.label_names[offset]) ind = np.where(self.scan.sem_label == self.classid) if ind[0].size > 50: print(self.scan_names[offset], ind[0].size) cluster = np.hstack( (self.scan.points[ind[0], :], np.expand_dims(self.scan.remissions[ind[0]], axis=1))) np.savetxt( "cluster" + str(ind[0].size) + "_" + str(count) + ".txt", cluster) count += 1 # interface def key_press(self, event): self.canvas.events.key_press.block() self.img_canvas.events.key_press.block() if event.key == 'N': self.offset += 1 if self.offset >= self.total: self.offset = 0 self.update_scan() elif event.key == 'B': self.offset -= 1 if self.offset < 0: self.offset = self.total - 1 self.update_scan() elif event.key == 'Q' or event.key == 'Escape': self.destroy() def draw(self, event): if self.canvas.events.key_press.blocked(): self.canvas.events.key_press.unblock() if self.img_canvas.events.key_press.blocked(): self.img_canvas.events.key_press.unblock() def destroy(self): # destroy the visualization self.canvas.close() self.img_canvas.close() vispy.app.quit() def run(self): vispy.app.run()
class SemanticKittiTool: """ Class that creates and handles point cloud data for other application""" def __init__(self, scan, scan_names, label_names, config, bbox_path, obj, offset=0, semantics=True, instances=False): self.scan = scan self.scan_names = scan_names self.label_names = label_names self.offset = offset self.config = config self.total = len(self.scan_names) self.semantics = semantics self.instances = instances self._labels_of_interest_name = yaml.load(open(obj)) self._labels_of_interest_num = self.GetLabelIdx( self._labels_of_interest_name) self.sizepoints = 1 self._bbox_path = bbox_path # path where bounding boxes will be stored self._scan_labels = dict([('inst', []), ('sem', [])]) # make instance colors max_inst_id = 100000 self.inst_color_lut = np.random.uniform(low=0.0, high=1.0, size=(max_inst_id, 3)) # force zero to a gray-ish color self.inst_color_lut[0] = np.full((3), 0.1) self.reset() def get_mpl_colormap(self, cmap_name): cmap = plt.get_cmap(cmap_name) # Initialize the matplotlib color map sm = plt.cm.ScalarMappable(cmap=cmap) # Obtain linear color range color_range = sm.to_rgba(np.linspace(0, 1, 256), bytes=True)[:, 2::-1] return color_range.reshape(256, 3).astype(np.float32) / 255.0 def GetLabelIdx(self, objects): config = yaml.load(open(self.config)) convlabel = config['labels'] objectlabels = list(objects.values()) key = [] for value in objectlabels[0]: key.append(self.getKeysByValue(convlabel, value)) return key def CreateAll3DBoundingBoxes(self): self.scan.reset() self.scan.open_scan(self.scan_names[self.offset]) self.scan.open_label(self.label_names[self.offset]) self.scan_labels = self.scan.sem_label self.scan_pts = self.scan.points # MergeColorFlag=1 - Merge all colors of all objects in same color frame # MergeColorFlag=0 - Create a color frame for each object bboxes, scan_color_all_obj = self.Create3DBoundingBoxes( self.scan_pts, self.scan_labels, MergeColorFlag=1) file_name = str(self.offset) + ".txt" self.Save3DBoundingBox(bboxes, file_name) # Plot objects & bounding boxes #shape = len(list(scan_color_all_obj)) shape = len(scan_color_all_obj.shape) #if shape==1: # Plot all objects with different colors (MergeColorFlag=1 ) # scan_pts,scan_labels = self.Color3DBoundingBox(bboxes,scan_color_all_obj) # self.colorizeObject(scan_labels) # self.PlotPcl(scan_pts) #else: # Plot all instances of each object class at a time # for scan_color in scan_color_all_obj: # scan_pts,scan_labels = self.Color3DBoundingBox(bboxes,scan_color) # self.colorizeObject(scan_color) # self.PlotPcl(scan_pts) self.colorize(self.scan_labels) self.PlotPcl(self.scan_pts) #self.SaveBoundingBoxes(boundingboxes) def Save3DBoundingBox(self, bboxes, filename): label_path = os.path.join(self._bbox_path, "labels_2") if os.path.isdir(label_path): true_label_path = label_path else: try: access_rights = 0x755 os.makedirs(label_path, access_rights) true_label_path = label_path except OSError: print("Creation of the directory %s failed" % label_path) else: print("Successfully created the directory %s " % label_path) file_path = os.path.join(true_label_path, filename) # full file path for bbox in bboxes.items(): kitti_labels = self.conv_to_kitti_format(bbox) kitti.write_label(kitti_labels, file_path) def conv_to_kitti_format(self, bbox): label = kitti.Object3d() labels = [] for obj in bbox[1]: objtype = bbox[0] w = obj['w'] h = obj['h'] l = obj['l'] rz = obj['rz'] t = obj['t'] score = 1 label.loadBox3D(objtype, h, w, l, t, rz, score) labels.append(label) return (labels) def Color3DBoundingBox(self, bboxes, colorframe=[]): #scan_labels = self.scan_labels scan_pts = self.scan_pts scan_labels = colorframe bb_pts = np.array([]) for classname, data in bboxes.items(): for idx in range(0, len(data)): vertices = np.asarray(data[idx]['bb']['vertices']) if (len(bb_pts[:]) == 0): bb_pts = vertices else: bb_pts = np.concatenate((bb_pts, vertices)) value = max(scan_labels) + 1 redcolor = np.ones(bb_pts.shape[0], dtype=int) * value scan_labels = np.concatenate((scan_labels, redcolor)) sizepoints = np.ones(self.scan_pts.shape[0], dtype=int) scan_pts = np.concatenate((scan_pts, bb_pts)) sizebb = np.ones(bb_pts.shape[0], dtype=int) * 4 self.sizepoints = np.concatenate((sizepoints, sizebb)) return (scan_pts, scan_labels) def Create3DBoundingBoxes(self, ScanPts, ScanLabels, MergeColorFlag=0): # Check if objects of interest exist in the labels # ... # Do it here # ... # A segment may contain more than one object from the same class object_segment_idx = self.SplitIntoObjectSegments(ScanLabels) boundingboxes, label_color_scan = self.ObjectClassClustering( ScanPts, object_segment_idx, MergeColorFlag) return (boundingboxes, label_color_scan) def SplitIntoObjectSegments(self, ScanLabels): labels_of_interest_num = self._labels_of_interest_num labels_of_interest_name = self._labels_of_interest_name scan_semantic_labels = ScanLabels pointsLabel = dict() for label_num in labels_of_interest_num: pts_idx = np.where(scan_semantic_labels == label_num)[0] if pts_idx.size > 0: index = labels_of_interest_num.index(label_num) class_name = list(labels_of_interest_name.values())[0][index] pointsLabel.update({class_name: pts_idx}) return pointsLabel def ObjectClassClustering(self, ScanPts, SegmentIdx, MergeColorFlag=0): # Create objects = dict() label_color_scan = [] for name, segment in SegmentIdx.items(): if (MergeColorFlag): bbox, single_class_objects, segment_scan_color = self.SegmentClustering( ScanPts, segment, label_color_scan) label_color_scan = segment_scan_color else: bbox, single_class_objects, segment_scan_color = self.SegmentClustering( ScanPts, segment, []) label_color_scan.append(segment_scan_color) objects.update({name: bbox}) return (objects, label_color_scan) def SegmentClustering(self, ScanPts, SegmentIdx, ColorScan=[]): # Split segment (with multiple objects of the same class) # in different instances (point clouds) # # Input: Segment with same objects # Output: dict with all instances (objects) if len(ColorScan) > 0: max_value = int(max(ColorScan)) + 1 segment_scan_color = ColorScan else: max_value = 1 segment_scan_color = np.zeros(ScanPts.shape[0], dtype=int) # Clustering segment_points = ScanPts[SegmentIdx] db = DBSCAN(eps=0.3, min_samples=5).fit(segment_points) # go through all clusters num_instances = int(max(db.labels_)) candidates = np.array(db.labels_) instance_boundle = [] bbox_list = [] for inst_label in range(0, num_instances): inst_index = np.where(candidates == inst_label) instance_index = SegmentIdx[inst_index] instance_points = segment_points[inst_index] segment_scan_color[instance_index] = int(max_value + inst_label) bb = self.Campute3DBoundingBox(instance_points) bbox_list.append(bb) instance = dict([('bb', bb), ('idx', instance_index)]) instance_boundle.append(instance) return (bbox_list, instance_boundle, segment_scan_color) def getKeysByValue(self, dictOfElements, valueToFind): listOfKeys = [] listOfItems = dictOfElements.items() for item in listOfItems: if item[1] == valueToFind: listOfKeys = item[0] return listOfKeys def Campute3DBoundingBox(self, points): # Transform to object referencial # translation t = np.mean(points, axis=0).reshape(3, 1) # compute object's mass center t[2] = 0 # object frame is mass center in the ground surface (as described kitti paper) pts = points.T - t # rotation # .... rz = 0 # Get 3D bounding box bounderies x = pts[0, :] y = pts[1, :] z = pts[2, :] xminbound = float(min(x)) xmaxbound = float(max(x)) yminbound = float(min(y)) ymaxbound = float(max(y)) zminbound = float(min(z)) zmaxbound = float(max(z)) height = np.abs(zmaxbound - zminbound) # height is z axis length = np.abs(xmaxbound - xminbound) # length width = np.abs(ymaxbound - yminbound) # width #Rot = r.as_matrix() obj = {'t': t, 'rz': rz, 'h': height, 'w': width, 'l': length} return (obj) def CreateTransf(self, R, T): row = np.zeros((1, 3), dtype=int) a = np.concatenate((R, row)) b = np.concatenate((T, np.array([1]).reshape(1, 1))) c = np.concatenate((a, b), axis=1) return (c) def rotationMatrixToEulerAngles(self, R): #assert(isRotationMatrix(R)) sy = math.sqrt(R[0, 0] * R[0, 0] + R[1, 0] * R[1, 0]) singular = sy < 1e-6 if not singular: x = math.atan2(R[2, 1], R[2, 2]) y = math.atan2(-R[2, 0], sy) z = math.atan2(R[1, 0], R[0, 0]) else: x = math.atan2(-R[1, 2], R[1, 1]) y = math.atan2(-R[2, 0], sy) z = 0 return np.array([x, y, z]) def reset(self): """ Reset. """ # last key press (it should have a mutex, but visualization is not # safety critical, so let's do things wrong) self.action = "no" # no, next, back, quit are the possibilities # new canvas prepared for visualizing data self.canvas = SceneCanvas(keys='interactive', show=True) # interface (n next, b back, q quit, very simple) self.canvas.events.key_press.connect(self.key_press) self.canvas.events.draw.connect(self.draw) # grid self.grid = self.canvas.central_widget.add_grid() #if self.instances: print("Using instances in visualizer") self.inst_view = vispy.scene.widgets.ViewBox(border_color='white', parent=self.canvas.scene) self.grid.add_widget(self.inst_view, 0, 0) self.inst_vis = visuals.Markers() self.inst_view.camera = 'turntable' self.inst_view.add(self.inst_vis) visuals.XYZAxis(parent=self.inst_view.scene) # self.inst_view.camera.link(self.scan_view.camera) def resetTwo(self): """ Reset. """ # last key press (it should have a mutex, but visualization is not # safety critical, so let's do things wrong) self.action = "no" # no, next, back, quit are the possibilities # new canvas prepared for visualizing data self.canvas = SceneCanvas(keys='interactive', show=True) # interface (n next, b back, q quit, very simple) self.canvas.events.key_press.connect(self.key_press) self.canvas.events.draw.connect(self.draw) # grid self.grid = self.canvas.central_widget.add_grid() # laserscan part self.scan_view = vispy.scene.widgets.ViewBox(border_color='white', parent=self.canvas.scene) self.grid.add_widget(self.scan_view, 0, 0) self.scan_vis = visuals.Markers() self.scan_view.camera = 'turntable' self.scan_view.add(self.scan_vis) visuals.XYZAxis(parent=self.scan_view.scene) #if self.instances: print("Using instances in visualizer") self.inst_view = vispy.scene.widgets.ViewBox(border_color='white', parent=self.canvas.scene) self.grid.add_widget(self.inst_view, 0, 1) self.inst_vis = visuals.Markers() self.inst_view.camera = 'turntable' self.inst_view.add(self.inst_vis) visuals.XYZAxis(parent=self.inst_view.scene) # self.inst_view.camera.link(self.scan_view.camera) def draw(self, event): if self.canvas.events.key_press.blocked(): self.canvas.events.key_press.unblock() # interface def key_press(self, event): self.canvas.events.key_press.block() if event.key == 'N': self.offset += 1 if self.offset >= self.total: self.offset = 0 self.CreateAll3DBoundingBoxes() elif event.key == 'B': self.offset -= 1 if self.offset < 0: self.offset = self.total - 1 self.CreateAll3DBoundingBoxes() elif event.key == 'Q' or event.key == 'Escape': self.destroy() def PlotPcl(self, points): self.inst_vis.set_data(points, face_color=self.inst_label_color[..., ::-1], edge_color=self.inst_label_color[..., ::-1], size=self.sizepoints) def PlotPclTwo(self, points): # Generate 3D bounding boxes of all Laser scans # plot scan power = 16 # print() range_data = np.copy(self.scan.unproj_range) # print(range_data.max(), range_data.min()) range_data = range_data**(1 / power) # print(range_data.max(), range_data.min()) viridis_range = ((range_data - range_data.min()) / (range_data.max() - range_data.min()) * 255).astype( np.uint8) viridis_map = self.get_mpl_colormap("viridis") viridis_colors = viridis_map[viridis_range] self.scan_vis.set_data(self.scan.points, face_color=viridis_colors[..., ::-1], edge_color=viridis_colors[..., ::-1], size=1) self.inst_vis.set_data(points, face_color=self.inst_label_color[..., ::-1], edge_color=self.inst_label_color[..., ::-1], size=self.sizepoints) def colorizeObject(self, ScanLabels): """ Colorize pointcloud with the color of each semantic label """ #shapescan = self.scan.points.shape #sem_label = self.scan_labels['sem'] #labels_of_interest_num = self._labels_of_interest_num #instlabel = self.scan_labels['inst'] self.inst_label_color = self.inst_color_lut[ScanLabels] self.inst_label_color = self.inst_label_color.reshape((-1, 3)) return def colorize(self, ScanLabels): """ Colorize pointcloud with the color of each semantic label """ shapescan = self.scan.points.shape sem_label = ScanLabels labels_of_interest_num = self._labels_of_interest_num #instlabel = self.scan_labels['inst'] new_scan_labels = np.zeros(shapescan[0], dtype=int) for labelnum in labels_of_interest_num: idx = np.where(sem_label == labelnum)[0] new_scan_labels[idx] = labelnum self.inst_label_color = self.inst_color_lut[new_scan_labels] self.inst_label_color = self.inst_label_color.reshape((-1, 3)) def destroy(self): # destroy the visualization self.canvas.close() vispy.app.quit() def run(self): vispy.app.run()