prevent infinite history track

trap/frame_emitter.py

@@ -95,9 +95,7 @@ class Track:
         coords = self.get_projected_history(H)
         return [{"x":c[0], "y":c[1]} for c in coords]
 
- 
 
-    
 
 @dataclass
 class Frame:

trap/renderer.py

@@ -18,16 +18,31 @@ import tempfile
 from pathlib import Path
 import shutil
 import math
+from collections import deque
 
 from pyglet import shapes
 from PIL import Image
 
 from trap.frame_emitter import DetectionState, Frame, Track
-
+from dataclasses import dataclass
 
 
 logger = logging.getLogger("trap.renderer")
 
+@dataclass
+class LineAnimConfig:
+    start_opacity: float
+    target_opacity: float
+    decay: float
+
+@dataclass
+class AnimConfig:
+    clear_color = (0,0,0,0)
+    video_opacity = 100 # on 255 scale
+    tracks: LineAnimConfig = LineAnimConfig(80, 180, 60)
+    predictions: LineAnimConfig = LineAnimConfig(20, 50, 10)
+
+
 class FrameAnimation:
     def __init__(self, frame: Frame):
         self.start_time = time.time()
@@ -62,27 +77,48 @@ class DrawnTrack:
         self.update_at = self.created_at = time.time()
         self.track_id = track_id
         self.renderer = renderer
-        self.set_track(track, H)
         self.drawn_positions = []
-        self.drawn_predictions = []
+        # self.drawn_predictions = []
+        self.predictions: deque[DrawnPrediction] = deque(maxlen=2) # TODO; make configurable
         self.shapes: list[pyglet.shapes.Line] = []
-        self.pred_shapes: list[list[pyglet.shapes.Line]] = []
+
+        self.set_track(track, H)
+        self.set_prediction(track)
+    
+    def __del__(self):
+        self.destroy()
+
+    def destroy(self):
+        # TODO)) not working yet 
+        logger.warning(f'del track  {self}')
+        for s in self.shapes:
+            s.delete()
+        
+        for p in self.predictions:
+            p.destroy()
 
     def set_track(self, track: Track, H):
         self.update_at = time.time()
 
         self.track = track
         self.H = H
-        self.coords = [d.get_foot_coords() for d in track.history]
+        self.coords = [d.get_foot_coords() for d in track.history[-30:]] # maximum of 30 past positions (prevent collapse of FPS on loitering object)
 
         # perhaps only do in constructor:
         self.inv_H = np.linalg.pinv(self.H)
 
+    def set_prediction(self, track: Track):
+        # TODO: turn into add_prediction
         pred_coords = []
+    
+        if not track.predictions:
+            return
+        
         for pred_i, pred in enumerate(track.predictions):
             pred_coords.append(cv2.perspectiveTransform(np.array([pred]), self.inv_H)[0].tolist())
         
-        self.pred_coords = pred_coords
+        # self.pred_coords = pred_coords
+        self.predictions.append(DrawnPrediction(self, pred_coords))
             # color = (128,0,128) if pred_i else (128,
         
         
