diff --git a/trap/base.py b/trap/base.py index b5ff242..f4c0a80 100644 --- a/trap/base.py +++ b/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) @@ -178,6 +192,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): @@ -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 diff --git a/trap/cv_renderer.py b/trap/cv_renderer.py index a02d95e..b6fe8f5 100644 --- a/trap/cv_renderer.py +++ b/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): @@ -348,6 +351,15 @@ def decorate_frame(frame: Frame, tracker_frame: Frame, prediction_frame: Frame, for track_id, track in tracks.items(): 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) diff --git a/trap/laser_calibration.py b/trap/laser_calibration.py index 1848f8b..6d13ebe 100644 --- a/trap/laser_calibration.py +++ b/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) diff --git a/trap/stage.py b/trap/stage.py index e8a9ddd..5c7ee22 100644 --- a/trap/stage.py +++ b/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): @@ -696,7 +698,6 @@ class DrawnScenario(TrackScenario): t = time.time() track_age = t - self.track.updated_at # Should be beginning lines = RenderableLines([]) - # track_age_in_frames = int(track_age * TRACK_FADE_ASSUME_FPS) @@ -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 - noisy_points = apply_perlin_noise_to_line_normal(self.drawn_positions, t/5, .3, .02) + 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 = [] - pos = offset % (dash_len + gap_len) - if pos > gap_len: - segments.append(substring(line, 0, pos - gap_len)) + 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) diff --git a/trap/tracker.py b/trap/tracker.py index 2117ae2..cc607ad 100644 --- a/trap/tracker.py +++ b/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_BYTETRACK_MINIMUM = .1 # bytetrack can track items iwth lower thershold +TRACKER_CONFIDENCE_MINIMUM = .001 +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 diff --git a/trap/video_sources.py b/trap/video_sources.py index 28ec519..cc798d0 100644 --- a/trap/video_sources.py +++ b/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.SetSharpeningSensitivityThreshold(2) + self.converter_settings.SetSharpeningMode(neoapi.ConverterSettings.Sharpening_Off) + 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(25000)) + # print('exposure time', self.camera.f.ExposureAutoMaxValue.Set(20000)) # shutter 1/50 (hence; 1000000/shutter) + 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