tracking, dashed line preview, that fades out

trap/base.py

@@ -156,11 +156,19 @@ class DistortedCamera(ABC):
     def from_calibfile(cls, calibration_path, H, fps):
         with calibration_path.open('r') as fp:
             data = json.load(fp)
-        return cls.from_calibdata(data, H, fps)
+        camera = cls.from_calibdata(data, H, fps)
+
+        points_file = calibration_path.with_name('irl_points.json')
+        if points_file.with_name('irl_points.json').exists():
+            with points_file.open('r') as fp:
+                debug_points = json.load(fp)
+            camera.init_debug_data(debug_points)
+            
+        return camera
 
 
     @classmethod
-    def from_paths(cls, calibration_path, h_path, fps):
+    def from_paths(cls, calibration_path: Path, h_path: Path, fps: float):
         H = H_from_path(h_path)
         with calibration_path.open('r') as fp:
             calibdata = json.load(fp)
@@ -168,6 +176,12 @@ class DistortedCamera(ABC):
                 camera = FisheyeCamera.from_calibdata(calibdata, H, fps)
         else:
             camera = Camera.from_calibdata(calibdata, H, fps)
+        
+        points_file = calibration_path.with_name('irl_points.json')
+        if points_file.with_name('irl_points.json').exists():
+            with points_file.open('r') as fp:
+                debug_points = json.load(fp)
+            camera.init_debug_data(debug_points)
         return camera
         
         # return cls.from_calibfile(calibration_path, H, fps)
@@ -179,6 +193,16 @@ class DistortedCamera(ABC):
         coords = self.project_points(coords, scale)
         return coords
     
+    def init_debug_data(self, points: List[List[float, float]]):
+        self.debug_points = points
+        self.debug_lines = [
+            [[11, 6.2], [4.046,6.2] ],
+            [self.debug_points[9], self.debug_points[2]],
+            [self.debug_points[4], self.debug_points[3]],
+            [self.debug_points[6], self.debug_points[7]],
+            [self.debug_points[7], self.debug_points[5]],
+        ]
+
 
 class FisheyeCamera(DistortedCamera):
     def __init__(self, dim1, dim2, dim3, K, D, new_K, scaled_K, balance, H, fps):
@@ -306,7 +330,7 @@ class Detection:
 
     @classmethod
     def from_deepsort(cls, dstrack: DeepsortTrack, frame_nr: int):
-        return cls(dstrack.track_id, *dstrack.to_ltwh(), dstrack.det_conf, DetectionState.from_deepsort_track(dstrack), frame_nr, dstrack.det_class)
+        return cls(dstrack.track_id, *dstrack.to_ltwh(), dstrack.det_conf or 0, DetectionState.from_deepsort_track(dstrack), frame_nr, dstrack.det_class)
     
 
     @classmethod

trap/cv_renderer.py

@@ -2,6 +2,7 @@
 from __future__ import annotations
 
 import datetime
+import json
 import logging
 import time
 from argparse import ArgumentParser, Namespace
@@ -54,6 +55,7 @@ class CvRenderer(Node):
         self.tracks: Dict[str, Track] = {}
         self.predictions: Dict[str, Track] = {}
 
+
     def refresh_labels(self, dt: float):
         """Every frame"""
         
@@ -337,6 +339,7 @@ def decorate_frame(frame: Frame, tracker_frame: Frame, prediction_frame: Frame,
             feet = [int(points[2][0] + .5 * w), points[2][1]]
             cv2.rectangle(img, points[1],  points[2], (255,255,0), 2)
             cv2.circle(img,  points[0], 5, (255,255,0), 2)
+            cv2.putText(img, f"{detection.conf:.02f}", (points[0][0], points[0][1]+20), cv2.FONT_HERSHEY_PLAIN, 1, (255,255,0), 1)
 
 
     def conversion(points):
@@ -349,6 +352,15 @@ def decorate_frame(frame: Frame, tracker_frame: Frame, prediction_frame: Frame,
             inv_H = np.linalg.pinv(tracker_frame.H)
             draw_track_projected(img, track, int(track_id), frame.camera, conversion)
 
+    if hasattr(frame.camera, 'debug_points'):
+        for num, point in enumerate(frame.camera.debug_points):
+            cv2.circle(img, (int(point[0]*scale), int(point[1]*scale)), 5, (255,0,0), 2)
+            cv2.putText(img, f"{num}", (int(point[0]*scale)+20, int(point[1]*scale)), cv2.FONT_HERSHEY_PLAIN, 1, (255,0,0), 1)
+        for num, points in enumerate(frame.camera.debug_lines):
+            points = [(int(point[0]*scale), int(point[1]*scale)) for point in points]
+            cv2.line(img, points[0], points[1], (255,0,0), 2)
+            
+    
     if not prediction_frame:
         cv2.putText(img, f"Waiting for prediction...", (500,17), cv2.FONT_HERSHEY_PLAIN, 1, (255,255,0), 1)
         # continue

trap/laser_calibration.py

@@ -128,7 +128,7 @@ class LaserCalibration(Node):
             else:
                 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)))
