From 8829189908780bea6cf1b85f3f096f03bc9c01e5 Mon Sep 17 00:00:00 2001 From: Ruben van de Ven Date: Wed, 26 Mar 2025 09:47:37 +0100 Subject: [PATCH] laser optimizations --- trap/laser_renderer.py | 74 ++++++++++++++++++++++++++++++++++++++---- 1 file changed, 68 insertions(+), 6 deletions(-) diff --git a/trap/laser_renderer.py b/trap/laser_renderer.py index aae43a6..f0e4842 100644 --- a/trap/laser_renderer.py +++ b/trap/laser_renderer.py @@ -70,6 +70,7 @@ class LaserFrame(): steps = int(distance // point_interval) for step in range(steps+1): # have both 0 and 1 in the lerp for empty points t = step/(steps+1) + t = 1 # just asap to starting point of next shape x = int(lerp(a.x, b.x, t)) y = int(lerp(a.y, b.y, t)) points.append(LaserPoint(x,y, (0,0,0), 0, True)) @@ -97,6 +98,58 @@ class LaserPath(): def first(self): return self.points[0] + def as_array(self): + np.array([[p.x, p.y] for p in self.points]) + + def simplyfied_path(self, start_v= 10., max_v= 20., a = 2): + """walk over the path with specific velocity, + continuously accelerate (a) until max_v is reached + place point at each step + + (see also tools.transition_path_points() ) + """ + if len(self.points) < 1: + return self.points + + path = self.as_array() + + # new_path = np.array([]) + lengths = np.sqrt(np.sum(np.diff(path, axis=0)**2, axis=1)) + cum_lenghts = np.cumsum(lengths) + # distance = cum_lenghts[-1] * t + # ts = np.concatenate((np.array([0.]), cum_lenghts / cum_lenghts[-1])) + # print(cum_lenghts[-1]) + # DRAW_SPEED = 35 # fixed speed (independent of lenght) TODO)) make variable + # ts = np.concatenate((np.array([0.]), cum_lenghts / DRAW_SPEED)) + new_path = [path[0]] + + position = 0 + next_pos = position + v + + for a, b, pos in zip(path[:-1], path[1:], cum_lenghts): + # TODO)) + if pos < (next_pos): + continue + + v = min(v+a, max_v) + next_pos = position + v + + relative_t = inv_lerp(t_a, t_b, t) + + pass + + # for a, b, t_a, t_b in zip(path[:-1], path[1:], ts[:-1], ts[1:]): + # if t_b < t: + # new_path.append(b) + # continue + # # interpolate + # relative_t = inv_lerp(t_a, t_b, t) + # x = lerp(a[0], b[0], relative_t) + # y = lerp(a[1], b[1], relative_t) + # new_path.append([x,y]) + # break + # return np.array(new_path) + class LaserPoint(): def __init__(self,x,y,c: Color = (255,0,0),i= 255,blank=False): self.x = x @@ -284,14 +337,18 @@ class LaserRenderer: first_time = tracker_frame.time - paths = render_frame_to_pointlist( tracker_frame, prediction_frame, first_time, self.config, self.tracks, self.predictions, self.config.render_clusters) + # print('-------') + paths = render_frame_to_pathlist( tracker_frame, prediction_frame, first_time, self.config, self.tracks, self.predictions, self.config.render_clusters) + print(f"Paths: {len(paths)} ... points {sum([len(p.points) for p in paths])}") laserframe = LaserFrame(paths) # pointlist=pointlist_test # print([(p.x, p.y) for p in pointlist]) # pointlist.extend(pointlist_test) - pointlist = laserframe.get_points_interpolated_by_distance(2, last_laser_point) - # print(len(pointlist)) + pointlist = laserframe.get_points_interpolated_by_distance(30, last_laser_point) + print(len(pointlist)) + # pointlist = pointlist[::2] + # print('decimated', len(pointlist)) if len(pointlist): last_laser_point = pointlist[-1] @@ -299,7 +356,7 @@ class LaserRenderer: frameType = CHeliosPoint * len(pointlist) frame = frameType() - # print(len(pointlist), last_laser_point.x, last_laser_point.y) + # print(len(pointlist)) #, last_laser_point.x, last_laser_point.y) for j, point in enumerate(pointlist): frame[j] = CHeliosPoint(int(point.x), int(point.y), point.color[0],point.color[1], point.color[2], point.i) @@ -363,7 +420,7 @@ def get_animation_position(track: Track, current_frame: Frame) -> float: def circle_points(cx, cy, r, c: Color): r = 100 - steps = 100 + steps = 30 pointlist: list[LaserPoint] = [] for i in range(steps): x = int(cx + math.cos(i * (2*math.pi)/steps) * r) @@ -384,7 +441,7 @@ def world_points_to_laser_points(points): return cv2.perspectiveTransform(np.array([points]), laser_H) # Deprecated -def render_frame_to_pointlist(tracker_frame: Optional[Frame], prediction_frame: Optional[Frame], first_time: Optional[float], config: Namespace, tracks: Dict[str, Track], predictions: Dict[str, Track], as_clusters = True): +def render_frame_to_pathlist(tracker_frame: Optional[Frame], prediction_frame: Optional[Frame], first_time: Optional[float], config: Namespace, tracks: Dict[str, Track], predictions: Dict[str, Track], as_clusters = True): # TODO: replace opencv with QPainter to support alpha? https://doc.qt.io/qtforpython-5/PySide2/QtGui/QPainter.html#PySide2.QtGui.PySide2.QtGui.QPainter.drawImage # or https://github.com/pygobject/pycairo?tab=readme-ov-file # or https://pyglet.readthedocs.io/en/latest/programming_guide/shapes.html @@ -444,6 +501,11 @@ def render_frame_to_pointlist(tracker_frame: Optional[Frame], prediction_frame: LaserPath(circle_points(0xFFF/2+2*test_r, 0xFFF/2, test_r, track_c)) ) else: + # if not len(tracks): + # paths.append( + # LaserPath(circle_points(0xFFF/2+4*test_r, 0xFFF/2, test_r/2, pred_c)) + # ) + for track_id, track in tracks.items(): inv_H = np.linalg.pinv(tracker_frame.H) history = track.get_projected_history(camera=config.camera)