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: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: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
44 changed files with 4408 additions and 24553 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.01,
"min_learning_rate": 1e-05,
"learning_decay_rate": 0.9999,
"prediction_horizon": 30,
"minimum_history_length": 5,
"maximum_history_length": 50,
"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": []
}

53
README.md
View file

@ -1,53 +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. Run the tracker, e.g. `uv run tracker --detector ultralytics --homography ../DATASETS/NAME/homography.json --video-src ../DATASETS/NAME/*.mp4 --calibration ../DATASETS/NAME/calibration.json --save-for-training EXPERIMENTS/raw/NAME/`
* Note: You can run this right of the camera stream: `uv run tracker --eval_device cuda:0 --detector ultralytics --video-src rtsp://USER:PW@ADDRESS/STREAM --homography ../DATASETS/NAME/homography.json --calibration ../DATASETS/NAME/calibration.json --save-for-training EXPERIMENTS/raw/NAME/`, each recording adding a new file to the `raw` folder.
4. Parse tracker data to Trajectron format: `uv run process_data --src-dir EXPERIMENTS/raw/NAME --dst-dir EXPERIMENTS/trajectron-data/ --name NAME` Optionally, smooth tracks: `--smooth-tracks`
* Optionally, add a map: ideally a RGB png: 3 layers of 0-255
* `uv run process_data --src-dir EXPERIMENTS/raw/NAME --dst-dir EXPERIMENTS/trajectron-data/ --name NAME --smooth-tracks --camera-fps 12 --homography ../DATASETS/NAME/homography.json --calibration ../DATASETS/NAME/calibration.json --filter-displacement 2 --map-img-path ../DATASETS/NAME/map.png`
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 `
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())"`

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

94
pyproject.toml
View file

@ -1,72 +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.0.15,<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",
[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"
trap_video_source = "trap.frame_emitter:FrameEmitter.parse_and_start"
trap_tracker = "trap.tracker:Tracker.parse_and_start"
trap_stage = "trap.stage:Stage.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
[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"}
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"

55
supervisord.conf
View file

@ -1,55 +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/homography.json --video-src gige://../DATASETS/hof3-cam-baumer/gige_config.json --calibration ../DATASETS/hof3-cam-baumer/calibration.json
directory=%(here)s
directory=%(here)s
[program:tracker]
command=uv run trap_tracker --smooth-tracks
directory=%(here)s
[program:stage]
command=uv run trap_stage
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
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
# during development auto restart some services when the code changes
[program:superfsmon]
command=superfsmon trap/stage.py stage
directory=%(here)s

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

832
test_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()

743
trap/base.py
View file

@ -1,743 +0,0 @@
from __future__ import annotations
from abc import ABC, abstractmethod
import argparse
from collections import defaultdict
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
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
@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.Lost:
return cls.Lost
# if track.time_since_update > 0:
if track.state == ByteTrackTrackState.Tracked:
return cls.Confirmed
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)
return cls.from_calibdata(data, H, fps)
@classmethod
def from_paths(cls, calibration_path, h_path, fps):
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)
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)
def undistort_img(self, img: MatLike):
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 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, 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.update_at = time.time()
def get_projected_history(self, H: Optional[cv2.Mat] = None, camera: Optional[DistortedCamera]= None) -> np.array:
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.array([])
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)
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,
)

117
trap/counter.py
View file

@ -1,117 +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):
self.iterations += 1
self.snapshot()
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)

415
trap/cv_renderer.py
View file

