tracking, dashed line preview, that fades out
This commit is contained in:
Ruben van de Ven
parent
0a4cfc1766
commit
416596797e
6 changed files with 115 additions and 30 deletions
30
trap/base.py
30
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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Reference in a new issue