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

Compare commits

base: security_vision:main
Branches Tags
security_vision:main
security_vision:betterproto
security_vision:lidar
security_vision:cluster_predictions
security_vision:yolo
security_vision:animation_window
security_vision:render_historic
..
compare: security_vision:render_historic
Branches Tags
security_vision:main
security_vision:betterproto
security_vision:lidar
security_vision:cluster_predictions
security_vision:yolo
security_vision:animation_window
security_vision:render_historic

3 commits
main ... render_his

Author SHA1 Message Date
Ruben van de Ven
9aff9c7c06 prevent infinite history track 2024-11-05 14:29:51 +01:00
Ruben van de Ven
9871848407 forecast trajectories don't transition, but remain and fade out 2024-11-05 13:45:20 +01:00
Ruben van de Ven
e837617e39 WIP refactor to keep predictions longer 2024-10-02 12:05:53 +02:00
59 changed files with 4432 additions and 32324 deletions
Show stats Expand all files Collapse all files

1
.gitignore vendored
View file

@ -1,7 +1,6 @@
.idea/
OUT/
EXPERIMENTS/
runs/
## Core latex/pdflatex auxiliary files:
*.aux

130
EXPERIMENTS/config.json
View file

@ -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,
10,
1
],
"output_size": 32,
"masks": [
5,
5,
5,
5
],
"strides": [
1,
1,
1,
1
],
"dropout": 0.5
}
},
"k": 1,
"k_eval": 25,
"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": 32,
"enc_rnn_dim_edge_influence": 32,
"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": "attention",
"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": true,
"use_map_encoding": true,
"augment": false,
"override_attention_radius": []
}

65
README.md
View file

@ -1,65 +0,0 @@
# Trajectory Prediction Video installation
## Install
* Run `bash build_opencv_with_gstreamer.sh` to build opencv with gstreamer support
* Use `uv` to install
## How to
> See also the sibling repo [traptools](https://git.rubenvandeven.com/security_vision/traptools) for camera calibration and homography tools that are needed for this repo. Also, [laserspace](https://git.rubenvandeven.com/security_vision/laserspace) is used to map the shapes (which are generated by `stage.py`) to lasers, as to use specific optimization techniques for the paths before sending them to the DAC.
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)
* 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)
* or on the RTSP stream. Which uses gstreamer to substantially reduce latency compared to the default ffmpeg bindings in OpenCV.
* To just have a single trajectory pulled from distribution use `--full-dist`. Also try `--z_mode`. -->
## Testnight 2025-06-13
Stappenplan:
* Hang lasers. Connect all cables etc.
* `DISPLAY=:0 cargo run --example laser_frame_stream_gui`
* Use numbers to pick a nice shape. Use this to make sure both lasers cover the right area. (if it doesn't work. Flip some switches in the gui, the laser output should now start)
* In trap folder: `uv run supervisorctl start video`
* In laserspace folder: `DISPLAY=:0 cargo run --bin render_lines_gui` and use gui to draw and tweak projection area
* Use the save button to store configuration
/*
* in trap folder: `DISPLAY=:0 uv run trap_laser_calibration`
* follow instructions:
* camera points: 1-9 or cursor to create/select/move points
* move laser: vim movement keys : hjkl, use shift to move faster
* `c` to calibrate. Matrix is output to cli.
* `q` to quit
* saved to `laser_calib.json`, copy H field to `trap_rust/src/trap/laser.rs` (to e.g. TMP_STUDIO_CM_8)
* Restart `render_lines_gui` with new homographies
* `DISPLAY=:0 cargo run --bin render_lines_gui`
*/
* change video source in `supervisord.conf` and run `uv run supervisorctl update` to switch
* **if tracking is slow and there's no prediction.**
* `uv run python -c "import torch;print(torch.cuda.is_available())"`

130
SETTINGS/2025-11-dortmund/trajectron.json
View file

@ -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": []
}

45
build_opencv_with_gstreamer.sh
View file

@ -1,45 +0,0 @@
#!/bin/bash
# When using RTSP gstreamer can provides a way lower latency then ffmpeg
# and exposes more options to tweak the connection. However, the pypi
# version of python-opencv is build without gstreamer. Thus, we need to
# build our own python wheel.
# adapted from https://github.com/opencv/opencv-python/issues/530#issuecomment-1006343643
# install gstreamer dependencies
sudo apt-get install --quiet -y --no-install-recommends \
gstreamer1.0-gl \
gstreamer1.0-opencv \
gstreamer1.0-plugins-bad \
gstreamer1.0-plugins-good \
gstreamer1.0-plugins-ugly \
gstreamer1.0-tools \
libgstreamer-plugins-base1.0-dev \
libgstreamer1.0-0 \
libgstreamer1.0-dev
# ffmpeg deps
sudo apt install ffmpeg libgtk2.0-dev libavformat-dev libavcodec-dev libavutil-dev libswscale-dev libtbb-dev libjpeg-dev libpng-dev libtiff-dev
OPENCV_VER="84" #fix at 4.10.0.84, or use rolling release: "4.x"
STARTDIR=$(pwd)
TMPDIR=$(mktemp -d)
# Build and install OpenCV from source.
echo $TMPDIR
# pyenv compatibility
cp .python-version $TMPDIR
cd "${TMPDIR}"
git clone --branch ${OPENCV_VER} --depth 1 --recurse-submodules --shallow-submodules https://github.com/opencv/opencv-python.git opencv-python-${OPENCV_VER}
cd opencv-python-${OPENCV_VER}
export ENABLE_CONTRIB=0
# export ENABLE_HEADLESS=1
# We want GStreamer support enabled.
export CMAKE_ARGS="-DWITH_GSTREAMER=ON -DWITH_FFMPEG=ON"
python -m pip wheel . --verbose -w $STARTDIR
# # Install OpenCV
# python3 -m pip install opencv_python*.whl
# cp opencv_python*.whl $STARTDIR

11
custom_bytetrack.yaml
View file

@ -1,11 +0,0 @@
# Ultralytics YOLO 🚀, AGPL-3.0 license
# Default YOLO tracker settings for ByteTrack tracker https://github.com/ifzhang/ByteTrack
tracker_type: bytetrack # tracker type, ['botsort', 'bytetrack']
track_high_thresh: 0.000001 # threshold for the first association
track_low_thresh: 0.000001 # threshold for the second association
new_track_thresh: 0.000001 # threshold for init new track if the detection does not match any tracks
track_buffer: 10 # buffer to calculate the time when to remove tracks
match_thresh: 0.99 # threshold for matching tracks
fuse_score: True # Whether to fuse confidence scores with the iou distances before matching
# min_box_area: 10 # threshold for min box areas(for tracker evaluation, not used for now)

3531
poetry.lock generated Normal file
View file

File diff suppressed because it is too large Load diff

111
pyproject.toml
View file

@ -1,89 +1,38 @@
[project]
[tool.poetry]
name = "trap"
version = "0.1.0"
description = "Art installation with trajectory prediction"
authors = [{ name = "Ruben van de Ven", email = "git@rubenvandeven.com" }]
requires-python = "~=3.10.4"
authors = ["Ruben van de Ven <git@rubenvandeven.com>"]
readme = "README.md"
dependencies = [
"trajectron-plus-plus",
"torch==1.12.1",
"torchvision==0.13.1",
"deep-sort-realtime>=1.3.2,<2",
"ultralytics~=8.3",
"ffmpeg-python>=0.2.0,<0.3",
"torchreid>=0.2.5,<0.3",
"gdown>=4.7.1,<5",
"pandas-helper-calc",
"tsmoothie>=1.0.5,<2",
"pyglet>=2.1.8,<3",
"pyglet-cornerpin>=0.3.0,<0.4",
"opencv-python",
"setproctitle>=1.3.3,<2",
"bytetracker",
"jsonlines>=4.0.0,<5",
"tensorboardx>=2.6.2.2,<3",
"shapely>=2.1",
#"shapely>=1,<2",
"baumer-neoapi",
"qrcode~=8.0",
"pyusb>=1.3.1,<2",
"ipywidgets>=8.1.5,<9",
"foucault",
"python-statemachine>=2.5.0",
"facenet-pytorch>=2.5.3",
"simplification>=0.7.12",
"supervisor>=4.2.5",
"superfsmon>=1.2.3",
"noise>=1.2.2",
"svgpathtools>=1.7.1",
"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",
[tool.poetry.scripts]
trapserv = "trap.plumber:start"
[tool.poetry.dependencies]
python = "^3.10,<3.12,"
trajectron-plus-plus = { path = "../Trajectron-plus-plus/", develop = true }
torch = [
{ version="1.12.1" },
# { url = "https://download.pytorch.org/whl/cu113/torch-1.12.1%2Bcu113-cp38-cp38-linux_x86_64.whl", markers = "python_version ~= '3.8' and sys_platform == 'linux'" },
{ url = "https://download.pytorch.org/whl/cu113/torch-1.12.1%2Bcu113-cp310-cp310-linux_x86_64.whl", markers = "python_version ~= '3.10' and sys_platform == 'linux'" },
]
[project.scripts]
start = "trap.conductofconduct:run"
trapserv = "trap.plumber:start"
tracker = "trap.tools:tracker_preprocess"
compare = "trap.tools:tracker_compare"
process_data = "trap.process_data:main"
blacklist = "trap.tools:blacklist_tracks"
rewrite_tracks = "trap.tools:rewrite_raw_track_files"
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_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]
[tool.uv.sources]
trajectron-plus-plus = { path = "../Trajectron-plus-plus/", editable = true }
torch = [{ url = "https://download.pytorch.org/whl/cu113/torch-1.12.1%2Bcu113-cp310-cp310-linux_x86_64.whl", marker = "python_version ~= '3.10' and sys_platform == 'linux'" }]
torchvision = [{ url = "https://download.pytorch.org/whl/cu113/torchvision-0.13.1%2Bcu113-cp310-cp310-linux_x86_64.whl", marker = "python_version ~= '3.10' and sys_platform == 'linux'" }]
pandas-helper-calc = { git = "https://github.com/scls19fr/pandas-helper-calc" }
bytetracker = { git = "https://github.com/rubenvandeven/bytetrack-pip" }
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" }
foucault = { git = "https://git.rubenvandeven.com/r/conductofconduct" }
opencv-python = {path="./opencv_python-4.10.0.84-cp310-cp310-linux_x86_64.whl"}
[tool.uv.workspace]
members = ["CenterTrack"]
torchvision = [
{ version="0.13.1" },
# { url = "https://download.pytorch.org/whl/cu113/torchvision-0.13.1%2Bcu113-cp38-cp38-linux_x86_64.whl", markers = "python_version ~= '3.8' and sys_platform == 'linux'" },
{ url = "https://download.pytorch.org/whl/cu113/torchvision-0.13.1%2Bcu113-cp310-cp310-linux_x86_64.whl", markers = "python_version ~= '3.10' and sys_platform == 'linux'" },
]
deep-sort-realtime = "^1.3.2"
ultralytics = "^8.0.200"
ffmpeg-python = "^0.2.0"
torchreid = "^0.2.5"
gdown = "^4.7.1"
pandas-helper-calc = {git = "https://github.com/scls19fr/pandas-helper-calc"}
tsmoothie = "^1.0.5"
pyglet = "^2.0.15"
[build-system]
requires = ["hatchling"]
build-backend = "hatchling.build"
requires = ["poetry-core"]
build-backend = "poetry.core.masonry.api"

97
supervisord.conf
View file

@ -1,97 +0,0 @@
[inet_http_server]
port = *:8293
# username = user
# password = 123
[supervisord]
nodaemon = false
; The rpcinterface:supervisor section must remain in the config file for
; RPC (supervisorctl/web interface) to work. Additional interfaces may be
; added by defining them in separate [rpcinterface:x] sections.
[rpcinterface:supervisor]
supervisor.rpcinterface_factory = supervisor.rpcinterface:make_main_rpcinterface
[supervisorctl]
serverurl = http://localhost:8293
[program:monitor]
command=uv run trap_monitor
numprocs=1
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
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
# 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
[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: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

1983
test_custom_rnn.ipynb
View file

File diff suppressed because one or more lines are too long

2301
test_custom_rnn_lstm-attention.ipynb
View file

File diff suppressed because one or more lines are too long

640
test_homography.ipynb
View file

@ -2,7 +2,7 @@
"cells": [
{
"cell_type": "code",
"execution_count": 3,
"execution_count": 1,
"metadata": {},
"outputs": [],
"source": [
@ -15,7 +15,7 @@
},
{
"cell_type": "code",
"execution_count": 4,
"execution_count": 2,
"metadata": {},
"outputs": [],
"source": [
@ -24,7 +24,7 @@
},
{
"cell_type": "code",
"execution_count": 5,
"execution_count": 3,
"metadata": {},
"outputs": [],
"source": [
@ -42,18 +42,13 @@
},
{
"cell_type": "code",
"execution_count": 29,
"execution_count": 4,
"metadata": {},
"outputs": [],
"source": [
"from os import PathLike\n",
"\n",
"\n",
"\n",
"def render_projection(src, dst, img: PathLike, points = []):\n",
" return render_projection_frame(src, dst, cv2.imread(str(img)), points)\n",
"\n",
"def render_projection_frame(src, dst, frame, points = []):\n",
" x_min = min(dst[:,0])\n",
" if x_min < 0:\n",
" dst[:,0] += x_min * -1\n",
@ -71,7 +66,7 @@
" H, status = cv2.findHomography(src,dst)\n",
" f, axes = plt.subplots(1, 2, figsize=(16,8))\n",
"\n",
" img = frame\n",
" img = cv2.imread(str(img))\n",
" img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)\n",
"\n",
" for i, p in enumerate(src):\n",
@ -102,7 +97,7 @@
},
{
"cell_type": "code",
"execution_count": 7,
"execution_count": 5,
"metadata": {},
"outputs": [
{
@ -123,7 +118,7 @@
" [-2.89572527e-04, 1.97232411e-03, 1.00000000e+00]])"
]
},
"execution_count": 7,
"execution_count": 5,
"metadata": {},
"output_type": "execute_result"
}
@ -146,7 +141,7 @@
},
{
"cell_type": "code",
"execution_count": 8,
"execution_count": 6,
"metadata": {},
"outputs": [
{
@ -206,7 +201,7 @@
},
{
"cell_type": "code",
"execution_count": 9,
"execution_count": 7,
"metadata": {},
"outputs": [
{
@ -245,7 +240,7 @@
},
{
"cell_type": "code",
"execution_count": 10,
"execution_count": 8,
"metadata": {},
"outputs": [],
"source": [
@ -256,7 +251,7 @@
},
{
"cell_type": "code",
"execution_count": 11,
"execution_count": 9,
"metadata": {},
"outputs": [
{
@ -306,7 +301,7 @@
},
{
"cell_type": "code",
"execution_count": 12,
"execution_count": 10,
"metadata": {},
"outputs": [
{
@ -351,7 +346,7 @@
},
{
"cell_type": "code",
"execution_count": 13,
"execution_count": 22,
"metadata": {},
"outputs": [
{
@ -376,615 +371,6 @@
"print(f\"{minx} < x < {maxx}\")\n",
"print(f\"{miny} < y < {maxy}\")"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# Expand to multiple video files"
]
},
{
"cell_type": "code",
"execution_count": 24,
"metadata": {},
"outputs": [],
"source": [
"# collect all files\n",
"video_paths = list(Path('../DATASETS/hof/').glob(\"*.m4v\"))"
]
},
{
"cell_type": "code",
"execution_count": 28,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"../DATASETS/hof/webcam20240110-4.m4v\n",
"[[[101 120 129]\n",
" [101 120 129]\n",
" [101 120 129]\n",
" ...\n",
" [122 110 112]\n",
" [121 120 100]\n",
" [123 122 102]]\n",
"\n",
" [[101 120 129]\n",
" [101 120 129]\n",
" [101 120 129]\n",
" ...\n",
" [122 110 112]\n",
" [121 120 100]\n",
" [123 122 102]]\n",
"\n",
" [[100 119 128]\n",
" [101 120 129]\n",
" [101 120 129]\n",
" ...\n",
" [128 112 110]\n",
" [128 120 101]\n",
" [130 122 103]]\n",
"\n",
" ...\n",
"\n",
" [[172 184 189]\n",
" [172 184 189]\n",
" [172 184 189]\n",
" ...\n",
" [149 203 245]\n",
" [149 203 245]\n",
" [149 203 245]]\n",
"\n",
" [[172 184 189]\n",
" [172 184 189]\n",
" [172 184 189]\n",
" ...\n",
" [151 203 245]\n",
" [151 203 245]\n",
" [151 203 245]]\n",
"\n",
" [[172 184 189]\n",
" [172 184 189]\n",
" [172 184 189]\n",
" ...\n",
" [151 203 245]\n",
" [151 203 245]\n",
" [151 203 245]]]\n",
"../DATASETS/hof/webcam20231103-4.m4v\n",
"[[[172 164 145]\n",
" [172 164 145]\n",
" [166 162 152]\n",
" ...\n",
" [146 125 104]\n",
" [146 125 104]\n",
" [146 125 104]]\n",
"\n",
" [[172 164 145]\n",
" [172 164 145]\n",
" [166 162 152]\n",
" ...\n",
" [146 125 104]\n",
" [146 125 104]\n",
" [146 125 104]]\n",
"\n",
" [[172 162 148]\n",
" [172 162 148]\n",
" [168 162 150]\n",
" ...\n",
" [146 125 104]\n",
" [146 125 104]\n",
" [146 125 104]]\n",
"\n",
" ...\n",
"\n",
" [[194 220 232]\n",
" [194 220 232]\n",
" [194 220 232]\n",
" ...\n",
" [209 217 214]\n",
" [209 217 214]\n",
" [209 217 214]]\n",
"\n",
" [[192 222 234]\n",
" [192 222 234]\n",
" [192 222 234]\n",
" ...\n",
" [205 216 217]\n",
" [205 216 217]\n",
" [205 216 217]]\n",
"\n",
" [[193 223 235]\n",
" [193 223 235]\n",
" [193 223 235]\n",
" ...\n",
" [205 216 217]\n",
" [205 216 217]\n",
" [205 216 217]]]\n",
"../DATASETS/hof/webcam20231103-2.m4v\n",
"[[[180 173 165]\n",
" [180 173 165]\n",
" [180 173 165]\n",
" ...\n",
" [158 132 107]\n",
" [158 132 107]\n",
" [158 132 107]]\n",
"\n",
" [[180 173 165]\n",
" [180 173 165]\n",
" [180 173 165]\n",
" ...\n",
" [158 132 107]\n",
" [158 132 107]\n",
" [158 132 107]]\n",
"\n",
" [[181 174 166]\n",
" [179 172 164]\n",
" [180 173 165]\n",
" ...\n",
" [156 130 105]\n",
" [156 130 105]\n",
" [156 130 105]]\n",
"\n",
" ...\n",
"\n",
" [[195 212 221]\n",
" [195 212 221]\n",
" [195 212 221]\n",
" ...\n",
" [208 213 211]\n",
" [208 213 211]\n",
" [208 213 211]]\n",
"\n",
" [[197 215 229]\n",
" [197 215 229]\n",
" [197 215 229]\n",
" ...\n",
" [206 214 213]\n",
" [206 214 213]\n",
" [206 214 213]]\n",
"\n",
" [[199 217 231]\n",
" [199 217 231]\n",
" [199 217 231]\n",
" ...\n",
" [206 214 213]\n",
" [206 214 213]\n",
" [206 214 213]]]\n",
"../DATASETS/hof/webcam20231103-3.m4v\n",
"[[[185 177 165]\n",
" [185 177 165]\n",
" [181 176 168]\n",
" ...\n",
" [156 142 135]\n",
" [156 142 135]\n",
" [156 142 135]]\n",
"\n",
" [[185 177 165]\n",
" [185 177 165]\n",
" [181 176 168]\n",
" ...\n",
" [156 142 135]\n",
" [156 142 135]\n",
" [156 142 135]]\n",
"\n",
" [[188 177 168]\n",
" [188 177 168]\n",
" [184 177 169]\n",
" ...\n",
" [156 142 135]\n",
" [156 142 135]\n",
" [156 142 135]]\n",
"\n",
" ...\n",
"\n",
" [[189 225 233]\n",
" [189 225 233]\n",
" [189 225 233]\n",
" ...\n",
" [211 219 223]\n",
" [211 219 223]\n",
" [211 219 223]]\n",
"\n",
" [[197 228 225]\n",
" [197 228 225]\n",
" [197 228 225]\n",
" ...\n",
" [208 220 225]\n",
" [208 220 225]\n",
" [208 220 225]]\n",
"\n",
" [[197 228 225]\n",
" [197 228 225]\n",
" [197 228 225]\n",
" ...\n",
" [208 220 225]\n",
" [208 220 225]\n",
" [208 220 225]]]\n",
"../DATASETS/hof/webcam20240619-1.m4v\n",
"\tNo homography for ../DATASETS/hof/webcam20240619-1.m4v\n",
"[[[106 105 115]\n",
" [108 107 117]\n",
" [112 111 121]\n",
" ...\n",
" [214 178 141]\n",
" [228 187 146]\n",
" [229 188 147]]\n",
"\n",
" [[105 104 114]\n",
" [107 106 116]\n",
" [111 110 120]\n",
" ...\n",
" [215 182 144]\n",
" [228 187 146]\n",
" [228 187 146]]\n",
"\n",
" [[104 103 113]\n",
" [105 104 114]\n",
" [109 108 118]\n",
" ...\n",
" [224 187 148]\n",
" [227 187 149]\n",
" [226 186 148]]\n",
"\n",
" ...\n",
"\n",
" [[146 133 122]\n",
" [146 133 122]\n",
" [146 133 122]\n",
" ...\n",
" [173 214 240]\n",
" [175 214 240]\n",
" [175 214 240]]\n",
"\n",
" [[147 134 123]\n",
" [147 134 123]\n",
" [147 134 123]\n",
" ...\n",
" [177 220 234]\n",
" [179 219 234]\n",
" [179 219 234]]\n",
"\n",
" [[149 136 125]\n",
" [149 136 125]\n",
" [149 136 125]\n",
" ...\n",
" [179 218 235]\n",
" [181 218 235]\n",
" [181 218 235]]]\n",
"../DATASETS/hof/webcam20240110-2.m4v\n",
"[[[190 227 226]\n",
" [190 227 226]\n",
" [190 227 226]\n",
" ...\n",
" [173 159 152]\n",
" [183 167 159]\n",
" [188 172 164]]\n",
"\n",
" [[190 227 226]\n",
" [190 227 226]\n",
" [190 227 226]\n",
" ...\n",
" [174 160 153]\n",
" [182 166 158]\n",
" [186 170 162]]\n",
"\n",
" [[190 227 226]\n",
" [190 227 226]\n",
" [190 227 226]\n",
" ...\n",
" [183 165 155]\n",
" [186 167 154]\n",
" [185 166 153]]\n",
"\n",
" ...\n",
"\n",
" [[223 223 223]\n",
" [223 223 223]\n",
" [223 223 223]\n",
" ...\n",
" [223 223 223]\n",
" [223 223 223]\n",
" [223 223 223]]\n",
"\n",
" [[224 224 224]\n",
" [224 224 224]\n",
" [224 224 224]\n",
" ...\n",
" [223 223 223]\n",
" [223 223 223]\n",
" [223 223 223]]\n",
"\n",
" [[224 224 224]\n",
" [224 224 224]\n",
" [224 224 224]\n",
" ...\n",
" [223 223 223]\n",
" [223 223 223]\n",
" [223 223 223]]]\n",
"../DATASETS/hof/webcam20240111-2.m4v\n",
"[[[ 62 77 100]\n",
" [ 59 74 97]\n",
" [ 62 77 100]\n",
" ...\n",
" [147 127 90]\n",
" [150 130 93]\n",
" [145 125 88]]\n",
"\n",
" [[ 75 90 113]\n",
" [ 66 81 104]\n",
" [ 62 77 100]\n",
" ...\n",
" [145 125 88]\n",
" [147 127 90]\n",
" [143 123 86]]\n",
"\n",
" [[ 83 91 108]\n",
" [ 74 82 99]\n",
" [ 70 78 95]\n",
" ...\n",
" [147 127 90]\n",
" [150 130 93]\n",
" [145 125 88]]\n",
"\n",
" ...\n",
"\n",
" [[123 121 112]\n",
" [123 121 112]\n",
" [123 121 112]\n",
" ...\n",
" [177 178 165]\n",
" [177 178 165]\n",
" [177 178 165]]\n",
"\n",
" [[123 121 112]\n",
" [123 121 112]\n",
" [123 121 112]\n",
" ...\n",
" [174 172 155]\n",
" [174 172 155]\n",
" [174 172 155]]\n",
"\n",
" [[123 121 112]\n",
" [123 121 112]\n",
" [123 121 112]\n",
" ...\n",
" [172 170 153]\n",
" [172 170 153]\n",
" [172 170 153]]]\n",
"../DATASETS/hof/webcam20240111-1.m4v\n",
"[[[ 64 81 111]\n",
" [ 61 78 108]\n",
" [ 53 70 100]\n",
" ...\n",
" [151 138 86]\n",
" [148 135 83]\n",
" [147 134 82]]\n",
"\n",
" [[ 66 83 113]\n",
" [ 62 79 109]\n",
" [ 54 71 101]\n",
" ...\n",
" [151 138 86]\n",
" [148 135 83]\n",
" [147 134 82]]\n",
"\n",
" [[ 76 89 110]\n",
" [ 72 85 106]\n",
" [ 64 77 98]\n",
" ...\n",
" [151 138 86]\n",
" [148 135 83]\n",
" [147 134 82]]\n",
"\n",
" ...\n",
"\n",
" [[127 126 115]\n",
" [127 126 115]\n",
" [127 126 115]\n",
" ...\n",
" [178 177 164]\n",
" [178 177 164]\n",
" [178 177 164]]\n",
"\n",
" [[127 126 115]\n",
" [127 126 115]\n",
" [127 126 115]\n",
" ...\n",
" [179 169 155]\n",
" [178 168 154]\n",
" [178 168 154]]\n",
"\n",
" [[127 126 115]\n",
" [127 126 115]\n",
" [127 126 115]\n",
" ...\n",
" [176 166 152]\n",
" [175 165 151]\n",
" [175 165 151]]]\n",
"../DATASETS/hof/webcam20240110-3.m4v\n",
"[[[174 201 215]\n",
" [174 201 215]\n",
" [173 200 214]\n",
" ...\n",
" [160 159 153]\n",
" [163 165 158]\n",
" [165 167 160]]\n",
"\n",
" [[175 202 216]\n",
" [175 202 216]\n",
" [174 201 215]\n",
" ...\n",
" [161 160 154]\n",
" [163 165 158]\n",
" [164 166 159]]\n",
"\n",
" [[178 205 219]\n",
" [178 205 219]\n",
" [177 204 218]\n",
" ...\n",
" [164 159 151]\n",
" [165 160 152]\n",
" [165 160 152]]\n",
"\n",
" ...\n",
"\n",
" [[224 224 224]\n",
" [224 224 224]\n",
" [224 224 224]\n",
" ...\n",
" [220 223 223]\n",
" [220 223 223]\n",
" [220 223 223]]\n",
"\n",
" [[224 224 224]\n",
" [224 224 224]\n",
" [224 224 224]\n",
" ...\n",
" [220 223 223]\n",
" [220 223 223]\n",
" [220 223 223]]\n",
"\n",
" [[224 224 224]\n",
" [224 224 224]\n",
" [224 224 224]\n",
" ...\n",
" [220 223 223]\n",
" [220 223 223]\n",
" [220 223 223]]]\n",
"../DATASETS/hof/webcam20240110-1.m4v\n",
"[[[224 224 224]\n",
" [224 224 224]\n",
" [224 224 224]\n",
" ...\n",
" [190 158 136]\n",
" [197 158 137]\n",
" [198 159 138]]\n",
"\n",
" [[224 224 224]\n",
" [224 224 224]\n",
" [224 224 224]\n",
" ...\n",
" [191 159 137]\n",
" [199 160 139]\n",
" [199 160 139]]\n",
"\n",
" [[224 224 224]\n",
" [224 224 224]\n",
" [224 224 224]\n",
" ...\n",
" [192 160 138]\n",
" [194 159 138]\n",
" [194 159 138]]\n",
"\n",
" ...\n",
"\n",
" [[223 223 223]\n",
" [223 223 223]\n",
" [223 223 223]\n",
" ...\n",
" [224 224 224]\n",
" [224 224 224]\n",
" [224 224 224]]\n",
"\n",
" [[223 223 223]\n",
" [223 223 223]\n",
" [223 223 223]\n",
" ...\n",
" [224 224 224]\n",
" [224 224 224]\n",
" [224 224 224]]\n",
"\n",
" [[223 223 223]\n",
" [223 223 223]\n",
" [223 223 223]\n",
" ...\n",
" [224 224 224]\n",
" [224 224 224]\n",
" [224 224 224]]]\n",
"../DATASETS/hof/webcam20240111-3.m4v\n",
"[[[ 65 83 103]\n",
" [ 60 78 98]\n",
" [ 60 78 98]\n",
" ...\n",
" [152 132 90]\n",
" [152 132 90]\n",
" [152 132 90]]\n",
"\n",
" [[ 67 85 105]\n",
" [ 62 80 100]\n",
" [ 59 77 97]\n",
" ...\n",
" [151 131 89]\n",
" [151 131 89]\n",
" [151 131 89]]\n",
"\n",
" [[ 78 92 106]\n",
" [ 70 84 98]\n",
" [ 64 78 92]\n",
" ...\n",
" [151 131 89]\n",
" [149 129 87]\n",
" [149 129 87]]\n",
"\n",
" ...\n",
"\n",
" [[129 125 115]\n",
" [129 125 115]\n",
" [129 125 115]\n",
" ...\n",
" [177 178 167]\n",
" [177 178 167]\n",
" [177 178 167]]\n",
"\n",
" [[129 125 115]\n",
" [129 125 115]\n",
" [129 125 115]\n",
" ...\n",
" [180 174 162]\n",
" [180 174 162]\n",
" [180 174 162]]\n",
"\n",
" [[129 125 115]\n",
" [129 125 115]\n",
" [129 125 115]\n",
" ...\n",
" [179 173 161]\n",
" [179 173 161]\n",
" [179 173 161]]]\n"
]
}
],
"source": [
"for video_path in video_paths:\n",
" print(video_path)\n",
" video = cv2.VideoCapture(str(video_path))\n",
" fps = video.get(cv2.CAP_PROP_FPS)\n",
" target_frame_duration = 1./fps\n",
" if '-' in video_path.stem:\n",
" path_stem = video_path.stem[:video_path.stem.rfind('-')]\n",
" else:\n",
" path_stem = video_path.stem\n",
" path_stem += \"-homography\"\n",
" homography_path = video_path.with_stem(path_stem).with_suffix('.txt')\n",
" if homography_path.exists():\n",
" #print(f'Found custom homography file! Using {homography_path}')\n",
" video_H = np.loadtxt(homography_path, delimiter=',')\n",
" else:\n",
" print(f\"\\tNo homography for {video_path}\")\n",
"\n",
" _, frame = video.read()\n",
" render_projection_frame()"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": []
}
],
"metadata": {

1079
test_model.ipynb
View file

File diff suppressed because one or more lines are too long

435
test_path_transforms.ipynb
View file

File diff suppressed because one or more lines are too long

332
test_tracking_data.ipynb
View file

File diff suppressed because one or more lines are too long

1031
test_training.ipynb
View file

File diff suppressed because one or more lines are too long

4072
test_training_data.ipynb
View file

File diff suppressed because one or more lines are too long

1028
tests/stage_lines.ipynb
View file

File diff suppressed because one or more lines are too long

776
tests/track_history.ipynb
View file

File diff suppressed because one or more lines are too long

872
tests/trajectron_maps.ipynb
View file

File diff suppressed because one or more lines are too long

517
trap/animation_renderer.py
View file

@ -1,517 +0,0 @@
# used for "Forward Referencing of type annotations"
from __future__ import annotations
import time
import tracemalloc
import ffmpeg
from argparse import Namespace
import datetime
import logging
from multiprocessing import Event
from multiprocessing.synchronize import Event as BaseEvent
import cv2
import numpy as np
import pyglet
import pyglet.event
import zmq
import tempfile
from pathlib import Path
import shutil
import math
from pyglet import shapes
from PIL import Image
import json
from trap.frame_emitter import DetectionState, Frame, Track
from trap.preview_renderer import DrawnTrack, PROJECTION_IMG, PROJECTION_MAP
from trap.utils import convert_world_space_to_img_space, display_top
logger = logging.getLogger("trap.renderer")
# COLOR_PRIMARY = (0,0,0,255)
COLOR_PRIMARY = (255,255,255, 255)
class AnimationRenderer:
def __init__(self, config: Namespace, is_running: BaseEvent):
tracemalloc.start()
self.config = config
self.is_running = is_running
context = zmq.Context()
self.prediction_sock = context.socket(zmq.SUB)
self.prediction_sock.setsockopt(zmq.CONFLATE, 1) # only keep latest frame. NB. make sure this comes BEFORE connect, otherwise it's ignored!!
self.prediction_sock.setsockopt(zmq.SUBSCRIBE, b'')
self.prediction_sock.connect(config.zmq_prediction_addr)
self.tracker_sock = context.socket(zmq.SUB)
self.tracker_sock.setsockopt(zmq.CONFLATE, 1) # only keep latest frame. NB. make sure this comes BEFORE connect, otherwise it's ignored!!
self.tracker_sock.setsockopt(zmq.SUBSCRIBE, b'')
self.tracker_sock.connect(config.zmq_trajectory_addr)
self.frame_noimg_sock = context.socket(zmq.SUB)
self.frame_noimg_sock.setsockopt(zmq.CONFLATE, 1) # only keep latest frame. NB. make sure this comes BEFORE connect, otherwise it's ignored!!
self.frame_noimg_sock.setsockopt(zmq.SUBSCRIBE, b'')
self.frame_noimg_sock.connect(config.zmq_frame_noimg_addr)
self.H = self.config.H
self.inv_H = np.linalg.pinv(self.H)
# TODO: get FPS from frame_emitter
# self.out = cv2.VideoWriter(str(filename), fourcc, 23.97, (1280,720))
self.fps = 60
self.frame_size = (self.config.camera.w,self.config.camera.h)
self.hide_stats = self.config.render_hide_stats
self.hide_bg = True
self.pause = False
self.out_writer = None # self.start_writer() if self.config.render_file else None
self.streaming_process = self.start_streaming() if self.config.render_url else None
# if self.config.render_window:
# pass
# # cv2.namedWindow("frame", cv2.WND_PROP_FULLSCREEN)
# # cv2.setWindowProperty("frame",cv2.WND_PROP_FULLSCREEN,cv2.WINDOW_FULLSCREEN)
# else:
if self.streaming_process is not None:
pyglet.options["headless"] = True
config = pyglet.gl.Config(sample_buffers=1, samples=4)
# , fullscreen=self.config.render_window
display = pyglet.display.get_display()
idx = -1 if self.config.render_window else 0
screen = display.get_screens()[idx]
print(display.get_screens())
if self.streaming_process is not None:
self.window = pyglet.window.Window(width=self.frame_size[0], height=self.frame_size[1], config=config, fullscreen=False, screen=screen)
else:
self.window = pyglet.window.Window(width=screen.width, height=screen.height, config=config, fullscreen=True, screen=screen)
self.window.set_handler('on_draw', self.on_draw)
self.window.set_handler('on_refresh', self.on_refresh)
self.window.set_handler('on_close', self.on_close)
self.window.set_handler('on_key_press', self.on_key_press)
# don't know why, but importing this before window leads to "x connection to :1 broken (explicit kill or server shutdown)"
from pyglet_cornerpin import PygletCornerPin
# self.pins = PygletCornerPin(self.window, corners=[[-144,-2], [2880,0], [-168,958], [3011,1553]])
# x1 540 y1 760-360
# x2 1380 y2 670-360
self.pins = PygletCornerPin(
self.window,
source_points=[[540, 670-360], [1380,670-360], [540,760-360], [1380,760-360]],
# corners=[[540, 670-360], [1380,670-360], [540,760-360], [1380,760-360]], # original test: short throw?
# corners=[[396, 442], [1644, 734], [350, 516], [1572, 796]], # beamer downstairs
# corners=[[270, 452], [1698, 784], [314, 568], [1572, 860]], # ??
# corners=[[471, 304], [1797, 376], [467, 387], [1792, 484]] # ??
# corners=[[576, 706], [1790, 696], [588, 794], [1728, 796]], # beamer boven
)
self.window.push_handlers(self.pins)
# pyglet.gl.glClearColor(255,255,255,255)
self.fps_display = pyglet.window.FPSDisplay(window=self.window, color=COLOR_PRIMARY)
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.drawn_tracks: dict[str, DrawnTrack] = {}
self.first_time: float|None = None
self.frame: Frame|None= None
self.tracker_frame: Frame|None = None
self.prediction_frame: Frame|None = None
self.batch_bg = pyglet.graphics.Batch()
self.batch_overlay = pyglet.graphics.Batch()
self.batch_anim = pyglet.graphics.Batch()
self.batch_debug = pyglet.graphics.Batch()
# if self.config.render_debug_shapes:
self.render_debug_shapes = self.config.render_debug_shapes
self.render_lines = True
self.debug_lines = [
pyglet.shapes.Line(1370, self.config.camera.h-360, 1380, 670-360, 2, COLOR_PRIMARY, batch=self.batch_debug),#v
pyglet.shapes.Line(0, 660-360, 1380, 670-360, 2, COLOR_PRIMARY, batch=self.batch_debug), #h
pyglet.shapes.Line(1140, 760-360, 1140, 675-360, 2, COLOR_PRIMARY, batch=self.batch_debug), #h
pyglet.shapes.Line(540, 760-360,540, 675-360, 2, COLOR_PRIMARY, batch=self.batch_debug), #v
pyglet.shapes.Line(0, 770-360, 1380, 770-360, 2, COLOR_PRIMARY, batch=self.batch_debug), #h
]
self.debug_points = []
# print(self.config.debug_points_file)
if self.config.debug_points_file:
with self.config.debug_points_file.open('r') as fp:
img_points = np.array(json.load(fp))
# to place points accurate I used a 2160p image, but during calibration and
# prediction I use(d) a 1440p image, so convert points to different space:
img_points = np.array(img_points)
# first undistort the points so that lines are actually straight
undistorted_img_points = cv2.undistortPoints(np.array([img_points]).astype('float32'), self.config.camera.mtx, self.config.camera.dist, None, self.config.camera.newcameramtx)
dst_img_points = cv2.perspectiveTransform(np.array(undistorted_img_points), self.config.camera.H)
if dst_img_points.shape[1:] == (1,2):
dst_img_points = np.reshape(dst_img_points, (dst_img_points.shape[0], 2))
self.debug_points = [
pyglet.shapes.Circle(p[0], self.window.height - p[1], 3, color=(255,0,0,255), batch=self.batch_debug) for p in dst_img_points
]
self.init_labels()
def start_streaming(self):
"""TODO)) This should be inherited from a generic renderer"""
return (
ffmpeg
.input('pipe:', format='rawvideo',codec="rawvideo", pix_fmt='bgr24', s='{}x{}'.format(*self.frame_size))
.output(
self.config.render_url,
#codec = "copy", # use same codecs of the original video
codec='libx264',
listen=1, # enables HTTP server
pix_fmt="yuv420p",
preset="ultrafast",
tune="zerolatency",
# g=f"{self.fps*2}",
g=f"{60*2}",
analyzeduration="2000000",
probesize="1000000",
f='mpegts'
)
.overwrite_output()
.run_async(pipe_stdin=True)
)
# return process
def init_labels(self):
base_color = COLOR_PRIMARY
color_predictor = (255,255,0, 255)
color_info = (255,0, 255, 255)
color_tracker = (0,255, 255, 255)
options = []
for option in ['prediction_horizon','num_samples','full_dist','gmm_mode','z_mode', 'model_dir']:
options.append(f"{option}: {self.config.__dict__[option]}")
self.labels = {
'waiting': pyglet.text.Label("Waiting for prediction"),
'frame_idx': pyglet.text.Label("", x=20, y=self.window.height - 17, color=base_color, batch=self.batch_overlay),
'tracker_idx': pyglet.text.Label("", x=90, y=self.window.height - 17, color=color_tracker, batch=self.batch_overlay),
'pred_idx': pyglet.text.Label("", x=110, y=self.window.height - 17, color=color_predictor, batch=self.batch_overlay),
'frame_time': pyglet.text.Label("t", x=140, y=self.window.height - 17, color=base_color, batch=self.batch_overlay),
'frame_latency': pyglet.text.Label("", x=235, y=self.window.height - 17, color=color_info, batch=self.batch_overlay),
'tracker_time': pyglet.text.Label("", x=300, y=self.window.height - 17, color=color_tracker, batch=self.batch_overlay),
'pred_time': pyglet.text.Label("", x=360, y=self.window.height - 17, color=color_predictor, batch=self.batch_overlay),
'track_len': pyglet.text.Label("", x=800, y=self.window.height - 17, color=color_tracker, batch=self.batch_overlay),
'options1': pyglet.text.Label(options.pop(-1), x=20, y=30, color=base_color, batch=self.batch_overlay),
'options2': pyglet.text.Label(" | ".join(options), x=20, y=10, color=base_color, batch=self.batch_overlay),
}
def refresh_labels(self, dt: float):
"""Every frame"""
if self.frame:
self.labels['frame_idx'].text = f"{self.frame.index:06d}"
self.labels['frame_time'].text = f"{self.frame.time - self.first_time: >10.2f}s"
self.labels['frame_latency'].text = f"{self.frame.time - time.time():.2f}s"
if self.frame.time - self.first_time > 30 and (not hasattr(self, 'has_snap') or self.has_snap == False):
snapshot = tracemalloc.take_snapshot()
display_top(snapshot, 'traceback', 15)
tracemalloc.stop()
self.has_snap = True
if self.tracker_frame and self.frame:
self.labels['tracker_idx'].text = f"{self.tracker_frame.index - self.frame.index}"
self.labels['tracker_time'].text = f"{self.tracker_frame.time - time.time():.3f}s"
self.labels['track_len'].text = f"{len(self.tracker_frame.tracks)} tracks"
if self.prediction_frame and self.frame:
self.labels['pred_idx'].text = f"{self.prediction_frame.index - self.frame.index}"
self.labels['pred_time'].text = f"{self.prediction_frame.time - time.time():.3f}s"
# self.labels['track_len'].text = f"{len(self.prediction_frame.tracks)} tracks"
# cv2.putText(img, f"{frame.index:06d}", (20,17), cv2.FONT_HERSHEY_PLAIN, 1, base_color, 1)
# cv2.putText(img, f"{frame.time - first_time:.3f}s", (120,17), cv2.FONT_HERSHEY_PLAIN, 1, base_color, 1)
# if prediction_frame:
# # render Δt and Δ frames
# cv2.putText(img, f"{prediction_frame.index - frame.index}", (90,17), cv2.FONT_HERSHEY_PLAIN, 1, info_color, 1)
# cv2.putText(img, f"{prediction_frame.time - time.time():.2f}s", (200,17), cv2.FONT_HERSHEY_PLAIN, 1, info_color, 1)
# cv2.putText(img, f"{len(prediction_frame.tracks)} tracks", (500,17), cv2.FONT_HERSHEY_PLAIN, 1, base_color, 1)
# cv2.putText(img, f"h: {np.average([len(t.history or []) for t in prediction_frame.tracks.values()]):.2f}", (580,17), cv2.FONT_HERSHEY_PLAIN, 1, info_color, 1)
# cv2.putText(img, f"ph: {np.average([len(t.predictor_history or []) for t in prediction_frame.tracks.values()]):.2f}", (660,17), cv2.FONT_HERSHEY_PLAIN, 1, info_color, 1)
# cv2.putText(img, f"p: {np.average([len(t.predictions or []) for t in prediction_frame.tracks.values()]):.2f}", (740,17), cv2.FONT_HERSHEY_PLAIN, 1, info_color, 1)
# options = []
# for option in ['prediction_horizon','num_samples','full_dist','gmm_mode','z_mode', 'model_dir']:
# options.append(f"{option}: {config.__dict__[option]}")
# cv2.putText(img, options.pop(-1), (20,img.shape[0]-30), cv2.FONT_HERSHEY_PLAIN, 1, base_color, 1)
# cv2.putText(img, " | ".join(options), (20,img.shape[0]-10), cv2.FONT_HERSHEY_PLAIN, 1, base_color, 1)
def check_frames(self, dt):
if self.pause:
return
new_tracks = False
try:
self.frame: Frame = self.frame_noimg_sock.recv_pyobj(zmq.NOBLOCK)
if not self.first_time:
self.first_time = self.frame.time
if self.frame.img:
img = self.frame.img
# newcameramtx, roi = cv2.getOptimalNewCameraMatrix(self.config.camera.mtx, self.config.camera.dist, (self.frame.img.shape[1], self.frame.img.shape[0]), 1, (self.frame.img.shape[1], self.frame.img.shape[0]))
img = cv2.undistort(img, self.config.camera.mtx, self.config.camera.dist, None, self.config.camera.newcameramtx)
img = cv2.warpPerspective(img, convert_world_space_to_img_space(self.config.camera.H), (self.config.camera.w, self.config.camera.h))
# img = cv2.GaussianBlur(img, (15, 15), 0)
img = cv2.flip(cv2.cvtColor(img, cv2.COLOR_BGR2RGB), 0)
img = pyglet.image.ImageData(self.frame_size[0], self.frame_size[1], 'RGB', img.tobytes())
# don't draw in batch, so that it is the background
if hasattr(self, 'video_sprite') and self.video_sprite:
self.video_sprite.delete()
self.frame.img = None
self.video_sprite = pyglet.sprite.Sprite(img=img, batch=self.batch_bg)
# transform to flipped coordinate system for pyglet
self.video_sprite.y = self.window.height - self.video_sprite.height
# self.frame.img = np.array([]) # clearing memory?
# self.video_sprite.opacity = 70
except zmq.ZMQError as e:
# idx = frame.index if frame else "NONE"
# logger.debug(f"reuse video frame {idx}")
pass
try:
self.prediction_frame: Frame = self.prediction_sock.recv_pyobj(zmq.NOBLOCK)
new_tracks = True
except zmq.ZMQError as e:
pass
try:
self.tracker_frame: Frame = self.tracker_sock.recv_pyobj(zmq.NOBLOCK)
new_tracks = True
except zmq.ZMQError as e:
pass
if new_tracks:
self.update_tracks()
def update_tracks(self):
"""Updates the track objects and shapes. Called after setting `prediction_frame`
"""
# clean up
# for track_id in list(self.drawn_tracks.keys()):
# if track_id not in self.prediction_frame.tracks.keys():
# # TODO fade out
# del self.drawn_tracks[track_id]
if self.tracker_frame:
for track_id, track in self.tracker_frame.tracks.items():
if track_id not in self.drawn_tracks:
self.drawn_tracks[track_id] = DrawnTrack(track_id, track, self, self.tracker_frame.H, PROJECTION_MAP, self.config.camera)
else:
self.drawn_tracks[track_id].set_track(track)
if self.prediction_frame:
for track_id, track in self.prediction_frame.tracks.items():
if track_id not in self.drawn_tracks:
self.drawn_tracks[track_id] = DrawnTrack(track_id, track, self, self.prediction_frame.H, PROJECTION_MAP, self.config.camera)
else:
self.drawn_tracks[track_id].set_predictions(track)
# clean up
for track_id in list(self.drawn_tracks.keys()):
# TODO make delay configurable
if self.drawn_tracks[track_id].update_at < time.time() - 5:
# TODO fade out
del self.drawn_tracks[track_id]
def on_key_press(self, symbol, modifiers):
print('A key was pressed, use f to hide')
if symbol == ord('f'):
self.window.set_fullscreen(not self.window.fullscreen)
if symbol == ord('h'):
self.hide_stats = not self.hide_stats
if symbol == ord('d'):
self.render_debug_shapes = not self.render_debug_shapes
if symbol == ord('p'):
self.pause = not self.pause
if symbol == ord('b'):
self.hide_bg = not self.hide_bg
if symbol == ord('l'):
self.render_lines = not self.render_lines
def check_running(self, dt):
if not self.is_running.is_set():
self.window.close()
self.event_loop.exit()
print('quit animation renderer')
def on_close(self):
self.is_running.clear()
def on_refresh(self, dt: float):
# update shapes
# self.bg =
for track_id, track in self.drawn_tracks.items():
track.update_drawn_positions(dt)
self.refresh_labels(dt)
# self.shape1 = shapes.Circle(700, 150, 100, color=(50, 0, 30), batch=self.batch_anim)
# self.shape3 = shapes.Circle(800, 150, 100, color=(100, 225, 30), batch=self.batch_anim)
pass
def on_draw(self):
self.window.clear()
if not self.hide_bg:
self.batch_bg.draw()
if self.render_debug_shapes:
self.batch_debug.draw()
self.pins.draw()
if self.render_lines:
for track in self.drawn_tracks.values():
for shape in track.shapes:
shape.draw() # for some reason the batches don't work
for track in self.drawn_tracks.values():
for shapes in track.pred_shapes:
for shape in shapes:
shape.draw()
# self.batch_anim.draw()
# pyglet.graphics.draw(3, pyglet.gl.GL_LINE, ("v2i", (100,200, 600,800)), ('c3B', (255,255,255, 255,255,255)))
if not self.hide_stats:
self.batch_overlay.draw()
self.fps_display.draw()
# if streaming, capture buffer and send
try:
if self.streaming_process or self.out_writer:
buf = pyglet.image.get_buffer_manager().get_color_buffer()
img_data = buf.get_image_data()
data = img_data.get_data() # alternative: .get_data("RGBA", image_data.pitch)
img = np.asanyarray(data).reshape((img_data.height, img_data.width, 4))
img = cv2.cvtColor(img, cv2.COLOR_BGRA2RGB)
img = np.flip(img, 0)
# img = cv2.flip(img, cv2.0)
# cv2.imshow('frame', img)
# cv2.waitKey(1)
if self.streaming_process:
self.streaming_process.stdin.write(img.tobytes())
if self.out_writer:
self.out_writer.write(img)
except Exception as e:
logger.exception(e)
def run(self):
frame = None
prediction_frame = None
tracker_frame = None
i=0
first_time = None
self.event_loop = pyglet.app.EventLoop()
pyglet.clock.schedule_interval(self.check_running, 0.1)
pyglet.clock.schedule(self.check_frames)
self.event_loop.run()
# while self.is_running.is_set():
# i+=1
# # zmq_ev = self.frame_sock.poll(timeout=2000)
# # if not zmq_ev:
# # # when no data comes in, loop so that is_running is checked
# # continue
# try:
# frame: Frame = self.frame_sock.recv_pyobj(zmq.NOBLOCK)
# except zmq.ZMQError as e:
# # idx = frame.index if frame else "NONE"
# # logger.debug(f"reuse video frame {idx}")
# pass
# # else:
# # logger.debug(f'new video frame {frame.index}')
# if frame is None:
# # might need to wait a few iterations before first frame comes available
# time.sleep(.1)
# continue
# try:
# prediction_frame: Frame = self.prediction_sock.recv_pyobj(zmq.NOBLOCK)
# except zmq.ZMQError as e:
# logger.debug(f'reuse prediction')
# if first_time is None:
# first_time = frame.time
# img = decorate_frame(frame, prediction_frame, first_time, self.config)
# img_path = (self.config.output_dir / f"{i:05d}.png").resolve()
# logger.debug(f"write frame {frame.time - first_time:.3f}s")
# if self.out_writer:
# self.out_writer.write(img)
# if self.streaming_process:
# self.streaming_process.stdin.write(img.tobytes())
# if self.config.render_window:
# cv2.imshow('frame',img)
# cv2.waitKey(1)
logger.info('Stopping')
logger.info(f'used corner pins {self.pins.pin_positions}')
print(self.pins.pin_positions)
# if i>2:
if self.streaming_process:
self.streaming_process.stdin.close()
if self.out_writer:
self.out_writer.release()
if self.streaming_process:
# oddly wrapped, because both close and release() take time.
self.streaming_process.wait()
def run_animation_renderer(config: Namespace, is_running: BaseEvent):
renderer = AnimationRenderer(config, is_running)
renderer.run()

181
trap/anomaly.py
View file

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

794
trap/base.py
View file

@ -1,794 +0,0 @@
from __future__ import annotations
from abc import ABC, abstractmethod
import argparse
from collections import defaultdict
from copy import deepcopy
from enum import IntFlag
from itertools import cycle
import json
import logging
from pathlib import Path
import time
import types
from typing import Iterable, Optional, Tuple, Union, List
import cv2
from dataclasses import dataclass, field
import dataclasses
from nptyping import Float64, NDArray, Shape
import numpy as np
from deep_sort_realtime.deep_sort.track import Track as DeepsortTrack
from deep_sort_realtime.deep_sort.track import TrackState as DeepsortTrackState
from bytetracker.byte_tracker import STrack as ByteTrackTrack
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
from cv2.typing import MatLike
logger = logging.getLogger('trap.base')
class UrlOrPath():
"""
Some video sources are on a path (files), others a url (some cameras).
Provide some utilities to easily deal with either.
"""
def __init__(self, string):
self.url = urlparse(str(string))
def __str__(self) -> str:
return self.url.geturl()
def is_url(self) -> bool:
return len(self.url.netloc) > 0
def path(self) -> Path:
if self.is_url():
return Path(self.url.path)
return Path(self.url.geturl()) # can include scheme, such as C:/
class Space(IntFlag):
Image = 1 # As detected in the image
Undistorted = 2 # After applying lense undistortiion
World = 4 # After lens undistort and homography
Render = 8 # View space of renderer
@dataclass
class Position:
x: float
y: float
conf: float
state: DetectionState
frame_nr: int
det_class: str
class DetectionState(IntFlag):
Tentative = 1 # state before n_init (see DeepsortTrack)
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):
if track.state == DeepsortTrackState.Tentative:
return cls.Tentative
if track.state == DeepsortTrackState.Confirmed:
if track.time_since_update > 0:
return cls.Lost
return cls.Confirmed
raise RuntimeError("Should not run into Deleted entries here")
@classmethod
def from_bytetrack_track(cls, track: ByteTrackTrack):
if track.state == ByteTrackTrackState.New:
return cls.Tentative
if track.state == ByteTrackTrackState.Removed:
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")
def H_from_path(path: Path):
if path.suffix == '.json':
with path.open('r') as fp:
H = np.array(json.load(fp))
else:
H = np.loadtxt(path, delimiter=',')
return H
PointList = List[Tuple[float, float]] | np.ndarray | cv2.typing.MatLike
def scale_homography(H: cv2.Mat, scale: float):
"""Transform the given matrix so that it immediately converts
the points to img space"""
new_H = H.copy()
new_H[:2] = H[:2] * scale
return new_H
class DistortedCamera(ABC):
@abstractmethod
def undistort_img(self, img: MatLike):
return cv2.remap(img, self.map1, self.map2, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT)
def project_img(self, undistorted_img: MatLike, scale: float = 1.0):
w, h = undistorted_img.shape[1], undistorted_img.shape[0]
if scale != 1:
H = scale_homography(self.H, scale)
else:
H = self.H
return cv2.warpPerspective(undistorted_img, H,(w, h))
def img_to_world(self, img: MatLike, scale = 1.):
img = self.undistort_img(img)
return self.project_img(img, scale)
@abstractmethod
def undistort_points(self, distorted_points: PointList):
pass
def project_point(self, point):
return self.project_points([point])[0]
def project_points(self, points: PointList, scale: float = 1.0):
if scale != 1:
H = scale_homography(self.H, scale)
else:
H = self.H
coords = cv2.perspectiveTransform(np.array([points]),H)
# if coords.shape[1:] == (1,2):
coords = np.reshape(coords, (len(points), 2))
return coords
@classmethod
def from_calibfile(cls, calibration_path, H, fps):
with calibration_path.open('r') as fp:
data = json.load(fp)
camera = cls.from_calibdata(data, H, fps)
return camera
@classmethod
def from_paths(cls, calibration_path: Path, h_path: Path, fps: float):
H = H_from_path(h_path)
with calibration_path.open('r') as fp:
calibdata = json.load(fp)
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)
return camera
# return cls.from_calibfile(calibration_path, H, fps)
def points_img_to_world(self, points: PointList, scale = 1.):
# undistort & project
coords = self.undistort_points(points)
coords = self.project_points(coords, scale)
return coords
class FisheyeCamera(DistortedCamera):
def __init__(self, dim1, dim2, dim3, K, D, new_K, scaled_K, balance, H, fps):
# dimensions as per: https://medium.com/@kennethjiang/calibrate-fisheye-lens-using-opencv-part-2-13990f1b157f
self.dim1 = dim1 # original image
self.dim2 = dim2 # dimension of the box you want to keep after un-distorting the image. influced by balance
self.dim3 = dim3 # Dimension of the final box where OpenCV will put the undistorted image.
self.K = K
self.D = D
self.new_K = new_K
self.scaled_K = scaled_K
self.balance = balance
self.H = H # Homography
self._R = np.eye(3)
self.fps = fps
self.map1, self.map2 = cv2.fisheye.initUndistortRectifyMap(self.scaled_K, self.D, self._R, self.new_K, self.dim3, cv2.CV_16SC2)
# self.map1, self.map2 = cv2.fisheye.initUndistortRectifyMap(self.scaled_K, self.D, self._R, self.new_K, self.dim3, cv2.CV_32FC1)
def undistort_img(self, img: MatLike):
# map1, map2 = adjust_remap_maps(self.map1, self.map2, 2, (0,0))
# this only works on the undistort, but screws up when doing subsequent homography,
# there needs to be a way to combine both this remap and warpPerspective into a
# single remap call...
# scale = 0.3
# cx = self.dim3[0] / 2
# cy = self.dim3[1] / 2
# map1 = (self.map1 - cx) / scale + cx
# map2 = (self.map2 - cy) / scale + cy
# map1 += 900 #translate x (>0 left, <0 right)
# map2 += 1500 #translate y (>0 up, <0 down)
return cv2.remap(img, self.map1, self.map2, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT)
def undistort_points(self, distorted_points: PointList):
points = cv2.fisheye.undistortPoints (np.array([distorted_points]).astype(np.float32), K=self.scaled_K, D=self.D, R=self._R, P=self.new_K)
return points[0]
@property
def projected_w(self):
return self.dim3[0]
@property
def projected_h(self):
return self.dim3[1]
@classmethod
def from_calibdata(cls, data, H, fps):
return cls(
data['dim1'],
data['dim2'],
data['dim3'],
np.array(data['K']),
np.array(data['D']),
np.array(data['new_K']),
np.array(data['scaled_K']),
data['balance'],
H, fps)
class UndistortedCamera(DistortedCamera):
def __init__(self, fps = 10):
self.fps = fps
self.H = np.eye(3,3)
def undistort_img(self, img: MatLike):
return deepcopy(img)
def undistort_points(self, distorted_points: PointList):
return deepcopy(distorted_points)
class Camera(DistortedCamera):
def __init__(self, mtx: cv2.Mat, dist: cv2.Mat, w: float, h: float, H: cv2.Mat, fps: float):
self.mtx = mtx
self.dist = dist
self.w = w
self.h = h
self.H = H
self.fps = fps
self.newcameramtx, self.roi = cv2.getOptimalNewCameraMatrix(self.mtx, self.dist, (self.w,self.h), 1, (self.w,self.h))
@classmethod
def from_calibdata(cls, data, H, fps):
return cls(
np.array(data['camera_matrix']),
np.array(data['dist_coeff']),
data['dim']['width'],
data['dim']['height'],
H, fps)
@property
def projected_w(self):
return self.w
@property
def projected_h(self):
return self.h
def undistort_img(self, img: MatLike):
return cv2.undistort(img, self.mtx, self.dist, None, self.newcameramtx)
def undistort_points(self, distorted_points: PointList):
points = cv2.undistortPoints(np.array([distorted_points]).astype('float32'), self.mtx, self.dist, None, self.newcameramtx)
# print(points.reshape())
return points.reshape(points.shape[0], 2)
@dataclass
class Detection:
track_id: str # deepsort track id association
l: int # left - image space
t: int # top - image space
w: int # width - image space
h: int # height - image space
conf: float # object detector probablity
state: DetectionState
frame_nr: int
det_class: str
def get_foot_coords(self) -> list[float, float]:
return [self.l + 0.5 * self.w, self.t+self.h]
@classmethod
def from_deepsort(cls, dstrack: DeepsortTrack, frame_nr: int):
return cls(dstrack.track_id, *dstrack.to_ltwh(), dstrack.det_conf or 0, DetectionState.from_deepsort_track(dstrack), frame_nr, dstrack.det_class)
@classmethod
def from_bytetrack(cls, bstrack: ByteTrackTrack, frame_nr: int):
return cls(bstrack.track_id, *bstrack.tlwh, bstrack.score, DetectionState.from_bytetrack_track(bstrack), frame_nr, bstrack.cls)
def get_scaled(self, scale: float = 1):
if scale == 1:
return self
return Detection(
self.track_id,
self.l*scale,
self.t*scale,
self.w*scale,
self.h*scale,
self.conf,
self.state,
self.frame_nr,
self.det_class)
def to_ltwh(self):
return (int(self.l), int(self.t), int(self.w), int(self.h))
def to_ltrb(self):
return (int(self.l), int(self.t), int(self.l+self.w), int(self.t+self.h))
# Proxy'd Track, which caches projected history
class ProjectedTrack(object):
def __init__(self, track: Track, camera: Camera):
self._track = track
self.camera = camera # keep to wrap other calls
self.projected_history = track.get_projected_history(camera=camera)
# TODO wrap functions of Track()
def __getattr__(self, attr):
return getattr(self._track, attr)
@dataclass
class Track:
"""A bit of an haphazardous wrapper around the 'real' tracker to provide
a history, with which the predictor can work, as we then can deduce velocity
and acceleration.
"""
track_id: str = None
history: List[Detection] = field(default_factory=list)
predictor_history: Optional[list] = None # in image space
predictions: Optional[list] = None
fps: int = 12 # TODO)) convert this to camera? That way, incorporates H and dist, alternatively, each track is as a whole attached to a space
source: Optional[int] = None # to keep track of processed tracks
lost: bool = False
created_at: Optional[float] = None
frame_index: int = 0
updated_at: Optional[float] = None
def __post_init__(self):
if not self.created_at:
self.created_at = time.time()
if not self.updated_at:
self.updated_at = time.time()
def track_age(self) -> float:
return time.time() - self.created_at
def track_update_dt(self) -> float:
return time.time() - self.updated_at
def get_projected_history(self, H: Optional[cv2.Mat] = None, camera: Optional[DistortedCamera]= None) -> NDArray[Shape["*, 2"], Float64]:
foot_coordinates = [d.get_foot_coords() for d in self.history]
# TODO)) Undistort points before perspective transform
if len(foot_coordinates):
if camera:
coords = camera.points_img_to_world(foot_coordinates)
return coords
# coords = cv2.undistortPoints(np.array([foot_coordinates]).astype('float32'), camera.mtx, camera.dist, None, camera.newcameramtx)
# coords = cv2.perspectiveTransform(np.array(coords),camera.H)
# return coords.reshape((coords.shape[0],2))
else:
coords = cv2.perspectiveTransform(np.array([foot_coordinates]),H)
return coords[0]
return np.empty(shape=(0,2)) #np.array([], shape)
def get_projected_history_as_dict(self, H, camera: Optional[DistortedCamera]= None) -> dict:
coords = self.get_projected_history(H, camera)
return [{"x":c[0], "y":c[1]} for c in coords]
def get_with_interpolated_history(self) -> Track:
# 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)]
# new_track = Track(track.track_id, new_history, track.predictor_history, track.predictions)
new_history = []
for j in range(len(self.history)):
a = self.history[j]
new_history.append(Detection(a.track_id, a.l, a.t, a.w, a.h, a.conf, a.state, a.frame_nr, a.det_class))
if j+1 >= len(self.history):
break
b = self.history[j+1]
gap = b.frame_nr - a.frame_nr
if gap < 1:
logger.error(f"WARNING, gap between frames {a.frame_nr} -> {b.frame_nr} is negative?")
if gap > 1:
for g in range(1, gap):
l = lerp(a.l, b.l, g/gap)
t = lerp(a.t, b.t, g/gap)
w = lerp(a.w, b.w, g/gap)
h = lerp(a.h, b.h, g/gap)
conf = 0
state = DetectionState.Lost
frame_nr = a.frame_nr + g
new_history.append(Detection(a.track_id, l, t, w, h, conf, state, frame_nr, a.det_class))
return self.get_with_new_history(new_history)
def get_with_new_history(self, new_history: List[Detection]):
return Track(
self.track_id,
new_history,
self.predictor_history,
self.predictions,
self.fps,
self.source,
self.lost,
self.created_at,
self.frame_index,
self.updated_at)
def is_complete(self):
diffs = [(b.frame_nr - a.frame_nr) for a,b in zip(self.history[:-1], self.history[1:])]
return any([d != 1 for d in diffs])
def get_sampled(self, step_size = 1, offset=0):
"""Get copy of track, with every n-th frame"""
if not self.is_complete():
t = self.get_with_interpolated_history()
else:
t = self
return Track(
t.track_id,
t.history[offset::step_size],
t.predictor_history,
t.predictions,
t.fps/step_size,
self.source,
self.lost,
self.created_at,
self.frame_index,
self.updated_at)
def get_simplified_history(self, distance: float, camera: Camera) -> list[tuple[float, float]]:
# TODO)) Simplify to get a point every n-th meter
# usefull for both predicting and rendering with laser
# raise RuntimeError("Not Implemented Yet")
if len(self.history) < 1:
return []
path = self.get_projected_history(H=None, camera=camera)
new_path: List[dict] = [path[0]]
lengths = np.sqrt(np.sum(np.diff(path, axis=0)**2, axis=1))
cum_lengths = np.cumsum(lengths)
pos = distance
for a, b, l_a, l_b in zip(path[:-1], path[1:], cum_lengths[:-1], cum_lengths[1:]):
# check if segment has our next point (pos)
# because running sequentially, this is if point b
# is lower then our target position
if l_b <= pos:
continue
relative_t = inv_lerp(l_a, l_b, pos)
x = lerp(a[0], b[0], relative_t)
y = lerp(a[1], b[1], relative_t)
new_path.append([x,y])
pos += distance
return new_path
def get_simplified_history_with_absolute_distance(self, distance: float, camera: Camera) -> list[tuple[float, float]]:
# Similar to get_simplified_history, but with absolute world-space distance
# not the distance of the track length
if len(self.history) < 1:
return []
path = self.get_projected_history(H=None, camera=camera)
new_path: List[dict] = [path[0]]
distance_sq = distance**2
for a, b in zip(path[:-1], path[1:]):
# check if segment has our next point (pos)
# because running sequentially, this is if point b
# is lower then our target position
b_distance_sq = ((b[0]-new_path[0])**2 + (b[1]-new_path[1])**2)
if b_distance_sq <= distance_sq:
continue
a_distance_sq = ((a[0]-new_path[0])**2 + (a[1]-new_path[1])**2)
relative_t = inv_lerp(a_distance_sq, b_distance_sq, distance_sq)
x = lerp(a[0], b[0], relative_t)
y = lerp(a[1], b[1], relative_t)
new_path.append([x,y])
return new_path
def get_binned(self, bin_size, camera: Camera, bin_start=True):
"""
For an experiment: what if we predict using only concrete positions, by mapping
dx,dy to a grid. Thus prediction can be for 8 moves, or rather headings
see ~/notes/attachments example svg
"""
history = self.get_projected_history_as_dict(H=None, camera=camera)
def round_to_grid_precision(x):
factor = 1/bin_size
return round(x * factor) / factor
new_history: List[dict] = []
for i, (det0, det1) in enumerate(zip(history[:-1], history[1:])):
if i == 0:
new_history.append({
'x': round_to_grid_precision(det0['x']),
'y': round_to_grid_precision(det0['y'])
} if bin_start else det0)
continue
if abs(det1['x'] - new_history[-1]['x']) < bin_size and abs(det1['y'] - new_history[-1]['y']) < bin_size:
continue
# det1 falls outside of the box [-bin_size:+bin_size] around last detection
# 1. Interpolate exact point between det0 and det1 that this happens
if abs(det1['x'] - new_history[-1]['x']) >= bin_size:
if det1['x'] - new_history[-1]['x'] >= bin_size:
# det1 left of last
x = new_history[-1]['x'] + bin_size
f = inv_lerp(det0['x'], det1['x'], x)
elif new_history[-1]['x'] - det1['x'] >= bin_size:
# det1 left of last
x = new_history[-1]['x'] - bin_size
f = inv_lerp(det0['x'], det1['x'], x)
y = lerp(det0['y'], det1['y'], f)
if abs(det1['y'] - new_history[-1]['y']) >= bin_size:
if det1['y'] - new_history[-1]['y'] >= bin_size:
# det1 left of last
y = new_history[-1]['y'] + bin_size
f = inv_lerp(det0['y'], det1['y'], y)
elif new_history[-1]['y'] - det1['y'] >= bin_size:
# det1 left of last
y = new_history[-1]['y'] - bin_size
f = inv_lerp(det0['y'], det1['y'], y)
x = lerp(det0['x'], det1['x'], f)
# 2. Find closest point on rectangle (rectangle's four corners, or 4 midpoints)
points = get_bins(bin_size)
points = [[new_history[-1]['x']+p[0], new_history[-1]['y'] + p[1]] for p in points]
distances = [np.linalg.norm([p[0] - x, p[1]-y]) for p in points]
closest = np.argmin(distances)
point = points[closest]
new_history.append({'x': point[0], 'y':point[1]})
# todo Offsets to points:[ history for in points]
return new_history
def to_dataframe(self, camera: Camera) -> pd.DataFrame:
positions = self.get_projected_history(None, camera)
velocity = np.gradient(positions, 1/self.fps, axis=0)
acceleration = np.gradient(velocity, 1/self.fps, axis=0)
# # we can calculate heading based on the velocity components
# heading = (np.arctan2(velocity[:,1], velocity[:,0]) * 180 / np.pi) % 360
# # and derive it to get the rate of change of the heading
# d_heading = np.gradient(heading, 1/self.fps, axis=0)
data_columns = pd.MultiIndex.from_product([['position', 'velocity', 'acceleration'], ['x', 'y']])
# data_columns = data_columns.append(pd.MultiIndex.from_tuples([('heading', '°'), ('heading', 'd°')]))
# vx = derivative_of(x, scene.dt)
# vy = derivative_of(y, scene.dt)
# ax = derivative_of(vx, scene.dt)
# ay = derivative_of(vy, scene.dt)
data_dict = {
('position', 'x'): positions[:,0],
('position', 'y'): positions[:,1],
('velocity', 'x'): velocity[:,0],
('velocity', 'y'): velocity[:,1],
('acceleration', 'x'): acceleration[:,0],
('acceleration', 'y'): acceleration[:,1],
# ('heading', '°'): heading,
# ('heading', 'd°'): d_heading,
}
return pd.DataFrame(data_dict, columns=data_columns)
def to_flat_dataframe(self, camera: Camera) -> pd.DataFrame:
positions = self.get_projected_history(None, camera)
data = pd.DataFrame(positions, columns=['x', 'y'])
data['dx'] = data['x'].diff()
data['dy'] = data['y'].diff()
return data.bfill()
def to_trajectron_node(self, camera: Camera, env: Environment) -> Node:
node_data = self.to_dataframe(camera)
new_first_idx = self.history[0].frame_nr
return Node(node_type=env.NodeType.PEDESTRIAN, node_id=self.track_id, data=node_data, first_timestep=new_first_idx)
@dataclass
class Frame:
index: int
img: np.array
time: float= field(default_factory=lambda: time.time())
tracks: Optional[dict[str, Track]] = None
H: Optional[np.array] = None
camera: Optional[Camera] = None
maps: Optional[List[cv2.Mat]] = None
log: dict = field(default_factory=lambda: {}) # settings used during processing. All intermediate nodes can store their config here
def aslist(self) -> List[dict]:
return { t.track_id:
{
'id': t.track_id,
'history': t.get_projected_history(self.H).tolist(),
'det_conf': t.history[-1].conf,
# 'det_conf': trajectory_data[node.id]['det_conf'],
# 'bbox': trajectory_data[node.id]['bbox'],
# 'history': history.tolist(),
'predictions': t.predictions
} for t in self.tracks.values()
}
def without_img(self):
return Frame(self.index, None, self.time, self.tracks, self.H, self.camera, self.maps)
class DataclassJSONEncoder(json.JSONEncoder):
def default(self, o):
if isinstance(o, np.ndarray):
return o.tolist()
# if isinstance(o, np.float32):
# return "float32!{o}"
if dataclasses.is_dataclass(o):
if isinstance(o, Frame):
tracks = {}
for track_id, track in o.tracks.items():
track_obj = dataclasses.asdict(track)
track_obj['history'] = track.get_projected_history(None, o.camera)
tracks[track_id] = track_obj
d = {
'index': o.index,
'time': o.time,
'tracks': tracks,
'camera': dataclasses.asdict(o.camera),
}
else:
d = dataclasses.asdict(o)
# if isinstance(o, Frame):
# # Don't send images over JSON
# del d['img']
return d
return super().default(o)
def video_src_from_config(config) -> Iterable[UrlOrPath]:
"""deprecated, now in video_source"""
if config.video_loop:
video_srcs: Iterable[UrlOrPath] = cycle(config.video_src)
else:
video_srcs: Iterable[UrlOrPath] = config.video_src
return video_srcs
@dataclass
class Trajectory:
# TODO)) Replace history and predictions in Track with Trajectory
space: Space
fps: int = 12
points: List[Detection] = field(default_factory=list)
def __iter__(self):
for d in self.points:
yield d
class HomographyAction(argparse.Action):
def __init__(self, option_strings, dest, nargs=None, **kwargs):
if nargs is not None:
raise ValueError("nargs not allowed")
super().__init__(option_strings, dest, **kwargs)
def __call__(self, parser, namespace, values: Path, option_string=None):
if values.suffix == '.json':
with values.open('r') as fp:
H = np.array(json.load(fp))
else:
H = np.loadtxt(values, delimiter=',')
setattr(namespace, self.dest, values)
setattr(namespace, 'H', H)
class CameraAction(argparse.Action):
def __init__(self, option_strings, dest, nargs=None, **kwargs):
if nargs is not None:
raise ValueError("nargs not allowed")
super().__init__(option_strings, dest, **kwargs)
def __call__(self, parser, namespace, values, option_string=None):
if values is None:
setattr(namespace, self.dest, None)
else:
values = Path(values)
with values.open('r') as fp:
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)
# # print(data['camera_matrix'])
# # camera = {
# # 'camera_matrix': np.array(data['camera_matrix']),
# # 'dist_coeff': np.array(data['dist_coeff']),
# # }
# camera = Camera(np.array(data['camera_matrix']), np.array(data['dist_coeff']), data['dim']['width'], data['dim']['height'], namespace.H, namespace.camera_fps)
setattr(namespace, 'camera', camera)
class LambdaParser(argparse.ArgumentParser):
"""Execute lambda functions
"""
def parse_args(self, args=None, namespace=None):
args = super().parse_args(args, namespace)
for key in vars(args):
f = args.__dict__[key]
if type(f) == types.LambdaType:
print(f'Getting default value for {key}')
args.__dict__[key] = f()
return args

119
trap/config.py
View file

@ -1,15 +1,24 @@
import argparse
from pathlib import Path
import types
import numpy as np
import json
from trap.tracker import DETECTORS, TRACKER_BYTETRACK, TRACKERS
from trap.frame_emitter import Camera
from trap.base import CameraAction, HomographyAction, LambdaParser
from trap.tracker import DETECTORS
from pyparsing import Optional
from trap.frame_emitter import UrlOrPath
class LambdaParser(argparse.ArgumentParser):
"""Execute lambda functions
"""
def parse_args(self, args=None, namespace=None):
args = super().parse_args(args, namespace)
for key in vars(args):
f = args.__dict__[key]
if type(f) == types.LambdaType:
print(f'Getting default value for {key}')
args.__dict__[key] = f()
return args
parser = LambdaParser()
# parser.parse_args()
@ -40,13 +49,6 @@ frame_emitter_parser = parser.add_argument_group('Frame emitter')
tracker_parser = parser.add_argument_group('Tracker')
render_parser = parser.add_argument_group('Renderer')
inference_parser.add_argument("--step-size",
# TODO)) Make dataset/model metadata
help="sample step size (should be the same as for data processing and augmentation)",
type=int,
default=1,
)
inference_parser.add_argument("--model_dir",
help="directory with the model to use for inference",
type=str, # TODO: make into Path
@ -164,20 +166,16 @@ inference_parser.add_argument('--num-samples',
default=5)
inference_parser.add_argument("--full-dist",
help="Trajectron.incremental_forward parameter",
action='store_true')
type=bool,
default=False)
inference_parser.add_argument("--gmm-mode",
help="Trajectron.incremental_forward parameter",
type=bool,
default=True)
inference_parser.add_argument("--z-mode",
help="Trajectron.incremental_forward parameter",
action='store_true')
inference_parser.add_argument('--cm-to-m',
help="Correct for homography that is in cm (i.e. {x,y}/100). Should also be used when processing data",
action='store_true')
inference_parser.add_argument('--center-data',
help="Center data around cx and cy. Should also be used when processing data",
action='store_true')
type=bool,
default=False)
# Internal connections.
@ -186,14 +184,6 @@ connection_parser.add_argument('--zmq-trajectory-addr',
help='Manually specity communication addr for the trajectory messages',
type=str,
default="ipc:///tmp/feeds_traj")
connection_parser.add_argument('--zmq-face-addr',
help='Manually specity communication addr for the face detector messages',
type=str,
default="ipc:///tmp/feeds_faces")
connection_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")
connection_parser.add_argument('--zmq-camera-stream-addr',
help='Manually specity communication addr for the camera stream messages',
@ -208,12 +198,7 @@ connection_parser.add_argument('--zmq-prediction-addr',
connection_parser.add_argument('--zmq-frame-addr',
help='Manually specity communication addr for the frame messages',
type=str,
default="ipc:///tmp/feeds_frame")
connection_parser.add_argument('--zmq-frame-noimg-addr',
help='Manually specity communication addr for the frame messages',
type=str,
default="ipc:///tmp/feeds_frame2")
default="ipc:///tmp/feeds_frame")
connection_parser.add_argument('--ws-port',
@ -228,18 +213,14 @@ connection_parser.add_argument('--bypass-prediction',
# Frame emitter
frame_emitter_parser.add_argument("--video-src",
help="source video to track from can be either a relative or absolute path, or a url, like an RTSP resource",
type=UrlOrPath,
help="source video to track from",
type=Path,
nargs='+',
default=lambda: [UrlOrPath(p) for p in Path('../DATASETS/VIRAT_subset_0102x/').glob('*.mp4')])
default=lambda: list(Path('../DATASETS/VIRAT_subset_0102x/').glob('*.mp4')))
frame_emitter_parser.add_argument("--video-offset",
help="Start playback from given frame. Note that when src is an array, this applies to all videos individually.",
default=None,
type=int)
frame_emitter_parser.add_argument("--video-end",
help="End (or loop) playback at given frame.",
default=None,
type=int)
#TODO: camera as source
frame_emitter_parser.add_argument("--video-loop",
@ -248,23 +229,12 @@ frame_emitter_parser.add_argument("--video-loop",
#TODO: camera as source
# Tracker
tracker_parser.add_argument("--camera-fps",
help="Camera FPS",
type=int,
default=12)
tracker_parser.add_argument("--homography",
help="File with homography params",
type=Path,
default='../DATASETS/VIRAT_subset_0102x/VIRAT_0102_homography_img2world.txt',
action=HomographyAction)
tracker_parser.add_argument("--calibration",
help="File with camera intrinsics and lens distortion params (calibration.json)",
# type=Path,
default=None,
action=CameraAction)
# Tracker
default='../DATASETS/VIRAT_subset_0102x/VIRAT_0102_homography_img2world.txt')
tracker_parser.add_argument("--save-for-training",
help="Specify the path in which to save",
type=Path,
@ -273,29 +243,11 @@ tracker_parser.add_argument("--detector",
help="Specify the detector to use",
type=str,
choices=DETECTORS)
tracker_parser.add_argument("--tracker",
help="Specify the detector to use",
type=str,
default=TRACKER_BYTETRACK,
choices=TRACKERS)
tracker_parser.add_argument("--smooth-tracks",
help="Smooth the tracker tracks before sending them to the predictor",
action='store_true')
# now in calibration.json
# tracker_parser.add_argument("--frame-width",
# help="width of the frames",
# type=int,
# default=1280)
# tracker_parser.add_argument("--frame-height",
# help="height of the frames",
# type=int,
# default=720)
# Renderer
# render_parser.add_argument("--disable-renderer",
# help="Disable the renderer all together. Usefull when using an external renderer",
# action="store_true")
render_parser.add_argument("--render-file",
help="Render a video file previewing the prediction, and its delay compared to the current frame",
@ -303,24 +255,9 @@ render_parser.add_argument("--render-file",
render_parser.add_argument("--render-window",
help="Render a previewing to a window",
action='store_true')
render_parser.add_argument("--render-animation",
help="Render animation (pyglet)",
action='store_true')
render_parser.add_argument("--render-laser",
help="Render laser (Helios DAC)",
action='store_true')
render_parser.add_argument("--render-debug-shapes",
help="Lines and points for debugging/mapping",
action='store_true')
render_parser.add_argument("--render-hide-stats",
help="Default toggle to hide (switch with 'h')",
action='store_true')
render_parser.add_argument("--full-screen",
help="Set Window full screen",
action='store_true')
render_parser.add_argument("--render-clusters",
help="renders arrowd clusters instead of individual predictions",
action='store_true')
render_parser.add_argument("--render-url",
help="""Stream renderer on given URL. Two easy approaches:
@ -332,9 +269,3 @@ render_parser.add_argument("--render-url",
type=str,
default=None)
render_parser.add_argument("--debug-points-file",
help="A json file with points to test projection/homography etc.",
type=Path,
required=False,
)

124
trap/counter.py
View file

@ -1,124 +0,0 @@
import collections
from gc import is_finalized
import logging
import statistics
import threading
import time
from typing import MutableSequence
import zmq
logger = logging.getLogger('counter')
class CounterSender:
def __init__(self, address = "ipc:///tmp/trap-counters2"):
# self.name = name
self.context = zmq.Context()
self.sock = self.context.socket(zmq.PUB)
self.sock.setsockopt(zmq.CONFLATE, 1) # only keep latest frame
# self.sock.sndhwm = 1
self.sock.connect(address)
def set(self, name:str, value:float):
try:
# we cannot use send_multipart in combination with conflate
self.sock.send_pyobj([name, value], flags=zmq.NOBLOCK)
except zmq.ZMQError as e:
logger.warning(f"No space in que to count {name} as {value}")
class CounterFpsSender():
def __init__(self, name:str , sender: CounterSender):
self.name = name
self.sender = sender
self.tocs: MutableSequence[(float, int)] = collections.deque(maxlen=5)
self.iterations: int = 0
# threading.Event.wait()
# TODO thread to daeomic loop so it automatically stops
self.thread = threading.Thread(target=self.interval, daemon=True)
self.is_finished = threading.Event()
def tick(self):
"""
returns dt since previous tock
"""
self.iterations += 1
self.snapshot()
if len(self.tocs) > 1:
return float(self.tocs[-1][0] - self.tocs[-2][0])
else:
return 0.
def snapshot(self):
self.tocs.append((time.perf_counter(), self.iterations))
self.sender.set(self.name, self.fps)
@property
def fps(self):
if len(self.tocs) < 2:
return 0
dt = self.tocs[-1][0] - self.tocs[0][0]
di = self.tocs[-1][1] - self.tocs[0][1]
return di/dt
def interval(self):
while True:
self.is_finished.wait(.5)
if self.is_finished.is_set():
break
self.snapshot()
# timer = threading.Timer(.5, self.interval)
# timer.start()
class CounterLog():
def __init__(self, history = 20):
self.history: MutableSequence[(float, float)] = collections.deque(maxlen=history)
def add(self, value):
self.history.append((time.perf_counter(), value))
def value(self):
return self.history[-1][1]
def has_value(self):
if not len(self.history):
return False
if (time.perf_counter() - self.history[-1][0]) > 4:
# no update in 4s: very slow. Dead thread?
return False
return True
def avg(self):
if not len(self.history):
return 0.
return statistics.fmean([h[1] for h in self.history])
class CounterListerner():
def __init__(self, address = "ipc:///tmp/trap-counters2"):
self.context = zmq.Context()
self.sock = self.context.socket(zmq.SUB)
self.sock.bind(address)
self.sock.subscribe( b'')
self.values: collections.defaultdict[str, CounterLog] = collections.defaultdict(lambda: CounterLog())
def snapshot(self):
messages = []
while self.sock.poll(0) == zmq.POLLIN:
msg = self.sock.recv_pyobj()
# print(msg)
name, value = msg
# name, value = name.decode('utf8'),float(value.decode('utf8'))
self.values[name].add(float(value))
def get_latest(self):
self.snapshot()
return self.values
def to_string(self):
strs = [(f"{k}: {v.value():.2f} ({v.avg():.2f})" if v.has_value() else f"{k}: --") for (k,v) in self.values.items()]
return " ".join(strs)

473
trap/cv_renderer.py
View file

@ -1,473 +0,0 @@
# used for "Forward Referencing of type annotations"
from __future__ import annotations
import datetime
import json
import logging
from pathlib import Path
import time
from argparse import ArgumentParser, Namespace
from multiprocessing.synchronize import Event as BaseEvent
from typing import Dict, List, Optional
from charset_normalizer import detect
import cv2
import ffmpeg
import numpy as np
import pyglet
import zmq
from pyglet import shapes
from trap.base import Detection, UndistortedCamera
from trap.counter import CounterListerner
from trap.frame_emitter import Frame, Track
from trap.lines import load_lines_from_svg
from trap.node import Node
from trap.preview_renderer import FrameWriter
from trap.tools import draw_track_predictions, draw_track_projected, to_point
from trap.utils import convert_world_points_to_img_points
logger = logging.getLogger("trap.simple_renderer")
class CvRenderer(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.H = self.config.H
# self.inv_H = np.linalg.pinv(self.H)
# TODO: get FPS from frame_emitter
# self.out = cv2.VideoWriter(str(filename), fourcc, 23.97, (1280,720))
self.fps = 60
self.frame_size = None # configure on first frame recv
# self.frame_size = (self.config.camera.projected_w,self.config.camera.projected_h)
self.out_writer = self.start_writer() if self.config.render_file else None
self.streaming_process = self.start_streaming() if self.config.render_url else None
self.first_time: float|None = None
self.frame: Frame|None= None
self.tracker_frame: Frame|None = None
self.prediction_frame: Frame|None = None
self.detections: List[Detection]|None = None
self.tracks: Dict[str, Track] = {}
self.predictions: Dict[str, Track] = {}
self.scale = 100
self.debug_lines = debug_lines = load_lines_from_svg(self.config.debug_map, self.scale, '') if self.config.debug_map else []
def refresh_labels(self, dt: float):
"""Every frame"""
if self.frame:
self.labels['frame_idx'].text = f"{self.frame.index:06d}"
self.labels['frame_time'].text = f"{self.frame.time - self.first_time: >10.2f}s"
self.labels['frame_latency'].text = f"{self.frame.time - time.time():.2f}s"
if self.tracker_frame:
self.labels['tracker_idx'].text = f"{self.tracker_frame.index - self.frame.index}"
self.labels['tracker_time'].text = f"{self.tracker_frame.time - time.time():.3f}s"
self.labels['track_len'].text = f"{len(self.tracker_frame.tracks)} tracks"
if self.prediction_frame:
self.labels['pred_idx'].text = f"{self.prediction_frame.index - self.frame.index}"
self.labels['pred_time'].text = f"{self.prediction_frame.time - time.time():.3f}s"
# self.labels['track_len'].text = f"{len(self.prediction_frame.tracks)} tracks"
def start_writer(self):
if not self.config.output_dir.exists():
raise FileNotFoundError("Path does not exist")
date_str = datetime.datetime.now().isoformat(timespec="minutes")
filename = self.config.output_dir / f"render_predictions-{date_str}-{self.config.detector}.mp4"
logger.info(f"Write to {filename}")
return FrameWriter(str(filename), self.fps, None)
# fourcc = cv2.VideoWriter_fourcc(*'vp09')
# return cv2.VideoWriter(str(filename), fourcc, self.fps, self.frame_size)
def start_streaming(self, frame_size=(1920,1080)):
return (
ffmpeg
.input('pipe:', format='rawvideo',codec="rawvideo", pix_fmt='bgr24', s='{}x{}'.format(*frame_size))
.output(
self.config.render_url,
#codec = "copy", # use same codecs of the original video
codec='libx264',
listen=1, # enables HTTP server
pix_fmt="yuv420p",
preset="ultrafast",
tune="zerolatency",
# g=f"{self.fps*2}",
g=f"{60*2}",
analyzeduration="2000000",
probesize="1000000",
f='mpegts'
)
.overwrite_output()
.run_async(pipe_stdin=True)
)
# return process
def run(self):
self.frame = None
prediction_frame = None
tracker_frame = None
i=0
first_time = None
cv2.namedWindow("frame", cv2.WINDOW_NORMAL)
# https://gist.github.com/ronekko/dc3747211543165108b11073f929b85e
cv2.moveWindow("frame", 0, -1)
if self.config.full_screen:
cv2.setWindowProperty("frame",cv2.WND_PROP_FULLSCREEN,cv2.WINDOW_FULLSCREEN)
cv2.setMouseCallback('frame',self.click_print_position)
# bgsub = cv2.createBackgroundSubtractorMOG2(120, 50, detectShadows=True)
while self.run_loop():
i += 1
# zmq_ev = self.frame_sock.poll(timeout=2000)
# if not zmq_ev:
# # when no data comes in, loop so that is_running is checked
# continue
try:
self.frame: Frame = self.frame_sock.recv_pyobj(zmq.NOBLOCK)
except zmq.ZMQError as e:
# idx = frame.index if frame else "NONE"
# logger.debug(f"reuse video frame {idx}")
pass
# else:
# logger.debug(f'new video frame {frame.index}')
if self.frame is None and i < 100:
# 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)
for track_id, track in prediction_frame.tracks.items():
prediction_id = f"{track_id}-{track.history[-1].frame_nr}"
self.predictions[prediction_id] = track
except zmq.ZMQError as e:
logger.debug(f'reuse prediction')
try:
tracker_frame: Frame = self.tracker_sock.recv_pyobj(zmq.NOBLOCK)
for track_id, track in tracker_frame.tracks.items():
self.tracks[track_id] = track
except zmq.ZMQError as e:
logger.debug(f'reuse tracks')
try:
self.detections = self.detector_sock.recv_pyobj(zmq.NOBLOCK)
# print('detections')
except zmq.ZMQError as e:
# print('no detections')
# idx = frame.index if frame else "NONE"
# logger.debug(f"reuse video frame {idx}")
pass
if first_time is None:
first_time = self.frame.time
# img = frame.img
# save_file = Path("videos/snap.png")
# if not save_file.exists():
# img = frame.camera.img_to_world(frame.img, 100)
# cv2.imwrite(save_file, img)
img = decorate_frame(self.frame, tracker_frame, prediction_frame, first_time, self.config, self.tracks, self.predictions, self.detections, self.config.render_clusters, self.debug_lines, self.scale)
logger.debug(f"write frame {self.frame.time - first_time:.3f}s")
if self.out_writer:
self.out_writer.write(img)
if self.streaming_process:
self.streaming_process.stdin.write(img.tobytes())
if not self.config.no_window:
cv2.imshow('frame',cv2.resize(img, (1920, 1080)))
# cv2.imshow('frame',img)
cv2.waitKey(10)
# clear out old tracks & predictions:
for track_id, track in list(self.tracks.items()):
if get_animation_position(track, self.frame) == 1:
self.tracks.pop(track_id)
for prediction_id, track in list(self.predictions.items()):
if get_animation_position(track, self.frame) == 1:
self.predictions.pop(prediction_id)
logger.info('Stopping')
# if i>2:
if self.streaming_process:
self.streaming_process.stdin.close()
if self.out_writer:
self.out_writer.release()
if self.streaming_process:
# oddly wrapped, because both close and release() take time.
logger.info('wait for closing stream')
self.streaming_process.wait()
logger.info('stopped')
@classmethod
def arg_parser(cls):
render_parser = ArgumentParser()
render_parser.add_argument('--zmq-frame-addr',
help='Manually specity communication addr for the frame messages',
type=str,
default="ipc:///tmp/feeds_frame")
render_parser.add_argument('--zmq-trajectory-addr',
help='Manually specity communication addr for the trajectory messages',
type=str,
default="ipc:///tmp/feeds_traj")
render_parser.add_argument('--zmq-detection-addr',
help='Manually specity communication addr for the detection messages',
type=str,
default="ipc:///tmp/feeds_dets")
render_parser.add_argument('--zmq-prediction-addr',
help='Manually specity communication addr for the prediction messages',
type=str,
default="ipc:///tmp/feeds_preds")
render_parser.add_argument("--render-file",
help="Render a video file previewing the prediction, and its delay compared to the current frame",
action='store_true')
render_parser.add_argument("--no-window",
help="Disable a previewing to a window",
action='store_true')
render_parser.add_argument("--full-screen",
help="Set Window full screen",
action='store_true')
render_parser.add_argument("--render-clusters",
help="renders arrowd clusters instead of individual predictions",
action='store_true')
render_parser.add_argument("--render-url",
help="""Stream renderer on given URL. Two easy approaches:
- using zmq wrapper one can specify the LISTENING ip. To listen to any incoming connection: zmq:tcp://0.0.0.0:5556
- alternatively, using e.g. UDP one needs to specify the IP of the client. E.g. udp://100.69.123.91:5556/stream
Note that with ZMQ you can have multiple clients connecting simultaneously. E.g. using `ffplay zmq:tcp://100.109.175.82:5556`
When using udp, connecting can be done using `ffplay udp://100.109.175.82:5556/stream`
""",
type=str,
default=None)
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")
return render_parser
def click_print_position(self, event,x,y,flags,param):
# if event == cv2.EVENT_LBUTTONDBLCLK:
if event == cv2.EVENT_LBUTTONUP:
if not self.frame:
return
scale = 100
print("click position:", x/scale, y/scale)
# self.frame.camera.points_img_to_world([[x, y]], 1)
# cv2.circle(img,(x,y),100,(255,0,0),-1)
mouseX,mouseY = x,y
# colorset = itertools.product([0,255], repeat=3) # but remove white
# colorset = [(0, 0, 0),
# (0, 0, 255),
# (0, 255, 0),
# (0, 255, 255),
# (255, 0, 0),
# (255, 0, 255),
# (255, 255, 0)
# ]
colorset = [
(255,255,100),
(255,100,255),
(100,255,255),
]
# colorset = [
# (0,0,0),
# ]
def get_animation_position(track: Track, current_frame: Frame):
fade_duration = current_frame.camera.fps * 3
diff = current_frame.index - track.history[-1].frame_nr
return max(0, min(1, diff / fade_duration))
# track.history[-1].frame_nr < (current_frame.index - current_frame.camera.fps * 3)
# track.history[-1].frame_nr < (current_frame.index - current_frame.camera.fps * 3)
def decorate_frame(frame: Frame, tracker_frame: Frame, prediction_frame: Frame, first_time: float, config: Namespace, tracks: Dict[str, Track], predictions: Dict[str, Track], detections: Optional[List[Detection]], as_clusters = True, debug_lines = [], scale: float = 100) -> np.array:
# TODO: replace opencv with QPainter to support alpha? https://doc.qt.io/qtforpython-5/PySide2/QtGui/QPainter.html#PySide2.QtGui.PySide2.QtGui.QPainter.drawImage
# or https://github.com/pygobject/pycairo?tab=readme-ov-file
# or https://pyglet.readthedocs.io/en/latest/programming_guide/shapes.html
# and use http://code.astraw.com/projects/motmot/pygarrayimage.html or https://gist.github.com/nkymut/1cb40ea6ae4de0cf9ded7332f1ca0d55
# or https://api.arcade.academy/en/stable/index.html (supports gradient color in line -- "Arcade is built on top of Pyglet and OpenGL.")
dst_img = frame.camera.img_to_world(frame.img, scale)
# mask = bg_subtractor.apply(dst_img)
# mask = cv2.cvtColor(mask, cv2.COLOR_GRAY2RGB).astype(float) / 255
# dst_img = dst_img * mask
# undistorted_img = cv2.undistort(frame.img, config.camera.mtx, config.camera.dist, None, config.camera.newcameramtx)
# dst_img = cv2.warpPerspective(undistorted_img,convert_world_space_to_img_space(config.camera.H),(config.camera.w,config.camera.h))
# dst_img2 = cv2.warpPerspective(undistorted_img,convert_world_space_to_img_space(config.camera.H), None)
# cv2.imwrite('/home/ruben/suspicion/DATASETS/hof3/camera2.png', dst_img2)
overlay = np.zeros(dst_img.shape, np.uint8)
# Fill image with red color(set each pixel to red)
overlay[:] = (0, 0, 0)
# img = cv2.addWeighted(dst_img, .2, overlay, .3, 0)
img = dst_img.copy()
# all not working:
# if i == 1:
# # thanks to GpG for fixing scaling issue: https://stackoverflow.com/a/39668864
# scale_factor = 1./20 # from 10m to 1000px
# S = np.array([[scale_factor, 0,0],[0,scale_factor,0 ],[ 0,0,1 ]])
# new_H = S * self.H * np.linalg.inv(S)
# warpedFrame = cv2.warpPerspective(img, new_H, (1000,1000))
# cv2.imwrite(str(self.config.output_dir / "orig.png"), warpedFrame)
cv2.rectangle(img, (0,0), (img.shape[1],25), (0,0,0), -1)
if detections:
for detection in detections:
points = [
detection.get_foot_coords(),
[detection.l, detection.t],
[detection.l + detection.w, detection.t + detection.h],
]
points = tracker_frame.camera.points_img_to_world(points, scale)
points = [to_point(p) for p in points] # to int
w = points[1][0]-points[2][0]
feet = [int(points[2][0] + .5 * w), points[2][1]]
cv2.rectangle(img, points[1], points[2], (255,255,0), 2)
cv2.circle(img, points[0], 5, (255,255,0), 2)
cv2.putText(img, f"{detection.conf:.02f}", (points[0][0], points[0][1]+20), cv2.FONT_HERSHEY_PLAIN, 1, (255,255,0), 1)
def conversion(points):
return convert_world_points_to_img_points(points, scale)
if not tracker_frame:
cv2.putText(img, f"and track", (650,17), cv2.FONT_HERSHEY_PLAIN, 1, (255,255,0), 1)
else:
for track_id, track in tracks.items():
inv_H = np.linalg.pinv(tracker_frame.H)
draw_track_projected(img, track, int(track_id), tracker_frame.camera, conversion)
for line in debug_lines:
for rp1, rp2 in zip(line.points, line.points[1:]):
p1 = (
int(rp1.position[0]*scale),
int(rp1.position[1]*scale),
)
p2 = (
int(rp2.position[0]*scale),
int(rp2.position[1]*scale),
)
cv2.line(img, p1, p2, (255,0,0), 2)
# points = [(int(point[0]*scale), int(point[1]*scale)) for point in points]
# for num, points in enumerate(frame.camera.debug_lines):
# cv2.line(img, points[0], points[1], (255,0,0), 2)
# if hasattr(frame.camera, 'debug_points'):
# for num, point in enumerate(frame.camera.debug_points):
# cv2.circle(img, (int(point[0]*scale), int(point[1]*scale)), 5, (255,0,0), 2)
# cv2.putText(img, f"{num}", (int(point[0]*scale)+20, int(point[1]*scale)), cv2.FONT_HERSHEY_PLAIN, 1, (255,0,0), 1)
if not prediction_frame:
cv2.putText(img, f"Waiting for prediction...", (500,17), cv2.FONT_HERSHEY_PLAIN, 1, (255,255,0), 1)
# continue
else:
for track_id, track in predictions.items():
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)
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):
map_img = np.ascontiguousarray(np.flipud(np.transpose(m[0], (2, 1, 0))*255), np.uint8)
cv2.circle(map_img, (10,50), 5, (0,255,0), 2)
cv2.line(map_img, (10,50), (10+15, 50), (0,0,255), 2)
cv2.rectangle(map_img, (0,0), (map_img.shape[1]-1, map_img.shape[0]-1), (255,255,255), 1)
height, width, _ = map_img.shape
padding= 50
y = img.shape[0] - padding - height
x = width*i
if x+width > img.shape[1]:
break # stop drawing maps when there's a lot of them
img[y:y+height,x:x+width] = map_img
base_color = (255,)*3
info_color = (255,255,0)
predictor_color = (255,0,255)
tracker_color = (0,255,255)
cv2.putText(img, f"{frame.index:06d}", (20,17), cv2.FONT_HERSHEY_PLAIN, 1, base_color, 1)
cv2.putText(img, f"{frame.time - first_time: >10.2f}s", (150,17), cv2.FONT_HERSHEY_PLAIN, 1, base_color, 1)
cv2.putText(img, f"{frame.time - time.time():.2f}s", (250,17), cv2.FONT_HERSHEY_PLAIN, 1, base_color, 1)
options = []
if prediction_frame:
# render Δt and Δ frames
cv2.putText(img, f"{tracker_frame.index - frame.index}", (90,17), cv2.FONT_HERSHEY_PLAIN, 1, tracker_color, 1)
cv2.putText(img, f"{prediction_frame.index - frame.index}", (120,17), cv2.FONT_HERSHEY_PLAIN, 1, predictor_color, 1)
cv2.putText(img, f"{tracker_frame.time - time.time():.2f}s", (310,17), cv2.FONT_HERSHEY_PLAIN, 1, tracker_color, 1)
cv2.putText(img, f"{prediction_frame.time - time.time():.2f}s", (380,17), cv2.FONT_HERSHEY_PLAIN, 1, predictor_color, 1)
cv2.putText(img, f"{len(tracker_frame.tracks)} tracks", (620,17), cv2.FONT_HERSHEY_PLAIN, 1, base_color, 1)
cv2.putText(img, f"h: {np.average([len(t.history or []) for t in prediction_frame.tracks.values()]):.2f}", (700,17), cv2.FONT_HERSHEY_PLAIN, 1, tracker_color, 1)
cv2.putText(img, f"ph: {np.average([len(t.predictor_history or []) for t in prediction_frame.tracks.values()]):.2f}", (780,17), cv2.FONT_HERSHEY_PLAIN, 1, predictor_color, 1)
cv2.putText(img, f"p: {np.average([len(t.predictions or []) for t in prediction_frame.tracks.values()]):.2f}", (860,17), cv2.FONT_HERSHEY_PLAIN, 1, predictor_color, 1)
for option, value in prediction_frame.log['predictor'].items():
options.append(f"{option}: {value}")
if len(options):
cv2.putText(img, options.pop(-1), (20,img.shape[0]-30), cv2.FONT_HERSHEY_PLAIN, 1, base_color, 1)
cv2.putText(img, " | ".join(options), (20,img.shape[0]-10), cv2.FONT_HERSHEY_PLAIN, 1, base_color, 1)
return img
def run_cv_renderer(config: Namespace, is_running: BaseEvent, timer_counter):
renderer = CvRenderer(config, is_running)
renderer.run(timer_counter)

170
trap/face_detector.py
View file

@ -1,170 +0,0 @@
from argparse import Namespace
from collections import defaultdict
import csv
from dataclasses import dataclass, field
import json
import logging
from math import nan
from multiprocessing import Event
import multiprocessing
from pathlib import Path
import pickle
import time
from typing import DefaultDict, Dict, Optional, List
import jsonlines
import numpy as np
import torch
import torchvision
import ultralytics
import zmq
import cv2
from facenet_pytorch import InceptionResnetV1, MTCNN
from trap.base import Frame
logger = logging.getLogger('trap.face_detector')
class FaceDetector:
def __init__(self, config: Namespace):
self.config = config
self.context = zmq.Context()
self.frame_sock = self.context.socket(zmq.SUB)
self.frame_sock.setsockopt(zmq.CONFLATE, 1) # only keep latest frame. NB. make sure this comes BEFORE connect, otherwise it's ignored!!
self.frame_sock.setsockopt(zmq.SUBSCRIBE, b'')
self.frame_sock.connect(self.config.zmq_frame_addr)
self.face_socket = self.context.socket(zmq.PUB)
self.face_socket.setsockopt(zmq.CONFLATE, 1) # only keep latest frame
self.face_socket.bind(self.config.zmq_face_addr)
# # TODO: config device
self.device = torch.device("cuda" if torch.cuda.is_available() else "cpu")
def track(self, is_running: Event, timer_counter: int = 0):
"""
Live tracking of frames coming in over zmq
"""
self.is_running = is_running
prev_frame_i = -1
# For a model pretrained on CASIA-Webface
# model = InceptionResnetV1(pretrained='casia-webface').eval().to(self.device)
# mtcnn = MTCNN(
# image_size=160, margin=0, min_face_size=10,
# thresholds=[0.3, 0.3, 0.3], factor=0.709, post_process=True,
# device=self.device, keep_all=True
# )
# modelpath = Path("face_detection_yunet_2023mar_int8bq.onnx")
modelpath = Path("face_detection_yunet_2023mar_int8.onnx")
# model = YuNet(modelPath=args.model,
# inputSize=[320, 320],
# confThreshold=args.conf_threshold,
# nmsThreshold=args.nms_threshold,
# topK=args.top_k,
# backendId=backend_id,
# targetId=target_id)
detector = cv2.FaceDetectorYN.create(
str(modelpath),
"",
(320, 320),
.3,
.3,
5000,
cv2.dnn.DNN_BACKEND_CUDA,
target_id=cv2.dnn.DNN_TARGET_CUDA
)
while self.is_running.is_set():
with timer_counter.get_lock():
timer_counter.value += 1
poll_time = time.time()
zmq_ev = self.frame_sock.poll(timeout=2000)
if not zmq_ev:
logger.warning('skip poll after 2000ms')
# when there's no data after timeout, loop so that is_running is checked
continue
start_time = time.time()
frame: Frame = self.frame_sock.recv_pyobj() # frame delivery in current setup: 0.012-0.03s
# print(time.time()- frame.time)
if frame.index > (prev_frame_i+1):
logger.warning(f"Dropped {frame.index - prev_frame_i - 1} frames ({frame.index=}, {prev_frame_i=}) -- poll time {start_time-poll_time:.5f}")
height, width, channels = frame.img.shape
detector.setInputSize((width//2, height//2))
img = cv2.resize(frame.img, (width//2, height//2))
faces = detector.detect(img)
prev_frame_i = frame.index
# print(f"send to {self.trajectory_socket}, {self.config.zmq_trajectory_addr}")
self.face_socket.send_pyobj(faces) # ditch image for faster passthrough
logger.info('Stopping')
def run_detector(config: Namespace, is_running: Event, timer_counter):
router = FaceDetector(config)
router.track(is_running, timer_counter)
def run():
# Frame emitter
import argparse
argparser = argparse.ArgumentParser()
argparser.add_argument('--zmq-frame-addr',
help='Manually specity communication addr for the frame messages',
type=str,
default="ipc:///tmp/feeds_frame")
argparser.add_argument('--zmq-trajectory-addr',
help='Manually specity communication addr for the trajectory messages',
type=str,
default="ipc:///tmp/feeds_traj")
argparser.add_argument("--save-for-training",
help="Specify the path in which to save",
type=Path,
default=None)
argparser.add_argument("--detector",
help="Specify the detector to use",
type=str,
default=DETECTOR_YOLOv8,
choices=DETECTORS)
argparser.add_argument("--tracker",
help="Specify the detector to use",
type=str,
default=TRACKER_BYTETRACK,
choices=TRACKERS)
argparser.add_argument("--smooth-tracks",
help="Smooth the tracker tracks before sending them to the predictor",
action='store_true')
config = argparser.parse_args()
is_running = multiprocessing.Event()
is_running.set()
timer_counter = timer.Timer('frame_emitter')
router = Tracker(config)
router.track(is_running, timer_counter.iterations)
is_running.clear()

295
trap/frame_emitter.py
View file

@ -1,128 +1,227 @@
from __future__ import annotations
from argparse import Namespace
from dataclasses import dataclass, field
from enum import IntFlag
from itertools import cycle
import logging
import pickle
from argparse import ArgumentParser, Namespace
from multiprocessing import Event
from pathlib import Path
from trap import node
from trap.base import *
from trap.base import LambdaParser
from trap.gemma import ImgMovementFilter
from trap.preview_renderer import FrameWriter
from trap.video_sources import get_video_source
import pickle
import sys
import time
from typing import Iterable, Optional
import numpy as np
import cv2
import zmq
from deep_sort_realtime.deep_sort.track import Track as DeepsortTrack
from deep_sort_realtime.deep_sort.track import TrackState as DeepsortTrackState
logger = logging.getLogger('trap.frame_emitter')
class FrameEmitter(node.Node):
class DetectionState(IntFlag):
Tentative = 1 # state before n_init (see DeepsortTrack)
Confirmed = 2 # after tentative
Lost = 4 # lost when DeepsortTrack.time_since_update > 0 but not Deleted
@classmethod
def from_deepsort_track(cls, track: DeepsortTrack):
if track.state == DeepsortTrackState.Tentative:
return cls.Tentative
if track.state == DeepsortTrackState.Confirmed:
if track.time_since_update > 0:
return cls.Lost
return cls.Confirmed
raise RuntimeError("Should not run into Deleted entries here")
@dataclass
class Detection:
track_id: str # deepsort track id association
l: int # left - image space
t: int # top - image space
w: int # width - image space
h: int # height - image space
conf: float # object detector probablity
state: DetectionState
frame_nr: int
def get_foot_coords(self) -> list[tuple[float, float]]:
return [self.l + 0.5 * self.w, self.t+self.h]
@classmethod
def from_deepsort(cls, dstrack: DeepsortTrack):
return cls(dstrack.track_id, *dstrack.to_ltwh(), dstrack.det_conf, DetectionState.from_deepsort_track(dstrack))
def get_scaled(self, scale: float = 1):
if scale == 1:
return self
return Detection(
self.track_id,
self.l*scale,
self.t*scale,
self.w*scale,
self.h*scale,
self.conf,
self.state)
def to_ltwh(self):
return (int(self.l), int(self.t), int(self.w), int(self.h))
def to_ltrb(self):
return (int(self.l), int(self.t), int(self.l+self.w), int(self.t+self.h))
@dataclass
class Track:
"""A bit of an haphazardous wrapper around the 'real' tracker to provide
a history, with which the predictor can work, as we then can deduce velocity
and acceleration.
"""
track_id: str = None
history: [Detection] = field(default_factory=lambda: [])
predictor_history: Optional[list] = None # in image space
predictions: Optional[list] = None
def get_projected_history(self, H) -> np.array:
foot_coordinates = [d.get_foot_coords() for d in self.history]
if len(foot_coordinates):
coords = cv2.perspectiveTransform(np.array([foot_coordinates]),H)
return coords[0]
return np.array([])
def get_projected_history_as_dict(self, H) -> dict:
coords = self.get_projected_history(H)
return [{"x":c[0], "y":c[1]} for c in coords]
@dataclass
class Frame:
index: int
img: np.array
time: float= field(default_factory=lambda: time.time())
tracks: Optional[dict[str, Track]] = None
H: Optional[np.array] = None
def aslist(self) -> [dict]:
return { t.track_id:
{
'id': t.track_id,
'history': t.get_projected_history(self.H).tolist(),
'det_conf': t.history[-1].conf,
# 'det_conf': trajectory_data[node.id]['det_conf'],
# 'bbox': trajectory_data[node.id]['bbox'],
# 'history': history.tolist(),
'predictions': t.predictions
} for t in self.tracks.values()
}
class FrameEmitter:
'''
Emit frame in a separate threat so they can be throttled,
or thrown away when the rest of the system cannot keep up
'''
def setup(self) -> None:
self.frame_sock = self.pub(self.config.zmq_frame_addr)
self.frame_noimg_sock = self.pub(self.config.zmq_frame_noimg_addr)
def __init__(self, config: Namespace, is_running: Event) -> None:
self.config = config
self.is_running = is_running
context = zmq.Context()
# TODO: to make things faster, a multiprocessing.Array might be a tad faster: https://stackoverflow.com/a/65201859
self.frame_sock = context.socket(zmq.PUB)
self.frame_sock.setsockopt(zmq.CONFLATE, 1) # only keep latest frame. make sure to set BEFORE connect/bind
self.frame_sock.bind(config.zmq_frame_addr)
logger.info(f"Connection socket {self.config.zmq_frame_addr}")
logger.info(f"Connection socket {self.config.zmq_frame_noimg_addr}")
logger.info(f"Connection socket {config.zmq_frame_addr}")
self.video_srcs = self.config.video_src
if self.config.video_loop:
self.video_srcs: Iterable[Path] = cycle(self.config.video_src)
else:
self.video_srcs: [Path] = self.config.video_src
def run(self):
offset = int(self.config.video_offset or 0)
source = get_video_source(self.video_srcs, self.config.camera, offset, self.config.video_end, self.config.video_loop)
video_gen = enumerate(source, start = offset)
def emit_video(self):
i = 0
for video_path in self.video_srcs:
logger.info(f"Play from '{str(video_path)}'")
if str(video_path).isdigit():
# numeric input is a CV camera
video = cv2.VideoCapture(int(str(video_path)))
# TODO: make config variables
video.set(cv2.CAP_PROP_FRAME_WIDTH, int(1280))
video.set(cv2.CAP_PROP_FRAME_HEIGHT, int(720))
print("exposure!", video.get(cv2.CAP_PROP_AUTO_EXPOSURE))
video.set(cv2.CAP_PROP_FPS, 5)
else:
video = cv2.VideoCapture(str(video_path))
fps = video.get(cv2.CAP_PROP_FPS)
target_frame_duration = 1./fps
logger.info(f"Emit frames at {fps} fps")
# writer = FrameWriter(self.config.record, None, None) if self.config.record else nullcontext
writer = FrameWriter(str(self.config.record), None, None) if self.config.record else None
try:
processor = ImgMovementFilter()
while self.run_loop():
if self.config.video_offset:
logger.info(f"Start at frame {self.config.video_offset}")
video.set(cv2.CAP_PROP_POS_FRAMES, self.config.video_offset)
i = self.config.video_offset
try:
i, img = next(video_gen)
except StopIteration as e:
logger.info("Video source ended")
if '-' in video_path.stem:
path_stem = video_path.stem[:video_path.stem.rfind('-')]
else:
path_stem = video_path.stem
path_stem += "-homography"
homography_path = video_path.with_stem(path_stem).with_suffix('.txt')
logger.info(f'check homography file {homography_path}')
if homography_path.exists():
logger.info(f'Found custom homography file! Using {homography_path}')
video_H = np.loadtxt(homography_path, delimiter=',')
else:
video_H = None
prev_time = time.time()
while self.is_running.is_set():
ret, img = video.read()
# seek to 0 if video has finished. Infinite loop
if not ret:
# now loading multiple files
break
# video.set(cv2.CAP_PROP_POS_FRAMES, 0)
# ret, img = video.read()
# assert ret is not False # not really error proof...
frame = Frame(i, img=img, H=self.config.camera.H, camera=self.config.camera)
# frame.img = processor.apply(frame.img)
if "DATASETS/hof/" in str(video_path):
# hack to mask out area
cv2.rectangle(img, (0,0), (800,200), (0,0,0), -1)
frame = Frame(index=i, img=img, H=video_H)
# TODO: this is very dirty, need to find another way.
# perhaps multiprocessing Array?
self.frame_noimg_sock.send(pickle.dumps(frame.without_img()))
self.frame_sock.send(pickle.dumps(frame))
if writer:
writer.write(frame.img)
finally:
if writer:
writer.release()
# defer next loop
now = time.time()
time_diff = (now - prev_time)
if time_diff < target_frame_duration:
time.sleep(target_frame_duration - time_diff)
now += target_frame_duration - time_diff
prev_time = now
i += 1
if not self.is_running.is_set():
# if not running, also break out of infinite generator loop
break
logger.info("Stopping")
@classmethod
def arg_parser(cls) -> ArgumentParser:
argparser = LambdaParser()
argparser.add_argument('--zmq-frame-addr',
help='Manually specity communication addr for the frame messages',
type=str,
default="ipc:///tmp/feeds_frame")
argparser.add_argument('--zmq-frame-noimg-addr',
help='Manually specity communication addr for the frame messages',
type=str,
default="ipc:///tmp/feeds_frame2")
argparser.add_argument("--video-src",
help="source video to track from can be either a relative or absolute path, or a url, like an RTSP resource, or use gige://RELATIVE_PATH_TO_GIGE_CONFIG_JSON",
type=UrlOrPath,
nargs='+',
default=lambda: [UrlOrPath(p) for p in Path('../DATASETS/VIRAT_subset_0102x/').glob('*.mp4')])
argparser.add_argument("--video-offset",
help="Start playback from given frame. Note that when src is an array, this applies to all videos individually.",
default=0,
type=int)
argparser.add_argument("--video-end",
help="End (or loop) playback at given frame.",
default=None,
type=int)
argparser.add_argument("--record",
help="Record source video to given filename",
default=None,
type=Path)
argparser.add_argument("--video-loop",
help="By default it emitter will run only once. This allows it to loop the video file to keep testing.",
action='store_true')
argparser.add_argument("--camera-fps",
help="Camera FPS",
type=int,
default=12)
argparser.add_argument("--homography",
help="File with homography params [Deprecated]",
type=Path,
default='../DATASETS/VIRAT_subset_0102x/VIRAT_0102_homography_img2world.txt',
action=HomographyAction)
argparser.add_argument("--calibration",
help="File with camera intrinsics and lens distortion params (calibration.json)",
# type=Path,
required=True,
# default=None,
action=CameraAction)
return argparser
def run_frame_emitter(config: Namespace, is_running: Event, timer_counter: int):
def run_frame_emitter(config: Namespace, is_running: Event):
router = FrameEmitter(config, is_running)
router.run(timer_counter)
is_running.clear()
router.emit_video()
is_running.clear()

97
trap/frame_writer.py
View file

@ -1,97 +0,0 @@
# used for "Forward Referencing of type annotations"
from __future__ import annotations
import datetime
import logging
import time
from argparse import ArgumentParser
from pathlib import Path
import zmq
from trap.frame_emitter import Frame
from trap.node import Node
from trap.preview_renderer import FrameWriter as CvFrameWriter
logger = logging.getLogger("trap.simple_renderer")
class FrameWriter(Node):
def setup(self):
self.frame_sock = self.sub(self.config.zmq_frame_addr)
self.out_writer = self.start_writer()
def start_writer(self):
if not self.config.output_dir.exists():
raise FileNotFoundError("Path does not exist")
date_str = datetime.datetime.now().isoformat(timespec="minutes")
filename = self.config.output_dir / f"render-source-{date_str}.mp4"
logger.info(f"Write to {filename}")
return CvFrameWriter(str(filename), None, None)
# fourcc = cv2.VideoWriter_fourcc(*'vp09')
# return cv2.VideoWriter(str(filename), fourcc, self.fps, self.frame_size)
def run(self):
i=0
try:
while self.run_loop():
i += 1
# zmq_ev = self.frame_sock.poll(timeout=2000)
# if not zmq_ev:
# # when no data comes in, loop so that is_running is checked
# continue
try:
frame: Frame = self.frame_sock.recv_pyobj(zmq.NOBLOCK)
# else:
# logger.debug(f'new video frame {frame.index}')
if frame is None:
# might need to wait a few iterations before first frame comes available
time.sleep(.1)
continue
self.logger.debug(f"write frame {frame.time:.3f}")
self.out_writer.write(frame.img)
except zmq.ZMQError as e:
# idx = frame.index if frame else "NONE"
# logger.debug(f"reuse video frame {idx}")
pass
except KeyboardInterrupt as e:
print('stopping on interrupt')
self.logger.info('Stopping')
# if i>2:
if self.out_writer:
self.out_writer.release()
self.logger.info(f'Wrote to {self.out_writer.filename}')
self.logger.info('stopped')
@classmethod
def arg_parser(cls):
argparser = ArgumentParser()
argparser.add_argument('--zmq-frame-addr',
help='Manually specity communication addr for the frame messages',
type=str,
default="ipc:///tmp/feeds_frame")
argparser.add_argument("--output-dir",
help="Directory to save the video in",
required=True,
type=Path)
return argparser

631
trap/helios.py
View file

@ -1,631 +0,0 @@
# code by phar: https://github.com/phar/heliospy
import usb.core
import usb.util
import struct
import time
import queue
from trap.hersey import *
from threading import Thread
import matplotlib.pyplot as plt
import numpy as np
HELIOS_VID = 0x1209
HELIOS_PID = 0xE500
EP_BULK_OUT = 0x02
EP_BULK_IN = 0x81
EP_INT_OUT = 0x06
EP_INT_IN = 0x83
INTERFACE_INT = 0
INTERFACE_BULK = 1
INTERFACE_ISO = 2
HELIOS_MAX_POINTS = 0x1000
HELIOS_MAX_RATE = 0xFFFF
HELIOS_MIN_RATE = 7
HELIOS_SUCCESS = 1
# Functions return negative values if something went wrong
# Attempted to perform an action before calling OpenDevices()
HELIOS_ERROR_NOT_INITIALIZED =-1
# Attempted to perform an action with an invalid device number
HELIOS_ERROR_INVALID_DEVNUM = -2
# WriteFrame() called with null pointer to points
HELIOS_ERROR_NULL_POINTS = -3
# WriteFrame() called with a frame containing too many points
HELIOS_ERROR_TOO_MANY_POINTS = -4
# WriteFrame() called with pps higher than maximum allowed
HELIOS_ERROR_PPS_TOO_HIGH = -5
# WriteFrame() called with pps lower than minimum allowed
HELIOS_ERROR_PPS_TOO_LOW = -6
# Errors from the HeliosDacDevice class begin at -1000
# Attempted to perform an operation on a closed DAC device
HELIOS_ERROR_DEVICE_CLOSED = -1000
# Attempted to send a new frame with HELIOS_FLAGS_DONT_BLOCK before previous DoFrame() completed
HELIOS_ERROR_DEVICE_FRAME_READY = -1001
#/ Operation failed because SendControl() failed (if operation failed because of libusb_interrupt_transfer failure, the error code will be a libusb error instead)
HELIOS_ERROR_DEVICE_SEND_CONTROL = -1002
# Received an unexpected result from a call to SendControl()
HELIOS_ERROR_DEVICE_RESULT = -1003
# Attempted to call SendControl() with a null buffer pointer
HELIOS_ERROR_DEVICE_NULL_BUFFER = -1004
# Attempted to call SendControl() with a control signal that is too long
HELIOS_ERROR_DEVICE_SIGNAL_TOO_LONG = -1005
HELIOS_ERROR_LIBUSB_BASE = -5000
HELIOS_FLAGS_DEFAULT = 0
HELIOS_FLAGS_START_IMMEDIATELY = (1 << 0)
HELIOS_FLAGS_SINGLE_MODE = (1 << 1)
HELIOS_FLAGS_DONT_BLOCK = (1 << 2)
HELIOS_CMD_STOP =0x0001
HELIOS_CMD_SHUTTER =0x0002
HELIOS_CMD_GET_STATUS =0x0003
HELIOS_GET_FWVERSION =0x0004
HELIOS_CMD_GET_NAME =0x0005
HELIOS_CMD_SET_NAME =0x0006
HELIOS_SET_SDK_VERSION =0x0007
HELIOS_CMD_ERASE_FIRMWARE =0x00de
HELIOS_SDK_VERSION = 6
class HeliosPoint():
def __init__(self,x,y,c = 0xff0000,i= 255,blank=False):
self.x = x
self.y = y
self.c = 0x010203
self.i = i
self.blank = blank
def __str__(self):
return "HeleiosPoint(%d, %d,0x%0x,%d,%d)" % (self.x, self.y, self.c,self.i, self.blank)
class HeliosDAC():
def __init__(self,queuethread=True, debug=0):
self.debug=debug
self.closed = 1
self.frameReady = 0
self.framebuffer = ""
self.threadqueue = queue.Queue(maxsize=20)
self.nextframebuffer = ""
self.adcbits = 12
self.dev = usb.core.find(idVendor=HELIOS_VID, idProduct=HELIOS_PID)
self.cfg = self.dev.get_active_configuration()
self.intf = self.cfg[(0,1,2)]
self.dev.reset()
self.palette = [( 0, 0, 0 ), # Black/blanked (fixed)
( 255, 255, 255 ), # White (fixed)
( 255, 0, 0 ), # Red (fixed)
( 255, 255, 0 ), # Yellow (fixed)
( 0, 255, 0 ), # Green (fixed)
( 0, 255, 255 ), # Cyan (fixed)
( 0, 0, 255 ), # Blue (fixed)
( 255, 0, 255 ), # Magenta (fixed)
( 255, 128, 128 ), # Light red
( 255, 140, 128 ),
( 255, 151, 128 ),
( 255, 163, 128 ),
( 255, 174, 128 ),
( 255, 186, 128 ),
( 255, 197, 128 ),
( 255, 209, 128 ),
( 255, 220, 128 ),
( 255, 232, 128 ),
( 255, 243, 128 ),
( 255, 255, 128 ), # Light yellow
( 243, 255, 128 ),
( 232, 255, 128 ),
( 220, 255, 128 ),
( 209, 255, 128 ),
( 197, 255, 128 ),
( 186, 255, 128 ),
( 174, 255, 128 ),
( 163, 255, 128 ),
( 151, 255, 128 ),
( 140, 255, 128 ),
( 128, 255, 128 ), # Light green
( 128, 255, 140 ),
( 128, 255, 151 ),
( 128, 255, 163 ),
( 128, 255, 174 ),
( 128, 255, 186 ),
( 128, 255, 197 ),
( 128, 255, 209 ),
( 128, 255, 220 ),
( 128, 255, 232 ),
( 128, 255, 243 ),
( 128, 255, 255 ), # Light cyan
( 128, 243, 255 ),
( 128, 232, 255 ),
( 128, 220, 255 ),
( 128, 209, 255 ),
( 128, 197, 255 ),
( 128, 186, 255 ),
( 128, 174, 255 ),
( 128, 163, 255 ),
( 128, 151, 255 ),
( 128, 140, 255 ),
( 128, 128, 255 ), # Light blue
( 140, 128, 255 ),
( 151, 128, 255 ),
( 163, 128, 255 ),
( 174, 128, 255 ),
( 186, 128, 255 ),
( 197, 128, 255 ),
( 209, 128, 255 ),
( 220, 128, 255 ),
( 232, 128, 255 ),
( 243, 128, 255 ),
( 255, 128, 255 ), # Light magenta
( 255, 128, 243 ),
( 255, 128, 232 ),
( 255, 128, 220 ),
( 255, 128, 209 ),
( 255, 128, 197 ),
( 255, 128, 186 ),
( 255, 128, 174 ),
( 255, 128, 163 ),
( 255, 128, 151 ),
( 255, 128, 140 ),
( 255, 0, 0 ), # Red (cycleable)
( 255, 23, 0 ),
( 255, 46, 0 ),
( 255, 70, 0 ),
( 255, 93, 0 ),
( 255, 116, 0 ),
( 255, 139, 0 ),
( 255, 162, 0 ),
( 255, 185, 0 ),
( 255, 209, 0 ),
( 255, 232, 0 ),
( 255, 255, 0 ), #Yellow (cycleable)
( 232, 255, 0 ),
( 209, 255, 0 ),
( 185, 255, 0 ),
( 162, 255, 0 ),
( 139, 255, 0 ),
( 116, 255, 0 ),
( 93, 255, 0 ),
( 70, 255, 0 ),
( 46, 255, 0 ),
( 23, 255, 0 ),
( 0, 255, 0 ), # Green (cycleable)
( 0, 255, 23 ),
( 0, 255, 46 ),
( 0, 255, 70 ),
( 0, 255, 93 ),
( 0, 255, 116 ),
( 0, 255, 139 ),
( 0, 255, 162 ),
( 0, 255, 185 ),
( 0, 255, 209 ),
( 0, 255, 232 ),
( 0, 255, 255 ), # Cyan (cycleable)
( 0, 232, 255 ),
( 0, 209, 255 ),
( 0, 185, 255 ),
( 0, 162, 255 ),
( 0, 139, 255 ),
( 0, 116, 255 ),
( 0, 93, 255 ),
( 0, 70, 255 ),
( 0, 46, 255 ),
( 0, 23, 255 ),
( 0, 0, 255 ), # Blue (cycleable)
( 23, 0, 255 ),
( 46, 0, 255 ),
( 70, 0, 255 ),
( 93, 0, 255 ),
( 116, 0, 255 ),
( 139, 0, 255 ),
( 162, 0, 255 ),
( 185, 0, 255 ),
( 209, 0, 255 ),
( 232, 0, 255 ),
( 255, 0, 255 ), # Magenta (cycleable)
( 255, 0, 232 ),
( 255, 0, 209 ),
( 255, 0, 185 ),
( 255, 0, 162 ),
( 255, 0, 139 ),
( 255, 0, 116 ),
( 255, 0, 93 ),
( 255, 0, 70 ),
( 255, 0, 46 ),
( 255, 0, 23 ),
( 128, 0, 0 ), # Dark red
( 128, 12, 0 ),
( 128, 23, 0 ),
( 128, 35, 0 ),
( 128, 47, 0 ),
( 128, 58, 0 ),
( 128, 70, 0 ),
( 128, 81, 0 ),
( 128, 93, 0 ),
( 128, 105, 0 ),
( 128, 116, 0 ),
( 128, 128, 0 ), # Dark yellow
( 116, 128, 0 ),
( 105, 128, 0 ),
( 93, 128, 0 ),
( 81, 128, 0 ),
( 70, 128, 0 ),
( 58, 128, 0 ),
( 47, 128, 0 ),
( 35, 128, 0 ),
( 23, 128, 0 ),
( 12, 128, 0 ),
( 0, 128, 0 ), # Dark green
( 0, 128, 12 ),
( 0, 128, 23 ),
( 0, 128, 35 ),
( 0, 128, 47 ),
( 0, 128, 58 ),
( 0, 128, 70 ),
( 0, 128, 81 ),
( 0, 128, 93 ),
( 0, 128, 105 ),
( 0, 128, 116 ),
( 0, 128, 128 ), # Dark cyan
( 0, 116, 128 ),
( 0, 105, 128 ),
( 0, 93, 128 ),
( 0, 81, 128 ),
( 0, 70, 128 ),
( 0, 58, 128 ),
( 0, 47, 128 ),
( 0, 35, 128 ),
( 0, 23, 128 ),
( 0, 12, 128 ),
( 0, 0, 128 ), # Dark blue
( 12, 0, 128 ),
( 23, 0, 128 ),
( 35, 0, 128 ),
( 47, 0, 128 ),
( 58, 0, 128 ),
( 70, 0, 128 ),
( 81, 0, 128 ),
( 93, 0, 128 ),
( 105, 0, 128 ),
( 116, 0, 128 ),
( 128, 0, 128 ), # Dark magenta
( 128, 0, 116 ),
( 128, 0, 105 ),
( 128, 0, 93 ),
( 128, 0, 81 ),
( 128, 0, 70 ),
( 128, 0, 58 ),
( 128, 0, 47 ),
( 128, 0, 35 ),
( 128, 0, 23 ),
( 128, 0, 12 ),
( 255, 192, 192 ), # Very light red
( 255, 64, 64 ), # Light-medium red
( 192, 0, 0 ), # Medium-dark red
( 64, 0, 0 ), # Very dark red
( 255, 255, 192 ), # Very light yellow
( 255, 255, 64 ), # Light-medium yellow
( 192, 192, 0 ), # Medium-dark yellow
( 64, 64, 0 ), # Very dark yellow
( 192, 255, 192 ), # Very light green
( 64, 255, 64 ), # Light-medium green
( 0, 192, 0 ), # Medium-dark green
( 0, 64, 0 ), # Very dark green
( 192, 255, 255 ), # Very light cyan
( 64, 255, 255 ), # Light-medium cyan
( 0, 192, 192 ), # Medium-dark cyan
( 0, 64, 64 ), # Very dark cyan
( 192, 192, 255 ), # Very light blue
( 64, 64, 255 ), # Light-medium blue
( 0, 0, 192 ), # Medium-dark blue
( 0, 0, 64 ), # Very dark blue
( 255, 192, 255 ), # Very light magenta
( 255, 64, 255 ), # Light-medium magenta
( 192, 0, 192 ), # Medium-dark magenta
( 64, 0, 64 ), # Very dark magenta
( 255, 96, 96 ), # Medium skin tone
( 255, 255, 255 ), # White (cycleable)
( 245, 245, 245 ),
( 235, 235, 235 ),
( 224, 224, 224 ), # Very light gray (7/8 intensity)
( 213, 213, 213 ),
( 203, 203, 203 ),
( 192, 192, 192 ), # Light gray (3/4 intensity)
( 181, 181, 181 ),
( 171, 171, 171 ),
( 160, 160, 160 ), # Medium-light gray (5/8 int.)
( 149, 149, 149 ),
( 139, 139, 139 ),
( 128, 128, 128 ), # Medium gray (1/2 intensity)
( 117, 117, 117 ),
( 107, 107, 107 ),
( 96, 96, 96 ), # Medium-dark gray (3/8 int.)
( 85, 85, 85 ),
( 75, 75, 75 ),
( 64, 64, 64 ), # Dark gray (1/4 intensity)
( 53, 53, 53 ),
( 43, 43, 43 ),
( 32, 32, 32 ), # Very dark gray (1/8 intensity)
( 21, 21, 21 ),
( 11, 11, 11 )] # Black
self.dev.set_interface_altsetting(interface = 0, alternate_setting = 1)
if self.dev.is_kernel_driver_active(0) is True:
self.dev.detach_kernel_driver(0)
# claim the device
usb.util.claim_interface(self.dev, 0)
if self.dev is None:
raise ValueError('Device not found')
else:
if self.debug:
print(self.dev)
try:
transferResult = self.intf[0].read(32,1)
except:
if self.debug:
print("no lingering data")
if self.debug:
print(self.GetName())
print(self.getHWVersion())
self.setSDKVersion()
self.closed = False
if queuethread:
self.runQueueThread()
def runQueueThread(self):
worker = Thread(target=self.doframe_thread_loop)
worker.setDaemon(True)
worker.start()
def doframe_thread_loop(self):
while self.closed == 0:
if self.closed:
return;
self.DoFrame();
def getHWVersion(self):
self.intf[1].write(struct.pack("<H",HELIOS_GET_FWVERSION))
transferResult = self.intf[0].read(32)
if transferResult[0] == 0x84:
return struct.unpack("<L",transferResult[1:])[0]
else:
return None
def setSDKVersion(self, version = HELIOS_SDK_VERSION):
self.intf[1].write(struct.pack("<H",(version << 8) | HELIOS_SET_SDK_VERSION))
return
def setShutter(self, shutter=False):
self.SendControl(struct.pack("<H",(shutter << 8) | HELIOS_CMD_SHUTTER))
return
def setName(self, name):
self.SendControl(struct.pack("<H", HELIOS_CMD_SET_NAME) + name[:30] + b"\x00")
return
def newFrame(self,pps, pntobjlist, flags = HELIOS_FLAGS_DEFAULT):
if self.closed:
return HELIOS_ERROR_DEVICE_CLOSED;
if ( len(pntobjlist) > HELIOS_MAX_POINTS):
return HELIOS_ERROR_TOO_MANY_POINTS
if (pps > HELIOS_MAX_RATE):
return HELIOS_ERROR_PPS_TOO_HIGH
if (pps < HELIOS_MIN_RATE):
return HELIOS_ERROR_PPS_TOO_LOW
#this is a bug workaround, the mcu won't correctly receive transfers with these sizes
ppsActual = pps;
numOfPointsActual = len(pntobjlist)
if (((len(pntobjlist)-45) % 64) == 0):
numOfPointsActual-=1
ppsActual = int((pps * numOfPointsActual / len(pntobjlist) + 0.5))
pntobjlist = pntobjlist[:numOfPointsActual]
nextframebuffer = b""
for pnt in pntobjlist:
a = (pnt.x >> 4) & 0xff
b = ((pnt.x & 0x0F) << 4) | (pnt.y >> 8)
c = pnt.y & 0xFF
if pnt.blank == False:
r = (pnt.c & 0xff0000) >> 16
g = (pnt.c & 0xff00) >> 8
b = (pnt.c & 0xff)
i = pnt.i
else:
r = 0
g = 0
b = 0
i = 0
nextframebuffer += struct.pack("BBBBBBB", a,b,c,r,g,b,i)
nextframebuffer += struct.pack("BBBBB", (ppsActual & 0xFF),(ppsActual >> 8) ,(len(pntobjlist) & 0xFF),(len(pntobjlist) >> 8),flags)
self.threadqueue.put(nextframebuffer)
def DoFrame(self):
if (self.closed):
return HELIOS_ERROR_DEVICE_CLOSED;
self.nextframebuffer = self.threadqueue.get(block=True)
self.intf[3].write(self.nextframebuffer)
t = time.time()
while(self.getStatus()[1] == 0): #wait for the laser
pass
return self.getStatus()
def GetName(self):
self.SendControl(struct.pack("<H",HELIOS_CMD_GET_NAME))
x = self.intf[0].read(32)[:16]
if x[0] == 0x85:
return "".join([chr(t) for t in x[1:]])
else:
return None
def SendControl(self, buffer):
if (buffer == None):
return HELIOS_ERROR_DEVICE_NULL_BUFFER;
if (len(buffer) > 32):
return HELIOS_ERROR_DEVICE_SIGNAL_TOO_LONG;
self.intf[1].write(buffer)
def stop(self):
self.SendControl(struct.pack("<H",0x0001), 2)
time.sleep(.1)
return
def getStatus(self):
self.SendControl(struct.pack("<H",0x0003))
ret = self.intf[0].read(32)
if self.debug:
print(ret)
return ret
def generateText(self,text,xpos,ypos,cindex=0,scale=1.0):
pointstream = []
ctr = 0
for c in text:
lastx = xpos
lasty = ypos
blank = True
for x,y in HERSHEY_FONT[ord(c)-32]:
if (x == -1) and (y == -1):
# pointstream.append(HeliosPoint(lastx,lasty,blank=blank))
blank = True
else:
lastx = int((x + (ctr * HERSHEY_WIDTH)) * scale)
lasty = int(y * scale)
blank = False
pointstream.append(HeliosPoint(lastx,lasty,self.palette[cindex],blank=blank))
ctr += 1
return pointstream
def loadILDfile(self,filename, xscale=1.0, yscale=1.0):
f = open(filename,"rb")
headerstruct = ">4s3xB8s8sHHHBx"
moreframes = True
frames = []
while moreframes:
(magic, format, fname, cname, rcnt, num, total_frames, projectorid) = struct.unpack(headerstruct,f.read(struct.calcsize(headerstruct)))
if magic == b"ILDA":
pointlist = []
palette = []
x = y = z = red = green = blue = 0
blank = 1
lastpoint = 0
if rcnt > 0:
for i in range(rcnt):
if format in [0,1,4,5]:
if format == 0:
fmt = ">hhhBB"
(x,y,z,status,cindex) = struct.unpack(fmt,f.read(struct.calcsize(fmt)))
elif format == 1:
fmt = ">hhBB"
(x,y,status,cindex) = struct.unpack(fmt,f.read(struct.calcsize(fmt)))
elif format == 4:
(x,y,z,status,red,green,blue) = struct.unpack(fmt,f.read(struct.calcsize(fmt)))
elif format == 5:
fmt = ">hhhBBBB"
(x,y,status,red,green,blue) = struct.unpack(fmt,f.read(struct.calcsize(fmt)))
blank = (status & 0x40) > 0
lastpoint = (status & 0x80) > 0
lessadcbits = (16 - self.adcbits)
x = int((x >> lessadcbits) * xscale)
y = int((y >> lessadcbits) * yscale)
pointlist.append(HeliosPoint(x,y,self.palette[cindex],blank=blank))
elif format == 2:
fmt = ">BBB"
(r,g,b) = struct.unpack(fmt,f.read(struct.calcsize(fmt)))
palette.append((r<<16) | (g<<8) | b)
if format == 2:
frames.append((("palette",fname,cname, num),palette))
else:
frames.append((("frame",fname,cname,num),pointlist))
else:
moreframes = 0
else:
moreframes = 0
return frames
def plot(self, pntlist):
fig, ax = plt.subplots() # Create a figure containing a single axes.
xlst = []
ylst = []
for p in pntlist:
if p.blank == False:
xlst.append(p.x)
ylst.append(p.y)
ax.plot(xlst,ylst)
plt.show()
if __name__ == "__main__":
a = HeliosDAC()
# a.runQueueThread()
# cal = a.generateText("hello World", 20,20,scale=10)
## print(cal)
# a.plot(cal)
#
# while(1):
# a.newFrame(2000,cal)
# a.DoFrame()
# cal = a.generateText("hello World", 0, 0,scale=10)
# pps = 20000
# while(1):
# a.newFrame(pps,cal)
# a.DoFrame()
# cal = a.loadILDfile("ildatest.ild")
# while(1):
# for (t,n1,n2,c),f in cal:
# print("playing %s,%s, %d" % (n1,n2,c))
# a.newFrame(5000,f)
# a.DoFrame()
# a.plot(f)
pps = 200
while(1):
a.newFrame(pps,[HeliosPoint(0,200, c=(255,255,255)), #draw a square
HeliosPoint(200,200, c=(255,255,255)),
HeliosPoint(200,0, c=(255,255,255)),
HeliosPoint(0,0, c=(255,255,255))])
a.DoFrame()
# while(1):
## a.newFrame(1000,[HeliosPoint(16000,16000)])
# a.newFrame(100,[HeliosPoint(16000-2500,16000),HeliosPoint(16000,16000),HeliosPoint(16000+2500,16000),HeliosPoint(16000,16000),HeliosPoint(16000,16000+2500),HeliosPoint(16000,16000),HeliosPoint(16000,16000-2500),HeliosPoint(16000,16000)])
# a.DoFrame()
# while(1):
# a.newFrame(1000,[HeliosPoint(0,200),
# HeliosPoint(200,200),
# HeliosPoint(200,0),
# HeliosPoint(0,0),
# ])
# a.DoFrame()

253
trap/helios_dac/calibration_points.py
View file

@ -1,253 +0,0 @@
# -*- coding: utf-8 -*-
"""
Example for using Helios DAC libraries in python (using C library with ctypes)
NB: If you haven't set up udev rules you need to use sudo to run the program for it to detect the DAC.
"""
from __future__ import annotations
import ctypes
import json
import math
from typing import Optional
import cv2
import numpy as np
def lerp(a: float, b: float, t: float) -> float:
"""Linear interpolate on the scale given by a to b, using t as the point on that scale.
Examples
--------
50 == lerp(0, 100, 0.5)
4.2 == lerp(1, 5, 0.8)
"""
return (1 - t) * a + t * b
class LaserFrame():
def __init__(self, paths: list[LaserPath]):
self.paths = paths
# def closest_path(cls, point, paths):
# distances = [min(p.last()-)]
# def optimise_paths_lazy(self, last_point = None):
# """Quick way to optimise order of paths
# last_point can be the ending point of previous frame.
# """
# ordered_paths = []
# if not last_point:
# ordered_paths.append(self.paths.pop(0))
# last_point = endpoint
# pass
def get_points_interpolated_by_distance(self, point_interval, last_point: Optional[LaserPoint] = None) -> list[LaserPoint]:
"""
Interpolate the gaps between paths (NOT THE PATHS THEMSELVES)
point_interval is the maximum interval at which a new point should be added
"""
points: list[LaserPoint] = []
for path in self.paths:
if last_point:
a = last_point
b = path.first()
dx = b.x - a.x
dy = b.y - a.y
distance = np.linalg.norm([dx,dy])
steps = int(distance // point_interval)
for step in range(steps+1): # have both 0 and 1 in the lerp for empty points
t = step/(steps+1)
x = int(lerp(a.x, b.x, t))
y = int(lerp(a.y, b.y, t))
points.append(LaserPoint(x,y, (0,0,0), 0, True))
# print('append', steps)
points.extend(path.points)
last_point = path.last()
return points
class LaserPath():
def __init__(self, points: list[LaserPoint] = []):
# if len(points) < 1:
# raise RuntimeError("LaserPath should have some points")
self.points = points
def last(self):
return self.points[-1]
def first(self):
return self.points[0]
class LaserPoint():
def __init__(self,x,y,c: Color = (255,0,0),i= 255,blank=False):
self.x = x
self.y = y
self.c = c
self._i = i
self.blank = blank
@property
def color(self):
if self.blank: return (0,0,0)
return self.c
@property
def i(self):
return 0 if self.blank else self._i
def circle_points(cx, cy, r, c: Color):
# r = 100
steps = r
pointlist: list[LaserPoint] = []
for i in range(steps):
x = int(cx + math.cos(i * (2*math.pi)/steps) * r)
y = int(cy + math.sin(i * (2*math.pi)/steps)* r)
pointlist.append(LaserPoint(x, y, c, blank=(i==(steps-1)or i==0)))
return pointlist
def cross_points(cx, cy, r, c: Color):
# r = 100
steps = r
pointlist: list[LaserPoint] = []
for i in range(steps):
x = int(cx)
y = int(cy + r - i * 2 * r/steps)
pointlist.append(LaserPoint(x, y, c, blank=(i==(steps-1)or i==0)))
path = LaserPath(pointlist)
pointlist: list[LaserPoint] = []
for i in range(steps):
y = int(cy)
x = int(cx + r - i * 2 * r/steps)
pointlist.append(LaserPoint(x, y, c, blank=(i==(steps-1)or i==0)))
path2 = LaserPath(pointlist)
return [path, path2]
Color = tuple[int, int, int]
#Define point structure
class HeliosPoint(ctypes.Structure):
#_pack_=1
_fields_ = [('x', ctypes.c_uint16),
('y', ctypes.c_uint16),
('r', ctypes.c_uint8),
('g', ctypes.c_uint8),
('b', ctypes.c_uint8),
('i', ctypes.c_uint8)]
#Load and initialize library
HeliosLib = ctypes.cdll.LoadLibrary("./libHeliosDacAPI.so")
numDevices = HeliosLib.OpenDevices()
print("Found ", numDevices, "Helios DACs")
# #Create sample frames
# frames = [0 for x in range(100)]
# frameType = HeliosPoint * 1000
# x = 0
# y = 0
# for i in range(100):
# y = round(i * 0xFFF / 100)
# # y = round(50*0xFFF/100)
# frames[i] = frameType()
# for j in range(1000):
# if (j < 500):
# x = round(j * 0xFFF / 500)
# offset = 0
# else:
# offset = 0
# x = round(0xFFF - ((j - 500) * 0xFFF / 500))
# # frames[i][j] = HeliosPoint(int(x),int(y+offset),0,(x%155),0,255)
# frames[i][j] = HeliosPoint(int(x),int(y+offset),0,100,0,255)
pct =0xfff/100
r=50
# TODO)) scriptje met sliders
paths = [
# LaserPath(circle_points(10*pct, 45*pct, r, (100,0,100))),
# *cross_points(10*pct, 45*pct, r, (100,0,100)), # magenta
*cross_points(13.7*pct, 38.9*pct, r, (100,0,100)), # magenta # punt 10
*cross_points(44.3*pct, 47.0*pct, r, (0,100,0)), # groen # punt 0
*cross_points(82.5*pct, 12.7*pct, r, (100,100,100)), # wit # punt 4
*cross_points(89*pct, 49*pct, r, (0,100,100)), # cyan # punt 2
*cross_points(36*pct, 81.7*pct, r, (100,100,0)), # geel # punt 7
]
calibration_points = [
(13.7*pct, 38.9*pct, 10,),
(44.3*pct, 47.0*pct, 0),
(82.5*pct, 12.7*pct, 4),
(89*pct, 49*pct, 2),
(36*pct, 81.7*pct, 7),
]
with open('/home/ruben/suspicion/DATASETS/hof3/irl_points.json') as fp:
irl_points = json.load(fp)
src_points = []
dst_points=[]
for x, y, index in calibration_points:
src_points.append(irl_points[index])
dst_points.append([x,y])
print(src_points)
H, status = cv2.findHomography(np.array(src_points), np.array(dst_points))
print("LASER HOMOGRAPHY MATRIX")
print(H)
dst_img_points = cv2.perspectiveTransform(np.array([[irl_points[1]]]), H)
print(dst_img_points)
paths.extend([
*cross_points(dst_img_points[0][0][0], dst_img_points[0][0][1], r, (100,100,0)), # geel # punt 7
])
frame = LaserFrame(paths)
pointlist = frame.get_points_interpolated_by_distance(3)
print(len(pointlist))
#Play frames on DAC
i=0
while True:
frameType = HeliosPoint * len(pointlist)
frame = frameType()
# print(len(pointlist), last_laser_point.x, last_laser_point.y)
for j, point in enumerate(pointlist):
frame[j] = HeliosPoint(point.x, point.y, point.color[0],point.color[1], point.color[2], point.i)
# Make 512 attempts for DAC status to be ready. After that, just give up and try to write the frame anyway
statusAttempts=0
while (statusAttempts < 512 and HeliosLib.GetStatus(0) != 1):
statusAttempts += 1
HeliosLib.WriteFrame(0, 50000, 0, ctypes.pointer(frame), len(pointlist))
# for i in range(250):
# i+=1
# for j in range(numDevices):
# statusAttempts = 0
# # Make 512 attempts for DAC status to be ready. After that, just give up and try to write the frame anyway
# while (statusAttempts < 512 and HeliosLib.GetStatus(j) != 1):
# statusAttempts += 1
# HeliosLib.WriteFrame(j, 50000, 0, ctypes.pointer(frames[i % 100]), 1000) #Send the frame
HeliosLib.CloseDevices()

196
trap/hersey.py
View file

@ -1,196 +0,0 @@
# part of heliospy, see helios.py
HERSHEY_HEIGHT = 28
HERSHEY_WIDTH = 28
HERSHEY_FONT = [
#Ascii 32
[(0,16),(-1, -1)],
#Ascii 33
[(8,10),(5, 21),(5, 7),(-1, -1),(5, 2),(4, 1),(5, 0),(6, 1),(5, 2),(-1, -1)],
#Ascii 34
[(5,16),(4, 21),(4, 14),(-1, -1),(12, 21),(12, 14),(-1, -1)],
#Ascii 35
[(11,21),(11, 25),(4, -7),(-1, -1),(17, 25),(10, -7),(-1, -1),(4, 12),(18, 12),(-1, -1),(3, 6),(17, 6),(-1, -1)],
#Ascii 36
[(26,20),(8, 25),(8, -4),(-1, -1),(12, 25),(12, -4),(-1, -1),(17, 18),(15, 20),(12, 21),(8, 21),(5, 20),(3, 18),(3, 16),(4, 14),(5, 13),(7, 12),(13, 10),(15, 9),(16, 8),(17, 6),(17, 3),(15, 1),(12, 0),(8, 0),(5, 1),(3, 3),(-1, -1)],
#Ascii 37
[(31,24),(21, 21),(3, 0),(-1, -1),(8, 21),(10, 19),(10, 17),(9, 15),(7, 14),(5, 14),(3, 16),(3, 18),(4, 20),(6, 21),(8, 21),(10, 20),(13, 19),(16, 19),(19, 20),(21, 21),(-1, -1),(17, 7),(15, 6),(14, 4),(14, 2),(16, 0),(18, 0),(20, 1),(21, 3),(21, 5),(19, 7),(17, 7),(-1, -1)],
#Ascii 38
[(34,26),(23, 12),(23, 13),(22, 14),(21, 14),(20, 13),(19, 11),(17, 6),(15, 3),(13, 1),(11, 0),(7, 0),(5, 1),(4, 2),(3, 4),(3, 6),(4, 8),(5, 9),(12, 13),(13, 14),(14, 16),(14, 18),(13, 20),(11, 21),(9, 20),(8, 18),(8, 16),(9, 13),(11, 10),(16, 3),(18, 1),(20, 0),(22, 0),(23, 1),(23, 2),(-1, -1)],
#Ascii 39
[(7,10),(5, 19),(4, 20),(5, 21),(6, 20),(6, 18),(5, 16),(4, 15),(-1, -1)],
#Ascii 40
[(10,14),(11, 25),(9, 23),(7, 20),(5, 16),(4, 11),(4, 7),(5, 2),(7, -2),(9, -5),(11, -7),(-1, -1)],
#Ascii 41
[(10,14),(3, 25),(5, 23),(7, 20),(9, 16),(10, 11),(10, 7),(9, 2),(7, -2),(5, -5),(3, -7),(-1, -1)],
#Ascii 42
[(8,16),(8, 21),(8, 9),(-1, -1),(3, 18),(13, 12),(-1, -1),(13, 18),(3, 12),(-1, -1)],
#Ascii 43
[(5,26),(13, 18),(13, 0),(-1, -1),(4, 9),(22, 9),(-1, -1)],
#Ascii 44
[(8,10),(6, 1),(5, 0),(4, 1),(5, 2),(6, 1),(6, -1),(5, -3),(4, -4),(-1, -1)],
#Ascii 45
[(2,26),(4, 9),(22, 9),(-1, -1)],
#Ascii 46
[(5,10),(5, 2),(4, 1),(5, 0),(6, 1),(5, 2),(-1, -1)],
#Ascii 47`
[(2,22),(20, 25),(2, -7),(-1, -1)],
#Ascii 48
[(17,20),(9, 21),(6, 20),(4, 17),(3, 12),(3, 9),(4, 4),(6, 1),(9, 0),(11, 0),(14, 1),(16, 4),(17, 9),(17, 12),(16, 17),(14, 20),(11, 21),(9, 21),(-1, -1)],
#Ascii 49
[(4,20),(6, 17),(8, 18),(11, 21),(11, 0),(-1, -1)],
#Ascii 50
[(14,20),(4, 16),(4, 17),(5, 19),(6, 20),(8, 21),(12, 21),(14, 20),(15, 19),(16, 17),(16, 15),(15, 13),(13, 10),(3, 0),(17, 0),(-1, -1)],
#Ascii 51
[(15,20),(5, 21),(16, 21),(10, 13),(13, 13),(15, 12),(16, 11),(17, 8),(17, 6),(16, 3),(14, 1),(11, 0),(8, 0),(5, 1),(4, 2),(3, 4),(-1, -1)],
#Ascii 52
[(6,20),(13, 21),(3, 7),(18, 7),(-1, -1),(13, 21),(13, 0),(-1, -1)],
#Ascii 53
[(17,20),(15, 21),(5, 21),(4, 12),(5, 13),(8, 14),(11, 14),(14, 13),(16, 11),(17, 8),(17, 6),(16, 3),(14, 1),(11, 0),(8, 0),(5, 1),(4, 2),(3, 4),(-1, -1)],
#Ascii 54
[(23,20),(16, 18),(15, 20),(12, 21),(10, 21),(7, 20),(5, 17),(4, 12),(4, 7),(5, 3),(7, 1),(10, 0),(11, 0),(14, 1),(16, 3),(17, 6),(17, 7),(16, 10),(14, 12),(11, 13),(10, 13),(7, 12),(5, 10),(4, 7),(-1, -1)],
#Ascii 55
[(5,20),(17, 21),(7, 0),(-1, -1),(3, 21),(17, 21),(-1, -1)],
#Ascii 56
[(29,20),(8, 21),(5, 20),(4, 18),(4, 16),(5, 14),(7, 13),(11, 12),(14, 11),(16, 9),(17, 7),(17, 4),(16, 2),(15, 1),(12, 0),(8, 0),(5, 1),(4, 2),(3, 4),(3, 7),(4, 9),(6, 11),(9, 12),(13, 13),(15, 14),(16, 16),(16, 18),(15, 20),(12, 21),(8, 21),(-1, -1)],
#Ascii 57
[(23,20),(16, 14),(15, 11),(13, 9),(10, 8),(9, 8),(6, 9),(4, 11),(3, 14),(3, 15),(4, 18),(6, 20),(9, 21),(10, 21),(13, 20),(15, 18),(16, 14),(16, 9),(15, 4),(13, 1),(10, 0),(8, 0),(5, 1),(4, 3),(-1, -1)],
#Ascii 58
[(11,10),(5, 14),(4, 13),(5, 12),(6, 13),(5, 14),(-1, -1),(5, 2),(4, 1),(5, 0),(6, 1),(5, 2),(-1, -1)],
#Ascii 59
[(14,10),(5, 14),(4, 13),(5, 12),(6, 13),(5, 14),(-1, -1),(6, 1),(5, 0),(4, 1),(5, 2),(6, 1),(6, -1),(5, -3),(4, -4),(-1, -1)],
#Ascii 60
[(3,24),(20, 18),(4, 9),(20, 0),(-1, -1)],
#Ascii 61
[(5,26),(4, 12),(22, 12),(-1, -1),(4, 6),(22, 6),(-1, -1)],
#Ascii 62
[(3,24),(4, 18),(20, 9),(4, 0),(-1, -1)],
#Ascii 63
[(20,18),(3, 16),(3, 17),(4, 19),(5, 20),(7, 21),(11, 21),(13, 20),(14, 19),(15, 17),(15, 15),(14, 13),(13, 12),(9, 10),(9, 7),(-1, -1),(9, 2),(8, 1),(9, 0),(10, 1),(9, 2),(-1, -1)],
#Ascii 64
[(55,27),(18, 13),(17, 15),(15, 16),(12, 16),(10, 15),(9, 14),(8, 11),(8, 8),(9, 6),(11, 5),(14, 5),(16, 6),(17, 8),(-1, -1),(12, 16),(10, 14),(9, 11),(9, 8),(10, 6),(11, 5),(-1, -1),(18, 16),(17, 8),(17, 6),(19, 5),(21, 5),(23, 7),(24, 10),(24, 12),(23, 15),(22, 17),(20, 19),(18, 20),(15, 21),(12, 21),(9, 20),(7, 19),(5, 17),(4, 15),(3, 12),(3, 9),(4, 6),(5, 4),(7, 2),(9, 1),(12, 0),(15, 0),(18, 1),(20, 2),(21, 3),(-1, -1),(19, 16),(18, 8),(18, 6),(19, 5),(8, 18),(-1,-1)],
#Ascii 65
[(8,18), (9,21), (1, 0),(-1,-1), (9,21),(17, 0),(-1,-1),( 4, 7),(14, 7),(-1,-1)],
#Ascii 66
[(23,21),(4, 21),(4, 0),(-1, -1),(4, 21),(13, 21),(16, 20),(17, 19),(18, 17),(18, 15),(17, 13),(16, 12),(13, 11),(-1, -1),(4, 11),(13, 11),(16, 10),(17, 9),(18, 7),(18, 4),(17, 2),(16, 1),(13, 0),(4, 0),(-1, -1)],
#Ascii 67
[(18,21),(18, 16),(17, 18),(15, 20),(13, 21),(9, 21),(7, 20),(5, 18),(4, 16),(3, 13),(3, 8),(4, 5),(5, 3),(7, 1),(9, 0),(13, 0),(15, 1),(17, 3),(18, 5),(-1, -1)],
#Ascii 68
[(15,21),(4, 21),(4, 0),(-1, -1),(4, 21),(11, 21),(14, 20),(16, 18),(17, 16),(18, 13),(18, 8),(17, 5),(16, 3),(14, 1),(11, 0),(4, 0),(-1, -1)],
#Ascii 69
[(11,19),(4, 21),(4, 0),(-1, -1),(4, 21),(17, 21),(-1, -1),(4, 11),(12, 11),(-1, -1),(4, 0),(17, 0),(-1, -1)],
#Ascii 70
[(8,18),(4, 21),(4, 0),(-1, -1),(4, 21),(17, 21),(-1, -1),(4, 11),(12, 11),(-1, -1)],
#Ascii 71
[(22,21),(18, 16),(17, 18),(15, 20),(13, 21),(9, 21),(7, 20),(5, 18),(4, 16),(3, 13),(3, 8),(4, 5),(5, 3),(7, 1),(9, 0),(13, 0),(15, 1),(17, 3),(18, 5),(18, 8),(-1, -1),(13, 8),(18, 8),(-1, -1)],
#Ascii 72
[(8,22),(4, 21),(4, 0),(-1, -1),(18, 21),(18, 0),(-1, -1),(4, 11),(18, 11),(-1, -1)],
#Ascii 73
[(2,8),(4, 21),(4, 0),(-1, -1)],
#Ascii 74
[(10,16),(12, 21),(12, 5),(11, 2),(10, 1),(8, 0),(6, 0),(4, 1),(3, 2),(2, 5),(2, 7),(-1, -1)],
#Ascii 75
[(8,21),(4, 21),(4, 0),(-1, -1),(18, 21),(4, 7),(-1, -1),(9, 12),(18, 0),(-1, -1)],
#Ascii 76
[(5,17),(4, 21),(4, 0),(-1, -1),(4, 0),(16, 0),(-1, -1)],
#Ascii 77
[(11,24),(4, 21),(4, 0),(-1, -1),(4, 21),(12, 0),(-1, -1),(20, 21),(12, 0),(-1, -1),(20, 21),(20, 0),(-1, -1)],
#Ascii 78
[(8,22),(4, 21),(4, 0),(-1, -1),(4, 21),(18, 0),(-1, -1),(18, 21),(18, 0),(-1, -1)],
#Ascii 79
[(21,22),(9, 21),(7, 20),(5, 18),(4, 16),(3, 13),(3, 8),(4, 5),(5, 3),(7, 1),(9, 0),(13, 0),(15, 1),(17, 3),(18, 5),(19, 8),(19, 13),(18, 16),(17, 18),(15, 20),(13, 21),(9, 21),(-1, -1)],
#Ascii 80
[(13,21),(4, 21),(4, 0),(-1, -1),(4, 21),(13, 21),(16, 20),(17, 19),(18, 17),(18, 14),(17, 12),(16, 11),(13, 10),(4, 10),(-1, -1)],
#Ascii 81
[(24,22),(9, 21),(7, 20),(5, 18),(4, 16),(3, 13),(3, 8),(4, 5),(5, 3),(7, 1),(9, 0),(13, 0),(15, 1),(17, 3),(18, 5),(19, 8),(19, 13),(18, 16),(17, 18),(15, 20),(13, 21),(9, 21),(-1, -1),(12, 4),(18, -2),(-1, -1)],
#Ascii 82
[(16,21),(4, 21),(4, 0),(-1, -1),(4, 21),(13, 21),(16, 20),(17, 19),(18, 17),(18, 15),(17, 13),(16, 12),(13, 11),(4, 11),(-1, -1),(11, 11),(18, 0),(-1, -1)],
#Ascii 83
[(20,20),(17, 18),(15, 20),(12, 21),(8, 21),(5, 20),(3, 18),(3, 16),(4, 14),(5, 13),(7, 12),(13, 10),(15, 9),(16, 8),(17, 6),(17, 3),(15, 1),(12, 0),(8, 0),(5, 1),(3, 3),(-1, -1)],
#Ascii 8,4
[(5,16),(8, 21),(8, 0),(-1, -1),(1, 21),(15, 21),(-1, -1)],
#Ascii 85
[(10,22),(4, 21),(4, 6),(5, 3),(7, 1),(10, 0),(12, 0),(15, 1),(17, 3),(18, 6),(18, 21),(-1, -1)],
#Ascii 86
[(5,18),(1, 21),(9, 0),(-1, -1),(17, 21),(9, 0),(-1, -1)],
#Ascii 87
[(11,24),(2, 21),(7, 0),(-1, -1),(12, 21),(7, 0),(-1, -1),(12, 21),(17, 0),(-1, -1),(22, 21),(17, 0),(-1, -1)],
#Ascii 88
[(5,20),(3, 21),(17, 0),(-1, -1),(17, 21),(3, 0),(-1, -1)],
#Ascii 89
[(6,18),(1, 21),(9, 11),(9, 0),(-1, -1),(17, 21),(9, 11),(-1, -1)],
#Ascii 90
[(8,20),(17, 21),(3, 0),(-1, -1),(3, 21),(17, 21),(-1, -1),(3, 0),(17, 0),(-1, -1)],
#Ascii 91
[(11,14),(4, 25),(4, -7),(-1, -1),(5, 25),(5, -7),(-1, -1),(4, 25),(11, 25),(-1, -1),(4, -7),(11, -7),(-1, -1)],
#Ascii 92
[(2,14),(0, 21),(14, -3),(-1, -1)],
#Ascii 93
[(11,14),(9, 25),(9, -7),(-1, -1),(10, 25),(10, -7),(-1, -1),(3, 25),(10, 25),(-1, -1),(3, -7),(10, -7),(-1, -1)],
#Ascii 94
[(10,16),(6, 15),(8, 18),(10, 15),(-1, -1),(3, 12),(8, 17),(13, 12),(-1, -1),(8, 17),(8, 0),(-1, -1)],
#Ascii 95
[(2,16),(0, -2),(16, -2),(-1, -1)],
#Ascii 96
[(7,10),(6, 21),(5, 20),(4, 18),(4, 16),(5, 15),(6, 16),(5, 17),(-1, -1)],
#Ascii 97
[(17,19),(15, 14),(15, 0),(-1, -1),(15, 11),(13, 13),(11, 14),(8, 14),(6, 13),(4, 11),(3, 8),(3, 6),(4, 3),(6, 1),(8, 0),(11, 0),(13, 1),(15, 3),(-1, -1)],
#Ascii 98
[(17,19),(4, 21),(4, 0),(-1, -1),(4, 11),(6, 13),(8, 14),(11, 14),(13, 13),(15, 11),(16, 8),(16, 6),(15, 3),(13, 1),(11, 0),(8, 0),(6, 1),(4, 3),(-1, -1)],
#Ascii 99
[(14,18),(15, 11),(13, 13),(11, 14),(8, 14),(6, 13),(4, 11),(3, 8),(3, 6),(4, 3),(6, 1),(8, 0),(11, 0),(13, 1),(15, 3),(-1, -1)],
#Ascii 100
[(17,19),(15, 21),(15, 0),(-1, -1),(15, 11),(13, 13),(11, 14),(8, 14),(6, 13),(4, 11),(3, 8),(3, 6),(4, 3),(6, 1),(8, 0),(11, 0),(13, 1),(15, 3),(-1, -1)],
#Ascii 101
[(17,18),(3, 8),(15, 8),(15, 10),(14, 12),(13, 13),(11, 14),(8, 14),(6, 13),(4, 11),(3, 8),(3, 6),(4, 3),(6, 1),(8, 0),(11, 0),(13, 1),(15, 3),(-1, -1)],
#Ascii 102
[(8,12),(10, 21),(8, 21),(6, 20),(5, 17),(5, 0),(-1, -1),(2, 14),(9, 14),(-1, -1)],
#Ascii 103
[(22,19),(15, 14),(15, -2),(14, -5),(13, -6),(11, -7),(8, -7),(6, -6),(-1, -1),(15, 11),(13, 13),(11, 14),(8, 14),(6, 13),(4, 11),(3, 8),(3, 6),(4, 3),(6, 1),(8, 0),(11, 0),(13, 1),(15, 3),(-1, -1)],
#Ascii 104
[(10,19),(4, 21),(4, 0),(-1, -1),(4, 10),(7, 13),(9, 14),(12, 14),(14, 13),(15, 10),(15, 0),(-1, -1)],
#Ascii 105
[(8,8),(3, 21),(4, 20),(5, 21),(4, 22),(3, 21),(-1, -1),(4, 14),(4, 0),(-1, -1)],
#Ascii 106
[(11,10),(5, 21),(6, 20),(7, 21),(6, 22),(5, 21),(-1, -1),(6, 14),(6, -3),(5, -6),(3, -7),(1, -7),(-1, -1)],
#Ascii 107
[(8,17),(4, 21),(4, 0),(-1, -1),(14, 14),(4, 4),(-1, -1),(8, 8),(15, 0),(-1, -1)],
#Ascii 108
[(2,8),(4, 21),(4, 0),(-1, -1),(18, 30),(-1,-1)],
#Ascii 109
[(18,30), (4,14),(4, 0),(-1,-1),(4,10),(7,13),(9,14),(12,14),(14,13),(15,10),(15, 0),(-1,-1),(15,10),(18,13),(20,14),(23,14),(25,13),(26,10),(26, 0),(-1,-1)],
#Ascii 110
[(10,19),(4, 14),(4, 0),(-1, -1),(4, 10),(7, 13),(9, 14),(12, 14),(14, 13),(15, 10),(15, 0),(-1, -1),(17, 19),(-1,-1)],
#Ascii 111 */
[(17,19),(8,14), (6,13), (4,11), (3, 8), (3, 6), (4, 3), (6, 1), (8, 0),(11, 0),(13, 1),(15, 3),(16,6),(16, 8),(15,11),(13,13),(11,14), (8,14), (-1,-1),(-1,-1)],
#Ascii 112
[(17,19),(4, 14),(4, -7),(-1, -1),(4, 11),(6, 13),(8, 14),(11, 14),(13, 13),(15, 11),(16, 8),(16, 6),(15, 3),(13, 1),(11, 0),(8, 0),(6, 1),(4, 3),(-1, -1),(17, 19),(-1,-1)],
#Ascii 113,
[(17,19), (15,14),(15,-7),(-1,-1),(15,11),(13,13),(11,14), (8,14), (6,13), (4,11), (3, 8), (3, 6), (4,3), (6, 1), (8, 0),(11, 0),(13, 1),(15, 3), (-1,-1), (-1,-1)],
#Ascii 114
[(8,13),(4, 14),(4, 0),(-1, -1),(4, 8),(5, 11),(7, 13),(9, 14),(12, 14),(-1, -1)],
#Ascii 115
[(17,17),(14, 11),(13, 13),(10, 14),(7, 14),(4, 13),(3, 11),(4, 9),(6, 8),(11, 7),(13, 6),(14, 4),(14, 3),(13, 1),(10, 0),(7, 0),(4, 1),(3, 3),(-1, -1)],
#Ascii 116
[(8,12),(5, 21),(5, 4),(6, 1),(8, 0),(10, 0),(-1, -1),(2, 14),(9, 14),(-1, -1)],
#Ascii 117
[(10,19),(4, 14),(4, 4),(5, 1),(7, 0),(10, 0),(12, 1),(15, 4),(-1, -1),(15, 14),(15, 0),(-1, -1)],
#Ascii 118
[(5,16),(2, 14),(8, 0),(-1, -1),(14, 14),(8, 0),(-1, -1)],
#Ascii 119
[(11,22),(3, 14),(7, 0),(-1, -1),(11, 14),(7, 0),(-1, -1),(11, 14),(15, 0),(-1, -1),(19, 14),(15, 0),(-1, -1)],
#Ascii 120
[(5,17),(3, 14),(14, 0),(-1, -1),(14, 14),(3, 0),(-1, -1)],
#Ascii 121
[(9,16),(2, 14),(8, 0),(-1, -1),(14, 14),(8, 0),(6, -4),(4, -6),(2, -7),(1, -7),(-1, -1)],
#Ascii 122
[(8,17),(14, 14),(3, 0),(-1, -1),(3, 14),(14, 14),(-1, -1),(3, 0),(14, 0),(-1, -1)],
#Ascii 123
[(39,14),(9, 25),(7, 24),(6, 23),(5, 21),(5, 19),(6, 17),(7, 16),(8, 14),(8, 12),(6, 10),(-1, -1),(7, 24),(6, 22),(6, 20),(7, 18),(8, 17),(9, 15),(9, 13),(8, 11),(4, 9),(8, 7),(9, 5),(9, 3),(8, 1),(7, 0),(6, -2),(6, -4),(7, -6),(-1, -1),(6, 8),(8, 6),(8, 4),(7, 2),(6, 1),(5, -1),(5, -3),(6, -5),(7, -6),(9, -7),(-1, -1)],
#Ascii 124
[(2,8),(4, 25),(4, -7),(-1, -1)],
#Ascii 125
[(39,14),(5, 25),(7, 24),(8, 23),(9, 21),(9, 19),(8, 17),(7, 16),(6, 14),(6, 12),(8, 10),(-1, -1),(7, 24),(8, 22),(8, 20),(7, 18),(6, 17),(5, 15),(5, 13),(6, 11),(10, 9),(6, 7),(5, 5),(5, 3),(6, 1),(7, 0),(8, -2),(8, -4),(7, -6),(-1, -1),(8, 8),(6, 6),(6, 4),(7, 2),(8, 1),(9, -1),(9, -3),(8, -5),(7, -6),(5, -7),(-1, -1)],
#Ascii 126
[(23,24),(3, 6),(3, 8),(4, 11),(6, 12),(8, 12),(10, 11),(14, 8),(16, 7),(18, 7),(20, 8),(21, 10),(-1, -1),(3, 8),(4, 10),(6, 11),(8, 11),(10, 10),(14, 7),(16, 6),(18, 6),(20, 7),(21, 10),(21, 12),(-1, -1)]]

292
trap/laser_calibration.py
View file

@ -1,292 +0,0 @@
from argparse import ArgumentParser
import enum
import json
from pathlib import Path
import time
from typing import Optional
import cv2
import numpy as np
from trap.base import DataclassJSONEncoder, DistortedCamera, Frame
from trap.lines import CoordinateSpace, RenderableLine, RenderableLines, RenderablePoint, RenderablePosition, SrgbaColor, cross_points
from trap.node import Node
from trap.stage import Coordinate
class Modes(enum.Enum):
POINTS = 1
TEST_LINE = 2
class LaserCalibration(Node):
"""
A calibrated camera can be used to reverse-map the points of the laser to world coordinates.
Note, it publishes on the address of the stage node, so they cannot run at the same time.
1. Draw points with the laser (use 1-9 to create/select, then position them with arrow keys)
2. Use cursor on camera stream to create an image point for.
- Locate nearby point to select and drag
3. Use image coordinate of point, undistort, homograph, gives world coordinate.
4. Perform homography on world coordinates + laser coordinates
"""
def setup(self):
# self.scenarios: List[DrawnScenario] = []
self.frame_sock = self.sub(self.config.zmq_frame_addr)
self.laser_sock = self.pub(self.config.zmq_stage_addr)
self.camera: Optional[DistortedCamera] = None
self._selected_point = None
self._is_dragging = False
self.laser_points = {}
self.image_points = {}
self.mode = Modes.POINTS
self.H = None
self.img_size = (1920,1080)
self.frame_img_factor = (1,1)
if self.config.calibfile.exists():
with self.config.calibfile.open('r') as fp:
calibdata = json.load(fp)
self.laser_points = calibdata['laser_points']
self.image_points = calibdata['image_points']
self.H = calibdata['H']
def run(self):
cv2.namedWindow("laser_calib", cv2.WINDOW_NORMAL)
# https://gist.github.com/ronekko/dc3747211543165108b11073f929b85e
# cv2.moveWindow("laser_calib", 0, -1)
cv2.setMouseCallback('laser_calib',self.mouse_event)
cv2.setWindowProperty("laser_calib",cv2.WND_PROP_FULLSCREEN,cv2.WINDOW_FULLSCREEN)
# arrow up (82), down (84), arrow left(81)
frame = None
while self.run_loop_capped_fps(60):
if self.frame_sock.poll(0):
frame: Frame = self.frame_sock.recv_pyobj()
if not self.camera:
self.camera = frame.camera
if frame is None:
continue
self.frame_img_factor = frame.img.shape[1] / self.img_size[0], frame.img.shape[0] / self.img_size[1]
img = frame.img
img = cv2.resize(img, self.img_size)
cv2.putText(img, 'press 1-0 to create/edit points', (10,20), cv2.FONT_HERSHEY_SIMPLEX, .5, (255,255,255))
if len(self.laser_points) < 4:
cv2.putText(img, 'add points to calculate homography', (10,40), cv2.FONT_HERSHEY_SIMPLEX, .5, (255,255,255))
else:
cv2.putText(img, 'press c to calculate homography', (10,40), cv2.FONT_HERSHEY_SIMPLEX, .5, (255,255,0))
cv2.putText(img, str(self.config.calibfile), (10,self.img_size[1]-30), cv2.FONT_HERSHEY_SIMPLEX, .5, (255,255,0))
if self._selected_point:
color = (0,255,255)
cv2.putText(img, f'selected {self._selected_point}', (10,60), cv2.FONT_HERSHEY_SIMPLEX, .5, color)
cv2.putText(img, 'press d to delete', (10,80), cv2.FONT_HERSHEY_SIMPLEX, .5, color)
cv2.putText(img, 'use arrows to position laser for this point', (10,100), cv2.FONT_HERSHEY_SIMPLEX, .5, color)
target = self.camera.points_img_to_world([self.image_points[self._selected_point]])[0].tolist()
target = round(target[0], 2), round(target[1], 2)
cv2.putText(img, f'map {self.laser_points[self._selected_point]} to {target} ({self.image_points[self._selected_point]})', (10,120), cv2.FONT_HERSHEY_SIMPLEX, .5, color)
for k, coord in self.image_points.items():
color = (0,0,255) if self._selected_point == k else (255,0,0)
coord = int(coord[0] / self.frame_img_factor[0]), int(coord[1] / self.frame_img_factor[1])
cv2.circle(img, coord, 4, color, thickness=2)
cv2.putText(img, str(k), (coord[0]+10, coord[1]), cv2.FONT_HERSHEY_SIMPLEX, .5, color)
key = cv2.waitKey(5) # or for arrows: full_key_code = cv2.waitKeyEx(0)
self.key_event(key)
# nr_keys = [ord(i) for i in range(10)] # select/add point
# cv2.
cv2.imshow('laser_calib', img)
lines = []
if self.mode == Modes.TEST_LINE:
lines.append(RenderableLine([
RenderablePoint((i,time.time()%18), SrgbaColor(0,1,0,1)) for i in range(-15, 40)
]))
# render in laser space
rl = RenderableLines(lines, CoordinateSpace.WORLD)
self.laser_sock.send_json(rl, cls=DataclassJSONEncoder)
else:
if self._selected_point:
point = self.laser_points[self._selected_point]
lines.extend(cross_points(point[0], point[1], .5, SrgbaColor(0,1,0,1)))
# render in laser space
rl = RenderableLines(lines, CoordinateSpace.LASER)
self.laser_sock.send_json(rl, cls=DataclassJSONEncoder)
# print(json.dumps(rl, cls=DataclassJSONEncoder))
def key_event(self, key: int):
if key < 0:
return
if key == ord('q'):
exit()
if key == 27: #esc
self._selected_point = None
if key == ord('c'):
self.calculate_homography()
self.save()
if key == ord('d') and self._selected_point:
self.delete_point(self._selected_point)
if key == ord('t'):
self.mode = Modes.TEST_LINE if self.mode == Modes.POINTS else Modes.POINTS
print(self.mode)
# arrow up (82), down (84), arrow left(81)
if self._selected_point and key in [81, 84, 82, 83,
ord('h'), ord('j'), ord('k'), ord('l'),
ord('H'), ord('J'), ord('K'), ord('L'),
]:
diff = [0,0]
if key in [81, ord('h')]:
diff[0] -= 1
if key == ord('H'):
diff[0] -= 10
if key in [83, ord('l')]:
diff[0] += 1
if key == ord('L'):
diff[0] += 10
if key in [82, ord('k')]:
diff[1] += 1
if key == ord('K'):
diff[1] += 10
if key in [84, ord('j')]:
diff[1] -= 1
if key == ord('J'):
diff[1] -= 10
self.laser_points[self._selected_point] = (
self.laser_points[self._selected_point][0] + diff[0],
self.laser_points[self._selected_point][1] + diff[1],
)
nr_keys = [ord(str(i)) for i in range(10)]
if key in nr_keys:
select = str(nr_keys.index(key))
self.create_or_select(select)
def mouse_event(self, event,x,y,flags,param):
x *= self.frame_img_factor[0]
y *= self.frame_img_factor[1]
if event == cv2.EVENT_MOUSEMOVE:
if not self._is_dragging or not self._selected_point:
return
self.image_points[self._selected_point] = (x, y)
if event == cv2.EVENT_LBUTTONDOWN:
# select or create
self._selected_point = None
for i, p in self.image_points.items():
d = (p[0]-x)**2 + (p[1]-y)**2
if d < 30:
self._selected_point = i
break
if self._selected_point is None:
self._selected_point = self.new_point((x,y), None)
self._is_dragging = True
if event == cv2.EVENT_LBUTTONUP:
self._is_dragging = False
# ... point stays selected to tweak laser
def create_or_select(self, nr: str):
if nr not in self.image_points:
self.new_point(None, None, nr)
self._selected_point = nr
return nr
def new_point(self, img_coord: Optional[Coordinate], laser_coord: Optional[Coordinate], nr: Optional[str]=None):
if nr:
new_nr = nr
else:
new_nr = None
for i in range(100):
k = str(i)
if k not in self.image_points:
new_nr = k
break
if not new_nr:
new_nr = 0 # cover unlikely case
self.image_points[new_nr] = img_coord or (100,100)
self.laser_points[new_nr] = laser_coord or (100,100)
return new_nr
def delete_point(self, point: str):
del self.image_points[point]
del self.laser_points[point]
self._selected_point = None
def calculate_homography(self):
if len(self.image_points) < 4:
return
world_points = self.camera.points_img_to_world(list(self.image_points.values()))
laser_points = np.array(list(self.laser_points.values()))
print('from', world_points)
print('to', laser_points)
self.H, status = cv2.findHomography(world_points, laser_points)
print('Found')
print(self.H)
def save(self):
with self.config.calibfile.open('w') as fp:
json.dump({
'laser_points': self.laser_points,
'image_points': self.image_points,
'H': self.H.tolist()
}, fp)
@classmethod
def arg_parser(cls) -> ArgumentParser:
argparser = ArgumentParser()
argparser.add_argument('--zmq-frame-addr',
help='Manually specity communication addr for the frame messages',
type=str,
default="ipc:///tmp/feeds_frame")
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('--calibfile',
help='specify file to save & load points with',
type=Path,
default=Path("./laser_calib.json"))
return argparser

693
trap/laser_renderer.py
View file

@ -1,693 +0,0 @@
# used for "Forward Referencing of type annotations"
from __future__ import annotations
import time
import ffmpeg
from argparse import Namespace
import datetime
import logging
from multiprocessing import Event
from multiprocessing.synchronize import Event as BaseEvent
import cv2
import numpy as np
import json
import pyglet
import pyglet.event
import zmq
import tempfile
from pathlib import Path
import shutil
import math
from typing import Dict, Iterable, Optional
from pyglet import shapes
from PIL import Image
# from trap.scenarios import TrackScenario
from trap.counter import CounterSender
from trap.frame_emitter import DetectionState, Frame, Track, Camera
# from trap.helios import HeliosDAC, HeliosPoint
from trap.preview_renderer import PROJECTION_MAP, DrawnTrack, FrameWriter
from trap.tools import draw_track, draw_track_predictions, draw_track_projected, draw_trackjectron_history, drawntrack_predictions_to_lines, to_point, track_predictions_to_lines
from trap.utils import convert_world_points_to_img_points, convert_world_space_to_img_space, lerp
logger = logging.getLogger("trap.laser_renderer")
import ctypes
class LaserFrame():
def __init__(self, paths: list[LaserPath]):
self.paths = paths
def point_count(self):
return sum([len(p.points) for p in self.paths])
# def closest_path(cls, point, paths):
# distances = [min(p.last()-)]
# def optimise_paths_lazy(self, last_point = None):
# """Quick way to optimise order of paths
# last_point can be the ending point of previous frame.
# """
# ordered_paths = []
# if not last_point:
# ordered_paths.append(self.paths.pop(0))
# last_point = endpoint
# pass
def as_cropped_to_projector(self):
paths = []
for path in self.paths:
p = path.as_cropped_to_projector()
if len(p.points):
paths.append(p)
return LaserFrame(paths)
def get_points_interpolated_by_distance(self, point_interval, last_point: Optional[LaserPoint] = None) -> list[LaserPoint]:
"""
Interpolate the gaps between paths (NOT THE PATHS THEMSELVES)
point_interval is the maximum interval at which a new point should be added
"""
points: list[LaserPoint] = []
for path in self.paths:
if last_point:
a = last_point
b = path.first()
dx = b.x - a.x
dy = b.y - a.y
distance = np.linalg.norm([dx,dy])
steps = int(distance // point_interval)
for step in range(steps+1): # have both 0 and 1 in the lerp for empty points
t = step/(steps+1)
t = 1 # just asap to starting point of next shape
x = int(lerp(a.x, b.x, t))
y = int(lerp(a.y, b.y, t))
points.append(LaserPoint(x,y, (0,0,0), 0, True))
# print('append', steps)
points.extend(path.points)
last_point = path.last()
return points
class LaserPath():
def __init__(self, points: list[LaserPoint] = []):
# if len(points) < 1:
# raise RuntimeError("LaserPath should have some points")
self.points = points
def last(self):
return self.points[-1]
def first(self):
return self.points[0]
def as_array(self):
np.array([[p.x, p.y] for p in self.points])
def as_cropped_to_projector(self):
"""Make sure all points fall within range of laser"""
points = [p for p in self.points if p.x >= 0 and p.y >= 0 and p.x < 0xFFF and p.y < 0xFFF ]
return LaserPath(points)
def simplyfied_path(self, start_v= 10., max_v= 20., a = 2):
"""walk over the path with specific velocity,
continuously accelerate (a) until max_v is reached
place point at each step
(see also tools.transition_path_points() )
"""
if len(self.points) < 1:
return self.points
path = self.as_array()
# new_path = np.array([])
lengths = np.sqrt(np.sum(np.diff(path, axis=0)**2, axis=1))
cum_lenghts = np.cumsum(lengths)
# distance = cum_lenghts[-1] * t
# ts = np.concatenate((np.array([0.]), cum_lenghts / cum_lenghts[-1]))
# print(cum_lenghts[-1])
# DRAW_SPEED = 35 # fixed speed (independent of lenght) TODO)) make variable
# ts = np.concatenate((np.array([0.]), cum_lenghts / DRAW_SPEED))
new_path = [path[0]]
position = 0
next_pos = position + v
for a, b, pos in zip(path[:-1], path[1:], cum_lenghts):
# TODO))
if pos < (next_pos):
continue
v = min(v+a, max_v)
next_pos = position + v
relative_t = inv_lerp(t_a, t_b, t)
pass
# for a, b, t_a, t_b in zip(path[:-1], path[1:], ts[:-1], ts[1:]):
# if t_b < t:
# new_path.append(b)
# continue
# # interpolate
# relative_t = inv_lerp(t_a, t_b, t)
# x = lerp(a[0], b[0], relative_t)
# y = lerp(a[1], b[1], relative_t)
# new_path.append([x,y])
# break
# return np.array(new_path)
class LaserPoint():
def __init__(self,x,y,c: Color = (255,0,0),i= 255,blank=False):
self.x = x
self.y = y
self.c = c
self._i = i
self.blank = blank
@property
def color(self):
if self.blank: return (0,0,0)
return self.c
@property
def i(self):
return 0 if self.blank else self._i
#Define point structure
class CHeliosPoint(ctypes.Structure):
#_pack_=1
_fields_ = [('x', ctypes.c_uint16),
('y', ctypes.c_uint16),
('r', ctypes.c_uint8),
('g', ctypes.c_uint8),
('b', ctypes.c_uint8),
('i', ctypes.c_uint8)]
class LaserRenderer:
def __init__(self, config: Namespace, is_running: BaseEvent):
self.config = config
self.is_running = is_running
context = zmq.Context()
self.prediction_sock = context.socket(zmq.SUB)
self.prediction_sock.setsockopt(zmq.CONFLATE, 1) # only keep latest frame. NB. make sure this comes BEFORE connect, otherwise it's ignored!!
self.prediction_sock.setsockopt(zmq.SUBSCRIBE, b'')
# self.prediction_sock.connect(config.zmq_prediction_addr if not self.config.bypass_prediction else config.zmq_trajectory_addr)
self.prediction_sock.connect(config.zmq_prediction_addr)
self.tracker_sock = context.socket(zmq.SUB)
self.tracker_sock.setsockopt(zmq.CONFLATE, 1) # only keep latest frame. NB. make sure this comes BEFORE connect, otherwise it's ignored!!
self.tracker_sock.setsockopt(zmq.SUBSCRIBE, b'')
self.tracker_sock.connect(config.zmq_trajectory_addr)
self.H = self.config.H
self.inv_H = np.linalg.pinv(self.H)
# TODO: get FPS from frame_emitter
# self.out = cv2.VideoWriter(str(filename), fourcc, 23.97, (1280,720))
self.fps = 60
self.frame_size = (self.config.camera.w,self.config.camera.h)
self.first_time: float|None = None
self.frame: Frame|None= None
self.tracker_frame: Frame|None = None
self.prediction_frame: Frame|None = None
self.tracks: Dict[str, Track] = {}
# self.scenarios: Dict[str, TrackScenario] = {}
self.predictions: Dict[str, Track] = {}
self.drawn_tracks: Dict[str, DrawnTrack] = {}
self.helios = ctypes.cdll.LoadLibrary("./trap/helios_dac/libHeliosDacAPI.so")
numDevices = self.helios.OpenDevices()
logger.info(f"Found {numDevices} Helios DACs")
# self.dac = HeliosDAC(debug=False)
# logger.info(f"{self.dac.dev}")
# logger.info(f"{self.dac.GetName()}")
# logger.info(f"{self.dac.getHWVersion()}")
# logger.info(f"Helios version: {self.dac.getHWVersion()}")
# self.init_shapes()
# self.init_labels()
def check_frames(self, dt):
new_tracks = False
try:
self.frame: Frame = self.frame_sock.recv_pyobj(zmq.NOBLOCK)
if not self.first_time:
self.first_time = self.frame.time
img = cv2.GaussianBlur(self.frame.img, (15, 15), 0)
img = cv2.flip(cv2.cvtColor(img, cv2.COLOR_BGR2RGB), 0)
img = pyglet.image.ImageData(self.frame_size[0], self.frame_size[1], 'RGB', img.tobytes())
# don't draw in batch, so that it is the background
self.video_sprite = pyglet.sprite.Sprite(img=img, batch=self.batch_bg)
self.video_sprite.opacity = 100
except zmq.ZMQError as e:
# idx = frame.index if frame else "NONE"
# logger.debug(f"reuse video frame {idx}")
pass
try:
self.prediction_frame: Frame = self.prediction_sock.recv_pyobj(zmq.NOBLOCK)
new_tracks = True
except zmq.ZMQError as e:
pass
try:
self.tracker_frame: Frame = self.tracker_sock.recv_pyobj(zmq.NOBLOCK)
new_tracks = True
except zmq.ZMQError as e:
pass
def run(self, timer_counter):
frame = None
prediction_frame = None
tracker_frame = None
i=0
first_time = None
kpps = 50000
# frames = [0 for x in range(30)]
# frameTr= CHeliosPoint(int(x),int(y),20,20,20,255)
# print(frames)
pointlist_test = []
# pointlist_test.append(HeliosPoint(10,10, blank=False))
for i in range(30):
if i < 15:
# y = int(i*0xfff/500)
y = int(i*10 + 0xfff/2)
else:
# y = int((15-i)*0xfff/500)
y = int((15-i)*10 + 0xfff/2)
pointlist_test.append(LaserPoint(int(0),0xfff-y, blank=False))
# pointlist_test.append(HeliosPoint(10,0xfff, blank=False))
# pointlist_test.append(HeliosPoint(8000,8000, blank=False))
# pointlist_test.append(HeliosPoint(8000,10, blank=False))
# pointlist_test.append(HeliosPoint(10,10, blank=True))
# frameType = CHeliosPoint * len(pointlist_test)
# frame = frameType()
# for j, point in enumerate(pointlist_test):
# frame[j] = CHeliosPoint(point.x, point.y, 0,40,0,0 if point.blank else 255)
counter = CounterSender()
print(f"RENDER DAC\n\n\n")
last_laser_point = None
# for i in range(150):
while self.is_running.is_set():
# Make 512 attempts for DAC status to be ready. After that, just give up and try to write the frame anyway
# statusAttempts=0
# while (statusAttempts < 512 and self.helios.GetStatus(0) != 1):
# statusAttempts += 1
# self.helios.WriteFrame(0, kpps, 0, ctypes.pointer(frame), len(pointlist))
# continue
i+=1
with timer_counter.get_lock():
timer_counter.value+=1
try:
prediction_frame: Frame = self.prediction_sock.recv_pyobj(zmq.NOBLOCK)
for track_id, track in prediction_frame.tracks.items():
prediction_id = f"{track_id}-{track.history[-1].frame_nr}"
self.predictions[prediction_id] = track
# TODO)) also for tracks:
if track_id not in self.drawn_tracks:
self.drawn_tracks[track_id] = DrawnTrack(track_id, track, self, prediction_frame.camera.H, PROJECTION_MAP, prediction_frame.camera)
elif self.drawn_tracks[track_id].update_predictions_at < (time.time() - .5): # TODO)) only update predictions every n frames. configure
# self.drawn_tracks[track_id].pred_track
self.drawn_tracks[track_id].set_predictions(track)
# if track_id in self.scenarios:
# self.scenarios[track_id].set_prediction(track)
# self.drawn_predictions[track_id] = track
except zmq.ZMQError as e:
logger.debug(f'reuse prediction')
try:
tracker_frame: Frame = self.tracker_sock.recv_pyobj(zmq.NOBLOCK)
for track_id, track in tracker_frame.tracks.items():
self.tracks[track_id] = track
# if not track_id in self.scenarios:
# self.scenarios[track_id] = TrackScenario(track)
# else:
# self.scenarios[track_id].set_track(track)
# self.scenarios[track_id].receive_track(track)
except zmq.ZMQError as e:
logger.debug(f'reuse tracks')
# if tracker_frame is None:
# # might need to wait a few iterations before first frame comes available
# time.sleep(.1)
# continue
if first_time is None and tracker_frame is not None:
first_time = tracker_frame.time
# print('-------')
paths = render_frame_to_pathlist( tracker_frame, prediction_frame, self.drawn_tracks, first_time, self.config, self.tracks, self.predictions, self.config.render_clusters)
counter.set('paths', len(paths))
counter.set('points', sum([len(p.points) for p in paths]))
if self.prediction_frame:
counter.set('pred_render_latency', time.time() - self.prediction_frame.time)
if self.tracker_frame:
counter.set('track_render_latency', time.time() - self.tracker_frame.time)
# print(f"Paths: {len(paths)} ... points {sum([len(p.points) for p in paths])}")
laserframe = LaserFrame(paths)
laserframe_cropped = laserframe.as_cropped_to_projector()
counter.set('laser.removed', laserframe_cropped.point_count() - laserframe.point_count())
if laserframe.point_count() > laserframe_cropped.point_count():
# logger.warning("Removed laser points out of frame!")
laserframe = laserframe_cropped
# pointlist=pointlist_test
# print([(p.x, p.y) for p in pointlist])
# pointlist.extend(pointlist_test)
pointlist = laserframe.get_points_interpolated_by_distance(30, last_laser_point)
# pointlist_cropped =
# pointlist = pointlist[::2]
# print('decimated', len(pointlist))
if len(pointlist):
last_laser_point = pointlist[-1]
frameType = CHeliosPoint * len(pointlist)
frame = frameType()
# print(len(pointlist)) #, last_laser_point.x, last_laser_point.y)
for j, point in enumerate(pointlist):
frame[j] = CHeliosPoint(int(point.x), int(point.y), point.color[0],point.color[1], point.color[2], point.i)
# Make 512 attempts for DAC status to be ready. After that, just give up and try to write the frame anyway
statusAttempts=0
while (statusAttempts < 512 and self.helios.GetStatus(0) != 1):
statusAttempts += 1
self.helios.WriteFrame(0, kpps, 0, ctypes.pointer(frame), len(pointlist))
# continue
# self.helios.WriteFrame(0, kpps, 0, ctypes.pointer(frame), len(pointlist))
# self.dac.newFrame(50000, pointlist)
# clear out old tracks & predictions:
for track_id, track in list(self.tracks.items()):
# TODO)) Migrate to using time() instead of framenr, to detach the two
if get_animation_position(track, tracker_frame) == 1:
self.tracks.pop(track_id)
for prediction_id, track in list(self.predictions.items()):
if get_animation_position(track, tracker_frame) == 1:
self.predictions.pop(prediction_id)
for track_id in list(self.drawn_tracks.keys()):
# TODO make delay configurable
if self.drawn_tracks[track_id].update_at < time.time() - 5:
# TODO fade out
del self.drawn_tracks[track_id]
logger.info('Stopping')
self.helios.CloseDevices()
# if i>2:
logger.info('stopped')
# colorset = itertools.product([0,255], repeat=3) # but remove white
# colorset = [(0, 0, 0),
# (0, 0, 255),
# (0, 255, 0),
# (0, 255, 255),
# (255, 0, 0),
# (255, 0, 255),
# (255, 255, 0)
# ]
colorset = [
(255,255,100),
(255,100,255),
(100,255,255),
]
# colorset = [
# (0,0,0),
# ]
def get_animation_position(track: Track, current_frame: Frame) -> float:
fade_duration = current_frame.camera.fps * 2
diff = current_frame.index - track.history[-1].frame_nr
return max(0, min(1, diff / fade_duration))
# track.history[-1].frame_nr < (current_frame.index - current_frame.camera.fps * 3)
# track.history[-1].frame_nr < (current_frame.index - current_frame.camera.fps * 3)
def circle_points(cx, cy, r, c: Color):
# r = r
steps = 30
pointlist: list[LaserPoint] = []
for i in range(steps):
x = int(cx + math.cos(i * (2*math.pi)/steps) * r)
y = int(cy + math.sin(i * (2*math.pi)/steps)* r)
pointlist.append(LaserPoint(x, y, c, blank=(i==(steps-1)or i==0)))
return pointlist
Color = tuple[int, int, int]
# derived with trap/helios_dac/calibration_points.py
# set points in the script to points from hof3/irl_points.json
laser_H =np.array([[ 2.47442963e+02, -7.01714050e+01, -9.71749119e+01],
[ 1.02328119e+01, 1.47185254e+02, 1.96295638e+02],
[-1.20921986e-03, -3.32735973e-02, 1.00000000e+00]])
def world_points_to_laser_points(points):
return cv2.perspectiveTransform(np.array([points]), laser_H)
# Deprecated
def render_frame_to_pathlist(tracker_frame: Optional[Frame], prediction_frame: Optional[Frame], drawn_tracks: Optional[Dict[str, DrawnTrack]], first_time: Optional[float], config: Namespace, tracks: Dict[str, Track], predictions: Dict[str, Track], as_clusters = True):
# TODO: replace opencv with QPainter to support alpha? https://doc.qt.io/qtforpython-5/PySide2/QtGui/QPainter.html#PySide2.QtGui.PySide2.QtGui.QPainter.drawImage
# or https://github.com/pygobject/pycairo?tab=readme-ov-file
# or https://pyglet.readthedocs.io/en/latest/programming_guide/shapes.html
# and use http://code.astraw.com/projects/motmot/pygarrayimage.html or https://gist.github.com/nkymut/1cb40ea6ae4de0cf9ded7332f1ca0d55
# or https://api.arcade.academy/en/stable/index.html (supports gradient color in line -- "Arcade is built on top of Pyglet and OpenGL.")
# pointlist: list[LaserPoint] = []
# frame = LaserFrame()
paths: list[LaserPath] = []
# pointlist.append(HeliosPoint(x,y, dac.palette[cindex],blank=blank))
# all not working:
# if i == 1:
# # thanks to GpG for fixing scaling issue: https://stackoverflow.com/a/39668864
# scale_factor = 1./20 # from 10m to 1000px
# S = np.array([[scale_factor, 0,0],[0,scale_factor,0 ],[ 0,0,1 ]])
# new_H = S * self.H * np.linalg.inv(S)
# warpedFrame = cv2.warpPerspective(img, new_H, (1000,1000))
# cv2.imwrite(str(self.config.output_dir / "orig.png"), warpedFrame)
# cv2.rectangle(img, (0,0), (img.shape[1],25), (0,0,0), -1)
intensity = 39 # range 0-255
test_r = 100
base_c = (0,0, intensity)
# base_c = (0,intensity, intensity)
track_c = (intensity,0,0)
pred_c = (0,intensity,0)
if not tracker_frame and not prediction_frame:
paths.append(
LaserPath(circle_points(0xFFF/2, 0xFFF/2, test_r, base_c))
)
# c = (0,intensity,0)#, dac.palette[4] # Green
# r = 100
# steps = 100
# for i in range(steps):
# x = int(0xFFF/2 + math.cos(i * (2*math.pi)/steps) * r)
# y = int(0xFFF/2 + math.sin(i * (2*math.pi)/steps)* r)
# pointlist.append(HeliosPoint(x, y, c, blank=i==99))
# pointlist.append(HeliosPoint(10,10, c,blank=False))
# pointlist.append(HeliosPoint(10,100, c,blank=False))
# pointlist.append(HeliosPoint(10,200, c,blank=False))
# pointlist.append(HeliosPoint(100,200, c,blank=False))
# pointlist.append(HeliosPoint(200,200, c,blank=False))
# pointlist.append(HeliosPoint(200,100, c,blank=False))
# pointlist.append(HeliosPoint(200,10, c,blank=False))
# pointlist.append(HeliosPoint(100,10, c,blank=False))
# pointlist.append(HeliosPoint(10,10, c,blank=True))
# return pointlist
# print(not tracker_frame, not prediction_frame)
if not tracker_frame:
paths.append(
LaserPath(circle_points(0xFFF/2+2*test_r, 0xFFF/2, test_r, track_c))
)
else:
# if not len(tracks):
# paths.append(
# LaserPath(circle_points(0xFFF/2+4*test_r, 0xFFF/2, test_r/2, pred_c))
# )
for track_id, track in tracks.items():
inv_H = np.linalg.pinv(tracker_frame.H)
# track = track.get_sampled(4)
projected_history = track.get_projected_history(camera=config.camera)
history_for_laser = world_points_to_laser_points(projected_history)[0]
# point_color = bgr_colors[color_index % len(bgr_colors)]
points = np.rint(history_for_laser.reshape((-1,1,2))).astype(np.int32)
# print('point len',len(points))
laserpoints = []
for i, point in enumerate(points):
laserpoints.append(LaserPoint(point[0][0], point[0][1], track_c, blank=False))
path = LaserPath(laserpoints)
paths.append(path)
paths.append(
LaserPath(circle_points(history_for_laser[-1][0], history_for_laser[-1][1], 20, track_c))
)
# draw_track_projected(img, track, int(track_id), config.camera, convert_world_points_to_img_points)
if not prediction_frame:
paths.append(
LaserPath(circle_points(0xFFF/2+4*test_r, 0xFFF/2, test_r, pred_c))
)
# cv2.putText(img, f"Waiting for prediction...", (500,17), cv2.FONT_HERSHEY_PLAIN, 1, (255,255,0), 1)
# continue
# elif True:
# pass
elif drawn_tracks:
inv_H = np.linalg.pinv(prediction_frame.H)
for track_id, drawn_track in drawn_tracks.items():
drawn_track.update_drawn_positions(dt=None, no_shapes=True)
# For debugging:
# draw_trackjectron_history(img, track, int(track.track_id), convert_world_points_to_img_points)
anim_position = 1 # TODO)) calculate without video frame: get_animation_position(track, tracker_frame)
lines = drawntrack_predictions_to_lines(drawn_track, config.camera, anim_position)
# if lines:
# lines.extend(get_prediction_text(drawn_track))
if not lines:
continue
# draw in a single pass
# line_points = line_points.reshape((1, -1,1,2))
for line in lines:
# print('prediction line')
line = world_points_to_laser_points(line)[0]
# line = convert_world_points_to_img_points(line)
line = np.rint(line).astype(np.int32)
laserpoints = []
for i, point in enumerate(line):
laserpoints.append(LaserPoint(point[0], point[1], pred_c, blank=False))
path = LaserPath(laserpoints)
paths.append(path)
# draw_track_predictions(img, track, int(track.track_id)+1, config.camera, convert_world_points_to_img_points, 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)
# print(len(paths))
return paths
def get_prediction_text(drawn_track: DrawnTrack)-> list[list[float, float]]:
position_index = 20
if not drawn_track.drawn_predictions:
return []
if len(drawn_track.drawn_predictions[0]) < position_index:
logger.warning("prediction to short!")
return []
# draw only for first prediction
draw_pos = drawn_track.drawn_predictions[0][position_index-1]
current_pos = drawn_track.drawn_positions[-1]
angle = np.arctan2(draw_pos[0]-current_pos[0], draw_pos[1]-current_pos[1]) + np.pi
# print('angle', angle)
text_paths = []
with open("your_future_points_test.json", 'r') as fp:
lines = json.load(fp)
for i, line in enumerate(lines):
if i != 0:
continue
points = np.array(line)
avg_x = np.average(points[:,0])
avg_y = np.average(points[:,1])
minx, maxx = np.min(points[:,0]), np.max(points[:,0])
miny, maxy = np.min(points[:,1]), np.max(points[:,1])
sx = maxx-minx
sy = maxy-miny
points[:,0] -= avg_x
points[:,1] -= avg_y - i/2
points /= sx # scale to 1
points @= rotateMatrix(angle)
points += draw_pos
text_paths.append(points)
return text_paths
def rotateMatrix(a):
return np.array([[np.cos(a), -np.sin(a)], [np.sin(a), np.cos(a)]])
def run_laser_renderer(config: Namespace, is_running: BaseEvent, timer_counter):
renderer = LaserRenderer(config, is_running)
renderer.run(timer_counter)

1320
trap/lidar_tracker.py
View file

File diff suppressed because it is too large Load diff

1669
trap/lines.py
View file

File diff suppressed because it is too large Load diff

65
trap/monitor.py
View file

@ -1,65 +0,0 @@
from argparse import ArgumentParser
import time
from trap.counter import CounterListerner
from trap.node import Node
class Monitor(Node):
"""
Render a stage, on which different TrackScenarios take place to a
single image of lines. Which can be passed to different renderers
E.g. the laser or image renderers.
"""
FPS = 1
def setup(self):
# self.scenarios: List[DrawnScenario] = []
self.counter_listener = CounterListerner()
def run(self):
prev_time = time.perf_counter()
while self.is_running.is_set():
# self.tick() # don't polute it with own data
self.counter_listener.snapshot()
stats = self.counter_listener.to_string()
if len(stats):
self.logger.info(stats)
# else:
# self.logger.info("no stats")
# for i, (k, v) in enumerate(self.counter_listener.get_latest().items()):
# print(k,v)
# cv2.putText(img, f"{k} {v.value()}", (20,img.shape[0]-(40*i)-40), cv2.FONT_HERSHEY_PLAIN, 1, base_color, 1)
# 3) calculate latency for desired FPS
now = time.perf_counter()
time_diff = (now - prev_time)
if time_diff < 1/self.FPS:
# print(f"sleep {1/self.FPS - time_diff}")
time.sleep(1/self.FPS - time_diff)
now += 1/self.FPS - time_diff
prev_time = now
@classmethod
def arg_parser(cls) -> ArgumentParser:
argparser = ArgumentParser()
# argparser.add_argument('--zmq-trajectory-addr',
# help='Manually specity communication addr for the trajectory messages',
# type=str,
# default="ipc:///tmp/feeds_traj")
# argparser.add_argument('--zmq-prediction-addr',
# help='Manually specity communication addr for the prediction messages',
# type=str,
# default="ipc:///tmp/feeds_preds")
# 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")
return argparser

217
trap/node.py
View file

@ -1,217 +0,0 @@
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
import zmq
from trap.counter import CounterFpsSender, CounterSender
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
self.zmq_context = zmq.Context()
self.logger = self._logger()
self._prev_loop_time = 0
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
def _logger(cls):
return logging.getLogger(f"trap.{cls.__name__}")
def tick(self):
self.dt_since_last_tick = self.fps_counter.tick()
# with self.fps_counter.get_lock():
# self.fps_counter.value+=1
def setup(self):
raise RuntimeError("Not implemented setup()")
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
"""
now = time.perf_counter()
time_diff = (now - self._prev_loop_time)
if warn_below_fps > 0 and time_diff > 1/warn_below_fps:
self.logger.warning(f"Running below {warn_below_fps} FPS: measured {1/time_diff} FPS")
if time_diff < 1/max_fps:
# print(f"sleep {1/max_fps - time_diff}")
time.sleep(1/max_fps - time_diff)
now += 1/max_fps - time_diff
self._prev_loop_time = now
return self.run_loop()
@classmethod
def arg_parser(cls) -> ArgumentParser:
raise RuntimeError("Not implemented arg_parser()")
@classmethod
def _get_arg_parser(cls) -> ArgumentParser:
parser = cls.arg_parser()
# add some defaults
parser.add_argument(
'--verbose',
'-v',
help="Increase verbosity. Add multiple times to increase further.",
action='count', default=0
)
parser.add_argument(
'--remote-log-addr',
help="Connect to a remote logger like cutelog. Specify the ip",
type=str,
default="100.72.38.82"
)
parser.add_argument(
'--remote-log-port',
help="Connect to a remote logger like cutelog. Specify the port",
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
def sub(self, addr: str):
"Default zmq sub configuration"
sock = self.zmq_context.socket(zmq.SUB)
sock.setsockopt(zmq.CONFLATE, 1) # only keep latest frame. NB. make sure this comes BEFORE connect, otherwise it's ignored!!
sock.setsockopt(zmq.SUBSCRIBE, b'')
sock.connect(addr)
return sock
def pub(self, addr: str):
"Default zmq pub configuration"
sock = self.zmq_context.socket(zmq.PUB)
sock.setsockopt(zmq.CONFLATE, 1) # only keep latest frame
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
def parse_and_start(cls):
"""To start the node from CLI/supervisor"""
config = cls._get_arg_parser().parse_args()
setup_logging(config) # running from cli, we need to setup logging
is_running = multiprocessing.Event()
is_running.set()
statsender = CounterSender()
counter = CounterFpsSender(f"trap.{cls.__name__}", statsender)
# timer_counter = Timer(cls.__name__)
cls.start(config, is_running, counter)
def setup_logging(config: Namespace):
loglevel = logging.NOTSET if config.verbose > 1 else logging.DEBUG if config.verbose > 0 else logging.INFO
stream_handler = logging.StreamHandler()
log_handlers = [stream_handler]
if config.remote_log_addr:
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)
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"
)

135
trap/plumber.py
View file

@ -1,32 +1,22 @@
import atexit
import logging
from logging.handlers import SocketHandler, QueueHandler, QueueListener
from logging.handlers import SocketHandler
from multiprocessing import Event, Process, Queue
import multiprocessing
import signal
import sys
import time
from trap.config import parser
from trap.counter import CounterListerner
from trap.cv_renderer import run_cv_renderer
from trap.face_detector import run_detector
from trap.frame_emitter import run_frame_emitter
from trap.laser_renderer import run_laser_renderer
from trap.prediction_server import run_prediction_server
from trap.preview_renderer import run_preview_renderer
from trap.animation_renderer import run_animation_renderer
from trap.renderer import run_renderer
from trap.socket_forwarder import run_ws_forwarder
from trap.stage import Stage
from trap.timer import TimerCollection
from trap.tracker import run_tracker
from setproctitle import setproctitle, setthreadtitle
logger = logging.getLogger("trap.plumbing")
class ExceptionHandlingProcess(Process):
def run(self):
@ -41,12 +31,10 @@ class ExceptionHandlingProcess(Process):
atexit.register(exit_handler)
signal.signal(signal.SIGTERM, exit_handler)
signal.signal(signal.SIGINT, exit_handler)
setproctitle(f"trap-{self.name}")
try:
super(Process, self).run()
print("finished ", self.name)
except BaseException as e:
except Exception as e:
logger.critical(f"Exception in {self.name}")
logger.exception(e)
self._kwargs['is_running'].clear()
@ -56,124 +44,55 @@ def start():
loglevel = logging.NOTSET if args.verbose > 1 else logging.DEBUG if args.verbose > 0 else logging.INFO
# print(args)
# exit()
logging.basicConfig(
level=loglevel,
)
# set per handler, so we can set it lower for the root logger if remote logging is enabled
root_logger = logging.getLogger()
[h.setLevel(loglevel) for h in root_logger.handlers]
isRunning = Event()
isRunning.set()
q = multiprocessing.Queue(-1)
queue_handler = QueueHandler(q)
stream_handler = logging.StreamHandler()
log_handlers = [stream_handler]
if args.remote_log_addr:
logging.captureWarnings(True)
# root_logger.setLevel(logging.NOTSET) # to send all records to cutelog
root_logger.setLevel(logging.NOTSET) # to send all records to cutelog
socket_handler = SocketHandler(args.remote_log_addr, args.remote_log_port)
socket_handler.setLevel(logging.NOTSET)
log_handlers.append(socket_handler)
root_logger.addHandler(socket_handler)
queue_listener = QueueListener(q, *log_handlers, respect_handler_level=True)
# root = logging.getLogger()
logging.basicConfig(
level=loglevel,
handlers=[queue_handler]
)
# root_logger = logging.getLogger()
# # set per handler, so we can set it lower for the root logger if remote logging is enabled
# [h.setLevel(loglevel) for h in root_logger.handlers]
# queue_listener.handlers.append(socket_handler)
timers = TimerCollection()
timer_fe = timers.new('frame_emitter')
timer_tracker = timers.new('tracker')
timer_faces = timers.new('faces')
timer_stage = timers.new('stage')
# instantiating process with arguments
procs = [
# ExceptionHandlingProcess(target=run_ws_forwarder, kwargs={'config': args, 'is_running': isRunning}, name='forwarder'),
ExceptionHandlingProcess(target=run_frame_emitter, kwargs={'config': args, 'is_running': isRunning, 'timer_counter': timer_fe.iterations}, name='frame_emitter'),
ExceptionHandlingProcess(target=run_tracker, kwargs={'config': args, 'is_running': isRunning, 'timer_counter': timer_tracker.iterations}, name='tracker'),
# ExceptionHandlingProcess(target=run_detector, kwargs={'config': args, 'is_running': isRunning, 'timer_counter': timer_faces.iterations}, name='detector'),
ExceptionHandlingProcess(target=Stage.start, kwargs={'config': args, 'is_running': isRunning, 'timer_counter': timer_stage.iterations}, name='stage'),
ExceptionHandlingProcess(target=run_ws_forwarder, kwargs={'config': args, 'is_running': isRunning}, name='forwarder'),
ExceptionHandlingProcess(target=run_frame_emitter, kwargs={'config': args, 'is_running': isRunning}, name='frame_emitter'),
ExceptionHandlingProcess(target=run_tracker, kwargs={'config': args, 'is_running': isRunning}, name='tracker'),
]
# if args.render_file or args.render_url or args.render_window:
if args.render_window or args.render_file or args.render_url:
timer_preview = timers.new('preview')
if args.render_file or args.render_url or args.render_window:
procs.append(
# ExceptionHandlingProcess(target=run_cv_renderer, kwargs={'config': args, 'is_running': isRunning}, name='preview')
ExceptionHandlingProcess(target=run_cv_renderer, kwargs={'config': args, 'is_running': isRunning, 'timer_counter': timer_preview.iterations}, name='preview')
)
if args.render_animation:
procs.append(
ExceptionHandlingProcess(target=run_animation_renderer, kwargs={'config': args, 'is_running': isRunning}, name='renderer')
)
if args.render_laser:
procs.append(
ExceptionHandlingProcess(target=run_laser_renderer, kwargs={'config': args, 'is_running': isRunning, 'timer_counter': timer_preview.iterations}, name='renderer')
ExceptionHandlingProcess(target=run_renderer, kwargs={'config': args, 'is_running': isRunning}, name='renderer')
)
if not args.bypass_prediction:
timer_predict = timers.new('predict')
procs.append(
ExceptionHandlingProcess(target=run_prediction_server, kwargs={'config': args, 'is_running':isRunning, 'timer_counter': timer_predict.iterations}, name='inference'),
ExceptionHandlingProcess(target=run_prediction_server, kwargs={'config': args, 'is_running':isRunning}, name='inference'),
)
def timer_process(timers: TimerCollection, is_running: Event):
counter_listener = CounterListerner()
while is_running.is_set():
time.sleep(1)
timers.snapshot()
counter_listener.snapshot()
print(timers.to_string(), counter_listener.to_string())
logger.info("start")
for proc in procs:
proc.start()
procs.append(
ExceptionHandlingProcess(target=timer_process, kwargs={'is_running':isRunning, 'timers': timers}, name='timer'),
)
# wait for processes to clean up
for proc in procs:
proc.join()
try:
logger.info("start")
for proc in procs:
proc.start()
logger.info('Stop')
# if start the listener before the subprocesses, it becomes a mess, because the
# running threat is forked too, but cannot easily be stopped in the forks.
# Thus, only start the queue-listener threat _after_ starting processes
queue_listener.start()
# wait for processes to clean up
for proc in procs:
proc.join()
isRunning.clear()
logger.info('Stop')
except BaseException as e:
# mainly for KeyboardInterrupt
# but in any case, on error all processed need to be signalled to shut down
logger.critical(f"Exception in plumber")
logger.exception(e)
isRunning.clear()
# while True:
# time.sleep(2)
# any_alive = False
# alive = [proc for proc in procs if proc.is_alive()]
# print("alive: ", [p.name for p in alive])
# if len(alive) < 1:
# break
print('stop listener')
queue_listener.stop()
print('stopped listener')
print("finished plumber")
if __name__ == "__main__":
start()

484
trap/prediction_server.py
View file

@ -1,31 +1,33 @@
# adapted from Trajectron++ online_server.py
import json
from argparse import Namespace
import logging
from multiprocessing import Event, Queue
import os
import pathlib
import pickle
import random
import sys
import time
from typing import List
import json
import traceback
import warnings
from argparse import ArgumentParser, Namespace
from multiprocessing import Event
import dill
import numpy as np
import shapely
import pandas as pd
import torch
import zmq
from trajectron.environment import Environment, Scene, GeometricMap
from trajectron.model.model_registrar import ModelRegistrar
from trajectron.model.online.online_trajectron import OnlineTrajectron
import dill
import random
import pathlib
import numpy as np
from trajectron.environment.data_utils import derivative_of
from trajectron.utils import prediction_output_to_trajectories
from trajectron.model.online.online_trajectron import OnlineTrajectron
from trajectron.model.model_registrar import ModelRegistrar
from trajectron.environment import Environment, Scene
from trajectron.environment.node import Node
from trajectron.environment.node_type import NodeType
import matplotlib.pyplot as plt
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
import zmq
from trap.frame_emitter import Frame
from trap.tracker import Track, Smoother
logger = logging.getLogger("trap.prediction")
@ -54,21 +56,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):
scene_maps = list()
scene_pts = list()
heading_angles = list()
patch_sizes = list()
@ -81,20 +81,15 @@ def get_maps_for_input(input_dict, scene: Scene, hyperparams, device):
heading_state_index = me_hyp['heading_state_index']
# We have to rotate the map in the opposit direction of the agent to match them
if type(heading_state_index) is list: # infer from velocity or heading vector
# heading_angle = -np.arctan2(x[-1, heading_state_index[1]],
# x[-1, heading_state_index[0]]) * 180 / np.pi
heading_angle = -np.arctan2(x[heading_state_index[1]],
x[heading_state_index[0]]) * 180 / np.pi
heading_angle = -np.arctan2(x[-1, heading_state_index[1]],
x[-1, heading_state_index[0]]) * 180 / np.pi
else:
heading_angle = -x[-1, heading_state_index] * 180 / np.pi
else:
heading_angle = None
scene_map: ImageMap = scene.map[node.type]
scene_map.set_bounds() # update old pickled maps
# map_point = x[-1, :2]
map_point = x[:2]
# map_point = x[:2].clip(0) # prevent crash for out of map point.
scene_map = scene.map[node.type]
map_point = x[-1, :2]
patch_size = hyperparams['map_encoder'][node.type]['patch_size']
@ -109,81 +104,38 @@ def get_maps_for_input(input_dict, scene: Scene, hyperparams, device):
else:
heading_angles = torch.Tensor(heading_angles)
# 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 = 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)
maps_dict = {node: maps[[i]].to(device) for i, node in enumerate(nodes_with_maps)}
maps_dict = {node: maps[[i]] for i, node in enumerate(nodes_with_maps)}
return maps_dict
# If homography is in cm, predictions can be terrible. Correct that here
# TODO)) This should actually not be here, but we should use alternative homography
# and then scale up in rendering
def history_cm_to_m(history):
return [(h[0]/100, h[1]/100) for h in history]
class PredictionServer:
def __init__(self, config: Namespace, is_running: Event):
self.config = config
self.is_running = is_running
# TODO)) variable. Now placeholders for hof2 dataset
cx = 11.874955125
cy = 7.186118765
def prediction_m_to_cm(source):
# histories_dict[t][node]
for t in source:
for node in source[t]:
# source[t][node][:,0] += cx
# source[t][node][:,1] += cy
source[t][node] *= 100
# print(t,node, source[t][node])
return source
def offset_trajectron_dict(source, x, y):
# histories_dict[t][node]
for t in source:
for node in source[t]:
source[t][node][:,0] += x
source[t][node][:,1] += y
return source
class PredictionServer(Node):
def setup(self):
if self.config.eval_device == 'cpu':
logger.warning("Running on CPU. Specifying --eval_device cuda:0 should dramatically speed up prediction")
if self.config.smooth_predictions:
self.smoother = Smoother(window_len=12, convolution=True) # convolution seems fine for predictions
self.smoother = Smoother(window_len=4)
self.trajectory_socket = self.sub(self.config.zmq_trajectory_addr)
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}")
context = zmq.Context()
self.trajectory_socket: zmq.Socket = context.socket(zmq.SUB)
self.trajectory_socket.setsockopt(zmq.SUBSCRIBE, b'')
self.trajectory_socket.setsockopt(zmq.CONFLATE, 1) # only keep last msg. Set BEFORE connect!
self.trajectory_socket.connect(config.zmq_trajectory_addr)
def send_frame(self, frame: Frame):
if self.external_predictions:
# data = json.dumps(frame, cls=DataclassJSONEncoder)
self.prediction_socket.send_json(obj=frame, cls=DataclassJSONEncoder)
else:
self.prediction_socket.send_pyobj(frame)
self.prediction_socket: zmq.Socket = context.socket(zmq.PUB)
self.prediction_socket.bind(config.zmq_prediction_addr)
# print(self.prediction_socket)
def run(self):
if self.config.seed is not None:
random.seed(self.config.seed)
np.random.seed(self.config.seed)
@ -197,12 +149,10 @@ 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:
logger.info(f"Load config from {config_file}")
hyperparams = json.load(conf_json)
# Add hyperparams from arguments
@ -220,10 +170,19 @@ class PredictionServer(Node):
# hyperparams['maximum_history_length'] = 12 # test
logger.info(f"Use hyperparams: {hyperparams=}")
output_save_dir = os.path.join(self.config.output_dir, 'pred_figs')
pathlib.Path(output_save_dir).mkdir(parents=True, exist_ok=True)
with open(self.config.eval_data_dict, 'rb') as f:
eval_env = dill.load(f, encoding='latin1')
if eval_env.robot_type is None and hyperparams['incl_robot_node']:
eval_env.robot_type = eval_env.NodeType[0] # TODO: Make more general, allow the user to specify?
for scene in eval_env.scenes:
scene.add_robot_from_nodes(eval_env.robot_type)
logger.info('Loaded data from %s' % (self.config.eval_data_dict,))
# Creating a dummy environment with a single scene that contains information about the world.
@ -235,17 +194,12 @@ class PredictionServer(Node):
init_timestep = 2
eval_scene = eval_env.scenes[scene_idx]
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')
highest_iter = max([int(p.stem.split('-')[-1]) for p in model_iterations])
logger.info(f"Loading model {highest_iter}")
model_registrar.load_models(iter_num=highest_iter)
@ -261,9 +215,15 @@ class PredictionServer(Node):
trajectron.set_environment(online_env, init_timestep)
timestep = init_timestep + 1
while self.run_loop():
prev_run_time = 0
while self.is_running.is_set():
timestep += 1
# this_run_time = time.time()
# logger.debug(f'test {prev_run_time - this_run_time}')
# time.sleep(max(0, prev_run_time - this_run_time + .5))
# prev_run_time = time.time()
# TODO: see process_data.py on how to create a node, the provide nodes + incoming data columns
# data_columns = pd.MultiIndex.from_product([['position', 'velocity', 'acceleration'], ['x', 'y']])
@ -291,19 +251,8 @@ class PredictionServer(Node):
# on no data loop so that is_running is checked
continue
t_init = time.time()
data = self.trajectory_socket.recv()
# print('recv tracker frame')
frame: Frame = pickle.loads(data)
# add settings to log
frame.log['predictor'] = {}
for option in ['prediction_horizon','num_samples','full_dist','gmm_mode','z_mode', 'model_dir']:
frame.log['predictor'][option] = self.config.__dict__[option]
# print('indexrecv', [frame.tracks[t].frame_index for t in frame.tracks])
# trajectory_data = {t.track_id: t.get_projected_history_as_dict(frame.H) for t in frame.tracks.values()}
# trajectory_data = json.loads(data)
# logger.debug(f"Receive {frame.index}")
@ -314,72 +263,39 @@ 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
# TODO: modify this into a mapping function between JS data an the expected Node format
# node = FakeNode(online_env.NodeType.PEDESTRIAN)
history = [[h['x'], h['y']] for h in track.get_projected_history_as_dict(frame.H)]
history = np.array(history)
x = history[:, 0]
y = history[:, 1]
# TODO: calculate dt based on input
vx = derivative_of(x, 0.1) #eval_scene.dt
vy = derivative_of(y, 0.1)
ax = derivative_of(vx, 0.1)
ay = derivative_of(vy, 0.1)
if self.config.step_size > 1:
if (len(track.history) % self.config.step_size) != 0:
# only add when having a new step
continue
track = track.get_sampled(self.config.step_size)
if len(track.history) < 2:
continue
node = track.to_trajectron_node(frame.camera, online_env)
data_dict = {('position', 'x'): x[:], # [-10:-1]
('position', 'y'): y[:], # [-10:-1]
('velocity', 'x'): vx[:], # [-10:-1]
('velocity', 'y'): vy[:], # [-10:-1]
('acceleration', 'x'): ax[:], # [-10:-1]
('acceleration', 'y'): ay[:]} # [-10:-1]
data_columns = pd.MultiIndex.from_product([['position', 'velocity', 'acceleration'], ['x', 'y']])
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:])
# print("get", node.get(np.array([frame.index-10,frame.index]), {'position': ['x', 'y']}))
# history = [[h['x'], h['y']] for h in track.get_projected_history_as_dict(frame.H, self.config.camera)]
# if self.config.cm_to_m:
# history = history_cm_to_m(history)
# history = np.array(history)
# x = history[:, 0] #- cx # we can create bigger steps by doing history[::5,0]
# y = history[:, 1] #- cy # history[::5,1]
# if self.config.center_data:
# x -= cx
# y -= cy
# # TODO: calculate dt based on input
# vx = derivative_of(x, .1) #eval_scene.dt
# vy = derivative_of(y, .1)
# ax = derivative_of(vx, .1)
# ay = derivative_of(vy, .1)
node_data = pd.DataFrame(data_dict, columns=data_columns)
node = Node(
node_type=online_env.NodeType.PEDESTRIAN,
node_id=identifier,
data=node_data,
first_timestep=timestep
)
# data_dict = {('position', 'x'): x[:], # [-10:-1]
# ('position', 'y'): y[:], # [-10:-1]
# ('velocity', 'x'): vx[:], # [-10:-1]
# ('velocity', 'y'): vy[:], # [-10:-1]
# ('acceleration', 'x'): ax[:], # [-10:-1]
# ('acceleration', 'y'): ay[:]} # [-10:-1]
# data_columns = pd.MultiIndex.from_product([['position', 'velocity', 'acceleration'], ['x', 'y']])
# node_data = pd.DataFrame(data_dict, columns=data_columns)
# node = Node(
# node_type=online_env.NodeType.PEDESTRIAN,
# node_id=identifier,
# data=node_data,
# first_timestep=timestep
# )
# input_dict[node] = np.array(object=[x[-1],y[-1],vx[-1],vy[-1],ax[-1],ay[-1]])
# break # only on
input_dict[node] = np.array([x[-1],y[-1],vx[-1],vy[-1],ax[-1],ay[-1]])
# print(input_dict)
@ -389,18 +305,13 @@ class PredictionServer(Node):
# And want to update the network
# data = json.dumps({})
# TODO)) signal doing nothing
# TODO)) And update the network
self.send_frame(frame)
self.prediction_socket.send_pyobj(frame)
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)
maps = get_maps_for_input(input_dict, eval_scene, hyperparams)
# print(maps)
# robot_present_and_future = None
@ -419,17 +330,19 @@ class PredictionServer(Node):
# in the OnlineMultimodalGenerativeCVAE (see trajectron.model.online_mgcvae.py) each node's distribution
# is put stored in self.latent.p_dist by OnlineMultimodalGenerativeCVAE.p_z_x(). Type: torch.distributions.OneHotCategorical
# Later sampling in discrete_latent.py: DiscreteLatent.sample_p()
# print(input_dict)
dists, preds = trajectron.incremental_forward(input_dict,
maps,
prediction_horizon=self.config.prediction_horizon, # TODO: make variable
num_samples=self.config.num_samples, # TODO: make variable
full_dist=self.config.full_dist, # "The mol’des full sampled output, where z and y are sampled sequentially"
full_dist=self.config.full_dist, # "The model’s full sampled output, where z and y are sampled sequentially"
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()
end = time.time()
logger.debug("took %.2f s (= %.2f Hz) w/ %d nodes and %d edges" % (end - start,
1. / (end - start), len(trajectron.nodes),
trajectron.scene_graph.get_num_edges()))
# unsure what this bit from online_prediction.py does:
# detailed_preds_dict = dict()
# for node in eval_scene.nodes:
@ -441,27 +354,12 @@ class PredictionServer(Node):
# histories_dict provides the trajectory used for prediction
# futures_dict is the Ground Truth, which is unvailable in an online setting
prediction_dict, histories_dict, futures_dict = prediction_output_to_trajectories({frame.index: preds},
prediction_dict, histories_dict, futures_dict = prediction_output_to_trajectories({timestep: preds},
eval_scene.dt,
hyperparams['maximum_history_length'],
hyperparams['prediction_horizon']
)
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),
trajectron.scene_graph.get_num_edges(), start-t_init))
# if self.config.center_data:
# prediction_dict, histories_dict, futures_dict = offset_trajectron_dict(prediction_dict, cx, cy), offset_trajectron_dict(histories_dict, cx, cy), offset_trajectron_dict(futures_dict, cx, cy)
# print('pred timesteps', list(prediction_dict.keys()))
# print('histories', [n.data.data.shape[0] for n in prediction_dict[frame.index].keys()])
if self.config.cm_to_m:
# convert back to fit homography
prediction_dict, histories_dict, futures_dict = prediction_m_to_cm(prediction_dict), prediction_m_to_cm(histories_dict), prediction_m_to_cm(futures_dict)
assert(len(prediction_dict.keys()) <= 1)
if len(prediction_dict.keys()) == 0:
return
@ -472,17 +370,14 @@ 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
predictions = prediction_dict[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=}")
continue
# response[node.id] = {
@ -493,189 +388,24 @@ class PredictionServer(Node):
# 'predictions': predictions[0].tolist() # use batch 0
# }
frame.tracks[node.id].predictor_history = history.tolist() #node.data[:,{'position': ['x', 'y']}].tolist()
frame.tracks[node.id].predictor_history = history.tolist()
frame.tracks[node.id].predictions = predictions[0].tolist() # use batch 0
# data = json.dumps(response)
if self.config.predict_training_data:
logger.info(f"Frame prediction: {len(trajectron.nodes)} nodes & {trajectron.scene_graph.get_num_edges()} edges. Trajectron: {end - start}s")
else:
logger.debug(f"Total frame delay = {time.time()-frame.time}s ({len(trajectron.nodes)} nodes & {trajectron.scene_graph.get_num_edges()} edges. Trajectron: {end - start}s)")
logger.info(f"Total frame delay = {time.time()-frame.time}s ({len(trajectron.nodes)} nodes & {trajectron.scene_graph.get_num_edges()} edges. Trajectron: {end - start}s)")
if self.config.smooth_predictions:
frame = self.smoother.smooth_frame_predictions(frame)
frame.maps = list([m.cpu().numpy() for m in maps.values()]) if maps else None
# print('index', [frame.tracks[t].frame_index for t in frame.tracks])
self.send_frame(frame)
self.prediction_socket.send_pyobj(frame)
logger.info('Stopping')
@classmethod
def arg_parser(cls) -> ArgumentParser:
inference_parser = ArgumentParser()
inference_parser.add_argument('--zmq-trajectory-addr',
help='Manually specity communication addr for the trajectory messages',
type=str,
default="ipc:///tmp/feeds_traj")
inference_parser.add_argument('--zmq-prediction-addr',
help='Manually specity communication addr for the prediction messages',
type=str,
default="ipc:///tmp/feeds_preds")
inference_parser.add_argument("--step-size",
# TODO)) Make dataset/model metadata
help="sample step size (should be the same as for data processing and augmentation)",
type=int,
default=1,
)
inference_parser.add_argument("--model_dir",
help="directory with the model to use for inference",
type=str, # TODO: make into Path
default='../Trajectron-plus-plus/experiments/trap/models/models_18_Oct_2023_19_56_22_virat_vel_ar3/')
# 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')
# Model Parameters (hyperparameters)
inference_parser.add_argument("--offline_scene_graph",
help="whether to precompute the scene graphs offline, options are 'no' and 'yes'",
type=str,
default='yes')
inference_parser.add_argument("--dynamic_edges",
help="whether to use dynamic edges or not, options are 'no' and 'yes'",
type=str,
default='yes')
inference_parser.add_argument("--edge_state_combine_method",
help="the method to use for combining edges of the same type",
type=str,
default='sum')
inference_parser.add_argument("--edge_influence_combine_method",
help="the method to use for combining edge influences",
type=str,
default='attention')
inference_parser.add_argument('--edge_addition_filter',
nargs='+',
help="what scaling to use for edges as they're created",
type=float,
default=[0.25, 0.5, 0.75, 1.0]) # We don't automatically pad left with 0.0, if you want a sharp
# and short edge addition, then you need to have a 0.0 at the
# beginning, e.g. [0.0, 1.0].
inference_parser.add_argument('--edge_removal_filter',
nargs='+',
help="what scaling to use for edges as they're removed",
type=float,
default=[1.0, 0.0]) # We don't automatically pad right with 0.0, if you want a sharp drop off like
# the default, then you need to have a 0.0 at the end.
inference_parser.add_argument('--incl_robot_node',
help="whether to include a robot node in the graph or simply model all agents",
action='store_true')
inference_parser.add_argument('--map_encoding',
help="Whether to use map encoding or not",
action='store_true')
inference_parser.add_argument('--no_edge_encoding',
help="Whether to use neighbors edge encoding",
action='store_true')
inference_parser.add_argument('--batch_size',
help='training batch size',
type=int,
default=512)
inference_parser.add_argument('--k_eval',
help='how many samples to take during evaluation',
type=int,
default=1)
# Data Parameters
inference_parser.add_argument("--eval_data_dict",
help="what file to load for evaluation data (WHEN NOT USING LIVE DATA)",
type=str,
default='../Trajectron-plus-plus/experiments/processed/eth_test.pkl')
inference_parser.add_argument("--output_dir",
help="what dir to save output (i.e., saved models, logs, etc) (WHEN NOT USING LIVE OUTPUT)",
type=pathlib.Path,
default='./OUT/test_inference')
# inference_parser.add_argument('--device',
# help='what device to perform training on',
# type=str,
# default='cuda:0')
inference_parser.add_argument("--eval_device",
help="what device to use during inference",
type=str,
default="cuda:0")
inference_parser.add_argument('--seed',
help='manual seed to use, default is 123',
type=int,
default=123)
inference_parser.add_argument('--predict_training_data',
help='Ignore tracker and predict data from the training dataset',
action='store_true')
inference_parser.add_argument("--smooth-predictions",
help="Smooth the predicted tracks",
action='store_true')
inference_parser.add_argument('--prediction-horizon',
help='Trajectron.incremental_forward parameter',
type=int,
default=30)
inference_parser.add_argument('--num-samples',
help='Trajectron.incremental_forward parameter',
type=int,
default=5)
inference_parser.add_argument("--full-dist",
help="Trajectron.incremental_forward parameter",
action='store_true')
inference_parser.add_argument("--gmm-mode",
help="Trajectron.incremental_forward parameter",
type=bool,
default=True)
inference_parser.add_argument("--z-mode",
help="Trajectron.incremental_forward parameter",
action='store_true')
inference_parser.add_argument('--cm-to-m',
help="Correct for homography that is in cm (i.e. {x,y}/100). Should also be used when processing data",
action='store_true')
inference_parser.add_argument('--center-data',
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
def run_prediction_server(config: Namespace, is_running: Event, timer_counter):
def run_prediction_server(config: Namespace, is_running: Event):
# attempt to trace the warnings coming from pytorch
# def warn_with_traceback(message, category, filename, lineno, file=None, line=None):
@ -686,4 +416,4 @@ def run_prediction_server(config: Namespace, is_running: Event, timer_counter):
# warnings.showwarning = warn_with_traceback
s = PredictionServer(config, is_running)
s.run(timer_counter)
s.run()

423
trap/process_data.py
View file

@ -1,423 +0,0 @@
from collections import defaultdict
import datetime
from pathlib import Path
from random import seed, shuffle
import sys
import os
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 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
#sys.path.append("../../")
from trajectron.environment import Environment, Scene, Node
from trajectron.utils import maybe_makedirs
from trajectron.environment import derivative_of
from trap.utils import ImageMap
FPS = 12
desired_max_time = 100
pred_indices = [2, 3]
state_dim = 6
frame_diff = 10
desired_frame_diff = 1
dt = 1/FPS # dt per frame (e.g. 1/FPS)
smooth_window = FPS # see also tracker.py
min_track_length = 20
standardization = {
'PEDESTRIAN': {
'position': {
'x': {'mean': 0, 'std': 1},
'y': {'mean': 0, 'std': 1}
},
'velocity': {
'x': {'mean': 0, 'std': 2},
'y': {'mean': 0, 'std': 2}
},
'acceleration': {
'x': {'mean': 0, 'std': 1},
'y': {'mean': 0, 'std': 1}
}
}
}
class RollingAverage():
def __init__(self):
self.v = 0
self.n = 0
def add(self, v):
self.v = (self.v * self.n + v) / (self.n +1)
self.n += 1
return self.v
@dataclass
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):
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)))
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]):
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"-f{filter_displacement}" if filter_displacement > 0 else ""
name += "-map" if map_img_path else "-nomap"
name += f"-{datetime.date.today()}"
print(f"Process data in {src_dir}, to {dst_dir}, identified by {name}")
if map_img_path:
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],
[0,0,1],
]) # 100 scale
img = cv2.imread(map_img_path)
img = cv2.resize(img, (img.shape[1]//20, img.shape[0]//20))
type_map['PEDESTRIAN'] = ImageMap(
img,
homography_matrix,
f"Map from {map_img_path.name}"
)
else:
type_map = None
nl = 0
l = 0
data_columns = pd.MultiIndex.from_product([['position', 'velocity', 'acceleration'], ['x', 'y']])
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
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
}
max_track = reader.get(str(max([int(k) for k in reader._tracks.keys()])))
max_frame_nr = max_track.history[-1].frame_nr
print(max_frame_nr)
# separate call so cursor is kept during multiple loops
# seed(123)
shuffle(tracks)
dt1 = RollingAverage()
dt2 = RollingAverage()
dt3 = RollingAverage()
dt4 = RollingAverage()
sets: Dict[str, List[Track]] = {}
offset = 0
for data_class, nr in destinations.items():
# TODO)) think of a way to shuffle while keeping scenes
sets[data_class] = tracks[offset : offset+nr]
offset += nr
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
for data_class, nr_of_items in destinations.items():
env = Environment(node_type_list=['PEDESTRIAN'], standardization=standardization)
attention_radius = dict()
attention_radius[(env.NodeType.PEDESTRIAN, env.NodeType.PEDESTRIAN)] = 2.0
env.attention_radius = attention_radius
scenes = []
split_id = f"{name}_{data_class}"
data_dict_path = dst_dir / (split_id + '.pkl')
names[data_class] = data_dict_path
# subpath = src_dir / data_class
# prev_src_file = None
# 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")
for i, track in enumerate(sets[data_class]):
bar.update()
track_source = track.source
# if track.source != prev_src_file:
# scene =
tot = (dt1.v+dt2.v+dt3.v+dt4.v)
if tot:
bar.set_description(f"{data_dict_path.name} {track_source} ({dt1.v/tot:.4f}, {dt2.v/tot:.4f}, {dt3.v/tot:.4f}, {dt4.v/tot:.4f}) - {len(scene_nodes)}")
# for file in subpath.glob("*.txt"):]
input_data_dict = dict()
if len(track.history) < min_track_length:
continue
a = time.time()
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_window, False).smooth_track(track)
c = time.time()
if iteration_settings.step_size > 1:
track = track.get_sampled(iteration_settings.step_size, iteration_settings.step_offset)
# redo test, it might fall out again
if len(track.history) < min_track_length:
continue
# 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()
# if center_data:
# data['pos_x'] -= cx
# data['pos_y'] -= cy
# if bin_positions:
# data['pos_x'] =np.digitize(data['pos_x'], bins=space_x)
# data['pos_y'] =np.digitize(data['pos_y'], bins=space_y)
# print(data['pos_x'])
scene_nodes[f"{track_source}_{variation_nr}"].append(node)
created+=1
e = time.time()
dt1.add(b-a)
dt2.add(c-b)
dt3.add(data_class-c)
dt4.add(e-data_class)
scene_nodes_splits = defaultdict(lambda: [])
for scene_nr, nodes in scene_nodes.items():
# Some scenes grow obscenely 'large', as in, they span many timesteps
# Even though most might be empty. Here, split the scenes into gaps
# (Hopefully this prevents OOM in training)
# nodes in order of appearance
nodes = sorted(nodes, key= lambda n: n.first_timestep)
split = 0
last_timestep = 0
for node in nodes:
if node.first_timestep > (last_timestep+5*60*camera.fps): # a little buffer of x minutes
split += 1
last_timestep = max(node.last_timestep, last_timestep)
scene_nodes_splits[f"{scene_nr}_{split}"].append(node)
for scene_nr, nodes in scene_nodes_splits.items():
first_ts = min([n.first_timestep for n in nodes])
# print(first_ts)
for node in nodes:
# print(f"set ts: {node.first_timestep} to {node.first_timestep-first_ts-1}")
node.first_timestep -= (first_ts - 1)
node._last_timestep = None # reset (should now be handled by setter)
# print(f" -- got: {node.first_timestep}")
last_ts = max([n.last_timestep for n in nodes])
first_ts = max([n.first_timestep for n in nodes])
# print(sorted([n.first_timestep for n in nodes]))
# TODO)) check use of maps: https://github.com/StanfordASL/Trajectron-plus-plus/issues/14
scene = Scene(timesteps=last_ts, dt=(1/camera.fps)*step_size, name=f'{split_id}_{scene_nr}', aug_func=None, map=type_map)
scene.nodes.extend(nodes)
scenes.append(scene)
# print(scene_nr, scene)
# 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)
env.scenes = scenes
# print(env.scenes)
if len(scenes) > 0:
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():
parser = argparse.ArgumentParser()
parser.add_argument("--src-dir", "-s", type=Path, required=True, help="Directory with tracker output in .txt files")
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")
parser.add_argument("--step-size", type=int, default=1, help=f"Take only every n-th point")
parser.add_argument("--camera-fps",
help="Camera FPS",
type=int,
default=12)
parser.add_argument("--homography",
help="File with homography params",
type=Path,
default='../DATASETS/VIRAT_subset_0102x/VIRAT_0102_homography_img2world.txt',
action=HomographyAction)
parser.add_argument("--calibration",
help="File with camera intrinsics and lens distortion params (calibration.json)",
# type=Path,
default=None,
action=CameraAction)
parser.add_argument("--filter-displacement",
help="Filter tracks with a final displacement less then the given value",
# type=Path,
default=0,
type=float)
parser.add_argument("--map-img-path",
help="Image file representing a mask of a map (uses camera homography, assumes: 3 layers, values 0-255)",
# type=Path,
default=None,
type=Path)
args = parser.parse_args()
# process_data(**args.__dict__)
process_data(
args.src_dir,
args.dst_dir,
args.name,
args.smooth_tracks,
args.noise_tracks,
args.offset_tracks,
args.center_data,
args.bin_positions,
args.camera,
args.step_size,
filter_displacement=args.filter_displacement,
map_img_path=args.map_img_path
)

0
trap/protobuf/__init__.py
View file

68
trap/protobuf/renderable.py
View file

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

50
trap/renderable.proto
View file

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

41
trap/renderable_pb2.py
View file

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

491
trap/preview_renderer.py → trap/renderer.py
View file

@ -10,7 +10,7 @@ from multiprocessing import Event
from multiprocessing.synchronize import Event as BaseEvent
import cv2
import numpy as np
import json
import pyglet
import pyglet.event
import zmq
@ -18,18 +18,30 @@ import tempfile
from pathlib import Path
import shutil
import math
from typing import List, Optional
from collections import deque
from pyglet import shapes
from PIL import Image
from trap.utils import convert_world_points_to_img_points, exponentialDecay, relativePointToPolar, relativePolarToPoint
from trap.frame_emitter import DetectionState, Frame, Track, Camera
from trap.frame_emitter import DetectionState, Frame, Track
from dataclasses import dataclass
logger = logging.getLogger("trap.renderer")
@dataclass
class LineAnimConfig:
start_opacity: float
target_opacity: float
decay: float
@dataclass
class AnimConfig:
clear_color = (0,0,0,0)
video_opacity = 100 # on 255 scale
tracks: LineAnimConfig = LineAnimConfig(80, 180, 60)
predictions: LineAnimConfig = LineAnimConfig(20, 50, 10)
logger = logging.getLogger("trap.preview")
class FrameAnimation:
def __init__(self, frame: Frame):
@ -45,251 +57,240 @@ class FrameAnimation:
def done(self):
return (time.time() - self.start_time) > 5
def exponentialDecay(a, b, decay, dt):
"""Exponential decay as alternative to Lerp
Introduced by Freya Holmér: https://www.youtube.com/watch?v=LSNQuFEDOyQ
"""
return b + (a-b) * math.exp(-decay * dt)
def relativePointToPolar(origin, point) -> tuple[float, float]:
x, y = point[0] - origin[0], point[1] - origin[1]
return np.sqrt(x**2 + y**2), np.arctan2(y, x)
def relativePolarToPoint(origin, r, angle) -> tuple[float, float]:
return r * np.cos(angle) + origin[0], r * np.sin(angle) + origin[1]
PROJECTION_IMG = 0
PROJECTION_UNDISTORT = 1
PROJECTION_MAP = 2
PROJECTION_PROJECTOR = 4
class DrawnTrack:
def __init__(self, track_id, track: Track, renderer: PreviewRenderer, H, draw_projection = PROJECTION_IMG, camera: Optional[Camera] = None):
def __init__(self, track_id, track: Track, renderer: Renderer, H):
# self.created_at = time.time()
self.draw_projection = draw_projection
self.update_at = self.created_at = self.update_predictions_at = time.time()
self.last_update_t = time.perf_counter()
self.update_at = self.created_at = time.time()
self.track_id = track_id
self.renderer = renderer
self.camera = camera
self.H = H # TODO)) Move H to Camera object
self.drawn_positions = []
self.drawn_predictions = []
self.drawn_pred_history = []
# self.drawn_predictions = []
self.predictions: deque[DrawnPrediction] = deque(maxlen=2) # TODO; make configurable
self.shapes: list[pyglet.shapes.Line] = []
self.pred_shapes: list[list[pyglet.shapes.Line]] = []
self.pred_history_shapes: list[pyglet.shapes.Line] = []
self.set_track(track, H)
self.set_predictions(track, H)
self.set_prediction(track)
def __del__(self):
self.destroy()
def set_track(self, track: Track, H = None):
def destroy(self):
# TODO)) not working yet
logger.warning(f'del track {self}')
for s in self.shapes:
s.delete()
for p in self.predictions:
p.destroy()
def set_track(self, track: Track, H):
self.update_at = time.time()
self.track = track
# self.H = H
self.coords = [d.get_foot_coords() for d in track.history] if self.draw_projection == PROJECTION_IMG else track.get_projected_history(None, self.camera)
self.H = H
self.coords = [d.get_foot_coords() for d in track.history[-30:]] # maximum of 30 past positions (prevent collapse of FPS on loitering object)
# perhaps only do in constructor:
self.inv_H = np.linalg.pinv(self.H)
def set_predictions(self, track: Track, H = None):
self.update_predictions_at = time.time()
def set_prediction(self, track: Track):
# TODO: turn into add_prediction
pred_coords = []
pred_history_coords = []
if track.predictions:
if self.draw_projection == PROJECTION_IMG:
for pred_i, pred in enumerate(track.predictions):
pred_coords.append(cv2.perspectiveTransform(np.array([pred]), self.inv_H)[0].tolist())
pred_history_coords = cv2.perspectiveTransform(np.array([track.predictor_history]), self.inv_H)[0].tolist()
elif self.draw_projection == PROJECTION_MAP:
pred_coords = [pred for pred in track.predictions]
pred_history_coords = track.predictor_history
if not track.predictions:
return
self.pred_track = track
self.pred_coords = pred_coords
self.pred_history_coords = pred_history_coords
for pred_i, pred in enumerate(track.predictions):
pred_coords.append(cv2.perspectiveTransform(np.array([pred]), self.inv_H)[0].tolist())
# self.pred_coords = pred_coords
self.predictions.append(DrawnPrediction(self, pred_coords))
# color = (128,0,128) if pred_i else (128,
def update_drawn_positions(self, dt: float|None, no_shapes=False) -> List:
def update_drawn_positions(self, dt) -> []:
'''
use dt to lerp the drawn positions in the direction of current prediction
'''
# TODO: make lerp, currently quick way to get results
def int_or_not(v):
"""quick wrapper to toggle int'ing"""
return v
# return int(v)
if dt is None:
t = time.perf_counter()
dt = t - self.last_update_t
self.last_update_t = t
# 1. track history
for i, pos in enumerate(self.drawn_positions):
self.drawn_positions[i][0] = self.coords[i][0]
self.drawn_positions[i][1] = self.coords[i][1]
# self.drawn_positions[i][0] = int_or_not(exponentialDecay(self.drawn_positions[i][0], self.coords[i][0], 16, dt))
# self.drawn_positions[i][1] = int_or_not(exponentialDecay(self.drawn_positions[i][1], self.coords[i][1], 16, dt))
# print(self.drawn_positions)
self.drawn_positions[i][0] = int(exponentialDecay(self.drawn_positions[i][0], self.coords[i][0], AnimConfig.tracks.decay, dt))
self.drawn_positions[i][1] = int(exponentialDecay(self.drawn_positions[i][1], self.coords[i][1], AnimConfig.tracks.decay, dt))
if len(self.coords) > len(self.drawn_positions):
self.drawn_positions.extend(self.coords[len(self.drawn_positions):])
# 2. history as seen by predictor (Trajectron)
for i, pos in enumerate(self.drawn_pred_history):
if len(self.pred_history_coords) > i:
self.drawn_pred_history[i][0] = int_or_not(exponentialDecay(self.drawn_pred_history[i][0], self.pred_history_coords[i][0], 16, dt))
self.drawn_pred_history[i][1] = int_or_not(exponentialDecay(self.drawn_pred_history[i][1], self.pred_history_coords[i][1], 16, dt))
# Superseded by individual drawnprediction elements
# for a, drawn_prediction in enumerate(self.drawn_predictions):
# for i, pos in enumerate(drawn_prediction):
# # 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 = 6
# origin = self.drawn_positions[-1]
# drawn_r, drawn_angle = relativePointToPolar( origin, drawn_prediction[i])
# pred_r, pred_angle = relativePointToPolar(origin, self.pred_coords[a][i])
# r = exponentialDecay(drawn_r, pred_r, decay, dt)
# angle = exponentialDecay(drawn_angle, pred_angle, decay, dt)
# x, y = relativePolarToPoint(origin, r, angle)
# self.drawn_predictions[a][i] = int(x), int(y)
# # self.drawn_predictions[i][0] = int(exponentialDecay(self.drawn_predictions[i][0], self.pred_coords[i][0], decay, dt))
# # self.drawn_predictions[i][1] = int(exponentialDecay(self.drawn_predictions[i][1], self.pred_coords[i][1], decay, dt))
if len(self.pred_history_coords) > len(self.drawn_pred_history):
self.drawn_pred_history.extend(self.coords[len(self.drawn_pred_history):])
# if len(self.pred_coords) > len(self.drawn_predictions):
# self.drawn_predictions.extend(self.pred_coords[len(self.drawn_predictions):])
# 3. predictions
if len(self.pred_coords):
for a, drawn_prediction in enumerate(self.drawn_predictions):
for i, pos in enumerate(drawn_prediction):
# 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 = 16
origin = self.drawn_positions[-1]
drawn_r, drawn_angle = relativePointToPolar( origin, drawn_prediction[i])
pred_r, pred_angle = relativePointToPolar(origin, self.pred_coords[a][i])
r = exponentialDecay(drawn_r, pred_r, decay, dt)
angle = exponentialDecay(drawn_angle, pred_angle, decay, dt)
x, y = relativePolarToPoint(origin, r, angle)
self.drawn_predictions[a][i] = int_or_not(x), int_or_not(y)
# self.drawn_predictions[i][0] = int(exponentialDecay(self.drawn_predictions[i][0], self.pred_coords[i][0], decay, dt))
# self.drawn_predictions[i][1] = int(exponentialDecay(self.drawn_predictions[i][1], self.pred_coords[i][1], decay, dt))
if len(self.pred_coords) > len(self.drawn_predictions):
self.drawn_predictions.extend(self.pred_coords[len(self.drawn_predictions):])
# for a, drawn_prediction in self.drawn_predictions:
# if len(self.pred_coords) > len(self.drawn_predictions):
# self.drawn_predictions.extend(self.pred_coords[len(self.drawn_predictions):])
# self.drawn_positions = self.coords
# finally: update shapes from coordinates
if not no_shapes: # to be used when not rendering to pyglet (e.g. laser renderer)
self.update_shapes(dt)
self.update_shapes(dt)
return self.drawn_positions
def update_shapes(self, dt):
if len(self.shapes) > len(self.drawn_positions):
self.shapes = self.shapes[:len(self.drawn_positions)]
drawn_positions = convert_world_points_to_img_points(self.coords[:500]) # TODO)) Glitch in self.drawn_positions, now also capped
drawn_pred_history = convert_world_points_to_img_points(self.drawn_pred_history)
drawn_predictions = [convert_world_points_to_img_points(p) for p in self.drawn_predictions]
# positions = convert_world_points_to_img_points(self.drawn_predictions)
# for i, pos in self.drawn_positions.enumerate():
for ci in range(1, len(self.drawn_positions)):
x, y = [int(p) for p in self.drawn_positions[ci-1]]
x2, y2 = [int(p) for p in self.drawn_positions[ci]]
# print("drawn",
# drawn_positions,'self', self.drawn_positions
# )
if len(self.shapes) > len(drawn_positions):
self.shapes = self.shapes[:len(drawn_positions)]
# for i, pos in drawn_positions.enumerate():
draw_dot = False # if False, draw line
for_laser = True
if True:
for ci in range(1, len(drawn_positions)):
x, y = [int(p) for p in drawn_positions[ci-1]]
x2, y2 = [int(p) for p in drawn_positions[ci]]
y, y2 = self.renderer.window.height - y, self.renderer.window.height - y2
color = [100+155*ci // len(drawn_positions)]*3
# print(x,y,x2,y2,color)
if ci >= len(self.shapes):
# TODO: add color2
if draw_dot:
line = pyglet.shapes.Arc(x2, y2, 10, thickness=2, color=color, batch=self.renderer.batch_anim)
else:
# line = self.renderer.gradientLine(x, y, x2, y2, 3, color, color, batch=self.renderer.batch_anim)
line = pyglet.shapes.Line(x, y, x2, y2, 3, color, batch=self.renderer.batch_anim)
# line = self.renderer.gradientLine(x, y, x2, y2, 3, color, color, batch=self.renderer.batch_anim)
line.opacity = 20 if not for_laser else 255
self.shapes.append(line)
else:
line = self.shapes[ci-1]
line.x, line.y = x, y
if draw_dot:
line.radius = int(exponentialDecay(line.radius, 1.5, 3, dt))
else:
line.x2, line.y2 = x2, y2
line.color = color
if not for_laser:
line.opacity = int(exponentialDecay(line.opacity, 180, 8, dt))
y, y2 = self.renderer.window.height - y, self.renderer.window.height - y2
color = [100+155*ci // len(self.drawn_positions)]*3
# print(x,y,x2,y2,color)
if ci >= len(self.shapes):
# TODO: add color2
line = self.renderer.gradientLine(x, y, x2, y2, 3, color, color, batch=self.renderer.batch_anim)
line.opacity = AnimConfig.tracks.start_opacity
self.shapes.append(line)
else:
line = self.shapes[ci-1]
line.x, line.y = x, y
line.x2, line.y2 = x2, y2
line.color = color
line.opacity = int(exponentialDecay(line.opacity, AnimConfig.tracks.target_opacity, AnimConfig.tracks.start_opacity, dt))
# TODO: basically a duplication of the above, do this smarter?
# TODO: add intermediate segment
color = colorset[self.track_id % len(colorset)]
#color = colorset[self.track_id % len(colorset)]
if False:
if len(self.pred_history_shapes) > len(drawn_pred_history):
self.pred_history_shapes = self.pred_history_shapes[:len(drawn_pred_history)]
for drawn_prediction in self.predictions:
drawn_prediction.update_opacities(dt)
# for a, drawn_predictions in enumerate(self.drawn_predictions):
# if len(self.pred_shapes) <= a:
# self.pred_shapes.append([])
# for i, pos in drawn_pred_history.enumerate():
for ci in range(1, len(drawn_pred_history)):
x, y = [int(p) for p in drawn_pred_history[ci-1]]
x2, y2 = [int(p) for p in drawn_pred_history[ci]]
# if len(self.pred_shapes[a]) > (len(drawn_predictions) +1):
# self.pred_shapes[a] = self.pred_shapes[a][:len(drawn_predictions)]
y, y2 = self.renderer.window.height - y, self.renderer.window.height - y2
# # for i, pos in drawn_predictions.enumerate():
# for ci in range(0, len(drawn_predictions)):
# if ci == 0:
# x, y = [int(p) for p in self.drawn_positions[-1]]
# else:
# x, y = [int(p) for p in drawn_predictions[ci-1]]
if ci >= len(self.pred_history_shapes):
# line = self.renderer.gradientLine(x, y, x2, y2, 3, color, color, batch=self.renderer.batch_anim)
line = pyglet.shapes.Line(x,y ,x2, y2, 2.5, color, batch=self.renderer.batch_anim)
# line = pyglet.shapes.Arc(x2, y2, 10, thickness=2, color=color, batch=self.renderer.batch_anim)
line.opacity = 120
self.pred_history_shapes.append(line)
# x2, y2 = [int(p) for p in drawn_predictions[ci]]
# y, y2 = self.renderer.window.height - y, self.renderer.window.height - y2
# # color = [255,0,0]
# # print(x,y,x2,y2,color)
# if ci >= len(self.pred_shapes[a]):
# # TODO: add color2
# line = self.renderer.gradientLine(x, y, x2, y2, 3, color, color, batch=self.renderer.batch_anim)
# line.opacity = 5
# self.pred_shapes[a].append(line)
# else:
# line = self.pred_shapes[a][ci-1]
# line.x, line.y = x, y
# line.x2, line.y2 = x2, y2
# line.color = color
# decay = (16/ci) if ci else 16
# half = len(drawn_predictions) / 2
# if ci < half:
# target_opacity = 180
# else:
# target_opacity = (1 - ((ci - half) / half)) * 180
# line.opacity = int(exponentialDecay(line.opacity, target_opacity, decay, dt))
class DrawnPrediction:
def __init__(self, drawn_track: DrawnTrack, coords: list[list] = []):
self.created_at = time.time()
# self.renderer = renderer
self.drawn_track = drawn_track
self.coords = coords
self.color = colorset[self.drawn_track.track_id % len(colorset)]
self.pred_shapes: list[list[pyglet.shapes.Line]] = []
# coords is a list of predictions
for a, coords in enumerate(self.coords):
prediction_shapes = []
for ci in range(0, len(coords)):
if ci == 0:
x, y = [int(p) for p in self.drawn_track.coords[-1]]
else:
line = self.pred_history_shapes[ci-1]
line.x, line.y = x, y
line.x2, line.y2 = x2, y2
# line.radius = int(exponentialDecay(line.radius, 1.5, 3, dt))
line.color = color
line.opacity = int(exponentialDecay(line.opacity, 180, 8, dt))
x, y = [int(p) for p in coords[ci-1]]
x2, y2 = [int(p) for p in coords[ci]]
# flip in window:
y, y2 = self.drawn_track.renderer.window.height - y, self.drawn_track.renderer.window.height - y2
line = self.drawn_track.renderer.gradientLine(x, y, x2, y2, 3, self.color, self.color, batch=self.drawn_track.renderer.batch_anim)
line.opacity = 5
prediction_shapes.append(line)
self.pred_shapes.append(prediction_shapes)
if True:
for a, drawn_prediction in enumerate(drawn_predictions):
if len(self.pred_shapes) <= a:
self.pred_shapes.append([])
def destroy(self):
for pred in self.pred_shapes:
for shape in pred:
shape.delete()
if len(self.pred_shapes[a]) > (len(drawn_prediction) +1):
self.pred_shapes[a] = self.pred_shapes[a][:len(drawn_prediction)]
# for i, pos in drawn_predictions.enumerate():
for ci in range(0, len(drawn_prediction)):
if ci == 0:
continue
# x, y = [int(p) for p in drawn_positions[-1]]
else:
x, y = [int(p) for p in drawn_prediction[ci-1]]
def update_opacities(self, dt: float):
"""
Update the opacties of the drawn line, by only using the dt provided by the renderer
Done using exponential decal, with a different decay value per item
"""
for a, coords in enumerate(self.coords):
for ci in range(0, len(coords)):
line = self.pred_shapes[a][ci-1]
# Positions of prediction no longer update
# line.x, line.y = x, y
# line.x2, line.y2 = x2, y2
# line.color = color
x2, y2 = [int(p) for p in drawn_prediction[ci]]
y, y2 = self.renderer.window.height - y, self.renderer.window.height - y2
# color = [255,0,0]
# print(x,y,x2,y2,color)
if ci >= len(self.pred_shapes[a]):
# TODO: add color2
# line = self.renderer.gradientLine(x, y, x2, y2, 3, color, color, batch=self.renderer.batch_anim)
line = pyglet.shapes.Line(x,y ,x2, y2, 1.5, color, batch=self.renderer.batch_anim)
# line = pyglet.shapes.Arc(x,y ,1.5, thickness=1.5, color=color, batch=self.renderer.batch_anim)
line.opacity = 5
self.pred_shapes[a].append(line)
else:
line = self.pred_shapes[a][ci-1]
line.x, line.y = x, y
line.x2, line.y2 = x2, y2
line.color = color
decay = (16/ci) if ci else 16
half = len(drawn_prediction) / 2
if ci < half:
target_opacity = 60
else:
target_opacity = (1 - ((ci - half) / half)) * 60
line.opacity = int(exponentialDecay(line.opacity, target_opacity, decay, dt))
# lower decay for further points slows down fade in
decay = (AnimConfig.predictions.decay/(3*ci)) if ci else AnimConfig.predictions.decay
half = len(coords) / 2
if ci < half:
target_opacity = AnimConfig.predictions.target_opacity
else:
target_opacity = (1 - ((ci - half) / half)) * AnimConfig.predictions.target_opacity
line.opacity = int(exponentialDecay(line.opacity, target_opacity, decay, dt))
# logger.info(f"{target_opacity=}, {line.opacity=}")
class FrameWriter:
@ -298,9 +299,9 @@ class FrameWriter:
framerate.
See https://video.stackexchange.com/questions/25811/ffmpeg-make-video-with-non-constant-framerate-from-image-filenames
"""
def __init__(self, filename: str, fps: float, frame_size: Optional[tuple] = None) -> None:
def __init__(self, filename: str, fps: float, frame_size: tuple) -> None:
self.filename = filename
self._fps = fps
self.fps = fps
self.frame_size = frame_size
self.tmp_dir = tempfile.TemporaryDirectory(prefix="trap-output-")
@ -331,7 +332,7 @@ class FrameWriter:
class PreviewRenderer:
class Renderer:
def __init__(self, config: Namespace, is_running: BaseEvent):
self.config = config
self.is_running = is_running
@ -340,8 +341,7 @@ class PreviewRenderer:
self.prediction_sock = context.socket(zmq.SUB)
self.prediction_sock.setsockopt(zmq.CONFLATE, 1) # only keep latest frame. NB. make sure this comes BEFORE connect, otherwise it's ignored!!
self.prediction_sock.setsockopt(zmq.SUBSCRIBE, b'')
# self.prediction_sock.connect(config.zmq_prediction_addr if not self.config.bypass_prediction else config.zmq_trajectory_addr)
self.prediction_sock.connect(config.zmq_prediction_addr)
self.prediction_sock.connect(config.zmq_prediction_addr if not self.config.bypass_prediction else config.zmq_trajectory_addr)
self.tracker_sock = context.socket(zmq.SUB)
self.tracker_sock.setsockopt(zmq.CONFLATE, 1) # only keep latest frame. NB. make sure this comes BEFORE connect, otherwise it's ignored!!
@ -353,23 +353,14 @@ class PreviewRenderer:
self.frame_sock.setsockopt(zmq.SUBSCRIBE, b'')
self.frame_sock.connect(config.zmq_frame_addr)
# TODO)) Move loading H to config.py
# if self.config.homography.suffix == '.json':
# with self.config.homography.open('r') as fp:
# self.H = np.array(json.load(fp))
# else:
# self.H = np.loadtxt(self.config.homography, delimiter=',')
# print('h', self.config.H)
self.H = self.config.H
self.H = np.loadtxt(self.config.homography, delimiter=',')
self.inv_H = np.linalg.pinv(self.H)
# TODO: get FPS from frame_emitter
# self.out = cv2.VideoWriter(str(filename), fourcc, 23.97, (1280,720))
self.fps = 60
self.frame_size = (self.config.camera.w,self.config.camera.h)
self.frame_size = (1280,720)
self.hide_stats = False
self.out_writer = self.start_writer() if self.config.render_file else None
self.streaming_process = self.start_streaming() if self.config.render_url else None
@ -388,7 +379,8 @@ class PreviewRenderer:
self.window.set_handler('on_refresh', self.on_refresh)
self.window.set_handler('on_close', self.on_close)
pyglet.gl.glClearColor(81./255, 20/255, 46./255, 0)
# Purple background color:
pyglet.gl.glClearColor(*AnimConfig.clear_color)
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
@ -524,34 +516,32 @@ class PreviewRenderer:
def check_frames(self, dt):
new_tracks = False
try:
self.frame: Frame = self.frame_sock.recv_pyobj(zmq.NOBLOCK)
if not self.first_time:
self.first_time = self.frame.time
img = cv2.GaussianBlur(self.frame.img, (15, 15), 0)
img = cv2.flip(cv2.cvtColor(img, cv2.COLOR_BGR2RGB), 0)
img = pyglet.image.ImageData(self.frame_size[0], self.frame_size[1], 'RGB', img.tobytes())
img = cv2.cvtColor(cv2.cvtColor(img, cv2.COLOR_BGR2GRAY), cv2.COLOR_GRAY2RGB)
channels = 3 # unfortunately, pyglet seems to draw single channel as Red only
img = pyglet.image.ImageData(self.frame_size[0], self.frame_size[1], 'RGB', img.tobytes(), pitch=self.frame_size[0] * -1 * channels)
# don't draw in batch, so that it is the background
self.video_sprite = pyglet.sprite.Sprite(img=img, batch=self.batch_bg)
self.video_sprite.opacity = 100
self.video_sprite.opacity = AnimConfig.video_opacity
except zmq.ZMQError as e:
# idx = frame.index if frame else "NONE"
# logger.debug(f"reuse video frame {idx}")
pass
try:
self.prediction_frame: Frame = self.prediction_sock.recv_pyobj(zmq.NOBLOCK)
new_tracks = True
self.update_predictions()
except zmq.ZMQError as e:
pass
try:
self.tracker_frame: Frame = self.tracker_sock.recv_pyobj(zmq.NOBLOCK)
new_tracks = True
self.update_tracks()
except zmq.ZMQError as e:
pass
if new_tracks:
self.update_tracks()
def update_tracks(self):
"""Updates the track objects and shapes. Called after setting `prediction_frame`
@ -563,20 +553,31 @@ class PreviewRenderer:
# # TODO fade out
# del self.drawn_tracks[track_id]
if self.prediction_frame:
for track_id, track in self.prediction_frame.tracks.items():
if self.tracker_frame:
for track_id, track in self.tracker_frame.tracks.items():
if track_id not in self.drawn_tracks:
self.drawn_tracks[track_id] = DrawnTrack(track_id, track, self, self.prediction_frame.H)
self.drawn_tracks[track_id] = DrawnTrack(track_id, track, self, self.tracker_frame.H)
else:
self.drawn_tracks[track_id].set_track(track, self.prediction_frame.H)
self.drawn_tracks[track_id].set_track(track, self.tracker_frame.H)
# clean up
for track_id in list(self.drawn_tracks.keys()):
# TODO make delay configurable
if self.drawn_tracks[track_id].update_at < time.time() - 5:
# TODO fade out
self.drawn_tracks[track_id].destroy()
del self.drawn_tracks[track_id]
def update_predictions(self):
if self.prediction_frame:
for track_id, track in self.prediction_frame.tracks.items():
if track_id not in self.drawn_tracks:
self.drawn_tracks[track_id] = DrawnTrack(track_id, track, self, self.prediction_frame.H)
logger.warning("Prediction for uninitialised frame. This should not happen? (maybe huge delay in prediction?)")
else:
self.drawn_tracks[track_id].set_prediction(track)
def on_key_press(self, symbol, modifiers):
print('A key was pressed, use f to hide')
@ -610,14 +611,16 @@ class PreviewRenderer:
self.window.clear()
self.batch_bg.draw()
for track in self.drawn_tracks.values():
for shape in track.shapes:
shape.draw() # for some reason the batches don't work
for track in self.drawn_tracks.values():
for shapes in track.pred_shapes:
for shape in shapes:
shape.draw()
for prediction in track.predictions:
for shapes in prediction.pred_shapes:
for shape in shapes:
shape.draw()
# self.batch_anim.draw()
self.batch_overlay.draw()
@ -758,27 +761,17 @@ class PreviewRenderer:
self.out_writer.release()
if self.streaming_process:
# oddly wrapped, because both close and release() take time.
logger.info('wait for closing stream')
self.streaming_process.wait()
logger.info('stopped')
# colorset = itertools.product([0,255], repeat=3) # but remove white
# colorset = [(0, 0, 0),
# (0, 0, 255),
# (0, 255, 0),
# (0, 255, 255),
# (255, 0, 0),
# (255, 0, 255),
# (255, 255, 0)
# ]
colorset = [
(255,255,100),
(255,100,255),
(100,255,255),
colorset = [(0, 0, 0),
(0, 0, 255),
(0, 255, 0),
(0, 255, 255),
(255, 0, 0),
(255, 0, 255),
(255, 255, 0)
]
# colorset = [
# (0,0,0),
# ]
# Deprecated
def decorate_frame(frame: Frame, prediction_frame: Frame, first_time: float, config: Namespace) -> np.array:
@ -789,12 +782,12 @@ def decorate_frame(frame: Frame, prediction_frame: Frame, first_time: float, con
# or https://api.arcade.academy/en/stable/index.html (supports gradient color in line -- "Arcade is built on top of Pyglet and OpenGL.")
frame.img
overlay = np.zeros(frame.img.shape, np.uint8)
# Fill image with red color(set each pixel to red)
overlay[:] = (130, 0, 75)
# # Fill image with red color(set each pixel to red)
# overlay = np.zeros(frame.img.shape, np.uint8)
# overlay[:] = (130, 0, 75)
img = cv2.addWeighted(frame.img, .4, overlay, .6, 0)
# img = frame.img.copy()
# img = cv2.addWeighted(frame.img, .4, overlay, .6, 0)
img = frame.img.copy()
# all not working:
# if i == 1:
@ -891,6 +884,6 @@ def decorate_frame(frame: Frame, prediction_frame: Frame, first_time: float, con
return img
def run_preview_renderer(config: Namespace, is_running: BaseEvent):
renderer = PreviewRenderer(config, is_running)
def run_renderer(config: Namespace, is_running: BaseEvent):
renderer = Renderer(config, is_running)
renderer.run()

175
trap/settings.py
View file

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

11
trap/shapes.py
View file

File diff suppressed because one or more lines are too long

950
trap/stage.py
View file

@ -1,950 +0,0 @@
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
import numpy as np
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.node import Node
from trap.track_history import TrackHistory
from trap.utils import lerp
logger = logging.getLogger('trap.stage')
OPTION_RENDER_DEBUG = False
OPTION_POSITION_MARKER = False
OPTION_GROW_ANOMALY_CIRCLE = False
# OPTION_RENDER_DIFF_SEGMENT = True
OPTION_TRACK_NOISE = False
TRACK_ASSUMED_FPS = 12
LOST_FADEOUT = 2 # seconds
PREDICTION_INTERVAL: int|None = int(TRACK_ASSUMED_FPS * 1.2) # 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_END_FADE = 30 # points
TRACK_FADE_ASSUME_FPS = TRACK_ASSUMED_FPS
# LOITERING_WINDOW = 8 * TRACK_ASSUMED_FPS
# LOITERING_DISTANCE = 1 # meter diff in LOITERING_WINDOW time
# LOITERING_MEDIAN_FILTER = TRACK_ASSUMED_FPS // 3 # frames: smooth out velocity over n frames
LOITERING_VELOCITY_TRESHOLD = .5 # m/s
LOITERING_DURATION_TO_LINGER = TRACK_ASSUMED_FPS * 1 # start counting as lingering after this many frames
LOITERING_LINGER_FACTOR = TRACK_ASSUMED_FPS * 4 # number of frames to reach loitering score of 1 (+LOITERING_DURATION_TO_LINGER)
class DefaultDictKeyed(dict):
def __init__(self, factory):
self.factory = factory
def __missing__(self, key):
self[key] = self.factory(key)
return self[key]
@dataclass
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)
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
self.track_id = track_id
self.scene: ScenarioScene = ScenarioScene.DETECTED
self.current_time = 0
self.track: Optional[ProjectedTrack] = None
self.prediction_tracks: List[ProjectedTrack] = []
self._last_diff_frame_idx: Optional[int] = 0
self.prediction_diffs: List[DiffSegment] = []
self.state_change_at = None
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 track_age(self):
if not self.track:
return 0
return time.time() - self.track.updated_at
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 lost_for(self):
if self.scene is ScenarioScene.LOST:
return time.perf_counter() - self.state_change_at
return None
def lost_factor(self):
l = self.lost_for()
if not l:
return 0
return l/LOST_FADEOUT
def anomaly_factor(self):
return calc_anomaly(self.prediction_diffs)
def deactivate(self):
self.take_over_at = None
def update(self):
"""Animation tick, check state."""
# 1) lost_score: unlike other states, this runs for each rendering pass to handle crashing tracker
self.check_lost()
def set_scene(self, scene: ScenarioScene):
if self.scene is scene:
return False
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
def update_state(self):
self.check_lost() or self.check_loitering() or self.check_track()
def check_lost(self):
if self.track and (self.track.lost or self.track.updated_at < time.time() - 5):
self.set_scene(ScenarioScene.LOST)
return True
return False
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:
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)
return True
if predictions and self.running_for() > 60 * 5:
self.set_scene(ScenarioScene.PLAY)
return True
if predictions:
self.set_scene(ScenarioScene.UPDATED_PREDICTION)
return True
if self.track:
if len(self.track.projected_history) > TRACK_ASSUMED_FPS * 2:
self.set_scene(ScenarioScene.TRACKED)
else:
self.set_scene(ScenarioScene.DETECTED)
return True
return False
# the tracker track: replace
def recv_track(self, track: ProjectedTrack):
if self.track and self.track.created_at > track.created_at:
# ignore old track
return
self.track = track
self.update_prediction_diff()
self.update_state()
def update_prediction_diff(self):
"""
gather the diffs of the trajectory with the most recent prediction
"""
if len(self.prediction_diffs) == 0:
return
self.prediction_diffs[-1].update_track(self.track)
# receive new predictions: accumulate
def recv_prediction(self, track: ProjectedTrack):
if not self.track:
# 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:
# just drop tracks if the predictions come to quick
return
if track._track.predictions is None or not len(track._track.predictions):
# don't count to predictions if no prediction is set of given track (e.g. young tracks, that are still passed by the predictor)
return
self.prediction_tracks.append(track)
if len(self.prediction_diffs):
self.prediction_diffs[-1].finish() # existing diffing can end
# and create a new one
self.prediction_diffs.append(DiffSegment(track))
# self.prediction_diffs.append(DiffSegmentScan(track))
self.update_state()
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):
"""
Scenario contains the controls (scene, target positions)
DrawnScenario class does the actual drawing of points incl. transitions
This distinction is only for ordering the code
"""
MAX_HISTORY = 130 # 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)
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(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.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.tracks_to_self: Optional[Generator] = None
self.tracks_to_self_pos = None
self.tracks_to_self_fetched_at = None
# self.line_prediction_drawn = self.line_prediction_faded
def update(self):
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():
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.start() # reset positions
# 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)
# special case: LOITERING
if self.stage.get_setting('stage.loitering_animation', True) and 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)
# TODO: transition fade, using to_alpha(), so it can fade back in again:
self.line_history.get(FadeOutJitterLine).set_alpha(1 - transition)
self.line_prediction.get(FadeOutLine).set_alpha(1 - transition)
current_position = self.track.projected_history[-1]
current_position_rounded = np.round(current_position*2) # cache per 1/2 meter
time_diff = inf if not self.tracks_to_self_fetched_at else time.perf_counter() - self.tracks_to_self_fetched_at
# 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
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)
self.track_ids_to_self = iter(track_ids)
self.tracks_to_self = 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)")
# 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)
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
logger.info(f"play history item: {current_history_id}")
self.line_others.get(FadeOutLine).set_alpha(1)
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:
pass
# if self.scene is ScenarioScene.CORRECTED_PREDICTION:
# self.line_prediction.get(DashedLine).skip = False
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)
# 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()
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!
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.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()
if self.config.debug_map:
debug_color = SrgbaColor(0.,0.,1.,1.)
self.debug_lines = RenderableLines(load_lines_from_svg(self.config.debug_map, 100, debug_color))
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
# t1 = time.perf_counter()
self.loop_receive()
# t2 = time.perf_counter()
self.loop_update_scenarios()
# t3 = time.perf_counter()
self.loop_render(dt)
# t4 = time.perf_counter()
# print(t2-t1, t3-t2, t4-t3)
def loop_receive(self):
# 1) receive predictions
try:
prediction_frame: Frame = self.prediction_sock.recv_pyobj(zmq.NOBLOCK)
for track_id, track in prediction_frame.tracks.items():
proj_track = ProjectedTrack(track, prediction_frame.camera)
self.scenarios[track_id].recv_prediction(proj_track)
except zmq.ZMQError as e:
# no msgs
pass
# 2) receive tracker tracks
try:
trajectory_frame: Frame = self.trajectory_sock.recv_pyobj(zmq.NOBLOCK)
for track_id, track in trajectory_frame.tracks.items():
proj_track = ProjectedTrack(track, trajectory_frame.camera)
self.scenarios[track_id].recv_track(proj_track)
except zmq.ZMQError as e:
pass
# self.logger.debug(f'reuse tracks')
def loop_update_scenarios(self):
"""Update active scenarios and handle pauses/completions."""
# 1) process timestep for all scenarios
for s in self.scenarios.values():
s.update()
# 2) Remove stale tracks and take-overs
for track_id, scenario in list(self.scenarios.items()):
if scenario.lost_factor() >= 1:
if scenario in self.active_scenarios:
self.active_scenarios = list(filter(scenario.__ne__, self.active_scenarios))
self.logger.info(f"rm lost track {track_id}")
del self.scenarios[track_id]
if scenario.takeover_factor() >= 1:
if scenario in self.active_scenarios:
self.active_scenarios = list(filter(scenario.__ne__, self.active_scenarios))
scenario.taken_over()
# 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]
# ... highest priority first
pending_scenarios.sort(key=lambda s: s.get_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):
scenario = pending_scenarios.pop(0)
self.active_scenarios.append(scenario)
scenario.start()
# 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()
]
eligible_active_scenarios.sort(key=lambda s: s.get_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():
# Takeover! Stop the active scenario
# will be cleaned up in update() loop after animation finishes
# automatically triggering the start of the highest priority scene
lowest_priority_active.take_over()
def loop_render(self, dt: DeltaT):
"""Draw all active scenarios onto the canvas."""
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
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)
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()
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()
@classmethod
def arg_parser(cls) -> ArgumentParser:
argparser = ArgumentParser()
argparser.add_argument('--zmq-frame-noimg-addr',
help='Manually specity communication addr for the frame messages',
type=str,
default="ipc:///tmp/feeds_frame2")
argparser.add_argument('--zmq-trajectory-addr',
help='Manually specity communication addr for the trajectory messages',
type=str,
default="ipc:///tmp/feeds_traj")
argparser.add_argument('--zmq-prediction-addr',
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")
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,
default=2)
# TODO: this should be subsumed to some sort of Track Dataset loader
historyargs = argparser.add_argument_group("Track History Loader")
historyargs.add_argument("--camera-fps",
help="Camera FPS",
type=int,
default=12)
historyargs.add_argument("--homography",
help="File with homography params [Deprecated]",
type=Path,
default='../DATASETS/VIRAT_subset_0102x/VIRAT_0102_homography_img2world.txt',
action=HomographyAction)
historyargs.add_argument("--calibration",
help="File with camera intrinsics and lens distortion params (calibration.json)",
# type=Path,
required=True,
# default=None,
action=CameraAction)
historyargs.add_argument("--cache-path",
help="Where to cache the Track History dataset",
type=Path,
required=True,
)
historyargs.add_argument("--tracker-output-dir",
help="Directory for the track reader (e.g. EXPERIMENT/raw/_name_)",
type=Path,
required=True,
)
return argparser

1009
trap/stage_old.py
View file

File diff suppressed because it is too large Load diff

276
trap/stage_renderer.py
View file

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

79
trap/timer.py
View file

@ -1,67 +1,54 @@
import collections
import time
from multiprocessing.sharedctypes import Value
from typing import MutableSequence
from multiprocessing.sharedctypes import RawValue, Value, Array
from ctypes import c_double
class Timer():
"""
Multiprocess timer. Count iterations in one process, while converting that
to fps in the other.
Measure 2 independent things: the freuency of tic, and the duration of tic->toc
Note that indeed these don't need to be equal
"""
def __init__(self, name = 'timer') -> None:
self.name = name
self.tocs: MutableSequence[(float, int)] = collections.deque(maxlen=5)
self.iterations = Value('i', 0)
# def tic(self):
# now = time.time()
# if self.last_tic is None:
# self.last_tic = now
# return
# duration = now - self.last_tic
# self.last_tic = now
def __init__(self) -> None:
self.last_tic = RawValue(c_double, -1)
self.last_toc = RawValue(c_double, -1)
self.fps = RawValue(c_double, -1)
self.processing_duration = RawValue(c_double, -1)
# current_fps = 1 / duration
# if not self.fps:
# self.fps = current_fps
# else:
# self.fps = self.fps * (1-self.smoothing) + current_fps * self.smoothing
self.smoothing = .1
def tic(self):
now = time.time()
if self.last_tic is None:
self.last_tic = now
return
duration = now - self.last_tic
self.last_tic = now
current_fps = 1 / duration
if not self.fps:
self.fps = current_fps
else:
self.fps = self.fps * (1-self.smoothing) + current_fps * self.smoothing
def toc(self):
self.iterations += 1
def snapshot(self):
self.tocs.append((time.perf_counter(), self.iterations.value))
self.last_toc = time.time()
duration = self.last_toc - self.last_tic
self.processing_duration = self.processing_duration * (1-self.smoothing) + duration * self.smoothing
@property
def fps(self):
if len(self.tocs) < 2:
return 0
dt = self.tocs[-1][0] - self.tocs[0][0]
di = self.tocs[-1][1] - self.tocs[0][1]
return di/dt
pass
class TimerCollection():
def __init__(self) -> None:
self._timers = set()
def snapshot(self):
for timer in self._timers:
timer.snapshot()
def to_string(self)->str:
strs = [f"{t.name} {t.fps:.2f}" for t in self._timers]
return " ".join(strs)
def new(self, name='timer'):
t = Timer(name)
self._timers.add(t)
return t
def print(self)->str:
print('Update', end='\r')

733
trap/tools.py
View file

@ -1,733 +0,0 @@
from __future__ import annotations
from argparse import Namespace
from dataclasses import dataclass
import json
import math
from pathlib import Path
import pickle
from tempfile import mktemp
import jsonlines
import numpy as np
import pandas as pd
import shapely
from shapely.ops import split
from trap.preview_renderer import DrawnTrack
import trap.tracker
from trap.config import parser
from trap.frame_emitter import Camera, Detection, DetectionState, video_src_from_config, Frame
from trap.tracker import DETECTOR_YOLOv8, FinalDisplacementFilter, Smoother, TrackReader, _ultralytics_track, Track, TrainingDataWriter, Tracker, read_tracks_json
from collections import defaultdict
import logging
import cv2
from typing import Callable, List, Iterable, Optional
from ultralytics import YOLO
from ultralytics.engine.results import Results as YOLOResult
import tqdm
from trap.utils import inv_lerp, lerp
logger = logging.getLogger('tools')
class FrameGenerator():
def __init__(self, config):
self.video_srcs = video_src_from_config(config)
self.config = config
if not hasattr(config, "H"):
raise RuntimeError("Set homography file with --homography param")
# store current position
self.video_path = None
self.video_nr = None
self.frame_count = None
self.frame_idx = None
self.n = 0
def __iter__(self):
for video_nr, video_path in enumerate(self.video_srcs):
self.video_path = video_path
self.video_nr = video_nr
logger.info(f"Play from '{str(video_path)}'")
video = cv2.VideoCapture(str(video_path))
fps = video.get(cv2.CAP_PROP_FPS)
self.frame_count = video.get(cv2.CAP_PROP_FRAME_COUNT)
if self.frame_count < 0:
self.frame_count = math.inf
self.frame_idx = 0
if self.config.video_offset:
logger.info(f"Start at frame {self.config.video_offset}")
video.set(cv2.CAP_PROP_POS_FRAMES, self.config.video_offset)
self.frame_idx = self.config.video_offset
while True:
ret, img = video.read()
self.frame_idx+=1
self.n+=1
# seek to 0 if video has finished. Infinite loop
if not ret:
# now loading multiple files
break
frame = Frame(index=self.n, img=img, H=self.config.H, camera=self.config.camera)
yield frame
def marquee_string(string: str, window: int, i: int):
if window > len(string):
return string
# too_much = len(string) - window
# offset = i % too_much
# return string[offset:offset+window]
too_much = len(string) - window
offset = i % (too_much*2)
if offset > too_much:
offset = too_much - (offset-too_much)
return string[offset:offset+window]
def tracker_preprocess():
config = parser.parse_args()
tracker = Tracker(config)
# model = YOLO('EXPERIMENTS/yolov8x.pt')
with TrainingDataWriter(config.save_for_training) as writer:
bar = tqdm.tqdm()
tracks = defaultdict(lambda: Track())
total = 0
frames = FrameGenerator(config)
total_tracks = set()
for frame in frames:
bar.update()
detections = tracker.track_frame(frame)
total += len(detections)
# detections = _yolov8_track(frame, model, imgsz=1440, classes=[0])
for detection in detections:
track = tracks[detection.track_id]
track.track_id = detection.track_id # for new tracks
track.history.append(detection) # add to history
active_track_ids = [d.track_id for d in detections]
active_tracks = {t.track_id: t for t in tracks.values() if t.track_id in active_track_ids}
total_tracks.update(active_track_ids)
bar.set_description(f"{frames.video_nr}/{len(frames.video_srcs)} [{frames.frame_idx}/{frames.frame_count}] {marquee_string(str(frames.video_path), 10, frames.n//2)} | dets {len(detections)}: {[d.track_id for d in detections]} (∑{total} → {len(total_tracks)})")
writer.add(frame, active_tracks.values())
logger.info("Done!")
bgr_colors = [
(255, 0, 0),
(0, 255, 0),
# (0, 0, 255),# red used for missing waypoints
(0, 255, 255),
]
def detection_color(detection: Detection, i, prev_detection: Optional[Detection] = None):
vague = detection.state == DetectionState.Lost or (prev_detection and detection.frame_nr - prev_detection.frame_nr > 1)
return bgr_colors[i % len(bgr_colors)] if not vague else (0,0,255)
def to_point(coord):
return (int(coord[0]), int(coord[1]))
def tracker_compare():
config = parser.parse_args()
trackers: List[Tracker] = []
# TODO, support all tracker.DETECTORS
for tracker_id in [
trap.tracker.DETECTOR_YOLOv8,
# trap.tracker.DETECTOR_MASKRCNN,
# trap.tracker.DETECTOR_RETINANET,
trap.tracker.DETECTOR_FASTERRCNN,
]:
tracker_config = Namespace(**vars(config))
tracker_config.detector = tracker_id
trackers.append(Tracker(tracker_config))
frames = FrameGenerator(config)
bar = tqdm.tqdm(frames)
cv2.namedWindow("frame", cv2.WND_PROP_FULLSCREEN)
cv2.setWindowProperty("frame",cv2.WND_PROP_FULLSCREEN,cv2.WINDOW_FULLSCREEN)
for frame in bar:
# frame.img = cv2.undistort(frame.img, config.camera.mtx, config.camera.dist, None, config.camera.newcameramtx) # try to undistort for better detections, seems not to matter at all
trackers_detections = [(t, t.track_frame(frame)) for t in trackers]
for i, tracker in enumerate(trackers):
cv2.putText(frame.img, tracker.config.detector, (10,30*(i+1)), cv2.FONT_HERSHEY_DUPLEX, 1, color=bgr_colors[i % len(bgr_colors)])
for i, (tracker, detections) in enumerate(trackers_detections):
for track_id in tracker.tracks:
draw_track(frame.img, tracker.tracks[track_id], i)
for detection in detections:
color = color = detection_color(detection, i)
l, t, r, b = detection.to_ltrb()
cv2.rectangle(frame.img, (l, t), (r,b), color)
cv2.putText(frame.img, f"{detection.track_id}", (l, b+10), cv2.FONT_HERSHEY_DUPLEX, 1, color=color)
conf = f"{detection.conf:.3f}" if detection.conf is not None else "None"
cv2.putText(frame.img, f"{detection.det_class} - {conf}", (l, t), cv2.FONT_HERSHEY_DUPLEX, .7, color=color)
cv2.imshow('frame',cv2.resize(frame.img, (1920, 1080)))
cv2.waitKey(1)
bar.set_description(f"[{frames.video_nr}/{len(frames.video_srcs)}] [{frames.frame_idx}/{frames.frame_count}] {str(frames.video_path)}")
def transition_path_points(path: np.array, t: float):
"""
"""
if t >= 1:
return path
if t <= 0:
return np.array([path[0]])
# new_path = np.array([])
lengths = np.sqrt(np.sum(np.diff(path, axis=0)**2, axis=1))
cum_lenghts = np.cumsum(lengths)
# distance = cum_lenghts[-1] * t
# ts = np.concatenate((np.array([0.]), cum_lenghts / cum_lenghts[-1]))
# print(cum_lenghts[-1])
DRAW_SPEED = 35 # fixed speed (independent of lenght) TODO)) make variable
ts = np.concatenate((np.array([0.]), cum_lenghts / DRAW_SPEED))
new_path = [path[0]]
for a, b, t_a, t_b in zip(path[:-1], path[1:], ts[:-1], ts[1:]):
if t_b < t:
new_path.append(b)
continue
# interpolate
relative_t = inv_lerp(t_a, t_b, t)
x = lerp(a[0], b[0], relative_t)
y = lerp(a[1], b[1], relative_t)
new_path.append([x,y])
break
return np.array(new_path)
from shapely.geometry import LineString
from shapely.geometry import Point
from sklearn.cluster import AgglomerativeClustering
@dataclass
class PointCluster:
point: np.ndarray
start: np.ndarray
source_points: List[np.ndarray]
probability: float
next_point_clusters: List[PointCluster]
def cluster_predictions_by_radius(start_point, lines: Iterable[np.ndarray] | LineString, radius = .5, p_factor = 1.) -> List[PointCluster]:
# start = lines[0][0]
p0 = Point(*start_point)
# print(lines[0][0], start_point)
circle = p0.buffer(radius).boundary
# print(lines)
# print([line.tolist() for line in lines])
intersections = []
remaining_lines = []
for line in lines:
linestring = line if type(line) is LineString else LineString(line.tolist())
intersection = circle.intersection(linestring)
if type(intersection) is LineString and intersection.is_empty:
# No intersection with circle, a dangling endpoint that we can skip
continue
if type(intersection) is not Point:
# with multiple intersections: use only the first one
intersection = intersection.geoms[0]
# set a buffer around the intersection to assure a match is fond oun the line
split_line = split(linestring, intersection.buffer(.01))
remaining_line = split_line.geoms[2] if len(split_line.geoms) > 2 else None
# print(intersection, split_line)
intersections.append(intersection)
remaining_lines.append(remaining_line)
if len(intersections) < 1:
return []
# linestrings = [LineString(line.tolist()) for line in lines]
# intersections = [circle.intersection(line) for line in linestrings]
# dangling_lines = [(type(i) is LineString and i.is_empty) for i in intersections]
# intersections = [False if is_end else (p if type(p) is Point else p.geoms[0]) for p, is_end in zip(intersections, dangling_lines)]
# as all intersections are on the same circle we can guestimate angle by
# estimating distance, as circumfence is 2*pi*r, thus distance ~ proportional with radius.
if len(intersections) > 1:
clustering = AgglomerativeClustering(None, linkage="ward", distance_threshold=2*math.pi * radius / 6)
coords = np.asarray([i.coords for i in intersections]).reshape((-1,2))
assigned_clusters = clustering.fit_predict(coords)
else:
assigned_clusters = [0] # only one item
clusters = defaultdict(lambda: [])
cluster_remainders = defaultdict(lambda: [])
for point, line, c in zip(intersections, remaining_lines, assigned_clusters):
clusters[c].append(point)
cluster_remainders[c].append(line)
line_clusters = []
for c, points in clusters.items():
mean = np.mean(points, axis=0)
prob = p_factor * len(points) / len(assigned_clusters)
remaining_lines = cluster_remainders[c]
remaining_lines = list(filter(None, remaining_lines))
next_points = cluster_predictions_by_radius(mean, remaining_lines, radius, prob)
line_clusters.append(PointCluster(mean, start_point, points, prob, next_points))
# split_lines = [shapely.ops.split(line, point) for line, point in zip(linestrings, intersections)]
# remaining_lines = [l[1] for l in split_lines if len(l) > 1]
# print(line_clusters)
return line_clusters
# def cosine_similarity(point1, point2):
# dot_product = np.dot(point1, point2)
# norm1 = np.linalg.norm(point1)
# norm2 = np.linalg.norm(point2)
# return dot_product / (norm1 * norm2)
# p = Point(5,5)
# c = p.buffer(3).boundary
# l = LineString([(0,0), (10, 10)])
# i = c.intersection(l)
def track_predictions_to_lines(track: Track, camera:Camera, anim_position=.8):
if not track.predictions:
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 = []
for pred_i, pred in enumerate(track.predictions):
pred_coords = pred #cv2.perspectiveTransform(np.array([pred]), inv_H)[0].tolist()
# line_points = pred_coords
line_points = np.concatenate(([current_point], pred_coords)) # 'current point' is amoving target
# print(pred_coords, current_point, line_points)
line_points = transition_path_points(line_points, slide_t)
lines.append(line_points)
# print("prediction line", len(line_points))
# break # TODO: only one
return lines
def drawntrack_predictions_to_lines(drawn_track: DrawnTrack, camera:Camera, anim_position=.8):
if not drawn_track.drawn_predictions:
return
# current_point = drawn_track.pred_track.get_projected_history(camera=camera)[-1] # not guaranteed to be up to date
current_point = drawn_track.drawn_predictions[0][0]
# print(current_point)
slide_t = min(1, max(0, inv_lerp(0, 0.8, anim_position))) # slide_position
lines = []
for pred_i, pred in enumerate(drawn_track.drawn_predictions):
pred_coords = pred #cv2.perspectiveTransform(np.array([pred]), inv_H)[0].tolist()
# line_points = pred_coords
line_points = np.concatenate(([current_point], pred_coords)) # 'current point' is amoving target
# print(pred_coords, current_point, line_points)
line_points = transition_path_points(line_points, slide_t)
lines.append(line_points)
# print("prediction line", len(line_points))
# break # TODO: only one
return lines
def draw_track_predictions(img: cv2.Mat, track: Track, color_index: int, camera:Camera, convert_points: Optional[Callable], anim_position=.8, as_clusters=False):
"""
anim_position: 0-1
"""
lines = track_predictions_to_lines(track, camera, anim_position)
if not lines:
return
opacity = 1-min(1, max(0, inv_lerp(0.8, 1, anim_position))) # fade out
# if convert_points:
# current_point = convert_points([current_point])[0]
color = bgr_colors[color_index % len(bgr_colors)]
color = tuple([int(c*opacity) for c in color])
if as_clusters:
clusters = cluster_predictions_by_radius(current_point, lines, 1.5)
def draw_cluster(img, cluster: PointCluster):
points = convert_points([cluster.start, cluster.point])
# cv2 only draws to integer coordinates
points = np.rint(points).astype(int)
thickness = max(1, int(cluster.probability * 6))
thickness=1
# if len(cluster.next_point_clusters) == 1:
# not a final point, nor a split:
cv2.line(img, points[0], points[1], color, thickness, lineType=cv2.LINE_AA)
# else:
# cv2.arrowedLine(img, points[0], points[1], color, thickness, cv2.LINE_AA)
for sub in cluster.next_point_clusters:
draw_cluster(img, sub)
# pass
# # cv2.circle(img, end, 2, color, 1, lineType=cv2.LINE_AA)
# print(clusters)
for cluster in clusters:
draw_cluster(img, cluster)
else:
# convert function (e.g. to project points to img space)
if convert_points:
lines = [convert_points(points) for points in lines]
# cv2 only draws to integer coordinates
lines = [np.rint(points).astype(int) for points in lines]
# draw in a single pass
# line_points = line_points.reshape((1, -1,1,2)) # TODO)) SEems to do nothing..
cv2.polylines(img, lines, False, color, 2, cv2.LINE_AA)
def draw_trackjectron_history(img: cv2.Mat, track: Track, color_index: int, convert_points: Optional[Callable]):
if not track.predictor_history:
return
coords = track.predictor_history #cv2.perspectiveTransform(np.array([track.predictor_history]), inv_H)[0].tolist()
if convert_points:
coords = convert_points(coords)
# color = (128,0,128) if pred_i else (128,128,0)
color = tuple(b/2 for b in bgr_colors[color_index % len(bgr_colors)])
for ci in range(0, len(coords)):
if ci == 0:
# TODO)) prev point
continue
# start = [int(p) for p in coords[-1]]
# start = [0,0]?
# print(start)
else:
start = [int(p) for p in coords[ci-1]]
end = [int(p) for p in coords[ci]]
cv2.line(img, start, end, color, 1, lineType=cv2.LINE_AA)
cv2.circle(img, end, 4, color, 1, lineType=cv2.LINE_AA)
def draw_track_projected(img: cv2.Mat, track: Track, color_index: int, camera: Camera, convert_points: Optional[Callable]):
history = track.get_projected_history(camera=camera)
if convert_points:
history = convert_points(history)
cv2.putText(img, f"{track.track_id} ({len(history)})", to_point(history[0]), cv2.FONT_HERSHEY_DUPLEX, 1, color=bgr_colors[color_index % len(bgr_colors)])
point_color = bgr_colors[color_index % len(bgr_colors)]
cv2.circle(img, to_point(history[0]), 3, point_color, 2)
points = np.rint(history.reshape((-1,1,2))).astype(np.int32)
cv2.polylines(img, [points], False, point_color, 1)
for j in range(len(history)-1):
# a = history[j]
b = history[j+1]
detection = track.history[j+1]
color = point_color if detection.state == DetectionState.Confirmed else (100,100,100)
# cv2.line(img, to_point(a), to_point(b), point_color, 1)
cv2.circle(img, to_point(b), 3, color, 2)
def draw_track(img: cv2.Mat, track: Track, color_index: int):
history = track.history
cv2.putText(img, f"{track.track_id} ({len(history)})", to_point(history[0].get_foot_coords()), cv2.FONT_HERSHEY_DUPLEX, 1, color=bgr_colors[color_index % len(bgr_colors)])
point_color = detection_color(history[0], color_index)
cv2.circle(img, to_point(history[0].get_foot_coords()), 3, point_color, 2)
for j in range(len(history)-1):
a = history[j]
b = history[j+1]
# TODO)) replace with Track.get_with_interpolated_history()
# gap = b.frame_nr - a.frame_nr - 1
# if gap < 0:
# print(f"WARNING, gap between frames {a.frame_nr} -> {b.frame_nr} is negative?")
# if gap > 0:
# for g in range(gap):
# p1 = a.get_foot_coords()
# p2 = b.get_foot_coords()
# point = (lerp(p1[0], p2[0], g/gap), lerp(p1[1], p2[1], g/gap))
# cv2.circle(img, to_point(point), 3, (0,0,255), 1)
color = detection_color(b, color_index, a)
cv2.line(img, to_point(a.get_foot_coords()), to_point(b.get_foot_coords()), color, 1)
point_color = detection_color(b, color_index)
cv2.circle(img, to_point(b.get_foot_coords()), 3, point_color, 2)
def blacklist_tracks():
config = parser.parse_args()
cv2.namedWindow("frame", cv2.WND_PROP_FULLSCREEN)
cv2.setWindowProperty("frame",cv2.WND_PROP_FULLSCREEN,cv2.WINDOW_FULLSCREEN)
backdrop = cv2.imread('../DATASETS/hof3/output.png')
blacklist = []
path: Path = config.save_for_training
reader = TrackReader(path, config.camera.fps, exclude_whitelisted = True)
tracks = [t for t in reader]
filter = FinalDisplacementFilter(2.0)
tracks = filter.apply(tracks, config.camera)
# blacklist_file = path / "blacklist.jsonl"
# whitelist_file = path / "whitelist.jsonl" # for skipping
# tracks_file = path / "tracks.json"
# FPS = 12 # TODO)) From config
# if whitelist_file.exists():
# # with whitelist_file.open('r') as fp:
# with jsonlines.open(whitelist_file, 'r') as reader:
# whitelist = [l for l in reader.iter(type=str)]
# else:
# whitelist = []
smoother = Smoother()
try:
for track in tqdm.tqdm(tracks):
if len(track.history) < 5:
continue
img = backdrop.copy()
draw_track(img, track.get_with_interpolated_history(), 0)
draw_track(img, smoother.smooth_track(track.get_with_interpolated_history()).get_sampled(5), 1)
imgS = cv2.resize(img, (1920, 1080))
cv2.imshow('frame', imgS)
while True:
k = cv2.waitKey(0)
if k==27: # Esc key to stop
raise StopIteration
elif k == ord('s'):
break # skip for now
elif k == ord('y'):
print('whitelist', track.track_id)
with jsonlines.open(reader.whitelist_file, mode='a') as writer:
# skip next time around
writer.write(track.track_id)
break
elif k == ord('n'):
print('blacklist', track.track_id)
# logger.info(f"Append {len(track)} items to {str(reader.blacklist_file)}")
with jsonlines.open(reader.blacklist_file, mode='a') as writer:
writer.write(track.track_id)
break
else:
# ignore all other keypresses
print(k) # else print its value
continue
except StopIteration as e:
pass
def rewrite_raw_track_files():
logging.basicConfig(level=logging.DEBUG)
config = parser.parse_args()
trap.tracker.rewrite_raw_track_files(config.save_for_training)
def interpolate_missing_frames(data: pd.DataFrame):
missing=0
old_size=len(data)
# slow way to append missing steps to the dataset
for ind, row in tqdm.tqdm(data.iterrows()):
if row['diff'] > 1:
for s in range(1, int(row['diff'])):
# add as many entries as missing
missing += 1
data.loc[len(data)] = [row['frame_id']-s, row['track_id'], np.nan, np.nan, np.nan, np.nan, np.nan, np.nan, 1, 1]
# new_frame = [data.loc[ind-1]['frame_id']+s, row['track_id'], np.nan, np.nan, np.nan, np.nan, np.nan]
# data.loc[len(data)] = new_frame
logger.info(f'was:{old_size} added:{missing}, new length: {len(data)}')
# now sort, so that the added data is in the right place
data.sort_values(by=['track_id', 'frame_id'], inplace=True)
df=data.copy()
df = df.groupby('track_id').apply(lambda group: group.interpolate(method='linear'))
df.reset_index(drop=True, inplace=True)
# update diff, shouldnow be 1 | NaN
data['diff'] = data.groupby(['track_id'])['frame_id'].diff()
# data = df
return df
def smooth(data: pd.DataFrame):
df=data.copy()
if 'x_raw' not in df:
df['x_raw'] = df['x']
if 'y_raw' not in df:
df['y_raw'] = df['y']
print("Running smoother")
# print(df)
# from tsmoothie.smoother import KalmanSmoother, ConvolutionSmoother
smoother = Smoother(convolution=False)
def smoothing(data):
# smoother = ConvolutionSmoother(window_len=SMOOTHING_WINDOW, window_type='ones', copy=None)
return smoother.smooth(data).tolist()
# df=df.assign(smooth_data=smoother.smooth_data[0])
# return smoother.smooth_data[0].tolist()
# operate smoothing per axis
print("smooth x")
df['x'] = df.groupby('track_id')['x_raw'].transform(smoothing)
print("smooth y")
df['y'] = df.groupby('track_id')['y_raw'].transform(smoothing)
return df
def load_tracks_from_csv(file: Path, fps: float, grid_size: Optional[int] = None, sample: Optional[int] = None):
cache_file = Path('/tmp/load_tracks-smooth-' + file.name)
if cache_file.exists():
data = pd.read_pickle(cache_file)
else:
# grid_size is in points per meter
# sample: sample to every n-th point. Thus sample=5 converts 12fps to 2.4fps, and 4 to 3fps
data = pd.read_csv(file, delimiter="\t", index_col=False, header=None)
# l,t,w,h: image space (pixels)
# x,y: world space (meters or cm depending on homography)
data.columns = ['frame_id', 'track_id', 'l', 't', 'w', 'h', 'x', 'y', 'state']
data['frame_id'] = pd.to_numeric(data['frame_id'], downcast='integer')
data['frame_id'] = data['frame_id'] // 10 # compatibility with Trajectron++
data.sort_values(by=['track_id', 'frame_id'],inplace=True)
data.set_index(['track_id', 'frame_id'])
# cm to meter
data['x'] = data['x']/100
data['y'] = data['y']/100
if grid_size is not None:
data['x'] = (data['x']*grid_size).round() / grid_size
data['y'] = (data['y']*grid_size).round() / grid_size
data['diff'] = data.groupby(['track_id'])['frame_id'].diff() #.fillna(0)
data['diff'] = pd.to_numeric(data['diff'], downcast='integer')
data = interpolate_missing_frames(data)
data = smooth(data)
data.to_pickle(cache_file)
if sample is not None:
print(f"Samping 1/{sample}, of {data.shape[0]} items")
data["idx_in_track"] = data.groupby(['track_id']).cumcount() # create index in group
groups = data.groupby(['track_id'])
# print(groups, data)
# selection = groups['idx_in_track'].apply(lambda x: x % sample == 0)
# print(selection)
selection = data["idx_in_track"].apply(lambda x: x % sample == 0)
# data = data[selection]
data = data.loc[selection].copy() # avoid errors
# # convert from e.g. 12Hz, to 2.4Hz (1/5)
# sampled_groups = []
# for name, group in data.groupby('track_id'):
# sampled_groups.append(group.iloc[::sample])
# print(f"Sampled {len(sampled_groups)} groups")
# data = pd.concat(sampled_groups, axis=1).T
print(f"Done sampling kept {data.shape[0]} items")
# String ot int
data['track_id'] = pd.to_numeric(data['track_id'], downcast='integer')
# redo diff after possible sampling:
data['diff'] = data.groupby(['track_id'])['frame_id'].diff()
# timestep to seconds
data['dt'] = data['diff'] * (1/fps)
# "Deriving displacement, velocity and accelation from x and y")
data['dx'] = data.groupby(['track_id'])['x'].diff()
data['dy'] = data.groupby(['track_id'])['y'].diff()
data['vx'] = data['dx'].div(data['dt'], axis=0)
data['vy'] = data['dy'].div(data['dt'], axis=0)
data['ax'] = data.groupby(['track_id'])['vx'].diff().div(data['dt'], axis=0)
data['ay'] = data.groupby(['track_id'])['vy'].diff().div(data['dt'], axis=0)
# then we need the velocity itself
data['v'] = np.sqrt(data['vx'].pow(2) + data['vy'].pow(2))
# and derive acceleration
data['a'] = data.groupby(['track_id'])['v'].diff().div(data['dt'], axis=0)
# we can calculate heading based on the velocity components
data['heading'] = (np.arctan2(data['vy'], data['vx']) * 180 / np.pi) % 360
# and derive it to get the rate of change of the heading
data['d_heading'] = data.groupby(['track_id'])['heading'].diff().div(data['dt'], axis=0)
# we can backfill the derived parameters (v and a), assuming they were constant when entering the frame
# so that our model can make estimations, based on these assumed values
group = data.groupby(['track_id'])
for field in ['dx', 'dy', 'vx', 'vy', 'ax', 'ay', 'v', 'a', 'heading', 'd_heading']:
data[field] = group[field].bfill()
data.set_index(['track_id', 'frame_id'], inplace=True) # use for quick access
return data
def filter_short_tracks(data: pd.DataFrame, n):
return data.groupby(['track_id']).filter(lambda group: len(group) >= n) # a lenght of 3 is neccessary to have all relevant derivatives of position
# print(filtered_data.shape[0], "items in filtered set, out of", data.shape[0], "in total set")
def normalise_position(data: pd.DataFrame):
mu = data[['x','y']].mean(axis=0)
std = data[['x','y']].std(axis=0)
data[['x_norm','y_norm']] = (data[['x','y']] - mu) / std
return data, mu, std

186
trap/track_history.py
View file

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

74
trap/track_writer.py
View file

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

1029
trap/tracker.py
View file

File diff suppressed because it is too large Load diff

255
trap/utils.py
View file

@ -1,255 +0,0 @@
# lerp & inverse lerp from https://gist.github.com/laundmo/b224b1f4c8ef6ca5fe47e132c8deab56
from collections import namedtuple
import linecache
import math
import os
from pathlib import Path
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:
"""Linear interpolate on the scale given by a to b, using t as the point on that scale.
Examples
--------
50 == lerp(0, 100, 0.5)
4.2 == lerp(1, 5, 0.8)
"""
return (1 - t) * a + t * b
def inv_lerp(a: float, b: float, v: float) -> float:
"""Inverse Linar Interpolation, get the fraction between a and b on which v resides.
Examples
--------
0.5 == inv_lerp(0, 100, 50)
0.8 == inv_lerp(1, 5, 4.2)
"""
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
def exponentialDecayRounded(a, b, decay, dt, abs_tolerance):
"""Exponential decay as alternative to Lerp
Introduced by Freya Holmér: https://www.youtube.com/watch?v=LSNQuFEDOyQ
"""
c = b + (a-b) * math.exp(-decay * dt)
if abs(b-c) < abs_tolerance:
return b
return c
def exponentialDecay(a, b, decay, dt):
"""Exponential decay as alternative to Lerp
Introduced by Freya Holmér: https://www.youtube.com/watch?v=LSNQuFEDOyQ
"""
return b + (a-b) * math.exp(-decay * dt)
def relativePointToPolar(origin, point) -> tuple[float, float]:
x, y = point[0] - origin[0], point[1] - origin[1]
return np.sqrt(x**2 + y**2), np.arctan2(y, x)
def relativePolarToPoint(origin, r, angle) -> tuple[float, float]:
return r * np.cos(angle) + origin[0], r * np.sin(angle) + origin[1]
# def line_intersection(line1, line2):
# xdiff = (line1[0][0] - line1[1][0], line2[0][0] - line2[1][0])
# ydiff = (line1[0][1] - line1[1][1], line2[0][1] - line2[1][1])
# def det(a, b):
# return a[0] * b[1] - a[1] * b[0]
# div = det(xdiff, ydiff)
# if div == 0:
# return None
# d = (det(*line1), det(*line2))
# x = det(d, xdiff) / div
# y = det(d, ydiff) / div
# return x, y
# def polyline_intersection(poly1, poly2):
# for i, p1_first_point in enumerate(poly1[:-1]):
# p1_second_point = poly1[i + 1]
# for j, p2_first_point in enumerate(poly2[:-1]):
# p2_second_point = poly2[j + 1]
# intersection = line_intersection((p1_first_point, p1_second_point), (p2_first_point, p2_second_point))
# if intersection:
# return intersection # returns x,y
# return None
def get_bins(bin_size: float):
return [[bin_size, 0], [bin_size, bin_size], [0, bin_size], [-bin_size, bin_size], [-bin_size, 0], [-bin_size, -bin_size], [0, -bin_size], [bin_size, -bin_size]]
def convert_world_space_to_img_space(H: cv2.Mat, scale=100):
"""Transform the given matrix so that it immediately converts
the points to img space"""
new_H = H.copy()
new_H[:2] = H[:2] * scale
return new_H
def convert_world_points_to_img_points(points: Iterable, scale=100):
"""Transform the given matrix so that it immediately converts
the points to img space"""
if isinstance(points, np.ndarray):
return np.array(points) * scale
return [[p[0]*scale, p[1]*scale] for p in points]
def display_top(snapshot: tracemalloc.Snapshot, key_type='lineno', limit=5):
snapshot = snapshot.filter_traces((
tracemalloc.Filter(False, "<frozen importlib._bootstrap>"),
tracemalloc.Filter(False, "<unknown>"),
))
top_stats = snapshot.statistics(key_type)
print("Top %s lines" % limit)
for index, stat in enumerate(top_stats[:limit], 1):
frame = stat.traceback[0]
# replace "/path/to/module/file.py" with "module/file.py"
filename = os.sep.join(frame.filename.split(os.sep)[-2:])
print("#%s: %s:%s: %.1f KiB"
% (index, filename, frame.lineno, stat.size / 1024))
line = linecache.getline(frame.filename, frame.lineno).strip()
if line:
print(' %s' % line)
other = top_stats[limit:]
if other:
size = sum(stat.size for stat in other)
print("%s other: %.1f KiB" % (len(other), size / 1024))
total = sum(stat.size for stat in top_stats)
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')
# homography_matrix = H_img_to_world.copy()
# homography_matrix /= homography_matrix[2, 2] # normalise? https://github.com/StanfordASL/Trajectron-plus-plus/issues/14#issuecomment-637880857
# homography_matrix = np.linalg.inv(homography_matrix)
homography_matrix = H_world_to_map
# RGB png image has 3 layers
img = img.astype(np.uint8)
# img = cv2.resize(img, (img.shape[1]//10, img.shape[0]//10))
img_reverse = img[::-1,:,:] # origin to bottom left, instead of top-left
layers = np.transpose(img, (2, 1, 0)) # array order: layers, x, y
layers = layers.copy() # copy to apply negative stride
# layers =
#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:
org_shape = scene_pts.shape
scene_pts = scene_pts.reshape((-1, 2))
N, dims = scene_pts.shape
points_with_one = np.ones((dims + 1, N))
points_with_one[:dims] = scene_pts.T
# map_points = np.fliplr((self.homography @ points_with_one).T[..., :dims]).astype(np.uint32)
# map_points = np.flipud((self.homography @ points_with_one).T[..., :dims]).astype(np.uint32)
map_points = (self.homography @ points_with_one).T[..., :dims].astype(np.uint32)
if org_shape is not None:
map_points = map_points.reshape(org_shape)
# print(scene_pts,'->', map_points)
# exit()
return map_points
# nuscener process_data.py
# type_map = dict()
# canvas_size = (np.round(3 * y_size).astype(int), np.round(3 * x_size).astype(int))
# homography = np.array([[3., 0., 0.], [0., 3., 0.], [0., 0., 3.]])
# layer_names = ['lane', 'road_segment', 'drivable_area', 'road_divider', 'lane_divider', 'stop_line',
# 'ped_crossing', 'stop_line', 'ped_crossing', 'walkway']
# map_mask = (nusc_map.get_map_mask(patch_box, patch_angle, layer_names, canvas_size) * 255.0).astype(
# np.uint8)
# map_mask = np.swapaxes(map_mask, 1, 2) # x axis comes first
# # PEDESTRIANS
# map_mask_pedestrian = np.stack((map_mask[9], map_mask[8], np.max(map_mask[:3], axis=0)), axis=0)
#
# type_map['PEDESTRIAN'] = GeometricMap(data=map_mask_pedestrian, homography=homography, description=', '.join(layer_names))
# Notes: map_mask is a list of masks
# map_mask = []
# _line_geom_to_mask
# def mask_for_lines(...):
# map_mask = np.zeros(canvas_size, np.uint8)
# if layer_name is 'traffic_light':
# return None
# for line in layer_geom:
# new_line = line.intersection(patch)
# if not new_line.is_empty:
# new_line = affinity.affine_transform(new_line,
# [1.0, 0.0, 0.0, 1.0, trans_x, trans_y])
# new_line = affinity.scale(new_line, xfact=scale_width, yfact=scale_height, origin=(0, 0))
# map_mask = self.mask_for_lines(new_line, map_mask)

298
trap/video_sources.py
View file

@ -1,298 +0,0 @@
from dataclasses import dataclass
from itertools import cycle
import json
import logging
import math
from os import PathLike
from pathlib import Path
import time
from typing import Any, Generator, Iterable, List, Literal, Optional, Tuple
import neoapi
import cv2
import numpy as np
from trap.base import Camera, UrlOrPath
logger = logging.getLogger('video_source')
class VideoSource:
"""Video Frame generator
"""
def recv(self) -> Generator[Optional[cv2.typing.MatLike], Any, None]:
raise RuntimeError("Not implemented")
def __iter__(self):
for i in self.recv():
yield i
BinningValue = Literal[1, 2]
Coordinate = Tuple[int, int]
@dataclass
class GigEConfig:
identifier: Optional[str] = None
binning_h: BinningValue = 1
binning_v: BinningValue = 1
pixel_format: int = neoapi.PixelFormat_BayerRG8
# when changing these values, make sure you also tweak the calibration
width: int = 2448
height: int = 2048
# changing these _automatically changes calibration cx and cy_!!
offset_x: int = 0
offset_y: int = 0
post_crop_tl: Optional[Coordinate] = None
post_crop_br: Optional[Coordinate] = None
@classmethod
def from_file(cls, file: PathLike):
with open(file, 'r') as fp:
return cls(**json.load(fp))
class GigE(VideoSource):
def __init__(self, config=GigEConfig):
self.config = config
self.camera = neoapi.Cam()
# self.camera.Connect('-B127')
self.camera.Connect(self.config.identifier)
# Default buffer mode, streaming, always returns latest frame
self.camera.SetImageBufferCount(10)
# neoAPI docs: Setting the neoapi.Cam.SetImageBufferCycleCount()to one ensures that all buffers but one are given back to the neoAPI to be re-cycled and never given to the user by the neoapi.Cam.GetImage() method.
self.camera.SetImageBufferCycleCount(1)
self.setPixelFormat(self.config.pixel_format)
self.cam_is_configured = False
self.converter_settings = neoapi.ConverterSettings()
self.converter_settings.SetDebayerFormat('BGR8') # opencv
self.converter_settings.SetDemosaicingMethod(neoapi.ConverterSettings.Demosaicing_Baumer5x5)
# self.converter_settings.SetSharpeningMode(neoapi.ConverterSettings.Sharpening_Global)
# self.converter_settings.SetSharpeningMode(neoapi.ConverterSettings.Sharpening_Adaptive)
# self.converter_settings.SetSharpeningMode(neoapi.ConverterSettings.Sharpening_ActiveNoiseReduction)
self.converter_settings.SetSharpeningMode(neoapi.ConverterSettings.Sharpening_Off)
self.converter_settings.SetSharpeningFactor(1)
self.converter_settings.SetSharpeningSensitivityThreshold(2)
def configCam(self):
if self.camera.IsConnected():
self.setPixelFormat(self.config.pixel_format)
# self.camera.f.PixelFormat.Set(neoapi.PixelFormat_RGB8)
self.camera.f.BinningHorizontal.Set(self.config.binning_h)
self.camera.f.BinningVertical.Set(self.config.binning_v)
self.camera.f.Height.Set(self.config.height)
self.camera.f.Width.Set(self.config.width)
self.camera.f.OffsetX.Set(self.config.offset_x)
self.camera.f.OffsetY.Set(self.config.offset_y)
# print('exposure time', self.camera.f.ExposureAutoMaxValue.Set(20000)) # shutter 1/50 (hence; 1000000/shutter)
print('exposure time', self.camera.f.ExposureAutoMaxValue.Set(60000)) # otherwise it becomes too blurry in movements
print('brightness targt', self.camera.f.BrightnessAutoNominalValue.Get())
print('brightness targt', self.camera.f.BrightnessAutoNominalValue.Set(value=35))
# print('brightness targt', self.camera.f.Auto.Set(neoapi.BrightnessCorrection_On))
# print('brightness targt', self.camera.f.BrightnessCorrection.Set(neoapi.BrightnessCorrection_On))
# print('brightness targt', self.camera.f.BrightnessCorrection.Set(neoapi.BrightnessCorrection_On))
print('exposure time', self.camera.f.ExposureTime.Get())
print('LUTEnable', self.camera.f.LUTEnable.Get())
print('LUTEnable', self.camera.f.LUTEnable.Set(True))
# print('LUTEnable', self.camera.f.LUTEnable.Set(False))
print('Gamma', self.camera.f.Gamma.Set(0.45))
# neoapi.region
# self.camera.f.regeo
# print('LUT', self.camera.f.LUTIndex.Get())
# print('LUT', self.camera.f.LUTEnable.Get())
# print('exposure time max', self.camera.f.ExposureTimeGapMax.Get())
# print('exposure time min', self.camera.f.ExposureTimeGapMin.Get())
# self.pixfmt = self.camera.f.PixelFormat.Get()
self.cam_is_configured = True
def setPixelFormat(self, pixfmt):
self.pixfmt = pixfmt
self.camera.f.PixelFormat.Set(pixfmt)
# self.pixfmt = self.camera.f.PixelFormat.Get()
def recv(self):
while True:
# print('receive')
if not self.camera.IsConnected():
self.cam_is_configured = False
return
if not self.cam_is_configured:
self.configCam()
i = self.camera.GetImage(0)
if i.IsEmpty():
time.sleep(.01)
continue
# print(i.GetAvailablePixelFormats())
i = i.Convert(self.converter_settings)
if i.IsEmpty():
time.sleep(.01)
continue
img = i.GetNPArray()
# imgarray = i.GetNPArray()
# if self.pixfmt == neoapi.PixelFormat_BayerRG12:
# img = cv2.cvtColor(imgarray, cv2.COLOR_BayerRG2RGB)
# elif self.pixfmt == neoapi.PixelFormat_BayerRG8:
# img = cv2.cvtColor(imgarray, cv2.COLOR_BayerRG2RGB)
# else:
# img = cv2.cvtColor(imgarray, cv2.COLOR_BGR2RGB)
# if img.dtype == np.uint16:
# img = cv2.convertScaleAbs(img, alpha=(255.0/65535.0))
img = self._crop(img)
yield img
def _crop(self, img):
tl = self.config.post_crop_tl or (0,0)
br = self.config.post_crop_br or (img.shape[1], img.shape[0])
return img[tl[1]:br[1],tl[0]:br[0],:]
class SingleCvVideoSource(VideoSource):
def recv(self):
while True:
ret, img = self.video.read()
self.frame_idx+=1
# seek to 0 if video has finished. Infinite loop
if not ret:
# now loading multiple files
break
# frame = Frame(index=self.n, img=img, H=self.camera.H, camera=self.camera)
yield img
class RtspSource(SingleCvVideoSource):
def __init__(self, video_url: str | Path, camera: Camera = None):
# keep max 1 frame in app-buffer (0 = unlimited)
# When using gstreamer 1.28 drop=true is deprecated, use: leaky-type=2 which frame to drop: https://gstreamer.freedesktop.org/documentation/applib/gstappsrc.html?gi-language=c
gst = f"rtspsrc location={video_url} latency=0 buffer-mode=auto ! decodebin ! videoconvert ! appsink max-buffers=1 drop=true"
logger.info(f"Capture gstreamer (gst-launch-1.0): {gst}")
self.video = cv2.VideoCapture(gst, cv2.CAP_GSTREAMER)
self.frame_idx = 0
class FilelistSource(SingleCvVideoSource):
def __init__(self, video_sources: Iterable[UrlOrPath], camera: Camera = None, delay = True, offset = 0, end: Optional[int] = None, loop=False):
# store current position
self.video_sources = video_sources if not loop else cycle(video_sources)
self.camera = camera
self.video_path = None
self.video_nr = None
self.frame_count = None
self.frame_idx = None
self.n = 0
self.delay_generation = delay
self.offset = offset
self.end = end
def recv(self):
prev_time = time.time()
for video_nr, video_path in enumerate(self.video_sources):
self.video_path = video_path
self.video_nr = video_nr
logger.info(f"Play from '{str(video_path)}'")
video = cv2.VideoCapture(str(video_path))
fps = video.get(cv2.CAP_PROP_FPS)
target_frame_duration = 1./fps
self.frame_count = video.get(cv2.CAP_PROP_FRAME_COUNT)
if self.frame_count < 0:
self.frame_count = math.inf
self.frame_idx = 0
# TODO)) Video offset
if self.offset:
logger.info(f"Start at frame {self.offset}")
video.set(cv2.CAP_PROP_POS_FRAMES, self.offset)
self.frame_idx = self.offset
while True:
ret, img = video.read()
self.frame_idx+=1
self.n+=1
# seek to 0 if video has finished. Infinite loop
if not ret:
# now loading multiple files
break
if "DATASETS/hof/" in str(video_path):
# hack to mask out area
cv2.rectangle(img, (0,0), (800,200), (0,0,0), -1)
# frame = Frame(index=self.n, img=img, H=self.camera.H, camera=self.camera)
yield img
if self.end is not None and self.frame_idx >= self.end:
logger.info(f"Reached frame {self.end}")
break
if self.delay_generation:
# defer next loop
now = time.time()
time_diff = (now - prev_time)
if time_diff < target_frame_duration:
time.sleep(target_frame_duration - time_diff)
now += target_frame_duration - time_diff
prev_time = now
class CameraSource(SingleCvVideoSource):
def __init__(self, identifier: int, camera: Camera):
self.video = cv2.VideoCapture(identifier)
self.camera = camera
# TODO: make config variables
self.video.set(cv2.CAP_PROP_FRAME_WIDTH, int(self.camera.w))
self.video.set(cv2.CAP_PROP_FRAME_HEIGHT, int(self.camera.h))
# print("exposure!", video.get(cv2.CAP_PROP_AUTO_EXPOSURE))
self.video.set(cv2.CAP_PROP_FPS, self.camera.fps)
self.frame_idx = 0
def get_video_source(video_sources: List[UrlOrPath], camera: Optional[Camera] = None, frame_offset=0, frame_end:Optional[int]=None, loop=False):
if str(video_sources[0]).isdigit():
# numeric input is a CV camera
if frame_offset:
logger.info("video-offset ignored for camera source")
return CameraSource(int(str(video_sources[0])), camera)
elif video_sources[0].url.scheme == 'rtsp':
# video_sources[0].url.hostname
if frame_offset:
logger.info("video-offset ignored for rtsp source")
return RtspSource(video_sources[0])
elif video_sources[0].url.scheme == 'gige':
if frame_offset:
logger.info("video-offset ignored for gige source")
config = GigEConfig.from_file(Path(video_sources[0].url.netloc + video_sources[0].url.path))
return GigE(config)
else:
return FilelistSource(video_sources, offset = frame_offset, end=frame_end, loop=loop)
# os.environ["OPENCV_FFMPEG_CAPTURE_OPTIONS"] = "fflags;nobuffer|flags;low_delay|avioflags;direct|rtsp_transport;udp"
def get_video_source_from_str(video_sources: List[str]):
paths = [UrlOrPath(s) for s in video_sources]
return get_video_source(paths)

3213
uv.lock
View file

File diff suppressed because it is too large Load diff