+                    lines.extend(cross_points(point[0], point[1], .5, SrgbaColor(0,1,0,1)))
             
                 # render in laser space
                 rl = RenderableLines(lines, CoordinateSpace.LASER)

trap/stage.py

@@ -29,7 +29,7 @@ from trap.laser_renderer import circle_points, rotateMatrix
 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
+from trap.utils import exponentialDecay, exponentialDecayRounded, lerp, relativePointToPolar, relativePolarToPoint
 
 from noise import snoise2
 
@@ -38,9 +38,11 @@ logger = logging.getLogger('trap.stage')
 Coordinate = Tuple[float, float]
 DeltaT = float # delta_t in seconds
 
+OPTION_RENDER_DEBUG = False
 OPTION_POSITION_MARKER = False
 OPTION_GROW_ANOMALY_CIRCLE = False
 # OPTION_RENDER_DIFF_SEGMENT = True
+OPTION_TRACK_NOISE = False
 
 class LineGenerator(ABC):
     @abstractmethod
@@ -331,13 +333,13 @@ class ScenarioScene(Enum):
     LOST = -1
 
 LOST_FADEOUT = 3
-PREDICTION_INTERVAL: float|None = 10 # frames
+PREDICTION_INTERVAL: float|None = 20 # frames
 PREDICTION_FADE_IN: float = 3
 PREDICTION_FADE_SLOPE: float = -10
-PREDICTION_FADE_AFTER_DURATION: float = 10 # seconds
+PREDICTION_FADE_AFTER_DURATION: float = 8 # seconds
 PREDICTION_END_FADE = 2 #frames
 # TRACK_MAX_POINTS = 100
-TRACK_FADE_AFTER_DURATION = 8. # seconds
+TRACK_FADE_AFTER_DURATION = 15. # seconds
 TRACK_END_FADE = 30 # points
 TRACK_FADE_ASSUME_FPS = 12
 
@@ -524,7 +526,7 @@ class DrawnScenario(TrackScenario):
 
     ANOMALY_DECAY = .2 # speed with which the cirlce shrinks over time
     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)
+    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):
@@ -698,7 +700,6 @@ class DrawnScenario(TrackScenario):
         lines = RenderableLines([])
 
         
-        
         # 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
 
@@ -709,7 +710,11 @@ class DrawnScenario(TrackScenario):
         # dt: change speed. Divide to make slower
         # amp: amplitude of noise
         # frequency: make smaller to make longer waves
+        if OPTION_TRACK_NOISE:
             noisy_points = apply_perlin_noise_to_line_normal(self.drawn_positions, t/5, .3, .02)
+        else:
+            noisy_points = self.drawn_positions
+
         drawable_points, alphas = points_fade_out_alpha_mask(noisy_points, track_age, TRACK_FADE_AFTER_DURATION, TRACK_END_FADE)
         color = SrgbaColor(1.,0.,1.,1.-self.lost_factor())
 
@@ -753,7 +758,7 @@ class DrawnScenario(TrackScenario):
             for a, drawn_prediction in enumerate(self.drawn_predictions):
                 if a < (len(self.drawn_predictions) - 1):
                     # not the newest: fade out:
-                    deprecation_age = t - self.predictions[a+1].created_at
+                    deprecation_age = t - self.predictions[a+1].updated_at
                     if deprecation_age > PREDICTION_FADE_IN:
                         # old: skip drawing.
                         continue 
@@ -761,7 +766,7 @@ class DrawnScenario(TrackScenario):
                         fade_factor = 1 - (deprecation_age / PREDICTION_FADE_IN)
                         color = color.as_faded(fade_factor)
                     
-                prediction_track_age = time.time() - self.predictions[a].created_at
+                prediction_track_age = time.time() - self.predictions[a].updated_at
                 t_factor = prediction_track_age / PREDICTION_FADE_IN
 
                 associated_diff = self.prediction_diffs[a]
@@ -788,7 +793,9 @@ class DrawnScenario(TrackScenario):
                 ls = LineString(drawn_prediction)
                 if t_factor < 1:
                     ls = substring(ls, 0, t_factor*ls.length, ls.length)
