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)