From fcaa24a860afe1683e96dbcac09ae780fd558ba0 Mon Sep 17 00:00:00 2001 From: Ruben van de Ven Date: Tue, 4 Nov 2025 11:58:53 +0100 Subject: [PATCH] filter lidar pcd with space outline --- trap/lidar_tracker.py | 91 ++++++++++++++++--------------------------- 1 file changed, 33 insertions(+), 58 deletions(-) diff --git a/trap/lidar_tracker.py b/trap/lidar_tracker.py index 4aedb8c..2c48544 100644 --- a/trap/lidar_tracker.py +++ b/trap/lidar_tracker.py @@ -301,92 +301,62 @@ class LidarTracker(Node): 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) + # 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) + # # update drawn pcd with two colors + # updated_live_pcd = True + # live_pcd.points = current_pcd.points + # live_pcd.colors = o3d.utility.Vector3dVector(colors) + + cropped_pcd = current_pcd.select_by_index(cropping_indexes, invert=True) + cropped_pcd.paint_uniform_color((0,0,1)) + filtered_pcds.append(cropped_pcd) + # 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]] - - # stamp, lidar_points = next(self.sequence_generator) - # stamp2, lidar_points2 = next(self.sequence_generator2) - # print(len(lidar_points)) - # print(stamp) - - - # current_pcd.scale(np.array([1,-1, 1]), ) - # current_pcd.transform(transform_matrix) - - # if not found_planes: - # self.planes = segment_planes(current_pcd, 4, distance_threshold=.2) - # found_planes = True - # self.logger.info(f"Found {len(self.planes)} planes in points cloud") - - # if self.config.viz: - # for i, plane in enumerate(self.planes): - # color = [0,0,0] - # color[i%3] = 1 - # # color = [1*i/len(self.planes),0,0] - # plane.paint_uniform_color(color) - # print(f"Add plane {len(plane.points)} points") - # vis.add_geometry(plane, True) - - # a = time.perf_counter() filtered_pcd, dropped_indexes = self.room_filter.remove_static(current_pcd) + + if self.config.viz: - live_pcd = live_pcd.select + static_pcd = current_pcd.select_by_index(dropped_indexes) + static_pcd.paint_uniform_color((.5,.5,.5)) + filtered_pcds.append(static_pcd) stat_static = len(filtered_pcd.points) - - if self.room_filter.initialised: # filtered_pcd, _ = filtered_pcd.remove_statistical_outlier(nb_neighbors=20, std_ratio=2.0) filtered_pcd, inlier_indexes = filtered_pcd.remove_radius_outlier(nb_points=5, radius=0.2) - stat_denoise = len(filtered_pcd.points) + if self.config.viz: + outlier_pcd = filtered_pcd.select_by_index(inlier_indexes, invert=True) + outlier_pcd.paint_uniform_color((1,0,0)) + filtered_pcds.append(outlier_pcd) + + # down sample filtered_pcd = filtered_pcd.voxel_down_sample(voxel_size=0.05) + stat_downsampled = len(filtered_pcd.points) if self.config.viz: + live_filtered_pcd.points = filtered_pcd.points - # live_pcd.colors = o3d.utility.Vector3dVector(np.asarray(rgb_image)[mask].reshape(-1, 3) / 255.0) - - stat_downsampled = len(filtered_pcd.points) - # b = time.perf_counter() - # 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}") @@ -492,6 +462,11 @@ class LidarTracker(Node): if self.config.viz: + if len(filtered_pcds): + dropped_pcd = o3d.geometry.concatenate_point_clouds(filtered_pcds) + live_pcd.points = dropped_pcd.points + live_pcd.colors = dropped_pcd.colors + 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_filtered_pcd.paint_uniform_color([0.2, 0.7, 0.2])