diff --git a/pyproject.toml b/pyproject.toml index b4b3397..5b255ea 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -52,6 +52,7 @@ trap_stage = "trap.stage:Stage.parse_and_start" trap_prediction = "trap.prediction_server:PredictionServer.parse_and_start" trap_render_cv = "trap.cv_renderer:CvRenderer.parse_and_start" trap_monitor = "trap.monitor:Monitor.parse_and_start" # migrate timer +trap_laser_calibration = "trap.laser_calibration:LaserCalibration.parse_and_start" # migrate timer [tool.uv] diff --git a/supervisord.conf b/supervisord.conf index c57b90b..bc73db5 100644 --- a/supervisord.conf +++ b/supervisord.conf @@ -28,7 +28,7 @@ directory=%(here)s directory=%(here)s [program:tracker] -command=uv run trap_tracker +command=uv run trap_tracker --smooth-tracks directory=%(here)s [program:stage] @@ -40,7 +40,7 @@ command=uv run trap_prediction --eval_device cuda:0 --model_dir EXPERIMENTS/mod directory=%(here)s [program:render_cv] -command=uv run trap_render_cv +command=uv run trap_render_cv directory=%(here)s environment=DISPLAY=":0" autostart=false diff --git a/trap/cv_renderer.py b/trap/cv_renderer.py index a897b13..399d288 100644 --- a/trap/cv_renderer.py +++ b/trap/cv_renderer.py @@ -115,7 +115,7 @@ class CvRenderer(Node): cv2.namedWindow("frame", cv2.WINDOW_NORMAL) # https://gist.github.com/ronekko/dc3747211543165108b11073f929b85e - cv2.moveWindow("frame", 1920, -1) + cv2.moveWindow("frame", 0, -1) if self.config.full_screen: cv2.setWindowProperty("frame",cv2.WND_PROP_FULLSCREEN,cv2.WINDOW_FULLSCREEN) # bgsub = cv2.createBackgroundSubtractorMOG2(120, 50, detectShadows=True) @@ -170,10 +170,10 @@ class CvRenderer(Node): self.out_writer.write(img) if self.streaming_process: self.streaming_process.stdin.write(img.tobytes()) - if self.config.render_window: + if not self.config.no_window: cv2.imshow('frame',cv2.resize(img, (1920, 1080))) # cv2.imshow('frame',img) - cv2.waitKey(1) + cv2.waitKey(10) # clear out old tracks & predictions: @@ -218,8 +218,8 @@ class CvRenderer(Node): render_parser.add_argument("--render-file", help="Render a video file previewing the prediction, and its delay compared to the current frame", action='store_true') - render_parser.add_argument("--render-window", - help="Render a previewing to a window", + render_parser.add_argument("--no-window", + help="Disable a previewing to a window", action='store_true') render_parser.add_argument("--full-screen", diff --git a/trap/laser_calibration.py b/trap/laser_calibration.py new file mode 100644 index 0000000..d5a36bb --- /dev/null +++ b/trap/laser_calibration.py @@ -0,0 +1,273 @@ + + +from argparse import ArgumentParser +import json +from pathlib import Path +from typing import Optional + +import cv2 +import numpy as np + +from trap.base import DataclassJSONEncoder, DistortedCamera, Frame +from trap.lines import CoordinateSpace, RenderableLines, SrgbaColor, cross_points +from trap.node import Node +from trap.stage import Coordinate + + + +class LaserCalibration(Node): + """ + A calibrated camera can be used to reverse-map the points of the laser to world coordinates. + Note, it publishes on the address of the stage node, so they cannot run at the same time. + + 1. Draw points with the laser (use 1-9 to create/select, then position them with arrow keys) + 2. Use cursor on camera stream to create an image point for. + - Locate nearby point to select and drag + 3. Use image coordinate of point, undistort, homograph, gives world coordinate. + 4. Perform homography on world coordinates + laser coordinates + """ + + def setup(self): + # self.scenarios: List[DrawnScenario] = [] + + self.frame_sock = self.sub(self.config.zmq_frame_addr) + self.laser_sock = self.pub(self.config.zmq_stage_addr) + + self.camera: Optional[DistortedCamera] = None + + self._selected_point = None + self._is_dragging = False + self.laser_points = {} + self.image_points = {} + self.H = None + + self.img_size = (1920,1080) + self.frame_img_factor = (1,1) + + if self.config.calibfile.exists(): + with self.config.calibfile.open('r') as fp: + calibdata = json.load(fp) + self.laser_points = calibdata['laser_points'] + self.image_points = calibdata['image_points'] + self.H = calibdata['H'] + + + + + def run(self): + + cv2.namedWindow("laser_calib", cv2.WINDOW_NORMAL) + # https://gist.github.com/ronekko/dc3747211543165108b11073f929b85e + # cv2.moveWindow("laser_calib", 0, -1) + cv2.setMouseCallback('laser_calib',self.mouse_event) + cv2.setWindowProperty("laser_calib",cv2.WND_PROP_FULLSCREEN,cv2.WINDOW_FULLSCREEN) + + # arrow up (82), down (84), arrow left(81) + + frame = None + while self.run_loop_capped_fps(60): + if self.frame_sock.poll(0): + frame: Frame = self.frame_sock.recv_pyobj() + if not self.camera: + self.camera = frame.camera + + if frame is None: + continue + + self.frame_img_factor = frame.img.shape[1] / self.img_size[0], frame.img.shape[0] / self.img_size[1] + + + img = frame.img + img = cv2.resize(img, self.img_size) + + cv2.putText(img, 'press 1-0 to create/edit points', (10,20), cv2.FONT_HERSHEY_SIMPLEX, .5, (255,255,255)) + if len(self.laser_points) < 4: + cv2.putText(img, 'add points to calculate homography', (10,40), cv2.FONT_HERSHEY_SIMPLEX, .5, (255,255,255)) + else: + cv2.putText(img, 'press c to calculate homography', (10,40), cv2.FONT_HERSHEY_SIMPLEX, .5, (255,255,0)) + + cv2.putText(img, str(self.config.calibfile), (10,self.img_size[1]-30), cv2.FONT_HERSHEY_SIMPLEX, .5, (255,255,0)) + + if self._selected_point: + color = (0,255,255) + cv2.putText(img, f'selected {self._selected_point}', (10,60), cv2.FONT_HERSHEY_SIMPLEX, .5, color) + cv2.putText(img, 'press d to delete', (10,80), cv2.FONT_HERSHEY_SIMPLEX, .5, color) + cv2.putText(img, 'use arrows to position laser for this point', (10,100), cv2.FONT_HERSHEY_SIMPLEX, .5, color) + target = self.camera.points_img_to_world([self.image_points[self._selected_point]])[0].tolist() + target = round(target[0], 2), round(target[1], 2) + cv2.putText(img, f'map {self.laser_points[self._selected_point]} to {target} ({self.image_points[self._selected_point]})', (10,120), cv2.FONT_HERSHEY_SIMPLEX, .5, color) + + + for k, coord in self.image_points.items(): + color = (0,0,255) if self._selected_point == k else (255,0,0) + coord = int(coord[0] / self.frame_img_factor[0]), int(coord[1] / self.frame_img_factor[1]) + cv2.circle(img, coord, 4, color, thickness=2) + cv2.putText(img, str(k), (coord[0]+10, coord[1]), cv2.FONT_HERSHEY_SIMPLEX, .5, color) + + key = cv2.waitKey(5) # or for arrows: full_key_code = cv2.waitKeyEx(0) + self.key_event(key) + # nr_keys = [ord(i) for i in range(10)] # select/add point + # cv2. + cv2.imshow('laser_calib', img) + + lines = [] + if self._selected_point: + point = self.laser_points[self._selected_point] + lines.extend(cross_points(point[0], point[1], 100, SrgbaColor(0,1,0,1))) + + # render in laser space + rl = RenderableLines(lines, CoordinateSpace.LASER) + self.laser_sock.send_json(rl, cls=DataclassJSONEncoder) + + # print(json.dumps(rl, cls=DataclassJSONEncoder)) + + def key_event(self, key: int): + if key < 0: + return + + if key == ord('q'): + exit() + + if key == 27: #esc + self._selected_point = None + + + if key == ord('c'): + self.calculate_homography() + self.save() + + if key == ord('d') and self._selected_point: + self.delete_point(self._selected_point) + + # arrow up (82), down (84), arrow left(81) + if self._selected_point and key in [81, 84, 82, 83, + ord('h'), ord('j'), ord('k'), ord('l'), + ord('H'), ord('J'), ord('K'), ord('L'), + ]: + diff = [0,0] + if key in [81, ord('h')]: + diff[0] -= 1 + if key == ord('H'): + diff[0] -= 10 + if key in [83, ord('l')]: + diff[0] += 1 + if key == ord('L'): + diff[0] += 10 + + if key in [82, ord('k')]: + diff[1] += 1 + if key == ord('K'): + diff[1] += 10 + if key in [84, ord('j')]: + diff[1] -= 1 + if key == ord('J'): + diff[1] -= 10 + + self.laser_points[self._selected_point] = ( + self.laser_points[self._selected_point][0] + diff[0], + self.laser_points[self._selected_point][1] + diff[1], + ) + + + nr_keys = [ord(str(i)) for i in range(10)] + if key in nr_keys: + select = str(nr_keys.index(key)) + self.create_or_select(select) + + + + + def mouse_event(self, event,x,y,flags,param): + x *= self.frame_img_factor[0] + y *= self.frame_img_factor[1] + if event == cv2.EVENT_MOUSEMOVE: + if not self._is_dragging or not self._selected_point: + return + + self.image_points[self._selected_point] = (x, y) + + if event == cv2.EVENT_LBUTTONDOWN: + # select or create + self._selected_point = None + for i, p in self.image_points.items(): + d = (p[0]-x)**2 + (p[1]-y)**2 + if d < 30: + self._selected_point = i + break + if self._selected_point is None: + self._selected_point = self.new_point((x,y), None) + + self._is_dragging = True + + if event == cv2.EVENT_LBUTTONUP: + self._is_dragging = False + # ... point stays selected to tweak laser + + def create_or_select(self, nr: str): + if nr not in self.image_points: + self.new_point(None, None, nr) + self._selected_point = nr + return nr + + def new_point(self, img_coord: Optional[Coordinate], laser_coord: Optional[Coordinate], nr: Optional[str]=None): + if nr: + new_nr = nr + else: + new_nr = None + for i in range(100): + k = str(i) + if k not in self.image_points: + new_nr = k + break + if not new_nr: + new_nr = 0 # cover unlikely case + + self.image_points[new_nr] = img_coord or (100,100) + self.laser_points[new_nr] = laser_coord or (100,100) + return new_nr + + def delete_point(self, point: str): + del self.image_points[point] + del self.laser_points[point] + self._selected_point = None + + def calculate_homography(self): + if len(self.image_points) < 4: + return + + world_points = self.camera.points_img_to_world(list(self.image_points.values())) + laser_points = np.array(list(self.laser_points.values())) + print('from', world_points) + print('to', laser_points) + self.H, status = cv2.findHomography(world_points, laser_points) + + print('Found') + print(self.H) + + def save(self): + with self.config.calibfile.open('w') as fp: + json.dump({ + 'laser_points': self.laser_points, + 'image_points': self.image_points, + 'H': self.H.tolist() + }, fp) + + + + @classmethod + def arg_parser(cls) -> ArgumentParser: + argparser = ArgumentParser() + argparser.add_argument('--zmq-frame-addr', + help='Manually specity communication addr for the frame messages', + type=str, + default="ipc:///tmp/feeds_frame") + argparser.add_argument('--zmq-stage-addr', + help='Manually specity communication addr for the stage messages (the rendered lines)', + type=str, + default="tcp://0.0.0.0:99174") + argparser.add_argument('--calibfile', + help='specify file to save & load points with', + type=Path, + default=Path("./laser_calib.json")) + + return argparser diff --git a/trap/lines.py b/trap/lines.py index 6317ae5..28ba645 100644 --- a/trap/lines.py +++ b/trap/lines.py @@ -1,7 +1,7 @@ from __future__ import annotations from dataclasses import dataclass -from enum import Enum +from enum import Enum, IntEnum import math from typing import List, Tuple import numpy as np @@ -13,6 +13,12 @@ See [notebook](../test_path_transforms.ipynb) for examples """ RenderablePosition = Tuple[float,float] + +class CoordinateSpace(IntEnum): + CAMERA = 1 + UNDISTORTED_CAMERA = 2 + WORLD = 3 + LASER = 4 @dataclass class SrgbaColor(): @@ -55,12 +61,12 @@ class SimplifyMethod(Enum): class RenderableLine(): points: List[RenderablePoint] - def as_simplified(self, method: SimplifyMethod = SimplifyMethod.RDP): + def as_simplified(self, method: SimplifyMethod = SimplifyMethod.RDP, factor = SIMPLIFY_FACTOR_RDP): linestring = [p.position for p in self.points] if method == SimplifyMethod.RDP: - indexes = simplify_coords_idx(linestring, SIMPLIFY_FACTOR_RDP) + indexes = simplify_coords_idx(linestring, factor) elif method == SimplifyMethod.VW: - indexes = simplify_coords_vw_idx(linestring, SIMPLIFY_FACTOR_VW) + indexes = simplify_coords_vw_idx(linestring, factor) points = [self.points[i] for i in indexes] return RenderableLine(points) @@ -68,11 +74,12 @@ class RenderableLine(): @dataclass class RenderableLines(): lines: List[RenderableLine] + space: CoordinateSpace = CoordinateSpace.WORLD - def as_simplified(self, method: SimplifyMethod = SimplifyMethod.RDP): + def as_simplified(self, method: SimplifyMethod = SimplifyMethod.RDP, factor = SIMPLIFY_FACTOR_RDP): """Wraps RenderableLine simplification""" return RenderableLines( - [line.as_simplified(method) for line in self.lines] + [line.as_simplified(method, factor) for line in self.lines] ) def append(self, rl: RenderableLine): @@ -81,6 +88,9 @@ class RenderableLines(): def append_lines(self, rls: RenderableLines): self.lines.extend(rls.lines) + def point_count(self): + return sum([len(l.points) for l in self.lines]) + # def merge(self, rl: RenderableLines): @@ -91,7 +101,7 @@ def circle_arc(cx, cy, r, t, l, c: SrgbaColor): for l*2pi, offset by t. Both t and l are 0<= [t,l] <= 1 """ - resolution = 40 + resolution = 30 steps = int(resolution * l) offset = int(resolution * t) pointlist: list[RenderablePoint] = [] @@ -102,4 +112,24 @@ def circle_arc(cx, cy, r, t, l, c: SrgbaColor): pointlist.append(RenderablePoint((x, y), c)) - return RenderableLine(pointlist) \ No newline at end of file + return RenderableLine(pointlist) + +def cross_points(cx, cy, r, c: SrgbaColor): + # r = 100 + steps = 3 + pointlist: list[RenderablePoint] = [] + for i in range(steps): + x = int(cx) + y = int(cy + r - i * 2 * r/steps) + pos = (x, y) + pointlist.append(RenderablePoint(pos, c)) + path = RenderableLine(pointlist) + pointlist: list[RenderablePoint] = [] + for i in range(steps): + y = int(cy) + x = int(cx + r - i * 2 * r/steps) + pos = (x, y) + pointlist.append(RenderablePoint(pos, c)) + path2 = RenderableLine(pointlist) + + return [path, path2] \ No newline at end of file diff --git a/trap/monitor.py b/trap/monitor.py new file mode 100644 index 0000000..6c19550 --- /dev/null +++ b/trap/monitor.py @@ -0,0 +1,65 @@ + +from argparse import ArgumentParser +import time +from trap.counter import CounterListerner +from trap.node import Node + + +class Monitor(Node): + """ + Render a stage, on which different TrackScenarios take place to a + single image of lines. Which can be passed to different renderers + E.g. the laser or image renderers. + """ + + FPS = 1 + + def setup(self): + # self.scenarios: List[DrawnScenario] = [] + self.counter_listener = CounterListerner() + + def run(self): + prev_time = time.perf_counter() + while self.is_running.is_set(): + # self.tick() # don't polute it with own data + + self.counter_listener.snapshot() + stats = self.counter_listener.to_string() + if len(stats): + self.logger.info(stats) + # else: + # self.logger.info("no stats") + + # for i, (k, v) in enumerate(self.counter_listener.get_latest().items()): + # print(k,v) + # cv2.putText(img, f"{k} {v.value()}", (20,img.shape[0]-(40*i)-40), cv2.FONT_HERSHEY_PLAIN, 1, base_color, 1) + + # 3) calculate latency for desired FPS + now = time.perf_counter() + time_diff = (now - prev_time) + if time_diff < 1/self.FPS: + # print(f"sleep {1/self.FPS - time_diff}") + time.sleep(1/self.FPS - time_diff) + now += 1/self.FPS - time_diff + + prev_time = now + + + @classmethod + def arg_parser(cls) -> ArgumentParser: + argparser = ArgumentParser() + # argparser.add_argument('--zmq-trajectory-addr', + # help='Manually specity communication addr for the trajectory messages', + # type=str, + # default="ipc:///tmp/feeds_traj") + # argparser.add_argument('--zmq-prediction-addr', + # help='Manually specity communication addr for the prediction messages', + # type=str, + # default="ipc:///tmp/feeds_preds") + # argparser.add_argument('--zmq-stage-addr', + # help='Manually specity communication addr for the stage messages (the rendered lines)', + # type=str, + # default="tcp://0.0.0.0:99174") + return argparser + + \ No newline at end of file diff --git a/trap/node.py b/trap/node.py index cd25295..b366d06 100644 --- a/trap/node.py +++ b/trap/node.py @@ -3,6 +3,7 @@ from logging.handlers import QueueHandler, QueueListener, SocketHandler import multiprocessing from multiprocessing.synchronize import Event as BaseEvent from argparse import ArgumentParser, Namespace +import time from typing import Optional import zmq @@ -19,6 +20,8 @@ class Node(): self.zmq_context = zmq.Context() self.logger = self._logger() + self._prev_loop_time = 0 + self.setup() @classmethod @@ -43,6 +46,21 @@ class Node(): self.tick() return self.is_running.is_set() + def run_loop_capped_fps(self, max_fps: float): + """Use in run(), to check if it should keep looping + Takes care of tick()'ing the iterations/second counter + """ + + now = time.perf_counter() + time_diff = (now - self._prev_loop_time) + if time_diff < 1/max_fps: + # print(f"sleep {1/max_fps - time_diff}") + time.sleep(1/max_fps - time_diff) + now += 1/max_fps - time_diff + self._prev_loop_time = now + + return self.run_loop() + @classmethod def arg_parser(cls) -> ArgumentParser: raise RuntimeError("Not implemented arg_parser()") diff --git a/trap/stage.py b/trap/stage.py index a23fef5..8daeae0 100644 --- a/trap/stage.py +++ b/trap/stage.py @@ -24,7 +24,7 @@ from trap import shapes from trap.base import Camera, DataclassJSONEncoder, DistortedCamera, Frame, ProjectedTrack, Track from trap.counter import CounterSender from trap.laser_renderer import circle_points, rotateMatrix -from trap.lines import RenderableLine, RenderableLines, RenderablePoint, RenderablePosition, SrgbaColor, circle_arc +from trap.lines import RenderableLine, RenderableLines, RenderablePoint, RenderablePosition, SimplifyMethod, SrgbaColor, circle_arc from trap.node import Node from trap.timer import Timer from trap.utils import exponentialDecay, exponentialDecayRounded, relativePointToPolar, relativePolarToPoint @@ -56,6 +56,9 @@ class AppendableLine(LineGenerator): self.ready = len(self.points) == 0 self.draw_decay_speed = draw_decay_speed + def nr_of_passed_points(self): + """The number of points passed in the animation""" + return len(self._drawn_points) - 1 def update_drawn_positions(self, dt: DeltaT): if len(self.points) == 0: @@ -164,6 +167,7 @@ class DiffSegment(): Scenario's anomaly score. """ DRAW_DECAY_SPEED = 25 + POINT_INTERVAL = 4 def __init__(self, prediction: ProjectedTrack): self.ptrack = prediction @@ -177,6 +181,12 @@ class DiffSegment(): def finish(self): self.finished = True + + def nr_of_passed_points(self): + if isinstance(self.line, AppendableLine): + return self.line.nr_of_passed_points() * self.POINT_INTERVAL + else: + return len(self.points) * self.POINT_INTERVAL # run on each track update received def update_track(self, track: ProjectedTrack): @@ -204,7 +214,7 @@ class DiffSegment(): line = [] for i, (p1, p2) in enumerate(zip(trajectory_range, prediction_range)): offset_from_start = (pred_diff_steps_forward + i) - if offset_from_start % 4 == 0: + if offset_from_start % self.POINT_INTERVAL == 0: self.line.points.extend([p1, p2]) self.points.extend([p1, p2]) @@ -222,33 +232,6 @@ class DiffSegment(): if isinstance(self.line, ProceduralChain): self.line.target = self._target_track.projected_history[-1] - # if len(self.points) == 0: - # # nothing to draw yet - # return - - # # self._drawn_points = self.points - - # if len(self._drawn_points) == 0: - # # create origin - # self._drawn_points.append(self.points[0]) - # # and drawing head - # self._drawn_points.append(self.points[0]) - - # idx = len(self._drawn_points) - 1 - # target = self.points[idx] - - # if np.isclose(self._drawn_points[-1], target, atol=.05).all(): - # # TODO: might want to migrate to np.isclose() - # if len(self._drawn_points) == len(self.points): - # self.ready = True - # return # done until a new point is added - # # add new point as drawing head - # self._drawn_points.append(self._drawn_points[-1]) - # self.ready = False - - # x = exponentialDecayRounded(self._drawn_points[-1][0], target[0], self.DRAW_DECAY_SPEED, dt, .05) - # y = exponentialDecayRounded(self._drawn_points[-1][1], target[1], self.DRAW_DECAY_SPEED, dt, .05) - # self._drawn_points[-1] = (float(x), float(y)) # if not self.finished or not self.line.ready: self.line.update_drawn_positions(dt) @@ -284,8 +267,8 @@ PREDICTION_FADE_SLOPE: float = -10 PREDICTION_FADE_AFTER_DURATION: float = 10 # seconds PREDICTION_END_FADE = 2 #frames # TRACK_MAX_POINTS = 100 -TRACK_FADE_AFTER_DURATION = 10. # seconds -TRACK_END_FADE = 50 # points +TRACK_FADE_AFTER_DURATION = 8. # seconds +TRACK_END_FADE = 30 # points TRACK_FADE_ASSUME_FPS = 12 # Don't render the first n points of the prediction, @@ -309,6 +292,7 @@ class TrackScenario(StateMachine): receive_prediction = detected.to(first_prediction) | substantial.to(first_prediction) | first_prediction.to(corrected_prediction, cond="prediction_is_stale") | corrected_prediction.to(play, cond="prediction_is_playing") + def __init__(self): self.track: ProjectedTrack = None self.camera: Optional[Camera] = None @@ -341,7 +325,7 @@ class TrackScenario(StateMachine): return False def check_lost(self): - if self.current_state is not self.lost and self.track and self.track.created_at < time.time() - 5: + if self.current_state is not self.lost and self.track and self.track.updated_at < time.time() - 5: self.mark_lost() def set_track(self, track: ProjectedTrack): @@ -464,8 +448,8 @@ class DrawnScenario(TrackScenario): """ ANOMALY_DECAY = .2 # speed with which the cirlce shrinks over time - DISTANCE_ANOMALY_FACTOR = .05 # the ammount to which the difference counts to the anomaly score - MAX_HISTORY = 80 # points of history of trajectory to display (preventing too long lines) + DISTANCE_ANOMALY_FACTOR = .03 # the ammount to which the difference counts to the anomaly score + MAX_HISTORY = 100 # 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): @@ -525,7 +509,8 @@ class DrawnScenario(TrackScenario): # 1. track history, direct update # positions = self._track.get_projected_history(None, self.camera)[-MAX_HISTORY:] - self.drawn_positions = self.track.projected_history[-self.MAX_HISTORY:] + # self.drawn_positions = self.track.projected_history[-self.MAX_HISTORY:] + self.drawn_positions = self.track.projected_history # 3. predictions if len(self.drawn_predictions) < len(self.predictions): @@ -533,21 +518,24 @@ class DrawnScenario(TrackScenario): if len(self.drawn_predictions) == 0: self.drawn_predictions.append(self.predictions[-1].predictions[0]) else: - # cut existing prediction - end_step = self.predictions[-1].frame_index - self.predictions[-2].frame_index + self.CUT_GAP - # print(end_step) + # if a new prediction has arised, transition from existing one. + # First, cut existing prediction + # CUT_GAP indicates that some is lost in the transition, to prevent glitches when velocity of person changes + end_step = self.predictions[-1].frame_index - self.predictions[-2].frame_index + self.CUT_GAP keep = self.drawn_predictions[-1][end_step:] last_item: Coordinate = keep[-1] - self.drawn_predictions[-1] = self.drawn_predictions[-1][:end_step] + self.drawn_predictions[-1] = self.drawn_predictions[-1][:end_step] # cut the old part # print(self.predictions[-1].frame_index, self.predictions[-2].frame_index, end_step, len(keep)) - ext = [last_item] * (len(self.predictions[-1].predictions[0]) - len(keep)) - # print(ext) + # duplicate last item, so the new one has the same nr. of points as the incoming prediction (so it can actually transition) + ext = [last_item] * (len(self.predictions[-1].predictions[0]) - len(keep)) keep.extend(ext) self.drawn_predictions.append(keep) for a, drawn_prediction in enumerate(self.drawn_predictions): # origin = self.predictions[a].predictions[0][0] origin = self.predictions[a].predictions[0][0] + # associated_diff = self.prediction_diffs[a] + # progress = associated_diff.nr_of_passed_points() 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 @@ -624,7 +612,7 @@ class DrawnScenario(TrackScenario): def to_renderable_lines(self) -> RenderableLines: t = time.time() - track_age = t - self.track.created_at + track_age = t - self.track.updated_at # Should be beginning lines = RenderableLines([]) @@ -633,8 +621,11 @@ class DrawnScenario(TrackScenario): # track_max_points = TRACK_FADE_AFTER_DURATION * TRACK_FADE_ASSUME_FPS - track_age_in_frames # 1. Trajectory history + # drawable_points, alphas = self.drawn_positions[:self.MAX_HISTORY], [1]*len(self.drawn_positions) + drawable_points, alphas = points_fade_out_alpha_mask(self.drawn_positions, track_age, TRACK_FADE_AFTER_DURATION, TRACK_END_FADE) 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)) @@ -642,9 +633,21 @@ class DrawnScenario(TrackScenario): 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.anomaly_score, anomaly_marker_color)) + # last point, (but this draws line in circle, requiring a 'jump back' for the laser) + cx, cy = self.drawn_positions[-1][0], self.drawn_positions[-1][1], + radius = max(.1, self._drawn_anomaly_score * 1.) + + steps=5 + if len(self.drawn_positions) >= steps: + dx, dy = self.drawn_positions[-1][0] - self.drawn_positions[-steps][0], self.drawn_positions[-1][1] - self.drawn_positions[-steps][1], + diff = np.array([dx,dy]) + diff = diff/np.linalg.norm(diff) * radius * 1.1 + cx += diff[0] + cy += diff[1] + lines.append(circle_arc( - self.drawn_positions[-1][0], self.drawn_positions[-1][1], - max(.1, self._drawn_anomaly_score * 1.), + cx, cy, + radius, 0, 1, anomaly_marker_color) ) @@ -655,7 +658,11 @@ class DrawnScenario(TrackScenario): 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: + for a, drawn_prediction in enumerate(self.drawn_predictions): + + + associated_diff = self.prediction_diffs[a] + progress = associated_diff.nr_of_passed_points() # drawn_prediction, alphas1 = points_fade_out_alpha_mask(drawn_prediction, prediction_track_age, TRACK_FADE_AFTER_DURATION, TRACK_END_FADE, no_frame_max=True) @@ -674,6 +681,7 @@ class DrawnScenario(TrackScenario): # 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)] + points = points[progress//2:] lines.append(RenderableLine(points)) # 4. Diffs @@ -870,10 +878,12 @@ class Stage(Node): # rl = RenderableLines(lines) # with open('/tmp/lines.pcl', 'wb') as fp: # pickle.dump(rl, fp) - rl = lines.as_simplified() # or segmentise (see shapely) + rl = lines.as_simplified(SimplifyMethod.RDP, .01) # 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()) # print(rl.__dict__) - self.stage_sock.send_json(rl, cls=DataclassJSONEncoder) + self.stage_sock.send_json(obj=rl, cls=DataclassJSONEncoder) # print(json.dumps(rl, cls=DataclassJSONEncoder))