From 1f079744662ed20c26bcaf500ce68d1f3fc2124c Mon Sep 17 00:00:00 2001 From: Ruben van de Ven Date: Fri, 17 Oct 2025 16:58:18 +0200 Subject: [PATCH] Render debug lines from a map --- pyproject.toml | 1 + trap/base.py | 20 ------------- trap/cv_renderer.py | 38 +++++++++++++++++++------ trap/lines.py | 63 +++++++++++++++++++++++++++++++++++++++-- trap/stage.py | 68 +++++++++++++++++++++++++++++++++------------ uv.lock | 25 +++++++++++++++++ 6 files changed, 167 insertions(+), 48 deletions(-) diff --git a/pyproject.toml b/pyproject.toml index 40260b8..73f09e4 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -36,6 +36,7 @@ dependencies = [ "supervisor>=4.2.5", "superfsmon>=1.2.3", "noise>=1.2.2", + "svgpathtools>=1.7.1", ] [project.scripts] diff --git a/trap/base.py b/trap/base.py index f4c0a80..fbe0056 100644 --- a/trap/base.py +++ b/trap/base.py @@ -157,12 +157,6 @@ class DistortedCamera(ABC): with calibration_path.open('r') as fp: data = json.load(fp) 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 @@ -177,11 +171,6 @@ class DistortedCamera(ABC): 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) @@ -193,15 +182,6 @@ 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): diff --git a/trap/cv_renderer.py b/trap/cv_renderer.py index b6fe8f5..f296ebc 100644 --- a/trap/cv_renderer.py +++ b/trap/cv_renderer.py @@ -4,6 +4,7 @@ from __future__ import annotations import datetime import json import logging +from pathlib import Path import time from argparse import ArgumentParser, Namespace from multiprocessing.synchronize import Event as BaseEvent @@ -20,6 +21,7 @@ from pyglet import shapes from trap.base import Detection from trap.counter import CounterListerner from trap.frame_emitter import Frame, Track +from trap.lines import load_lines_from_svg from trap.node import Node from trap.preview_renderer import FrameWriter from trap.tools import draw_track_predictions, draw_track_projected, to_point @@ -178,6 +180,11 @@ class CvRenderer(Node): first_time = frame.time # img = frame.img + # save_file = Path("videos/snap.png") + # if not save_file.exists(): + # img = frame.camera.img_to_world(frame.img, 100) + # cv2.imwrite(save_file, img) + img = decorate_frame(frame, tracker_frame, prediction_frame, first_time, self.config, self.tracks, self.predictions, self.detections, self.config.render_clusters) logger.debug(f"write frame {frame.time - first_time:.3f}s") @@ -351,15 +358,30 @@ 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) + + debug_lines = load_lines_from_svg("../DATASETS/hof3/map_hof.svg", scale, '') + for line in debug_lines: + for rp1, rp2 in zip(line.points, line.points[1:]): + p1 = ( + int(rp1.position[0]*scale), + int(rp1.position[1]*scale), + ) + p2 = ( + int(rp2.position[0]*scale), + int(rp2.position[1]*scale), + ) + cv2.line(img, p1, p2, (255,0,0), 2) + # points = [(int(point[0]*scale), int(point[1]*scale)) for point in points] - 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) - + # for num, points in enumerate(frame.camera.debug_lines): + # cv2.line(img, points[0], points[1], (255,0,0), 2) + + + + # 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) 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/lines.py b/trap/lines.py index e6de99c..e8c4bb8 100644 --- a/trap/lines.py +++ b/trap/lines.py @@ -3,10 +3,12 @@ from __future__ import annotations from dataclasses import dataclass from enum import Enum, IntEnum import math -from typing import List, Tuple +from pathlib import Path +from typing import Dict, List, Tuple import numpy as np from simplification.cutil import simplify_coords_idx, simplify_coords_vw_idx +import svgpathtools """ See [notebook](../test_path_transforms.ipynb) for examples @@ -93,7 +95,7 @@ class RenderableLines(): # def merge(self, rl: RenderableLines): - +RenderableLayers = Dict[int, RenderableLines] def circle_arc(cx, cy, r, t, l, c: SrgbaColor): """ @@ -132,4 +134,59 @@ def cross_points(cx, cy, r, c: SrgbaColor): pointlist.append(RenderablePoint(pos, c)) path2 = RenderableLine(pointlist) - return [path, path2] \ No newline at end of file + return [path, path2] + + +def load_lines_from_svg(svg_path: Path, scale: float, c: SrgbaColor) -> List[RenderableLine]: + + lines = [] + paths, attributes = svgpathtools.svg2paths(svg_path) + + for path in paths: + try: + # segments = path.segments + coordinates = [] + for i, segment in enumerate(path): + if isinstance(segment, svgpathtools.Line): + if i == 0: + # avoid duplicate coords + coordinates.append((segment.start.real, segment.start.imag)) + coordinates.append((segment.end.real, segment.end.imag)) + elif isinstance(segment, svgpathtools.Arc): + #Approximating arcs with line segments (adjust steps for precision) + steps = 10 + for i in range(steps + 1): + t = i / steps + x = segment.point(t).real + y = segment.point(t).imag + coordinates.append((x, y)) + elif isinstance(segment, svgpathtools.CubicBezier): + steps = 10 + for i in range(steps + 1): + t = i / steps + x = segment.point(t).real + y = segment.point(t).imag + coordinates.append((x, y)) + + elif isinstance(segment, svgpathtools.QuadraticBezier): + steps = 10 + for i in range(steps + 1): + t = i / steps + x = segment.point(t).real + y = segment.point(t).imag + coordinates.append((x, y)) + else: + print(f"Unsupported segment type: {type(segment)}") + + # Create LineString from coordinates + if len(coordinates) > 1: + coordinates = (np.array(coordinates) / scale).tolist() + points = [RenderablePoint(pos, c) for pos in coordinates] + line = RenderableLine(points) + lines.append(line) + # linestring = shapely.geometry.LineString(coordinates) + # linestrings.append(linestring) + + except Exception as e: + print(f"Error processing path: {e}") + return lines \ No newline at end of file diff --git a/trap/stage.py b/trap/stage.py index 5c7ee22..5d8558d 100644 --- a/trap/stage.py +++ b/trap/stage.py @@ -15,6 +15,7 @@ from matplotlib.pyplot import isinteractive import numpy as np from shapely import LineString, MultiLineString, line_locate_point, linestrings +import shapely from shapely.ops import substring from statemachine import Event, State, StateMachine from statemachine.exceptions import TransitionNotAllowed @@ -26,7 +27,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, SimplifyMethod, SrgbaColor, circle_arc +from trap.lines import RenderableLayers, RenderableLine, RenderableLines, RenderablePoint, RenderablePosition, SimplifyMethod, SrgbaColor, circle_arc, load_lines_from_svg from trap.node import Node from trap.timer import Timer from trap.utils import exponentialDecay, exponentialDecayRounded, lerp, relativePointToPolar, relativePolarToPoint @@ -795,11 +796,33 @@ class DrawnScenario(TrackScenario): ls = substring(ls, 0, t_factor*ls.length, ls.length) # print(prediction_track_age) - dashed = dashed_line(ls, 1, .5, prediction_track_age, False) - # print(dashed) + + # Option 1 : dashes + dashed = dashed_line(ls, .8, 1., prediction_track_age, False) for line in dashed.geoms: dash_points = [RenderablePoint(point, color) for point in line.coords] lines.append(RenderableLine(dash_points)) + + # Option 2 : flash + flash_distance = prediction_track_age * 5 + # flashes = [] + # for i in range(10): + # flashes.append(substring(ls, flash_distance*i, flash_distance + .5)) + + # flash_multiline = shapely.union_all(flashes) + # flashes = flash_multiline.geoms if isinstance(flash_multiline, MultiLineString) else [flash_multiline] + # print(flashes) + # for flash_ls in flashes: + # flash_points = [RenderablePoint(point, color) for point in flash_ls.coords] + # if len(flash_points) > 1: + # lines.append(RenderableLine(flash_points)) + + + # flash_points = [RenderablePoint(point, color) for point in flash_ls.coords] + # if len(flash_points) > 1: + # lines.append(RenderableLine(flash_points)) + + # lines.append(RenderableLine(points)) @@ -937,6 +960,9 @@ class Stage(Node): self.counter = CounterSender() self.frame: Optional[Frame] = None + + debug_color = SrgbaColor(0.,0.,1.,1.) + self.debug_lines = RenderableLines(load_lines_from_svg("../DATASETS/hof3/map_hof.svg", 100, debug_color)) def run(self): @@ -1003,19 +1029,7 @@ class Stage(Node): # 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: @@ -1033,9 +1047,29 @@ class Stage(Node): 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()) + + + # debug_lines = RenderableLines([]) + # if self.frame and hasattr(self.frame.camera, 'debug_lines'): + # 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)) + + # debug_lines.append(RenderableLine(line_points)) + + layers: RenderableLayers = { + 1: rl, + 2: self.debug_lines, + } + # print(rl.__dict__) - self.stage_sock.send_json(obj=rl, cls=DataclassJSONEncoder) + self.stage_sock.send_json(obj=layers, cls=DataclassJSONEncoder) # print(json.dumps(rl, cls=DataclassJSONEncoder)) diff --git a/uv.lock b/uv.lock index 7ba260d..e302579 100644 --- a/uv.lock +++ b/uv.lock @@ -2234,6 +2234,29 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/69/4d/3a493f15f5b80608857ef157f382ace494f51d9031e6bee6082437dd1403/supervisor_win-4.7.0-py2.py3-none-any.whl", hash = "sha256:bd98554c2a0878704c3f3fd95e38965d9986eae6a2ad29f34d73d0aee138a481", size = 303996 }, ] +[[package]] +name = "svgpathtools" +version = "1.7.1" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "numpy" }, + { name = "scipy" }, + { name = "svgwrite" }, +] +sdist = { url = "https://files.pythonhosted.org/packages/df/5c/27c896f25e794d8eb1e75a1ab04fad3fcc272b5251d20f634a669e858da0/svgpathtools-1.7.1.tar.gz", hash = "sha256:beaef20fd78164aa5f0a7d4fd164ef20cb0d3d015cdec50c8c168e9d6547f041", size = 2135227 } +wheels = [ + { url = "https://files.pythonhosted.org/packages/a6/00/c23f53a9e91092239ff6f1fcc39463626e293f6b24898739996fe2a6eebd/svgpathtools-1.7.1-py2.py3-none-any.whl", hash = "sha256:3cbb8ba0e8d200f9639034608d9c55b68efbc1bef99ea99559a3e7cb024fb738", size = 68280 }, +] + +[[package]] +name = "svgwrite" +version = "1.4.3" +source = { registry = "https://pypi.org/simple" } +sdist = { url = "https://files.pythonhosted.org/packages/16/c1/263d4e93b543390d86d8eb4fc23d9ce8a8d6efd146f9427364109004fa9b/svgwrite-1.4.3.zip", hash = "sha256:a8fbdfd4443302a6619a7f76bc937fc683daf2628d9b737c891ec08b8ce524c3", size = 189516 } +wheels = [ + { url = "https://files.pythonhosted.org/packages/84/15/640e399579024a6875918839454025bb1d5f850bb70d96a11eabb644d11c/svgwrite-1.4.3-py3-none-any.whl", hash = "sha256:bb6b2b5450f1edbfa597d924f9ac2dd099e625562e492021d7dd614f65f8a22d", size = 67122 }, +] + [[package]] name = "tensorboard" version = "2.19.0" @@ -2541,6 +2564,7 @@ dependencies = [ { name = "simplification" }, { name = "superfsmon" }, { name = "supervisor" }, + { name = "svgpathtools" }, { name = "tensorboardx" }, { name = "torch", version = "1.12.1", source = { registry = "https://pypi.org/simple" }, marker = "sys_platform != 'linux'" }, { name = "torch", version = "1.12.1+cu113", source = { url = "https://download.pytorch.org/whl/cu113/torch-1.12.1%2Bcu113-cp310-cp310-linux_x86_64.whl" }, marker = "sys_platform == 'linux'" }, @@ -2576,6 +2600,7 @@ requires-dist = [ { name = "simplification", specifier = ">=0.7.12" }, { name = "superfsmon", specifier = ">=1.2.3" }, { name = "supervisor", specifier = ">=4.2.5" }, + { name = "svgpathtools", specifier = ">=1.7.1" }, { name = "tensorboardx", specifier = ">=2.6.2.2,<3" }, { name = "torch", marker = "python_full_version < '3.10' or python_full_version >= '4' or sys_platform != 'linux'", specifier = "==1.12.1" }, { name = "torch", marker = "python_full_version >= '3.10' and python_full_version < '4' and sys_platform == 'linux'", url = "https://download.pytorch.org/whl/cu113/torch-1.12.1%2Bcu113-cp310-cp310-linux_x86_64.whl" },