@@ -92,29 +128,32 @@ class DrawnTrack:
         '''
         # TODO: make lerp, currently quick way to get results
         for i, pos in enumerate(self.drawn_positions):
-            self.drawn_positions[i][0] = int(exponentialDecay(self.drawn_positions[i][0], self.coords[i][0], 16, dt))
-            self.drawn_positions[i][1] = int(exponentialDecay(self.drawn_positions[i][1], self.coords[i][1], 16, dt))
+            self.drawn_positions[i][0] = int(exponentialDecay(self.drawn_positions[i][0], self.coords[i][0], AnimConfig.tracks.decay, dt))
+            self.drawn_positions[i][1] = int(exponentialDecay(self.drawn_positions[i][1], self.coords[i][1], AnimConfig.tracks.decay, dt))
 
         if len(self.coords) > len(self.drawn_positions):
             self.drawn_positions.extend(self.coords[len(self.drawn_positions):])
 
-        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 = 6
-                origin = self.drawn_positions[-1]
-                drawn_r, drawn_angle = relativePointToPolar(  origin, drawn_prediction[i])
-                pred_r, pred_angle = relativePointToPolar(origin, self.pred_coords[a][i])
-                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(x), int(y)
-                # self.drawn_predictions[i][0] = int(exponentialDecay(self.drawn_predictions[i][0], self.pred_coords[i][0], decay, dt))
-                # self.drawn_predictions[i][1] = int(exponentialDecay(self.drawn_predictions[i][1], self.pred_coords[i][1], decay, dt))
+        # Superseded by individual drawnprediction elements
+        # 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 = 6
+        #         origin = self.drawn_positions[-1]
+        #         drawn_r, drawn_angle = relativePointToPolar(  origin, drawn_prediction[i])
+        #         pred_r, pred_angle = relativePointToPolar(origin, self.pred_coords[a][i])
+        #         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(x), int(y)
+        #         # self.drawn_predictions[i][0] = int(exponentialDecay(self.drawn_predictions[i][0], self.pred_coords[i][0], decay, dt))
+        #         # self.drawn_predictions[i][1] = int(exponentialDecay(self.drawn_predictions[i][1], self.pred_coords[i][1], decay, dt))
+
+        #     if len(self.pred_coords) > len(self.drawn_predictions):
+        #         self.drawn_predictions.extend(self.pred_coords[len(self.drawn_predictions):])
+
 
-        if len(self.pred_coords) > len(self.drawn_predictions):
-            self.drawn_predictions.extend(self.pred_coords[len(self.drawn_predictions):])
         # 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):])
@@ -139,7 +178,7 @@ class DrawnTrack:
             if ci >= len(self.shapes):
             # TODO: add color2
                 line = self.renderer.gradientLine(x, y, x2, y2, 3, color, color, batch=self.renderer.batch_anim)
-                line.opacity = 5
+                line.opacity = AnimConfig.tracks.start_opacity
                 self.shapes.append(line)
                 
             else:
@@ -147,50 +186,111 @@ class DrawnTrack:
                 line.x, line.y = x, y
                 line.x2, line.y2 = x2, y2
                 line.color = color
-                line.opacity = int(exponentialDecay(line.opacity, 180, 3, dt))
+                line.opacity = int(exponentialDecay(line.opacity, AnimConfig.tracks.target_opacity, AnimConfig.tracks.start_opacity, dt))
     
         # TODO: basically a duplication of the above,  do this smarter?
         # TODO: add intermediate segment
-        color = colorset[self.track_id % len(colorset)]
+        #color = colorset[self.track_id % len(colorset)]
 
-        for a, drawn_predictions in enumerate(self.drawn_predictions):
-            if len(self.pred_shapes) <= a:
-                self.pred_shapes.append([])
+        for drawn_prediction in self.predictions:
+            drawn_prediction.update_opacities(dt)
+        
+        # for a, drawn_predictions in enumerate(self.drawn_predictions):
+        #     if len(self.pred_shapes) <= a:
+        #         self.pred_shapes.append([])
 
-            if len(self.pred_shapes[a]) > (len(drawn_predictions) +1):
-                self.pred_shapes[a] = self.pred_shapes[a][:len(drawn_predictions)]
+        #     if len(self.pred_shapes[a]) > (len(drawn_predictions) +1):
+        #         self.pred_shapes[a] = self.pred_shapes[a][:len(drawn_predictions)]
 
-            # for i, pos in drawn_predictions.enumerate():
-            for ci in range(0, len(drawn_predictions)):
-                if ci == 0:
-                    x, y = [int(p) for p in self.drawn_positions[-1]]
-                else:
-                    x, y = [int(p) for p in drawn_predictions[ci-1]]
+        #     # for i, pos in drawn_predictions.enumerate():
+        #     for ci in range(0, len(drawn_predictions)):
+        #         if ci == 0:
+        #             x, y = [int(p) for p in self.drawn_positions[-1]]
+        #         else:
+        #             x, y = [int(p) for p in drawn_predictions[ci-1]]
 
-                x2, y2 = [int(p) for p in drawn_predictions[ci]]
+        #         x2, y2 = [int(p) for p in drawn_predictions[ci]]
 
-                y, y2 = self.renderer.window.height - y, self.renderer.window.height - y2
-                # color = [255,0,0]
-                # print(x,y,x2,y2,color)
+        #         y, y2 = self.renderer.window.height - y, self.renderer.window.height - y2
+        #         # color = [255,0,0]
+        #         # print(x,y,x2,y2,color)
 
-                if ci >= len(self.pred_shapes[a]):
-                # TODO: add color2
-                    line = self.renderer.gradientLine(x, y, x2, y2, 3, color, color, batch=self.renderer.batch_anim)
-                    line.opacity = 5
-                    self.pred_shapes[a].append(line)
+        #         if ci >= len(self.pred_shapes[a]):
+        #         # TODO: add color2
+        #             line = self.renderer.gradientLine(x, y, x2, y2, 3, color, color, batch=self.renderer.batch_anim)
+        #             line.opacity = 5
+        #             self.pred_shapes[a].append(line)
                     
+        #         else:
+        #             line = self.pred_shapes[a][ci-1]
+        #             line.x, line.y = x, y
+        #             line.x2, line.y2 = x2, y2
+        #             line.color = color
+        #             decay = (16/ci) if ci else 16
+        #             half = len(drawn_predictions) / 2
+        #             if ci < half:
+        #                 target_opacity = 180
+        #             else:
+        #                 target_opacity = (1 - ((ci - half) / half)) * 180
+        #             line.opacity = int(exponentialDecay(line.opacity, target_opacity, decay, dt))
+ 
+class DrawnPrediction:
+    def __init__(self, drawn_track: DrawnTrack, coords: list[list] = []):
+        self.created_at = time.time()
+        # self.renderer = renderer
+        self.drawn_track = drawn_track
+        self.coords = coords
+        self.color = colorset[self.drawn_track.track_id % len(colorset)]
+        self.pred_shapes: list[list[pyglet.shapes.Line]] = []
+
+        # coords is a list of predictions
+        for a, coords in enumerate(self.coords):
+            prediction_shapes = []
+            for ci in range(0, len(coords)):
+                if ci == 0:
+                    x, y = [int(p) for p in self.drawn_track.coords[-1]]
                 else:
-                    line = self.pred_shapes[a][ci-1]
-                    line.x, line.y = x, y
-                    line.x2, line.y2 = x2, y2
-                    line.color = color
-                    decay = (16/ci) if ci else 16
-                    half = len(drawn_predictions) / 2
-                    if ci < half:
-                        target_opacity = 180
-                    else:
-                        target_opacity = (1 - ((ci - half) / half)) * 180
-                    line.opacity = int(exponentialDecay(line.opacity, target_opacity, decay, dt))
+                    x, y = [int(p) for p in coords[ci-1]]
+
+                x2, y2 = [int(p) for p in coords[ci]]
+
+                # flip in window:
+                y, y2 = self.drawn_track.renderer.window.height - y, self.drawn_track.renderer.window.height - y2
+                
+                line = self.drawn_track.renderer.gradientLine(x, y, x2, y2, 3, self.color, self.color, batch=self.drawn_track.renderer.batch_anim)
+                line.opacity = 5
+                prediction_shapes.append(line)
+            self.pred_shapes.append(prediction_shapes)
+    
+    def destroy(self):
+        for pred in self.pred_shapes:
+            for shape in pred:
+                shape.delete()
+
+
+    def update_opacities(self, dt: float):
+        """
+        Update the opacties of the drawn line, by only using the dt provided by the renderer
+        Done using exponential decal, with a different decay value per item
+        """
+        for a, coords in enumerate(self.coords):
+            for ci in range(0, len(coords)):
+                line = self.pred_shapes[a][ci-1]
+                # Positions of prediction no longer update
+                # line.x, line.y = x, y
+                # line.x2, line.y2 = x2, y2
+                # line.color = color
+
+                # lower decay for further points slows down fade in
+                decay = (AnimConfig.predictions.decay/(3*ci)) if ci else AnimConfig.predictions.decay
+                half = len(coords) / 2
+                
+                if ci < half:
+                    target_opacity = AnimConfig.predictions.target_opacity
+                else:
+                    target_opacity = (1 - ((ci - half) / half)) * AnimConfig.predictions.target_opacity
+                line.opacity = int(exponentialDecay(line.opacity, target_opacity, decay, dt))
+                # logger.info(f"{target_opacity=}, {line.opacity=}")
         
 
 class FrameWriter:
@@ -279,7 +379,8 @@ class Renderer:
         self.window.set_handler('on_refresh', self.on_refresh)
         self.window.set_handler('on_close', self.on_close)
 
-        pyglet.gl.glClearColor(81./255, 20/255, 46./255, 0)
+        # Purple background color:
+        pyglet.gl.glClearColor(*AnimConfig.clear_color)
         self.fps_display = pyglet.window.FPSDisplay(window=self.window, color=(255,255,255,255))
         self.fps_display.label.x = self.window.width - 50
         self.fps_display.label.y = self.window.height - 17
@@ -415,34 +516,32 @@ class Renderer:
         
 
     def check_frames(self, dt):
-        new_tracks = False
         try:
             self.frame: Frame = self.frame_sock.recv_pyobj(zmq.NOBLOCK)
             if not self.first_time:
                 self.first_time = self.frame.time
             img = cv2.GaussianBlur(self.frame.img, (15, 15), 0)
-            img = cv2.flip(cv2.cvtColor(img, cv2.COLOR_BGR2RGB), 0)
-            img = pyglet.image.ImageData(self.frame_size[0], self.frame_size[1], 'RGB', img.tobytes())
+            img = cv2.cvtColor(cv2.cvtColor(img, cv2.COLOR_BGR2GRAY), cv2.COLOR_GRAY2RGB)
+            channels = 3 # unfortunately, pyglet seems to draw single channel as Red only
+            img = pyglet.image.ImageData(self.frame_size[0], self.frame_size[1], 'RGB', img.tobytes(), pitch=self.frame_size[0] * -1 * channels)
             # don't draw in batch, so that it is the background
             self.video_sprite = pyglet.sprite.Sprite(img=img, batch=self.batch_bg)
-            self.video_sprite.opacity = 100
+            self.video_sprite.opacity = AnimConfig.video_opacity
         except zmq.ZMQError as e:
             # idx = frame.index if frame else "NONE"
             # logger.debug(f"reuse video frame {idx}")
             pass
         try:
             self.prediction_frame: Frame = self.prediction_sock.recv_pyobj(zmq.NOBLOCK)
-            new_tracks = True
+            self.update_predictions()
         except zmq.ZMQError as e:
             pass
         try:
             self.tracker_frame: Frame = self.tracker_sock.recv_pyobj(zmq.NOBLOCK)
-            new_tracks = True
+            self.update_tracks()
         except zmq.ZMQError as e:
             pass
         
-        if new_tracks:
-            self.update_tracks()
 
     def update_tracks(self):
         """Updates the track objects and shapes. Called after setting `prediction_frame`
@@ -454,20 +553,31 @@ class Renderer:
         #         # TODO fade out
         #         del self.drawn_tracks[track_id]
         
-        if self.prediction_frame:
-            for track_id, track in self.prediction_frame.tracks.items():
+        if self.tracker_frame:
+            for track_id, track in self.tracker_frame.tracks.items():
                 if track_id not in self.drawn_tracks:
-                    self.drawn_tracks[track_id] = DrawnTrack(track_id, track, self, self.prediction_frame.H)
+                    self.drawn_tracks[track_id] = DrawnTrack(track_id, track, self, self.tracker_frame.H)
                 else:
-                    self.drawn_tracks[track_id].set_track(track, self.prediction_frame.H)
+                    self.drawn_tracks[track_id].set_track(track, self.tracker_frame.H)
         
         # clean up
         for track_id in list(self.drawn_tracks.keys()):
             # TODO make delay configurable
             if self.drawn_tracks[track_id].update_at < time.time() - 5:
                 # TODO fade out
+                self.drawn_tracks[track_id].destroy()
                 del self.drawn_tracks[track_id]
+    
+    def update_predictions(self):
+        if self.prediction_frame:
+            for track_id, track in self.prediction_frame.tracks.items():
+                if track_id not in self.drawn_tracks:
+                    self.drawn_tracks[track_id] = DrawnTrack(track_id, track, self, self.prediction_frame.H)
+                    logger.warning("Prediction for uninitialised frame. This should not happen? (maybe huge delay in prediction?)")
+                else:
+                    self.drawn_tracks[track_id].set_prediction(track)
 
+        
     
     def on_key_press(self, symbol, modifiers):
         print('A key was pressed, use f to hide')
@@ -501,14 +611,16 @@ class Renderer:
         self.window.clear()
 
         self.batch_bg.draw()
+
         
         for track in self.drawn_tracks.values():
             for shape in track.shapes:
                 shape.draw() # for some reason the batches don't work
         for track in self.drawn_tracks.values():
-            for shapes in track.pred_shapes:
-                for shape in shapes:
-                    shape.draw()
+            for prediction in track.predictions:
+                for shapes in prediction.pred_shapes:
+                    for shape in shapes:
+                        shape.draw()
         # self.batch_anim.draw()
         self.batch_overlay.draw()
         
@@ -670,12 +782,12 @@ def decorate_frame(frame: Frame, prediction_frame: Frame, first_time: float, con
     # or https://api.arcade.academy/en/stable/index.html (supports gradient color in line -- "Arcade is built on top of Pyglet and OpenGL.")
     frame.img
 
-    overlay = np.zeros(frame.img.shape, np.uint8)
-    # Fill image with red color(set each pixel to red)
-    overlay[:] = (130, 0, 75)
+    # # Fill image with red color(set each pixel to red)
+    # overlay = np.zeros(frame.img.shape, np.uint8)
+    # overlay[:] = (130, 0, 75)
 
-    img = cv2.addWeighted(frame.img, .4, overlay, .6, 0)
-    # img = frame.img.copy()
+    # img = cv2.addWeighted(frame.img, .4, overlay, .6, 0)
+    img = frame.img.copy()
 
     # all not working:
     # if i == 1: