security_vision/trap
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

Render debug lines from a map

Browse source
This commit is contained in:
Ruben van de Ven 2025-10-17 16:58:18 +02:00
parent 416596797e
commit 1f07974466
6 changed files with 167 additions and 48 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

1
pyproject.toml
View file

@ -36,6 +36,7 @@ dependencies = [
"supervisor>=4.2.5",
"superfsmon>=1.2.3",
"noise>=1.2.2",
"svgpathtools>=1.7.1",
]
[project.scripts]

20
trap/base.py
View file

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

38
trap/cv_renderer.py
View file

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

63
trap/lines.py
View file

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

68
trap/stage.py
View file

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

25
uv.lock
View file

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