Compare commits
No commits in common. "main" and "lidar" have entirely different histories.
30 changed files with 540 additions and 5522 deletions
|
|
@ -2,12 +2,12 @@
|
|||
"batch_size": 512,
|
||||
"grad_clip": 1.0,
|
||||
"learning_rate_style": "exp",
|
||||
"learning_rate": 0.001,
|
||||
"learning_rate": 0.01,
|
||||
"min_learning_rate": 1e-05,
|
||||
"learning_decay_rate": 0.9999,
|
||||
"prediction_horizon": 60,
|
||||
"prediction_horizon": 30,
|
||||
"minimum_history_length": 5,
|
||||
"maximum_history_length": 150,
|
||||
"maximum_history_length": 50,
|
||||
"map_encoder": {
|
||||
"PEDESTRIAN": {
|
||||
"heading_state_index": [2, 3],
|
||||
|
|
|
|||
18
README.md
18
README.md
|
|
@ -14,24 +14,12 @@ These are roughly the steps to go from datagathering to training
|
|||
1. Make sure to have some recordings with a fixed camera. [UPDATE: not needed anymore, except for calibration & homography footage]
|
||||
* Recording can be done with `ffmpeg -rtsp_transport udp -i rtsp://USER:PASS@IP:554/Streaming/Channels/1.mp4 hof2-cam-$(date "+%Y%m%d-%H%M").mp4`
|
||||
2. Follow the steps in the auxilary [traptools](https://git.rubenvandeven.com/security_vision/traptools) repository to obtain (1) camera matrix, lens distortion, image dimensions, and (2+3) homography
|
||||
3. Track lidar or video data:
|
||||
1. Video: Run the video source & video tracker nodes:
|
||||
* `uv run trap_video_source --homography ../DATASETS/hof4-test-angle/homography.json --video-src gige://../DATASETS/hof4-test-angle/gige_config.json --calibration ../DATASETS/hof4-test-angle/calibration.json` (Optionally, use recorded video with `--video-src videos/render-source-2025-10-19T21\:09.mp4 --video-offset 300`)
|
||||
* `uv run trap_tracker --smooth-tracks --eval_device cuda:0 --detector ultralytics`
|
||||
2. Lidar: `uv run trap_lidar --min-box-area 0 --pi LOCAL_IP --smooth-tracks`
|
||||
4. Save the tracks emitted by the video or lidar tracker: `uv run trap_track_writer --output-dir EXPERIMENTS/raw/hof-lidar`
|
||||
* Each recording adds a new txt file to the `raw` folder.
|
||||
4. Parse tracker data to Trajectron format: `uv run process_data --src-dir EXPERIMENTS/raw/NAME --dst-dir EXPERIMENTS/trajectron-data/ --name NAME`
|
||||
* Optionally, smooth tracks: `--smooth-tracks`
|
||||
* Optionally, and variations with noise: `--noise-tracks 2` (creates 2 variations)
|
||||
* Optionally, and variations with at a random offset: `--offset-tracks 2` (creates 2 variations)
|
||||
3. Run the tracker, e.g. `uv run tracker --detector ultralytics --homography ../DATASETS/NAME/homography.json --video-src ../DATASETS/NAME/*.mp4 --calibration ../DATASETS/NAME/calibration.json --save-for-training EXPERIMENTS/raw/NAME/`
|
||||
* Note: You can run this right of the camera stream: `uv run tracker --eval_device cuda:0 --detector ultralytics --video-src rtsp://USER:PW@ADDRESS/STREAM --homography ../DATASETS/NAME/homography.json --calibration ../DATASETS/NAME/calibration.json --save-for-training EXPERIMENTS/raw/NAME/`, each recording adding a new file to the `raw` folder.
|
||||
4. Parse tracker data to Trajectron format: `uv run process_data --src-dir EXPERIMENTS/raw/NAME --dst-dir EXPERIMENTS/trajectron-data/ --name NAME` Optionally, smooth tracks: `--smooth-tracks`
|
||||
* Optionally, add a map: ideally a RGB png: 3 layers of 0-255
|
||||
* `uv run process_data --src-dir EXPERIMENTS/raw/NAME --dst-dir EXPERIMENTS/trajectron-data/ --name NAME --smooth-tracks --camera-fps 12 --homography ../DATASETS/NAME/homography.json --calibration ../DATASETS/NAME/calibration.json --filter-displacement 2 --map-img-path ../DATASETS/NAME/map.png`
|
||||
* See [[tests/trajectron_maps.ipynb]] for more info how to do so (e.g. the homography map/scale settings, which are also set in process_data)
|
||||
|
||||
5. Train Trajectron model `uv run trajectron_train --eval_every 10 --vis_every 1 --train_data_dict NAME_train.pkl --eval_data_dict NAME_val.pkl --offline_scene_graph no --preprocess_workers 8 --log_dir EXPERIMENTS/models --log_tag _NAME --train_epochs 100 --conf EXPERIMENTS/config.json --batch_size 256 --data_dir EXPERIMENTS/trajectron-data `
|
||||
* For faster training disalble edges:
|
||||
` uv run trajectron_train --eval_every 200 --train_data_dict dortmund-nostep-nosmooth-noise2-offsets1-f2.0-map-2025-11-11_train.pkl --eval_data_dict dortmund-nostep-nosmooth-noise2-offsets1-f2.0-map-2025-11-11_val.pkl --offline_scene_graph no --preprocess_workers 8 --log_dir /home/ruben/suspicion/trap/SETTINGS/2025-11-dortmund/models --log_tag _dortmund-nostep-nosmooth-noise2-offsets1-f2.0-map-2025-11-11 --train_epochs 100 --conf /home/ruben/suspicion/trap/SETTINGS/2025-11-dortmund/trajectron.json --data_dir SETTINGS/2025-11-dortmund/trajectron --map_encoding --no_edge_encoding --dynamic_edges yes --no_edge_encoding --edge_influence_combine_method max --batch_size 512`
|
||||
6. The run!
|
||||
* `uv run supervisord`
|
||||
<!-- * On a video file (you can use a wildcard) `DISPLAY=:1 uv run trapserv --remote-log-addr 100.69.123.91 --eval_device cuda:0 --detector ultralytics --homography ../DATASETS/NAME/homography.json --eval_data_dict EXPERIMENTS/trajectron-data/hof2s-m_test.pkl --video-src ../DATASETS/NAME/*.mp4 --model_dir EXPERIMENTS/models/models_DATE_NAME/--smooth-predictions --smooth-tracks --num-samples 3 --render-window --calibration ../DATASETS/NAME/calibration.json` (the DISPLAY environment variable is used here to running over SSH connection and display on local monitor)
|
||||
|
|
|
|||
|
|
@ -1,130 +0,0 @@
|
|||
{
|
||||
"batch_size": 512,
|
||||
"grad_clip": 1.0,
|
||||
"learning_rate_style": "exp",
|
||||
"learning_rate": 0.001,
|
||||
"min_learning_rate": 1e-05,
|
||||
"learning_decay_rate": 0.9999,
|
||||
"prediction_horizon": 60,
|
||||
"minimum_history_length": 5,
|
||||
"maximum_history_length": 150,
|
||||
"map_encoder": {
|
||||
"PEDESTRIAN": {
|
||||
"heading_state_index": [2, 3],
|
||||
"patch_size": [
|
||||
50,
|
||||
10,
|
||||
50,
|
||||
90
|
||||
],
|
||||
"map_channels": 3,
|
||||
"hidden_channels": [
|
||||
10,
|
||||
20,
|
||||
5,
|
||||
1
|
||||
],
|
||||
"output_size": 32,
|
||||
"masks": [
|
||||
5,
|
||||
5,
|
||||
5,
|
||||
5
|
||||
],
|
||||
"strides": [
|
||||
1,
|
||||
1,
|
||||
1,
|
||||
1
|
||||
],
|
||||
"dropout": 0.5
|
||||
}
|
||||
},
|
||||
"k": 1,
|
||||
"k_eval": 1,
|
||||
"kl_min": 0.07,
|
||||
"kl_weight": 100.0,
|
||||
"kl_weight_start": 0,
|
||||
"kl_decay_rate": 0.99995,
|
||||
"kl_crossover": 400,
|
||||
"kl_sigmoid_divisor": 4,
|
||||
"rnn_kwargs": {
|
||||
"dropout_keep_prob": 0.75
|
||||
},
|
||||
"MLP_dropout_keep_prob": 0.9,
|
||||
"enc_rnn_dim_edge": 1,
|
||||
"enc_rnn_dim_edge_influence": 1,
|
||||
"enc_rnn_dim_history": 32,
|
||||
"enc_rnn_dim_future": 32,
|
||||
"dec_rnn_dim": 128,
|
||||
"q_z_xy_MLP_dims": null,
|
||||
"p_z_x_MLP_dims": 32,
|
||||
"GMM_components": 1,
|
||||
"log_p_yt_xz_max": 6,
|
||||
"N": 1,
|
||||
"K": 25,
|
||||
"tau_init": 2.0,
|
||||
"tau_final": 0.05,
|
||||
"tau_decay_rate": 0.997,
|
||||
"use_z_logit_clipping": true,
|
||||
"z_logit_clip_start": 0.05,
|
||||
"z_logit_clip_final": 5.0,
|
||||
"z_logit_clip_crossover": 300,
|
||||
"z_logit_clip_divisor": 5,
|
||||
"dynamic": {
|
||||
"PEDESTRIAN": {
|
||||
"name": "SingleIntegrator",
|
||||
"distribution": true,
|
||||
"limits": {}
|
||||
}
|
||||
},
|
||||
"state": {
|
||||
"PEDESTRIAN": {
|
||||
"position": [
|
||||
"x",
|
||||
"y"
|
||||
],
|
||||
"velocity": [
|
||||
"x",
|
||||
"y"
|
||||
],
|
||||
"acceleration": [
|
||||
"x",
|
||||
"y"
|
||||
]
|
||||
}
|
||||
},
|
||||
"pred_state": {
|
||||
"PEDESTRIAN": {
|
||||
"position": [
|
||||
"x",
|
||||
"y"
|
||||
]
|
||||
}
|
||||
},
|
||||
"log_histograms": false,
|
||||
"dynamic_edges": "yes",
|
||||
"edge_state_combine_method": "sum",
|
||||
"edge_influence_combine_method": "max",
|
||||
"edge_addition_filter": [
|
||||
0.25,
|
||||
0.5,
|
||||
0.75,
|
||||
1.0
|
||||
],
|
||||
"edge_removal_filter": [
|
||||
1.0,
|
||||
0.0
|
||||
],
|
||||
"offline_scene_graph": "yes",
|
||||
"incl_robot_node": false,
|
||||
"node_freq_mult_train": false,
|
||||
"node_freq_mult_eval": false,
|
||||
"scene_freq_mult_train": false,
|
||||
"scene_freq_mult_eval": false,
|
||||
"scene_freq_mult_viz": false,
|
||||
"edge_encoding": false,
|
||||
"use_map_encoding": true,
|
||||
"augment": false,
|
||||
"override_attention_radius": []
|
||||
}
|
||||
|
|
@ -16,7 +16,7 @@ dependencies = [
|
|||
"gdown>=4.7.1,<5",
|
||||
"pandas-helper-calc",
|
||||
"tsmoothie>=1.0.5,<2",
|
||||
"pyglet>=2.1.8,<3",
|
||||
"pyglet>=2.0.15,<3",
|
||||
"pyglet-cornerpin>=0.3.0,<0.4",
|
||||
"opencv-python",
|
||||
"setproctitle>=1.3.3,<2",
|
||||
|
|
@ -40,9 +40,6 @@ dependencies = [
|
|||
"velodyne-decoder>=3.1.0",
|
||||
"open3d>=0.19.0",
|
||||
"nptyping>=2.5.0",
|
||||
"py-to-proto>=0.6.0",
|
||||
"grpcio-tools>=1.76.0",
|
||||
"dearpygui>=2.1.0",
|
||||
]
|
||||
|
||||
[project.scripts]
|
||||
|
|
@ -59,15 +56,12 @@ model_train = "trap.models.train:train"
|
|||
trap_video_source = "trap.frame_emitter:FrameEmitter.parse_and_start"
|
||||
trap_video_writer = "trap.frame_writer:FrameWriter.parse_and_start"
|
||||
trap_tracker = "trap.tracker:Tracker.parse_and_start"
|
||||
trap_track_writer = "trap.track_writer:TrackWriter.parse_and_start"
|
||||
trap_lidar = "trap.lidar_tracker:Lidar.parse_and_start"
|
||||
trap_lidar = "trap.lidar_tracker:LidarTracker.parse_and_start"
|
||||
trap_stage = "trap.stage:Stage.parse_and_start"
|
||||
trap_render_stage = "trap.stage_renderer:StageRenderer.parse_and_start"
|
||||
trap_prediction = "trap.prediction_server:PredictionServer.parse_and_start"
|
||||
trap_render_cv = "trap.cv_renderer:CvRenderer.parse_and_start"
|
||||
trap_monitor = "trap.monitor:Monitor.parse_and_start" # migrate timer
|
||||
trap_laser_calibration = "trap.laser_calibration:LaserCalibration.parse_and_start" # migrate timer
|
||||
trap_settings = "trap.settings:Settings.parse_and_start" # migrate timer
|
||||
|
||||
[tool.uv]
|
||||
|
||||
|
|
|
|||
|
|
@ -23,45 +23,23 @@ directory=%(here)s
|
|||
autostart=false
|
||||
|
||||
[program:video]
|
||||
# command=uv run trap_video_source --homography ../DATASETS/hof3/homography.json --video-src ../DATASETS/hof3/hof3-cam-demo-twoperson.mp4 --calibration ../DATASETS/hof3/calibration.json --video-loop
|
||||
command=uv run trap_video_source --homography ../DATASETS/hof3-cam-baumer-cropped/homography.json --video-src gige://../DATASETS/hof3-cam-baumer-cropped/gige_config.json --calibration ../DATASETS/hof3-cam-baumer-cropped/calibration.json
|
||||
command=uv run trap_video_source --homography ../DATASETS/hof3/homography.json --video-src ../DATASETS/hof3/hof3-cam-demo-twoperson.mp4 --calibration ../DATASETS/hof3/calibration.json --video-loop
|
||||
# command=uv run trap_video_source --homography ../DATASETS/hof3-cam-baumer-cropped/homography.json --video-src gige://../DATASETS/hof3-cam-baumer-cropped/gige_config.json --calibration ../DATASETS/hof3-cam-baumer-cropped/calibration.json
|
||||
directory=%(here)s
|
||||
directory=%(here)s
|
||||
|
||||
[program:tracker]
|
||||
command=uv run trap_tracker --smooth-tracks
|
||||
# command=uv run trap_lidar --min-box-area 0 --viz --smooth-tracks
|
||||
# environment=DISPLAY=":0"
|
||||
directory=%(here)s
|
||||
autostart=false
|
||||
|
||||
[program:lidar]
|
||||
command=uv run trap_lidar --min-box-area 0.1 --viz
|
||||
environment=DISPLAY=":0"
|
||||
directory=%(here)s
|
||||
autostart=false
|
||||
|
||||
[program:track_writer]
|
||||
command=uv run trap_track_writer --output-dir EXPERIMENTS/raw/hof-lidar
|
||||
# environment=DISPLAY=":0"
|
||||
directory=%(here)s
|
||||
autostart=false
|
||||
stopwaitsecs=60
|
||||
|
||||
[program:stage]
|
||||
# command=uv run trap_stage
|
||||
command=uv run trap_stage --verbose --camera-fps 12 --homography ../DATASETS/hof3/homography.json --calibration ../DATASETS/hof3/calibration.json --cache-path /tmp/history_cache-hof3.pcl --tracker-output-dir EXPERIMENTS/raw/hof3/
|
||||
directory=%(here)s
|
||||
|
||||
[program:settings]
|
||||
command=uv run trap_settings
|
||||
autostart=true
|
||||
environment=DISPLAY=":0"
|
||||
directory=%(here)s
|
||||
|
||||
[program:predictor]
|
||||
# command=uv run trap_prediction --eval_device cuda:0 --model_dir EXPERIMENTS/models/models_20241229_21_35_13_hof3-m2-ud-split-conv12-f2.0-map-2024-12-29/ --num-samples 1 --map_encoding --eval_data_dict EXPERIMENTS/trajectron-data/hof3-m2-ud-split-nostep-conv12-f2.0-map-2024-12-29_val.pkl --prediction-horizon 120 --gmm-mode True --z-mode
|
||||
command=uv run trap_prediction --eval_device cuda:0 --model_dir SETTINGS/2025-11-dortmund/models/models_20251111_19_06_29_dortmund-nostep-nosmooth-noise2-offsets1-f2.0-map-2025-11-11/ --num-samples 1 --map_encoding --eval_data_dict SETTINGS/2025-11-dortmund/trajectron/dortmund-nostep-nosmooth-noise2-offsets1-f2.0-map-2025-11-12_val.pkl --prediction-horizon 120 --gmm-mode True --z-mode --conf SETTINGS/2025-11-dortmund/trajectron.json
|
||||
# command=uv run trap_prediction --eval_device cuda:0 --model_dir EXPERIMENTS/models/models_20251106_11_51_00_hof-lidar-m2-ud-nostep-kalsmooth-noise2-offsets2-f2.0-map-2025-11-06/ --num-samples 1 --map_encoding --eval_data_dict EXPERIMENTS/trajectron-data/hof-lidar-m2-ud-nostep-kalsmooth-noise2-offsets2-f2.0-map-2025-11-06_val.pkl --prediction-horizon 120 --gmm-mode True --z-mode
|
||||
command=uv run trap_prediction --eval_device cuda:0 --model_dir EXPERIMENTS/models/models_20241229_21_35_13_hof3-m2-ud-split-conv12-f2.0-map-2024-12-29/ --num-samples 1 --map_encoding --eval_data_dict EXPERIMENTS/trajectron-data/hof3-m2-ud-split-nostep-conv12-f2.0-map-2024-12-29_val.pkl --prediction-horizon 120 --gmm-mode True --z-mode
|
||||
|
||||
# uv run trajectron_train --continue_training_from EXPERIMENTS/models/models_20241229_21_35_13_hof3-m2-ud-split-conv12-f2.0-map-2024-12-29/ --eval_every 5 --train_data_dict hof3-nostep-conv12-f2.0-map-2024-12-27_train.pkl --eval_data_dict hof3-nostep-conv12-f2.0-map-2024-12-27_val.pkl --offline_scene_graph no --preprocess_workers 8 --log_dir EXPERIMENTS/models --log_tag _hof3-conv12-f2.0-map-2024-12-27 --train_epochs 10 --conf EXPERIMENTS/config.json --data_dir EXPERIMENTS/trajectron-data --map_encoding
|
||||
directory=%(here)s
|
||||
|
||||
|
|
@ -73,25 +51,8 @@ autostart=false
|
|||
; can be long to quit if rendering to video file
|
||||
stopwaitsecs=60
|
||||
|
||||
[program:render_cv]
|
||||
command=uv run trap_render_cv
|
||||
directory=%(here)s
|
||||
environment=DISPLAY=":0"
|
||||
autostart=false
|
||||
; can be long to quit if rendering to video file
|
||||
stopwaitsecs=60
|
||||
|
||||
[program:laserspace]
|
||||
command=cargo run --release tcp://127.0.0.1:99174 ../trap/SETTINGS/2025-11-dortmund/laserspace.json
|
||||
directory=%(here)s/../laserspace
|
||||
environment=DISPLAY=":0"
|
||||
autostart=false
|
||||
; can be long to quit if rendering to video file
|
||||
stopwaitsecs=60
|
||||
|
||||
|
||||
# during development auto restart some services when the code changes
|
||||
[program:superfsmon]
|
||||
command=superfsmon trap/stage.py stage
|
||||
directory=%(here)s
|
||||
autostart=false
|
||||
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
181
trap/anomaly.py
181
trap/anomaly.py
|
|
@ -1,181 +0,0 @@
|
|||
from __future__ import annotations
|
||||
|
||||
import logging
|
||||
from typing import List
|
||||
|
||||
import numpy as np
|
||||
from trap.base import ProjectedTrack
|
||||
from trap.lines import AppendableLine, Coordinate, DeltaT, ProceduralChain, RenderableLines, SrgbaColor, StaticLine
|
||||
|
||||
logger = logging.getLogger('anomaly')
|
||||
|
||||
def calc_anomaly(segments: List[DiffSegment], window: int = 3):
|
||||
"""Calculate anomaly score based on provided segments
|
||||
considering a sliding window of the last n items
|
||||
"""
|
||||
|
||||
relevant_segments = segments[-window:]
|
||||
scores = [s.avg_score() for s in relevant_segments]
|
||||
s = list(filter(lambda x: x is not None,scores))
|
||||
|
||||
return np.average(s)
|
||||
|
||||
|
||||
class DiffSegment():
|
||||
"""
|
||||
A segment of a prediction track, that can be diffed
|
||||
with a track. The track is continously updated.
|
||||
If a new prediction comes in, the diff is marked as
|
||||
finished. After which it is animated and added to the
|
||||
Scenario's anomaly score.
|
||||
"""
|
||||
DRAW_DECAY_SPEED = 25
|
||||
POINT_INTERVAL = 4
|
||||
|
||||
def __init__(self, prediction: ProjectedTrack):
|
||||
self.ptrack = prediction
|
||||
self._last_diff_frame_idx: int = 0
|
||||
self.finished = False
|
||||
|
||||
self.line = StaticLine()
|
||||
self.points: List[Coordinate] = []
|
||||
self._drawn_points = []
|
||||
self._target_track = prediction
|
||||
|
||||
self.score = 0
|
||||
|
||||
def finish(self):
|
||||
self.finished = True
|
||||
|
||||
def nr_of_passed_points(self) -> int:
|
||||
if not self._last_diff_frame_idx:
|
||||
return 0
|
||||
return self._last_diff_frame_idx - self.ptrack.frame_index
|
||||
|
||||
# if isinstance(self.line, AppendableLine):
|
||||
# return self.line.nr_of_passed_points() * self.POINT_INTERVAL
|
||||
# else:
|
||||
# return len(self.points) * self.POINT_INTERVAL
|
||||
|
||||
def avg_score(self):
|
||||
frames_passed = self.nr_of_passed_points()
|
||||
if not frames_passed:
|
||||
return None
|
||||
else:
|
||||
return self.score/frames_passed
|
||||
|
||||
|
||||
# run on each track update received
|
||||
def update_track(self, track: ProjectedTrack):
|
||||
self._target_track = track
|
||||
|
||||
if self.finished:
|
||||
# don't add new points if finished
|
||||
return
|
||||
|
||||
# migrate SceneraioScene function
|
||||
start_frame_idx = max(self.ptrack.frame_index, self._last_diff_frame_idx)
|
||||
traj_diff_steps_back = track.frame_index - start_frame_idx # positive value
|
||||
pred_diff_steps_forward = start_frame_idx - self.ptrack.frame_index # positive value
|
||||
|
||||
if traj_diff_steps_back < 0 or len(track.history) < traj_diff_steps_back:
|
||||
logger.warning("Track history doesn't reach prediction start. Should not be possible. Skip")
|
||||
# elif len(ptrack.predictions[0]) < pred_diff_steps_back:
|
||||
# logger.warning("Prediction does not reach prediction start. Should not be possible. Skip")
|
||||
else:
|
||||
trajectory = track.projected_history
|
||||
|
||||
# from start to as far as it gets
|
||||
trajectory_range = trajectory[-1*traj_diff_steps_back:]
|
||||
prediction_range = self.ptrack.predictions[0][pred_diff_steps_forward:] # in world coordinate space
|
||||
line = []
|
||||
for i, (p1, p2) in enumerate(zip(trajectory_range, prediction_range)):
|
||||
diff = (p1[0]-p2[0], p1[1]-p2[1])
|
||||
self.score += np.linalg.norm(diff)
|
||||
|
||||
offset_from_start = (pred_diff_steps_forward + i)
|
||||
if offset_from_start % self.POINT_INTERVAL == 0:
|
||||
self.line.extend([p1, p2])
|
||||
self.points.extend([p1, p2])
|
||||
|
||||
self._last_diff_frame_idx = track.frame_index
|
||||
|
||||
|
||||
# # run each render tick
|
||||
# def update_drawn_positions(self, dt: DeltaT):
|
||||
# if isinstance(self.line, AppendableLine):
|
||||
# if self.finished and self.line.ready:
|
||||
# # convert when fully drawn
|
||||
# # print(self, "CONVERT LINE")
|
||||
# self.line = ProceduralChain.from_appendable_line(self.line)
|
||||
|
||||
# if isinstance(self.line, ProceduralChain):
|
||||
# self.line.target = self._target_track.projected_history[-1]
|
||||
|
||||
# # if not self.finished or not self.line.ready:
|
||||
# self.line.update_drawn_positions(dt)
|
||||
|
||||
|
||||
|
||||
def as_renderable(self) -> RenderableLines:
|
||||
color = SrgbaColor(0,0,1,1)
|
||||
|
||||
# if not self.finished or not self.line.ready:
|
||||
return self.line.as_renderable(color)
|
||||
# return self.line.as_renderable(color)
|
||||
|
||||
|
||||
def calculate_loitering_scores(track: ProjectedTrack, min_duration_to_linger, linger_factor, velocity_threshold, window = None):
|
||||
"""
|
||||
Calculates a loitering score (0-1) for each track.
|
||||
|
||||
Args:
|
||||
tracks: A list of tracks, where each track is a list of (frame_id, x, y, width, height).
|
||||
min_duration_to_linger: Minimum number of frames to start considering a segment as lingering.
|
||||
linger_factor: Divide number of lingering frames by 'linger_factor' to get a score 0-1
|
||||
velocity_threshold: Maximum velocity (meters/frame) to consider as lingering.
|
||||
|
||||
Returns:
|
||||
A generator providing loitering scores for each frame
|
||||
"""
|
||||
|
||||
total_frames = len(track.projected_history)
|
||||
|
||||
if total_frames < 2:
|
||||
return 0.0 # Not enough data
|
||||
|
||||
offset = window * -1 if window is not None else 0
|
||||
|
||||
x_coords = [t[0] for t in track.projected_history[offset:]]
|
||||
y_coords = [t[1] for t in track.projected_history[offset:]]
|
||||
|
||||
# Calculate velocities
|
||||
velocities = np.sqrt(np.diff(x_coords)**2 + np.diff(y_coords)**2)
|
||||
|
||||
# Calculate distances
|
||||
# distances = np.diff(x_coords)
|
||||
# distances_y = np.diff(y_coords)
|
||||
# distances_total = np.sqrt(distances**2 + distances_y**2)
|
||||
|
||||
linger_duration = 0
|
||||
linger_frames = 0
|
||||
|
||||
|
||||
for i in range(len(velocities)):
|
||||
if velocities[i] < velocity_threshold:
|
||||
linger_duration += 1
|
||||
if linger_duration >= min_duration_to_linger:
|
||||
linger_frames +=1
|
||||
else:
|
||||
# decay if moving faster
|
||||
linger_duration = max(linger_duration - 1.5, 0)
|
||||
linger_frames = max(linger_frames - 1.5, 0)
|
||||
|
||||
# Calculate loitering score
|
||||
if total_frames > 0:
|
||||
loitering_score = min(1, max(0, linger_frames / linger_factor))
|
||||
else:
|
||||
loitering_score = 0.0
|
||||
|
||||
yield loitering_score
|
||||
|
||||
10
trap/base.py
10
trap/base.py
|
|
@ -25,7 +25,6 @@ from bytetracker.basetrack import TrackState as ByteTrackTrackState
|
|||
import pandas as pd
|
||||
from shapely import Point
|
||||
|
||||
|
||||
from trap.utils import get_bins, inv_lerp, lerp
|
||||
from trajectron.environment import Environment, Node, Scene
|
||||
from urllib.parse import urlparse
|
||||
|
|
@ -74,7 +73,6 @@ class DetectionState(IntFlag):
|
|||
Confirmed = 2 # after tentative
|
||||
Lost = 4 # lost when DeepsortTrack.time_since_update > 0 but not Deleted
|
||||
Interpolated = 8 # A position estimated through interpolation of adjecent detections
|
||||
# Interpolated = 8 # A position estimated through interpolation of adjecent detections
|
||||
|
||||
@classmethod
|
||||
def from_deepsort_track(cls, track: DeepsortTrack):
|
||||
|
|
@ -90,13 +88,11 @@ class DetectionState(IntFlag):
|
|||
def from_bytetrack_track(cls, track: ByteTrackTrack):
|
||||
if track.state == ByteTrackTrackState.New:
|
||||
return cls.Tentative
|
||||
if track.state == ByteTrackTrackState.Removed:
|
||||
if track.state == ByteTrackTrackState.Lost:
|
||||
return cls.Lost
|
||||
# if track.time_since_update > 0:
|
||||
if track.state == ByteTrackTrackState.Tracked:
|
||||
return cls.Confirmed
|
||||
if track.state == ByteTrackTrackState.Lost:
|
||||
return cls.Tentative
|
||||
raise RuntimeError("Should not run into Deleted entries here")
|
||||
|
||||
|
||||
|
|
@ -174,8 +170,6 @@ class DistortedCamera(ABC):
|
|||
calibdata = json.load(fp)
|
||||
if 'type' in calibdata and calibdata['type'] == 'fisheye':
|
||||
camera = FisheyeCamera.from_calibdata(calibdata, H, fps)
|
||||
elif 'type' in calibdata and calibdata['type'] == 'undistorted':
|
||||
camera = UndistortedCamera(calibdata['fps'])
|
||||
else:
|
||||
camera = Camera.from_calibdata(calibdata, H, fps)
|
||||
|
||||
|
|
@ -765,8 +759,6 @@ class CameraAction(argparse.Action):
|
|||
data = json.load(fp)
|
||||
if 'type' in data and data['type'] == 'fisheye':
|
||||
camera = FisheyeCamera.from_calibfile(Path(values), namespace.H, namespace.camera_fps)
|
||||
elif 'type' in data and data['type'] == 'undistorted':
|
||||
camera = UndistortedCamera(namespace.camera_fps)
|
||||
else:
|
||||
camera = Camera.from_calibfile(Path(values), namespace.H, namespace.camera_fps)
|
||||
# # print(data)
|
||||
|
|
|
|||
|
|
@ -18,7 +18,7 @@ import pyglet
|
|||
import zmq
|
||||
from pyglet import shapes
|
||||
|
||||
from trap.base import Detection, UndistortedCamera
|
||||
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
|
||||
|
|
@ -151,12 +151,10 @@ class CvRenderer(Node):
|
|||
# logger.debug(f'new video frame {frame.index}')
|
||||
|
||||
|
||||
if self.frame is None and i < 100:
|
||||
if self.frame is None:
|
||||
# might need to wait a few iterations before first frame comes available
|
||||
time.sleep(.1)
|
||||
continue
|
||||
else:
|
||||
self.frame = Frame(i, np.zeros((1920,1080,3)), camera=UndistortedCamera(12))
|
||||
|
||||
try:
|
||||
prediction_frame: Frame = self.prediction_sock.recv_pyobj(zmq.NOBLOCK)
|
||||
|
|
@ -276,7 +274,7 @@ class CvRenderer(Node):
|
|||
render_parser.add_argument('--debug-map',
|
||||
help='specify a map (svg-file) from which to load lines which will be overlayed',
|
||||
type=str,
|
||||
default="../DATASETS/hof-lidar/map_hof.svg")
|
||||
default="../DATASETS/hof3/map_hof.svg")
|
||||
return render_parser
|
||||
|
||||
def click_print_position(self, event,x,y,flags,param):
|
||||
|
|
@ -412,9 +410,8 @@ def decorate_frame(frame: Frame, tracker_frame: Frame, prediction_frame: Frame,
|
|||
inv_H = np.linalg.pinv(prediction_frame.H)
|
||||
# For debugging:
|
||||
# draw_trackjectron_history(img, track, int(track.track_id), conversion)
|
||||
# anim_position = get_animation_position(track, frame)
|
||||
anim_position = 1
|
||||
draw_track_predictions(img, track, int(track.track_id)+1, prediction_frame.camera, conversion, anim_position=anim_position, as_clusters=as_clusters)
|
||||
anim_position = get_animation_position(track, frame)
|
||||
draw_track_predictions(img, track, int(track.track_id)+1, frame.camera, conversion, anim_position=anim_position, as_clusters=as_clusters)
|
||||
cv2.putText(img, f"{len(track.predictor_history) if track.predictor_history else 'none'}", to_point(track.history[0].get_foot_coords()), cv2.FONT_HERSHEY_COMPLEX, 1, (255,255,255), 1)
|
||||
if prediction_frame.maps:
|
||||
for i, m in enumerate(prediction_frame.maps):
|
||||
|
|
|
|||
|
|
@ -37,6 +37,7 @@ class FrameEmitter(node.Node):
|
|||
video_gen = enumerate(source, start = offset)
|
||||
|
||||
# writer = FrameWriter(self.config.record, None, None) if self.config.record else nullcontext
|
||||
print(self.config.record)
|
||||
writer = FrameWriter(str(self.config.record), None, None) if self.config.record else None
|
||||
try:
|
||||
processor = ImgMovementFilter()
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load diff
674
trap/lines.py
674
trap/lines.py
|
|
@ -5,7 +5,6 @@ import copy
|
|||
from dataclasses import dataclass
|
||||
from enum import Enum, IntEnum
|
||||
from functools import partial
|
||||
import logging
|
||||
import math
|
||||
from pathlib import Path
|
||||
import time
|
||||
|
|
@ -21,17 +20,13 @@ import svgpathtools
|
|||
|
||||
from noise import snoise2
|
||||
|
||||
from trap import renderable_pb2
|
||||
from trap.utils import easeInOutQuad, exponentialDecay, exponentialDecayRounded, inv_lerp, lerp, relativePointToPolar, relativePolarToPoint
|
||||
from trap.utils import exponentialDecayRounded, inv_lerp
|
||||
|
||||
|
||||
"""
|
||||
See [notebook](../test_path_transforms.ipynb) for examples
|
||||
"""
|
||||
|
||||
|
||||
logger = logging.getLogger('lines')
|
||||
|
||||
RenderablePosition = Tuple[float,float]
|
||||
Coordinate = Tuple[float, float]
|
||||
DeltaT = float # delta_t in seconds
|
||||
|
|
@ -58,9 +53,6 @@ class SrgbaColor():
|
|||
def __eq__(self, other):
|
||||
return math.isclose(self.red, other.red) and math.isclose(self.green, other.green) and math.isclose(self.blue, other.blue) and math.isclose(self.alpha, other.alpha)
|
||||
|
||||
def as_array(self):
|
||||
return np.array([self.red, self.green, self.blue, self.alpha])
|
||||
|
||||
|
||||
@dataclass
|
||||
class RenderablePoint():
|
||||
|
|
@ -314,31 +306,12 @@ class AppendableLine(LineGenerator):
|
|||
# create origin
|
||||
self.drawn_points.append(self.target_points[0])
|
||||
# and drawing head
|
||||
# self.drawn_points.append(self.target_points[0])
|
||||
|
||||
target_l = shapely.geometry.LineString(self.target_points).length
|
||||
current_l = shapely.geometry.LineString(self.drawn_points).length if len(self.drawn_points) > 1 else 0
|
||||
req_l = exponentialDecayRounded(self.current_l, target_l, self.draw_decay_speed, dt, .05)
|
||||
|
||||
if np.isclose(req_l, target_l):
|
||||
self.drawn_points = self.target_points
|
||||
else:
|
||||
distance_to_do = req_l - current_l
|
||||
self.drawn_points.append(self.target_points[0])
|
||||
|
||||
idx = len(self.drawn_points) - 1
|
||||
while distance_to_do:
|
||||
target = self.target_points[idx]
|
||||
this_distance = np.array(self.drawn_points[-1] ) - np.array(target)
|
||||
if this_distance > distance_to_do:
|
||||
break
|
||||
distance_to_do - this_distance
|
||||
|
||||
idx+=1
|
||||
self.drawn_points.append(target)
|
||||
|
||||
|
||||
|
||||
if np.isclose(self.drawn_points[-1], self.target_points[idx], atol=.05).all():
|
||||
if np.isclose(self.drawn_points[-1], target, atol=.05).all():
|
||||
# TODO: might want to migrate to np.isclose()
|
||||
if len(self.drawn_points) == len(self.target_points):
|
||||
self.ready = True
|
||||
|
|
@ -482,7 +455,7 @@ class LineAnimator(StaticLine):
|
|||
# print(self, target_line, bool(target_line), target_line is not None)
|
||||
self.target = target_line if target_line is not None else StaticLine()
|
||||
self.ready = len(self.target) == 0
|
||||
self.start_t = time.perf_counter()
|
||||
self.start_t = time.time()
|
||||
self.skip = False
|
||||
|
||||
def extend(self, coords):
|
||||
|
|
@ -504,23 +477,15 @@ class LineAnimator(StaticLine):
|
|||
def is_ready(self):
|
||||
return (self.ready or self.skip) and self.target.is_ready()
|
||||
|
||||
|
||||
def start(self):
|
||||
self.target.start()
|
||||
|
||||
self.start_t = time.perf_counter()
|
||||
self.start_t = time.time()
|
||||
return True
|
||||
|
||||
def is_started(self):
|
||||
return bool(self.start_t)
|
||||
|
||||
def is_running(self):
|
||||
# when ready, consider not running
|
||||
return bool(self.start_t) and not self.is_ready()
|
||||
|
||||
def running_for(self):
|
||||
if self.start_t:
|
||||
return time.perf_counter() - self.start_t
|
||||
return time.time() - self.start_t
|
||||
return 0.
|
||||
|
||||
|
||||
|
|
@ -539,10 +504,9 @@ class AppendableLineAnimator(LineAnimator):
|
|||
|
||||
|
||||
def apply(self, target_line, dt: DeltaT) -> RenderableLine:
|
||||
if len(target_line) < 2:
|
||||
return target_line
|
||||
if len(target_line) == 0:
|
||||
# nothing to draw yet
|
||||
# return RenderableLine([])
|
||||
return RenderableLine([])
|
||||
|
||||
|
||||
|
||||
|
|
@ -556,12 +520,6 @@ class AppendableLineAnimator(LineAnimator):
|
|||
self.drawn_points.append(copy.deepcopy(self.drawn_points[-1]))
|
||||
|
||||
idx = len(self.drawn_points) - 1
|
||||
if idx > len(target_line.points) - 1:
|
||||
logger.warning("Target line shorter that appendable line, shorten")
|
||||
self.drawn_points = self.drawn_points[:len(target_line)]
|
||||
idx = len(self.drawn_points) - 1
|
||||
|
||||
|
||||
target = target_line.points[idx]
|
||||
|
||||
if np.isclose(self.drawn_points[-1].position, target.position, atol=.05).all():
|
||||
|
|
@ -615,20 +573,6 @@ class FadeOutLine(LineAnimator):
|
|||
|
||||
return target_line
|
||||
|
||||
class SimplifyLine(LineAnimator):
|
||||
"""
|
||||
Simplify the line
|
||||
"""
|
||||
def __init__(self, target_line = None, factor: float =.003):
|
||||
super().__init__(target_line)
|
||||
self.factor = factor
|
||||
self.ready = True # filter holds no state, so always ready
|
||||
|
||||
|
||||
|
||||
def apply(self, target_line: RenderableLine, dt: DeltaT) -> RenderableLine:
|
||||
return target_line.as_simplified(SimplifyMethod.RDP, self.factor)
|
||||
|
||||
class FadeOutJitterLine(LineAnimator):
|
||||
"""
|
||||
Fade the line providing an alpha, 1 by default
|
||||
|
|
@ -666,94 +610,15 @@ class CropLine(LineAnimator):
|
|||
Crop the line at a max nr of points (thus not actual lenght!)
|
||||
Keeps the tail, removes the start
|
||||
"""
|
||||
def __init__(self, target_line = None, max_points: Optional[int] = None, start_offset: Optional[int] = None):
|
||||
def __init__(self, target_line = None, max_points = 200):
|
||||
super().__init__(target_line)
|
||||
self.start_offset = start_offset
|
||||
self.max_points = max_points
|
||||
self.ready = True # static filter, always ready
|
||||
|
||||
def apply(self, target_line: RenderableLine, dt: DeltaT) -> RenderableLine:
|
||||
if self.start_offset:
|
||||
if len(target_line) <= self.start_offset:
|
||||
return RenderableLine([])
|
||||
target_line.points = target_line.points[self.start_offset:]
|
||||
|
||||
if self.max_points:
|
||||
target_line.points = target_line.points[-1 * self.max_points:]
|
||||
|
||||
return target_line
|
||||
|
||||
|
||||
class CropAnimationLine(LineAnimator):
|
||||
"""
|
||||
Similar to segment, but on points instead of lenght, and as animation
|
||||
"""
|
||||
def __init__(self, target_line = None, max_points = 200, assume_fps=12):
|
||||
super().__init__(target_line)
|
||||
self.max_points = max_points
|
||||
self.assume_fps = assume_fps
|
||||
# self.ready = True # static filter, always ready
|
||||
|
||||
# def set_frame_offset(self, frame_offset: int):
|
||||
# self.frame_offset = frame_offset
|
||||
|
||||
def apply(self, target_line: RenderableLine, dt: DeltaT) -> RenderableLine:
|
||||
|
||||
dt = self.running_for()
|
||||
frame_offset = int(dt * self.assume_fps)
|
||||
|
||||
max_points = int(self.max_points(dt)) if callable(self.max_points) else self.max_points
|
||||
|
||||
|
||||
head = frame_offset + 1
|
||||
tail = max(0, frame_offset + 1 - max_points)
|
||||
# print(self.running_for(), frame_offset, head,tail)
|
||||
|
||||
|
||||
target_line.points = target_line.points[tail:head]
|
||||
self.ready = len(target_line.points) < 1
|
||||
|
||||
return target_line
|
||||
|
||||
|
||||
class FadedEndsLine(LineAnimator):
|
||||
"""
|
||||
Static filter; Fade the tail of the line. Always (not only cropped)
|
||||
"""
|
||||
def __init__(self, target_line = None, in_fade_steps: int = 30, out_fade_steps: int = 30):
|
||||
super().__init__(target_line)
|
||||
self.ready = True
|
||||
self.fade_in_steps = in_fade_steps
|
||||
self.fade_out_steps = out_fade_steps
|
||||
|
||||
|
||||
def apply(self, target_line: RenderableLine, dt: DeltaT) -> RenderableLine:
|
||||
l = len(target_line.points)
|
||||
points = []
|
||||
|
||||
# TODO: fractional divide if fade_in and out are not equal
|
||||
half_points = l // 2
|
||||
fade_in = min(self.fade_in_steps, half_points)
|
||||
fade_out = min(self.fade_out_steps, half_points)
|
||||
|
||||
|
||||
for i, point in enumerate(target_line.points):
|
||||
if i < fade_in:
|
||||
t = i / self.fade_in_steps
|
||||
elif i > (l - fade_out):
|
||||
t = 1 - (i - (l - fade_out)) / self.fade_out_steps
|
||||
else:
|
||||
t = 1
|
||||
|
||||
alpha = t
|
||||
if alpha >= 0:
|
||||
alpha = min(1, alpha)
|
||||
point.color = point.color.as_faded(alpha)
|
||||
points.append(point)
|
||||
|
||||
|
||||
return RenderableLine(points)
|
||||
|
||||
class FadedTailLine(LineAnimator):
|
||||
"""
|
||||
Fade the tail of the line, proving a max length
|
||||
|
|
@ -792,7 +657,6 @@ class FadedTailLine(LineAnimator):
|
|||
return RenderableLine(points)
|
||||
|
||||
|
||||
|
||||
class NoiseLine(LineAnimator):
|
||||
"""
|
||||
Apply animated noise to line normals
|
||||
|
|
@ -868,7 +732,7 @@ class NoiseLine(LineAnimator):
|
|||
# noise_y = noise([x * frequency, (y + dt) * frequency]) * amplitude * normal_y
|
||||
noise = snoise2(i * frequency, t % 1000, octaves=4)
|
||||
|
||||
use_amp = amplitude[i] if hasattr(amplitude, "__getitem__") else amplitude
|
||||
use_amp = amplitude
|
||||
if fade_over_n_points > 0:
|
||||
rev_step = len(drawable_points) - i
|
||||
amp_factor = rev_step / fade_over_n_points
|
||||
|
|
@ -940,19 +804,6 @@ class SegmentLine(LineAnimator):
|
|||
else:
|
||||
return (0, ls.length * t)
|
||||
|
||||
@classmethod
|
||||
def anim_follow_in_front(cls, t: float, ls: shapely.geometry.LineString):
|
||||
keypoints = [
|
||||
(0.0, 0, 0), #
|
||||
(0.07, 1.0, 2.0), #
|
||||
(0.3, 1.5, 2.0), # At t=0.7, value=0.5
|
||||
(0.6, 1, 1.5), # At t=0.7, value=0.5
|
||||
(0.7, 1.2, 1.5), # At t=0.7, value=0.5
|
||||
(0.7, 1, 1.8), # At t=0.7, value=0.5
|
||||
(1.0, .5, ls.length), # At t=1, value=0
|
||||
]
|
||||
return KeyframeAnimator(keypoints, easeInOutQuad).get_value(t)
|
||||
|
||||
|
||||
def apply(self, target_line: RenderableLine, dt: DeltaT):
|
||||
if len(target_line) < 2:
|
||||
|
|
@ -974,57 +825,6 @@ class SegmentLine(LineAnimator):
|
|||
|
||||
|
||||
|
||||
class KeyframeAnimator:
|
||||
def __init__(
|
||||
self,
|
||||
keypoints: List[Tuple[float, float, float]],
|
||||
easing_func: Callable[[float], float] = None,
|
||||
):
|
||||
"""
|
||||
Initialize the animator with keypoints and an optional easing function.
|
||||
|
||||
Args:
|
||||
keypoints: List of (t_value, value) tuples, where t_value is in [0, 1].
|
||||
easing_func: Optional function to apply easing to `t` (e.g., easeInOutQuad).
|
||||
"""
|
||||
self.keypoints = sorted(keypoints, key=lambda x: x[0])
|
||||
self.easing_func = easing_func
|
||||
|
||||
def get_value(self, t: float) -> float:
|
||||
"""
|
||||
Get the interpolated value at time `t` (0 <= t <= 1).
|
||||
|
||||
Args:
|
||||
t: Normalized time (0 to 1).
|
||||
|
||||
Returns:
|
||||
Interpolated value at time `t`.
|
||||
"""
|
||||
if self.easing_func:
|
||||
t = self.easing_func(t)
|
||||
|
||||
# Handle edge cases
|
||||
if t <= self.keypoints[0][0]:
|
||||
return self.keypoints[0][1], self.keypoints[0][2]
|
||||
if t >= self.keypoints[-1][0]:
|
||||
return self.keypoints[-1][1], self.keypoints[-1][2]
|
||||
|
||||
# Find the two keypoints surrounding `t`
|
||||
for i in range(len(self.keypoints) - 1):
|
||||
t0, val0, valb0 = self.keypoints[i]
|
||||
t1, val1, valb1 = self.keypoints[i + 1]
|
||||
|
||||
if t0 <= t <= t1:
|
||||
# Normalize `t` between t0 and t1
|
||||
local_t = inv_lerp(t0, t1, t)
|
||||
|
||||
# Interpolate between val0 and val1
|
||||
|
||||
return lerp(val0, val1, local_t), lerp(valb0, valb1, local_t)
|
||||
|
||||
return self.keypoints[-1][1], self.keypoints[-1][2] # fallback
|
||||
|
||||
|
||||
class DashedLine(LineAnimator):
|
||||
"""
|
||||
Dashed line
|
||||
|
|
@ -1066,7 +866,6 @@ class DashedLine(LineAnimator):
|
|||
segments.append(dash)
|
||||
pos += dash_len + gap_len
|
||||
|
||||
segments = [segment for segment in segments if isinstance(segment, shapely.geometry.LineString)]
|
||||
# TODO: return all color together with the points
|
||||
return shapely.geometry.MultiLineString(segments)
|
||||
|
||||
|
|
@ -1080,11 +879,8 @@ class DashedLine(LineAnimator):
|
|||
self.ready = True
|
||||
return target_line
|
||||
|
||||
gap_len = gap_len if not callable(self.gap_len) else self.gap_len(dt, self.running_for())
|
||||
dash_len = dash_len if not callable(self.dash_len) else self.dash_len(dt, self.running_for())
|
||||
|
||||
ls = target_line.as_linestring()
|
||||
multilinestring = self.dashed_line(ls, dash_len, gap_len, self.t_factor * self.running_for(), self.loop_offset)
|
||||
multilinestring = self.dashed_line(ls, self.dash_len, self.gap_len, self.t_factor * self.running_for(), self.loop_offset)
|
||||
|
||||
|
||||
# when looping, it is always ready, otherwise, only if totally gone
|
||||
|
|
@ -1094,377 +890,6 @@ class DashedLine(LineAnimator):
|
|||
|
||||
return RenderableLine.from_multilinestring(multilinestring, color)
|
||||
|
||||
class StartFromClosestPoint(LineAnimator):
|
||||
"""
|
||||
Dashed line
|
||||
"""
|
||||
def __init__(self, target_line = None, from_point: Optional[Tuple[float,float]] = None):
|
||||
super().__init__(target_line)
|
||||
self.from_point = from_point
|
||||
self.ready = True # static filter, always ready
|
||||
|
||||
def disable(self):
|
||||
self.from_point = None
|
||||
|
||||
def set_point(self, point: Tuple[float,float]):
|
||||
self.from_point = point
|
||||
|
||||
def apply(self, target_line: RenderableLine, dt: DeltaT) -> RenderableLine:
|
||||
"""
|
||||
warning, dashing (for now) removes all color
|
||||
"""
|
||||
|
||||
if len(target_line) < 1 or self.from_point is None:
|
||||
return target_line
|
||||
|
||||
|
||||
distance = math.inf
|
||||
idx = 0
|
||||
from_point = np.array(self.from_point)
|
||||
# print(from_point)
|
||||
|
||||
for i, point in enumerate(target_line.points):
|
||||
if i < 2:
|
||||
continue # skip the first to avoid jitter
|
||||
|
||||
to_point = np.array(point.position)
|
||||
new_distance = np.linalg.norm(from_point-to_point)
|
||||
if new_distance < distance:
|
||||
distance = new_distance
|
||||
idx = i+1
|
||||
else:
|
||||
break
|
||||
|
||||
if idx >= len(target_line.points):
|
||||
logger.warning("Empty line")
|
||||
return RenderableLine([])
|
||||
points = []
|
||||
if idx > 0:
|
||||
p = target_line.points[idx]
|
||||
p.position = self.from_point
|
||||
|
||||
points.append(p)
|
||||
points.extend(target_line.points[idx:])
|
||||
# print(from_point, idx, [p.position for p in target_line.points[max(0,idx-5):idx+5]])
|
||||
return RenderableLine(points)
|
||||
|
||||
@classmethod
|
||||
def find_closest_to_point(cls, point: Tuple[Coordinate], points: List[Coordinate]):
|
||||
from_point = np.array(point)
|
||||
# idx = 0
|
||||
# distance = math.inf
|
||||
if len(points) == 0:
|
||||
return None
|
||||
|
||||
# print(points)
|
||||
# print(points- from_point)
|
||||
# print(np.array(points) - from_point)
|
||||
idx = np.argmin(np.linalg.norm(np.array(points) - from_point, axis=1))
|
||||
# for i, to_point in enumerate(points):
|
||||
# to_point = np.array(to_point)
|
||||
# new_distance = np.linalg.norm(from_point-to_point)
|
||||
# if new_distance < distance:
|
||||
# distance = new_distance
|
||||
# idx = i+1
|
||||
# else:
|
||||
# break
|
||||
return idx
|
||||
|
||||
|
||||
|
||||
class RotatingLine(LineAnimator):
|
||||
"""
|
||||
Rotate the line around starting point towards new shape
|
||||
"""
|
||||
def __init__(self, target_line = None, decay_speed=16):
|
||||
super().__init__(target_line)
|
||||
self.decay_speed = decay_speed
|
||||
self.drawn_points: List[RenderablePoint] = []
|
||||
|
||||
|
||||
|
||||
def apply(self, target_line: RenderableLine, dt: DeltaT) -> RenderableLine:
|
||||
"""
|
||||
warning, dashing (for now) removes all color
|
||||
"""
|
||||
|
||||
if len(target_line) < 2:
|
||||
self.ready = True
|
||||
return target_line
|
||||
|
||||
if len(self.drawn_points) < 1:
|
||||
self.drawn_points = target_line.points
|
||||
|
||||
# find closest point to start from:
|
||||
|
||||
origin = target_line.points[0]
|
||||
# print(origin, target_line.points[-1])
|
||||
# closest_idx = StartFromClosestPoint.find_closest_to_point(origin.position, [p.position for p in self.drawn_points])
|
||||
# if closest_idx:
|
||||
# print('cut at', closest_idx)
|
||||
# self.drawn_points = self.drawn_points[closest_idx:] # hard cutof
|
||||
|
||||
|
||||
|
||||
diff_length = len(target_line) - len(self.drawn_points)
|
||||
if diff_length < 0: # drawn points is larger
|
||||
self.drawn_points = self.drawn_points[:len(target_line)]
|
||||
if diff_length > 0: # target line is larger
|
||||
self.drawn_points += [self.drawn_points[-1]] * diff_length
|
||||
|
||||
# associated_diff = self.prediction_diffs[a]
|
||||
# progress = associated_diff.nr_of_passed_points()
|
||||
is_ready: List[bool] = []
|
||||
|
||||
# target_point = target_line.points[-1]
|
||||
|
||||
for i, (target_point, drawn_point) in enumerate(zip(target_line.points, list(self.drawn_points))):
|
||||
# TODO: this should be done in polar space starting from origin (i.e. self.drawn_posision[-1])
|
||||
decay = max(3, (18/i) if i else 10) # points further away move with more delay
|
||||
decay = self.decay_speed
|
||||
|
||||
drawn_r, drawn_angle = relativePointToPolar( origin.position, drawn_point.position)
|
||||
pred_r, pred_angle = relativePointToPolar(origin.position, target_point.position)
|
||||
r = exponentialDecay(drawn_r, pred_r, decay, dt)
|
||||
|
||||
# make circular coordinates transition through the smaller arc
|
||||
# TODO 20251108 bring this back, but calculated for the whole line:
|
||||
if abs(drawn_angle - pred_angle) > math.pi:
|
||||
pred_angle -= math.pi * 2
|
||||
angle = exponentialDecay(drawn_angle, pred_angle, decay, dt)
|
||||
x, y = relativePolarToPoint(origin.position, r, angle)
|
||||
|
||||
r = exponentialDecay(drawn_point.color.red, target_point.color.red, decay, dt)
|
||||
g = exponentialDecay(drawn_point.color.green, target_point.color.green, decay, dt)
|
||||
b = exponentialDecay(drawn_point.color.blue, target_point.color.blue, decay, dt)
|
||||
a = exponentialDecay(drawn_point.color.alpha, target_point.color.alpha, decay, dt)
|
||||
|
||||
|
||||
|
||||
self.drawn_points[i].position = (x, y)
|
||||
self.drawn_points[i].color = SrgbaColor(r, g, b, a)
|
||||
is_ready.append(np.isclose(drawn_point.position, target_point.position, atol=.05).all())
|
||||
|
||||
self.ready = all(is_ready)
|
||||
|
||||
|
||||
|
||||
return RenderableLine(self.drawn_points)
|
||||
|
||||
from scipy.ndimage import gaussian_filter1d
|
||||
from scipy.signal import find_peaks
|
||||
|
||||
class NoodleWiggler(LineAnimator):
|
||||
"""When a line is 'noodling' don't draw it as a whole, but sway it animated
|
||||
|
||||
Work in progress
|
||||
"""
|
||||
|
||||
def __init__(self, target_line = None):
|
||||
super().__init__(target_line)
|
||||
self.range = 10
|
||||
self.threshold = 1.8
|
||||
self.smoothing_sigma = 2
|
||||
self.t = 0
|
||||
self.t_factor = 4
|
||||
|
||||
def apply(self, target_line, dt):
|
||||
if len(target_line) < 2:
|
||||
return target_line
|
||||
|
||||
self.t += dt
|
||||
|
||||
scores = []
|
||||
distances = [np.linalg.norm(np.array(a.position)- np.array(b.position)) for a,b in zip(target_line.points, target_line.points[1:])]
|
||||
|
||||
# 1) find points with high distance traveled relative to displacement. This is noodling
|
||||
for i, point in enumerate(target_line.points):
|
||||
if i < self.range:
|
||||
scores.append(0)
|
||||
if len(target_line.points) < i + self.range:
|
||||
scores.append(0)
|
||||
|
||||
a = target_line.points[i-self.range]
|
||||
b = target_line.points[i+self.range]
|
||||
|
||||
net_distance = np.linalg.norm(np.array(a.position)- np.array(b.position))
|
||||
gross_distance = sum(distances[i-self.range:i-self.range-1])
|
||||
self.scores.append(max(0, gross_distance/net_distance - self.threshold))
|
||||
|
||||
# 2) smooth the curve
|
||||
smoothed_scores = gaussian_filter1d(scores, sigma=self.smoothing_sigma)
|
||||
|
||||
# 3) find the peaks ; most intense noodling points
|
||||
peak_idxs, _ = find_peaks(smoothed_scores, height=0.5)
|
||||
|
||||
# 4) for peaks, find start-peak-end indexes
|
||||
segments = self.connect_peaks_to_segments(smoothed_scores, peak_idxs)
|
||||
|
||||
points, scores = self.replace_noodling_segments(scores, target_line, segments, 4)
|
||||
|
||||
# 5) apply noise according to score
|
||||
new_points = NoiseLine.apply_perlin_noise_to_line_normal(
|
||||
drawable_points=[p.position for p in points],
|
||||
amplitude=scores,
|
||||
t = self.t * self.t_factor,
|
||||
frequency= 2
|
||||
)
|
||||
|
||||
# 6) rebuild as line
|
||||
points = []
|
||||
for point, pos in zip(target_line.points, new_positions):
|
||||
p = copy.deepcopy(point)
|
||||
p.position = pos
|
||||
points.append(p)
|
||||
|
||||
|
||||
return RenderableLine(points)
|
||||
|
||||
|
||||
@classmethod
|
||||
def connect_peaks_to_segments(cls, scores, peaks, threshold=0.3, min_segment_length=1) -> List [Tuple[int, int, int]]:
|
||||
"""
|
||||
returns a list of tuples: (start_idx, peak_idx, end_idx)
|
||||
"""
|
||||
segments = []
|
||||
i = 0
|
||||
n = len(scores)
|
||||
|
||||
while i < len(peaks):
|
||||
peak = peaks[i]
|
||||
start = peak
|
||||
end = peak
|
||||
|
||||
# Expand left
|
||||
while start > 0 and scores[start - 1] > threshold:
|
||||
start -= 1
|
||||
|
||||
# Expand right
|
||||
while end < n - 1 and scores[end + 1] > threshold:
|
||||
end += 1
|
||||
|
||||
# Merge with next peak if close
|
||||
if i < len(peaks) - 1 and peaks[i + 1] - end < min_segment_length:
|
||||
i += 1
|
||||
continue
|
||||
|
||||
segments.append((start, peak, end))
|
||||
i += 1
|
||||
|
||||
return segments
|
||||
|
||||
@classmethod
|
||||
def replace_noodling_segments(cls, scores: List[float], target_line: RenderableLine, segments, num_interpolated_points = 4):
|
||||
"""
|
||||
Replace noodling segments in target_line with a fixed number of interpolated points,
|
||||
while preserving all non-noodling sections.
|
||||
|
||||
Args:
|
||||
target_line: Object with a `points` attribute (list of point objects).
|
||||
segments: List of tuples (start_index, end_index) for noodling segments.
|
||||
num_interpolated_points: Number of intermediate points to insert between start and end.
|
||||
|
||||
Returns:
|
||||
A new list of points with noodling segments replaced.
|
||||
"""
|
||||
new_points = []
|
||||
new_scores = []
|
||||
i = 0
|
||||
n = len(target_line.points)
|
||||
|
||||
for start, peak, end in segments:
|
||||
# Add all points up to the start of the noodling segment
|
||||
while i < start:
|
||||
new_points.append(target_line.points[i])
|
||||
new_scores.append(scores[i])
|
||||
i += 1
|
||||
|
||||
# Skip the noodling segment and add interpolated points
|
||||
start_point = target_line.points[start]
|
||||
peak_point = target_line.points[peak]
|
||||
end_point = target_line.points[end]
|
||||
|
||||
start_score = scores[start]
|
||||
peak_score = scores[peak]
|
||||
end_score = scores[end]
|
||||
|
||||
# Interpolate between start and peak
|
||||
for j in range(num_interpolated_points + 2): # +2 to include start and peak
|
||||
t = inv_lerp(0, num_interpolated_points + 1, j)
|
||||
new_x = lerp(start_point.position[0], peak_point.position[0], t)
|
||||
new_y = lerp(start_point.position[1], peak_point.position[1], t)
|
||||
new_score = lerp(start_score, peak_score, t)
|
||||
new_point = RenderablePoint(position=(new_x, new_y), color=start_point.color)
|
||||
new_points.append(new_point)
|
||||
new_scores.append(new_score)
|
||||
|
||||
# Interpolate between peak and end (skip peak to avoid duplication)
|
||||
for j in range(1, num_interpolated_points + 2): # +2 to include peak and end
|
||||
t = inv_lerp(0, num_interpolated_points + 1, j)
|
||||
new_x = lerp(peak_point.position[0], end_point.position[0], t)
|
||||
new_y = lerp(peak_point.position[1], end_point.position[1], t)
|
||||
new_score = lerp(peak_score, end_score, t)
|
||||
new_point = RenderablePoint(position=(new_x, new_y), color=start_point.color)
|
||||
new_points.append(new_point)
|
||||
new_scores.append(new_score)
|
||||
|
||||
i = end + 1 # Skip to the end of the noodling segment
|
||||
|
||||
# Add remaining points after the last noodling segment
|
||||
while i < n:
|
||||
new_points.append(target_line.points[i])
|
||||
new_scores.append(scores[i])
|
||||
i += 1
|
||||
|
||||
return new_points, scores
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
# @classmethod
|
||||
# def detect_noodling_sections(cls, line, segment_length=0.1, ratio_threshold=2.0):
|
||||
# """
|
||||
# Detects noodling sections in a LineString using the ratio of actual length to straight-line distance.
|
||||
|
||||
# Args:
|
||||
# line (LineString): Input LineString.
|
||||
# segment_length (float): Length of each segment as a fraction of the total line length.
|
||||
# ratio_threshold (float): Threshold for the ratio (actual_length / straight_line_distance).
|
||||
|
||||
# Returns:
|
||||
# list: List of noodling segments (as LineStrings).
|
||||
# """
|
||||
# noodling_sections = []
|
||||
# total_length = line.length
|
||||
# segment_start = 0.0
|
||||
|
||||
# while segment_start < 1.0:
|
||||
# segment_end = min(segment_start + segment_length, 1.0)
|
||||
# segment = substring(line, segment_start, segment_end, normalized=True)
|
||||
|
||||
# # Calculate straight-line distance between start and end points
|
||||
# start_point = Point(segment.coords[0])
|
||||
# end_point = Point(segment.coords[-1])
|
||||
# straight_line_distance = start_point.distance(end_point)
|
||||
|
||||
# # Calculate actual length of the segment
|
||||
# actual_length = segment.length
|
||||
|
||||
# # Check if the ratio exceeds the threshold
|
||||
# if straight_line_distance > 0 and (actual_length / straight_line_distance) > ratio_threshold:
|
||||
# noodling_sections.append(segment)
|
||||
|
||||
# segment_start = segment_end
|
||||
|
||||
# return noodling_sections
|
||||
|
||||
# # Example usage:
|
||||
# line = LineString([(0, 0), (0.1, 0.1), (0.2, 0.2), (0.3, 0.1), (0.4, 0.2), (1, 1)])
|
||||
# noodling_segments = detect_noodling_sections(line, segment_length=0.2, area_threshold=0.02)
|
||||
|
||||
# for i, segment in enumerate(noodling_segments):
|
||||
# print(f"Noodling segment {i+1}: {segment}")
|
||||
|
||||
IndexAndOffset = Tuple[int, float]
|
||||
|
||||
|
|
@ -1558,9 +983,6 @@ class LineAnimationStack():
|
|||
def is_ready(self):
|
||||
return self.tail.is_ready()
|
||||
|
||||
def is_running(self):
|
||||
return self.tail.is_running()
|
||||
|
||||
|
||||
class LineAnimationSequenceStep(NamedTuple):
|
||||
line: LineAnimator
|
||||
|
|
@ -1590,80 +1012,8 @@ class LineAnimationSequence():
|
|||
|
||||
|
||||
def start(self):
|
||||
self.start_at = time.perf_counter()
|
||||
self.start_at = time.time()
|
||||
self.idx = 0
|
||||
|
||||
def as_renderable_line(self, dt: DeltaT):
|
||||
return self.seq[self.idx].as_renderable_line(dt)
|
||||
|
||||
|
||||
def layers_to_protobuf(layers: RenderableLayers):
|
||||
pb_layers = renderable_pb2.RenderableLayers()
|
||||
|
||||
new_layers = {}
|
||||
|
||||
for n, lines in layers.items():
|
||||
# pb_lines = pb_layers.layers[n].value.add()
|
||||
pb_lines = renderable_pb2.RenderableLines()
|
||||
pb_lines.space = renderable_pb2.CoordinateSpace.WORLD
|
||||
|
||||
for line in lines.lines:
|
||||
l = renderable_pb2.RenderableLine()
|
||||
for p in line.points:
|
||||
point = renderable_pb2.RenderablePoint()
|
||||
point.position.x = p.position[0]
|
||||
point.position.y = p.position[1]
|
||||
point.color.red = p.color.red
|
||||
point.color.green = p.color.green
|
||||
point.color.blue = p.color.blue
|
||||
point.color.alpha = p.color.alpha
|
||||
l.points.append(point)
|
||||
pb_lines.lines.append(l)
|
||||
|
||||
|
||||
# new_layers[n] = pb_lines
|
||||
pb_layers.layers[n].CopyFrom(pb_lines)
|
||||
# pb_layers.layers = new_layers
|
||||
return pb_layers
|
||||
|
||||
def layers_to_message(layers: RenderableLayers):
|
||||
# t1 = time.perf_counter()
|
||||
buf = layers_to_protobuf(layers)
|
||||
# t2 = time.perf_counter()
|
||||
s = buf.SerializeToString()
|
||||
# t3 = time.perf_counter()
|
||||
|
||||
# print( t2-t1,t3-t2)
|
||||
|
||||
return s
|
||||
|
||||
def message_to_layers(s: str) -> RenderableLayers:
|
||||
"""Decode the protobuf"""
|
||||
pb_layers = renderable_pb2.RenderableLayers()
|
||||
pb_layers.ParseFromString(s)
|
||||
|
||||
|
||||
layers = {}
|
||||
|
||||
for n, pb_lines in pb_layers.layers.items():
|
||||
lines = []
|
||||
for pb_line in pb_lines.lines:
|
||||
points = []
|
||||
for pb_point in pb_line.points:
|
||||
color = SrgbaColor(
|
||||
pb_point.color.red,
|
||||
pb_point.color.green,
|
||||
pb_point.color.blue,
|
||||
pb_point.color.alpha,
|
||||
)
|
||||
|
||||
point = RenderablePoint(
|
||||
(pb_point.position.x, pb_point.position.y),
|
||||
color
|
||||
)
|
||||
points.append(point)
|
||||
|
||||
lines.append(RenderableLine(points))
|
||||
layers[n] = RenderableLines(lines, CoordinateSpace.WORLD)
|
||||
|
||||
return layers
|
||||
74
trap/node.py
74
trap/node.py
|
|
@ -1,11 +1,10 @@
|
|||
from collections import defaultdict
|
||||
import logging
|
||||
from logging.handlers import QueueHandler, QueueListener, SocketHandler
|
||||
import multiprocessing
|
||||
from multiprocessing.synchronize import Event as BaseEvent
|
||||
from argparse import ArgumentParser, Namespace
|
||||
import time
|
||||
from typing import Any, Optional
|
||||
from typing import Optional
|
||||
|
||||
import zmq
|
||||
|
||||
|
|
@ -15,8 +14,6 @@ from trap.timer import Timer
|
|||
|
||||
class Node():
|
||||
def __init__(self, config: Namespace, is_running: BaseEvent, fps_counter: CounterFpsSender):
|
||||
self.node_id = self.__class__.__name__.lower()
|
||||
|
||||
self.config = config
|
||||
self.is_running = is_running
|
||||
self.fps_counter = fps_counter
|
||||
|
|
@ -27,11 +24,6 @@ class Node():
|
|||
|
||||
self.dt_since_last_tick = 0
|
||||
|
||||
self.config_sock = self.sub(self.config.zmq_config_addr)
|
||||
self.config_init_sock = self.push(self.config.zmq_config_init_addr) # a sending sub
|
||||
self.settings = defaultdict(None)
|
||||
self.refresh_settings()
|
||||
|
||||
self.setup()
|
||||
|
||||
@classmethod
|
||||
|
|
@ -49,45 +41,13 @@ class Node():
|
|||
def run(self):
|
||||
raise RuntimeError("Not implemented run()")
|
||||
|
||||
def stop(self):
|
||||
"""
|
||||
Called when runloop is stopped. Override to clean up what was initiated in start() and run() methods
|
||||
"""
|
||||
pass
|
||||
|
||||
def refresh_settings(self):
|
||||
try:
|
||||
self.config_init_sock.send_string(self.node_id, zmq.NOBLOCK)
|
||||
except Exception as e:
|
||||
self.logger.warning('No settings socket available')
|
||||
self.logger.exception(e)
|
||||
|
||||
|
||||
def run_loop(self):
|
||||
"""Use in run(), to check if it should keep looping
|
||||
Takes care of tick()'ing the iterations/second counter
|
||||
"""
|
||||
self.tick()
|
||||
self.check_config()
|
||||
return self.is_running.is_set()
|
||||
|
||||
def check_config(self):
|
||||
while True:
|
||||
try:
|
||||
config = self.config_sock.recv_json(zmq.NOBLOCK)
|
||||
|
||||
for field, value in config.items():
|
||||
self.settings[field] = value
|
||||
except zmq.ZMQError as e:
|
||||
# no msgs
|
||||
break
|
||||
|
||||
def get_setting(self, name: str, default: Any):
|
||||
if name in self.settings:
|
||||
return self.settings[name]
|
||||
return default
|
||||
|
||||
|
||||
def run_loop_capped_fps(self, max_fps: float, warn_below_fps: float = 0.):
|
||||
"""Use in run(), to check if it should keep looping
|
||||
Takes care of tick()'ing the iterations/second counter
|
||||
|
|
@ -132,14 +92,6 @@ class Node():
|
|||
type=int,
|
||||
default=19996
|
||||
)
|
||||
parser.add_argument('--zmq-config-addr',
|
||||
help='Manually specity communication addr for the config messages',
|
||||
type=str,
|
||||
default="ipc:///tmp/feeds_config")
|
||||
parser.add_argument('--zmq-config-init-addr',
|
||||
help='Manually specity communication addr for req-rep config messages',
|
||||
type=str,
|
||||
default="ipc:///tmp/feeds_config_rr")
|
||||
return parser
|
||||
|
||||
|
||||
|
|
@ -158,27 +110,10 @@ class Node():
|
|||
sock.bind(addr)
|
||||
return sock
|
||||
|
||||
def push(self, addr: str):
|
||||
"push-pull pair"
|
||||
sock = self.zmq_context.socket(zmq.PUSH)
|
||||
# sock.setsockopt(zmq.LINGER, 0)
|
||||
sock.connect(addr)
|
||||
return sock
|
||||
|
||||
def pull(self, addr: str):
|
||||
"Push-pull pair"
|
||||
sock = self.zmq_context.socket(zmq.PULL)
|
||||
sock.bind(addr)
|
||||
return sock
|
||||
|
||||
@classmethod
|
||||
def start(cls, config: Namespace, is_running: BaseEvent, timer_counter: Optional[Timer]):
|
||||
instance = cls(config, is_running, timer_counter)
|
||||
try:
|
||||
instance.run()
|
||||
except Exception as e:
|
||||
instance.logger.exception(f"{e}")
|
||||
instance.stop()
|
||||
instance.logger.info("Stopping")
|
||||
|
||||
@classmethod
|
||||
|
|
@ -204,14 +139,11 @@ def setup_logging(config: Namespace):
|
|||
logging.captureWarnings(True)
|
||||
# root_logger.setLevel(logging.NOTSET) # to send all records to cutelog
|
||||
socket_handler = SocketHandler(config.remote_log_addr, config.remote_log_port)
|
||||
|
||||
# print(socket_handler.host, socket_handler.port)
|
||||
print(socket_handler.host, socket_handler.port)
|
||||
socket_handler.setLevel(logging.NOTSET)
|
||||
log_handlers.append(socket_handler)
|
||||
|
||||
logging.basicConfig(
|
||||
level=loglevel,
|
||||
handlers=log_handlers, # [queue_handler]
|
||||
format="%(asctime)s %(levelname)s:%(name)s:%(message)s",
|
||||
datefmt="%H:%M:%S"
|
||||
handlers=log_handlers # [queue_handler]
|
||||
)
|
||||
|
|
@ -6,26 +6,22 @@ import pathlib
|
|||
import pickle
|
||||
import random
|
||||
import time
|
||||
from typing import List
|
||||
import warnings
|
||||
from argparse import ArgumentParser, Namespace
|
||||
from multiprocessing import Event
|
||||
|
||||
import dill
|
||||
import numpy as np
|
||||
import shapely
|
||||
import torch
|
||||
import zmq
|
||||
from trajectron.environment import Environment, Scene, GeometricMap
|
||||
from trajectron.environment import Environment, Scene
|
||||
from trajectron.model.model_registrar import ModelRegistrar
|
||||
from trajectron.model.online.online_trajectron import OnlineTrajectron
|
||||
from trajectron.utils import prediction_output_to_trajectories
|
||||
|
||||
from trap.frame_emitter import DataclassJSONEncoder, Frame
|
||||
from trap.lines import load_lines_from_svg
|
||||
from trap.node import Node
|
||||
from trap.tracker import Smoother
|
||||
from trap.utils import ImageMap
|
||||
|
||||
logger = logging.getLogger("trap.prediction")
|
||||
|
||||
|
|
@ -54,21 +50,19 @@ def create_online_env(env, hyperparams, scene_idx, init_timestep):
|
|||
init_timestep + 1),
|
||||
state=hyperparams['state'])
|
||||
online_scene.robot = test_scene.robot
|
||||
radius = {k: 0 for k,v in env.attention_radius.items()}
|
||||
|
||||
online_scene.calculate_scene_graph(attention_radius=radius,
|
||||
online_scene.calculate_scene_graph(attention_radius=env.attention_radius,
|
||||
edge_addition_filter=hyperparams['edge_addition_filter'],
|
||||
edge_removal_filter=hyperparams['edge_removal_filter'])
|
||||
|
||||
return Environment(node_type_list=env.node_type_list,
|
||||
standardization=env.standardization,
|
||||
scenes=[online_scene],
|
||||
attention_radius=radius,
|
||||
attention_radius=env.attention_radius,
|
||||
robot_type=env.robot_type)
|
||||
|
||||
|
||||
def get_maps_for_input(input_dict, scene: Scene, hyperparams, device):
|
||||
scene_maps: List[ImageMap] = list()
|
||||
def get_maps_for_input(input_dict, scene, hyperparams, device):
|
||||
scene_maps = list()
|
||||
scene_pts = list()
|
||||
heading_angles = list()
|
||||
patch_sizes = list()
|
||||
|
|
@ -90,11 +84,10 @@ def get_maps_for_input(input_dict, scene: Scene, hyperparams, device):
|
|||
else:
|
||||
heading_angle = None
|
||||
|
||||
scene_map: ImageMap = scene.map[node.type]
|
||||
scene_map.set_bounds() # update old pickled maps
|
||||
scene_map = scene.map[node.type]
|
||||
# map_point = x[-1, :2]
|
||||
map_point = x[:2]
|
||||
# map_point = x[:2].clip(0) # prevent crash for out of map point.
|
||||
# map_point = x[:2]
|
||||
map_point = x[:2].clip(0) # prevent crash for out of map point.
|
||||
|
||||
patch_size = hyperparams['map_encoder'][node.type]['patch_size']
|
||||
|
||||
|
|
@ -111,16 +104,11 @@ def get_maps_for_input(input_dict, scene: Scene, hyperparams, device):
|
|||
|
||||
# print(scene_maps, patch_sizes, heading_angles)
|
||||
# print(scene_pts)
|
||||
try:
|
||||
maps = scene_maps[0].get_cropped_maps_from_scene_map_batch(scene_maps,
|
||||
scene_pts=torch.Tensor(scene_pts),
|
||||
patch_size=patch_sizes[0],
|
||||
rotation=heading_angles,
|
||||
device='cpu')
|
||||
except Exception as e:
|
||||
# print(scene_maps)
|
||||
logger.warning(f"Crash on getting maps for points: {scene_pts=} {heading_angles=} {patch_size=}")
|
||||
raise e
|
||||
|
||||
maps_dict = {node: maps[[i]].to(device) for i, node in enumerate(nodes_with_maps)}
|
||||
return maps_dict
|
||||
|
|
@ -166,15 +154,6 @@ class PredictionServer(Node):
|
|||
self.prediction_socket = self.pub(self.config.zmq_prediction_addr)
|
||||
self.external_predictions = not self.config.zmq_prediction_addr.startswith("ipc://")
|
||||
|
||||
self.cutoff_shape = None
|
||||
if self.config.cutoff_map:
|
||||
|
||||
self.cutoff_line = load_lines_from_svg(self.config.cutoff_map, 100, '')[0]
|
||||
self.cutoff_shape = shapely.Polygon([p.position for p in self.cutoff_line.points])
|
||||
|
||||
logger.info(f"{self.cutoff_shape}")
|
||||
|
||||
|
||||
|
||||
def send_frame(self, frame: Frame):
|
||||
if self.external_predictions:
|
||||
|
|
@ -197,8 +176,7 @@ class PredictionServer(Node):
|
|||
# model_dir = 'models/models_04_Oct_2023_21_04_48_eth_vel_ar3'
|
||||
|
||||
# Load hyperparameters from json
|
||||
# config_file = os.path.join(self.config.model_dir, self.config.conf)
|
||||
config_file = self.config.conf
|
||||
config_file = os.path.join(self.config.model_dir, self.config.conf)
|
||||
if not os.path.exists(config_file):
|
||||
raise ValueError('Config json not found!')
|
||||
with open(config_file, 'r') as conf_json:
|
||||
|
|
@ -238,9 +216,6 @@ class PredictionServer(Node):
|
|||
logger.info(f"Basing online env on {eval_scene=} -- loaded from {self.config.eval_data_dict}")
|
||||
online_env = create_online_env(eval_env, hyperparams, scene_idx, init_timestep)
|
||||
|
||||
print("overriding attention radius")
|
||||
online_env.attention_radius = {(online_env.NodeType.PEDESTRIAN, online_env.NodeType.PEDESTRIAN): 0.1}
|
||||
|
||||
# auto-find highest iteration
|
||||
model_registrar = ModelRegistrar(self.config.model_dir, self.config.eval_device)
|
||||
model_iterations = pathlib.Path(self.config.model_dir).glob('model_registrar-*.pt')
|
||||
|
|
@ -314,7 +289,6 @@ class PredictionServer(Node):
|
|||
|
||||
input_dict = {}
|
||||
for identifier, track in frame.tracks.items():
|
||||
|
||||
# if len(trajectory['history']) < 7:
|
||||
# # TODO: these trajectories should still be in the output, but without predictions
|
||||
# continue
|
||||
|
|
@ -331,16 +305,7 @@ class PredictionServer(Node):
|
|||
if len(track.history) < 2:
|
||||
continue
|
||||
|
||||
|
||||
|
||||
node = track.to_trajectron_node(frame.camera, online_env)
|
||||
|
||||
if self.cutoff_shape:
|
||||
position = shapely.Point(node.data.data[-1][:2])
|
||||
if not shapely.contains(self.cutoff_shape, position):
|
||||
# logger.debug(f"Skip position {position}")
|
||||
continue
|
||||
|
||||
# print(node.data.data[-1])
|
||||
input_dict[node] = np.array(object=node.data.data[-1])
|
||||
# print("history", node.data.data[-10:])
|
||||
|
|
@ -379,7 +344,6 @@ class PredictionServer(Node):
|
|||
# )
|
||||
|
||||
# input_dict[node] = np.array(object=[x[-1],y[-1],vx[-1],vy[-1],ax[-1],ay[-1]])
|
||||
# break # only on
|
||||
|
||||
# print(input_dict)
|
||||
|
||||
|
|
@ -396,11 +360,9 @@ class PredictionServer(Node):
|
|||
continue
|
||||
|
||||
maps = None
|
||||
start_maps = time.time()
|
||||
if hyperparams['use_map_encoding']:
|
||||
maps = get_maps_for_input(input_dict, eval_scene, hyperparams, device=self.config.eval_device)
|
||||
|
||||
|
||||
# print(maps)
|
||||
|
||||
# robot_present_and_future = None
|
||||
|
|
@ -428,8 +390,7 @@ class PredictionServer(Node):
|
|||
gmm_mode=self.config.gmm_mode, # "If True: The mode of the Gaussian Mixture Model (GMM) is sampled (see trajectron.model.mgcvae.py)"
|
||||
z_mode=self.config.z_mode # "Predictions from the model’s most-likely high-level latent behavior mode" (see trajecton.models.components.discrete_latent:sample_p(most_likely_z=z_mode))
|
||||
)
|
||||
print(len(dists), len (preds))
|
||||
intermediate = time.time()
|
||||
|
||||
# unsure what this bit from online_prediction.py does:
|
||||
# detailed_preds_dict = dict()
|
||||
# for node in eval_scene.nodes:
|
||||
|
|
@ -449,8 +410,8 @@ class PredictionServer(Node):
|
|||
|
||||
|
||||
end = time.time()
|
||||
logger.debug("took %.2f s (= %.2f Hz), maps: %.2f, forward: %.2f w/ %d nodes and %d edges -- init: %.2f s" % (end - start,
|
||||
1. / (end - start), (start-start_maps)/(end - start), (intermediate-start)/(end - start), len(trajectron.nodes),
|
||||
logger.debug("took %.2f s (= %.2f Hz) w/ %d nodes and %d edges -- init: %.2f s" % (end - start,
|
||||
1. / (end - start), len(trajectron.nodes),
|
||||
trajectron.scene_graph.get_num_edges(), start-t_init))
|
||||
|
||||
# if self.config.center_data:
|
||||
|
|
@ -472,7 +433,7 @@ class PredictionServer(Node):
|
|||
futures_dict = futures_dict[ts_key]
|
||||
|
||||
response = {}
|
||||
# logger.debug(f"{histories_dict=}")
|
||||
logger.debug(f"{histories_dict=}")
|
||||
for node in histories_dict:
|
||||
history = histories_dict[node]
|
||||
# future = futures_dict[node] # ground truth dict
|
||||
|
|
@ -480,9 +441,7 @@ class PredictionServer(Node):
|
|||
# print('preds', len(predictions[0][0]))
|
||||
|
||||
if not len(history) or np.isnan(history[-1]).any():
|
||||
logger.warning(f'skip for no history for {node} @ {ts_key} [{len(prediction_dict)=}, {len(histories_dict)=}, {len(futures_dict)=}]')
|
||||
# logger.info(f"{preds=}")
|
||||
|
||||
logger.warning('skip for no history')
|
||||
continue
|
||||
|
||||
# response[node.id] = {
|
||||
|
|
@ -540,9 +499,9 @@ class PredictionServer(Node):
|
|||
# default='../Trajectron-plus-plus/experiments/pedestrians/models/models_04_Oct_2023_21_04_48_eth_vel_ar3')
|
||||
|
||||
inference_parser.add_argument("--conf",
|
||||
help="path to json config file for hyperparameters",
|
||||
type=pathlib.Path,
|
||||
default='EXPERIMENTS/config.json')
|
||||
help="path to json config file for hyperparameters, relative to model_dir",
|
||||
type=str,
|
||||
default='config.json')
|
||||
|
||||
# Model Parameters (hyperparameters)
|
||||
inference_parser.add_argument("--offline_scene_graph",
|
||||
|
|
@ -597,12 +556,12 @@ class PredictionServer(Node):
|
|||
inference_parser.add_argument('--batch_size',
|
||||
help='training batch size',
|
||||
type=int,
|
||||
default=512)
|
||||
default=256)
|
||||
|
||||
inference_parser.add_argument('--k_eval',
|
||||
help='how many samples to take during evaluation',
|
||||
type=int,
|
||||
default=1)
|
||||
default=25)
|
||||
|
||||
# Data Parameters
|
||||
inference_parser.add_argument("--eval_data_dict",
|
||||
|
|
@ -624,7 +583,7 @@ class PredictionServer(Node):
|
|||
inference_parser.add_argument("--eval_device",
|
||||
help="what device to use during inference",
|
||||
type=str,
|
||||
default="cuda:0")
|
||||
default="cpu")
|
||||
|
||||
|
||||
inference_parser.add_argument('--seed',
|
||||
|
|
@ -665,11 +624,6 @@ class PredictionServer(Node):
|
|||
help="Center data around cx and cy. Should also be used when processing data",
|
||||
action='store_true')
|
||||
|
||||
inference_parser.add_argument('--cutoff-map',
|
||||
help='specify a map (svg-file) that specifies projection boundaries. In here, degrade chance to be selectede',
|
||||
type=str,
|
||||
default="../DATASETS/hof-lidar/map_hof.svg")
|
||||
|
||||
|
||||
return inference_parser
|
||||
|
||||
|
|
|
|||
|
|
@ -8,18 +8,16 @@ import time
|
|||
from xml.dom.pulldom import default_bufsize
|
||||
from attr import dataclass
|
||||
import cv2
|
||||
import noise
|
||||
import numpy as np
|
||||
import pandas as pd
|
||||
import dill
|
||||
import tqdm
|
||||
import argparse
|
||||
from typing import Dict, List, Optional
|
||||
from typing import List, Optional
|
||||
|
||||
from trap.base import Track
|
||||
from trap.config import CameraAction, HomographyAction
|
||||
from trap.frame_emitter import Camera
|
||||
from trap.tracker import FinalDisplacementFilter, Noiser, RandomOffset, Smoother, TrackReader
|
||||
from trap.tracker import FinalDisplacementFilter, Smoother, TrackReader
|
||||
|
||||
#sys.path.append("../../")
|
||||
from trajectron.environment import Environment, Scene, Node
|
||||
|
|
@ -74,29 +72,22 @@ class TrackIteration:
|
|||
smooth: bool
|
||||
step_size: int
|
||||
step_offset: int
|
||||
noisy: bool = False
|
||||
offset: bool = False
|
||||
|
||||
@classmethod
|
||||
def iteration_variations(cls, smooth = True, toggle_smooth=True, sample_step_size=1, noisy_variations=0, offset_variations=0):
|
||||
def iteration_variations(cls, smooth = True, toggle_smooth=True, sample_step_size=1):
|
||||
iterations: List[TrackIteration] = []
|
||||
for i in range(sample_step_size):
|
||||
for n in range(noisy_variations+1):
|
||||
for f in range(offset_variations+1):
|
||||
iterations.append(TrackIteration(smooth, sample_step_size, i, noisy=bool(n), offset=bool(f)))
|
||||
if smooth and toggle_smooth:
|
||||
iterations.append(TrackIteration(not smooth, sample_step_size, i, noisy=bool(n), offset=bool(f)))
|
||||
iterations.append(TrackIteration(smooth, sample_step_size, i))
|
||||
if toggle_smooth:
|
||||
iterations.append(TrackIteration(not smooth, sample_step_size, i))
|
||||
return iterations
|
||||
|
||||
# maybe_makedirs('trajectron-data')
|
||||
# for desired_source in [ 'hof2', ]:# ,'hof-maskrcnn', 'hof-yolov8', 'VIRAT-0102-parsed', 'virat-resnet-keypoints-full']:
|
||||
|
||||
def process_data(src_dir: Path, dst_dir: Path, name: str, smooth_tracks: bool, noise_tracks: int, offset_tracks: int, center_data: bool, bin_positions: bool, camera: Camera, step_size: int, filter_displacement:float, map_img_path: Optional[Path]):
|
||||
def process_data(src_dir: Path, dst_dir: Path, name: str, smooth_tracks: bool, cm_to_m: bool, center_data: bool, bin_positions: bool, camera: Camera, step_size: int, filter_displacement:float, map_img_path: Optional[Path]):
|
||||
name += f"-nostep" if step_size == 1 else f"-step{step_size}"
|
||||
# name += f"-conv{smooth_window}" if smooth_tracks else f"-nosmooth"
|
||||
name += f"-kalsmooth" if smooth_tracks else f"-nosmooth"
|
||||
name += f"-noise{noise_tracks}" if noise_tracks else f""
|
||||
name += f"-offsets{offset_tracks}" if offset_tracks else f""
|
||||
name += f"-conv{smooth_window}" if smooth_tracks else f"-nosmooth"
|
||||
name += f"-f{filter_displacement}" if filter_displacement > 0 else ""
|
||||
name += "-map" if map_img_path else "-nomap"
|
||||
name += f"-{datetime.date.today()}"
|
||||
|
|
@ -107,15 +98,9 @@ def process_data(src_dir: Path, dst_dir: Path, name: str, smooth_tracks: bool, n
|
|||
if not map_img_path.exists():
|
||||
raise RuntimeError(f"Map image does not exists {map_img_path}")
|
||||
|
||||
print(f"Using map {map_img_path}")
|
||||
|
||||
type_map = {}
|
||||
# TODO)) For now, assume the map is a 100x scale of the world coordinates (i.e. 100px per meter)
|
||||
# thus when we do a homography of 5px per meter, scale down by 20
|
||||
map_H_path = map_img_path.with_suffix('.json')
|
||||
if map_H_path.exists():
|
||||
homography_matrix = np.loadtxt(map_H_path)
|
||||
else:
|
||||
homography_matrix = np.array([
|
||||
[5, 0,0],
|
||||
[0, 5,0],
|
||||
|
|
@ -138,37 +123,21 @@ def process_data(src_dir: Path, dst_dir: Path, name: str, smooth_tracks: bool, n
|
|||
skipped_for_error = 0
|
||||
created = 0
|
||||
|
||||
# smoother = Smoother(window_len=smooth_window, convolution=True) if smooth_tracks else None
|
||||
smoother = Smoother(convolution=False) if smooth_tracks else None
|
||||
noiser = Noiser(amplitude=.1) if noise_tracks else None
|
||||
smoother = Smoother(window_len=smooth_window, convolution=True) if smooth_tracks else None
|
||||
|
||||
reader = TrackReader(src_dir, camera.fps)
|
||||
tracks = [t for t in reader]
|
||||
print(f"Unfiltered total: {len(tracks)} tracks")
|
||||
if filter_displacement > 0:
|
||||
filter = FinalDisplacementFilter(filter_displacement)
|
||||
tracks = filter.apply(tracks, camera)
|
||||
print(f"Filtered: {len(tracks)} tracks")
|
||||
|
||||
skip_idxs = []
|
||||
for idx, track in enumerate(tracks):
|
||||
track_history = track.get_projected_history(camera=camera)
|
||||
distances = np.sqrt(np.sum(np.diff(track_history, axis=0)**2, axis=1))
|
||||
# print(trajectory_org)
|
||||
# print(distances)
|
||||
if any(distances > 3):
|
||||
skip_idxs.append(idx)
|
||||
for idx in skip_idxs:
|
||||
tracks.pop(idx)
|
||||
print(f"Filtered {len(skip_idxs)} tracks which contained leaps")
|
||||
|
||||
total = len(tracks)
|
||||
bar = tqdm.tqdm(total=total)
|
||||
|
||||
destinations = {
|
||||
'train': int(total * .91),
|
||||
'val': int(total * .08),
|
||||
'test': int(total * .01), # I don't realyl care about this
|
||||
'train': int(total * .8),
|
||||
'val': int(total * .12),
|
||||
'test': int(total * .08),
|
||||
}
|
||||
|
||||
max_track = reader.get(str(max([int(k) for k in reader._tracks.keys()])))
|
||||
|
|
@ -184,7 +153,7 @@ def process_data(src_dir: Path, dst_dir: Path, name: str, smooth_tracks: bool, n
|
|||
dt3 = RollingAverage()
|
||||
dt4 = RollingAverage()
|
||||
|
||||
sets: Dict[str, List[Track]] = {}
|
||||
sets = {}
|
||||
offset = 0
|
||||
for data_class, nr in destinations.items():
|
||||
# TODO)) think of a way to shuffle while keeping scenes
|
||||
|
|
@ -194,8 +163,7 @@ def process_data(src_dir: Path, dst_dir: Path, name: str, smooth_tracks: bool, n
|
|||
|
||||
print(f"Camera FPS: {camera.fps}, actual fps: {camera.fps/step_size} (or {(1/camera.fps)*step_size})")
|
||||
|
||||
names: Dict[str, Path] = {}
|
||||
max_pos = 0
|
||||
names = {}
|
||||
|
||||
for data_class, nr_of_items in destinations.items():
|
||||
env = Environment(node_type_list=['PEDESTRIAN'], standardization=standardization)
|
||||
|
|
@ -214,9 +182,7 @@ def process_data(src_dir: Path, dst_dir: Path, name: str, smooth_tracks: bool, n
|
|||
# scene = None
|
||||
|
||||
scene_nodes = defaultdict(lambda: [])
|
||||
variations = TrackIteration.iteration_variations(smooth_tracks, True, step_size, noise_tracks, offset_tracks)
|
||||
|
||||
print(f"Create {len(variations)} variations")
|
||||
variations = TrackIteration.iteration_variations(smooth_tracks, False, step_size)
|
||||
|
||||
for i, track in enumerate(sets[data_class]):
|
||||
bar.update()
|
||||
|
|
@ -244,20 +210,13 @@ def process_data(src_dir: Path, dst_dir: Path, name: str, smooth_tracks: bool, n
|
|||
interpolated_track = track.get_with_interpolated_history()
|
||||
b = time.time()
|
||||
|
||||
|
||||
|
||||
for variation_nr, iteration_settings in enumerate(variations):
|
||||
track = interpolated_track
|
||||
|
||||
if iteration_settings.noisy:
|
||||
track = noiser.apply_track(track)
|
||||
if iteration_settings.offset:
|
||||
offset = RandomOffset(amplitude=.1)
|
||||
track = offset.apply_track(track)
|
||||
if iteration_settings.smooth:
|
||||
track = smoother.smooth_track(track)
|
||||
track = smoother.smooth_track(interpolated_track)
|
||||
# track = Smoother(smooth_window, False).smooth_track(track)
|
||||
|
||||
else:
|
||||
track = interpolated_track # TODO)) Copy & move smooth outside iter loop
|
||||
c = time.time()
|
||||
|
||||
if iteration_settings.step_size > 1:
|
||||
|
|
@ -268,7 +227,6 @@ def process_data(src_dir: Path, dst_dir: Path, name: str, smooth_tracks: bool, n
|
|||
|
||||
# track.get_projected_history(H=None, camera=self.config.camera)
|
||||
node = track.to_trajectron_node(camera, env)
|
||||
max_pos = max(node.data.data[0][0], max_pos)
|
||||
|
||||
data_class = time.time()
|
||||
|
||||
|
|
@ -330,8 +288,7 @@ def process_data(src_dir: Path, dst_dir: Path, name: str, smooth_tracks: bool, n
|
|||
|
||||
# print(scene.nodes[0].first_timestep)
|
||||
|
||||
print(f'Processed {len(scenes)} scene with {sum([len(s.nodes) for s in scenes])} nodes for data class {data_class}')
|
||||
# print("MAXIMUM!!", max_pos)
|
||||
print(f'Processed {len(scenes):.2f} scene for data class {data_class}')
|
||||
|
||||
env.scenes = scenes
|
||||
|
||||
|
|
@ -341,29 +298,9 @@ def process_data(src_dir: Path, dst_dir: Path, name: str, smooth_tracks: bool, n
|
|||
with open(data_dict_path, 'wb') as f:
|
||||
dill.dump(env, f, protocol=dill.HIGHEST_PROTOCOL)
|
||||
|
||||
bar.close()
|
||||
|
||||
# print(f"Linear: {l}")
|
||||
# print(f"Non-Linear: {nl}")
|
||||
print(f"error: {skipped_for_error}, used: {created}")
|
||||
print("Run with")
|
||||
target_model_dir = (dst_dir / "../models/").resolve()
|
||||
target_config = (dst_dir / "../trajectron.json").resolve()
|
||||
# set eval_every very high, because we're not interested in theoretical evaluations, and we don't mind overfitting
|
||||
print(f"""
|
||||
uv run trajectron_train --eval_every 200 \\
|
||||
--train_data_dict {names['train'].name} \\
|
||||
--eval_data_dict {names['val'].name} \\
|
||||
--offline_scene_graph no --preprocess_workers 8 \\
|
||||
--log_dir {target_model_dir} \\
|
||||
--log_tag _{name} \\
|
||||
--train_epochs 100 \\
|
||||
--conf {target_config} \\
|
||||
--data_dir {dst_dir} \\
|
||||
{"--map_encoding" if map_img_path else ""} \\
|
||||
--no_edge_encoding
|
||||
""")
|
||||
|
||||
return names
|
||||
|
||||
def main():
|
||||
|
|
@ -372,8 +309,6 @@ def main():
|
|||
parser.add_argument("--dst-dir", "-d", type=Path, required=True, help="Destination directory to store parsed .pkl files (typically 'trajectron-data')")
|
||||
parser.add_argument("--name", "-n", type=str, required=True, help="Identifier to prefix the output .pkl files with (result is NAME-train.pkl, NAME-test.pkl)")
|
||||
parser.add_argument("--smooth-tracks", action='store_true', help=f"Enable smoother. Set to {smooth_window} frames")
|
||||
parser.add_argument("--noise-tracks", type=int, default=0, help=f"Enable Noiser. provide number for how many noisy variations")
|
||||
parser.add_argument("--offset-tracks", type=int, default=0, help=f"Enable Offset. provide number for how many random offset variations")
|
||||
parser.add_argument("--cm-to-m", action='store_true', help=f"If homography is in cm, convert tracked points to meter for beter results")
|
||||
parser.add_argument("--center-data", action='store_true', help=f"Normalise around center")
|
||||
parser.add_argument("--bin-positions", action='store_true', help=f"Experiment to put round positions to a grid")
|
||||
|
|
@ -411,8 +346,7 @@ def main():
|
|||
args.dst_dir,
|
||||
args.name,
|
||||
args.smooth_tracks,
|
||||
args.noise_tracks,
|
||||
args.offset_tracks,
|
||||
args.cm_to_m,
|
||||
args.center_data,
|
||||
args.bin_positions,
|
||||
args.camera,
|
||||
|
|
|
|||
|
|
@ -1,68 +0,0 @@
|
|||
# Generated by the protocol buffer compiler. DO NOT EDIT!
|
||||
# sources: renderable.proto
|
||||
# plugin: python-betterproto
|
||||
from dataclasses import dataclass
|
||||
from typing import Dict, List
|
||||
|
||||
import betterproto
|
||||
|
||||
|
||||
class CoordinateSpace(betterproto.Enum):
|
||||
"""Enum for coordinate spaces"""
|
||||
|
||||
UNDEFINED = 0
|
||||
CAMERA = 1
|
||||
UNDISTORTED_CAMERA = 2
|
||||
WORLD = 3
|
||||
LASER = 4
|
||||
RAW_LASER = 8
|
||||
|
||||
|
||||
@dataclass
|
||||
class RenderablePosition(betterproto.Message):
|
||||
"""Message for RenderablePosition (Tuple[float, float])"""
|
||||
|
||||
x: float = betterproto.float_field(1)
|
||||
y: float = betterproto.float_field(2)
|
||||
|
||||
|
||||
@dataclass
|
||||
class SrgbaColor(betterproto.Message):
|
||||
"""Message for SrgbaColor"""
|
||||
|
||||
red: float = betterproto.float_field(1)
|
||||
green: float = betterproto.float_field(2)
|
||||
blue: float = betterproto.float_field(3)
|
||||
alpha: float = betterproto.float_field(4)
|
||||
|
||||
|
||||
@dataclass
|
||||
class RenderablePoint(betterproto.Message):
|
||||
"""Message for RenderablePoint"""
|
||||
|
||||
position: "RenderablePosition" = betterproto.message_field(1)
|
||||
color: "SrgbaColor" = betterproto.message_field(2)
|
||||
|
||||
|
||||
@dataclass
|
||||
class RenderableLine(betterproto.Message):
|
||||
"""Message for RenderableLine"""
|
||||
|
||||
points: List["RenderablePoint"] = betterproto.message_field(1)
|
||||
|
||||
|
||||
@dataclass
|
||||
class RenderableLines(betterproto.Message):
|
||||
"""Message for RenderableLines"""
|
||||
|
||||
lines: List["RenderableLine"] = betterproto.message_field(1)
|
||||
space: "CoordinateSpace" = betterproto.enum_field(2)
|
||||
|
||||
|
||||
@dataclass
|
||||
class RenderableLayers(betterproto.Message):
|
||||
"""Message to represent RenderableLayers (Dict[int, RenderableLines])"""
|
||||
|
||||
layers: Dict[int, "RenderableLines"] = betterproto.map_field(
|
||||
1, betterproto.TYPE_INT32, betterproto.TYPE_MESSAGE
|
||||
)
|
||||
|
|
@ -1,50 +0,0 @@
|
|||
syntax = "proto3";
|
||||
|
||||
package renderable;
|
||||
|
||||
// Enum for coordinate spaces
|
||||
enum CoordinateSpace {
|
||||
UNDEFINED=0;
|
||||
CAMERA = 1;
|
||||
UNDISTORTED_CAMERA = 2;
|
||||
WORLD = 3;
|
||||
LASER = 4;
|
||||
RAW_LASER = 8;
|
||||
}
|
||||
|
||||
// Message for RenderablePosition (Tuple[float, float])
|
||||
message RenderablePosition {
|
||||
float x = 1;
|
||||
float y = 2;
|
||||
}
|
||||
|
||||
// Message for SrgbaColor
|
||||
message SrgbaColor {
|
||||
float red = 1;
|
||||
float green = 2;
|
||||
float blue = 3;
|
||||
float alpha = 4;
|
||||
}
|
||||
|
||||
// Message for RenderablePoint
|
||||
message RenderablePoint {
|
||||
RenderablePosition position = 1;
|
||||
SrgbaColor color = 2;
|
||||
}
|
||||
|
||||
// Message for RenderableLine
|
||||
message RenderableLine {
|
||||
repeated RenderablePoint points = 1;
|
||||
}
|
||||
|
||||
// Message for RenderableLines
|
||||
message RenderableLines {
|
||||
repeated RenderableLine lines = 1;
|
||||
CoordinateSpace space = 2;
|
||||
}
|
||||
|
||||
// Message to represent RenderableLayers (Dict[int, RenderableLines])
|
||||
message RenderableLayers {
|
||||
map<int32, RenderableLines> layers = 1;
|
||||
}
|
||||
|
||||
|
|
@ -1,41 +0,0 @@
|
|||
# -*- coding: utf-8 -*-
|
||||
# Generated by the protocol buffer compiler. DO NOT EDIT!
|
||||
# source: renderable.proto
|
||||
"""Generated protocol buffer code."""
|
||||
from google.protobuf.internal import builder as _builder
|
||||
from google.protobuf import descriptor as _descriptor
|
||||
from google.protobuf import descriptor_pool as _descriptor_pool
|
||||
from google.protobuf import symbol_database as _symbol_database
|
||||
# @@protoc_insertion_point(imports)
|
||||
|
||||
_sym_db = _symbol_database.Default()
|
||||
|
||||
|
||||
|
||||
|
||||
DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\x10renderable.proto\x12\nrenderable\"*\n\x12RenderablePosition\x12\t\n\x01x\x18\x01 \x01(\x02\x12\t\n\x01y\x18\x02 \x01(\x02\"E\n\nSrgbaColor\x12\x0b\n\x03red\x18\x01 \x01(\x02\x12\r\n\x05green\x18\x02 \x01(\x02\x12\x0c\n\x04\x62lue\x18\x03 \x01(\x02\x12\r\n\x05\x61lpha\x18\x04 \x01(\x02\"j\n\x0fRenderablePoint\x12\x30\n\x08position\x18\x01 \x01(\x0b\x32\x1e.renderable.RenderablePosition\x12%\n\x05\x63olor\x18\x02 \x01(\x0b\x32\x16.renderable.SrgbaColor\"=\n\x0eRenderableLine\x12+\n\x06points\x18\x01 \x03(\x0b\x32\x1b.renderable.RenderablePoint\"h\n\x0fRenderableLines\x12)\n\x05lines\x18\x01 \x03(\x0b\x32\x1a.renderable.RenderableLine\x12*\n\x05space\x18\x02 \x01(\x0e\x32\x1b.renderable.CoordinateSpace\"\x98\x01\n\x10RenderableLayers\x12\x38\n\x06layers\x18\x01 \x03(\x0b\x32(.renderable.RenderableLayers.LayersEntry\x1aJ\n\x0bLayersEntry\x12\x0b\n\x03key\x18\x01 \x01(\x05\x12*\n\x05value\x18\x02 \x01(\x0b\x32\x1b.renderable.RenderableLines:\x02\x38\x01*Z\n\x0f\x43oordinateSpace\x12\r\n\tUNDEFINED\x10\x00\x12\n\n\x06\x43\x41MERA\x10\x01\x12\x16\n\x12UNDISTORTED_CAMERA\x10\x02\x12\t\n\x05WORLD\x10\x03\x12\t\n\x05LASER\x10\x04\x62\x06proto3')
|
||||
|
||||
_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, globals())
|
||||
_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'renderable_pb2', globals())
|
||||
if _descriptor._USE_C_DESCRIPTORS == False:
|
||||
|
||||
DESCRIPTOR._options = None
|
||||
_RENDERABLELAYERS_LAYERSENTRY._options = None
|
||||
_RENDERABLELAYERS_LAYERSENTRY._serialized_options = b'8\001'
|
||||
_COORDINATESPACE._serialized_start=579
|
||||
_COORDINATESPACE._serialized_end=669
|
||||
_RENDERABLEPOSITION._serialized_start=32
|
||||
_RENDERABLEPOSITION._serialized_end=74
|
||||
_SRGBACOLOR._serialized_start=76
|
||||
_SRGBACOLOR._serialized_end=145
|
||||
_RENDERABLEPOINT._serialized_start=147
|
||||
_RENDERABLEPOINT._serialized_end=253
|
||||
_RENDERABLELINE._serialized_start=255
|
||||
_RENDERABLELINE._serialized_end=316
|
||||
_RENDERABLELINES._serialized_start=318
|
||||
_RENDERABLELINES._serialized_end=422
|
||||
_RENDERABLELAYERS._serialized_start=425
|
||||
_RENDERABLELAYERS._serialized_end=577
|
||||
_RENDERABLELAYERS_LAYERSENTRY._serialized_start=503
|
||||
_RENDERABLELAYERS_LAYERSENTRY._serialized_end=577
|
||||
# @@protoc_insertion_point(module_scope)
|
||||
175
trap/settings.py
175
trap/settings.py
|
|
@ -1,175 +0,0 @@
|
|||
from argparse import ArgumentParser
|
||||
import json
|
||||
import math
|
||||
from pathlib import Path
|
||||
from typing import Any, Dict
|
||||
|
||||
import zmq
|
||||
from trap.node import Node
|
||||
|
||||
import dearpygui.dearpygui as dpg
|
||||
|
||||
class Settings(Node):
|
||||
"""
|
||||
Quickndirty gui to change some settings ad-hoc
|
||||
no storage of values, no defaults. No detection of lost nodes, or sending config on them starting
|
||||
|
||||
"""
|
||||
def setup(self):
|
||||
|
||||
self.config_sock.close() # setup by default for all nodes, but we want to publish
|
||||
self.config_sock = self.pub(self.config.zmq_config_addr)
|
||||
|
||||
self.config_init_sock.close() # setup by default for all nodes, but we want to publish
|
||||
self.config_init_sock = self.pull(self.config.zmq_config_init_addr)
|
||||
|
||||
self.settings_fields = {}
|
||||
self.settings: Dict[str, Any] = {}
|
||||
|
||||
|
||||
self.load()
|
||||
|
||||
dpg.create_context()
|
||||
dpg.create_viewport(title='Trap settings', width=600, height=1200)
|
||||
dpg.setup_dearpygui()
|
||||
|
||||
|
||||
with dpg.window(label="General", pos=(0, 0)):
|
||||
dpg.add_text(f"Settings from {self.config.settings_file}")
|
||||
dpg.add_button(label="Save", callback=self.save)
|
||||
|
||||
with dpg.window(label="Renderer", pos=(0, 600)):
|
||||
for i in range(8) :
|
||||
self.register_setting(f'stagerenderer.layer.{i}', dpg.add_checkbox(label=f"layer {i}", default_value=self.get_setting(f'stagerenderer.layer.{i}', True), callback=self.on_change))
|
||||
self.register_setting(f'stagerenderer.scale', dpg.add_slider_float(label="scale", default_value=self.get_setting(f'stagerenderer.scale', 1), max_value=3, callback=self.on_change))
|
||||
self.register_setting(f'stagerenderer.dx', dpg.add_slider_int(label="dx", default_value=self.get_setting(f'stagerenderer.dx', 0), min_value=-300, max_value=300, callback=self.on_change))
|
||||
self.register_setting(f'stagerenderer.dy', dpg.add_slider_int(label="dy", default_value=self.get_setting(f'stagerenderer.dy', 0), min_value=-300, max_value=300, callback=self.on_change))
|
||||
self.register_setting(f'stagerenderer.fade', dpg.add_slider_float(label="fade factor", default_value=self.get_setting(f'stagerenderer.fade', 0.27), max_value=1, callback=self.on_change))
|
||||
|
||||
with dpg.window(label="Stage", pos=(150, 0)):
|
||||
self.register_setting(f'stage.fps', dpg.add_slider_int(label="FPS cap", default_value=self.get_setting(f'stage.fps', 30), callback=self.on_change))
|
||||
self.register_setting(f'stage.prediction_interval', dpg.add_slider_int(label="prediction interval", default_value=self.get_setting('stage.prediction_interval', 18), callback=self.on_change))
|
||||
self.register_setting(f'stage.loitering_animation', dpg.add_checkbox(label="loitering_animation", default_value=self.get_setting('stage.loitering_animation', True), callback=self.on_change))
|
||||
|
||||
with dpg.window(label="Lidar", pos=(0, 100), autosize=True):
|
||||
self.register_setting(f'lidar.crop_map_boundaries', dpg.add_checkbox(label="crop_map_boundaries", default_value=self.get_setting(f'lidar.crop_map_boundaries', True), callback=self.on_change))
|
||||
self.register_setting(f'lidar.viz_cropping', dpg.add_checkbox(label="viz_cropping", default_value=self.get_setting(f'lidar.viz_cropping', True), callback=self.on_change))
|
||||
# self.register_setting(f'lidar.voxel_downsample', dpg.add_checkbox(label="voxel_downsample", default_value=self.get_setting(f'lidar.voxel_downsample', True), callback=self.on_change))
|
||||
self.register_setting(f'lidar.tracking_enabled', dpg.add_checkbox(label="tracking_enabled", default_value=self.get_setting(f'lidar.tracking_enabled', True), callback=self.on_change))
|
||||
self.register_setting(f'lidar.kalman_factor', dpg.add_slider_float(label="kalman_factor", default_value=self.get_setting(f'lidar.kalman_factor', 1.3), max_value=3, callback=self.on_change))
|
||||
|
||||
|
||||
dpg.add_separator(label="Clustering")
|
||||
cluster_methods = ("birch", "optics", "dbscan")
|
||||
self.register_setting('lidar.cluster.method', dpg.add_combo(label="Method", items=cluster_methods, default_value=self.get_setting('lidar.cluster.method', default='dbscan'), callback=self.on_change))
|
||||
self.register_setting(f'lidar.eps', dpg.add_slider_float(label="DBSCAN epsilon", default_value=self.get_setting(f'lidar.eps', 0.3), max_value=1, callback=self.on_change))
|
||||
self.register_setting(f'lidar.min_samples', dpg.add_slider_int(label="DBSCAN min_samples", default_value=self.get_setting(f'lidar.min_samples', 8), max_value=30, callback=self.on_change))
|
||||
dpg.add_text("When using BIRCH, the resulting subclusters can be postprocessed by DBSCAN:")
|
||||
self.register_setting('lidar.birch_process_subclusters', dpg.add_checkbox(label="Process subclusters", default_value=self.get_setting('lidar.birch_process_subclusters', True), callback=self.on_change))
|
||||
self.register_setting('lidar.birch_threshold', dpg.add_slider_float(label="Threshold", default_value=self.get_setting('lidar.birch_threshold', 1), max_value=2.5, callback=self.on_change))
|
||||
self.register_setting('lidar.birch_branching_factor', dpg.add_slider_int(label="Branching factor", default_value=self.get_setting('lidar.birch_branching_factor', 50), max_value=100, callback=self.on_change))
|
||||
|
||||
dpg.add_separator(label="Cluster filter")
|
||||
self.register_setting(f'lidar.min_box_area', dpg.add_slider_float(label="min_box_area", default_value=self.get_setting(f'lidar.min_box_area', .1), min_value=0, max_value=1, callback=self.on_change))
|
||||
self.register_setting(f'lidar.max_box_area', dpg.add_slider_float(label="max_box_area", default_value=self.get_setting(f'lidar.max_box_area', 5), min_value=.5, max_value=10, callback=self.on_change))
|
||||
|
||||
for i, lidar in enumerate(["192.168.1.16", "192.168.0.10"]):
|
||||
name = lidar.replace(".", "_")
|
||||
with dpg.window(label=f"Lidar {lidar}", pos=(i * 300, 450),autosize=True):
|
||||
# dpg.add_text("test")
|
||||
# dpg.add_input_text(label="string", default_value="Quick brown fox")
|
||||
self.register_setting(f'lidar.{name}.enabled', dpg.add_checkbox(label="enabled", default_value=self.get_setting(f'lidar.{name}.enabled', True), callback=self.on_change))
|
||||
self.register_setting(f'lidar.{name}.rot_x', dpg.add_slider_float(label="rot_x", default_value=self.get_setting(f'lidar.{name}.rot_x', 0), max_value=math.pi * 2, callback=self.on_change))
|
||||
self.register_setting(f'lidar.{name}.rot_y', dpg.add_slider_float(label="rot_y", default_value=self.get_setting(f'lidar.{name}.rot_y', 0), max_value=math.pi * 2, callback=self.on_change))
|
||||
self.register_setting(f'lidar.{name}.rot_z', dpg.add_slider_float(label="rot_z", default_value=self.get_setting(f'lidar.{name}.rot_z', 0), max_value=math.pi * 2, callback=self.on_change))
|
||||
self.register_setting(f'lidar.{name}.trans_x', dpg.add_slider_float(label="trans_x", default_value=self.get_setting(f'lidar.{name}.trans_x', 0), min_value=-15, max_value=15, callback=self.on_change))
|
||||
self.register_setting(f'lidar.{name}.trans_y', dpg.add_slider_float(label="trans_y", default_value=self.get_setting(f'lidar.{name}.trans_y', 0), min_value=-15, max_value=15, callback=self.on_change))
|
||||
self.register_setting(f'lidar.{name}.trans_z', dpg.add_slider_float(label="trans_z", default_value=self.get_setting(f'lidar.{name}.trans_z', 0), min_value=-15, max_value=15, callback=self.on_change))
|
||||
|
||||
self.send_for_prefix("") # spread the defaults
|
||||
|
||||
dpg.show_viewport()
|
||||
|
||||
def stop(self):
|
||||
|
||||
dpg.destroy_context()
|
||||
|
||||
|
||||
def check_config(self):
|
||||
# override node function to disable it
|
||||
pass
|
||||
|
||||
|
||||
def refresh_settings(self):
|
||||
# override node function to disable it
|
||||
pass
|
||||
|
||||
|
||||
|
||||
def get_setting(self, name: str, default: Any):
|
||||
"""
|
||||
Automatically configure the value with the default when requesting it
|
||||
"""
|
||||
r = super().get_setting(name, default)
|
||||
self.settings[name] = r
|
||||
return r
|
||||
|
||||
def register_setting(self, name: str, field: int):
|
||||
self.settings_fields[field] = name
|
||||
|
||||
def on_change(self, sender, value, user_data = None):
|
||||
# print(sender, app_data, user_data)
|
||||
setting = self.settings_fields[sender]
|
||||
print(setting, value)
|
||||
self.settings[setting] = value
|
||||
self.config_sock.send_json({setting: value})
|
||||
|
||||
def send_for_prefix(self, prefix: str):
|
||||
self.config_sock.send_json(self.get_by_prefix(prefix))
|
||||
|
||||
def save(self):
|
||||
with self.config.settings_file.open('w') as fp:
|
||||
self.logger.info(f"Save to {self.config.settings_file}")
|
||||
json.dump(self.settings, fp)
|
||||
|
||||
def get_by_prefix(self, prefix: str) -> Dict[str, Any]:
|
||||
return {key: value for key, value in self.settings.items() if key.startswith(prefix)}
|
||||
|
||||
|
||||
def load(self) -> Dict[str, Any]:
|
||||
if not self.config.settings_file.exists():
|
||||
self.logger.info(f"No config at {self.config.settings_file}")
|
||||
return {}
|
||||
|
||||
self.logger.info(f"Loading from {self.config.settings_file}")
|
||||
with self.config.settings_file.open('r') as fp:
|
||||
self.settings = json.load(fp)
|
||||
|
||||
def run(self):
|
||||
# below replaces, start_dearpygui()
|
||||
while self.run_loop() and dpg.is_dearpygui_running():
|
||||
|
||||
# 1) receive init requests
|
||||
try:
|
||||
init_msg = self.config_init_sock.recv_string(zmq.NOBLOCK)
|
||||
self.logger.info(f"Send init for {init_msg}")
|
||||
print('init', init_msg)
|
||||
self.send_for_prefix(init_msg)
|
||||
except zmq.ZMQError as e:
|
||||
# no msgs
|
||||
pass
|
||||
|
||||
dpg.render_dearpygui_frame()
|
||||
|
||||
|
||||
|
||||
@classmethod
|
||||
def arg_parser(cls):
|
||||
argparser = ArgumentParser()
|
||||
argparser.add_argument('--settings-file',
|
||||
help='Where to store settings',
|
||||
type=Path,
|
||||
default=Path("./settings.json"))
|
||||
|
||||
return argparser
|
||||
|
||||
540
trap/stage.py
540
trap/stage.py
|
|
@ -1,17 +1,13 @@
|
|||
from __future__ import annotations
|
||||
|
||||
from abc import abstractmethod
|
||||
from argparse import ArgumentParser
|
||||
from collections import defaultdict
|
||||
from dataclasses import dataclass
|
||||
from enum import Enum
|
||||
from functools import partial
|
||||
import json
|
||||
import logging
|
||||
from math import inf
|
||||
import math
|
||||
from pathlib import Path
|
||||
import random
|
||||
import time
|
||||
import threading
|
||||
from typing import Dict, Generator, List, Optional, Type, TypeVar
|
||||
|
|
@ -22,10 +18,9 @@ import zmq
|
|||
from trap.anomaly import DiffSegment, calc_anomaly, calculate_loitering_scores
|
||||
from trap.base import CameraAction, DataclassJSONEncoder, Frame, HomographyAction, ProjectedTrack, Track
|
||||
from trap.counter import CounterSender
|
||||
from trap.lines import AppendableLine, AppendableLineAnimator, Coordinate, CoordinateSpace, CropAnimationLine, CropLine, DashedLine, DeltaT, FadeOutJitterLine, FadeOutLine, FadedEndsLine, FadedTailLine, LineAnimationStack, LineAnimator, NoiseLine, RenderableLayers, RenderableLine, RenderableLines, RotatingLine, SegmentLine, SimplifyLine, SimplifyMethod, SrgbaColor, StartFromClosestPoint, StaticLine, layers_to_message, load_lines_from_svg
|
||||
from trap.lines import AppendableLine, AppendableLineAnimator, Coordinate, CropLine, DashedLine, DeltaT, FadeOutJitterLine, FadeOutLine, FadedTailLine, LineAnimationStack, LineAnimator, NoiseLine, RenderableLayers, RenderableLine, RenderableLines, SegmentLine, SimplifyMethod, SrgbaColor, StaticLine, load_lines_from_svg
|
||||
from trap.node import Node
|
||||
from trap.track_history import TrackHistory
|
||||
from trap.utils import lerp
|
||||
|
||||
|
||||
logger = logging.getLogger('trap.stage')
|
||||
|
|
@ -38,15 +33,15 @@ OPTION_TRACK_NOISE = False
|
|||
|
||||
TRACK_ASSUMED_FPS = 12
|
||||
|
||||
|
||||
TAKEOVER_FADEOUT = 3
|
||||
LOST_FADEOUT = 2 # seconds
|
||||
PREDICTION_INTERVAL: int|None = int(TRACK_ASSUMED_FPS * 1.2) # frames
|
||||
PREDICTION_INTERVAL: float|None = 20 # frames
|
||||
PREDICTION_FADE_IN: float = 3
|
||||
PREDICTION_FADE_SLOPE: float = -10
|
||||
PREDICTION_FADE_AFTER_DURATION: float = 8 # seconds
|
||||
PREDICTION_END_FADE = 2 #frames
|
||||
# TRACK_MAX_POINTS = 100
|
||||
TRACK_FADE_AFTER_DURATION = 9. # seconds
|
||||
TRACK_FADE_AFTER_DURATION = 15. # seconds
|
||||
TRACK_END_FADE = 30 # points
|
||||
TRACK_FADE_ASSUME_FPS = TRACK_ASSUMED_FPS
|
||||
|
||||
|
|
@ -71,79 +66,25 @@ class SceneInfo:
|
|||
priority: int
|
||||
description: str = ""
|
||||
takeover_possible: bool = False # whether to allow for other scenarios to steal the stage
|
||||
takeover_possible_after: float = -1
|
||||
|
||||
class ScenarioScene(Enum):
|
||||
DETECTED = SceneInfo(4, "First detection")
|
||||
TRACKED = SceneInfo(6, "Multiple detections")
|
||||
PREDICTION_AVAILABLE = SceneInfo(10, "Prediction is ready")
|
||||
UPDATED_PREDICTION = SceneInfo(11, "Multiple predictions")
|
||||
LOITERING = SceneInfo(7, "Foundto be loitering", takeover_possible=True, takeover_possible_after=10) # TODO: create "possible after"
|
||||
PLAY = SceneInfo(7, description="After many predictions; just fooling around", takeover_possible=True, takeover_possible_after=10)
|
||||
LOST = SceneInfo(-1, description="Track lost", takeover_possible=True, takeover_possible_after=0)
|
||||
SUBSTANTIAL = SceneInfo(6, "Multiple detections")
|
||||
FIRST_PREDICTION = SceneInfo(10, "Prediction is ready")
|
||||
CORRECTED_PREDICTION = SceneInfo(11, "Multiple predictions")
|
||||
LOITERING = SceneInfo(7, "Foundto be loitering", takeover_possible=True)
|
||||
PLAY = SceneInfo(7, description="After many predictions; just fooling around", takeover_possible=True)
|
||||
LOST = SceneInfo(-1, description="Track lost", takeover_possible=True)
|
||||
|
||||
Time = float
|
||||
|
||||
class PrioritySlotItem():
|
||||
TAKEOVER_FADEOUT = 3
|
||||
|
||||
def __init__(self, identifier):
|
||||
self.identifier = identifier
|
||||
self.start_time = 0.
|
||||
self.take_over_at: Optional[Time] = None
|
||||
|
||||
def take_over(self):
|
||||
if self.take_over_at:
|
||||
return
|
||||
|
||||
self.take_over_at = time.perf_counter()
|
||||
|
||||
def taken_over(self):
|
||||
self.is_running = False
|
||||
self.take_over_at = None
|
||||
|
||||
def takenover_for(self):
|
||||
if self.take_over_at:
|
||||
return time.perf_counter() - self.take_over_at
|
||||
return None
|
||||
|
||||
def takeover_factor(self):
|
||||
l = self.takenover_for()
|
||||
if not l:
|
||||
return 0
|
||||
return l/self.TAKEOVER_FADEOUT
|
||||
|
||||
def start(self):
|
||||
# change when visible
|
||||
logger.info(f"Start {self.identifier}: {self.get_state_name()}")
|
||||
self.start_time = time.perf_counter()
|
||||
self.is_running = True
|
||||
|
||||
def running_for(self):
|
||||
return time.perf_counter() - self.start_time
|
||||
|
||||
@abstractmethod
|
||||
def get_priority(self) -> int:
|
||||
raise RuntimeError("Not implemented")
|
||||
|
||||
@abstractmethod
|
||||
def get_state_name(self) -> str:
|
||||
raise RuntimeError("Not implemented")
|
||||
|
||||
@abstractmethod
|
||||
def can_be_taken_over(self):
|
||||
raise RuntimeError("Not implemented")
|
||||
|
||||
|
||||
class Scenario(PrioritySlotItem):
|
||||
def __init__(self, track_id, stage: Stage):
|
||||
super().__init__(track_id)
|
||||
self.stage = stage
|
||||
class Scenario:
|
||||
def __init__(self, track_id):
|
||||
self.track_id = track_id
|
||||
self.scene: ScenarioScene = ScenarioScene.DETECTED
|
||||
|
||||
self.start_time = 0.
|
||||
self.current_time = 0
|
||||
|
||||
self.take_over_at: Optional[Time] = None
|
||||
|
||||
self.track: Optional[ProjectedTrack] = None
|
||||
self.prediction_tracks: List[ProjectedTrack] = []
|
||||
|
|
@ -156,26 +97,12 @@ class Scenario(PrioritySlotItem):
|
|||
|
||||
self.is_running = False
|
||||
|
||||
self.loitering_factor = 0
|
||||
|
||||
logger.info(f"Found {self.track_id}: {self.scene.name}")
|
||||
|
||||
def get_state_name(self):
|
||||
return self.scene.name
|
||||
|
||||
def get_priority(self) -> int:
|
||||
# newer higher prio
|
||||
distance = 0
|
||||
# todo: check if last point is within bounds
|
||||
if self.track and len(self.track.projected_history) > 5:
|
||||
distance = np.linalg.norm(self.track.projected_history[-1] - self.track.projected_history[0])
|
||||
return (self.scene.value.priority, distance)
|
||||
|
||||
def can_be_taken_over(self):
|
||||
if self.scene.value.takeover_possible:
|
||||
if time.perf_counter() - self.state_change_at > self.scene.value.takeover_possible_after:
|
||||
return True
|
||||
return False
|
||||
def start(self):
|
||||
# change when visible
|
||||
logger.info(f"Start {self.track_id}: {self.scene.name}")
|
||||
self.is_running = True
|
||||
|
||||
def track_age(self):
|
||||
if not self.track:
|
||||
|
|
@ -186,7 +113,7 @@ class Scenario(PrioritySlotItem):
|
|||
if self.take_over_at:
|
||||
return
|
||||
|
||||
self.take_over_at = time.perf_counter()
|
||||
self.take_over_at = time.time()
|
||||
|
||||
def taken_over(self):
|
||||
self.is_running = False
|
||||
|
|
@ -194,19 +121,19 @@ class Scenario(PrioritySlotItem):
|
|||
|
||||
def takenover_for(self):
|
||||
if self.take_over_at:
|
||||
return time.perf_counter() - self.take_over_at
|
||||
return time.time() - self.take_over_at
|
||||
return None
|
||||
|
||||
def takeover_factor(self):
|
||||
l = self.takenover_for()
|
||||
if not l:
|
||||
return 0
|
||||
return l/self.TAKEOVER_FADEOUT
|
||||
return l/TAKEOVER_FADEOUT
|
||||
|
||||
|
||||
def lost_for(self):
|
||||
if self.scene is ScenarioScene.LOST:
|
||||
return time.perf_counter() - self.state_change_at
|
||||
return time.time() - self.state_change_at
|
||||
return None
|
||||
|
||||
def lost_factor(self):
|
||||
|
|
@ -216,7 +143,7 @@ class Scenario(PrioritySlotItem):
|
|||
return l/LOST_FADEOUT
|
||||
|
||||
def anomaly_factor(self):
|
||||
return calc_anomaly(self.prediction_diffs)
|
||||
return calc_anomaly(self.prediction_diffs, 10)
|
||||
|
||||
def deactivate(self):
|
||||
self.take_over_at = None
|
||||
|
|
@ -229,12 +156,11 @@ class Scenario(PrioritySlotItem):
|
|||
|
||||
def set_scene(self, scene: ScenarioScene):
|
||||
if self.scene is scene:
|
||||
return False
|
||||
return
|
||||
|
||||
logger.info(f"Changing scene for {self.track_id}: {self.scene.name} -> {scene.name}")
|
||||
self.scene = scene
|
||||
self.state_change_at = time.perf_counter()
|
||||
return True
|
||||
self.state_change_at = time.time()
|
||||
|
||||
def update_state(self):
|
||||
self.check_lost() or self.check_loitering() or self.check_track()
|
||||
|
|
@ -248,31 +174,25 @@ class Scenario(PrioritySlotItem):
|
|||
|
||||
def check_loitering(self):
|
||||
scores = [s for s in calculate_loitering_scores(self.track, LOITERING_DURATION_TO_LINGER, LOITERING_LINGER_FACTOR, LOITERING_VELOCITY_TRESHOLD/TRACK_ASSUMED_FPS, 150)]
|
||||
|
||||
if not len(scores):
|
||||
logger.warning(f"No loitering score for {self.track_id}")
|
||||
return False
|
||||
|
||||
self.loitering_factor = scores[-1]
|
||||
if self.loitering_factor > .99:
|
||||
if scores[-1] > .99:
|
||||
self.set_scene(ScenarioScene.LOITERING)
|
||||
return True
|
||||
return False
|
||||
|
||||
def check_track(self):
|
||||
predictions = len(self.prediction_tracks)
|
||||
if predictions and self.running_for() < 20:
|
||||
self.set_scene(ScenarioScene.PREDICTION_AVAILABLE)
|
||||
if predictions == 1:
|
||||
self.set_scene(ScenarioScene.FIRST_PREDICTION)
|
||||
return True
|
||||
if predictions and self.running_for() > 60 * 5:
|
||||
if predictions > 10:
|
||||
self.set_scene(ScenarioScene.PLAY)
|
||||
return True
|
||||
if predictions:
|
||||
self.set_scene(ScenarioScene.UPDATED_PREDICTION)
|
||||
self.set_scene(ScenarioScene.CORRECTED_PREDICTION)
|
||||
return True
|
||||
if self.track:
|
||||
if len(self.track.projected_history) > TRACK_ASSUMED_FPS * 2:
|
||||
self.set_scene(ScenarioScene.TRACKED)
|
||||
if len(self.track.projected_history) > TRACK_ASSUMED_FPS * 3:
|
||||
self.set_scene(ScenarioScene.SUBSTANTIAL)
|
||||
else:
|
||||
self.set_scene(ScenarioScene.DETECTED)
|
||||
return True
|
||||
|
|
@ -306,8 +226,8 @@ class Scenario(PrioritySlotItem):
|
|||
# in case of the unlikely event that prediction was received sooner
|
||||
self.recv_track(track)
|
||||
|
||||
interval = self.stage.get_setting('stage.prediction_interval', PREDICTION_INTERVAL)
|
||||
if interval is not None and len(self.prediction_tracks) and (track.frame_index - self.prediction_tracks[-1].frame_index) < interval:
|
||||
|
||||
if PREDICTION_INTERVAL is not None and len(self.prediction_tracks) and (track.frame_index - self.prediction_tracks[-1].frame_index) < PREDICTION_INTERVAL:
|
||||
# just drop tracks if the predictions come to quick
|
||||
return
|
||||
|
||||
|
|
@ -327,19 +247,6 @@ class Scenario(PrioritySlotItem):
|
|||
|
||||
|
||||
|
||||
def build_line_others():
|
||||
others_color = SrgbaColor(1,1,0,1)
|
||||
line_others = LineAnimationStack(StaticLine([], others_color))
|
||||
# line_others.add(SegmentLine(line_others.tail, duration=3, anim_f=partial(SegmentLine.anim_grow, in_and_out=True, max_len=5)))
|
||||
line_others.add(SimplifyLine(line_others.tail, 0.001)) # Simplify before effects, so they don't distort
|
||||
line_others.add(CropAnimationLine(line_others.tail, 70, assume_fps=TRACK_ASSUMED_FPS*2)) # speed up
|
||||
line_others.add(NoiseLine(line_others.tail, amplitude=0, t_factor=.3))
|
||||
# line_others.add(DashedLine(line_others.tail, t_factor=4, loop_offset=True))
|
||||
# line_others.get(DashedLine).skip = True
|
||||
line_others.add(FadedEndsLine(line_others.tail, 30, 30))
|
||||
line_others.add(FadeOutLine(line_others.tail))
|
||||
line_others.get(FadeOutLine).set_alpha(0)
|
||||
return line_others
|
||||
|
||||
class DrawnScenario(Scenario):
|
||||
"""
|
||||
|
|
@ -348,49 +255,39 @@ class DrawnScenario(Scenario):
|
|||
This distinction is only for ordering the code
|
||||
"""
|
||||
|
||||
MAX_HISTORY = 130 # points of history of trajectory to display (preventing too long lines)
|
||||
MAX_HISTORY = 300 # 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, track_id, stage: Stage):
|
||||
super().__init__(track_id, stage)
|
||||
def __init__(self, track_id):
|
||||
super().__init__(track_id)
|
||||
self.last_update_t = time.perf_counter()
|
||||
self.active_ptrack: Optional[ProjectedTrack] = None
|
||||
|
||||
history_color = SrgbaColor(1.,0.,1.,1.)
|
||||
history = StaticLine([], history_color)
|
||||
self.line_history = LineAnimationStack(history)
|
||||
self.line_history.add(AppendableLineAnimator(self.line_history.tail, draw_decay_speed=120, transition_in_on_init=False))
|
||||
self.line_history.add(AppendableLineAnimator(self.line_history.tail, draw_decay_speed=25))
|
||||
|
||||
self.line_history.add(CropLine(self.line_history.tail, self.MAX_HISTORY))
|
||||
self.line_history.add(SimplifyLine(self.line_history.tail, 0.002)) # Simplify before effects, so they don't distort
|
||||
self.line_history.add(FadedTailLine(self.line_history.tail, TRACK_FADE_AFTER_DURATION * TRACK_ASSUMED_FPS, TRACK_END_FADE))
|
||||
self.line_history.add(NoiseLine(self.line_history.tail, amplitude=0, t_factor=.3))
|
||||
self.line_history.add(FadeOutJitterLine(self.line_history.tail, frequency=5, t_factor=.5))
|
||||
|
||||
self.prediction_color = SrgbaColor(0,1,0,1)
|
||||
self.line_prediction = LineAnimationStack(StaticLine([], self.prediction_color))
|
||||
|
||||
self.line_prediction.add(CropLine(self.line_prediction.tail, start_offset=0))
|
||||
self.line_prediction.add(StartFromClosestPoint(self.line_prediction.tail))
|
||||
self.line_prediction.get(StartFromClosestPoint).skip=True
|
||||
self.line_prediction.add(RotatingLine(self.line_prediction.tail, decay_speed=16))
|
||||
self.line_prediction.get(RotatingLine).skip = False
|
||||
self.line_prediction.add(SegmentLine(self.line_prediction.tail, duration=7 / 3, anim_f=SegmentLine.anim_follow_in_front))
|
||||
self.line_prediction.get(SegmentLine).skip = False
|
||||
self.line_prediction.add(SimplifyLine(self.line_prediction.tail, 0.002)) # Simplify before effects, so they don't distort
|
||||
GAP_DURATION = 5
|
||||
def dash_len(dt, t):
|
||||
t=min(1, t/GAP_DURATION)
|
||||
return lerp(.99, .6, t)
|
||||
def gap_len(dt, t):
|
||||
t=min(1, t/GAP_DURATION)
|
||||
return lerp(0.01, .9, t)
|
||||
|
||||
self.line_prediction.add(DashedLine(self.line_prediction.tail, dash_len=dash_len, gap_len=gap_len, t_factor=2, loop_offset=True))
|
||||
self.line_prediction.add(SegmentLine(self.line_prediction.tail, duration=.5))
|
||||
self.line_prediction.add(DashedLine(self.line_prediction.tail, t_factor=4, loop_offset=True))
|
||||
self.line_prediction.get(DashedLine).skip = True
|
||||
self.line_prediction.add(FadeOutLine(self.line_prediction.tail))
|
||||
|
||||
# when rendering tracks from others similar/close to the current one
|
||||
self.line_others = build_line_others()
|
||||
self.others_color = SrgbaColor(1,1,0,1)
|
||||
self.line_others = LineAnimationStack(StaticLine([], self.others_color))
|
||||
self.line_others.add(SegmentLine(self.line_others.tail, duration=3, anim_f=partial(SegmentLine.anim_grow, in_and_out=True, max_len=8)))
|
||||
# self.line_others.add(DashedLine(self.line_others.tail, t_factor=4, loop_offset=True))
|
||||
# self.line_others.get(DashedLine).skip = True
|
||||
self.line_others.add(FadeOutLine(self.line_others.tail))
|
||||
self.line_others.get(FadeOutLine).set_alpha(0)
|
||||
|
||||
self.tracks_to_self: Optional[Generator] = None
|
||||
self.tracks_to_self_pos = None
|
||||
|
|
@ -398,83 +295,53 @@ class DrawnScenario(Scenario):
|
|||
# self.line_prediction_drawn = self.line_prediction_faded
|
||||
|
||||
|
||||
def update(self):
|
||||
def update(self, stage: Stage):
|
||||
super().update()
|
||||
if self.track:
|
||||
self.line_history.root.points = self.track.projected_history
|
||||
|
||||
lost_factor = self.lost_factor() # fade out when lost
|
||||
start_factor = 0#1 - min(1, self.running_for()) # fade in when starting
|
||||
# print(start_factor)
|
||||
self.line_history.get(FadeOutJitterLine).set_alpha(1- lost_factor - start_factor)
|
||||
self.line_prediction.get(FadeOutLine).set_alpha(1-lost_factor)
|
||||
self.line_history.get(NoiseLine).amplitude = lost_factor * 1.8
|
||||
|
||||
if len(self.prediction_tracks):
|
||||
# now_p = np.array(self.line_history.root.points[-1])
|
||||
# prev_p = np.array(self.line_history.root.points[-1 * min(4, len(self.line_history.root.points))])
|
||||
# diff = now_p - prev_p
|
||||
self.line_prediction.get(StartFromClosestPoint).set_point(self.line_history.root.points[-1])
|
||||
# print("set origin", self.line_history.root.points[-1])
|
||||
|
||||
|
||||
# TODO: only when animation is ready for it? or collect lines
|
||||
if self.is_running:
|
||||
if not self.active_ptrack:
|
||||
# draw the first prediction
|
||||
self.active_ptrack = self.prediction_tracks[-1]
|
||||
self.line_prediction.root.points = self.active_ptrack._track.predictions[0]
|
||||
|
||||
self.line_prediction.start() # reset positions
|
||||
|
||||
elif self.active_ptrack._track.updated_at < self.prediction_tracks[-1]._track.updated_at:
|
||||
# stale prediction
|
||||
# switch only if drawing animation is ready
|
||||
# if self.line_prediction.is_ready():
|
||||
if self.line_prediction.is_ready():
|
||||
self.active_ptrack = self.prediction_tracks[-1]
|
||||
self.line_prediction.root.points = self.active_ptrack._track.predictions[0]
|
||||
if self.line_prediction.is_ready() and self.line_prediction.get(DashedLine).skip == True:
|
||||
self.line_prediction.get(SegmentLine).skip = True
|
||||
self.line_prediction.get(DashedLine).skip = False
|
||||
|
||||
self.line_prediction.get(SegmentLine).anim_f = partial(SegmentLine.anim_arrive, length=.3)
|
||||
self.line_prediction.get(SegmentLine).duration = .5
|
||||
self.line_prediction.get(DashedLine).skip = True
|
||||
# print('restart')
|
||||
self.line_prediction.start() # reset positions
|
||||
# print(self.line_prediction.get(SegmentLine).running_for())
|
||||
else:
|
||||
if self.line_prediction.is_ready():
|
||||
# little hack: check is dashedline skips, to only run this once per animation:
|
||||
if self.line_prediction.get(DashedLine).skip:
|
||||
# no new yet, but ready with anim, start stage 2
|
||||
self.line_prediction.get(SegmentLine).anim_f = partial(SegmentLine.anim_grow)
|
||||
self.line_prediction.get(SegmentLine).duration = 1
|
||||
# self.line_prediction.get(SegmentLine).skip = True
|
||||
self.line_prediction.get(DashedLine).skip = False
|
||||
self.line_prediction.start()
|
||||
elif self.line_prediction.get(SegmentLine).duration != 2: # hack to only play once
|
||||
self.line_prediction.get(SegmentLine).anim_f = partial(SegmentLine.anim_grow, reverse=True)
|
||||
self.line_prediction.get(SegmentLine).duration = 2
|
||||
self.line_prediction.get(SegmentLine).start()
|
||||
|
||||
# self.line_prediction.get(SegmentLine).anim_f = partial(SegmentLine.anim_arrive, length=.3)
|
||||
# self.line_prediction.get(SegmentLine).duration = .5
|
||||
# self.line_prediction.get(DashedLine).skip = True
|
||||
# # print('restart')
|
||||
# self.line_prediction.start() # reset positions
|
||||
# # print(self.line_prediction.get(SegmentLine).running_for())
|
||||
# else:
|
||||
# if self.line_prediction.is_ready():
|
||||
# # little hack: check is dashedline skips, to only run this once per animation:
|
||||
# if self.line_prediction.get(DashedLine).skip:
|
||||
# # no new yet, but ready with anim, start stage 2
|
||||
# self.line_prediction.get(SegmentLine).anim_f = partial(SegmentLine.anim_grow)
|
||||
# self.line_prediction.get(SegmentLine).duration = 1
|
||||
# # self.line_prediction.get(SegmentLine).skip = True
|
||||
# self.line_prediction.get(DashedLine).skip = False
|
||||
# self.line_prediction.start()
|
||||
# elif self.line_prediction.get(SegmentLine).duration != 2: # hack to only play once
|
||||
# self.line_prediction.get(SegmentLine).anim_f = partial(SegmentLine.anim_grow, reverse=True)
|
||||
# self.line_prediction.get(SegmentLine).duration = 2
|
||||
# self.line_prediction.get(SegmentLine).start()
|
||||
|
||||
if self.active_ptrack:
|
||||
# TODO: this should crop by distance/lenght
|
||||
self.line_prediction.get(CropLine).start_offset = self.track._track.frame_index - self.active_ptrack._track.frame_index
|
||||
|
||||
|
||||
|
||||
# self.line_prediction_dashed.set_offset_t(self.active_ptrack._track.track_update_dt() * 4)
|
||||
|
||||
self.line_prediction.root.points = self.active_ptrack._track.predictions[0]
|
||||
|
||||
|
||||
# special case: LOITERING
|
||||
if self.stage.get_setting('stage.loitering_animation', True) and self.scene is ScenarioScene.LOITERING: # or self.state_change_at:
|
||||
if self.scene is ScenarioScene.LOITERING or self.state_change_at:
|
||||
# logger.info('loitering')
|
||||
transition = min(1, (time.perf_counter() - self.state_change_at)/1.4)
|
||||
# print('loitering factor', transition)
|
||||
transition = min(1, (time.time() - self.state_change_at)/1.4)
|
||||
|
||||
|
||||
# TODO: transition fade, using to_alpha(), so it can fade back in again:
|
||||
|
|
@ -488,27 +355,21 @@ class DrawnScenario(Scenario):
|
|||
# print(transition > .999, self.is_running, current_position_rounded, time_diff)
|
||||
|
||||
if transition > .999 and self.is_running and not all(self.tracks_to_self_pos == current_position_rounded) and time_diff > 5: # only do these expensive calls when running
|
||||
logger.info(f"Fetch similar tracks for {self.track_id}")
|
||||
t = time.perf_counter()
|
||||
self.tracks_to_self_pos = current_position_rounded
|
||||
self.tracks_to_self_fetched_at = time.perf_counter()
|
||||
|
||||
# fetch lines nearby
|
||||
track_ids = self.stage.history.get_nearest_tracks(current_position, 30)
|
||||
track_ids = stage.history.get_nearest_tracks(current_position, 15)
|
||||
self.track_ids_to_self = iter(track_ids)
|
||||
self.tracks_to_self = self.stage.history.ids_as_trajectory(track_ids)
|
||||
self.tracks_to_self = stage.history.ids_as_trajectory(track_ids)
|
||||
|
||||
self.stage.logger.info(f"Fetched similar tracks for {self.track_id}. (Took {time.perf_counter() - self.tracks_to_self_fetched_at}s)")
|
||||
print(time.perf_counter() - t, "fetch delya")
|
||||
|
||||
# if self.tracks_to_self and not len(self.line_others.root.points):
|
||||
if self.tracks_to_self and not self.line_others.is_running():
|
||||
try:
|
||||
current_history = next(self.tracks_to_self)
|
||||
if self.tracks_to_self and self.line_others.is_ready():
|
||||
current_history_id = next(self.track_ids_to_self)
|
||||
self.line_others.get(CropAnimationLine).assume_fps = min(
|
||||
self.line_others.get(CropAnimationLine).assume_fps + TRACK_ASSUMED_FPS*1.5 , # faster each time
|
||||
TRACK_ASSUMED_FPS * 6 # capped at 6x
|
||||
)
|
||||
|
||||
self.line_others.get(NoiseLine).amplitude = .05
|
||||
current_history = next(self.tracks_to_self)
|
||||
|
||||
logger.info(f"play history item: {current_history_id}")
|
||||
self.line_others.get(FadeOutLine).set_alpha(1)
|
||||
|
|
@ -516,17 +377,10 @@ class DrawnScenario(Scenario):
|
|||
self.line_others.root.points = current_history
|
||||
# print(self.line_others.root.points)
|
||||
self.line_others.start()
|
||||
except StopIteration as e:
|
||||
pass
|
||||
# logger.info("Exhausted similar tracks?")
|
||||
|
||||
else:
|
||||
# reset loitering values
|
||||
self.line_others.get(CropAnimationLine).assume_fps = TRACK_ASSUMED_FPS*2
|
||||
self.line_others.get(NoiseLine).amplitude = 0
|
||||
|
||||
# special case: PLAY
|
||||
if self.scene is ScenarioScene.PLAY:
|
||||
elif self.scene is ScenarioScene.PLAY:
|
||||
pass
|
||||
# if self.scene is ScenarioScene.CORRECTED_PREDICTION:
|
||||
# self.line_prediction.get(DashedLine).skip = False
|
||||
|
|
@ -536,174 +390,48 @@ class DrawnScenario(Scenario):
|
|||
def to_renderable_lines(self, dt: DeltaT) -> RenderableLines:
|
||||
# each scene is handled differently:
|
||||
|
||||
t1 = time.perf_counter()
|
||||
|
||||
# 1) history, fade out when lost
|
||||
# self.line_history.get(StaticLine).color = SrgbaColor(1, 0, 1-self.anomaly_factor(), 1)
|
||||
|
||||
|
||||
self.line_history.get(FadeOutJitterLine).set_alpha(1-self.lost_factor())
|
||||
self.line_prediction.get(FadeOutLine).set_alpha(1-self.lost_factor())
|
||||
self.line_history.get(NoiseLine).amplitude = self.lost_factor()
|
||||
|
||||
# fade out history after max duration, given in frames
|
||||
track_age_in_frames = self.track_age() * TRACK_ASSUMED_FPS
|
||||
self.line_history.get(FadedTailLine).set_frame_offset(track_age_in_frames)
|
||||
|
||||
t2 = time.perf_counter()
|
||||
|
||||
|
||||
# 2) also fade-out when moving into loitering mode.
|
||||
# when fading out is done, start drawing historical data
|
||||
|
||||
history_line = self.line_history.as_renderable_line(dt)
|
||||
t3 = time.perf_counter()
|
||||
prediction_line = self.line_prediction.as_renderable_line(dt)
|
||||
t4 = time.perf_counter()
|
||||
others_line = self.line_others.as_renderable_line(dt)
|
||||
t5 = time.perf_counter()
|
||||
|
||||
# print(history_line)
|
||||
# print(self.track_id, len(self.line_history.points), len(history_line))
|
||||
|
||||
timings = (t5-t4, t4-t3, t3-t2, t2-t1)
|
||||
|
||||
return RenderableLines([
|
||||
history_line,
|
||||
prediction_line,
|
||||
others_line
|
||||
]), timings
|
||||
|
||||
def set_scene(self, scene):
|
||||
"""Create log message for the auxilary interface
|
||||
"""
|
||||
original = self.scene.name
|
||||
changed = super().set_scene(scene)
|
||||
if changed:
|
||||
try:
|
||||
self.stage.log_sock.send_string(f"Visitor {self.track_id}: {original} -> {self.scene.name}", zmq.NOBLOCK)
|
||||
except Exception as e:
|
||||
logger.warning("Not sent the scene change message, broken socket?")
|
||||
return changed
|
||||
|
||||
class NoTracksScenario(PrioritySlotItem):
|
||||
TAKEOVER_FADEOUT = 1 # override default to be faster
|
||||
|
||||
def __init__(self, stage: Stage, i: int):
|
||||
super().__init__(f"screensaver_{i}")
|
||||
self.stage = stage
|
||||
self.line = build_line_others()
|
||||
|
||||
def get_priority(self):
|
||||
# super low priority
|
||||
return (-1, -1)
|
||||
|
||||
def can_be_taken_over(self):
|
||||
return True
|
||||
|
||||
def get_state_name(self):
|
||||
return "previewing"
|
||||
|
||||
def update(self, stage: Stage):
|
||||
pass
|
||||
|
||||
def to_renderable_lines(self, dt: DeltaT):
|
||||
timings = []
|
||||
lines = RenderableLines([], CoordinateSpace.WORLD)
|
||||
if not self.line.is_running():
|
||||
track_id = random.choice(list(self.stage.history.state.tracks.keys()))
|
||||
# print('track_id', track_id)
|
||||
positions = self.stage.history.state.track_histories[track_id]
|
||||
self.line.root.points = positions
|
||||
self.line.start()
|
||||
|
||||
alpha = 1 - self.takeover_factor()
|
||||
self.line.get(FadeOutLine).set_alpha(alpha)
|
||||
|
||||
lines.lines.append(
|
||||
self.line.as_renderable_line(dt)
|
||||
)
|
||||
|
||||
|
||||
return lines, timings
|
||||
|
||||
|
||||
class DebugDrawer():
|
||||
def __init__(self, stage: Stage):
|
||||
self.stage = stage
|
||||
|
||||
def positions_to_renderable_lines(self, dt: DeltaT):
|
||||
lines = RenderableLines([], CoordinateSpace.WORLD)
|
||||
past_color = SrgbaColor(1,0,1,1)
|
||||
current_color = SrgbaColor(1,0,0,.6)
|
||||
for scenario in self.stage.scenarios.values():
|
||||
# lines.append(StaticLine(scenario.track.projected_history, past_color).as_renderable_line(dt).as_simplified(factor=.005))
|
||||
center = scenario.track.projected_history[-1]
|
||||
|
||||
lines.append(StaticLine([[center[0], center[1]-.2], [center[0], center[1]+.2]], current_color).as_renderable_line(dt))
|
||||
lines.append(StaticLine([[center[0]-.2, center[1]], [center[0]+.2, center[1]]], current_color).as_renderable_line(dt))
|
||||
return lines
|
||||
|
||||
def predictions_to_renderable_lines(self, dt: DeltaT):
|
||||
lines = RenderableLines([], CoordinateSpace.WORLD)
|
||||
future_color = SrgbaColor(0,1,0,.6)
|
||||
for scenario in self.stage.scenarios.values():
|
||||
# lines.append(StaticLine(scenario.track.projected_history, past_color).as_renderable_line(dt).as_simplified(factor=.005))
|
||||
if scenario.active_ptrack:
|
||||
lines.append(StaticLine(scenario.active_ptrack._track.predictions[0], future_color).as_renderable_line(dt))
|
||||
return lines
|
||||
|
||||
|
||||
class DatasetDrawer():
|
||||
def __init__(self, stage: Stage):
|
||||
self.stage = stage
|
||||
|
||||
line_color = SrgbaColor(0,1,1,1)
|
||||
self.track_line = LineAnimationStack(StaticLine([], line_color))
|
||||
# self.track_line.add(SimplifyLine(self.track_line.tail, 0.004)) # Simplify before cropping, to get less noodling
|
||||
self.track_line.add(SimplifyLine(self.track_line.tail, 0.002)) # no laser in dortmund
|
||||
self.track_line.add(CropAnimationLine(self.track_line.tail, 50, assume_fps=TRACK_ASSUMED_FPS*20)) # speed up
|
||||
|
||||
# self.track_line.add(DashedLine(self.track_line.tail, t_factor=4, loop_offset=True))
|
||||
# self.track_line.get(DashedLine).skip = True
|
||||
# self.track_line.add(FadedEndsLine(self.track_line.tail, 10, 10))
|
||||
self.track_line.add(FadeOutJitterLine(self.track_line.tail, t_factor=3))
|
||||
# self.track_line.add(FadeOutLine(self.track_line.tail))
|
||||
self.track_line.get(FadeOutJitterLine).set_alpha(np.random.random()*.3+.7)
|
||||
|
||||
|
||||
def to_renderable_lines(self, dt: DeltaT):
|
||||
lines = RenderableLines([], CoordinateSpace.WORLD)
|
||||
if not self.track_line.is_running():
|
||||
# print('update')
|
||||
track_id = random.choice(list(self.stage.history.state.tracks.keys()))
|
||||
# print('track_id', track_id)
|
||||
positions = self.stage.history.state.track_histories[track_id]
|
||||
self.track_line.root.points = positions
|
||||
self.track_line.start()
|
||||
# else:
|
||||
# print('-')
|
||||
|
||||
lines.lines.append(
|
||||
self.track_line.as_renderable_line(dt)
|
||||
)
|
||||
# print(lines)
|
||||
|
||||
return lines
|
||||
|
||||
|
||||
])
|
||||
|
||||
|
||||
class Stage(Node):
|
||||
|
||||
FALLBACK_FPS = 30 # we render to lasers, no need to go faster!
|
||||
FPS = 60
|
||||
|
||||
def setup(self):
|
||||
self.active_scenarios: List[DrawnScenario] = [] # List of currently running Scenario instances
|
||||
|
||||
|
||||
self.scenarios: Dict[str, DrawnScenario] = DefaultDictKeyed(lambda key: DrawnScenario(key, self))
|
||||
self.scenarios: Dict[str, DrawnScenario] = DefaultDictKeyed(lambda key: DrawnScenario(key))
|
||||
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.detection_sock = self.sub(self.config.zmq_detection_addr)
|
||||
self.stage_sock = self.pub(self.config.zmq_stage_addr)
|
||||
self.log_sock = self.push(self.config.zmq_log_addr)
|
||||
# self.stage_py_sock = self.pub(self.config.zmq_stage_py_addr)
|
||||
self.counter = CounterSender()
|
||||
|
||||
|
||||
|
|
@ -713,17 +441,11 @@ class Stage(Node):
|
|||
|
||||
self.history = TrackHistory(self.config.tracker_output_dir, self.config.camera, self.config.cache_path)
|
||||
|
||||
self.auxilary = DatasetDrawer(self)
|
||||
self.debug_drawer = DebugDrawer(self)
|
||||
|
||||
# 'screensavers'
|
||||
self.notrack_scenarios = [] #[NoTracksScenario(self, i) for i in range(self.config.max_active_scenarios)]
|
||||
|
||||
|
||||
|
||||
def run(self):
|
||||
while self.run_loop_capped_fps(self.get_setting('stage.fps', self.FALLBACK_FPS), warn_below_fps=10):
|
||||
dt = max(1/ self.get_setting('stage.fps', self.FALLBACK_FPS), self.dt_since_last_tick) # never dt of 0
|
||||
while self.run_loop_capped_fps(self.FPS, warn_below_fps=10):
|
||||
dt = max(1/ self.FPS, self.dt_since_last_tick) # never dt of 0
|
||||
|
||||
# t1 = time.perf_counter()
|
||||
self.loop_receive()
|
||||
|
|
@ -761,7 +483,7 @@ class Stage(Node):
|
|||
"""Update active scenarios and handle pauses/completions."""
|
||||
# 1) process timestep for all scenarios
|
||||
for s in self.scenarios.values():
|
||||
s.update()
|
||||
s.update(self)
|
||||
|
||||
|
||||
# 2) Remove stale tracks and take-overs
|
||||
|
|
@ -779,9 +501,9 @@ class Stage(Node):
|
|||
|
||||
|
||||
# 3) determine set of pending scenarios (all except running)
|
||||
pending_scenarios = [s for s in list(self.scenarios.values()) + self.notrack_scenarios if s not in self.active_scenarios]
|
||||
pending_scenarios = [s for s in self.scenarios.values() if s not in self.active_scenarios]
|
||||
# ... highest priority first
|
||||
pending_scenarios.sort(key=lambda s: s.get_priority(), reverse=True)
|
||||
pending_scenarios.sort(key=lambda s: s.scene.value.priority, reverse=True)
|
||||
|
||||
# 4) check if there's a slot free:
|
||||
while len(self.active_scenarios) < self.config.max_active_scenarios and len(pending_scenarios):
|
||||
|
|
@ -792,15 +514,15 @@ class Stage(Node):
|
|||
# 5) Takeover Logic: If no space, try to replace a lower-priority active scenario
|
||||
# which is in a scene in which takeover is possible
|
||||
eligible_active_scenarios = [
|
||||
s for s in self.active_scenarios if s.can_be_taken_over()
|
||||
s for s in self.active_scenarios if s.scene.value.takeover_possible
|
||||
]
|
||||
eligible_active_scenarios.sort(key=lambda s: s.get_priority())
|
||||
eligible_active_scenarios.sort(key=lambda s: s.scene.value.priority)
|
||||
|
||||
if eligible_active_scenarios and pending_scenarios:
|
||||
lowest_priority_active = eligible_active_scenarios[0]
|
||||
highest_priority_waiting = pending_scenarios[0]
|
||||
|
||||
if highest_priority_waiting.get_priority() > lowest_priority_active.get_priority():
|
||||
if highest_priority_waiting.scene.value.priority > lowest_priority_active.scene.value.priority:
|
||||
# Takeover! Stop the active scenario
|
||||
# will be cleaned up in update() loop after animation finishes
|
||||
# automatically triggering the start of the highest priority scene
|
||||
|
|
@ -812,61 +534,21 @@ class Stage(Node):
|
|||
lines = RenderableLines([])
|
||||
|
||||
# TODO: sometimes very slow!
|
||||
t1 = time.perf_counter()
|
||||
training_lines = self.auxilary.to_renderable_lines(dt)
|
||||
t2 = time.perf_counter()
|
||||
active_positions = self.debug_drawer.positions_to_renderable_lines(dt)
|
||||
all_predictions = self.debug_drawer.predictions_to_renderable_lines(dt)
|
||||
|
||||
t2b = time.perf_counter()
|
||||
|
||||
timings = []
|
||||
for scenario in self.active_scenarios:
|
||||
scenario_lines, timing = scenario.to_renderable_lines(dt)
|
||||
lines.append_lines(scenario_lines)
|
||||
timings.append(timing)
|
||||
if not len(self.active_scenarios):
|
||||
lines = training_lines
|
||||
lines.append_lines(scenario.to_renderable_lines(dt))
|
||||
|
||||
t2c = time.perf_counter()
|
||||
# rl_scenario = lines.as_simplified(SimplifyMethod.RDP, .003) # or segmentise (see shapely)
|
||||
# rl_training = training_lines.as_simplified(SimplifyMethod.RDP, .003) # or segmentise (see shapely)
|
||||
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", lines.point_count())
|
||||
t3 = time.perf_counter()
|
||||
|
||||
self.counter.set("stage.points_orig", lines.point_count())
|
||||
self.counter.set("stage.points", rl.point_count())
|
||||
|
||||
layers: RenderableLayers = {
|
||||
1: lines,
|
||||
2: self.debug_lines,
|
||||
3: training_lines,
|
||||
4: active_positions,
|
||||
5: all_predictions,
|
||||
}
|
||||
|
||||
t4 = time.perf_counter()
|
||||
|
||||
# msg = json.dumps(layers, cls=DataclassJSONEncoder).encode("utf8")
|
||||
msg = layers_to_message(layers)
|
||||
|
||||
t5 = time.perf_counter()
|
||||
self.stage_sock.send(msg)
|
||||
# self.stage_sock.send_pyobj(layers)
|
||||
|
||||
# self.stage_sock.send_json(obj=layers, cls=DataclassJSONEncoder)
|
||||
|
||||
t6 = time.perf_counter()
|
||||
|
||||
t = (t2-t1, t2b-t2, t2c-t2b, t3-t2c, t2b-t2, t4-t3, t5-t4, t6-t5)
|
||||
if sum(t) > .1:
|
||||
print(t)
|
||||
print(len(lines.lines))
|
||||
print(lines.point_count())
|
||||
print(len(msg))
|
||||
print('scenario timings:', timings)
|
||||
# print(msg)
|
||||
# exit()
|
||||
self.stage_sock.send_json(obj=layers, cls=DataclassJSONEncoder)
|
||||
|
||||
|
||||
|
||||
|
|
@ -885,30 +567,14 @@ class Stage(Node):
|
|||
help='Manually specity communication addr for the prediction messages',
|
||||
type=str,
|
||||
default="ipc:///tmp/feeds_preds")
|
||||
argparser.add_argument('--zmq-detection-addr',
|
||||
help='Manually specity communication addr for the detection messages',
|
||||
type=str,
|
||||
default="ipc:///tmp/feeds_dets")
|
||||
argparser.add_argument('--zmq-stage-addr',
|
||||
help='Manually specity communication addr for the stage messages (the rendered lines)',
|
||||
type=str,
|
||||
default="tcp://0.0.0.0:99174")
|
||||
argparser.add_argument('--zmq-log-addr',
|
||||
help='Manually specity communication addr for the log messages',
|
||||
type=str,
|
||||
default="tcp://0.0.0.0:99188")
|
||||
argparser.add_argument('--zmq-stage-py-addr',
|
||||
help='Sometimes there is no need for protobuf',
|
||||
type=str,
|
||||
default="ipc:///tmp/feeds_stage")
|
||||
argparser.add_argument('--debug-map',
|
||||
help='specify a map (svg-file) from which to load lines which will be overlayed',
|
||||
type=str,
|
||||
default="../DATASETS/hof-lidar/map_hof.svg")
|
||||
argparser.add_argument('--cutoff-map',
|
||||
help='specify a map (svg-file) that specifies projection boundaries. In here, degrade chance to be selectede',
|
||||
type=str,
|
||||
default="../DATASETS/hof-lidar/map_hof.svg")
|
||||
default="../DATASETS/hof3/map_hof.svg")
|
||||
argparser.add_argument('--max-active-scenarios',
|
||||
help='Maximum number of active scenarios that can be drawn at once (to not overlod the laser)',
|
||||
type=int,
|
||||
|
|
|
|||
|
|
@ -1,276 +0,0 @@
|
|||
|
||||
|
||||
from argparse import ArgumentParser
|
||||
from collections import deque
|
||||
import math
|
||||
import re
|
||||
from typing import List
|
||||
import numpy as np
|
||||
import pyglet
|
||||
from torch import mul
|
||||
import zmq
|
||||
from trap.lines import RenderableLayers, message_to_layers
|
||||
from trap.node import Node
|
||||
|
||||
BG_COLOR = (0,0,255)
|
||||
class StageRenderer(Node):
|
||||
def setup(self):
|
||||
# self.prediction_sock = self.sub(self.config.zmq_prediction_addr)
|
||||
# self.tracker_sock = self.sub(self.config.zmq_trajectory_addr)
|
||||
# self.detector_sock = self.sub(self.config.zmq_detection_addr)
|
||||
# self.frame_sock = self.sub(self.config.zmq_frame_addr)
|
||||
self.stage_sock = self.sub(self.config.zmq_stage_addr)
|
||||
self.log_sock = self.pull(self.config.zmq_log_addr)
|
||||
|
||||
# setup pyglet:
|
||||
display = pyglet.display.get_display()
|
||||
screens = display.get_screens()
|
||||
|
||||
# use configured montior, fall back to whatever is available
|
||||
self.screen = sorted(screens, reverse=True, key=lambda s: s.get_monitor_name() == self.config.monitor)[0]
|
||||
|
||||
if self.screen.get_monitor_name() != self.config.monitor:
|
||||
self.logger.warning(f"Not displaying on configured monitor. {self.screen.get_monitor_name()} instead of {self.config.monitor}")
|
||||
|
||||
# print(self.screen.get_modes())
|
||||
|
||||
|
||||
config = pyglet.gl.Config(sample_buffers=1, samples=4)
|
||||
|
||||
# when screen is in portrait, window mode here expects still (larger x smaller) number.
|
||||
# self.window.get_size() will be reported properly
|
||||
wh = sorted((self.screen.width, self.screen.height), reverse=self.config.fullscreen)
|
||||
|
||||
self.window = pyglet.window.Window(width=wh[0], height=wh[1], config=config, fullscreen=self.config.fullscreen, screen=self.screen)
|
||||
self.window.set_exclusive_keyboard(True)
|
||||
self.window.set_exclusive_keyboard(False)
|
||||
self.window.set_exclusive_mouse(True)
|
||||
self.window.set_exclusive_mouse(False)
|
||||
|
||||
# self.window.set_size(1080, 1920)
|
||||
|
||||
window_size = self.window.get_size()
|
||||
|
||||
padding = 40
|
||||
|
||||
print(window_size)
|
||||
self.window.set_handler('on_draw', self.on_draw)
|
||||
# self.window.set_handler('on_close', self.on_close)
|
||||
|
||||
# pyglet.gl.glClearColor(81./255, 20/255, 46./255, 0)
|
||||
pyglet.gl.glClearColor(0/255, 0/255, 255/255, 0)
|
||||
self.fps_display = pyglet.window.FPSDisplay(window=self.window, color=(255,255,255,255))
|
||||
self.fps_display.label.x = self.window.width - 50
|
||||
self.fps_display.label.y = self.window.height - 17
|
||||
self.fps_display.label.bold = False
|
||||
self.fps_display.label.font_size = 10
|
||||
|
||||
self.current_layers: RenderableLayers = {}
|
||||
|
||||
self.lines: List[pyglet.shapes.Line] = []
|
||||
self.lines_batch = pyglet.graphics.Batch()
|
||||
self.text = pyglet.text.document.FormattedDocument("")
|
||||
self.text_batch = pyglet.graphics.Batch()
|
||||
self.text_layout = pyglet.text.layout.TextLayout(
|
||||
self.text, padding, (self.window.get_size()[0]-padding*2) // 2 - 100,
|
||||
width=self.window.get_size()[1] - 2*padding,
|
||||
height=(self.window.get_size()[0] - padding) // 2,
|
||||
multiline=True, wrap_lines=False, batch=self.text_batch)
|
||||
|
||||
max_len = 31
|
||||
self.log_msgs = deque([], maxlen=max_len)
|
||||
self.log_msgs.extend(["-"] * max_len)
|
||||
|
||||
|
||||
translate = (10,-400)
|
||||
# scale = 5
|
||||
|
||||
smallest_dimension = min(self.window.get_size())
|
||||
max_x = 16.3
|
||||
max_y = 14.3
|
||||
scale = min(smallest_dimension / max_x, smallest_dimension/max_y)
|
||||
|
||||
|
||||
self.logger.info(f"Use {scale=}")
|
||||
|
||||
|
||||
self.transform = np.array([
|
||||
[scale, 0,translate[0]],
|
||||
[0,-scale,window_size[1]],
|
||||
[0,0,1]
|
||||
])
|
||||
|
||||
self.bg_image = pyglet.image.load(self.config.floorplan)
|
||||
scale = (window_size[0] - padding*2) / (self.bg_image.width)
|
||||
print('image_scale', scale, self.bg_image.width, self.bg_image.height)
|
||||
# self.bg_image.height = int(self.bg_image.height / 3)
|
||||
# self.bg_image.width = int(self.bg_image.width / 3)
|
||||
img_y = window_size[1]-int(self.bg_image.height*scale)-padding*2
|
||||
self.bg_sprite = pyglet.sprite.Sprite(img=self.bg_image, x=padding, y=img_y)
|
||||
self.bg_sprite.scale = scale
|
||||
|
||||
|
||||
clear_area = img_y
|
||||
self.clear_transparent = pyglet.shapes.Rectangle(0, window_size[1]-clear_area, window_size[0], clear_area, color=(*BG_COLOR,255//70))
|
||||
self.clear_fully= pyglet.shapes.Rectangle(0, 0, window_size[0], window_size[1]-clear_area, color=(*BG_COLOR,255))
|
||||
|
||||
self.window.clear()
|
||||
|
||||
|
||||
def check_running(self, dt):
|
||||
if not self.run_loop():
|
||||
self.window.close()
|
||||
self.event_loop.exit()
|
||||
|
||||
def run(self):
|
||||
self.event_loop = pyglet.app.EventLoop()
|
||||
pyglet.clock.schedule_interval(self.check_running, 0.1)
|
||||
# pyglet.clock.schedule(self.receive)
|
||||
self.event_loop.run()
|
||||
|
||||
|
||||
def receive(self, dt):
|
||||
try:
|
||||
msg = self.stage_sock.recv(zmq.NOBLOCK)
|
||||
self.current_layers = message_to_layers(msg)
|
||||
self.update_lines()
|
||||
except zmq.ZMQError as e:
|
||||
# idx = frame.index if frame else "NONE"
|
||||
# logger.debug(f"reuse video frame {idx}")
|
||||
pass
|
||||
|
||||
while True:
|
||||
try:
|
||||
log_msg = self.log_sock.recv_string(zmq.NOBLOCK)
|
||||
self.log_msgs.append(log_msg)
|
||||
except zmq.ZMQError as e:
|
||||
# idx = frame.index if frame else "NONE"
|
||||
# logger.debug(f"reuse video frame {idx}")
|
||||
break
|
||||
self.update_msgs()
|
||||
|
||||
|
||||
def update_lines(self):
|
||||
"""
|
||||
Render the renderable lines of selected layers
|
||||
"""
|
||||
|
||||
additional_scale = self.get_setting('stagerenderer.scale', 1)
|
||||
dx = self.get_setting('stagerenderer.dx', 0)
|
||||
dy = self.get_setting('stagerenderer.dy', 0)
|
||||
transform = self.transform.copy()
|
||||
transform[0][0] *= additional_scale
|
||||
transform[1][1] *= additional_scale
|
||||
transform[0][2] += dx
|
||||
transform[1][2] += dy
|
||||
|
||||
i = -1
|
||||
for nr, lines in self.current_layers.items():
|
||||
|
||||
if not self.get_setting(f'stagerenderer.layer.{nr}', True):
|
||||
continue
|
||||
|
||||
|
||||
for line in lines.lines:
|
||||
for p1, p2 in zip(line.points, line.points[1:]):
|
||||
i += 1
|
||||
pp1 = np.array([p1.position[0], p1.position[1], 1])
|
||||
pp2 = np.array([p2.position[0], p2.position[1], 1])
|
||||
|
||||
pos1 = (transform@pp1)[:2].astype(int)
|
||||
pos2 = (transform@pp2)[:2].astype(int)
|
||||
|
||||
color = (p2.color.as_array()*255).astype(int)
|
||||
|
||||
if i < len(self.lines):
|
||||
shape = self.lines[i]
|
||||
shape.x = pos1[0]
|
||||
shape.y = pos1[1]
|
||||
shape.x2 = pos2[0]
|
||||
shape.y2 = pos2[1]
|
||||
shape.color = color
|
||||
else:
|
||||
self.lines.append(pyglet.shapes.Line(pos1[0], pos1[1],
|
||||
pos2[0],
|
||||
pos2[1],
|
||||
3,
|
||||
color,
|
||||
batch=self.lines_batch))
|
||||
|
||||
|
||||
too_many = len(self.lines) - 1 - i
|
||||
if too_many > 0:
|
||||
for j in reversed(range(i, i+too_many)):
|
||||
self.lines[i].delete()
|
||||
del self.lines[i]
|
||||
|
||||
|
||||
def update_msgs(self):
|
||||
text = "\n".join(self.log_msgs)
|
||||
self.text.text = text
|
||||
self.text.set_style(0, len(self.text.text), dict(
|
||||
font_name='Arial', # change to a font installed on your system
|
||||
font_size=18,
|
||||
color=(255, 255, 255, 255),
|
||||
))
|
||||
|
||||
|
||||
colorsmap = {
|
||||
'ANOMALOUS': (255, 0, 0, 255),
|
||||
'LOITERING': (255, 255, 0, 255),
|
||||
'DETECTED': (255, 0, 255, 255),
|
||||
'SUBSTANTIAL': (255, 0, 255, 255),
|
||||
'LOST': (0, 0, 0, 255),
|
||||
}
|
||||
|
||||
matchtext = "".join(self.log_msgs) # find no newlines
|
||||
for state,color in colorsmap.items():
|
||||
for match in re.finditer(state, matchtext):
|
||||
self.text.set_style(match.start(), match.end(), dict(
|
||||
color=color
|
||||
))
|
||||
|
||||
|
||||
|
||||
|
||||
def on_draw(self):
|
||||
self.receive(.1)
|
||||
# self.window.clear()
|
||||
self.clear_transparent.color = (*BG_COLOR, int(3))
|
||||
self.clear_transparent.draw()
|
||||
self.clear_fully.draw()
|
||||
self.fps_display.draw()
|
||||
|
||||
self.bg_sprite.draw()
|
||||
|
||||
self.lines_batch.draw()
|
||||
self.text_batch.draw()
|
||||
|
||||
|
||||
@classmethod
|
||||
def arg_parser(cls):
|
||||
render_parser = ArgumentParser()
|
||||
|
||||
render_parser.add_argument('--zmq-stage-addr',
|
||||
help='Manually specity communication addr for the stage messages (the rendered lines)',
|
||||
type=str,
|
||||
default="tcp://0.0.0.0:99174")
|
||||
render_parser.add_argument('--zmq-log-addr',
|
||||
help='Manually specity communication addr for the log messages',
|
||||
type=str,
|
||||
default="tcp://0.0.0.0:99188")
|
||||
|
||||
render_parser.add_argument("--fullscreen",
|
||||
help="Set Window full screen",
|
||||
action='store_true')
|
||||
|
||||
render_parser.add_argument('--floorplan',
|
||||
help='specify a map (png-file) onto which overlayed',
|
||||
type=str,
|
||||
default="SETTINGS/2025-11-dortmund/space/floorplan.png")
|
||||
render_parser.add_argument('--monitor',
|
||||
help='Specify a screen on which to output (eg. HDMI-0)',
|
||||
type=str,
|
||||
default="HDMI-0")
|
||||
return render_parser
|
||||
|
||||
|
|
@ -330,7 +330,6 @@ def track_predictions_to_lines(track: Track, camera:Camera, anim_position=.8):
|
|||
return
|
||||
|
||||
current_point = track.get_projected_history(camera=camera)[-1]
|
||||
|
||||
slide_t = min(1, max(0, inv_lerp(0, 0.8, anim_position))) # slide_position
|
||||
|
||||
lines = []
|
||||
|
|
@ -373,7 +372,6 @@ def draw_track_predictions(img: cv2.Mat, track: Track, color_index: int, camera:
|
|||
|
||||
lines = track_predictions_to_lines(track, camera, anim_position)
|
||||
|
||||
|
||||
if not lines:
|
||||
return
|
||||
|
||||
|
|
|
|||
|
|
@ -1,186 +0,0 @@
|
|||
from dataclasses import dataclass
|
||||
import logging
|
||||
from pathlib import Path
|
||||
import pickle
|
||||
from threading import Lock
|
||||
import time
|
||||
from typing import Dict, Iterable, List, Optional, Set
|
||||
|
||||
import numpy as np
|
||||
from trap.base import Camera, Track
|
||||
from trap.lines import Coordinate
|
||||
from trap.tracker import FinalDisplacementFilter, Smoother, TrackReader
|
||||
|
||||
from scipy.spatial import KDTree
|
||||
|
||||
logger = logging.getLogger('history')
|
||||
|
||||
@dataclass
|
||||
class TrackHistoryState():
|
||||
"""
|
||||
The lock of TrackHistory is not pickle-able so separate it into a separate state
|
||||
"""
|
||||
tracks: List[Track]
|
||||
track_histories: Dict[str, np.ndarray]
|
||||
indexed_track_ids: List[str]
|
||||
tree: KDTree
|
||||
|
||||
|
||||
|
||||
class TrackHistory():
|
||||
def __init__(self, path: Path, camera: Camera, cache_path: Optional[Path]):
|
||||
self.path = path
|
||||
self.camera = camera
|
||||
self.cache_path = cache_path
|
||||
self.lock = Lock()
|
||||
self.load_from_cache() or self.reload()
|
||||
|
||||
|
||||
def load_from_cache(self):
|
||||
if self.cache_path is None:
|
||||
return False
|
||||
|
||||
if self.cache_path.exists():
|
||||
logger.debug("Load history state from cache")
|
||||
with self.cache_path.open('rb') as fp:
|
||||
try:
|
||||
state = pickle.load(fp)
|
||||
if not isinstance(state, TrackHistoryState):
|
||||
raise RuntimeError("Pickled data is not a trackhistorystate")
|
||||
self.state = state
|
||||
return True
|
||||
except Exception as e:
|
||||
logger.warning(f"Cannot read cache {self.cache_path}: {e}")
|
||||
|
||||
return False
|
||||
|
||||
def build_tree(self):
|
||||
reader = TrackReader(self.path, self.camera.fps)
|
||||
logger.debug(f'loaded {len(reader)} tracks')
|
||||
|
||||
track_filter = FinalDisplacementFilter(2)
|
||||
tracks = track_filter.apply(reader, self.camera)
|
||||
logger.debug(f'after filtering left with {len(tracks)} tracks')
|
||||
|
||||
|
||||
tracks: List[Track] = [t.get_with_interpolated_history() for t in tracks]
|
||||
logger.debug(f'interpolated {len(tracks)} tracks')
|
||||
|
||||
# use convolution here, because precision does not matter and it is _way_ faster
|
||||
smoother = Smoother(convolution=True)
|
||||
tracks = [smoother.smooth_track(t) for t in tracks]
|
||||
logger.debug(f'smoothed')
|
||||
|
||||
tracks = {track.track_id: track for track in tracks}
|
||||
|
||||
|
||||
track_histories = {t.track_id: t.get_projected_history(camera=self.camera) for t in tracks.values()}
|
||||
downsampled_histories = {t_id: self.downsample_history(h) for t_id, h in track_histories.items()}
|
||||
logger.debug(f'projected to world space')
|
||||
|
||||
|
||||
# Sample data (coordinates and metadata)
|
||||
# coordinates = [(1, 2, 'Point A'), (3, 4, 'Point B'), (5, 6, 'Point C'), (7, 8, 'Point D')]
|
||||
all_points = []
|
||||
indexed_track_ids: List[str] = []
|
||||
for track_id, history in downsampled_histories.items():
|
||||
all_points.extend([
|
||||
[point[0], point[1]] for point in history
|
||||
])
|
||||
indexed_track_ids.extend([track_id] * len(history))
|
||||
|
||||
# self.flat_idx = self.flat_histories[:,2]
|
||||
|
||||
# Create the KD-Tree
|
||||
tree = KDTree(all_points)
|
||||
|
||||
logger.debug('built tree')
|
||||
return TrackHistoryState(
|
||||
tracks, track_histories, indexed_track_ids, tree
|
||||
)
|
||||
|
||||
def reload(self):
|
||||
state = self.build_tree()
|
||||
|
||||
# aquire lock as brief as possible
|
||||
with self.lock:
|
||||
self.state = state
|
||||
|
||||
|
||||
if self.cache_path:
|
||||
with self.cache_path.open('wb') as fp:
|
||||
logger.debug("Writing history to cache")
|
||||
pickle.dump(self.state, fp)
|
||||
|
||||
|
||||
|
||||
def get_nearest_tracks(self, point: Coordinate, k:int, max_r: Optional[float] = np.inf):
|
||||
with self.lock:
|
||||
distances, indexes = self.state.tree.query(point, k, distance_upper_bound=max_r)
|
||||
# filter out when there's no
|
||||
indexes = indexes[distances != np.inf]
|
||||
track_ids: Set[str] = {self.state.indexed_track_ids[idx] for idx in indexes}
|
||||
|
||||
# nearby_indexes = self.tree.query_ball_point(point, r)
|
||||
# track_ids = set([self.flat_idx[idx] for idx in nearby_indexes])
|
||||
|
||||
return track_ids
|
||||
|
||||
def ids_as_trajectory(self, track_ids: Iterable[str]):
|
||||
for track_id in track_ids:
|
||||
yield self.state.tracks[track_id].get_projected_history(camera=self.camera)
|
||||
|
||||
|
||||
|
||||
@classmethod
|
||||
def downsample_history(cls, history, cell_size=.3):
|
||||
|
||||
|
||||
if not len(history):
|
||||
return []
|
||||
|
||||
positions = np.unique(np.round(history / cell_size), axis=0) * cell_size
|
||||
|
||||
return positions
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
path = Path("EXPERIMENTS/raw/hof3/")
|
||||
logging.basicConfig(level=logging.DEBUG)
|
||||
|
||||
calibration_path = Path("../DATASETS/hof3/calibration.json")
|
||||
homography_path = Path("../DATASETS/hof3/homography.json")
|
||||
camera = Camera.from_paths(calibration_path, homography_path, 12)
|
||||
# device = device = torch.device('cuda' if torch.cuda.is_available() else 'cpu')
|
||||
|
||||
|
||||
|
||||
s = time.time()
|
||||
history = TrackHistory(path, camera, Path("/tmp/historystate_hof3.pcl"))
|
||||
dt = time.time() - s
|
||||
print(f'loaded {len(history.state.tracks)} tracks in {dt}s')
|
||||
|
||||
|
||||
track = list(history.state.tracks.values())[25]
|
||||
trajectory_crop = TrackHistory.downsample_history(history.state.track_histories[track.track_id])
|
||||
trajectory_org = track.get_projected_history(camera=camera)
|
||||
target_point = trajectory_org[len(trajectory_org)//2+90]
|
||||
|
||||
import matplotlib.pyplot as plt # Visualization
|
||||
|
||||
track_set = history.get_nearest_tracks(target_point, 10, max_r=np.inf)
|
||||
|
||||
|
||||
|
||||
plt.gca().set_aspect('equal')
|
||||
plt.scatter(trajectory_crop[:,0], trajectory_crop[:,1], c='orange')
|
||||
plt.plot(trajectory_org[:,0], trajectory_org[:,1], c='blue', alpha=1)
|
||||
plt.scatter(target_point[0], target_point[1], c='red', alpha=1)
|
||||
for track_id in track_set:
|
||||
closeby = history.state.tracks[track_id].get_projected_history(camera=camera)
|
||||
plt.plot(closeby[:,0], closeby[:,1], c='green', alpha=.1)
|
||||
|
||||
plt.show()
|
||||
|
|
@ -1,74 +0,0 @@
|
|||
# used for "Forward Referencing of type annotations"
|
||||
from __future__ import annotations
|
||||
|
||||
from argparse import ArgumentParser
|
||||
from pathlib import Path
|
||||
|
||||
import zmq
|
||||
|
||||
from trap.base import Track
|
||||
from trap.frame_emitter import Frame
|
||||
from trap.node import Node
|
||||
from trap.tracker import TrainingDataWriter, TrainingTrackWriter
|
||||
|
||||
|
||||
class TrackWriter(Node):
|
||||
def setup(self):
|
||||
self.track_sock = self.sub(self.config.zmq_lost_addr)
|
||||
self.log_sock = self.push(self.config.zmq_log_addr)
|
||||
|
||||
|
||||
def run(self):
|
||||
with TrainingTrackWriter(self.config.output_dir) as writer:
|
||||
try:
|
||||
while self.run_loop():
|
||||
zmq_ev = self.track_sock.poll(timeout=1000)
|
||||
if not zmq_ev:
|
||||
# when no data comes in, loop so that is_running is checked
|
||||
continue
|
||||
|
||||
try:
|
||||
track: Track = self.track_sock.recv_pyobj()
|
||||
|
||||
if len(track.history) < 20:
|
||||
self.logger.debug(f"ignore short track {len(track.history)}")
|
||||
continue
|
||||
|
||||
writer.add(track)
|
||||
|
||||
self.logger.info(f"Added track {track.track_id}")
|
||||
|
||||
try:
|
||||
self.log_sock.send_string(f"Added track {track.track_id} to dataset, {len(track.history)} datapoints", zmq.NOBLOCK)
|
||||
except Exception as e:
|
||||
self.logger.warning("Not sent the message, broken socket?")
|
||||
|
||||
except zmq.ZMQError as e:
|
||||
|
||||
pass
|
||||
except KeyboardInterrupt as e:
|
||||
print('stopping on interrupt')
|
||||
|
||||
self.logger.info('Stopping')
|
||||
|
||||
|
||||
|
||||
@classmethod
|
||||
def arg_parser(cls):
|
||||
argparser = ArgumentParser()
|
||||
argparser.add_argument('--zmq-log-addr',
|
||||
help='Manually specity communication addr for the log messages',
|
||||
type=str,
|
||||
default="tcp://0.0.0.0:99188")
|
||||
argparser.add_argument('--zmq-lost-addr',
|
||||
help='Manually specity communication addr for the trajectory messages',
|
||||
type=str,
|
||||
default="ipc:///tmp/feeds_lost")
|
||||
argparser.add_argument("--output-dir",
|
||||
help="Directory to save the video in",
|
||||
required=True,
|
||||
default=Path("EXPERIMENTS/raw/hof-lidar"),
|
||||
type=Path)
|
||||
return argparser
|
||||
|
||||
|
||||
182
trap/tracker.py
182
trap/tracker.py
|
|
@ -1,4 +1,3 @@
|
|||
from abc import ABC, abstractmethod
|
||||
import argparse
|
||||
import csv
|
||||
import json
|
||||
|
|
@ -119,7 +118,6 @@ class FinalDisplacementFilter(TrackFilter):
|
|||
|
||||
def filter(self, track: Track, camera: Camera):
|
||||
history = track.get_projected_history(H=None, camera=camera)
|
||||
|
||||
displacement = np.linalg.norm(history[0]-history[-1])
|
||||
return displacement > self.min_displacement
|
||||
|
||||
|
|
@ -127,36 +125,13 @@ class TrackReader:
|
|||
def __init__(self, path: Path, fps: int, include_blacklisted = False, exclude_whitelisted = False):
|
||||
self.blacklist_file = path / "blacklist.jsonl"
|
||||
self.whitelist_file = path / "whitelist.jsonl" # for skipping
|
||||
# self.tracks_file = path / "tracks.pkl"
|
||||
self.tracks_files = path.glob('tracks*.pkl')
|
||||
self.tracks_file = path / "tracks.pkl"
|
||||
|
||||
# with self.tracks_file.open('r') as fp:
|
||||
# tracks_dict: dict = json.load(fp)
|
||||
|
||||
tracks: Dict[str, Track] = {}
|
||||
for tracks_file in self.tracks_files:
|
||||
logger.info(f"Read {tracks_file}")
|
||||
with tracks_file.open('rb') as fp:
|
||||
while True:
|
||||
# multiple tracks can be pickled separately
|
||||
try:
|
||||
trackset: Dict[str, Track] = pickle.load(fp)
|
||||
for track_id, track in trackset.items():
|
||||
if len(tracks) < 1:
|
||||
max_item = 0
|
||||
else:
|
||||
max_item = max([int(t) for t in tracks.keys()])
|
||||
|
||||
if int(track.track_id) < max_item:
|
||||
track_id = str(max_item+1)
|
||||
else:
|
||||
track_id = track.track_id
|
||||
|
||||
track.track_id = track_id
|
||||
tracks[track.track_id] = track
|
||||
except EOFError:
|
||||
break
|
||||
|
||||
with self.tracks_file.open('rb') as fp:
|
||||
tracks: dict = pickle.load(fp)
|
||||
|
||||
|
||||
if self.blacklist_file.exists():
|
||||
|
|
@ -280,48 +255,6 @@ class TrainingDataWriter:
|
|||
rewrite_raw_track_files(self.path)
|
||||
|
||||
|
||||
class TrainingTrackWriter:
|
||||
"""
|
||||
Supersedes TrainingDataWriter, by writing full tracks"""
|
||||
def __init__(self, training_path: Optional[Path]):
|
||||
if training_path is None:
|
||||
self.path = None
|
||||
return
|
||||
|
||||
if not isinstance(training_path, Path):
|
||||
raise ValueError("save-for-training should be a path")
|
||||
if not training_path.exists():
|
||||
logger.info(f"Making path for training data: {training_path}")
|
||||
training_path.mkdir(parents=True, exist_ok=False)
|
||||
else:
|
||||
logger.warning(f"Path for training-data exists: {training_path}. Continuing assuming that's ok.")
|
||||
|
||||
self.path = training_path
|
||||
|
||||
def __enter__(self):
|
||||
if self.path:
|
||||
d = datetime.now().isoformat(timespec="minutes")
|
||||
self.training_fp = open(self.path / f'tracks-{d}.pcl', 'wb')
|
||||
logger.debug(f"Writing tracker data to {self.training_fp.name}")
|
||||
# following https://github.com/StanfordASL/Trajectron-plus-plus/blob/master/experiments/pedestrians/process_data.py
|
||||
# self.csv = csv.DictWriter(self.training_fp, fieldnames=FIELDNAMES, delimiter='\t', quoting=csv.QUOTE_NONE)
|
||||
self.count = 0
|
||||
return self
|
||||
|
||||
def add(self, track: Track):
|
||||
self.count += 1;
|
||||
pickle.dump(track, self.training_fp)
|
||||
|
||||
|
||||
def __exit__(self, exc_type, exc_value, exc_tb):
|
||||
# ... ignore exception (type, value, traceback)
|
||||
if not self.path:
|
||||
return
|
||||
|
||||
self.training_fp.close()
|
||||
# rewrite_raw_track_files(self.path)
|
||||
|
||||
|
||||
|
||||
def rewrite_raw_track_files(path: Path):
|
||||
source_files = list(sorted(path.glob("*.txt"))) # we loop twice, so need a list instead of generator
|
||||
|
|
@ -362,7 +295,7 @@ def rewrite_raw_track_files(path: Path):
|
|||
with file.open('w') as target_fp:
|
||||
|
||||
for i in range(line_nrs):
|
||||
line = sources.readline().rstrip()
|
||||
line = sources.readline()
|
||||
current_file = sources.current_file
|
||||
if prev_file != current_file:
|
||||
offset: int = max_track_id
|
||||
|
|
@ -842,33 +775,49 @@ def run():
|
|||
is_running.clear()
|
||||
|
||||
|
||||
class TrackPointFilter(ABC):
|
||||
@abstractmethod
|
||||
def apply(self, points: List[float]):
|
||||
pass
|
||||
class Smoother:
|
||||
|
||||
def __init__(self, window_len=6, convolution=False):
|
||||
# for some reason this smoother messes the predictions. Probably skews the points too much??
|
||||
if convolution:
|
||||
self.smoother = ConvolutionSmoother(window_len=window_len, window_type='hanning', copy=None)
|
||||
else:
|
||||
# "Unlike Kalman filtering, which focuses on predicting and updating the current state using historical measurements, Kalman smoothing enhances the accuracy of past state values"
|
||||
# see https://medium.com/@shahalkp1/kalman-smoothing-using-tsmoothie-0175260464e5
|
||||
self.smoother = KalmanSmoother(component='level_trend', component_noise={'level':0.02, 'season': .01, 'trend':0.02},n_seasons = 2, copy=None)
|
||||
|
||||
|
||||
def apply_track(self, track: Track) -> Track:
|
||||
|
||||
def smooth(self, points: List[float]):
|
||||
self.smoother.smooth(points)
|
||||
return self.smoother.smooth_data[0]
|
||||
|
||||
def smooth_track(self, track: Track) -> Track:
|
||||
ls = [d.l for d in track.history]
|
||||
ts = [d.t for d in track.history]
|
||||
ws = [d.w for d in track.history]
|
||||
hs = [d.h for d in track.history]
|
||||
ls = self.apply(ls)
|
||||
ts = self.apply(ts)
|
||||
ws = self.apply(ws)
|
||||
hs = self.apply(hs)
|
||||
self.smoother.smooth(ls)
|
||||
ls = self.smoother.smooth_data[0]
|
||||
self.smoother.smooth(ts)
|
||||
ts = self.smoother.smooth_data[0]
|
||||
self.smoother.smooth(ws)
|
||||
ws = self.smoother.smooth_data[0]
|
||||
self.smoother.smooth(hs)
|
||||
hs = self.smoother.smooth_data[0]
|
||||
new_history = [Detection(d.track_id, l, t, w, h, d.conf, d.state, d.frame_nr, d.det_class) for l, t, w, h, d in zip(ls,ts,ws,hs, track.history)]
|
||||
return track.get_with_new_history(new_history)
|
||||
# return Track(track.track_id, new_history, track.predictor_history, track.predictions, track.fps)
|
||||
|
||||
def apply_to_frame_tracks(self, frame: Frame) -> Frame:
|
||||
def smooth_frame_tracks(self, frame: Frame) -> Frame:
|
||||
new_tracks = []
|
||||
for track in frame.tracks.values():
|
||||
new_track = self.apply_track(track)
|
||||
new_track = self.smooth_track(track)
|
||||
new_tracks.append(new_track)
|
||||
frame.tracks = {t.track_id: t for t in new_tracks}
|
||||
return frame
|
||||
|
||||
def apply_to_frame_predictions(self, frame: Frame) -> Frame:
|
||||
def smooth_frame_predictions(self, frame: Frame) -> Frame:
|
||||
|
||||
for track in frame.tracks.values():
|
||||
new_predictions = []
|
||||
|
|
@ -879,69 +828,14 @@ class TrackPointFilter(ABC):
|
|||
xs = [d[0] for d in prediction]
|
||||
ys = [d[1] for d in prediction]
|
||||
|
||||
xs = self.apply(xs)
|
||||
ys = self.apply(ys)
|
||||
self.smoother.smooth(xs)
|
||||
xs = self.smoother.smooth_data[0]
|
||||
self.smoother.smooth(ys)
|
||||
ys = self.smoother.smooth_data[0]
|
||||
|
||||
filtered_prediction = [[x,y] for x, y in zip(xs, ys)]
|
||||
smooth_prediction = [[x,y] for x, y in zip(xs, ys)]
|
||||
|
||||
new_predictions.append(filtered_prediction)
|
||||
new_predictions.append(smooth_prediction)
|
||||
track.predictions = new_predictions
|
||||
|
||||
return frame
|
||||
|
||||
class Smoother(TrackPointFilter):
|
||||
|
||||
def __init__(self, window_len=6, convolution=False):
|
||||
# for some reason this smoother messes the predictions. Probably skews the points too much??
|
||||
if convolution:
|
||||
self.smoother = ConvolutionSmoother(window_len=window_len, window_type='hanning', copy=None)
|
||||
else:
|
||||
# "Unlike Kalman filtering, which focuses on predicting and updating the current state using historical measurements, Kalman smoothing enhances the accuracy of past state values"
|
||||
# see https://medium.com/@shahalkp1/kalman-smoothing-using-tsmoothie-0175260464e5
|
||||
# self.smoother = KalmanSmoother(component='level_trend', component_noise={'level':0.02, 'season': .01, 'trend':0.02},n_seasons = 2, copy=False)
|
||||
self.smoother = KalmanSmoother(component='level', component_noise={'level':0.01},observation_noise=.3, n_seasons = 0, copy=False)
|
||||
|
||||
|
||||
|
||||
|
||||
def apply(self, points: List[float]):
|
||||
self.smoother.smooth(points)
|
||||
return self.smoother.smooth_data[0]
|
||||
|
||||
|
||||
# aliases, for historic reasons
|
||||
def smooth(self, points: List[float]):
|
||||
return self.apply(points)
|
||||
|
||||
def smooth_track(self, track: Track) -> Track:
|
||||
return self.apply_track(track)
|
||||
|
||||
def smooth_frame_tracks(self, frame: Frame) -> Frame:
|
||||
return self.apply_to_frame_tracks(frame)
|
||||
|
||||
def smooth_frame_predictions(self, frame: Frame) -> Frame:
|
||||
return self.apply_to_frame_predictions(frame)
|
||||
|
||||
class Noiser(TrackPointFilter):
|
||||
|
||||
def __init__(self, amplitude=.1):
|
||||
self.amplitude = amplitude
|
||||
|
||||
|
||||
def apply(self, points: List[float]):
|
||||
return np.random.normal(points, scale=self.amplitude).tolist()
|
||||
|
||||
|
||||
class RandomOffset(TrackPointFilter):
|
||||
"""
|
||||
A bit hacky way to offset the whole track. Does x & y & w & h with the same value
|
||||
"""
|
||||
def __init__(self, amplitude=.1):
|
||||
self.amplitude = np.random.normal(scale=amplitude)
|
||||
|
||||
|
||||
|
||||
def apply(self, points: List[float]):
|
||||
return [p + self.amplitude for p in points]
|
||||
|
||||
|
||||
|
|
@ -1,5 +1,4 @@
|
|||
# lerp & inverse lerp from https://gist.github.com/laundmo/b224b1f4c8ef6ca5fe47e132c8deab56
|
||||
from collections import namedtuple
|
||||
import linecache
|
||||
import math
|
||||
import os
|
||||
|
|
@ -8,7 +7,6 @@ import tracemalloc
|
|||
from typing import Iterable
|
||||
import cv2
|
||||
import numpy as np
|
||||
import torch
|
||||
from trajectron.environment.map import GeometricMap
|
||||
|
||||
def lerp(a: float, b: float, t: float) -> float:
|
||||
|
|
@ -30,12 +28,6 @@ def inv_lerp(a: float, b: float, v: float) -> float:
|
|||
"""
|
||||
return (v - a) / (b - a)
|
||||
|
||||
def easeInOutQuad(t: float) -> float:
|
||||
"""Quadratic easing in/out - smoothing the transition."""
|
||||
if t < 0.5:
|
||||
return 2 * t * t
|
||||
else:
|
||||
return 1 - np.power(-2 * t + 2, 2) / 2
|
||||
|
||||
|
||||
|
||||
|
|
@ -136,7 +128,6 @@ def display_top(snapshot: tracemalloc.Snapshot, key_type='lineno', limit=5):
|
|||
print("Total allocated size: %.1f KiB" % (total / 1024))
|
||||
|
||||
|
||||
ImageMapBounds = namedtuple('ImageMapBounds', ['min_x', 'max_x', 'min_y', 'max_y'])
|
||||
class ImageMap(GeometricMap): # TODO Implement for image maps -> watch flipped coordinate system
|
||||
def __init__(self, img: cv2.Mat, H_world_to_map: cv2.Mat, description=None):
|
||||
# homography_matrix = np.loadtxt('H.txt')
|
||||
|
|
@ -153,57 +144,12 @@ class ImageMap(GeometricMap): # TODO Implement for image maps -> watch flipped
|
|||
layers = layers.copy() # copy to apply negative stride
|
||||
# layers =
|
||||
|
||||
#scale 255
|
||||
|
||||
#alternatively: morph image to world space with a scale, as in trajectron/experiments/nuscenes/process_data.py
|
||||
|
||||
super().__init__(layers, homography_matrix, description)
|
||||
|
||||
self.set_bounds()
|
||||
|
||||
def set_bounds(self):
|
||||
"""
|
||||
Use homography and image to calculate the limits of positions in world coordinates
|
||||
"""
|
||||
# print(self.data.shape)
|
||||
|
||||
max_x = self.data.shape[1]
|
||||
max_y = self.data.shape[2]
|
||||
|
||||
# this assumes a map that is only scaled and translated, not skewed
|
||||
points_in_map = np.array([
|
||||
[0, 0],
|
||||
[max_x, max_y],
|
||||
])
|
||||
|
||||
# calculate bounds:
|
||||
H_map_to_world = np.linalg.inv(self.homography)
|
||||
|
||||
# Convert points to homogeneous coordinates and Apply the transformation
|
||||
homogeneous_points = np.hstack((points_in_map, np.ones((points_in_map.shape[0], 1))))
|
||||
transformed_points = np.dot(homogeneous_points, H_map_to_world.T)
|
||||
# Convert back to Cartesian coordinates
|
||||
transformed_points = transformed_points[:, :2]
|
||||
|
||||
self.bounds = ImageMapBounds(
|
||||
transformed_points[0][0],
|
||||
transformed_points[1][0],
|
||||
transformed_points[0][1],
|
||||
transformed_points[1][1]
|
||||
)
|
||||
|
||||
@classmethod
|
||||
def get_cropped_maps_from_scene_map_batch(cls, maps, scene_pts, patch_size, rotation=None, device='cpu'):
|
||||
min_bounds = [maps[0].bounds.min_x, maps[0].bounds.min_y]
|
||||
max_bounds = [maps[0].bounds.max_x, maps[0].bounds.max_y]
|
||||
|
||||
if torch.is_tensor(scene_pts):
|
||||
min_bounds = torch.Tensor(min_bounds)
|
||||
max_bounds = torch.Tensor(max_bounds)
|
||||
|
||||
scene_pts = scene_pts.clip(min=min_bounds, max=max_bounds)
|
||||
|
||||
return super().get_cropped_maps_from_scene_map_batch(maps, scene_pts, patch_size, rotation, device)
|
||||
|
||||
def to_map_points(self, scene_pts):
|
||||
org_shape = None
|
||||
if len(scene_pts.shape) > 2:
|
||||
|
|
|
|||
121
uv.lock
121
uv.lock
|
|
@ -91,15 +91,6 @@ wheels = [
|
|||
{ url = "https://files.pythonhosted.org/packages/ec/6a/bc7e17a3e87a2985d3e8f4da4cd0f481060eb78fb08596c42be62c90a4d9/aiosignal-1.3.2-py2.py3-none-any.whl", hash = "sha256:45cde58e409a301715980c2b01d0c28bdde3770d8290b5eb2173759d9acb31a5", size = 7597 },
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "alchemy-logging"
|
||||
version = "1.5.0"
|
||||
source = { registry = "https://pypi.org/simple" }
|
||||
sdist = { url = "https://files.pythonhosted.org/packages/f0/2a/950fc0f382a65023e64301d9a3a24458b0f7d9be45df7ca7bd242bee4f8a/alchemy-logging-1.5.0.tar.gz", hash = "sha256:ef87de99898f1e62c7cae99e4d2ed4c02c32ccf4c21d6c7a08d93af2bc9945cc", size = 20433 }
|
||||
wheels = [
|
||||
{ url = "https://files.pythonhosted.org/packages/ec/93/0c3073f2a18173d5486abf74dd28d9157e36e159092535e12379333c2f89/alchemy_logging-1.5.0-py3-none-any.whl", hash = "sha256:459ee641c00553175a38f47587ff654ca544985626938e097cb6f85325bd241e", size = 18765 },
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "annotated-types"
|
||||
version = "0.7.0"
|
||||
|
|
@ -417,17 +408,6 @@ wheels = [
|
|||
{ url = "https://files.pythonhosted.org/packages/d3/36/e0010483ca49b9bf6f389631ccea07b3ff6b678d14d8c7a0a4357860c36a/dash-3.2.0-py3-none-any.whl", hash = "sha256:4c1819588d83bed2cbcf5807daa5c2380c8c85789a6935a733f018f04ad8a6a2", size = 7900661 },
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "dearpygui"
|
||||
version = "2.1.0"
|
||||
source = { registry = "https://pypi.org/simple" }
|
||||
wheels = [
|
||||
{ url = "https://files.pythonhosted.org/packages/2e/26/93db234a69d01ae84ed61abb991a3da5555410abdf38d5932fb7ed594e12/dearpygui-2.1.0-cp310-cp310-macosx_10_6_x86_64.whl", hash = "sha256:374d4c605affdf8a49eef4cf434f76e17df374590e51745b62c67025d1d41ec3", size = 2101011 },
|
||||
{ url = "https://files.pythonhosted.org/packages/ef/13/0301fd7fd3ac01ed23003873681c835f18d14267953c54ff6889cb1d0212/dearpygui-2.1.0-cp310-cp310-macosx_13_0_arm64.whl", hash = "sha256:074985fa9d1622edda89c7f113d7a25ae5543f2e3f684bba3601e688c873936f", size = 1874384 },
|
||||
{ url = "https://files.pythonhosted.org/packages/5d/54/5e53d99a1d352f387bd18250d8bcfe9e60594eefc062f246f61810c1bd80/dearpygui-2.1.0-cp310-cp310-manylinux1_x86_64.whl", hash = "sha256:49808389f1d1acfb4f5cbd9f1b1ec188fbcd2d828414094864f7035e27158db2", size = 2636636 },
|
||||
{ url = "https://files.pythonhosted.org/packages/34/44/2508c4ba08b28cc9e827a04ae00fc73dbe6820531241eb43ba28f370372b/dearpygui-2.1.0-cp310-cp310-win_amd64.whl", hash = "sha256:792c1cd34dd0d03bf15838cc1e66ad01282c672ef0d2b9981368b6dcd37e67d3", size = 1810184 },
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "debugpy"
|
||||
version = "1.8.13"
|
||||
|
|
@ -682,46 +662,20 @@ wheels = [
|
|||
|
||||
[[package]]
|
||||
name = "grpcio"
|
||||
version = "1.76.0"
|
||||
version = "1.71.0"
|
||||
source = { registry = "https://pypi.org/simple" }
|
||||
dependencies = [
|
||||
{ name = "typing-extensions" },
|
||||
]
|
||||
sdist = { url = "https://files.pythonhosted.org/packages/b6/e0/318c1ce3ae5a17894d5791e87aea147587c9e702f24122cc7a5c8bbaeeb1/grpcio-1.76.0.tar.gz", hash = "sha256:7be78388d6da1a25c0d5ec506523db58b18be22d9c37d8d3a32c08be4987bd73", size = 12785182 }
|
||||
sdist = { url = "https://files.pythonhosted.org/packages/1c/95/aa11fc09a85d91fbc7dd405dcb2a1e0256989d67bf89fa65ae24b3ba105a/grpcio-1.71.0.tar.gz", hash = "sha256:2b85f7820475ad3edec209d3d89a7909ada16caab05d3f2e08a7e8ae3200a55c", size = 12549828 }
|
||||
wheels = [
|
||||
{ url = "https://files.pythonhosted.org/packages/88/17/ff4795dc9a34b6aee6ec379f1b66438a3789cd1315aac0cbab60d92f74b3/grpcio-1.76.0-cp310-cp310-linux_armv7l.whl", hash = "sha256:65a20de41e85648e00305c1bb09a3598f840422e522277641145a32d42dcefcc", size = 5840037 },
|
||||
{ url = "https://files.pythonhosted.org/packages/4e/ff/35f9b96e3fa2f12e1dcd58a4513a2e2294a001d64dec81677361b7040c9a/grpcio-1.76.0-cp310-cp310-macosx_11_0_universal2.whl", hash = "sha256:40ad3afe81676fd9ec6d9d406eda00933f218038433980aa19d401490e46ecde", size = 11836482 },
|
||||
{ url = "https://files.pythonhosted.org/packages/3e/1c/8374990f9545e99462caacea5413ed783014b3b66ace49e35c533f07507b/grpcio-1.76.0-cp310-cp310-manylinux2014_aarch64.manylinux_2_17_aarch64.whl", hash = "sha256:035d90bc79eaa4bed83f524331d55e35820725c9fbb00ffa1904d5550ed7ede3", size = 6407178 },
|
||||
{ url = "https://files.pythonhosted.org/packages/1e/77/36fd7d7c75a6c12542c90a6d647a27935a1ecaad03e0ffdb7c42db6b04d2/grpcio-1.76.0-cp310-cp310-manylinux2014_i686.manylinux_2_17_i686.whl", hash = "sha256:4215d3a102bd95e2e11b5395c78562967959824156af11fa93d18fdd18050990", size = 7075684 },
|
||||
{ url = "https://files.pythonhosted.org/packages/38/f7/e3cdb252492278e004722306c5a8935eae91e64ea11f0af3437a7de2e2b7/grpcio-1.76.0-cp310-cp310-manylinux2014_x86_64.manylinux_2_17_x86_64.whl", hash = "sha256:49ce47231818806067aea3324d4bf13825b658ad662d3b25fada0bdad9b8a6af", size = 6611133 },
|
||||
{ url = "https://files.pythonhosted.org/packages/7e/20/340db7af162ccd20a0893b5f3c4a5d676af7b71105517e62279b5b61d95a/grpcio-1.76.0-cp310-cp310-musllinux_1_2_aarch64.whl", hash = "sha256:8cc3309d8e08fd79089e13ed4819d0af72aa935dd8f435a195fd152796752ff2", size = 7195507 },
|
||||
{ url = "https://files.pythonhosted.org/packages/10/f0/b2160addc1487bd8fa4810857a27132fb4ce35c1b330c2f3ac45d697b106/grpcio-1.76.0-cp310-cp310-musllinux_1_2_i686.whl", hash = "sha256:971fd5a1d6e62e00d945423a567e42eb1fa678ba89072832185ca836a94daaa6", size = 8160651 },
|
||||
{ url = "https://files.pythonhosted.org/packages/2c/2c/ac6f98aa113c6ef111b3f347854e99ebb7fb9d8f7bb3af1491d438f62af4/grpcio-1.76.0-cp310-cp310-musllinux_1_2_x86_64.whl", hash = "sha256:9d9adda641db7207e800a7f089068f6f645959f2df27e870ee81d44701dd9db3", size = 7620568 },
|
||||
{ url = "https://files.pythonhosted.org/packages/90/84/7852f7e087285e3ac17a2703bc4129fafee52d77c6c82af97d905566857e/grpcio-1.76.0-cp310-cp310-win32.whl", hash = "sha256:063065249d9e7e0782d03d2bca50787f53bd0fb89a67de9a7b521c4a01f1989b", size = 3998879 },
|
||||
{ url = "https://files.pythonhosted.org/packages/10/30/d3d2adcbb6dd3ff59d6ac3df6ef830e02b437fb5c90990429fd180e52f30/grpcio-1.76.0-cp310-cp310-win_amd64.whl", hash = "sha256:a6ae758eb08088d36812dd5d9af7a9859c05b1e0f714470ea243694b49278e7b", size = 4706892 },
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "grpcio-tools"
|
||||
version = "1.76.0"
|
||||
source = { registry = "https://pypi.org/simple" }
|
||||
dependencies = [
|
||||
{ name = "grpcio" },
|
||||
{ name = "protobuf" },
|
||||
{ name = "setuptools" },
|
||||
]
|
||||
sdist = { url = "https://files.pythonhosted.org/packages/a0/77/17d60d636ccd86a0db0eccc24d02967bbc3eea86b9db7324b04507ebaa40/grpcio_tools-1.76.0.tar.gz", hash = "sha256:ce80169b5e6adf3e8302f3ebb6cb0c3a9f08089133abca4b76ad67f751f5ad88", size = 5390807 }
|
||||
wheels = [
|
||||
{ url = "https://files.pythonhosted.org/packages/57/4b/6fceb806f6d5055793f5db0d7a1e3449ea16482c2aec3ad93b05678c325a/grpcio_tools-1.76.0-cp310-cp310-linux_armv7l.whl", hash = "sha256:9b99086080ca394f1da9894ee20dedf7292dd614e985dcba58209a86a42de602", size = 2545596 },
|
||||
{ url = "https://files.pythonhosted.org/packages/3b/11/57af2f3f32016e6e2aae063a533aae2c0e6c577bc834bef97277a7fa9733/grpcio_tools-1.76.0-cp310-cp310-macosx_11_0_universal2.whl", hash = "sha256:8d95b5c2394bbbe911cbfc88d15e24c9e174958cb44dad6aa8c46fe367f6cc2a", size = 5843462 },
|
||||
{ url = "https://files.pythonhosted.org/packages/3f/8b/470bedaf7fb75fb19500b4c160856659746dcf53e3d9241fcc17e3af7155/grpcio_tools-1.76.0-cp310-cp310-manylinux2014_aarch64.manylinux_2_17_aarch64.whl", hash = "sha256:d54e9ce2ffc5d01341f0c8898c1471d887ae93d77451884797776e0a505bd503", size = 2591938 },
|
||||
{ url = "https://files.pythonhosted.org/packages/77/3e/530e848e00d6fe2db152984b2c9432bb8497a3699719fd7898d05cb7d95e/grpcio_tools-1.76.0-cp310-cp310-manylinux2014_i686.manylinux_2_17_i686.whl", hash = "sha256:c83f39f64c2531336bd8d5c846a2159c9ea6635508b0f8ed3ad0d433e25b53c9", size = 2905296 },
|
||||
{ url = "https://files.pythonhosted.org/packages/75/b5/632229d17364eb7db5d3d793131172b2380323c4e6500f528743e477267c/grpcio_tools-1.76.0-cp310-cp310-manylinux2014_x86_64.manylinux_2_17_x86_64.whl", hash = "sha256:be480142fae0d986d127d6cb5cbc0357e4124ba22e96bb8b9ece32c48bc2c8ea", size = 2656266 },
|
||||
{ url = "https://files.pythonhosted.org/packages/ff/71/5756aa9a14d16738b04677b89af8612112d69fb098ffdbc5666020933f23/grpcio_tools-1.76.0-cp310-cp310-musllinux_1_2_aarch64.whl", hash = "sha256:7fefd41fc4ca11fab36f42bdf0f3812252988f8798fca8bec8eae049418deacd", size = 3105798 },
|
||||
{ url = "https://files.pythonhosted.org/packages/ab/de/9058021da11be399abe6c5d2a9a2abad1b00d367111018637195d107539b/grpcio_tools-1.76.0-cp310-cp310-musllinux_1_2_i686.whl", hash = "sha256:63551f371082173e259e7f6ec24b5f1fe7d66040fadd975c966647bca605a2d3", size = 3654923 },
|
||||
{ url = "https://files.pythonhosted.org/packages/8e/93/29f04cc18f1023b2a4342374a45b1cd87a0e1458fc44aea74baad5431dcd/grpcio_tools-1.76.0-cp310-cp310-musllinux_1_2_x86_64.whl", hash = "sha256:75a2c34584c99ff47e5bb267866e7dec68d30cd3b2158e1ee495bfd6db5ad4f0", size = 3322558 },
|
||||
{ url = "https://files.pythonhosted.org/packages/d9/ab/8936708d30b9a2484f6b093dfc57843c1d0380de0eba78a8ad8693535f26/grpcio_tools-1.76.0-cp310-cp310-win32.whl", hash = "sha256:908758789b0a612102c88e8055b7191eb2c4290d5d6fc50fb9cac737f8011ef1", size = 993621 },
|
||||
{ url = "https://files.pythonhosted.org/packages/3d/d2/c5211feb81a532eca2c4dddd00d4971b91c10837cd083781f6ab3a6fdb5b/grpcio_tools-1.76.0-cp310-cp310-win_amd64.whl", hash = "sha256:ec6e49e7c4b2a222eb26d1e1726a07a572b6e629b2cf37e6bb784c9687904a52", size = 1158401 },
|
||||
{ url = "https://files.pythonhosted.org/packages/7c/c5/ef610b3f988cc0cc67b765f72b8e2db06a1db14e65acb5ae7810a6b7042e/grpcio-1.71.0-cp310-cp310-linux_armv7l.whl", hash = "sha256:c200cb6f2393468142eb50ab19613229dcc7829b5ccee8b658a36005f6669fdd", size = 5210643 },
|
||||
{ url = "https://files.pythonhosted.org/packages/bf/de/c84293c961622df302c0d5d07ec6e2d4cd3874ea42f602be2df09c4ad44f/grpcio-1.71.0-cp310-cp310-macosx_12_0_universal2.whl", hash = "sha256:b2266862c5ad664a380fbbcdbdb8289d71464c42a8c29053820ee78ba0119e5d", size = 11308962 },
|
||||
{ url = "https://files.pythonhosted.org/packages/7c/38/04c9e0dc8c904570c80faa1f1349b190b63e45d6b2782ec8567b050efa9d/grpcio-1.71.0-cp310-cp310-manylinux_2_17_aarch64.whl", hash = "sha256:0ab8b2864396663a5b0b0d6d79495657ae85fa37dcb6498a2669d067c65c11ea", size = 5699236 },
|
||||
{ url = "https://files.pythonhosted.org/packages/95/96/e7be331d1298fa605ea7c9ceafc931490edd3d5b33c4f695f1a0667f3491/grpcio-1.71.0-cp310-cp310-manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:c30f393f9d5ff00a71bb56de4aa75b8fe91b161aeb61d39528db6b768d7eac69", size = 6339767 },
|
||||
{ url = "https://files.pythonhosted.org/packages/5d/b7/7e7b7bb6bb18baf156fd4f2f5b254150dcdd6cbf0def1ee427a2fb2bfc4d/grpcio-1.71.0-cp310-cp310-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:f250ff44843d9a0615e350c77f890082102a0318d66a99540f54769c8766ab73", size = 5943028 },
|
||||
{ url = "https://files.pythonhosted.org/packages/13/aa/5fb756175995aeb47238d706530772d9a7ac8e73bcca1b47dc145d02c95f/grpcio-1.71.0-cp310-cp310-musllinux_1_1_aarch64.whl", hash = "sha256:e6d8de076528f7c43a2f576bc311799f89d795aa6c9b637377cc2b1616473804", size = 6031841 },
|
||||
{ url = "https://files.pythonhosted.org/packages/54/93/172783e01eed61f7f180617b7fa4470f504e383e32af2587f664576a7101/grpcio-1.71.0-cp310-cp310-musllinux_1_1_i686.whl", hash = "sha256:9b91879d6da1605811ebc60d21ab6a7e4bae6c35f6b63a061d61eb818c8168f6", size = 6651039 },
|
||||
{ url = "https://files.pythonhosted.org/packages/6f/99/62654b220a27ed46d3313252214f4bc66261143dc9b58004085cd0646753/grpcio-1.71.0-cp310-cp310-musllinux_1_1_x86_64.whl", hash = "sha256:f71574afdf944e6652203cd1badcda195b2a27d9c83e6d88dc1ce3cfb73b31a5", size = 6198465 },
|
||||
{ url = "https://files.pythonhosted.org/packages/68/35/96116de833b330abe4412cc94edc68f99ed2fa3e39d8713ff307b3799e81/grpcio-1.71.0-cp310-cp310-win32.whl", hash = "sha256:8997d6785e93308f277884ee6899ba63baafa0dfb4729748200fcc537858a509", size = 3620382 },
|
||||
{ url = "https://files.pythonhosted.org/packages/b7/09/f32ef637e386f3f2c02effac49699229fa560ce9007682d24e9e212d2eb4/grpcio-1.71.0-cp310-cp310-win_amd64.whl", hash = "sha256:7d6ac9481d9d0d129224f6d5934d5832c4b1cddb96b59e7eba8416868909786a", size = 4280302 },
|
||||
]
|
||||
|
||||
[[package]]
|
||||
|
|
@ -1753,17 +1707,16 @@ wheels = [
|
|||
|
||||
[[package]]
|
||||
name = "protobuf"
|
||||
version = "6.33.0"
|
||||
version = "6.30.1"
|
||||
source = { registry = "https://pypi.org/simple" }
|
||||
sdist = { url = "https://files.pythonhosted.org/packages/19/ff/64a6c8f420818bb873713988ca5492cba3a7946be57e027ac63495157d97/protobuf-6.33.0.tar.gz", hash = "sha256:140303d5c8d2037730c548f8c7b93b20bb1dc301be280c378b82b8894589c954", size = 443463 }
|
||||
sdist = { url = "https://files.pythonhosted.org/packages/55/de/8216061897a67b2ffe302fd51aaa76bbf613001f01cd96e2416a4955dd2b/protobuf-6.30.1.tar.gz", hash = "sha256:535fb4e44d0236893d5cf1263a0f706f1160b689a7ab962e9da8a9ce4050b780", size = 429304 }
|
||||
wheels = [
|
||||
{ url = "https://files.pythonhosted.org/packages/7e/ee/52b3fa8feb6db4a833dfea4943e175ce645144532e8a90f72571ad85df4e/protobuf-6.33.0-cp310-abi3-win32.whl", hash = "sha256:d6101ded078042a8f17959eccd9236fb7a9ca20d3b0098bbcb91533a5680d035", size = 425593 },
|
||||
{ url = "https://files.pythonhosted.org/packages/7b/c6/7a465f1825872c55e0341ff4a80198743f73b69ce5d43ab18043699d1d81/protobuf-6.33.0-cp310-abi3-win_amd64.whl", hash = "sha256:9a031d10f703f03768f2743a1c403af050b6ae1f3480e9c140f39c45f81b13ee", size = 436882 },
|
||||
{ url = "https://files.pythonhosted.org/packages/e1/a9/b6eee662a6951b9c3640e8e452ab3e09f117d99fc10baa32d1581a0d4099/protobuf-6.33.0-cp39-abi3-macosx_10_9_universal2.whl", hash = "sha256:905b07a65f1a4b72412314082c7dbfae91a9e8b68a0cc1577515f8df58ecf455", size = 427521 },
|
||||
{ url = "https://files.pythonhosted.org/packages/10/35/16d31e0f92c6d2f0e77c2a3ba93185130ea13053dd16200a57434c882f2b/protobuf-6.33.0-cp39-abi3-manylinux2014_aarch64.whl", hash = "sha256:e0697ece353e6239b90ee43a9231318302ad8353c70e6e45499fa52396debf90", size = 324445 },
|
||||
{ url = "https://files.pythonhosted.org/packages/e6/eb/2a981a13e35cda8b75b5585aaffae2eb904f8f351bdd3870769692acbd8a/protobuf-6.33.0-cp39-abi3-manylinux2014_s390x.whl", hash = "sha256:e0a1715e4f27355afd9570f3ea369735afc853a6c3951a6afe1f80d8569ad298", size = 339159 },
|
||||
{ url = "https://files.pythonhosted.org/packages/21/51/0b1cbad62074439b867b4e04cc09b93f6699d78fd191bed2bbb44562e077/protobuf-6.33.0-cp39-abi3-manylinux2014_x86_64.whl", hash = "sha256:35be49fd3f4fefa4e6e2aacc35e8b837d6703c37a2168a55ac21e9b1bc7559ef", size = 323172 },
|
||||
{ url = "https://files.pythonhosted.org/packages/07/d1/0a28c21707807c6aacd5dc9c3704b2aa1effbf37adebd8caeaf68b17a636/protobuf-6.33.0-py3-none-any.whl", hash = "sha256:25c9e1963c6734448ea2d308cfa610e692b801304ba0908d7bfa564ac5132995", size = 170477 },
|
||||
{ url = "https://files.pythonhosted.org/packages/83/f6/28460c49a8a93229e2264cd35fd147153fb524cbd944789db6b6f3cc9b13/protobuf-6.30.1-cp310-abi3-win32.whl", hash = "sha256:ba0706f948d0195f5cac504da156d88174e03218d9364ab40d903788c1903d7e", size = 419150 },
|
||||
{ url = "https://files.pythonhosted.org/packages/96/82/7045f5b3f3e338a8ab5852d22ce9c31e0a40d8b0f150a3735dc494be769a/protobuf-6.30.1-cp310-abi3-win_amd64.whl", hash = "sha256:ed484f9ddd47f0f1bf0648806cccdb4fe2fb6b19820f9b79a5adf5dcfd1b8c5f", size = 431007 },
|
||||
{ url = "https://files.pythonhosted.org/packages/b0/b6/732d04d0cdf457d05b7cba83ae73735d91ceced2439735b4500e311c44a5/protobuf-6.30.1-cp39-abi3-macosx_10_9_universal2.whl", hash = "sha256:aa4f7dfaed0d840b03d08d14bfdb41348feaee06a828a8c455698234135b4075", size = 417579 },
|
||||
{ url = "https://files.pythonhosted.org/packages/fc/22/29dd085f6e828ab0424e73f1bae9dbb9e8bb4087cba5a9e6f21dc614694e/protobuf-6.30.1-cp39-abi3-manylinux2014_aarch64.whl", hash = "sha256:47cd320b7db63e8c9ac35f5596ea1c1e61491d8a8eb6d8b45edc44760b53a4f6", size = 317319 },
|
||||
{ url = "https://files.pythonhosted.org/packages/26/10/8863ba4baa4660e3f50ad9ae974c47fb63fa6d4089b15f7db82164b1c879/protobuf-6.30.1-cp39-abi3-manylinux2014_x86_64.whl", hash = "sha256:e3083660225fa94748ac2e407f09a899e6a28bf9c0e70c75def8d15706bf85fc", size = 316213 },
|
||||
{ url = "https://files.pythonhosted.org/packages/a1/d6/683a3d470398e45b4ad9b6c95b7cbabc32f9a8daf454754f0e3df1edffa6/protobuf-6.30.1-py3-none-any.whl", hash = "sha256:3c25e51e1359f1f5fa3b298faa6016e650d148f214db2e47671131b9063c53be", size = 167064 },
|
||||
]
|
||||
|
||||
[[package]]
|
||||
|
|
@ -1817,20 +1770,6 @@ wheels = [
|
|||
{ url = "https://files.pythonhosted.org/packages/e0/a9/023730ba63db1e494a271cb018dcd361bd2c917ba7004c3e49d5daf795a2/py_cpuinfo-9.0.0-py3-none-any.whl", hash = "sha256:859625bc251f64e21f077d099d4162689c762b5d6a4c3c97553d56241c9674d5", size = 22335 },
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "py-to-proto"
|
||||
version = "0.6.0"
|
||||
source = { registry = "https://pypi.org/simple" }
|
||||
dependencies = [
|
||||
{ name = "alchemy-logging" },
|
||||
{ name = "protobuf" },
|
||||
]
|
||||
wheels = [
|
||||
{ url = "https://files.pythonhosted.org/packages/2d/66/521c7f0e630dd0cd32cd25e2e25d2360cfcf179dbe5de68d3d6142b83336/py_to_proto-0.6.0-py310-none-any.whl", hash = "sha256:48a2a109b81574c63a757e79c09b80a4d18a828928db8232646ff2e4c13e4263", size = 33383 },
|
||||
{ url = "https://files.pythonhosted.org/packages/85/d4/077c7f02971e243630dc44fe75544d7694ec49fda7a35bf56935af045d1c/py_to_proto-0.6.0-py38-none-any.whl", hash = "sha256:01c5f2f64b31f0bd9dcb073309264c1f6fa3d8fb410edf329ee537d6d69184b3", size = 33397 },
|
||||
{ url = "https://files.pythonhosted.org/packages/e8/13/58fcb27c1981ad1ab2b2c7921f611c597d741e73e6e1a1340aa19baf96e5/py_to_proto-0.6.0-py39-none-any.whl", hash = "sha256:1d8a4800b4d829819d598f79feb78acdd274f5f50446e3e10e9482edd383104d", size = 33397 },
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "pycparser"
|
||||
version = "2.22"
|
||||
|
|
@ -1889,11 +1828,11 @@ wheels = [
|
|||
|
||||
[[package]]
|
||||
name = "pyglet"
|
||||
version = "2.1.11"
|
||||
version = "2.1.3"
|
||||
source = { registry = "https://pypi.org/simple" }
|
||||
sdist = { url = "https://files.pythonhosted.org/packages/e3/6b/84c397a74cd33eb377168c682e9e3d6b90c1c10c661e11ea5b397ac8497c/pyglet-2.1.11.tar.gz", hash = "sha256:8285d0af7d0ab443232a81df4d941e0d5c48c18a23ec770b3e5c59a222f5d56e", size = 6594448 }
|
||||
sdist = { url = "https://files.pythonhosted.org/packages/08/90/7f8a8d939dbf8f6875b957540cc3091e936e41c4ac8f190a9517589678f8/pyglet-2.1.3.tar.gz", hash = "sha256:9a2c3c84228402ea7103443ac8a52060cc1c91419951ec1105845ce30fed2ce8", size = 6515859 }
|
||||
wheels = [
|
||||
{ url = "https://files.pythonhosted.org/packages/77/a2/2b09fbff0eedbe44fbf164b321439a38f7c5568d8b754aa197ee45886431/pyglet-2.1.11-py3-none-any.whl", hash = "sha256:fa0f4fdf366cfc5040aeb462416910b0db2fa374b7d620b7a432178ca3fa8af1", size = 1032213 },
|
||||
{ url = "https://files.pythonhosted.org/packages/83/d6/41208b6741e732a7faf160e89346a17e81b14899bd7ae90058da858083d6/pyglet-2.1.3-py3-none-any.whl", hash = "sha256:5a7eaf35869ecf7451fb49cc064c4c0e9a118eaa5e771667c607125b13f85e33", size = 962091 },
|
||||
]
|
||||
|
||||
[[package]]
|
||||
|
|
@ -2768,13 +2707,11 @@ source = { editable = "." }
|
|||
dependencies = [
|
||||
{ name = "baumer-neoapi" },
|
||||
{ name = "bytetracker" },
|
||||
{ name = "dearpygui" },
|
||||
{ name = "deep-sort-realtime" },
|
||||
{ name = "facenet-pytorch" },
|
||||
{ name = "ffmpeg-python" },
|
||||
{ name = "foucault" },
|
||||
{ name = "gdown" },
|
||||
{ name = "grpcio-tools" },
|
||||
{ name = "ipywidgets" },
|
||||
{ name = "jsonlines" },
|
||||
{ name = "noise" },
|
||||
|
|
@ -2782,7 +2719,6 @@ dependencies = [
|
|||
{ name = "open3d" },
|
||||
{ name = "opencv-python" },
|
||||
{ name = "pandas-helper-calc" },
|
||||
{ name = "py-to-proto" },
|
||||
{ name = "pyglet" },
|
||||
{ name = "pyglet-cornerpin" },
|
||||
{ name = "python-statemachine" },
|
||||
|
|
@ -2810,13 +2746,11 @@ dependencies = [
|
|||
requires-dist = [
|
||||
{ name = "baumer-neoapi", path = "../../Downloads/Baumer_neoAPI_1.5.0_lin_x86_64_python/wheel/baumer_neoapi-1.5.0-cp34.cp35.cp36.cp37.cp38.cp39.cp310.cp311.cp312-none-linux_x86_64.whl" },
|
||||
{ name = "bytetracker", git = "https://github.com/rubenvandeven/bytetrack-pip" },
|
||||
{ name = "dearpygui", specifier = ">=2.1.0" },
|
||||
{ name = "deep-sort-realtime", specifier = ">=1.3.2,<2" },
|
||||
{ name = "facenet-pytorch", specifier = ">=2.5.3" },
|
||||
{ name = "ffmpeg-python", specifier = ">=0.2.0,<0.3" },
|
||||
{ name = "foucault", git = "https://git.rubenvandeven.com/r/conductofconduct" },
|
||||
{ name = "gdown", specifier = ">=4.7.1,<5" },
|
||||
{ name = "grpcio-tools", specifier = ">=1.76.0" },
|
||||
{ name = "ipywidgets", specifier = ">=8.1.5,<9" },
|
||||
{ name = "jsonlines", specifier = ">=4.0.0,<5" },
|
||||
{ name = "noise", specifier = ">=1.2.2" },
|
||||
|
|
@ -2824,8 +2758,7 @@ requires-dist = [
|
|||
{ name = "open3d", specifier = ">=0.19.0" },
|
||||
{ name = "opencv-python", path = "opencv_python-4.10.0.84-cp310-cp310-linux_x86_64.whl" },
|
||||
{ name = "pandas-helper-calc", git = "https://github.com/scls19fr/pandas-helper-calc" },
|
||||
{ name = "py-to-proto", specifier = ">=0.6.0" },
|
||||
{ name = "pyglet", specifier = ">=2.1.8,<3" },
|
||||
{ name = "pyglet", specifier = ">=2.0.15,<3" },
|
||||
{ name = "pyglet-cornerpin", specifier = ">=0.3.0,<0.4" },
|
||||
{ name = "python-statemachine", specifier = ">=2.5.0" },
|
||||
{ name = "pyusb", specifier = ">=1.3.1,<2" },
|
||||
|
|
@ -2873,11 +2806,11 @@ wheels = [
|
|||
|
||||
[[package]]
|
||||
name = "typing-extensions"
|
||||
version = "4.15.0"
|
||||
version = "4.13.0"
|
||||
source = { registry = "https://pypi.org/simple" }
|
||||
sdist = { url = "https://files.pythonhosted.org/packages/72/94/1a15dd82efb362ac84269196e94cf00f187f7ed21c242792a923cdb1c61f/typing_extensions-4.15.0.tar.gz", hash = "sha256:0cea48d173cc12fa28ecabc3b837ea3cf6f38c6d1136f85cbaaf598984861466", size = 109391 }
|
||||
sdist = { url = "https://files.pythonhosted.org/packages/0e/3e/b00a62db91a83fff600de219b6ea9908e6918664899a2d85db222f4fbf19/typing_extensions-4.13.0.tar.gz", hash = "sha256:0a4ac55a5820789d87e297727d229866c9650f6521b64206413c4fbada24d95b", size = 106520 }
|
||||
wheels = [
|
||||
{ url = "https://files.pythonhosted.org/packages/18/67/36e9267722cc04a6b9f15c7f3441c2363321a3ea07da7ae0c0707beb2a9c/typing_extensions-4.15.0-py3-none-any.whl", hash = "sha256:f0fa19c6845758ab08074a0cfa8b7aecb71c999ca73d62883bc25cc018c4e548", size = 44614 },
|
||||
{ url = "https://files.pythonhosted.org/packages/e0/86/39b65d676ec5732de17b7e3c476e45bb80ec64eb50737a8dce1a4178aba1/typing_extensions-4.13.0-py3-none-any.whl", hash = "sha256:c8dd92cc0d6425a97c18fbb9d1954e5ff92c1ca881a309c45f06ebc0b79058e5", size = 45683 },
|
||||
]
|
||||
|
||||
[[package]]
|
||||
|
|
|
|||
Loading…
Reference in a new issue