filter lidar pcd with space outline
This commit is contained in:
Ruben van de Ven
parent
acdb869430
commit
e09c9d44e8
3 changed files with 107 additions and 27 deletions
|
|
@ -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, '')
|
||||
|
|
@ -171,6 +172,17 @@ class LidarTracker(Node):
|
|||
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:
|
||||
|
|
|
|||
|
|
@ -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):
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
|
|
@ -254,6 +254,18 @@ class Scenario:
|
|||
|
||||
|
||||
|
||||
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
|
||||
|
|
@ -505,6 +529,8 @@ class Stage(Node):
|
|||
|
||||
self.auxilary = DatasetDrawer(self)
|
||||
|
||||
self.notrack_lines = [NoTracksScenario() for i in range(self.config.max_active_scenarios)]
|
||||
|
||||
|
||||
|
||||
def run(self):
|
||||
|
|
|
|||
Loading…
Reference in a new issue