-                dashed = dashed_line(ls, 1, .5, t)
+
+                # print(prediction_track_age)
+                dashed = dashed_line(ls, 1, .5, prediction_track_age, False)
                 # print(dashed)
                 for line in dashed.geoms:
                     dash_points = [RenderablePoint(point, color) for point in line.coords]
@@ -923,17 +930,19 @@ class Stage(Node):
     def setup(self):
         # self.scenarios: List[DrawnScenario] = []
         self.scenarios: Dict[str, DrawnScenario] = defaultdict(lambda: DrawnScenario())
+        self.frame_noimg_sock = self.sub(self.config.zmq_frame_noimg_addr)
         self.trajectory_sock = self.sub(self.config.zmq_trajectory_addr)
         self.prediction_sock = self.sub(self.config.zmq_prediction_addr)
         self.stage_sock = self.pub(self.config.zmq_stage_addr)
 
         self.counter = CounterSender()
-        self.camera: Optional[DistortedCamera] = None
+        self.frame: Optional[Frame] = None
         
     
     def run(self):
         prev_time = time.perf_counter()
         while self.is_running.is_set():
+
             self.tick() 
 
             # 1) poll & update
@@ -953,6 +962,13 @@ class Stage(Node):
             prev_time = now
 
     def loop_receive(self):
+        # 1) receive frames
+        try:
+            camera_frame: Frame = self.frame_noimg_sock.recv_pyobj(zmq.NOBLOCK)
+            self.frame = camera_frame
+        except zmq.ZMQError as e:
+            pass
+
         # 1) receive predictions
         try:
             prediction_frame: Frame = self.prediction_sock.recv_pyobj(zmq.NOBLOCK)
@@ -984,6 +1000,25 @@ class Stage(Node):
 
     def loop_render(self):
         lines =  RenderableLines([])
+
+                
+        # 0. DEBUG lines:
+        if OPTION_RENDER_DEBUG:
+            if self.frame and hasattr(self.frame.camera, 'debug_lines'):
+                debug_color = SrgbaColor(0.,0.,1.,1.)
+                for points in self.frame.camera.debug_lines:
+                    line_points = []
+                    # interpolate, so the laser can correct the lines
+                    for i in range(20):
+                        t = i / 19
+                        x = lerp(points[0][0], points[1][0], t)
+                        y = lerp(points[0][1], points[1][1], t)
+                        line_points.append(RenderablePoint((x, y), debug_color))
+
+                    lines.append(RenderableLine(line_points))
+
+
+        # 1. Draw each scenario:
         for track_id, scenario in self.scenarios.items():
             scenario.update_drawn_positions()
             
@@ -993,11 +1028,13 @@ class Stage(Node):
         # rl = RenderableLines(lines)
         # with open('/tmp/lines.pcl', 'wb') as fp:
         #     pickle.dump(rl, fp)
+        # rl = lines
         rl = 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())
         # print(rl.__dict__)
+
         self.stage_sock.send_json(obj=rl, cls=DataclassJSONEncoder)
         
         # print(json.dumps(rl, cls=DataclassJSONEncoder))
@@ -1005,6 +1042,10 @@ class Stage(Node):
     @classmethod
     def arg_parser(cls) -> ArgumentParser:
         argparser = ArgumentParser()
