Render debug lines from a map

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]

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):

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, points in enumerate(frame.camera.debug_lines):
-        for num, point in enumerate(frame.camera.debug_points):
+    #     cv2.line(img, points[0], points[1], (255,0,0), 2)
-            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]
+    # if hasattr(frame.camera, 'debug_points'):
-            cv2.line(img, points[0], points[1], (255,0,0), 2)
+    #     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)

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]
+    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

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))
 

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" },