diff --git a/trap/lidar_tracker.py b/trap/lidar_tracker.py index 64aee05..4aedb8c 100644 --- a/trap/lidar_tracker.py +++ b/trap/lidar_tracker.py @@ -161,6 +161,7 @@ class LidarTracker(Node): self.debug_lines = load_lines_from_svg(self.config.debug_map, 100, '') if self.config.debug_map else [] self.map_outline = None + self.map_outline_volume = None if self.config.map_outlines: lines = load_lines_from_svg(self.config.map_outlines, 100, '') @@ -170,6 +171,17 @@ class LidarTracker(Node): if len(lines) > 1: logger.warning("Multiple lines in outline file, using first only") self.map_outline = shapely.Polygon([p.position for p in lines[0].points]) + + self.map_outline_volume = o3d.visualization.SelectionPolygonVolume() + self.map_outline_volume.orthogonal_axis = "Z" # extrusion direction of polygon + self.map_outline_volume.axis_min = 0 + self.map_outline_volume.axis_max = 3.5 + polygon_points =np.array([[*p.position, 0] for p in lines[0].points]) + + + self.map_outline_volume.bounding_polygon = o3d.utility.Vector3dVector(polygon_points) + + self.remotes = {} @@ -276,10 +288,46 @@ class LidarTracker(Node): lidar_items = next(self.sequence_generator) current_pcd = self.build_cloud(lidar_items) - current_pcd = filter_pcd_points_below_z(current_pcd, .1) - current_pcd = filter_raw_points_by_distance(current_pcd, 10) - if self.map_outline and self.get_setting('lidar.crop_map_boundaries', False): - current_pcd = filter_pcd_points_outside_bounds(current_pcd, self.map_outline) + # current_pcd = filter_pcd_points_below_z(current_pcd, .1) + # current_pcd = filter_raw_points_by_distance(current_pcd, 10) + + filtered_pcds: List[o3d.geometry.PointCloud] = [] + + updated_live_pcd = False + if self.map_outline_volume: + # t0 = time.perf_counter() + if self.get_setting('lidar.crop_map_boundaries', True): + if not self.get_setting('lidar.viz_cropping', False): + current_pcd = self.map_outline_volume.crop_point_cloud(current_pcd) + else: # some more effort to visualise what falls outside + cropping_indexes = self.map_outline_volume.crop_in_polygon(current_pcd) + in_color = np.array([1,1,0]).reshape(1,-1) + out_color = np.array([0,0,1]).reshape(1,-1) + mask_array = np.zeros(len(current_pcd.points), dtype=int) + mask_array[cropping_indexes] = 1 + # print(any(cropping_indexes), all(cropping_indexes)) + colors = np.where(mask_array[:,None], in_color, out_color) + + # update drawn pcd with two colors + updated_live_pcd = True + live_pcd.points = current_pcd.points + live_pcd.colors = o3d.utility.Vector3dVector(colors) + + # still crop + current_pcd = current_pcd.select_by_index(cropping_indexes) + + + if self.config.viz and not updated_live_pcd: + live_pcd.points = current_pcd.points + live_pcd.colors = current_pcd.colors + + # current_pcd = current_pcd.select_by_index(cropping_indexes) + # outside_pcd = current_pcd.select_by_index(cropping_indexes, invert=True) + # print(time.perf_counter()- t0) + # pass + # current_pcd = self.map_outline_volume.crop_point_cloud(current_pcd) + # if self.map_outline and self.get_setting('lidar.crop_map_boundaries', False): + # current_pcd = filter_pcd_points_outside_bounds(current_pcd, self.map_outline) # a = list(lidar_items.keys()) # stamp, lidar_points = lidar_items[a[0]] @@ -309,23 +357,25 @@ class LidarTracker(Node): # a = time.perf_counter() - filtered_pcd = self.room_filter.remove_static(current_pcd) + filtered_pcd, dropped_indexes = self.room_filter.remove_static(current_pcd) + + if self.config.viz: + live_pcd = live_pcd.select stat_static = len(filtered_pcd.points) - if self.config.viz: - live_pcd.points = current_pcd.points + if self.room_filter.initialised: # filtered_pcd, _ = filtered_pcd.remove_statistical_outlier(nb_neighbors=20, std_ratio=2.0) - filtered_pcd, _ = filtered_pcd.remove_radius_outlier(nb_points=5, radius=0.2) + filtered_pcd, inlier_indexes = filtered_pcd.remove_radius_outlier(nb_points=5, radius=0.2) stat_denoise = len(filtered_pcd.points) # down sample - filtered_pcd = filtered_pcd.voxel_down_sample(voxel_size=0.15) + filtered_pcd = filtered_pcd.voxel_down_sample(voxel_size=0.05) if self.config.viz: live_filtered_pcd.points = filtered_pcd.points @@ -338,7 +388,7 @@ class LidarTracker(Node): # filtered_pcd2 = room_filter2.remove_static(current_pcd) # c = time.perf_counter() - # print(f"after static object removal: {stat_static} / after outlier removal: {stat_denoise} / after_downsample: {stat_downsampled}") + print(f"after static object removal: {stat_static} / after outlier removal: {stat_denoise} / after_downsample: {stat_downsampled}") # pcd.points = filtered_pcd.points @@ -443,7 +493,7 @@ class LidarTracker(Node): if self.config.viz: color = [0.5, 0.5, 0.7] if self.room_filter.initialised else [0.7, 0.7, 0.7] - live_pcd.paint_uniform_color(color) + # live_pcd.paint_uniform_color(color) live_filtered_pcd.paint_uniform_color([0.2, 0.7, 0.2]) if not geom_added: @@ -774,10 +824,13 @@ class VoxelOccupancyFilter: # Filter out static points from current point cloud filtered_points = [] - for pt in np.asarray(current_pcd.points): + dropped_indexes = [] + for i, pt in enumerate(np.asarray(current_pcd.points)): voxel = tuple(np.floor(pt / self.voxel_size).astype(np.int32)) if voxel not in self.static_voxels: filtered_points.append(pt) + else: + dropped_indexes.append(i) # Update history: until full, from there every n frame @@ -804,7 +857,7 @@ class VoxelOccupancyFilter: filtered_pcd = o3d.geometry.PointCloud() if filtered_points: filtered_pcd.points = o3d.utility.Vector3dVector(np.array(filtered_points)) - return filtered_pcd + return filtered_pcd, dropped_indexes class DecayingVoxelOccupancyFilter: diff --git a/trap/settings.py b/trap/settings.py index 4d95c4e..4dfd093 100644 --- a/trap/settings.py +++ b/trap/settings.py @@ -33,7 +33,8 @@ class Settings(Node): dpg.add_button(label="Save", callback=self.save) with dpg.window(label="Lidar", pos=(0, 150)): - self.register_setting(f'lidar.crop_map_boundaries', dpg.add_checkbox(label="crop_map_boundaries", default_value=self.get_setting(f'lidar.crop_map_boundaries', False), callback=self.on_change)) + self.register_setting(f'lidar.crop_map_boundaries', dpg.add_checkbox(label="crop_map_boundaries", default_value=self.get_setting(f'lidar.crop_map_boundaries', True), callback=self.on_change)) + self.register_setting(f'lidar.viz_cropping', dpg.add_checkbox(label="viz_cropping", default_value=self.get_setting(f'lidar.viz_cropping', True), callback=self.on_change)) for lidar in ["192.168.0.16", "192.168.0.10"]: name = lidar.replace(".", "_") with dpg.window(label=f"Lidar {lidar}", pos=(200, 0),autosize=True): diff --git a/trap/stage.py b/trap/stage.py index 4bc4470..4e8dd15 100644 --- a/trap/stage.py +++ b/trap/stage.py @@ -75,7 +75,7 @@ class ScenarioScene(Enum): SUBSTANTIAL = SceneInfo(6, "Multiple detections") FIRST_PREDICTION = SceneInfo(10, "Prediction is ready") CORRECTED_PREDICTION = SceneInfo(11, "Multiple predictions") - LOITERING = SceneInfo(7, "Foundto be loitering", takeover_possible=True) + LOITERING = SceneInfo(7, "Foundto be loitering", takeover_possible=True) # TODO: create "possible after" PLAY = SceneInfo(7, description="After many predictions; just fooling around", takeover_possible=True) LOST = SceneInfo(-1, description="Track lost", takeover_possible=True) @@ -191,7 +191,7 @@ class Scenario: if predictions == 1: self.set_scene(ScenarioScene.FIRST_PREDICTION) return True - if predictions > 10: + if predictions > 30: self.set_scene(ScenarioScene.PLAY) return True if predictions: @@ -253,7 +253,19 @@ class Scenario: self.update_state() - + +def build_line_others(): + others_color = SrgbaColor(1,1,0,1) + line_others = LineAnimationStack(StaticLine([], others_color)) + # line_others.add(SegmentLine(line_others.tail, duration=3, anim_f=partial(SegmentLine.anim_grow, in_and_out=True, max_len=5))) + line_others.add(CropAnimationLine(line_others.tail, lambda dt: 10 + math.sin(dt/4) * 70, assume_fps=TRACK_ASSUMED_FPS*2)) # speed up + line_others.add(NoiseLine(line_others.tail, amplitude=0, t_factor=.3)) + # line_others.add(DashedLine(line_others.tail, t_factor=4, loop_offset=True)) + # line_others.get(DashedLine).skip = True + line_others.add(FadedEndsLine(line_others.tail, 30, 30)) + line_others.add(FadeOutLine(line_others.tail)) + line_others.get(FadeOutLine).set_alpha(0) + return line_others class DrawnScenario(Scenario): """ @@ -288,16 +300,7 @@ class DrawnScenario(Scenario): self.line_prediction.add(FadeOutLine(self.line_prediction.tail)) # when rendering tracks from others similar/close to the current one - self.others_color = SrgbaColor(1,1,0,1) - self.line_others = LineAnimationStack(StaticLine([], self.others_color)) - # self.line_others.add(SegmentLine(self.line_others.tail, duration=3, anim_f=partial(SegmentLine.anim_grow, in_and_out=True, max_len=5))) - self.line_others.add(CropAnimationLine(self.line_others.tail, lambda dt: 10 + math.sin(dt/4) * 70, assume_fps=TRACK_ASSUMED_FPS*2)) # speed up - self.line_others.add(NoiseLine(self.line_others.tail, amplitude=0, t_factor=.3)) - # self.line_others.add(DashedLine(self.line_others.tail, t_factor=4, loop_offset=True)) - # self.line_others.get(DashedLine).skip = True - self.line_others.add(FadedEndsLine(self.line_others.tail, 30, 30)) - self.line_others.add(FadeOutLine(self.line_others.tail)) - self.line_others.get(FadeOutLine).set_alpha(0) + self.line_others = build_line_others() self.tracks_to_self: Optional[Generator] = None self.tracks_to_self_pos = None @@ -448,6 +451,27 @@ class DrawnScenario(Scenario): ]), timings +class NoTracksScenario(): + def __init__(self, stage: Stage): + self.stage = stage + self.line = build_line_others() + + def to_renderable_lines(self, dt: DeltaT): + lines = RenderableLines([], CoordinateSpace.WORLD) + if not self.line.is_running(): + track_id = random.choice(list(self.stage.history.state.tracks.keys())) + # print('track_id', track_id) + positions = self.stage.history.state.track_histories[track_id] + self.line.root.points = positions + self.line.start() + + lines.lines.append( + self.line.as_renderable_line(dt) + ) + # print(lines) + + return lines + class DatasetDrawer(): def __init__(self, stage: Stage): self.stage = stage @@ -504,6 +528,8 @@ class Stage(Node): self.history = TrackHistory(self.config.tracker_output_dir, self.config.camera, self.config.cache_path) self.auxilary = DatasetDrawer(self) + + self.notrack_lines = [NoTracksScenario() for i in range(self.config.max_active_scenarios)]