@ -1,415 +0,0 @@
# used for "Forward Referencing of type annotations"
from __future__ import annotations
import datetime
import logging
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
from trap.counter import CounterListerner
from trap.frame_emitter import Frame, Track
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] = {}
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):
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)
# 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:
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)
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 = frame.time
# img = frame.img
img = decorate_frame(frame, tracker_frame, prediction_frame, first_time, self.config, self.tracks, self.predictions, self.detections, self.config.render_clusters)
logger.debug(f"write frame {frame.time - first_time:.3f}s")
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, frame) == 1:
self.tracks.pop(track_id)
for prediction_id, track in list(self.predictions.items()):
if get_animation_position(track, 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)
return render_parser
# 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) -> np.array:
scale = 100
# 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 = frame.camera.points_img_to_world(points, scale)
points = [to_point(p) for p in points] # to int
cv2.rectangle(img, points[1], points[2], (255,255,0), 2)
cv2.circle(img, points[0], 5, (255,255,0), 2)
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), frame.camera, conversion)
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)
draw_track_predictions(img, track, int(track.track_id)+1, frame.camera, conversion, anim_position=anim_position, as_clusters=as_clusters)
cv2.putText(img, f"{len(track.predictor_history) if track.predictor_history else 'none'}", to_point(track.history[0].get_foot_coords()), cv2.FONT_HERSHEY_COMPLEX, 1, (255,255,255), 1)
if prediction_frame.maps:
for i, m in enumerate(prediction_frame.maps):
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()

298
trap/frame_emitter.py
View file

@ -1,131 +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
print(self.config.record)
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()

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], 100, 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)

135
trap/lines.py
View file

@ -1,135 +0,0 @@
from __future__ import annotations
from dataclasses import dataclass
from enum import Enum, IntEnum
import math
from typing import List, Tuple
import numpy as np
from simplification.cutil import simplify_coords_idx, simplify_coords_vw_idx
"""
See [notebook](../test_path_transforms.ipynb) for examples
"""
RenderablePosition = Tuple[float,float]
class CoordinateSpace(IntEnum):
CAMERA = 1
UNDISTORTED_CAMERA = 2
WORLD = 3
LASER = 4
@dataclass
class SrgbaColor():
red: float
green: float
blue: float
alpha: float
def with_alpha(self, alpha: float) -> SrgbaColor:
return SrgbaColor(self.red, self.green, self.blue, alpha)
def as_faded(self, alpha: float) -> SrgbaColor:
return SrgbaColor(self.red, self.green, self.blue, self.alpha * alpha)
@dataclass
class RenderablePoint():
position: RenderablePosition
color: SrgbaColor
def __post_init__(self):
if type(self.position) is np.ndarray:
# convert if wrong type, so it can be serialised
# print('convert')
self.position = tuple(self.position.tolist())
# self.position = (float(self.position[0]), float(self.position[0]))
# pass
@classmethod
def from_list(cls, l: List[float, float], color: SrgbaColor) -> RenderablePoint:
return cls([float(l[0]), float(l[1])], color)
SIMPLIFY_FACTOR_RDP = .001 # smaller is more detailed
SIMPLIFY_FACTOR_VW = 10
class SimplifyMethod(Enum):
RDP = 1 #' Ramer–Douglas–Peucker'
VW = 2 # Visvalingam-Whyatt
@dataclass
class RenderableLine():
points: List[RenderablePoint]
def as_simplified(self, method: SimplifyMethod = SimplifyMethod.RDP, factor = SIMPLIFY_FACTOR_RDP):
linestring = [p.position for p in self.points]
if method == SimplifyMethod.RDP:
indexes = simplify_coords_idx(linestring, factor)
elif method == SimplifyMethod.VW:
indexes = simplify_coords_vw_idx(linestring, factor)
points = [self.points[i] for i in indexes]
return RenderableLine(points)
@dataclass
class RenderableLines():
lines: List[RenderableLine]
space: CoordinateSpace = CoordinateSpace.WORLD
def as_simplified(self, method: SimplifyMethod = SimplifyMethod.RDP, factor = SIMPLIFY_FACTOR_RDP):
"""Wraps RenderableLine simplification, smaller factor is more detailed"""
return RenderableLines(
[line.as_simplified(method, factor) for line in self.lines]
)
def append(self, rl: RenderableLine):
self.lines.append(rl)
def append_lines(self, rls: RenderableLines):
self.lines.extend(rls.lines)
def point_count(self):
return sum([len(l.points) for l in self.lines])
# def merge(self, rl: RenderableLines):
def circle_arc(cx, cy, r, t, l, c: SrgbaColor):
"""
draw an cirlce arc, around point cx,cy, with radius r
for l*2pi, offset by t. Both t and l are 0<= [t,l] <= 1
"""
resolution = 30
steps = int(resolution * l)
offset = int(resolution * t)
pointlist: list[RenderablePoint] = []
for i in range(offset, offset+steps):
x = cx + math.cos(i * (2*math.pi)/resolution) * r
y = cy + math.sin(i * (2*math.pi)/resolution)* r
pointlist.append(RenderablePoint((x, y), c))
return RenderableLine(pointlist)
def cross_points(cx, cy, r, c: SrgbaColor):
# r = 100
steps = 3
pointlist: list[RenderablePoint] = []
for i in range(steps):
x = int(cx)
y = int(cy + r - i * 2 * r/steps)
pos = (x, y)
pointlist.append(RenderablePoint(pos, c))
path = RenderableLine(pointlist)
pointlist: list[RenderablePoint] = []
for i in range(steps):
y = int(cy)
x = int(cx + r - i * 2 * r/steps)
pos = (x, y)
pointlist.append(RenderablePoint(pos, c))
path2 = RenderableLine(pointlist)
return [path, path2]

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