+        argparser.add_argument('--zmq-frame-noimg-addr',
+                            help='Manually specity communication addr for the frame messages',
+                            type=str,
+                            default="ipc:///tmp/feeds_frame2")
         argparser.add_argument('--zmq-trajectory-addr',
                     help='Manually specity communication addr for the trajectory messages',
                     type=str,
@@ -1121,14 +1162,19 @@ def distance(p1, p2):
     return math.hypot(p2[0] - p1[0], p2[1] - p1[1])
 
 
-def dashed_line(line: LineString, dash_len: float, gap_len: float, offset: float = 0) -> MultiLineString:
+def dashed_line(line: LineString, dash_len: float, gap_len: float, offset: float = 0, loop_offset = True) -> MultiLineString:
     total_length = line.length
 
     segments = []
+
+    if loop_offset:
+        # by default, prepend skipped gap
         pos = offset % (dash_len + gap_len)
 
         if pos > gap_len:
             segments.append(substring(line, 0, pos - gap_len))
+    else:
+        pos = offset
 
     while pos < total_length:
         end = min(pos + dash_len, total_length)

trap/tracker.py

@@ -61,23 +61,24 @@ TRACKER_BYTETRACK = 'bytetrack'
 DETECTORS = [DETECTOR_RETINANET, DETECTOR_MASKRCNN, DETECTOR_FASTERRCNN, DETECTOR_YOLOv8, DETECTOR_RTDETR]
 TRACKERS =[TRACKER_DEEPSORT, TRACKER_BYTETRACK]
 
-TRACKER_CONFIDENCE_MINIMUM = .2
+TRACKER_CONFIDENCE_MINIMUM = .001
-TRACKER_BYTETRACK_MINIMUM = .1 # bytetrack can track items iwth lower thershold
+TRACKER_BYTETRACK_MINIMUM = .001 # bytetrack can track items iwth lower thershold
 NON_MAXIMUM_SUPRESSION = 1
 RCNN_SCALE = .4 # seems to have no impact on detections in the corners
 
 def _ultralytics_track(img: cv2.Mat, frame_idx: int, model: UltralyticsModel, **kwargs) -> List[Detection]:
         
-        results: List[UltralyticsResult] = list(model.track(img, persist=True, tracker="custom_bytetrack.yaml", verbose=False, conf=0.000001, **kwargs))
+        results: List[UltralyticsResult] = list(model.track(img, persist=True, tracker="custom_bytetrack.yaml", verbose=False, conf=0.001, **kwargs))
         
         if results[0].boxes is None or results[0].boxes.id is None:
             # work around https://github.com/ultralytics/ultralytics/issues/5968
             return []
         
         boxes = results[0].boxes.xywh.cpu()
+        confidence = results[0].boxes.conf.cpu().tolist()
         track_ids = results[0].boxes.id.int().cpu().tolist()
         classes = results[0].boxes.cls.int().cpu().tolist()
-        return [Detection(track_id, bbox[0]-.5*bbox[2], bbox[1]-.5*bbox[3], bbox[2], bbox[3], 1, DetectionState.Confirmed, frame_idx, class_id) for bbox, track_id, class_id in zip(boxes, track_ids, classes)]
+        return [Detection(track_id, bbox[0]-.5*bbox[2], bbox[1]-.5*bbox[3], bbox[2], bbox[3], conf, DetectionState.Confirmed, frame_idx, class_id) for bbox, track_id, class_id, conf in zip(boxes, track_ids, classes, confidence)]
 
 class Multifile():
     def __init__(self, srcs: List[Path]):
@@ -726,7 +727,7 @@ class Tracker(Node):
         argparser.add_argument("--imgsz",
                             help="Detector imgsz parameter (applicable to ultralytics detectors)",
                             type=int,
-                            default=480)
+                            default=640)
         return argparser
 
 

trap/video_sources.py

@@ -72,9 +72,11 @@ class GigE(VideoSource):
         self.converter_settings.SetDebayerFormat('BGR8') # opencv
         self.converter_settings.SetDemosaicingMethod(neoapi.ConverterSettings.Demosaicing_Baumer5x5)
         # self.converter_settings.SetSharpeningMode(neoapi.ConverterSettings.Sharpening_Global)
+        # self.converter_settings.SetSharpeningMode(neoapi.ConverterSettings.Sharpening_Adaptive)
         # self.converter_settings.SetSharpeningMode(neoapi.ConverterSettings.Sharpening_ActiveNoiseReduction)
-        # self.converter_settings.SetSharpeningFactor(3)
+        self.converter_settings.SetSharpeningMode(neoapi.ConverterSettings.Sharpening_Off)
-        # self.converter_settings.SetSharpeningSensitivityThreshold(2)
+        self.converter_settings.SetSharpeningFactor(1)
+        self.converter_settings.SetSharpeningSensitivityThreshold(2)
         
 
         
@@ -91,12 +93,12 @@ class GigE(VideoSource):
             self.camera.f.OffsetX.Set(self.config.offset_x)
             self.camera.f.OffsetY.Set(self.config.offset_y)
 
-            # print('exposure time', self.camera.f.ExposureAutoMaxValue.Set(20000)) # shutter 1/50
+            # print('exposure time', self.camera.f.ExposureAutoMaxValue.Set(20000)) # shutter 1/50 (hence; 1000000/shutter)
-            print('exposure time', self.camera.f.ExposureAutoMaxValue.Set(25000))
+            print('exposure time', self.camera.f.ExposureAutoMaxValue.Set(60000)) # otherwise it becomes too blurry in movements
             print('brightness targt', self.camera.f.BrightnessAutoNominalValue.Get())
-            print('brightness targt', self.camera.f.BrightnessAutoNominalValue.Set(30))
+            print('brightness targt', self.camera.f.BrightnessAutoNominalValue.Set(35))
             print('exposure time', self.camera.f.ExposureTime.Get())
-            print('Gamma', self.camera.f.Gamma.Set(0.39))
+            print('Gamma', self.camera.f.Gamma.Set(0.45))
 
             # neoapi.region
             # self.camera.f.regeo