filter lidar pcd with space outline
This commit is contained in:
Ruben van de Ven
parent
e09c9d44e8
commit
fcaa24a860
1 changed files with 33 additions and 58 deletions
|
|
@ -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])
|
||||
|
|
|
|||
Loading…
Reference in a new issue