security_vision/trap
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

Multiple changes to stage and line drawing

Browse source
This commit is contained in:
Ruben van de Ven 2025-11-05 10:47:43 +01:00
parent c4ef961a78
commit 472eebf9a0
4 changed files with 198 additions and 33 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

73
trap/lines.py
View file

@ -5,6 +5,7 @@ import copy
from dataclasses import dataclass
from enum import Enum, IntEnum
from functools import partial
import logging
import math
from pathlib import Path
import time
@ -21,13 +22,16 @@ import svgpathtools
from noise import snoise2
from trap import renderable_pb2
from trap.utils import exponentialDecayRounded, inv_lerp
from trap.utils import exponentialDecay, exponentialDecayRounded, inv_lerp, relativePointToPolar, relativePolarToPoint
"""
See [notebook](../test_path_transforms.ipynb) for examples
"""
logger = logging.getLogger('lines')
RenderablePosition = Tuple[float,float]
Coordinate = Tuple[float, float]
DeltaT = float # delta_t in seconds
@ -526,6 +530,12 @@ class AppendableLineAnimator(LineAnimator):
self.drawn_points.append(copy.deepcopy(self.drawn_points[-1]))
idx = len(self.drawn_points) - 1
if idx > len(target_line.points) - 1:
logger.warning("Target line shorter that appendable line, shorten")
self.drawn_points = self.drawn_points[:len(target_line)]
idx = len(self.drawn_points) - 1
target = target_line.points[idx]
if np.isclose(self.drawn_points[-1].position, target.position, atol=.05).all():
@ -734,6 +744,7 @@ class FadedTailLine(LineAnimator):
return RenderableLine(points)
class NoiseLine(LineAnimator):
"""
Apply animated noise to line normals
@ -968,6 +979,66 @@ class DashedLine(LineAnimator):
return RenderableLine.from_multilinestring(multilinestring, color)
class RotatingLine(LineAnimator):
"""
Rotate the line around starting point towards new shape
"""
def __init__(self, target_line = None, decay_speed=16):
super().__init__(target_line)
self.decay_speed = decay_speed
self.drawn_points: List[RenderablePoint] = []
def apply(self, target_line: RenderableLine, dt: DeltaT) -> RenderableLine:
"""
warning, dashing (for now) removes all color
"""
if len(target_line) < 2:
self.ready = True
return target_line
origin = target_line.points[0]
if len(self.drawn_points) < 1:
self.drawn_points = target_line.points
diff_length = len(target_line) - len(self.drawn_points)
if diff_length < 0: # drawn points is larger
self.drawn_points = self.drawn_points[:len(target_line)]
if diff_length > 0: # target line is larger
self.drawn_points += [self.drawn_points[-1]] * diff_length
# associated_diff = self.prediction_diffs[a]
# progress = associated_diff.nr_of_passed_points()
for i, (target_point, drawn_point) in enumerate(zip(target_line.points, list(self.drawn_points))):
# TODO: this should be done in polar space starting from origin (i.e. self.drawn_posision[-1])
decay = max(3, (18/i) if i else 10) # points further away move with more delay
decay = self.decay_speed
drawn_r, drawn_angle = relativePointToPolar( origin.position, drawn_point.position)
pred_r, pred_angle = relativePointToPolar(origin.position, target_point.position)
r = exponentialDecay(drawn_r, pred_r, decay, dt)
# make circular coordinates transition through the smaller arc
if abs(drawn_angle - pred_angle) > math.pi:
pred_angle -= math.pi * 2
angle = exponentialDecay(drawn_angle, pred_angle, decay, dt)
x, y = relativePolarToPoint(origin, r, angle)
r = exponentialDecay(drawn_point.color.red, target_point.color.red, decay, dt)
g = exponentialDecay(drawn_point.color.green, target_point.color.green, decay, dt)
b = exponentialDecay(drawn_point.color.blue, target_point.color.blue, decay, dt)
a = exponentialDecay(drawn_point.color.alpha, target_point.color.alpha, decay, dt)
self.drawn_points[i].position = (x, y)
self.drawn_points[i].color = SrgbaColor(r, g, b, a)
return RenderableLine(self.drawn_points)
IndexAndOffset = Tuple[int, float]
class LineStringIncrementingDistanceOffset():

4
trap/prediction_server.py
View file

@ -104,11 +104,15 @@ def get_maps_for_input(input_dict, scene, hyperparams, device):
# print(scene_maps, patch_sizes, heading_angles)
# print(scene_pts)
try:
maps = scene_maps[0].get_cropped_maps_from_scene_map_batch(scene_maps,
scene_pts=torch.Tensor(scene_pts),
patch_size=patch_sizes[0],
rotation=heading_angles,
device='cpu')
except Exception as e:
logger.warning(f"Crash on getting maps for points: {scene_pts=} {heading_angles=} {patch_size=}")
raise e
maps_dict = {node: maps[[i]].to(device) for i, node in enumerate(nodes_with_maps)}
return maps_dict

3
trap/settings.py
View file

@ -41,11 +41,13 @@ class Settings(Node):
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', 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))
self.register_setting(f'lidar.tracking_enabled', dpg.add_checkbox(label="tracking_enabled", default_value=self.get_setting(f'lidar.tracking_enabled', 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):
# dpg.add_text("test")
# dpg.add_input_text(label="string", default_value="Quick brown fox")
self.register_setting(f'lidar.{name}.enabled', dpg.add_checkbox(label="enabled", default_value=self.get_setting(f'lidar.{name}.enabled', True), callback=self.on_change))
self.register_setting(f'lidar.{name}.rot_x', dpg.add_slider_float(label="rot_x", default_value=self.get_setting(f'lidar.{name}.rot_x', 0), max_value=math.pi * 2, callback=self.on_change))
self.register_setting(f'lidar.{name}.rot_y', dpg.add_slider_float(label="rot_y", default_value=self.get_setting(f'lidar.{name}.rot_y', 0), max_value=math.pi * 2, callback=self.on_change))
self.register_setting(f'lidar.{name}.rot_z', dpg.add_slider_float(label="rot_z", default_value=self.get_setting(f'lidar.{name}.rot_z', 0), max_value=math.pi * 2, callback=self.on_change))
@ -87,6 +89,7 @@ class Settings(Node):
def on_change(self, sender, value, user_data = None):
# print(sender, app_data, user_data)
setting = self.settings_fields[sender]
print(setting, value)
self.settings[setting] = value
self.config_sock.send_json({setting: value})

141
trap/stage.py
View file

@ -1,5 +1,6 @@
from __future__ import annotations
from abc import abstractmethod
from argparse import ArgumentParser
from collections import defaultdict
from dataclasses import dataclass
@ -21,7 +22,7 @@ import zmq
from trap.anomaly import DiffSegment, calc_anomaly, calculate_loitering_scores
from trap.base import CameraAction, DataclassJSONEncoder, Frame, HomographyAction, ProjectedTrack, Track
from trap.counter import CounterSender
from trap.lines import AppendableLine, AppendableLineAnimator, Coordinate, CoordinateSpace, CropAnimationLine, CropLine, DashedLine, DeltaT, FadeOutJitterLine, FadeOutLine, FadedEndsLine, FadedTailLine, LineAnimationStack, LineAnimator, NoiseLine, RenderableLayers, RenderableLine, RenderableLines, SegmentLine, SimplifyMethod, SrgbaColor, StaticLine, layers_to_message, load_lines_from_svg
from trap.lines import AppendableLine, AppendableLineAnimator, Coordinate, CoordinateSpace, CropAnimationLine, CropLine, DashedLine, DeltaT, FadeOutJitterLine, FadeOutLine, FadedEndsLine, FadedTailLine, LineAnimationStack, LineAnimator, NoiseLine, RenderableLayers, RenderableLine, RenderableLines, RotatingLine, SegmentLine, SimplifyMethod, SrgbaColor, StaticLine, layers_to_message, load_lines_from_svg
from trap.node import Node
from trap.track_history import TrackHistory
@ -36,7 +37,7 @@ OPTION_TRACK_NOISE = False
TRACK_ASSUMED_FPS = 12
TAKEOVER_FADEOUT = 3
LOST_FADEOUT = 2 # seconds
PREDICTION_INTERVAL: float|None = 20 # frames
PREDICTION_FADE_IN: float = 3
@ -81,13 +82,62 @@ class ScenarioScene(Enum):
Time = float
class Scenario:
class PrioritySlotItem():
TAKEOVER_FADEOUT = 3
def __init__(self, identifier):
self.identifier = identifier
self.start_time = 0.
self.take_over_at: Optional[Time] = None
def take_over(self):
if self.take_over_at:
return
self.take_over_at = time.perf_counter()
def taken_over(self):
self.is_running = False
self.take_over_at = None
def takenover_for(self):
if self.take_over_at:
return time.perf_counter() - self.take_over_at
return None
def takeover_factor(self):
l = self.takenover_for()
if not l:
return 0
return l/self.TAKEOVER_FADEOUT
def start(self):
# change when visible
logger.info(f"Start {self.identifier}: {self.get_state_name()}")
self.start_time = time.perf_counter()
self.is_running = True
@abstractmethod
def get_priority(self) -> int:
raise RuntimeError("Not implemented")
@abstractmethod
def get_state_name(self) -> str:
raise RuntimeError("Not implemented")
@abstractmethod
def can_be_taken_over(self):
raise RuntimeError("Not implemented")
class Scenario(PrioritySlotItem):
def __init__(self, track_id):
super().__init__(track_id)
self.track_id = track_id
self.scene: ScenarioScene = ScenarioScene.DETECTED
self.start_time = 0.
self.current_time = 0
self.take_over_at: Optional[Time] = None
self.track: Optional[ProjectedTrack] = None
self.prediction_tracks: List[ProjectedTrack] = []
@ -104,10 +154,14 @@ class Scenario:
logger.info(f"Found {self.track_id}: {self.scene.name}")
def start(self):
# change when visible
logger.info(f"Start {self.track_id}: {self.scene.name}")
self.is_running = True
def get_state_name(self):
return self.scene.name
def get_priority(self) -> int:
return self.scene.value.priority
def can_be_taken_over(self):
return self.scene.value.takeover_possible
def track_age(self):
if not self.track:
@ -133,7 +187,7 @@ class Scenario:
l = self.takenover_for()
if not l:
return 0
return l/TAKEOVER_FADEOUT
return l/self.TAKEOVER_FADEOUT
def lost_for(self):
@ -180,6 +234,10 @@ class Scenario:
def check_loitering(self):
scores = [s for s in calculate_loitering_scores(self.track, LOITERING_DURATION_TO_LINGER, LOITERING_LINGER_FACTOR, LOITERING_VELOCITY_TRESHOLD/TRACK_ASSUMED_FPS, 150)]
if not len(scores):
logger.warning(f"No loitering score for {self.track_id}")
return False
self.loitering_factor = scores[-1]
if self.loitering_factor > .99:
self.set_scene(ScenarioScene.LOITERING)
@ -274,7 +332,7 @@ class DrawnScenario(Scenario):
This distinction is only for ordering the code
"""
MAX_HISTORY = 300 # points of history of trajectory to display (preventing too long lines)
MAX_HISTORY = 200 # points of history of trajectory to display (preventing too long lines)
CUT_GAP = 5 # when adding a new prediction, keep the existing prediction until that point + this CUT_GAP margin
def __init__(self, track_id):
@ -294,6 +352,8 @@ class DrawnScenario(Scenario):
self.prediction_color = SrgbaColor(0,1,0,1)
self.line_prediction = LineAnimationStack(StaticLine([], self.prediction_color))
self.line_prediction.add(RotatingLine(self.line_prediction.tail, decay_speed=16))
self.line_prediction.get(RotatingLine).skip = True
self.line_prediction.add(SegmentLine(self.line_prediction.tail, duration=.5))
self.line_prediction.add(DashedLine(self.line_prediction.tail, t_factor=4, loop_offset=True))
self.line_prediction.get(DashedLine).skip = True
@ -451,12 +511,29 @@ class DrawnScenario(Scenario):
]), timings
class NoTracksScenario():
def __init__(self, stage: Stage):
class NoTracksScenario(PrioritySlotItem):
TAKEOVER_FADEOUT = 1 # override default to be faster
def __init__(self, stage: Stage, i: int):
super().__init__(f"screensaver_{i}")
self.stage = stage
self.line = build_line_others()
def get_priority(self):
# super low priority
return -1
def can_be_taken_over(self):
return True
def get_state_name(self):
return "previewing"
def update(self, stage: Stage):
pass
def to_renderable_lines(self, dt: DeltaT):
timings = []
lines = RenderableLines([], CoordinateSpace.WORLD)
if not self.line.is_running():
track_id = random.choice(list(self.stage.history.state.tracks.keys()))
@ -465,12 +542,16 @@ class NoTracksScenario():
self.line.root.points = positions
self.line.start()
alpha = 1 - self.takeover_factor()
self.line.get(FadeOutLine).set_alpha(alpha)
lines.lines.append(
self.line.as_renderable_line(dt)
)
# print(lines)
return lines
return lines, timings
class DatasetDrawer():
def __init__(self, stage: Stage):
@ -529,7 +610,8 @@ class Stage(Node):
self.auxilary = DatasetDrawer(self)
self.notrack_lines = [NoTracksScenario() for i in range(self.config.max_active_scenarios)]
# 'screensavers'
self.notrack_scenarios = [] #[NoTracksScenario(self, i) for i in range(self.config.max_active_scenarios)]
@ -591,9 +673,9 @@ class Stage(Node):
# 3) determine set of pending scenarios (all except running)
pending_scenarios = [s for s in self.scenarios.values() if s not in self.active_scenarios]
pending_scenarios = [s for s in list(self.scenarios.values()) + self.notrack_scenarios if s not in self.active_scenarios]
# ... highest priority first
pending_scenarios.sort(key=lambda s: s.scene.value.priority, reverse=True)
pending_scenarios.sort(key=lambda s: s.get_priority(), reverse=True)
# 4) check if there's a slot free:
while len(self.active_scenarios) < self.config.max_active_scenarios and len(pending_scenarios):
@ -604,15 +686,15 @@ class Stage(Node):
# 5) Takeover Logic: If no space, try to replace a lower-priority active scenario
# which is in a scene in which takeover is possible
eligible_active_scenarios = [
s for s in self.active_scenarios if s.scene.value.takeover_possible
s for s in self.active_scenarios if s.can_be_taken_over()
]
eligible_active_scenarios.sort(key=lambda s: s.scene.value.priority)
eligible_active_scenarios.sort(key=lambda s: s.get_priority())
if eligible_active_scenarios and pending_scenarios:
lowest_priority_active = eligible_active_scenarios[0]
highest_priority_waiting = pending_scenarios[0]
if highest_priority_waiting.scene.value.priority > lowest_priority_active.scene.value.priority:
if highest_priority_waiting.get_priority() > lowest_priority_active.get_priority():
# Takeover! Stop the active scenario
# will be cleaned up in update() loop after animation finishes
# automatically triggering the start of the highest priority scene
@ -625,26 +707,31 @@ class Stage(Node):
# TODO: sometimes very slow!
t1 = time.perf_counter()
training_lines = self.auxilary.to_renderable_lines(dt)
t2 = time.perf_counter()
timings = []
for scenario in self.active_scenarios:
scenario_lines, timing = scenario.to_renderable_lines(dt)
lines.append_lines(scenario_lines)
timings.append(timing)
if not len(self.active_scenarios):
lines = training_lines
t2 = time.perf_counter()
training_lines = self.auxilary.to_renderable_lines(dt)
t2b = time.perf_counter()
rl = lines.as_simplified(SimplifyMethod.RDP, .003) # or segmentise (see shapely)
rl_scenario = lines.as_simplified(SimplifyMethod.RDP, .003) # or segmentise (see shapely)
rl_training = training_lines.as_simplified(SimplifyMethod.RDP, .003) # or segmentise (see shapely)
self.counter.set("stage.lines", len(lines.lines))
self.counter.set("stage.points_orig", lines.point_count())
self.counter.set("stage.points", rl.point_count())
self.counter.set("stage.points", rl_scenario.point_count())
t3 = time.perf_counter()
layers: RenderableLayers = {
1: lines,
1: rl_scenario,
2: self.debug_lines,
3: training_lines,
3: rl_training,
}
t4 = time.perf_counter()