def quad(top_left=[0, 0, 0], bottom_left=[0, 1, 0], bottom_right=[1, 1, 0], top_right=None, texcoords=[[0, 0], [0, 1], [1, 1], [1, 0]], filled=False, color=QColor("blue"), effect_f=CustomEffects.material, textures=None, matrix=np.eye(4, dtype='f4'), name="quad"): color = ensure_QColor(color) top_left = utils.to_numpy(top_left) bottom_left = utils.to_numpy(bottom_left) bottom_right = utils.to_numpy(bottom_right) v_top = top_left - bottom_left v_right = bottom_right - bottom_left if top_right is None: top_right = bottom_right + v_top else: top_right = utils.to_numpy(top_right) indices = [0, 1, 2, 3, 0, 2 ] if filled else [0, 1, 1, 2, 2, 0, 3, 0, 0, 2, 2, 3] vertices = np.vstack((top_left, bottom_left, bottom_right, top_right)) normals = np.empty_like(vertices) n = np.cross(v_right, v_top) normals[:] = n / np.linalg.norm(n) if textures is not None: effect = effect_f(textures=textures, color=color) else: effect = effect_f(color=color) return Actors.Actor(geometry=Geometry.Geometry( indices=Array.Array(ndarray=np.array(indices, 'u4')), attribs=CustomAttribs.TexcoordsAttribs( vertices=Array.Array(ndarray=np.array(vertices, 'f4')), normals=Array.Array(ndarray=np.array(normals, 'f4')), texcoords0=Array.Array(ndarray=np.array(texcoords, 'f4'))), primitive_type=Geometry.PrimitiveType.TRIANGLES if filled else Geometry.PrimitiveType.LINES), effect=effect, transform=ensure_Transform(matrix), name=name)
def text(text, font="Arial", font_size=6, line_width=1, color=QColor("blue"), matrix=np.eye(4, dtype='f4'), is_billboard=True, name="text", scale=0.1, origin=[0, 0, 0], u=[1, 0, 0], v=[0, 1, 0], w=[0, 0, 1]): ''' Warning, this function can crash if called before any call to QApplication(sys.argv) ''' color = ensure_QColor(color) origin = utils.to_numpy(origin) u = utils.to_numpy(u) v = utils.to_numpy(v) w = utils.to_numpy(w) indices = [] vertices = [] path = QPainterPath() path.addText(QPointF(0, 0), QFont(font, font_size), text) polygons = path.toSubpathPolygons() for polygon in polygons: for point in polygon: indices.append(len(vertices)) p = utils.to_numpy([point.x(), point.y(), 0]) * scale vertices.append(origin + p[0] * u + p[1] * v + p[2] * w) indices.append(-1) return Actors.Actor(geometry=Geometry.Geometry( indices=Array.Array(ndarray=np.array(indices, 'u4')), attribs=Geometry.Attribs(vertices=Array.Array( ndarray=np.array(vertices, 'f4'))), primitive_type=Geometry.PrimitiveType.LINE_LOOP), effect=CustomEffects.emissive( color, line_width=line_width, is_billboard=is_billboard), transform=ensure_Transform(matrix), name=f"{name}_{text}")
def arrow(start=[0, 0, 0], end=[0, 0, 1], thickness=0.1, color=QColor("blue"), effect_f=CustomEffects.material, matrix=np.eye(4, dtype='f4'), name="arrow"): start = utils.to_numpy(start) end = utils.to_numpy(end) direction = end - start cyl = pymesh.generate_cylinder(start, start + direction * 0.9, thickness, thickness, 10) head = pymesh.generate_cylinder(start + direction * 0.9, end, thickness * 2, thickness * 0.1, 10) return from_mesh(pymesh.merge_meshes([cyl, head]), color, effect_f, 1, matrix, name)
def __update_bbox_3d(self, sample:Image): for ds_name, show in self.show_bbox_3d.items(): if not show: continue box_source = categories.get_source(parse_datasource_name(ds_name)[2]) box3d_sample:Box3d = self.platform[ds_name].get_at_timestamp(sample.timestamp) if np.abs(float(box3d_sample.timestamp) - float(sample.timestamp)) > 1e6: continue box3d = box3d_sample.set_referential(self.datasource, ignore_orientation=True) category_numbers = box3d.get_category_numbers() poly_collection = [] color_collection = [] for box_index in range(len(box3d)): center = box3d.get_centers()[box_index] dimension = box3d.get_dimensions()[box_index] rotation = box3d.get_rotations()[box_index] confidence = box3d.get_confidences()[box_index] category_name, color = categories.get_name_color(box_source, category_numbers[box_index]) id = box3d.get_ids()[box_index] if confidence: if confidence < self.conf_threshold: continue if self.category_filter is not '': if category_name not in self.category_filter: continue color = np.array(color)/255 if self.use_box_colors: color = utils.to_numpy(QColor(self.box_3d_colors[ds_name]))[:3] vertices = linalg.bbox_to_8coordinates(center, dimension, rotation) p, mask_fov = sample.project_pts(vertices, mask_fov=False, output_mask=True, undistorted=self.undistortimage, margin=1000) if p[mask_fov].shape[0] < 8: continue faces = [[0,1,3,2],[0,1,5,4],[0,2,6,4],[7,3,1,5],[7,5,4,6],[7,6,2,3]] for face in faces: poly = np.vstack([p[face[0]],p[face[1]],p[face[2]],p[face[3]],p[face[0]]]) poly_collection.append(poly) color_collection.append(color) if self.box_labels_size > 0: text_label = category_name if id: text_label += f" {id}" if confidence: text_label += f" ({int(confidence*100)}%)" txt = self.ax.text(p[:,0].min(),p[:,1].min(), text_label, color='w', fontweight='bold', fontsize=self.box_labels_size, clip_on=True) txt.set_path_effects([PathEffects.withStroke(linewidth=1, foreground='k')]) alpha = 0.05 facecolors = [list(c)+[alpha] for c in color_collection] poly_collection = PolyCollection(poly_collection, linewidths=0.5, edgecolors=color_collection, facecolors=facecolors) self.ax.add_collection(poly_collection)
def __update_box2D(self, sample, image, box): datasources = [ ds_name for ds_name, show in self.show_bbox_2d.items() if show ] for ds_name in datasources: _, _, ds_type = parse_datasource_name(ds_name) box_source = categories.get_source(ds_type) box2d_sample = self.platform[ds_name].get_at_timestamp( sample.timestamp) if np.abs(np.int64(sample.timestamp) - box2d_sample.timestamp) <= 1e6: raw = box2d_sample.raw if 'confidence' in raw: mask = (raw['confidence'] > self.conf_threshold) box2d = raw['data'][mask] else: box2d = raw['data'] if len(box2d) > 0: for i, box in enumerate(box2d): top = (box['x'] - box['h'] / 2) * image.shape[0] left = (box['y'] - box['w'] / 2) * image.shape[1] name, color = categories.get_name_color( box_source, box['classes']) if self.category_filter is not '': if name not in self.category_filter: continue color = np.array(color) / 255 if self.use_box_colors: color = utils.to_numpy( QColor(self.box_2d_colors[ds_name]))[:3] if 'confidence' in raw: conf = raw['confidence'][mask][i] name = f"{name}({conf:.3f})" rect = Rectangle((left, top), box['w'] * image.shape[1], box['h'] * image.shape[0], linewidth=1, edgecolor=color, facecolor=list(color) + [0.15]) self.ax.add_patch(rect) if self.box_labels_size > 0: txt = self.ax.text(left, top, name + ':' + str(box['id']), color='w', fontweight='bold', fontsize=self.box_labels_size, clip_on=True) txt.set_path_effects([ PathEffects.withStroke(linewidth=1, foreground='k') ])
def __update_box2D(self, sample, image): for ds_name, show in self.show_bbox_2d.items(): if not show: continue box_source = categories.get_source(parse_datasource_name(ds_name)[2]) box2d:Box2d = self.platform[ds_name].get_at_timestamp(sample.timestamp) if np.abs(float(box2d.timestamp) - float(sample.timestamp)) > 1e6: continue category_numbers = box2d.get_category_numbers() for box_index in range(len(box2d)): center = box2d.get_centers()[box_index] dimension = box2d.get_dimensions()[box_index] confidence = box2d.get_confidences()[box_index] category_name, color = categories.get_name_color(box_source, category_numbers[box_index]) id = box2d.get_ids()[box_index] if confidence: if confidence < self.conf_threshold: continue if self.category_filter is not '': if category_name not in self.category_filter: continue color = np.array(color)/255 if self.use_box_colors: color = utils.to_numpy(QColor(self.box_3d_colors[ds_name]))[:3] top = (center[0]-dimension[0]/2)*image.shape[0] left = (center[1]-dimension[1]/2)*image.shape[1] rect = Rectangle((left,top), dimension[1]*image.shape[1], dimension[0]*image.shape[0], linewidth=1, edgecolor=color, facecolor=list(color)+[0.15]) self.ax.add_patch(rect) if self.box_labels_size > 0: text_label = category_name if id: text_label += f" {id}" if confidence: text_label += f" ({int(confidence*100)}%)" txt = self.ax.text(left, top, text_label, color='w', fontweight='bold', fontsize=self.box_labels_size, clip_on=True) txt.set_path_effects([PathEffects.withStroke(linewidth=1, foreground='k')])
def pick(self, clicked_x, clicked_y, modifiers = None): # http://schabby.de/picking-opengl-ray-tracing/ aspect_ratio = self.aspect_ratio() cam_origin = self._camera.eye cam_direction = (self._camera.center - cam_origin).normalized() # The coordinate system we chose has x pointing right, y pointing down, z pointing into the screen # in screen coordinates, the vertical axis points down, this coincides with our 'y' axis. v = -self._camera._up # our y axis points down # in screen coordinates, the horizontal axis points right, this coincides with our x axis h = QVector3D.crossProduct(cam_direction, self._camera._up).normalized() # cam_direction points into the screen # in InFobRenderer::render(), we use Viewport::perspective_matrix(), where self._camera.fov is used # as QMatrix4x4::perspective()'s verticalAngle parameter, so near clipping plane's vertical scale is given by: v_scale = math.tan( math.radians(self._camera.vfov) / 2 ) * self._camera.near h_scale = v_scale * aspect_ratio # translate mouse coordinates so that the origin lies in the center # of the viewport (screen coordinates origin is top, left) x = clicked_x - self.width() / 2 y = clicked_y - self.height() / 2 # scale mouse coordinates so that half the view port width and height # becomes 1 (to be coherent with v_scale, which takes half of fov) x /= (self.width() / 2) y /= (self.height() / 2) # the picking ray origin: corresponds to the intersection of picking ray with # near plane (we don't want to pick actors behind the near plane) world_origin = cam_origin + cam_direction * self._camera.near + h * h_scale * x + v * v_scale * y # the picking ray direction world_direction = (world_origin - cam_origin).normalized() if self._debug and modifiers is not None and (modifiers & Qt.ShiftModifier): np_origin = utils.to_numpy(world_origin) np_v = utils.to_numpy(v) np_h = utils.to_numpy(h) self._debug_actors.clearActors() self._debug_actors.addActor(CustomActors.arrow(np_origin, np_origin + np_h, 0.01, QColor('red'))) self._debug_actors.addActor(CustomActors.arrow(np_origin, np_origin + np_v, 0.01, QColor('green'))) self._debug_actors.addActor(CustomActors.arrow(np_origin, np_origin + utils.to_numpy(world_direction) * 100, 0.01, QColor('magenta'))) min_t = float("inf") min_result = None for actor in self.renderer.sorted_actors: if actor._geometry: if actor._geometry.indices is not None\ and actor._geometry.attribs.vertices is not None: bvh = actor._geometry.goc_bvh() if bvh is None: continue bvh.update() if bvh.bvh is None: continue # bring back the actor at the origin m = actor.transform.worldTransform() if actor.transform else QMatrix4x4() m_inv = m.inverted()[0] # bring the ray in the actor's referential local_origin, local_direction = m_inv.map(world_origin), m_inv.mapVector(world_direction) local_origin_np, local_direction_np = utils.to_numpy(local_origin), utils.to_numpy(local_direction) # try to intersect the actor's geometry! if bvh.primitiveType == BVH.PrimitiveType.TRIANGLES or bvh.primitiveType == BVH.PrimitiveType.LINES: ids, tuvs = bvh.bvh.intersect_ray(local_origin_np, local_direction_np, True) if ids.size > 0: actor_min_t = tuvs[:,0].min() if actor_min_t < min_t: min_t = actor_min_t min_result = (actor, ids, tuvs, world_origin, world_direction, local_origin, local_direction) elif bvh.primitiveType == BVH.PrimitiveType.POINTS: object_id, distance, t = bvh.bvh.ray_distance(local_origin_np, local_direction_np) real_distance = math.sqrt(t**2 + distance**2) if real_distance < min_t: min_t = real_distance min_result = (actor, bvh.indices.ndarray[object_id, None], np.array([[t, distance, real_distance]]), world_origin, world_direction, local_origin, local_direction) if min_result is not None: return min_result raise NothingToPickException()
def __update_actors(self, sample:Image): datasources = [ds_name for ds_name, show in dict(self.show_actor, **dict(self.show_seg_3d)).items() if show] all_points2D = dict() all_colors = dict() all_indices = dict() for datasource_name in datasources: is_seg3D = datasource_name in self.show_seg_3d output_ds_name = datasource_name if is_seg3D: output_ds_name = datasource_name datasource_name = self.__get_datasource_to_show_seg3d(datasource_name) cloud_sample = self.__get_sample(sample, datasource_name) try: if isinstance(cloud_sample, PointCloud): points = cloud_sample.get_point_cloud(referential = self.datasource, undistort = self.undistort, reference_ts = int(sample.timestamp), dtype=np.float64) amplitudes = cloud_sample.get_field('i') # FIXME: dirty hack to get a valid field from a PointCloud without 'i' in its fields (radars) if amplitudes is None: amplitudes = np.clip(cloud_sample.get_field(cloud_sample.fields[3]), 0.01, np.inf) indices = np.arange(cloud_sample.size) elif isinstance(cloud_sample, Echo): points, amplitudes, indices = cloud_sample.get_cloud(referential = self.datasource, undistort = self.undistort, reference_ts = int(sample.timestamp), dtype=np.float64) except Sensor.NoPathToReferential as e: self.has_referential[datasource_name]['hasReferential'] = False continue if points.size == 0: continue self.has_referential[datasource_name]['hasReferential'] = True pts2d, points_mask = sample.project_pts(points, mask_fov=False, output_mask=True, undistorted=self.undistortimage) all_points2D[output_ds_name] = pts2d if is_seg3D: seg_sample = self.platform[output_ds_name].get_at_timestamp(cloud_sample.timestamp) mode = 'quad_cloud' if isinstance(cloud_sample, Echo) else None seg_colors = seg_sample.colors(mode=mode) if seg_colors.shape[0] != points.shape[0]: print(f'Warning. The length ({seg_colors.shape[0]}) of the segmentation 3D data' \ +f'does not match the length ({points.shape[0]}) of the point cloud.') continue all_colors[output_ds_name] = seg_colors if self.category_filter is not '': points_mask &= seg_sample.mask_category(self.category_filter) elif '-rgb' in datasource_name: #TODO: generalize how colors are obtained from the sample rgb_colors = np.ones((cloud_sample.size,4)) rgb_colors[:,0] = cloud_sample.get_field('r')/255 rgb_colors[:,1] = cloud_sample.get_field('g')/255 rgb_colors[:,2] = cloud_sample.get_field('b')/255 all_colors[output_ds_name] = rgb_colors else: a_min, a_max = amplitudes.min(), amplitudes.max() if self.log_scale: norm = matplotlib.colors.LogNorm(1 + a_min, 1 + a_min + a_max) else: norm = matplotlib.colors.Normalize(amplitudes.min(), amplitudes.max()) if self.use_colors: c = np.full((points.shape[0], 4), utils.to_numpy(QColor(self.ds_colors[datasource_name]))) c[:,3] = (0.25 + norm(amplitudes + ((1 + a_min) if self.log_scale else 0)))/1.25 #to make sure every point is visible all_colors[output_ds_name] = c else: all_colors[output_ds_name] = norm(amplitudes) all_indices[output_ds_name] = self.__filter_indices(points_mask, indices) self.window.hasReferential = self.has_referential self.__clean_plot_canvas() for ds_name, indices in all_indices.items(): points2d = all_points2D[ds_name][indices] colors = np.squeeze(all_colors[ds_name][indices[:,0] if indices.ndim>1 else indices]) #all colors are the same in a given triangle if indices.ndim == 2: if colors.ndim == 1: poly_coll = PolyCollection(points2d, array=colors, cmap=plt.cm.viridis, edgecolors=None, alpha=0.7) else: poly_coll = PolyCollection(points2d, facecolors=colors, edgecolors=None, alpha=0.7) self.ax.add_collection(poly_coll) self.ax.figure.canvas.draw() else: self.scatter = self.ax.scatter(points2d[:, 0], points2d[:, 1], s=self.point_size, c=colors)
def __update_bbox_3d(self, sample, image, box): datasources = [ ds_name for ds_name, show in self.show_bbox_3d.items() if show ] for ds_name in datasources: _, _, ds_type = parse_datasource_name(ds_name) box_source = categories.get_source(ds_type) if box_source not in categories.CATEGORIES: #FIXME: this should not be here box_source = 'deepen' box3d_sample = self.platform[ds_name].get_at_timestamp( sample.timestamp) if np.abs(np.int64(sample.timestamp) - box3d_sample.timestamp) <= 1e6: raw = box3d_sample.raw box3d = box3d_sample.mapto(self.datasource, ignore_orientation=True) mask = (box3d['flags'] >= 0) if 'confidence' in raw: mask = mask & (raw['confidence'] > self.conf_threshold) if len(box3d[mask]) > 0: poly_collection = [] color_collection = [] for i, box in enumerate(box3d[mask]): name, color = categories.get_name_color( box_source, box['classes']) if self.category_filter is not '' and name not in self.category_filter: break color = np.array(color) / 255 if self.use_box_colors: color = utils.to_numpy( QColor(self.box_3d_colors[ds_name]))[:3] if 'confidence' in raw: conf = raw['confidence'][mask][i] name = f"{name}({conf:.3f})" vertices = linalg.bbox_to_8coordinates( box['c'], box['d'], box['r']) p, mask_fov = sample.project_pts( vertices, mask_fov=False, output_mask=True, undistorted=self.undistortimage, margin=1000) if p[mask_fov].shape[0] < 8: continue faces = [[0, 1, 3, 2], [0, 1, 5, 4], [0, 2, 6, 4], [7, 3, 1, 5], [7, 5, 4, 6], [7, 6, 2, 3]] for face in faces: poly = np.vstack([ p[face[0]], p[face[1]], p[face[2]], p[face[3]], p[face[0]] ]) poly_collection.append(poly) color_collection.append(color) if self.box_labels_size > 0: txt = self.ax.text(p[:, 0].min(), p[:, 1].min(), name + ':' + str(box['id']), color='w', fontweight='bold', fontsize=self.box_labels_size, clip_on=True) txt.set_path_effects([ PathEffects.withStroke(linewidth=1, foreground='k') ]) alpha = 0.05 facecolors = [list(c) + [alpha] for c in color_collection] poly_collection = PolyCollection( poly_collection, linewidths=0.5, edgecolors=color_collection, facecolors=facecolors) self.ax.add_collection(poly_collection)
def load_collada(filename, scale=1, matrix=np.eye(4, dtype='f4'), name="collada", bake_matrix=True, merge_actors=True, ignore_non_textured=False, invert_normals=False, type_id=-1, instance_id=-1): actors = Actors.Actors(shared_transform=ensure_Transform( np.eye(4, dtype='f4')) if bake_matrix else ensure_Transform(matrix), name=name, type_id=type_id, instance_id=instance_id) mesh = collada.Collada(filename) np_matrix = utils.to_numpy(matrix) textures_cache = {} actors_cache = {} bbox = np.full((2, 3), np.finfo('f4').max) bbox[1, :] = np.finfo('f4').min actors.all_vertices = [] actors.scale = scale for coll_geom in tqdm.tqdm(mesh.scene.objects('geometry')): for coll_prim in coll_geom.primitives(): #FIXME: stop ignoring colladas transforms if isinstance(coll_prim, collada.triangleset.BoundTriangleSet): triangles = coll_prim elif isinstance(coll_prim, collada.polylist.BoundPolylist): triangles = coll_prim.triangleset() else: LoggingManager.instance().warning( f"{type(coll_prim)} not implementend") continue textures = {} effect_signature = [] #for merging actors uniforms = {} for effect_name in triangles.material.effect.supported: value = getattr(triangles.material.effect, effect_name) if isinstance(value, collada.material.Map): texture_image = value.sampler.surface.image effect_signature.append((effect_name, texture_image.id)) if texture_image.id in textures_cache: textures[effect_name] = textures_cache[ texture_image.id] else: array = textures[effect_name] = textures_cache[ texture_image.id] = Array.Array( ndarray=utils.load_texture( texture_image.pilimage)) elif isinstance(value, tuple): uniforms[effect_name] = QColor.fromRgbF(*value) effect_signature.append((effect_name, value)) elif isinstance(value, float): uniforms[effect_name] = value effect_signature.append((effect_name, value)) elif value is not None: LoggingManager.instance().warning( f"Unsupported type {effect_name}: {type(value)}") if not textures and ignore_non_textured: continue effect_signature = frozenset(effect_signature) triangles.generateNormals() vertices = triangles.vertex.astype('f4') * scale normals = triangles.normal.astype('f4') if invert_normals: normals = normals * -1 if bake_matrix: vertices = linalg.map_points(np_matrix, vertices) normals = linalg.map_vectors(np_matrix, normals) indices = triangles.vertex_index.flatten().astype('u4') attributes_ndarrays = {"vertices": vertices, "normals": normals} indexed_vertices = vertices[triangles.vertex_index.flatten()] for i in range(3): bbox[0, i] = min(bbox[0, i], indexed_vertices[:, i].min()) bbox[1, i] = max(bbox[1, i], indexed_vertices[:, i].max()) if textures: if len(triangles.texcoordset) > 1: LoggingManager.instance().warning( f"warning, {type(coll_prim)} not implementend") orig_tc0 = triangles.texcoordset[0].astype('f4') tc0_idx = triangles.texcoord_indexset[0].flatten() if not np.all(tc0_idx == indices): assert tc0_idx.shape == indices.shape, "texcoord indices must be the same shape as vertex indices" #this will duplicate shared vertices so that we can have a separate texcoords for each triangle sharing vertices attributes_ndarrays['vertices'] = indexed_vertices attributes_ndarrays['normals'] = normals[ triangles.normal_index.flatten()] indices = np.arange(indices.shape[0], dtype=indices.dtype) uv = orig_tc0[tc0_idx] else: uv = np.empty((vertices.shape[0], 2), 'f4') uv[indices] = orig_tc0[tc0_idx] attributes_ndarrays['texcoords0'] = uv attribs = CustomAttribs.TexcoordsAttribs( vertices=Array.Array( ndarray=attributes_ndarrays['vertices']), normals=Array.Array( ndarray=attributes_ndarrays['normals']), texcoords0=Array.Array( ndarray=attributes_ndarrays['texcoords0'])) #FIXME: bind collada uniforms if present effect = CustomEffects.textured_material(textures) else: attribs = Geometry.Attribs( vertices=Array.Array( ndarray=attributes_ndarrays['vertices']), normals=Array.Array( ndarray=attributes_ndarrays['normals'])) #FIXME: bind other uniforms if present effect = CustomEffects.material(color=uniforms['diffuse'], back_color=uniforms['diffuse']) if invert_normals: indices = indices.reshape((indices.shape[0] // 3), 3)[:, [0, 2, 1]].flatten() if merge_actors and effect_signature in actors_cache: actor = actors_cache[effect_signature] actor_attributes = actor.geometry.attribs.get_attributes() n_vertices_before = actor_attributes['vertices'].shape[0] for attr_name, value in actor_attributes.items(): value.set_ndarray( np.vstack( (value.ndarray, attributes_ndarrays[attr_name]))) actor.geometry.indices.set_ndarray( np.hstack((actor.geometry.indices.ndarray, indices + n_vertices_before))) else: geometry = Geometry.Geometry( indices=Array.Array(ndarray=indices), attribs=attribs) actor = actors.addActor( Actors.Actor(geometry=geometry, effect=effect, transform=actors.shared_transform, name=f"{name}_{coll_geom.original.id}", type_id=type_id, instance_id=instance_id)) actors_cache[effect_signature] = actor actors.all_vertices.append( actor.geometry.attribs.vertices ) #if in merge actor mode, vertices are already there actors.bbox = bbox return actors