def update_gated(self, label_idx=None): path = self.get_path('gated') # @ TODO check topic mapping if 'gated_full_rect8' != self.gated_topic: topic_id = self.gated_topic.split('_')[0][-1] else: topic_id = '4' mapping ={ '0': '0', '1': '1', '2': '2', '4': 'full' } if topic_id not in mapping.keys(): timestamp = None else: timestamp = self.get_timestamp_from_data_name(topic='gated%s'%(mapping[topic_id])) if timestamp is not None: time_diff = get_time_difference(self.stereo_timestamp, timestamp) self.timeGatedEdit.setText('{} ms'.format(time_diff)) else: self.timeGatedEdit.setText('No timestamp found!') try: image = np.right_shift(cv2.imread(path, cv2.IMREAD_ANYCOLOR | cv2.IMREAD_ANYDEPTH), 2).astype(np.uint8) image_rgb = cv2.cvtColor(image, cv2.COLOR_GRAY2RGB) image = self.draw_3d_labels_in_rgb(image_rgb, topic_type='gated', label_idx=label_idx) self.displayImage(image, self.gatedLabel) except: print('The gated_full_topic has not been recorded by the sensor in this case!')
def update_lidar3d(self, label_idx=None): path = self.get_path('lidar3d') timestamp = self.get_timestamp_from_data_name(topic='lidar') if timestamp is not None: time_diff = get_time_difference(self.stereo_timestamp, timestamp) self.timeLidar3dEdit.setText('{} ms'.format(time_diff)) else: self.timeLidar3dEdit.setText('No timestamp found!') pc = np.fromfile(path, dtype=np.float32) try: pc = pc.reshape((-1, 5)) except Exception: pc = pc.reshape((-1, 4)) norm = mpl.colors.Normalize(vmin=3, vmax=80) cmap = cm.jet m = cm.ScalarMappable(norm=norm, cmap=cmap) if label_idx is None: label_idx = slice(len(self.labels_rgb)) colors = m.to_rgba(np.linalg.norm(pc[:, 0:3], axis=1)) colors[:, [2, 1, 0, 3]] = colors[:, [0, 1, 2, 3]] colors[:, 3] = 0.5 try: self.w.removeItem(self.plot) for box in self.boxes: self.w.removeItem(box) except Exception: pass self.boxes = [] size = QtGui.QVector3D(1, 1, 1) for objects in self.labels_rgb[label_idx]: box = gl.GLBoxItem(size, color=(255, 255, 255, 255)) box.setSize(objects['length'], objects['width'], objects['height']) box.translate(-objects['length']/2, -objects['width']/2, -objects['height']/2) box.rotate(angle=-objects['rotz'] * 180 / 3.14159265359, x=0, y=0, z=1) box.rotate(angle=-objects['roty'] * 180 / 3.14159265359, x=0, y=1, z=0) box.rotate(angle=-objects['rotx'] * 180 / 3.14159265359, x=1, y=0, z=0) box.translate(0, 0, objects['height']/2) box.translate(objects['posx_lidar'], objects['posy_lidar'], objects['posz_lidar']) self.boxes.append(box) self.plot = gl.GLScatterPlotItem(pos=np.asarray(pc[:, 0:3]), size=5.5, color=colors) self.w.addItem(self.plot) for box in self.boxes: self.w.addItem(box) self.lidar3dGridLayout.addWidget(self.w)
def update_lidar(self): path = self.get_path('lidar') timestamp = self.get_timestamp_from_data_name(topic='lidar') if timestamp is not None: time_diff = get_time_difference(self.stereo_timestamp, timestamp) self.timeLidarEdit.setText('{} ms'.format(time_diff)) else: self.timeLidarEdit.setText('No timestamp found!') depth = np.load(path)['arr_0'] image = colorize_pointcloud(depth) self.displayImage(image, self.lidarLabel)