From d1703a7a86cc80916ff36cd0d6f1e817d1b07733 Mon Sep 17 00:00:00 2001 From: Ruben van de Ven Date: Mon, 12 May 2025 18:56:35 +0200 Subject: [PATCH] laser drawing WIP --- trap/base.py | 16 +++- trap/lines.py | 21 +++++ trap/prediction_server.py | 4 + trap/stage.py | 162 ++++++++++++++++++++++++-------------- trap/tracker.py | 3 +- 5 files changed, 145 insertions(+), 61 deletions(-) diff --git a/trap/base.py b/trap/base.py index 2b9af00..b0f392d 100644 --- a/trap/base.py +++ b/trap/base.py @@ -381,12 +381,20 @@ class Track: frame_nr = a.frame_nr + g new_history.append(Detection(a.track_id, l, t, w, h, conf, state, frame_nr, a.det_class)) + return self.get_with_new_history(new_history) + + def get_with_new_history(self, new_history: List[Detection]): + return Track( self.track_id, new_history, self.predictor_history, self.predictions, - self.fps) + self.fps, + self.source, + self.lost, + self.created_at, + self.frame_index) def is_complete(self): diffs = [(b.frame_nr - a.frame_nr) for a,b in zip(self.history[:-1], self.history[1:])] @@ -405,7 +413,11 @@ class Track: t.history[offset::step_size], t.predictor_history, t.predictions, - t.fps/step_size) + t.fps/step_size, + self.source, + self.lost, + self.created_at, + self.frame_index) def get_simplified_history(self, distance: float, camera: Camera) -> list[tuple[float, float]]: # TODO)) Simplify to get a point every n-th meter diff --git a/trap/lines.py b/trap/lines.py index 44ccea0..1100e3d 100644 --- a/trap/lines.py +++ b/trap/lines.py @@ -2,6 +2,7 @@ from __future__ import annotations from dataclasses import dataclass from enum import Enum +import math from typing import List, Tuple from simplification.cutil import simplify_coords_idx, simplify_coords_vw_idx @@ -65,3 +66,23 @@ class RenderableLines(): [line.as_simplified(method) for line in self.lines] ) + + +def circle_arc(cx, cy, r, t, l, c: SrgbaColor): + """ + draw an cirlce arc, around point cx,cy, with radius r + for l*2pi, offset by t. Both t and l are 0<= [t,l] <= 1 + """ + + resolution = 40 + steps = int(resolution * l) + offset = int(resolution * t) + pointlist: list[RenderablePoint] = [] + for i in range(offset, offset+steps): + x = cx + math.cos(i * (2*math.pi)/resolution) * r + y = cy + math.sin(i * (2*math.pi)/resolution)* r + + pointlist.append(RenderablePoint((x, y), c)) + + + return RenderableLine(pointlist) \ No newline at end of file diff --git a/trap/prediction_server.py b/trap/prediction_server.py index ad22efd..5c8934d 100644 --- a/trap/prediction_server.py +++ b/trap/prediction_server.py @@ -294,6 +294,8 @@ class PredictionServer: data = self.trajectory_socket.recv() # print('recv tracker frame') frame: Frame = pickle.loads(data) + + # print('indexrecv', [frame.tracks[t].frame_index for t in frame.tracks]) # trajectory_data = {t.track_id: t.get_projected_history_as_dict(frame.H) for t in frame.tracks.values()} # trajectory_data = json.loads(data) # logger.debug(f"Receive {frame.index}") @@ -482,6 +484,8 @@ class PredictionServer: frame.maps = list([m.cpu().numpy() for m in maps.values()]) if maps else None + # print('index', [frame.tracks[t].frame_index for t in frame.tracks]) + self.send_frame(frame) logger.info('Stopping') diff --git a/trap/stage.py b/trap/stage.py index d38d383..ee96c99 100644 --- a/trap/stage.py +++ b/trap/stage.py @@ -18,8 +18,8 @@ from sgan.sgan import data from trap import shapes from trap.base import DataclassJSONEncoder, Frame, Track from trap.counter import CounterSender -from trap.laser_renderer import rotateMatrix -from trap.lines import RenderableLine, RenderableLines, RenderablePoint, SrgbaColor +from trap.laser_renderer import circle_points, rotateMatrix +from trap.lines import RenderableLine, RenderableLines, RenderablePoint, SrgbaColor, circle_arc from trap.node import Node from trap.timer import Timer from trap.utils import exponentialDecay, relativePointToPolar, relativePolarToPoint @@ -34,7 +34,7 @@ class ScenarioScene(Enum): LOST = -1 LOST_FADEOUT = 3 -PREDICTION_INTERVAL: float|None = 1 +PREDICTION_INTERVAL: float|None = 15 # frames PREDICTION_FADE_IN: float = 3 PREDICTION_FADE_SLOPE: float = -10 PREDICTION_FADE_AFTER_DURATION: float = 10 # seconds @@ -46,7 +46,7 @@ TRACK_FADE_ASSUME_FPS = 12 # Don't render the first n points of the prediction, # helps to avoid jitter in the line transition -PREDICTION_OFFSET = int(TRACK_FADE_ASSUME_FPS * PREDICTION_INTERVAL * .8) +# PREDICTION_OFFSET = int(TRACK_FADE_ASSUME_FPS * PREDICTION_INTERVAL * .8) class TrackScenario(StateMachine): detected = State(initial=True) @@ -67,8 +67,9 @@ class TrackScenario(StateMachine): def __init__(self): self._track = None - self.first_prediction_track: Optional[Track] = None - self.prediction_track: Optional[Track] = None + # self.first_prediction_track: Optional[Track] = None + # self.prediction_track: Optional[Track] = None + self._predictions: List[Track] = [] super().__init__() def track_is_long(self, track: Track): @@ -84,7 +85,7 @@ class TrackScenario(StateMachine): def prediction_is_stale(self, track: Track): # TODO use displacement instead of time - return bool(self.prediction_track and self.prediction_track.created_at < (time.time() - 2)) + return bool(len(self._predictions) and self._predictions[-1].created_at < (time.time() - 2)) def prediction_is_playing(self, Track): return False @@ -105,20 +106,20 @@ class TrackScenario(StateMachine): # state change is optional pass - def set_prediction(self, track: Track): + def add_prediction(self, track: Track): if not self._track: # in case of the unlikely event that prediction was passed sooner self.set_track(track) - if not self.first_prediction_track: - self.first_prediction_track = track + # if not self.first_prediction_track: + # self.first_prediction_track = track - if PREDICTION_INTERVAL is not None and self.prediction_track and (track.created_at - self.prediction_track.created_at) < PREDICTION_INTERVAL: + if PREDICTION_INTERVAL is not None and len(self._predictions) and (track.frame_index - self._predictions[-1].frame_index) < PREDICTION_INTERVAL: # just drop tracks if the predictions come to quick return - self.prediction_track = track + self._predictions.append(track) try: self.receive_prediction(track) except TransitionNotAllowed as e: @@ -140,9 +141,10 @@ class TrackScenario(StateMachine): def after_receive_prediction(self, track: Track): # after - self.prediction_track = track - if not self.first_prediction_track: - self.first_prediction_track = track + pass + # self.prediction_track = track + # if not self.first_prediction_track: + # self.first_prediction_track = track def on_enter_corrected_prediction(self): print('corrected!') @@ -200,6 +202,8 @@ class DrawnScenario(TrackScenario): self.drawn_text = "" self.drawn_text_lines: List[RenderableLine] = [] + + self.anomly_score = .3 # TODO: variable super().__init__() def update_drawn_positions(self) -> List: @@ -222,67 +226,98 @@ class DrawnScenario(TrackScenario): # 1. track history, direct update MAX_HISTORY = 80 # positions = self._track.get_projected_history(None, self.camera)[-MAX_HISTORY:] - positions = self._track.get_projected_history(None, self.camera) + self.drawn_positions = self._track.get_projected_history(None, self.camera) # TODO)) Limit history to N points, or N lenght - for i, pos in enumerate(self.drawn_positions): - self.drawn_positions[i][0] = positions[i][0] - self.drawn_positions[i][1] = positions[i][1] + # for i, pos in enumerate(self.drawn_positions): + # self.drawn_positions[i][0] = positions[i][0] + # self.drawn_positions[i][1] = positions[i][1] - if len(positions) > len(self.drawn_positions): - self.drawn_positions.extend(positions[len(self.drawn_positions):]) + # if len(positions) > len(self.drawn_positions): + # self.drawn_positions.extend(positions[len(self.drawn_positions):]) - if self.prediction_track and self.prediction_track.predictor_history: - # 2. history as seen by predictor (Trajectron) - for i, pos in enumerate(self.drawn_pred_history): - if len(self.prediction_track.predictor_history) > i: - self.drawn_pred_history[i][0] = int_or_not(exponentialDecay(self.drawn_pred_history[i][0], self.prediction_track.predictor_history[i][0], 16, dt)) - self.drawn_pred_history[i][1] = int_or_not(exponentialDecay(self.drawn_pred_history[i][1], self.prediction_track.predictor_history[i][1], 16, dt)) + # 2. history as seen by predictor (Trajectron) + # if self.prediction_track and self.prediction_track.predictor_history: + # for i, pos in enumerate(self.drawn_pred_history): + # if len(self.prediction_track.predictor_history) > i: + # self.drawn_pred_history[i][0] = int_or_not(exponentialDecay(self.drawn_pred_history[i][0], self.prediction_track.predictor_history[i][0], 16, dt)) + # self.drawn_pred_history[i][1] = int_or_not(exponentialDecay(self.drawn_pred_history[i][1], self.prediction_track.predictor_history[i][1], 16, dt)) - if len(self.prediction_track.predictor_history) > len(self.drawn_pred_history): - self.drawn_pred_history.extend(positions[len(self.drawn_pred_history):]) + # if len(self.prediction_track.predictor_history) > len(self.drawn_pred_history): + # self.drawn_pred_history.extend(positions[len(self.drawn_pred_history):]) - if self.prediction_track and self.prediction_track.predictions: - # 3. predictions - prediction_offset = self._track.frame_index - self.prediction_track.frame_index - if len(self.prediction_track.predictions): - for a, drawn_prediction in enumerate(self.drawn_predictions): - for i, pos in enumerate(drawn_prediction): - # 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 = 16 - origin = self.drawn_positions[-1] - drawn_r, drawn_angle = relativePointToPolar( origin, drawn_prediction[i]) - pred_r, pred_angle = relativePointToPolar(origin, self.prediction_track.predictions[a][i+prediction_offset]) - r = exponentialDecay(drawn_r, pred_r, decay, dt) - angle = exponentialDecay(drawn_angle, pred_angle, decay, dt) - x, y = relativePolarToPoint(origin, r, angle) - self.drawn_predictions[a][i] = int_or_not(x), int_or_not(y) - # self.drawn_predictions[i][0] = int(exponentialDecay(self.drawn_predictions[i][0], self.prediction_track.predictions[i][0], decay, dt)) - # self.drawn_predictions[i][1] = int(exponentialDecay(self.drawn_predictions[i][1], self.prediction_track.predictions[i][1], decay, dt)) + # 3. predictions + self.drawn_predictions = [] + for a, (ptrack, next_ptrack) in enumerate(zip(self._predictions, [*self._predictions[1:], None])): + + prediction = ptrack.predictions[0] # only use one prediction per timestep/frame/track + if next_ptrack is not None: + # not the last one, cut off + next_ptrack: Track = self._predictions[a+1] + end_step = next_ptrack.frame_index - ptrack.frame_index + else: + end_step = None # not last item; show all + self.drawn_predictions.append(ptrack.predictions[0][:end_step]) + + + + + # print(self.drawn_predictions) + # line = [] + # for i, pos in enumerate(ptrack.predictions): + # line.append((ptrack.predictions[i][0], ptrack.predictions[i][1])) + # print(line) + + # if len(self.drawn_predictions) <= a: + # self.drawn_predictions.append(line) + # else: + # self.drawn_predictions[a] = line + + + # if self.prediction_track and self.prediction_track.predictions: + # prediction_offset = self._track.frame_index - self.prediction_track.frame_index + # if len(self.prediction_track.predictions): + # for a, drawn_prediction in enumerate(self.drawn_predictions): + # for i, pos in enumerate(drawn_prediction): + # # 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 = 16 + # origin = self.drawn_positions[-1] + # drawn_r, drawn_angle = relativePointToPolar( origin, drawn_prediction[i]) + # pred_r, pred_angle = relativePointToPolar(origin, self.prediction_track.predictions[a][i+prediction_offset]) + # r = exponentialDecay(drawn_r, pred_r, decay, dt) + # angle = exponentialDecay(drawn_angle, pred_angle, decay, dt) + # x, y = relativePolarToPoint(origin, r, angle) + # self.drawn_predictions[a][i] = int_or_not(x), int_or_not(y) + # # self.drawn_predictions[i][0] = int(exponentialDecay(self.drawn_predictions[i][0], self.prediction_track.predictions[i][0], decay, dt)) + # # self.drawn_predictions[i][1] = int(exponentialDecay(self.drawn_predictions[i][1], self.prediction_track.predictions[i][1], decay, dt)) + + # if len(self.prediction_track.predictions) > len(self.drawn_predictions): + # for pred in self.prediction_track.predictions[len(self.drawn_predictions):]: + # self.drawn_predictions.append(pred[prediction_offset:]) + - if len(self.prediction_track.predictions) > len(self.drawn_predictions): - for pred in self.prediction_track.predictions[len(self.drawn_predictions):]: - self.drawn_predictions.append(pred[prediction_offset:]) - # for a, drawn_prediction in self.drawn_predictions: - # if len(self.pred_coords) > len(self.drawn_predictions): - # self.drawn_predictions.extend(self.pred_coords[len(self.drawn_predictions):]) def to_renderable_lines(self) -> RenderableLines: - track_age = time.time() - self._track.created_at + t = time.time() + track_age = t - self._track.created_at + lines: List[RenderableLine] = [] + drawable_points, alphas = points_fade_out_alpha_mask(self.drawn_positions, track_age, TRACK_FADE_AFTER_DURATION, TRACK_END_FADE) + # track_age_in_frames = int(track_age * TRACK_FADE_ASSUME_FPS) # track_max_points = TRACK_FADE_AFTER_DURATION * TRACK_FADE_ASSUME_FPS - track_age_in_frames - lines: List[RenderableLine] = [] color = SrgbaColor(1.,0.,0.,1.-self.lost_factor()) points = [RenderablePoint(pos, color.as_faded(a)) for pos, a in zip(drawable_points, alphas)] lines.append(RenderableLine(points)) + anomaly_marker_color = SrgbaColor(0.,0.,1, 1.-self.lost_factor()) # fadeout + lines.append(circle_arc(self.drawn_positions[-1][0], self.drawn_positions[-1][1], 1, t, self.anomly_score, anomaly_marker_color)) if len(self.drawn_predictions): color = SrgbaColor(0.,1,0.,1.-self.lost_factor()) - prediction_track_age = time.time() - self.first_prediction_track.created_at + prediction_track_age = time.time() - self._predictions[0].created_at t_factor = prediction_track_age / PREDICTION_FADE_IN # positions = [RenderablePosition.from_list(pos) for pos in self.drawn_positions] for drawn_prediction in self.drawn_predictions: @@ -302,7 +337,8 @@ class DrawnScenario(TrackScenario): # colors = [color.as_faded(a1*a2) for a1, a2 in zip(alphas1, alphas2)] - points = [RenderablePoint(pos, pos_color) for pos, pos_color in zip(drawn_prediction[PREDICTION_OFFSET:], colors[PREDICTION_OFFSET:])] + # points = [RenderablePoint(pos, pos_color) for pos, pos_color in zip(drawn_prediction[PREDICTION_OFFSET:], colors[PREDICTION_OFFSET:])] + points = [RenderablePoint(pos, pos_color) for pos, pos_color in zip(drawn_prediction, colors)] lines.append(RenderableLine(points)) # # print(self.current_state) @@ -353,6 +389,16 @@ class DrawnScenario(TrackScenario): return self.drawn_text_lines return None +# def circle_points(cx, cy, r, c: Color): PointList +# # r = r +# steps = 30 +# pointlist: list[LaserPoint] = [] +# for i in range(steps): +# x = int(cx + math.cos(i * (2*math.pi)/steps) * r) +# y = int(cy + math.sin(i * (2*math.pi)/steps)* r) +# pointlist.append(LaserPoint(x, y, c, blank=(i==(steps-1)or i==0))) + +# return pointlist def points_fade_out_alpha_mask(positions: List, track_age_seconds: float, fade_after_duration: float, fade_frames: int, no_frame_max=False): @@ -442,7 +488,7 @@ class Stage(Node): try: prediction_frame: Frame = self.prediction_sock.recv_pyobj(zmq.NOBLOCK) for track_id, track in prediction_frame.tracks.items(): - self.scenarios[track_id].set_prediction(track) + self.scenarios[track_id].add_prediction(track) except zmq.ZMQError as e: self.logger.debug(f'reuse prediction') diff --git a/trap/tracker.py b/trap/tracker.py index b9b292c..4a56f49 100644 --- a/trap/tracker.py +++ b/trap/tracker.py @@ -772,7 +772,8 @@ class Smoother: self.smoother.smooth(hs) hs = self.smoother.smooth_data[0] new_history = [Detection(d.track_id, l, t, w, h, d.conf, d.state, d.frame_nr, d.det_class) for l, t, w, h, d in zip(ls,ts,ws,hs, track.history)] - return Track(track.track_id, new_history, track.predictor_history, track.predictions, track.fps) + return track.get_with_new_history(new_history) + # return Track(track.track_id, new_history, track.predictor_history, track.predictions, track.fps) def smooth_frame_tracks(self, frame: Frame) -> Frame: new_tracks = []