144
trap/node.py
View file

@ -1,144 +0,0 @@
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 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.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.setup()
@classmethod
def _logger(cls):
return logging.getLogger(f"trap.{cls.__name__}")
def tick(self):
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 run_loop(self):
"""Use in run(), to check if it should keep looping
Takes care of tick()'ing the iterations/second counter
"""
self.tick()
return self.is_running.is_set()
def run_loop_capped_fps(self, max_fps: float):
"""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 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
)
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
@classmethod
def start(cls, config: Namespace, is_running: BaseEvent, timer_counter: Optional[Timer]):
instance = cls(config, is_running, timer_counter)
instance.run()
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]
)

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

418
trap/prediction_server.py
View file

@ -1,27 +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
import json
import traceback
import warnings
from argparse import ArgumentParser, Namespace
from multiprocessing import Event
import dill
import numpy as np
import pandas as pd
import torch
import zmq
from trajectron.environment import Environment, Scene
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.node import Node
from trap.tracker import Smoother
import zmq
from trap.frame_emitter import Frame
from trap.tracker import Track, Smoother
logger = logging.getLogger("trap.prediction")
@ -61,7 +67,7 @@ def create_online_env(env, hyperparams, scene_idx, init_timestep):
robot_type=env.robot_type)
def get_maps_for_input(input_dict, scene, hyperparams, device):
def get_maps_for_input(input_dict, scene, hyperparams):
scene_maps = list()
scene_pts = list()
heading_angles = list()
@ -75,18 +81,15 @@ def get_maps_for_input(input_dict, 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 = scene.map[node.type]
# map_point = x[-1, :2]
map_point = x[:2]
map_point = x[-1, :2]
patch_size = hyperparams['map_encoder'][node.type]['patch_size']
@ -101,66 +104,38 @@ def get_maps_for_input(input_dict, scene, hyperparams, device):
else:
heading_angles = torch.Tensor(heading_angles)
# print(scene_maps, patch_sizes, heading_angles)
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')
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://")
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)
@ -178,7 +153,6 @@ class PredictionServer(Node):
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
@ -196,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.
@ -211,14 +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)
# 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)
@ -234,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']])
@ -264,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}")
@ -293,55 +269,33 @@ class PredictionServer(Node):
# 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)
# 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)
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']])
# 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
)
# 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]])
input_dict[node] = np.array([x[-1],y[-1],vx[-1],vy[-1],ax[-1],ay[-1]])
# print(input_dict)
@ -351,16 +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
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
@ -379,16 +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))
)
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:
@ -400,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) w/ %d nodes and %d edges -- init: %.2f s" % (end - start,
1. / (end - start), len(trajectron.nodes),
trajectron.scene_graph.get_num_edges(), start-t_init))
# if self.config.center_data:
# 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
@ -432,14 +371,13 @@ class PredictionServer(Node):
response = {}
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('skip for no history')
continue
# response[node.id] = {
@ -450,184 +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, relative to model_dir",
type=str,
default='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=256)
inference_parser.add_argument('--k_eval',
help='how many samples to take during evaluation',
type=int,
default=25)
# 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="cpu")
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')
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):
@ -638,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()

353
trap/process_data.py
View file

@ -1,353 +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 numpy as np
import pandas as pd
import dill
import tqdm
import argparse
from typing import List, Optional
from trap.config import CameraAction, HomographyAction
from trap.frame_emitter import Camera
from trap.tracker import FinalDisplacementFilter, 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
@classmethod
def iteration_variations(cls, smooth = True, toggle_smooth=True, sample_step_size=1):
iterations: List[TrackIteration] = []
for i in range(sample_step_size):
iterations.append(TrackIteration(smooth, sample_step_size, i))
if toggle_smooth:
iterations.append(TrackIteration(not smooth, sample_step_size, i))
return iterations
# maybe_makedirs('trajectron-data')
# for desired_source in [ 'hof2', ]:# ,'hof-maskrcnn', 'hof-yolov8', 'VIRAT-0102-parsed', 'virat-resnet-keypoints-full']:
def process_data(src_dir: Path, dst_dir: Path, name: str, smooth_tracks: bool, cm_to_m: bool, center_data: bool, bin_positions: bool, camera: Camera, step_size: int, filter_displacement:float, map_img_path: Optional[Path]):
name += f"-nostep" if step_size == 1 else f"-step{step_size}"
name += f"-conv{smooth_window}" if smooth_tracks else f"-nosmooth"
name += f"-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}")
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
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
reader = TrackReader(src_dir, camera.fps)
tracks = [t for t in reader]
if filter_displacement > 0:
filter = FinalDisplacementFilter(filter_displacement)
tracks = filter.apply(tracks, camera)
total = len(tracks)
bar = tqdm.tqdm(total=total)
destinations = {
'train': int(total * .8),
'val': int(total * .12),
'test': int(total * .08),
}
max_track = reader.get(str(max([int(k) for k in reader._tracks.keys()])))
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 = {}
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})")
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')
# subpath = src_dir / data_class
# prev_src_file = None
# scene = None
scene_nodes = defaultdict(lambda: [])
variations = TrackIteration.iteration_variations(smooth_tracks, False, step_size)
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):
if iteration_settings.smooth:
track = smoother.smooth_track(interpolated_track)
# track = Smoother(smooth_window, False).smooth_track(track)
else:
track = interpolated_track # TODO)) Copy & move smooth outside iter loop
c = time.time()
if iteration_settings.step_size > 1:
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)
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):.2f} scene for data class {data_class}')
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)
# print(f"Linear: {l}")
# print(f"Non-Linear: {nl}")
print(f"error: {skipped_for_error}, used: {created}")
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("--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.cm_to_m,
args.center_data,
args.bin_positions,
args.camera,
args.step_size,
filter_displacement=args.filter_displacement,
map_img_path=args.map_img_path
)

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

11
trap/shapes.py
View file

File diff suppressed because one or more lines are too long

1085
trap/stage.py
View file

File diff suppressed because it is too large Load diff

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

731
trap/tools.py
View file

@ -1,731 +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

890
trap/tracker.py
View file

File diff suppressed because it is too large Load diff

201
trap/utils.py
View file

@ -1,201 +0,0 @@
# lerp & inverse lerp from https://gist.github.com/laundmo/b224b1f4c8ef6ca5fe47e132c8deab56
import linecache
import math
import os
from pathlib import Path
import tracemalloc
from typing import Iterable
import cv2
import numpy as np
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 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))
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 =
#scale 255
#alternatively: morph image to world space with a scale, as in trajectron/experiments/nuscenes/process_data.py
super().__init__(layers, homography_matrix, description)
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)

239
trap/video_sources.py
View file

@ -1,239 +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
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)
if self.camera.IsConnected():
# 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)
# print('exposure time', self.camera.f.ExposureAutoMaxValue.Set(20000)) # shutter 1/50
print('exposure time', self.camera.f.ExposureAutoMaxValue.Set(25000))
print('brightness targt', self.camera.f.BrightnessAutoNominalValue.Get())
print('brightness targt', self.camera.f.BrightnessAutoNominalValue.Set(30))
print('exposure time', self.camera.f.ExposureTime.Get())
print('Gamma', self.camera.f.Gamma.Set(0.39))
# 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()
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:
if not self.camera.IsConnected():
return
i = self.camera.GetImage(0)
if i.IsEmpty():
time.sleep(.01)
continue
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)

2915
uv.lock
View file

File diff suppressed because it is too large Load diff