predict position instead of velocity

.gitignore

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

EXPERIMENTS/config.json

@@ -0,0 +1,130 @@
+{
+    "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": []
+}

README.md

@@ -0,0 +1,26 @@
+# Trajectory Prediction Video installation
+
+## Install
+
+* Run `bash build_opencv_with_gstreamer.sh` to build opencv with gstreamer support
+* Use pyenv + poetry 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.
+
+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. `poetry 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: `poetry 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: `poetry 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
+        * `poetry 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 `poetry 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!
+    * On a video file (you can use a wildcard) `DISPLAY=:1 poetry 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`.

build_opencv_with_gstreamer.sh

@@ -0,0 +1,45 @@
+#!/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

custom_bytetrack.yaml

@@ -0,0 +1,11 @@
+# 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.0001 # threshold for the first association
+track_low_thresh: 0.0001 # threshold for the second association
+new_track_thresh: 0.0001 # threshold for init new track if the detection does not match any tracks
+track_buffer: 50 # buffer to calculate the time when to remove tracks
+match_thresh: 0.95 # 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)

pyproject.toml

@@ -7,12 +7,17 @@ readme = "README.md"
 
 [tool.poetry.scripts]
 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"
 
 [tool.poetry.dependencies]
 python = "^3.10,<3.12,"
 
 trajectron-plus-plus = { path = "../Trajectron-plus-plus/", develop = true }
+#trajectron-plus-plus = { git = "https://git.rubenvandeven.com/security_vision/Trajectron-plus-plus/" }
 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'" },
@@ -25,13 +30,20 @@ torchvision = [
        { 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"
+ultralytics = "^8.3"
 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"
+pyglet-cornerpin = "^0.3.0"
+opencv-python = {file="./opencv_python-4.10.0.84-cp310-cp310-linux_x86_64.whl"}
+setproctitle = "^1.3.3"
+bytetracker =  { git = "https://github.com/rubenvandeven/bytetrack-pip" }
+jsonlines = "^4.0.0"
+tensorboardx = "^2.6.2.2"
+shapely = "^1"
 
 [build-system]
 requires = ["poetry-core"]

test_homography.ipynb

@@ -2,7 +2,7 @@
  "cells": [
   {
    "cell_type": "code",
-   "execution_count": 1,
+   "execution_count": 3,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -15,7 +15,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 2,
+   "execution_count": 4,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -24,7 +24,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 3,
+   "execution_count": 5,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -42,13 +42,18 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 4,
+   "execution_count": 29,
    "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",
@@ -66,7 +71,7 @@
     "    H, status = cv2.findHomography(src,dst)\n",
     "    f, axes = plt.subplots(1, 2, figsize=(16,8))\n",
     "\n",
-    "    img = cv2.imread(str(img))\n",
+    "    img = frame\n",
     "    img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)\n",
     "\n",
     "    for i, p in enumerate(src):\n",
@@ -97,7 +102,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 5,
+   "execution_count": 7,
    "metadata": {},
    "outputs": [
     {
@@ -118,7 +123,7 @@
        "       [-2.89572527e-04,  1.97232411e-03,  1.00000000e+00]])"
       ]
      },
-     "execution_count": 5,
+     "execution_count": 7,
      "metadata": {},
      "output_type": "execute_result"
     }
@@ -141,7 +146,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 6,
+   "execution_count": 8,
    "metadata": {},
    "outputs": [
     {
@@ -201,7 +206,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 7,
+   "execution_count": 9,
    "metadata": {},
    "outputs": [
     {
@@ -240,7 +245,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 8,
+   "execution_count": 10,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -251,7 +256,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 9,
+   "execution_count": 11,
    "metadata": {},
    "outputs": [
     {
@@ -301,7 +306,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 10,
+   "execution_count": 12,
    "metadata": {},
    "outputs": [
     {
@@ -346,7 +351,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 22,
+   "execution_count": 13,
    "metadata": {},
    "outputs": [
     {
@@ -371,6 +376,615 @@
     "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": {

trap/animation_renderer.py

@@ -0,0 +1,566 @@
+# 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.canvas.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_shapes()
+
+        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_shapes(self):
+        '''
+        Due to error when running headless, we need to configure options before extending the shapes class
+        '''
+        class GradientLine(shapes.Line):
+            def __init__(self, x, y, x2, y2, width=1, color1=[255,255,255], color2=[255,255,255], batch=None, group=None):
+                # print('colors!', colors)
+                # assert len(colors) == 6
+                
+                r, g, b, *a = color1
+                self._rgba1 = (r, g, b, a[0] if a else 255)
+                r, g, b, *a = color2
+                self._rgba2 = (r, g, b, a[0] if a else 255)
+                
+                # print('rgba', self._rgba)
+
+                super().__init__(x, y, x2, y2, width, color1, batch=None, group=None)
+                # <pyglet.graphics.vertexdomain.VertexList
+                # pyglet.graphics.vertexdomain
+                # print(self._vertex_list)
+            
+            def _create_vertex_list(self):
+                '''
+                copy of super()._create_vertex_list but with additional colors'''
+                self._vertex_list = self._group.program.vertex_list(
+                    6, self._draw_mode, self._batch, self._group,
+                    position=('f', self._get_vertices()),
+                    colors=('Bn', self._rgba1+ self._rgba2 + self._rgba2 + self._rgba1 + self._rgba2 +self._rgba1 ),
+                    translation=('f', (self._x, self._y) * self._num_verts))
+            
+            def _update_colors(self):
+                self._vertex_list.colors[:] = self._rgba1+ self._rgba2 + self._rgba2 + self._rgba1 + self._rgba2 +self._rgba1
+            
+            def color1(self, color):
+                r, g, b, *a = color
+                self._rgba1 = (r, g, b, a[0] if a else 255)
+                self._update_colors()
+            
+            def color2(self, color):
+                r, g, b, *a = color
+                self._rgba2 = (r, g, b, a[0] if a else 255)
+                self._update_colors()
+                
+        self.gradientLine = GradientLine
+
+    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()

trap/config.py

@@ -1,10 +1,14 @@
 import argparse
 from pathlib import Path
 import types
+import numpy as np
+import json
 
-from trap.tracker import DETECTORS
+from trap.tracker import DETECTORS, TRACKER_BYTETRACK, TRACKERS
+from trap.frame_emitter import Camera
 
 from pyparsing import Optional
+from trap.frame_emitter import UrlOrPath
 
 class LambdaParser(argparse.ArgumentParser):
     """Execute lambda functions
@@ -49,6 +53,51 @@ frame_emitter_parser = parser.add_argument_group('Frame emitter')
 tracker_parser = parser.add_argument_group('Tracker')
 render_parser = parser.add_argument_group('Renderer')
 
+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:
+            camera = Camera.from_calibfile(Path(values), namespace.H, namespace.camera_fps)
+            # values = Path(values)
+            # with values.open('r') as fp:
+            #     data = json.load(fp)
+            #     # 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)
+
+
+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
@@ -166,16 +215,20 @@ inference_parser.add_argument('--num-samples',
                     default=5)
 inference_parser.add_argument("--full-dist",
                     help="Trajectron.incremental_forward parameter",
-                    type=bool,
+                    action='store_true')
-                    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",
-                    type=bool,
+                    action='store_true')
-                    default=False)
+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')
 
 
 # Internal connections.
@@ -200,6 +253,11 @@ connection_parser.add_argument('--zmq-frame-addr',
                     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")
+
 
 connection_parser.add_argument('--ws-port',
                     help='Port to listen for incomming websocket connections. Also serves the testing html-page.',
@@ -213,10 +271,10 @@ connection_parser.add_argument('--bypass-prediction',
 # Frame emitter
 
 frame_emitter_parser.add_argument("--video-src",
-                    help="source video to track from",
+                    help="source video to track from can be either a relative or absolute path, or a url, like an RTSP resource",
-                    type=Path,
+                    type=UrlOrPath,
                     nargs='+',
-                    default=lambda: list(Path('../DATASETS/VIRAT_subset_0102x/').glob('*.mp4')))
+                    default=lambda: [UrlOrPath(p) for p in 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,
@@ -231,10 +289,20 @@ frame_emitter_parser.add_argument("--video-loop",
 
 # 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')
+                    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_parser.add_argument("--save-for-training",
                     help="Specify the path in which to save",
                     type=Path,
@@ -243,11 +311,29 @@ 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",
@@ -255,6 +341,15 @@ 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-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')
@@ -269,3 +364,9 @@ 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,
+                           )

trap/cv_renderer.py

@@ -0,0 +1,558 @@
+# 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.frame_emitter import DetectionState, Frame, Track, Camera
+from trap.preview_renderer import FrameWriter
+from trap.tools import draw_track, draw_track_predictions, draw_track_projected, draw_trackjectron_history, to_point
+from trap.utils import convert_world_points_to_img_points, convert_world_space_to_img_space
+
+
+
+logger = logging.getLogger("trap.simple_renderer")
+
+class CvRenderer:
+    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.frame_sock = 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(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 = (self.config.camera.w,self.config.camera.h)
+        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
+
+        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.predictions: Dict[str, Track] = {}
+
+        
+        # self.init_shapes()
+
+        # self.init_labels()
+
+    
+    def init_shapes(self):
+        '''
+        Due to error when running headless, we need to configure options before extending the shapes class
+        '''
+        class GradientLine(shapes.Line):
+            def __init__(self, x, y, x2, y2, width=1, color1=[255,255,255], color2=[255,255,255], batch=None, group=None):
+                # print('colors!', colors)
+                # assert len(colors) == 6
+                
+                r, g, b, *a = color1
+                self._rgba1 = (r, g, b, a[0] if a else 255)
+                r, g, b, *a = color2
+                self._rgba2 = (r, g, b, a[0] if a else 255)
+                
+                # print('rgba', self._rgba)
+
+                super().__init__(x, y, x2, y2, width, color1, batch=None, group=None)
+                # <pyglet.graphics.vertexdomain.VertexList
+                # pyglet.graphics.vertexdomain
+                # print(self._vertex_list)
+            
+            def _create_vertex_list(self):
+                '''
+                copy of super()._create_vertex_list but with additional colors'''
+                self._vertex_list = self._group.program.vertex_list(
+                    6, self._draw_mode, self._batch, self._group,
+                    position=('f', self._get_vertices()),
+                    colors=('Bn', self._rgba1+ self._rgba2 + self._rgba2 + self._rgba1 + self._rgba2 +self._rgba1 ),
+                    translation=('f', (self._x, self._y) * self._num_verts))
+            
+            def _update_colors(self):
+                self._vertex_list.colors[:] = self._rgba1+ self._rgba2 + self._rgba2 + self._rgba1 + self._rgba2 +self._rgba1
+            
+            def color1(self, color):
+                r, g, b, *a = color
+                self._rgba1 = (r, g, b, a[0] if a else 255)
+                self._update_colors()
+            
+            def color2(self, color):
+                r, g, b, *a = color
+                self._rgba2 = (r, g, b, a[0] if a else 255)
+                self._update_colors()
+                
+        self.gradientLine = GradientLine
+
+    def init_labels(self):
+        base_color = (255,)*4
+        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.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"
+
+
+        # 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):
+        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 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
+    
+    def check_running(self, dt):
+        if not self.is_running.is_set():
+            self.window.close()
+            self.event_loop.exit()
+    
+    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()
+
+        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()
+        # self.batch_anim.draw()
+        self.batch_overlay.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.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 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, self.frame_size)
+
+        fourcc = cv2.VideoWriter_fourcc(*'vp09')
+
+        return cv2.VideoWriter(str(filename), fourcc, self.fps, self.frame_size)
+
+    def start_streaming(self):
+        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 run(self, timer_counter):
+        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", 1920, -1)
+        cv2.setWindowProperty("frame",cv2.WND_PROP_FULLSCREEN,cv2.WINDOW_FULLSCREEN)
+
+        while self.is_running.is_set():
+            i+=1
+            with timer_counter.get_lock():
+                timer_counter.value+=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')
+
+            if first_time is None:
+                    first_time = frame.time
+
+            img = decorate_frame(frame, tracker_frame, prediction_frame, first_time, self.config, self.tracks, self.predictions)
+
+            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',cv2.resize(img, (1920, 1080)))
+                # cv2.imshow('frame',img)
+                cv2.waitKey(1)
+
+            # 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')
+# 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)
+
+
+
+
+
+# Deprecated
+def decorate_frame(frame: Frame, tracker_frame: Frame, prediction_frame: Frame, first_time: float, config: Namespace, tracks: Dict[str, Track], predictions: Dict[str, Track]) -> np.array:
+    # TODO: replace opencv with QPainter to support alpha? https://doc.qt.io/qtforpython-5/PySide2/QtGui/QPainter.html#PySide2.QtGui.PySide2.QtGui.QPainter.drawImage
+    # or https://github.com/pygobject/pycairo?tab=readme-ov-file
+    # or https://pyglet.readthedocs.io/en/latest/programming_guide/shapes.html
+    # and use http://code.astraw.com/projects/motmot/pygarrayimage.html or https://gist.github.com/nkymut/1cb40ea6ae4de0cf9ded7332f1ca0d55
+    # or https://api.arcade.academy/en/stable/index.html (supports gradient color in line -- "Arcade is built on top of Pyglet and OpenGL.")
+    
+    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 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), config.camera, convert_world_points_to_img_points)
+    
+    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)
+            # draw_track(img, track, int(track_id))
+            draw_trackjectron_history(img, track, int(track.track_id), convert_world_points_to_img_points)
+            anim_position = get_animation_position(track, frame)
+            draw_track_predictions(img, track, int(track.track_id)+1, config.camera, convert_world_points_to_img_points, anim_position=anim_position, as_clusters=True)
+            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)
+
+    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)
+    
+    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)
+
+    return img
+
+    
+def run_cv_renderer(config: Namespace, is_running: BaseEvent, timer_counter):
+    renderer = CvRenderer(config, is_running)
+    renderer.run(timer_counter)

trap/frame_emitter.py

@@ -1,26 +1,87 @@
+from __future__ import annotations
+
 from argparse import Namespace
 from dataclasses import dataclass, field
+import dataclasses
 from enum import IntFlag
 from itertools import cycle
+import json
 import logging
 from multiprocessing import Event
 from pathlib import Path
 import pickle
 import sys
 import time
-from typing import Iterable, Optional
+from typing import Iterable, List, Optional
 import numpy as np
 import cv2
+import pandas as pd
 import zmq
+import os
 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
+from trajectron.environment import Environment, Node, Scene
+from urllib.parse import urlparse
+
+from trap.utils import get_bins
+from trap.utils import inv_lerp, lerp
 
 logger = logging.getLogger('trap.frame_emitter')
 
+class DataclassJSONEncoder(json.JSONEncoder):
+        def default(self, o):
+            if isinstance(o, np.ndarray):
+                return o.tolist()
+            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)
+
+
+class UrlOrPath():
+    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
+    
 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):
@@ -32,6 +93,78 @@ class DetectionState(IntFlag):
             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
+
+@dataclass
+class Camera:
+    mtx: cv2.Mat
+    dist: cv2.Mat
+    w: float
+    h: float
+    H: cv2.Mat # homography
+
+    newcameramtx: cv2.Mat = field(init=False)
+    roi: cv2.typing.Rect = field(init=False)
+    fps: float
+
+    def __post_init__(self):
+        self.newcameramtx, self.roi = cv2.getOptimalNewCameraMatrix(self.mtx, self.dist, (self.w,self.h), 1, (self.w,self.h))
+
+    @classmethod
+    def from_calibfile(cls, calibration_path, H, fps):
+        with calibration_path.open('r') as fp:
+            data = json.load(fp)
+            # print(data)
+            # print(data['camera_matrix'])
+        # camera = {
+        #     'camera_matrix': np.array(data['camera_matrix']), 
+        #     'dist_coeff': np.array(data['dist_coeff']),
+        # }
+        return cls(
+            np.array(data['camera_matrix']), 
+            np.array(data['dist_coeff']), 
+            data['dim']['width'], 
+            data['dim']['height'], 
+            H, fps)
+    
+    @classmethod
+    def from_paths(cls, calibration_path, h_path, fps):
+        H = H_from_path(h_path)
+        return cls.from_calibfile(calibration_path, H, fps)
+    
+    # def __init__(self, mtx, dist, w, h, H):
+    #     self.mtx = mtx
+    #     self.dist = dist
+    #     self.w = w
+    #     self.h = h
+    #     self.newcameramtx, self.roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w,h), 1, (w,h))
+    #     self.H = H # homography
+
+@dataclass
+class Position:
+    x: float
+    y: float
+    conf: float
+    state: DetectionState
+    frame_nr: int
+    det_class: str
 
 @dataclass
 class Detection:
@@ -43,13 +176,19 @@ class Detection:
     conf: float # object detector probablity
     state: DetectionState
     frame_nr: int
+    det_class: str
 
-    def get_foot_coords(self) -> list[tuple[float, float]]:
+    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):
+    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))
+        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:
@@ -62,7 +201,9 @@ class Detection:
             self.w*scale,
             self.h*scale,
             self.conf,
-            self.state)
+            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))
@@ -70,6 +211,16 @@ class Detection:
     def to_ltrb(self):
         return (int(self.l), int(self.t), int(self.l+self.w), int(self.t+self.h))
 
+@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
     
     
 @dataclass
@@ -79,22 +230,170 @@ class Track:
     and acceleration.
     """
     track_id: str = None
-    history: [Detection] = field(default_factory=lambda: [])
+    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
 
-    def get_projected_history(self, H) -> np.array:
+    def get_projected_history(self, H: Optional[cv2.Mat] = None, camera: Optional[Camera]= 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 = 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) -> dict:
+    def get_projected_history_as_dict(self, H, camera: Optional[Camera]= None) -> dict:
-        coords = self.get_projected_history(H)
+        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 Track(
+            self.track_id,
+            new_history,
+            self.predictor_history,
+            self.predictions,
+            self.fps)
+
+    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):
+        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)
+    
+    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_trajectron_node(self, camera: Camera, env: Environment) -> Node:
+        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)
+
+        new_first_idx = self.history[0].frame_nr
+
+        data_columns = pd.MultiIndex.from_product([['position', 'velocity', 'acceleration'], ['x', 'y']])
+        
+
+        # 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]
+            }
+
+        node_data = pd.DataFrame(data_dict, columns=data_columns)
+        
+        return Node(node_type=env.NodeType.PEDESTRIAN, node_id=self.track_id, data=node_data, first_timestep=new_first_idx)
         
 
     
@@ -106,6 +405,8 @@ class Frame:
     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
 
     def aslist(self) -> [dict]:
         return { t.track_id:
@@ -120,6 +421,16 @@ class Frame:
             } 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)
+
+def video_src_from_config(config) -> UrlOrPath:
+    if config.video_loop:
+        video_srcs: Iterable[UrlOrPath] = cycle(config.video_src)
+    else:
+        video_srcs: Iterable[UrlOrPath] = config.video_src
+    return video_srcs
+
 class FrameEmitter:
     '''
     Emit frame in a separate threat so they can be throttled,
@@ -135,28 +446,39 @@ class FrameEmitter:
         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)
 
+        self.frame_noimg_sock = context.socket(zmq.PUB)
+        self.frame_noimg_sock.setsockopt(zmq.CONFLATE, 1) # only keep latest frame. make sure to set BEFORE connect/bind
+        self.frame_noimg_sock.bind(config.zmq_frame_noimg_addr)
+        
         logger.info(f"Connection socket {config.zmq_frame_addr}")
+        logger.info(f"Connection socket {config.zmq_frame_noimg_addr}")
 
-        if self.config.video_loop:
+        self.video_srcs = video_src_from_config(self.config)
-            self.video_srcs: Iterable[Path] = cycle(self.config.video_src)
-        else:
-            self.video_srcs: [Path] = self.config.video_src
 
 
-    def emit_video(self):
+    def emit_video(self, timer_counter):
         i = 0
+        delay_generation = False
         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_WIDTH, int(self.config.camera.w))
-                video.set(cv2.CAP_PROP_FRAME_HEIGHT, int(720))
+                video.set(cv2.CAP_PROP_FRAME_HEIGHT, int(self.config.camera.h))
                 print("exposure!", video.get(cv2.CAP_PROP_AUTO_EXPOSURE))
                 video.set(cv2.CAP_PROP_FPS, 5)
+                fps=5
+            elif video_path.url.scheme == 'rtsp':
+                gst = f"rtspsrc location={video_path} latency=0 buffer-mode=auto ! decodebin ! videoconvert ! appsink max-buffers=0 drop=true"
+                logger.info(f"Capture gstreamer (gst-launch-1.0): {gst}")
+                video = cv2.VideoCapture(gst, cv2.CAP_GSTREAMER)
+                fps=12
             else:
+                # os.environ["OPENCV_FFMPEG_CAPTURE_OPTIONS"] = "fflags;nobuffer|flags;low_delay|avioflags;direct|rtsp_transport;udp"
                 video = cv2.VideoCapture(str(video_path))
+                delay_generation = True 
                 fps = video.get(cv2.CAP_PROP_FPS)
             target_frame_duration = 1./fps
             logger.info(f"Emit frames at {fps} fps")
@@ -167,22 +489,27 @@ class FrameEmitter:
                 i = self.config.video_offset
 
 
-            if '-' in video_path.stem:
+            # if '-' in video_path.path().stem:
-                path_stem = video_path.stem[:video_path.stem.rfind('-')]
+            #     path_stem = video_path.stem[:video_path.stem.rfind('-')]
-            else:
+            # else:
-                path_stem = video_path.stem
+            #     path_stem = video_path.stem
-            path_stem += "-homography"
+            # path_stem += "-homography"
-            homography_path = video_path.with_stem(path_stem).with_suffix('.txt')
+            # homography_path = video_path.with_stem(path_stem).with_suffix('.txt')
-            logger.info(f'check homography file {homography_path}')
+            # logger.info(f'check homography file {homography_path}')
-            if homography_path.exists():
+            # if homography_path.exists():
-                logger.info(f'Found custom homography file! Using {homography_path}')
+            #     logger.info(f'Found custom homography file! Using {homography_path}')
-                video_H = np.loadtxt(homography_path, delimiter=',')
+            #     video_H = np.loadtxt(homography_path, delimiter=',')
-            else:
+            # else:
-                video_H = None
+            #     video_H = None
+            video_H = self.config.camera.H
 
             prev_time = time.time()
             
+            
             while self.is_running.is_set():
+                with timer_counter.get_lock():
+                    timer_counter.value += 1
+
                 ret, img = video.read()
 
                 # seek to 0 if video has finished. Infinite loop
@@ -198,11 +525,15 @@ class FrameEmitter:
                     # 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)
+                frame = Frame(index=i, img=img, H=self.config.H, camera=self.config.camera)
                 # 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))
 
+                # only delay consuming the next frame when using a file.
+                # Otherwise, go ASAP
+                if delay_generation:
                     # defer next loop
                     now = time.time()
                     time_diff = (now - prev_time)
@@ -223,7 +554,7 @@ class FrameEmitter:
 
       
 
-def run_frame_emitter(config: Namespace, is_running: Event):
+def run_frame_emitter(config: Namespace, is_running: Event, timer_counter: int):
     router = FrameEmitter(config, is_running)
-    router.emit_video()
+    router.emit_video(timer_counter)
     is_running.clear()

trap/plumber.py

@@ -1,22 +1,28 @@
 import atexit
 import logging
-from logging.handlers import SocketHandler
+from logging.handlers import SocketHandler, QueueHandler, QueueListener
 from multiprocessing import Event, Process, Queue
 import multiprocessing
 import signal
 import sys
 import time
 from trap.config import parser
+from trap.cv_renderer import run_cv_renderer
 from trap.frame_emitter import run_frame_emitter
 from trap.prediction_server import run_prediction_server
-from trap.renderer import run_renderer
+from trap.preview_renderer import run_preview_renderer
+from trap.animation_renderer import run_animation_renderer
 from trap.socket_forwarder import run_ws_forwarder
+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):
@@ -31,10 +37,12 @@ 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()
-        except Exception as e:
+            print("finished ", self.name)
+        except BaseException as e:
             logger.critical(f"Exception in {self.name}")
             logger.exception(e)
             self._kwargs['is_running'].clear()
@@ -44,55 +52,112 @@ 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)
-        root_logger.addHandler(socket_handler)
+        socket_handler.setLevel(logging.NOTSET)
+        log_handlers.append(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')
 
     # instantiating process with arguments
     procs = [
-        ExceptionHandlingProcess(target=run_ws_forwarder, kwargs={'config': args, 'is_running': isRunning}, name='forwarder'),
+        # 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_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}, name='tracker'),
+        ExceptionHandlingProcess(target=run_tracker, kwargs={'config': args, 'is_running': isRunning, 'timer_counter': timer_tracker.iterations}, name='tracker'),
     ]
     
-    if args.render_file or args.render_url or args.render_window:
+    # 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')
         procs.append(
-            ExceptionHandlingProcess(target=run_renderer, kwargs={'config': args, 'is_running': isRunning}, name='renderer')
+            # 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 not args.bypass_prediction:
+        timer_predict = timers.new('predict')
         procs.append(
-            ExceptionHandlingProcess(target=run_prediction_server, kwargs={'config': args, 'is_running':isRunning}, name='inference'),
+            ExceptionHandlingProcess(target=run_prediction_server, kwargs={'config': args, 'is_running':isRunning, 'timer_counter': timer_predict.iterations}, name='inference'),
         )
         
+    def timer_process(timers: TimerCollection, is_running: Event):
+        while is_running.is_set():
+            time.sleep(1)
+            timers.snapshot()
+            print(timers.to_string())
+
+    procs.append(
+            ExceptionHandlingProcess(target=timer_process, kwargs={'is_running':isRunning, 'timers': timers}, name='timer'),
+        )
+    
+    try:
         logger.info("start")
         for proc in procs:
             proc.start()
 
+        # 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()
         
-    logger.info('Stop')
+        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()

trap/prediction_server.py

@@ -26,7 +26,7 @@ import matplotlib.pyplot as plt
 
 import zmq
 
-from trap.frame_emitter import Frame
+from trap.frame_emitter import DataclassJSONEncoder, Frame
 from trap.tracker import Track, Smoother
 
 logger = logging.getLogger("trap.prediction")
@@ -67,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):
+def get_maps_for_input(input_dict, scene, hyperparams, device):
     scene_maps = list()
     scene_pts = list()
     heading_angles = list()
@@ -81,15 +81,18 @@ def get_maps_for_input(input_dict, scene, hyperparams):
                 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]],
+                    # heading_angle = -np.arctan2(x[-1, heading_state_index[1]],
-                                                x[-1, heading_state_index[0]]) * 180 / np.pi
+                    #                             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
                 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[-1, :2]
+            map_point = x[:2]
 
             patch_size = hyperparams['map_encoder'][node.type]['patch_size']
 
@@ -104,15 +107,45 @@ def get_maps_for_input(input_dict, scene, hyperparams):
     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)
+                                                               rotation=heading_angles,
+                                                               device='cpu')
 
-    maps_dict = {node: maps[[i]] for i, node in enumerate(nodes_with_maps)}
+    maps_dict = {node: maps[[i]].to(device) 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]
+
+# 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:
     def __init__(self, config: Namespace, is_running: Event):
         self.config = config
@@ -122,7 +155,7 @@ class PredictionServer:
             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=4)
+            self.smoother = Smoother(window_len=12, convolution=True) # convolution seems fine for predictions
         
         context = zmq.Context()
         self.trajectory_socket: zmq.Socket = context.socket(zmq.SUB)
@@ -132,10 +165,18 @@ class PredictionServer:
         
         self.prediction_socket: zmq.Socket = context.socket(zmq.PUB)
         self.prediction_socket.bind(config.zmq_prediction_addr)
+        self.external_predictions = not self.config.zmq_prediction_addr.startswith("ipc://")
         # print(self.prediction_socket)
 
-    def run(self):
+    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)
 
+    def run(self, timer_counter):
+        print(self.config)
         if self.config.seed is not None:
             random.seed(self.config.seed)
             np.random.seed(self.config.seed)
@@ -153,6 +194,7 @@ class PredictionServer:
         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
@@ -171,18 +213,9 @@ class PredictionServer:
 
         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.
@@ -194,12 +227,14 @@ class PredictionServer:
         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)
 
@@ -218,6 +253,8 @@ class PredictionServer:
         prev_run_time = 0
         while self.is_running.is_set():
             timestep += 1
+            with timer_counter.get_lock():
+                timer_counter.value+=1
             
             # this_run_time = time.time()
             # logger.debug(f'test {prev_run_time - this_run_time}')
@@ -246,12 +283,16 @@ class PredictionServer:
             if self.config.predict_training_data:
                 input_dict = eval_scene.get_clipped_input_dict(timestep, hyperparams['state'])
             else:
+                # print('await', self.config.zmq_trajectory_addr)
                 zmq_ev = self.trajectory_socket.poll(timeout=2000)
                 if not zmq_ev:
                     # 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)
                 # trajectory_data = {t.track_id: t.get_projected_history_as_dict(frame.H) for t in frame.tracks.values()}
                 # trajectory_data = json.loads(data)
@@ -269,33 +310,55 @@ class PredictionServer:
 
                     # 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)
 
-                    data_dict = {('position', 'x'): x[:], # [-10:-1]
+                    if self.config.step_size > 1:
-                                ('position', 'y'): y[:], # [-10:-1]
+                        if (len(track.history) % self.config.step_size) != 0:
-                                ('velocity', 'x'): vx[:], # [-10:-1]
+                            # only add when having a new step
-                                ('velocity', 'y'): vy[:], # [-10:-1]
+                            continue
-                                ('acceleration', 'x'): ax[:], # [-10:-1]
+                        track = track.get_sampled(self.config.step_size)
-                                ('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)
+                    if len(track.history) < 2:
-                    node = Node(
+                        continue
-                        node_type=online_env.NodeType.PEDESTRIAN,
-                        node_id=identifier,
-                        data=node_data,
-                        first_timestep=timestep
-                        )
                     
-                    input_dict[node] = np.array([x[-1],y[-1],vx[-1],vy[-1],ax[-1],ay[-1]])
+                    node = track.to_trajectron_node(self.config.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']])
+
+                    # 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]])
 
                 # print(input_dict)
 
@@ -305,13 +368,16 @@ class PredictionServer:
                     # And want to update the network
 
                     # data = json.dumps({})
-                    self.prediction_socket.send_pyobj(frame)
+                    # TODO)) signal doing nothing
+                    # TODO)) And update the network 
+                    self.send_frame(frame)
                     
                     continue
 
             maps = None
             if hyperparams['use_map_encoding']:
-                maps = get_maps_for_input(input_dict, eval_scene, hyperparams)
+                maps = get_maps_for_input(input_dict, eval_scene, hyperparams, device=self.config.eval_device)
+                
             # print(maps)
 
             # robot_present_and_future = None
@@ -330,18 +396,15 @@ class PredictionServer:
                 # 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 model’s full sampled output, where z and y are sampled sequentially"
+                                                            full_dist=self.config.full_dist, # "The mol’des 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()
@@ -354,12 +417,27 @@ class PredictionServer:
             # 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({timestep: preds},
+            prediction_dict, histories_dict, futures_dict = prediction_output_to_trajectories({frame.index: 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
@@ -371,13 +449,14 @@ class PredictionServer:
 
             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] = {
@@ -388,24 +467,28 @@ class PredictionServer:
                 #     'predictions': predictions[0].tolist() # use batch 0
                 # }
 
-                frame.tracks[node.id].predictor_history = history.tolist()
+
+                frame.tracks[node.id].predictor_history = history.tolist() #node.data[:,{'position': ['x', 'y']}].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.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)")
+                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)")
             
             if self.config.smooth_predictions:
                 frame = self.smoother.smooth_frame_predictions(frame)
             
-            self.prediction_socket.send_pyobj(frame)
+            frame.maps = list([m.cpu().numpy() for m in maps.values()]) if maps else None
+
+            self.send_frame(frame)
+            
         logger.info('Stopping')
 
             
 
-def run_prediction_server(config: Namespace, is_running: Event):
+def run_prediction_server(config: Namespace, is_running: Event, timer_counter):
 
     # attempt to trace the warnings coming from pytorch
     # def warn_with_traceback(message, category, filename, lineno, file=None, line=None):
@@ -416,4 +499,4 @@ def run_prediction_server(config: Namespace, is_running: Event):
 
     # warnings.showwarning = warn_with_traceback
     s = PredictionServer(config, is_running)
-    s.run()
+    s.run(timer_counter)

trap/preview_renderer.py

@@ -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,15 +18,18 @@ import tempfile
 from pathlib import Path
 import shutil
 import math
+from typing import List, Optional
+
 
 from pyglet import shapes
 from PIL import Image
 
-from trap.frame_emitter import DetectionState, Frame, Track
+from trap.utils import convert_world_points_to_img_points
+from trap.frame_emitter import DetectionState, Frame, Track, Camera
 
 
 
-logger = logging.getLogger("trap.renderer")
+logger = logging.getLogger("trap.preview")
 
 class FrameAnimation:
     def __init__(self, frame: Frame):
@@ -55,61 +58,105 @@ def relativePointToPolar(origin, point) -> tuple[float, float]:
 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: Renderer, H):
+    def __init__(self, track_id, track: Track, renderer: PreviewRenderer, H, draw_projection = PROJECTION_IMG, camera: Optional[Camera] = None):
         # self.created_at = time.time()
+        self.draw_projection = draw_projection
         self.update_at = self.created_at = time.time()
         self.track_id = track_id
         self.renderer = renderer
-        self.set_track(track, H)
+        self.camera = camera
+        self.H = H # TODO)) Move H to Camera object
         self.drawn_positions = []
         self.drawn_predictions = []
+        self.drawn_pred_history = []
         self.shapes: list[pyglet.shapes.Line] = []
         self.pred_shapes: list[list[pyglet.shapes.Line]] = []
+        self.pred_history_shapes: list[pyglet.shapes.Line] = []
 
-    def set_track(self, track: Track, H):
+        self.set_track(track, H)
+        self.set_predictions(track, H)
+
+    def set_track(self, track: Track, H = None):
         self.update_at = time.time()
 
         self.track = track
-        self.H = H
+        # self.H = H 
-        self.coords = [d.get_foot_coords() for d in track.history]
+        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)
 
         # perhaps only do in constructor:
         self.inv_H = np.linalg.pinv(self.H)
 
+    def set_predictions(self, track: Track, H = None):
+
         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
+        
+        self.pred_track = track
         self.pred_coords = pred_coords
+        self.pred_history_coords = pred_history_coords
             # color = (128,0,128) if pred_i else (128,
         
         
-    def update_drawn_positions(self, dt) -> []:
+    def update_drawn_positions(self, dt) -> List:
         '''
         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)
+
+        # 1. track history
         for i, pos in enumerate(self.drawn_positions):
-            self.drawn_positions[i][0] = int(exponentialDecay(self.drawn_positions[i][0], self.coords[i][0], 16, dt))
+            self.drawn_positions[i][0] = self.coords[i][0]
-            self.drawn_positions[i][1] = int(exponentialDecay(self.drawn_positions[i][1], self.coords[i][1], 16, dt))
+            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)
 
         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))
+
+        if len(self.pred_history_coords) > len(self.drawn_pred_history):
+            self.drawn_pred_history.extend(self.coords[len(self.drawn_pred_history):])
+
+        # 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 = 6
+                    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(x), int(y)
+                    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))
 
@@ -120,54 +167,106 @@ class DrawnTrack:
         #         self.drawn_predictions.extend(self.pred_coords[len(self.drawn_predictions):])
 
         # self.drawn_positions = self.coords
+
+
+        # finally: update shapes from coordinates
         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)]
 
-        # for i, pos in self.drawn_positions.enumerate():
+        drawn_positions = convert_world_points_to_img_points(self.coords[:500]) # TODO)) Glitch in self.drawn_positions, now also capped
-        for ci in range(1, len(self.drawn_positions)):
+        drawn_pred_history = convert_world_points_to_img_points(self.drawn_pred_history)
-            x, y = [int(p) for p in self.drawn_positions[ci-1]]
+        drawn_predictions = [convert_world_points_to_img_points(p) for p in self.drawn_predictions]
-            x2, y2 = [int(p) for p in self.drawn_positions[ci]]
+        # positions = convert_world_points_to_img_points(self.drawn_predictions)
+
+        # 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(self.drawn_positions)]*3
+                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.opacity = 5
+                    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
-                line.opacity = int(exponentialDecay(line.opacity, 180, 3, dt))
+                    if not for_laser:
+                        line.opacity = int(exponentialDecay(line.opacity, 180, 8, dt))
 
         # TODO: basically a duplication of the above,  do this smarter?
         # TODO: add intermediate segment
         color = colorset[self.track_id % len(colorset)]
 
-        for a, drawn_predictions in enumerate(self.drawn_predictions):
+        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 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]]
+
+                y, y2 = self.renderer.window.height - y, self.renderer.window.height - y2
+
+                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)
+                    
+                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))
+    
+        if True:
+            for a, drawn_prediction in enumerate(drawn_predictions):
                 if len(self.pred_shapes) <= a:
                     self.pred_shapes.append([])
 
-            if len(self.pred_shapes[a]) > (len(drawn_predictions) +1):
+                if len(self.pred_shapes[a]) > (len(drawn_prediction) +1):
-                self.pred_shapes[a] = self.pred_shapes[a][:len(drawn_predictions)]
+                    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_predictions)):
+                for ci in range(0, len(drawn_prediction)):
                     if ci == 0:
-                    x, y = [int(p) for p in self.drawn_positions[-1]]
+                        continue
+                        # x, y = [int(p) for p in drawn_positions[-1]]
                     else:
-                    x, y = [int(p) for p in drawn_predictions[ci-1]]
+                        x, y = [int(p) for p in drawn_prediction[ci-1]]
 
-                x2, y2 = [int(p) for p in drawn_predictions[ci]]
+                    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]
@@ -175,7 +274,9 @@ class DrawnTrack:
 
                     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 = 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)
                         
@@ -185,11 +286,11 @@ class DrawnTrack:
                         line.x2, line.y2 = x2, y2
                         line.color = color
                         decay = (16/ci) if ci else 16
-                    half = len(drawn_predictions) / 2
+                        half = len(drawn_prediction) / 2
                         if ci < half:
-                        target_opacity = 180
+                            target_opacity = 60
                         else:
-                        target_opacity = (1 - ((ci - half) / half)) * 180
+                            target_opacity = (1 - ((ci - half) / half)) * 60
                         line.opacity = int(exponentialDecay(line.opacity, target_opacity, decay, dt))
         
 
@@ -232,7 +333,7 @@ class FrameWriter:
         
 
 
-class Renderer:
+class PreviewRenderer:
     def __init__(self, config: Namespace, is_running: BaseEvent):
         self.config = config
         self.is_running = is_running
@@ -241,7 +342,8 @@ class Renderer:
         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 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!!
@@ -253,14 +355,23 @@ class Renderer:
         self.frame_sock.setsockopt(zmq.SUBSCRIBE, b'')
         self.frame_sock.connect(config.zmq_frame_addr)
     
-        self.H = np.loadtxt(self.config.homography, delimiter=',')
+        
+        # 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.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 = (1280,720)
+        self.frame_size = (self.config.camera.w,self.config.camera.h)
         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
@@ -649,17 +760,27 @@ class Renderer:
             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),
+# colorset = [(0, 0, 0),
- (0, 0, 255),
+#  (0, 0, 255),
- (0, 255, 0),
+#  (0, 255, 0),
- (0, 255, 255),
+#  (0, 255, 255),
- (255, 0, 0),
+#  (255, 0, 0),
- (255, 0, 255),
+#  (255, 0, 255),
- (255, 255, 0)
+#  (255, 255, 0)
+#  ]
+colorset = [
+    (255,255,100),
+    (255,100,255),
+    (100,255,255),
  ]
+# colorset = [
+#     (0,0,0),
+#  ]
 
 # Deprecated
 def decorate_frame(frame: Frame, prediction_frame: Frame, first_time: float, config: Namespace) -> np.array:
@@ -772,6 +893,6 @@ def decorate_frame(frame: Frame, prediction_frame: Frame, first_time: float, con
     return img
 
     
-def run_renderer(config: Namespace, is_running: BaseEvent):
+def run_preview_renderer(config: Namespace, is_running: BaseEvent):
-    renderer = Renderer(config, is_running)
+    renderer = PreviewRenderer(config, is_running)
     renderer.run()

trap/process_data.py

@@ -0,0 +1,353 @@
+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
+    )
+

trap/timer.py

@@ -1,6 +1,9 @@
+import collections
+from re import A
 import time
 from multiprocessing.sharedctypes import RawValue, Value, Array
 from ctypes import c_double
+from typing import MutableSequence
 
 
 class Timer():
@@ -8,47 +11,58 @@ class Timer():
     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) -> None:
+    def __init__(self, name = 'timer') -> None:
-        self.last_tic = RawValue(c_double, -1)
+        self.name = name
-        self.last_toc = RawValue(c_double, -1)
+        self.tocs: MutableSequence[(float, int)] = collections.deque(maxlen=5)
-        self.fps = RawValue(c_double, -1)
+        self.iterations = Value('i', 0)
-        self.processing_duration = RawValue(c_double, -1)
         
-        self.smoothing = .1
+    # def tic(self):
+    #     now = time.time()
+    #     if self.last_tic is None:
             
-    def tic(self):
+    #         self.last_tic = now
-        now = time.time()
+    #         return
-        if self.last_tic is None:
         
-            self.last_tic = now
+    #     duration = now - self.last_tic
-            return
+    #     self.last_tic = now
 
-        duration = now - self.last_tic
+    #     current_fps = 1 / duration
-        self.last_tic = now
+    #     if not self.fps:
-
+    #         self.fps = current_fps
-        current_fps = 1 / duration
+    #     else:
-        if not self.fps:
+    #         self.fps = self.fps * (1-self.smoothing) + current_fps * self.smoothing
-            self.fps = current_fps
-        else:
-            self.fps = self.fps * (1-self.smoothing) + current_fps * self.smoothing
         
     def toc(self):
-        self.last_toc = time.time()
+        self.iterations += 1
-        duration = self.last_toc - self.last_tic
-
-        self.processing_duration = self.processing_duration * (1-self.smoothing) + duration * self.smoothing
     
+    def snapshot(self):
+        self.tocs.append((time.perf_counter(), self.iterations.value))
 
     @property
     def fps(self):
+        fpses = []
+        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 print(self)->str:
+    def snapshot(self):
-        print('Update', end='\r')
+        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
 
         

trap/tools.py

@@ -0,0 +1,697 @@
+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
+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, _yolov8_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 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
+    """
+    if not track.predictions:
+        return
+
+    current_point = track.get_projected_history(camera=camera)[-1]
+
+    opacity = 1-min(1, max(0, inv_lerp(0.8, 1, anim_position))) # fade out
+    slide_t = min(1, max(0, inv_lerp(0, 0.8, anim_position))) # slide_position
+
+
+    # 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])
+    
+    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)
+        
+    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))
+            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))
+        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]
+
+        # cv2.line(img, to_point(a), to_point(b), point_color, 1)
+        cv2.circle(img, to_point(b), 3, point_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

trap/tracker.py

@@ -4,17 +4,20 @@ import csv
 from dataclasses import dataclass, field
 import json
 import logging
+from math import nan
 from multiprocessing import Event
 from pathlib import Path
 import pickle
 import time
-from typing import Optional
+from typing import Dict, Optional, List
+import jsonlines
 import numpy as np
 import torch
+import torchvision
 import zmq
 import cv2
 
-from torchvision.models.detection import retinanet_resnet50_fpn_v2, RetinaNet_ResNet50_FPN_V2_Weights, keypointrcnn_resnet50_fpn, KeypointRCNN_ResNet50_FPN_Weights, maskrcnn_resnet50_fpn_v2, MaskRCNN_ResNet50_FPN_V2_Weights
+from torchvision.models.detection import retinanet_resnet50_fpn_v2, RetinaNet_ResNet50_FPN_V2_Weights, keypointrcnn_resnet50_fpn, KeypointRCNN_ResNet50_FPN_Weights, maskrcnn_resnet50_fpn_v2, MaskRCNN_ResNet50_FPN_V2_Weights, FasterRCNN_ResNet50_FPN_V2_Weights, fasterrcnn_resnet50_fpn_v2
 from deep_sort_realtime.deepsort_tracker import DeepSort
 from torchvision.models import ResNet50_Weights
 from deep_sort_realtime.deep_sort.track import Track as DeepsortTrack
@@ -22,11 +25,12 @@ from deep_sort_realtime.deep_sort.track import Track as DeepsortTrack
 from ultralytics import YOLO
 from ultralytics.engine.results import Results as YOLOResult
 
-from trap.frame_emitter import DetectionState, Frame, Detection, Track
+from trap.frame_emitter import Camera, DataclassJSONEncoder, DetectionState, Frame, Detection, Track
-
+from bytetracker import BYTETracker
 
 from tsmoothie.smoother import KalmanSmoother, ConvolutionSmoother
 import tsmoothie.smoother
+from datetime import datetime, timedelta
 
 # Detection = [int, int, int, int, float, int]
 # Detections = [Detection]
@@ -44,25 +48,340 @@ DETECTOR_MASKRCNN = 'maskrcnn'
 DETECTOR_FASTERRCNN = 'fasterrcnn'
 DETECTOR_YOLOv8 = 'ultralytics'
 
-DETECTORS = [DETECTOR_RETINANET, DETECTOR_MASKRCNN, DETECTOR_FASTERRCNN, DETECTOR_YOLOv8]
+TRACKER_DEEPSORT = 'deepsort'
+TRACKER_BYTETRACK = 'bytetrack'
 
+DETECTORS = [DETECTOR_RETINANET, DETECTOR_MASKRCNN, DETECTOR_FASTERRCNN, DETECTOR_YOLOv8]
+TRACKERS =[TRACKER_DEEPSORT, TRACKER_BYTETRACK]
+
+TRACKER_CONFIDENCE_MINIMUM = .2
+TRACKER_BYTETRACK_MINIMUM = .1 # bytetrack can track items iwth lower thershold
+NON_MAXIMUM_SUPRESSION = 1
+RCNN_SCALE = .4 # seems to have no impact on detections in the corners
+
+def _yolov8_track(frame: Frame, model: YOLO, **kwargs) -> List[Detection]:
+        
+        results: List[YOLOResult] = list(model.track(frame.img, persist=True, tracker="custom_bytetrack.yaml", verbose=False, conf=0.00001, **kwargs))
+        if results[0].boxes is None or results[0].boxes.id is None:
+            # work around https://github.com/ultralytics/ultralytics/issues/5968
+            return []
+        
+        boxes = results[0].boxes.xywh.cpu()
+        track_ids = results[0].boxes.id.int().cpu().tolist()
+        classes = results[0].boxes.cls.int().cpu().tolist()
+        return [Detection(track_id, bbox[0]-.5*bbox[2], bbox[1]-.5*bbox[3], bbox[2], bbox[3], 1, DetectionState.Confirmed, frame.index, class_id) for bbox, track_id, class_id in zip(boxes, track_ids, classes)]
+
+class Multifile():
+    def __init__(self, srcs: List[Path]):
+        self.srcs = srcs
+        self.g = self.__iter__()
+        self.current_file = None
+
+    @property
+    def name(self):
+        return ", ".join([s.name for s in self.srcs])
+    
+    def __iter__(self):
+        for path in self.srcs:
+            self.current_file = path
+            with path.open('r') as fp:
+                for l in fp:
+                    yield l
+    
+    def readline(self):
+        return self.g.__next__()
+
+FIELDNAMES = ['frame_id', 'track_id', 'l', 't', 'w', 'h', 'x', 'y', 'state', 'source']
+
+class TrackFilter:
+    pass
+
+    def apply(self, tracks: List[Track], camera: Camera):
+        return [t for t in tracks if self.filter(t, camera)]
+    
+    def apply_to_dict(self, tracks: Dict[str, Track], camera: Camera):
+        tracks = self.apply(tracks.values(), camera)
+        return {t.track_id: t for t in tracks}
+
+class FinalDisplacementFilter(TrackFilter):
+    def __init__(self, min_displacement):
+        self.min_displacement = min_displacement
+    
+    def filter(self, track: Track, camera: Camera):
+        history = track.get_projected_history(H=None, camera=camera)
+        displacement = np.linalg.norm(history[0]-history[-1])
+        return displacement > self.min_displacement
+
+class TrackReader:
+    def __init__(self, path: Path, fps: int, include_blacklisted = False, exclude_whitelisted = False):
+        self.blacklist_file = path / "blacklist.jsonl"
+        self.whitelist_file = path / "whitelist.jsonl" # for skipping
+        self.tracks_file = path / "tracks.pkl"
+
+        # with self.tracks_file.open('r') as fp:
+        #     tracks_dict: dict = json.load(fp)
+            
+        with self.tracks_file.open('rb') as fp:
+            tracks: dict = pickle.load(fp)
+
+        
+        if self.blacklist_file.exists():
+            with jsonlines.open(self.blacklist_file, 'r') as reader:
+                blacklist = [track_id for track_id in reader.iter(type=str)]
+        else:
+            blacklist = []
+            
+        
+        if self.whitelist_file.exists():
+            with jsonlines.open(self.whitelist_file, 'r') as reader:
+                whitelist = [track_id for track_id in reader.iter(type=str)]
+        else:
+            whitelist = []
+            
+
+        self._tracks = { track_id: detection_values
+            for track_id, detection_values in tracks.items()
+            if (include_blacklisted or track_id not in blacklist) and
+                (not exclude_whitelisted or track_id not in whitelist)
+            }
+        self.fps = fps
+    
+    def __len__(self):
+        return len(self._tracks)
+    
+    def get(self, track_id):
+        return self._tracks[track_id]
+        # detection_values = self._tracks[track_id]
+        # history = []
+        # # for detection_values in 
+        # source = None
+        # for detection_items in detection_values:
+        #     d = dict(zip(FIELDNAMES, detection_items))
+        #     history.append(Detection(
+        #         d['track_id'],
+        #         d['l'],
+        #         d['t'],
+        #         d['w'],
+        #         d['h'],
+        #         nan,
+        #         d['state'],
+        #         d['frame_id'],
+        #         1,
+        #     ))
+        #     source = int(d['source'])
+
+        # return Track(track_id, history, fps=self.fps, source=source)
+            
+    def __iter__(self):
+        for track_id in self._tracks:
+            yield self.get(track_id)
+
+def read_tracks_json(path: Path, fps):
+    """
+    Reader for tracks.json produced by TrainingDataWriter
+    """
+    reader = TrackReader(path, fps)
+    for t in reader:
+        yield t
+
+class TrainingDataWriter:
+    def __init__(self, training_path: Optional[Path]):
+        if training_path is None:
+            self.path = None
+            return
+        
+        if not isinstance(training_path, Path):
+            raise ValueError("save-for-training should be a path")
+        if not training_path.exists():
+            logger.info(f"Making path for training data: {training_path}")
+            training_path.mkdir(parents=True, exist_ok=False)
+        else:
+            logger.warning(f"Path for training-data exists: {training_path}. Continuing assuming that's ok.")
+            
+        # following https://github.com/StanfordASL/Trajectron-plus-plus/blob/master/experiments/pedestrians/process_data.py
+        
+        self.path = training_path
+
+    def __enter__(self):
+        if self.path:
+            d = datetime.now().isoformat(timespec="minutes")
+            self.training_fp = open(self.path / f'all-{d}.txt', 'w')
+            logger.debug(f"Writing tracker data to {self.training_fp.name}")
+            # following https://github.com/StanfordASL/Trajectron-plus-plus/blob/master/experiments/pedestrians/process_data.py
+            self.csv = csv.DictWriter(self.training_fp, fieldnames=FIELDNAMES, delimiter='\t', quoting=csv.QUOTE_NONE)
+            self.count = 0
+        return self
+    
+    def add(self, frame: Frame, tracks: List[Track]):
+        if not self.path:
+            # skip if disabled
+            return
+        
+        self.csv.writerows([{
+                    'frame_id': round(frame.index * 10., 1), # not really time
+                    'track_id': t.track_id,
+                    'l': float(t.history[-1].l), # to float, so we're sure it's not a torch.tensor()
+                    't': float(t.history[-1].t),
+                    'w': float(t.history[-1].w),
+                    'h': float(t.history[-1].h),
+                    'x': t.get_projected_history(frame.H, frame.camera)[-1][0],
+                    'y': t.get_projected_history(frame.H, frame.camera)[-1][1],
+                    'state': t.history[-1].state.value
+                    # only keep _actual_detections, no lost entries
+                    } for t in tracks
+                    # if t.history[-1].state != DetectionState.Lost
+                    ])
+        self.count += len(tracks)
+        
+    
+    def __exit__(self, exc_type, exc_value, exc_tb):
+        # ... ignore exception (type, value, traceback)
+        if not self.path:
+            return
+        
+        self.training_fp.close()
+        rewrite_raw_track_files(self.path)
+        
+                    
+
+def rewrite_raw_track_files(path: Path):
+    source_files = list(sorted(path.glob("*.txt"))) # we loop twice, so need a list instead of generator
+    total = 0
+    sources = Multifile(source_files)
+    for line in sources:
+        if len(line) > 3: # make sure not to count empty lines
+            total += 1 
+
+    
+    destinations = {
+        'train': int(total * .8),
+        'val': int(total * .12),
+        'test': int(total * .08),
+    }
+
+    logger.info(f"Splitting gathered data from {source_files}")
+    # for source_file in source_files:
+
+    tracks_file = path / 'tracks.json'
+    tracks_pkl = path / 'tracks.pkl'
+    tracks = defaultdict(lambda: Track())
+    
+    offset = 0
+    max_track_id = 0
+    prev_file = None
+
+    # all-2024-11-12T13:30.txt
+    file_date = None
+
+    src_file_nr = 0
+
+    for name, line_nrs in destinations.items():
+        dir_path = path / name
+        dir_path.mkdir(exist_ok=True)
+        file = dir_path / 'tracked.txt'
+        logger.debug(f"- Write {line_nrs} lines to {file}")
+        with file.open('w') as target_fp:
+            
+            for i in range(line_nrs):
+                line = sources.readline()
+                current_file = sources.current_file
+                if prev_file != current_file:
+                    offset: int = max_track_id
+                    
+                    logger.info(f'{name} - update offset {offset} ({sources.current_file})')
+                    prev_file = current_file
+                    src_file_nr += 1
+
+                    try:
+                        file_date = datetime.strptime(current_file.name, 'all-%Y-%m-%dT%H:%M.txt')
+                    except ValueError as e:
+                        logger.error(str(e))
+                        file_date = None
+
+
+                parts = line.split('\t')
+                track_id = int(parts[1]) + offset
+                
+                if file_date:
+                    frame_date = file_date + timedelta(seconds = int(float(parts[0]))//10)
+                else:
+                    frame_date = None
+                
+                if track_id > max_track_id:
+                    max_track_id = track_id
+                
+                track_id = str(track_id)
+                target_fp.write("\t".join(parts))
+                
+                parts = [float(p) for p in parts]
+                #  ['frame_id', 'track_id', 'l', 't', 'w', 'h', 'x', 'y', 'state', 'source']
+                
+                point = Detection(track_id, parts[2], parts[3], parts[4], parts[5], 1, DetectionState(int(parts[8])), int(parts[0]/10), 1)
+                # history = [
+                     
+                #     for d in parts]
+                tracks[track_id].track_id = track_id
+                tracks[track_id].source = src_file_nr
+                tracks[track_id].history.append(point)
+                # tracks[track_id].append([
+                #     int(parts[0] / 10),
+                #     track_id,
+                #     ] + parts[2:8] + [int(parts[8]), src_file_nr])
+    
+    with tracks_file.open('w') as fp:
+        logger.info(f"Write {len(tracks)} tracks to {str(tracks_file)}")
+        json.dump(tracks, fp, cls=DataclassJSONEncoder, indent=2)
+    with tracks_pkl.open('wb') as fp:
+        logger.info(f"Write {len(tracks)} tracks to {str(tracks_pkl)}")
+        pickle.dump(dict(tracks), fp)
+    
+
+
+class TrackerWrapper():
+    def __init__(self, tracker):
+        self.tracker = tracker
+
+    def track_detections():
+        raise RuntimeError("Not implemented")
+    
+    @classmethod
+    def init_type(cls, tracker_type: str):
+        if tracker_type == TRACKER_BYTETRACK:
+            return ByteTrackWrapper(BYTETracker(track_thresh=TRACKER_BYTETRACK_MINIMUM, match_thresh=TRACKER_CONFIDENCE_MINIMUM, frame_rate=12)) # TODO)) Framerate from emitter
+        else:
+            return DeepSortWrapper(DeepSort(n_init=5, max_age=30, nms_max_overlap=NON_MAXIMUM_SUPRESSION,
+                                        embedder='torchreid', embedder_wts="../MODELS/osnet_x1_0_imagenet.pth"
+                                        ))
+    
+class DeepSortWrapper(TrackerWrapper):
+    def track_detections(self, detections, img: cv2.Mat, frame_idx: int):
+        detections = Tracker.detect_persons_deepsort_wrapper(detections)
+        tracks: List[DeepsortTrack] = self.tracker.update_tracks(detections, frame=img)
+        active_tracks = [t for t in tracks if t.is_confirmed()]
+        return [Detection.from_deepsort(t, frame_idx) for t in active_tracks]
+        # raw_detections, embeds=None, frame=None, today=None, others=None, instance_masks=Non
+        
+    
+class ByteTrackWrapper(TrackerWrapper):
+    def __init__(self, tracker: BYTETracker):
+        self.tracker = tracker
+        
+    def track_detections(self, detections: tuple[list[float,float,float,float], float, float], img: cv2.Mat, frame_idx: int):
+        #  detections
+        if detections.shape[0] == 0:
+            detections = np.ndarray((0,0)) # needs to be 2-D
+
+        _ = self.tracker.update(detections)
+        active_tracks = [track for track in self.tracker.tracked_stracks if track.is_activated]
+        active_tracks = [track for track in active_tracks if track.start_frame < (self.tracker.frame_id - 5)]
+        return [Detection.from_bytetrack(track, frame_idx) for track in active_tracks]
 
         
 
 class Tracker:
-    def __init__(self, config: Namespace, is_running: Event):
+    def __init__(self, config: Namespace):
         self.config = config
-        self.is_running = is_running
 
-        context = zmq.Context()
-        self.frame_sock = 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(config.zmq_frame_addr)
-
-        self.trajectory_socket = context.socket(zmq.PUB)
-        self.trajectory_socket.setsockopt(zmq.CONFLATE, 1) # only keep latest frame
-        self.trajectory_socket.bind(config.zmq_trajectory_addr)
         
         # # TODO: config device
         self.device = torch.device("cuda" if torch.cuda.is_available() else "cpu")
@@ -73,72 +392,123 @@ class Tracker:
 
         logger.debug(f"Load tracker: {self.config.detector}")
 
+        conf = TRACKER_BYTETRACK_MINIMUM if self.config.tracker == TRACKER_BYTETRACK else TRACKER_CONFIDENCE_MINIMUM
         if self.config.detector == DETECTOR_RETINANET:
             # weights = RetinaNet_ResNet50_FPN_V2_Weights.DEFAULT
             # self.model = retinanet_resnet50_fpn_v2(weights=weights, score_thresh=0.2)
-            weights = KeypointRCNN_ResNet50_FPN_Weights.DEFAULT
+            weights = KeypointRCNN_ResNet50_FPN_Weights.COCO_V1
-            self.model = keypointrcnn_resnet50_fpn(weights=weights, box_score_thresh=0.35)
+            self.model = keypointrcnn_resnet50_fpn(weights=weights, box_score_thresh=conf)
             self.model.to(self.device)
             # Put the model in inference mode
             self.model.eval()
             # Get the transforms for the model's weights
             self.preprocess = weights.transforms().to(self.device)
-            self.mot_tracker = DeepSort(max_iou_distance=1, max_cosine_distance=0.5, max_age=15, nms_max_overlap=0.9,
+            self.mot_tracker = TrackerWrapper.init_type(self.config.tracker)
-                                    #   embedder='torchreid', embedder_wts="../MODELS/osnet_x1_0_imagenet.pth"
+        elif self.config.detector == DETECTOR_FASTERRCNN:
-                                    )
+            # weights = RetinaNet_ResNet50_FPN_V2_Weights.DEFAULT
+            # self.model = retinanet_resnet50_fpn_v2(weights=weights, score_thresh=0.2)
+            weights = FasterRCNN_ResNet50_FPN_V2_Weights.COCO_V1
+            self.model = fasterrcnn_resnet50_fpn_v2(weights=weights, box_score_thresh=conf)
+            self.model.to(self.device)
+            # Put the model in inference mode
+            self.model.eval()
+            # Get the transforms for the model's weights
+            self.preprocess = weights.transforms().to(self.device)
+            self.mot_tracker = TrackerWrapper.init_type(self.config.tracker)
         elif self.config.detector == DETECTOR_MASKRCNN:
             weights = MaskRCNN_ResNet50_FPN_V2_Weights.COCO_V1
-            self.model = maskrcnn_resnet50_fpn_v2(weights=weights, box_score_thresh=0.7)
+            self.model = maskrcnn_resnet50_fpn_v2(weights=weights, box_score_thresh=conf) # if we use ByteTrack we can work with low probablity!
             self.model.to(self.device)
             # Put the model in inference mode
             self.model.eval()
             # Get the transforms for the model's weights
             self.preprocess = weights.transforms().to(self.device)
-            self.mot_tracker = DeepSort(n_init=5, max_iou_distance=1, max_cosine_distance=0.5, max_age=15, nms_max_overlap=0.9,
+            # self.mot_tracker = DeepSort(n_init=5, max_iou_distance=1, max_cosine_distance=0.2, max_age=15, nms_max_overlap=0.9,
-                                    #   embedder='torchreid', embedder_wts="../MODELS/osnet_x1_0_imagenet.pth"
+            self.mot_tracker = TrackerWrapper.init_type(self.config.tracker)
-                                    )
         elif self.config.detector == DETECTOR_YOLOv8:
-            self.model = YOLO('EXPERIMENTS/yolov8x.pt')
+            # self.model = YOLO('EXPERIMENTS/yolov8x.pt')
+            self.model = YOLO('yolo11x.pt')
         else: 
             raise RuntimeError(f"{self.config.detector} is not implemented yet. See --help")
 
 
         # homography = list(source.glob('*img2world.txt'))[0]
 
-        self.H = np.loadtxt(self.config.homography, delimiter=',')
+        self.H = self.config.H
 
         if self.config.smooth_tracks:
             logger.info("Smoother enabled")
-            self.smoother = Smoother()
+            fps = 12 # TODO)) make configurable, or get from cam
+            self.smoother = Smoother(window_len=fps*5, convolution=False)
         else:
             logger.info("Smoother Disabled (enable with --smooth-tracks)")
 
 
         logger.debug("Set up tracker")
 
+    def track_frame(self, frame: Frame):
+        if self.config.detector == DETECTOR_YOLOv8:
+            detections: List[Detection]  = _yolov8_track(frame, self.model, classes=[0, 15, 16], imgsz=[1152, 640])
+        else :
+            detections: List[Detection] = self._resnet_track(frame, scale = RCNN_SCALE)
+        
+        for detection in detections:
+            track = self.tracks[detection.track_id]
+            track.track_id = detection.track_id # for new tracks
+            track.fps = self.config.camera.fps # for new tracks
+
+            track.history.append(detection) # add to history
+        
+        return detections
+        
+
+    def track(self,  is_running: Event, timer_counter: int = 0):
+        """
+        Live tracking of frames coming in over zmq
+        """
+
+        self.is_running = is_running
+
+
+        context = zmq.Context()
+        self.frame_sock = 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.trajectory_socket = context.socket(zmq.PUB)
+        self.trajectory_socket.setsockopt(zmq.CONFLATE, 1) # only keep latest frame
+        self.trajectory_socket.bind(self.config.zmq_trajectory_addr)
 
-    def track(self):
         prev_run_time = 0
 
-        training_fp = None
+        # training_fp = None
-        training_csv = None
+        # training_csv = None
-        training_frames = 0
+        # training_frames = 0
 
-        if self.config.save_for_training is not None:
+        # if self.config.save_for_training is not None:
-            if not isinstance(self.config.save_for_training, Path):
+        #     if not isinstance(self.config.save_for_training, Path):
-                raise ValueError("save-for-training should be a path")
+        #         raise ValueError("save-for-training should be a path")
-            if not self.config.save_for_training.exists():
+        #     if not self.config.save_for_training.exists():
-                logger.info(f"Making path for training data: {self.config.save_for_training}")
+        #         logger.info(f"Making path for training data: {self.config.save_for_training}")
-                self.config.save_for_training.mkdir(parents=True, exist_ok=False)
+        #         self.config.save_for_training.mkdir(parents=True, exist_ok=False)
-            else:
+        #     else:
-                logger.warning(f"Path for training-data exists: {self.config.save_for_training}. Continuing assuming that's ok.")
+        #         logger.warning(f"Path for training-data exists: {self.config.save_for_training}. Continuing assuming that's ok.")
-            training_fp = open(self.config.save_for_training / 'all.txt', 'w')
+        #     training_fp = open(self.config.save_for_training / 'all.txt', 'w')
-            # following https://github.com/StanfordASL/Trajectron-plus-plus/blob/master/experiments/pedestrians/process_data.py
+        #     # following https://github.com/StanfordASL/Trajectron-plus-plus/blob/master/experiments/pedestrians/process_data.py
-            training_csv = csv.DictWriter(training_fp, fieldnames=['frame_id', 'track_id', 'l', 't', 'w', 'h', 'x', 'y', 'state'], delimiter='\t', quoting=csv.QUOTE_NONE)
+        #     training_csv = csv.DictWriter(training_fp, fieldnames=['frame_id', 'track_id', 'l', 't', 'w', 'h', 'x', 'y', 'state'], delimiter='\t', quoting=csv.QUOTE_NONE)
 
         prev_frame_i = -1
         
+        with TrainingDataWriter(self.config.save_for_training) as writer:
+            end_time = None
+            tracker_dt = None
+            w_time = None
+            displacement_filter = FinalDisplacementFilter(.2)
             while self.is_running.is_set():
+                
+                with timer_counter.get_lock():
+                    timer_counter.value += 1
                 # this waiting for target_dt causes frame loss. E.g. with target_dt at .1, it
                 # skips exactly 1 frame on a 10 fps video (which, it obviously should not do)
                 # so for now, timing should move to emitter
@@ -147,9 +517,10 @@ class Tracker:
                 # time.sleep(max(0, prev_run_time - this_run_time + TARGET_DT))
                 # prev_run_time = time.time()
 
+                poll_time = time.time()
                 zmq_ev = self.frame_sock.poll(timeout=2000)
                 if not zmq_ev:
-                logger.warn('skip poll after 2000ms')
+                    logger.warning('skip poll after 2000ms')
                     # when there's no data after timeout, loop so that is_running is checked
                     continue
 
@@ -157,7 +528,10 @@ class Tracker:
                 frame: Frame = self.frame_sock.recv_pyobj() # frame delivery in current setup: 0.012-0.03s
 
                 if frame.index > (prev_frame_i+1):
-                logger.warn(f"Dropped {frame.index - prev_frame_i - 1} frames ({frame.index=}, {prev_frame_i=})")
+                    logger.warning(f"Dropped {frame.index - prev_frame_i - 1} frames ({frame.index=}, {prev_frame_i=}) -- poll time {start_time-poll_time:.5f}")
+                    if tracker_dt:
+                        logger.warning(f"last loop took {tracker_dt} (finished {start_time - end_time:0.5f} ago, writing took {w_time-end_time} and finshed {start_time - w_time} ago).. {writer.path}")
+
 
                 
                 prev_frame_i = frame.index
@@ -170,19 +544,16 @@ class Tracker:
                 # logger.info(f"Frame delivery delay = {time.time()-frame.time}s")
 
 
-            if self.config.detector == DETECTOR_YOLOv8:
+                detections: List[Detection] = self.track_frame(frame)
-                detections: [Detection]  = self._yolov8_track(frame)
-            else :
-                detections: [Detection] = self._resnet_track(frame.img, scale = 1)
-            
                     
                 # Store detections into tracklets
                 projected_coordinates = []
-            for detection in detections:
+                # now in track_frame()
-                track = self.tracks[detection.track_id]
+                # for detection in detections:
-                track.track_id = detection.track_id # for new tracks
+                #     track = self.tracks[detection.track_id]
+                #     track.track_id = detection.track_id # for new tracks
 
-                track.history.append(detection) # add to history
+                #     track.history.append(detection) # add to history
                     # projected_coordinates.append(track.get_projected_history(self.H)) # then get full history
                     
                     # TODO: hadle occlusions, and dissappearance
@@ -202,7 +573,12 @@ class Tracker:
                 #          "history": [{"x":c[0], "y":c[1]} for c in coords[0]] if not self.config.bypass_prediction else coords[0].tolist() # already doubles nested, fine for test
                 #      }
                 active_track_ids = [d.track_id for d in detections]
-            active_tracks = {t.track_id: t for t in self.tracks.values() if t.track_id in active_track_ids}
+                active_tracks = {t.track_id: t.get_with_interpolated_history() for t in self.tracks.values() if t.track_id in active_track_ids}
+                active_tracks =  displacement_filter.apply_to_dict(active_tracks, frame.camera)# a filter to remove just detecting static objects
+                
+
+                # active_tracks = {t.track_id: t for t in self.tracks.values() if t.track_id in active_track_ids}
+                # active_tracks = {t.track_id: t for t in self.tracks.values() if t.track_id in active_track_ids}
                 # logger.info(f"{trajectories}")
                 frame.tracks = active_tracks
 
@@ -213,72 +589,46 @@ class Tracker:
                 if self.config.smooth_tracks:
                     frame = self.smoother.smooth_frame_tracks(frame)
 
-            self.trajectory_socket.send_pyobj(frame)
+                # print(f"send to {self.trajectory_socket}, {self.config.zmq_trajectory_addr}")
+                self.trajectory_socket.send_pyobj(frame.without_img()) # ditch image for faster passthrough
                     
-            current_time = time.time()
+                end_time = time.time()
-            logger.debug(f"Trajectories: {len(active_tracks)}. Current frame delay = {current_time-frame.time}s (trajectories: {current_time - start_time}s)")
+                tracker_dt = end_time - start_time
+                
+
+                # having {end_time-frame.time} creates incidental delay... don't know why, maybe because of send?. So add n/a for now
+                # or is it {len(active_tracks)}     or  {tracker_dt}
+                # logger.debug(f"Trajectories: n/a. Current frame delay = n/a s (trajectories:s)")
                 
                 # self.trajectory_socket.send_string(json.dumps(trajectories))
                 # provide a {ID: {id: ID, history: [[x,y],[x,y],...]}}
                 # TODO: provide a track object that actually keeps history (unlike tracker)
 
-            #TODO calculate fps (also for other loops to see asynchonity)
+                # TODO calculate fps (also for other loops to see asynchonity)
                 # fpsfilter=fpsfilter*.9+(1/dt)*.1    #trust value in order to stabilize fps display
-            if training_csv:
+                
-                training_csv.writerows([{
+                writer.add(frame, active_tracks.values())
-                    'frame_id': round(frame.index * 10., 1), # not really time
+                
-                    'track_id': t.track_id,
+                w_time = time.time()
-                    'l': t.history[-1].l,
+                
-                    't': t.history[-1].t,
+                
-                    'w': t.history[-1].w,
-                    'h': t.history[-1].h,
-                    'x': t.get_projected_history(frame.H)[-1][0],
-                    'y': t.get_projected_history(frame.H)[-1][1],
-                    'state': t.history[-1].state.value
-                    # only keep _actual_detections, no lost entries
-                    } for t in active_tracks.values() 
-                    # if t.history[-1].state != DetectionState.Lost
-                    ])
-                training_frames += len(active_tracks)
-            # print(time.time() - start_time)
                 
                 
-        if training_fp:
-            training_fp.close()
-            lines = {
-                'train': int(training_frames * .8),
-                'val': int(training_frames * .12),
-                'test': int(training_frames * .08),
-            }
-            logger.info(f"Splitting gathered data from {training_fp.name}")
-            with open(training_fp.name, 'r') as source_fp:
-                for name, line_nrs in lines.items():
-                    dir_path = self.config.save_for_training / name
-                    dir_path.mkdir(exist_ok=True)
-                    file = dir_path / 'tracked.txt'
-                    logger.debug(f"- Write {line_nrs} lines to {file}")
-                    with file.open('w') as target_fp:
-                        for i in range(line_nrs):
-                            target_fp.write(source_fp.readline())
             
         logger.info('Stopping')
 
-    def _yolov8_track(self, frame: Frame,) -> [Detection]:
-        results: [YOLOResult] = self.model.track(frame.img, persist=True, tracker="bytetrack.yaml", verbose=False)
-        if results[0].boxes is None or results[0].boxes.id is None:
-            # work around https://github.com/ultralytics/ultralytics/issues/5968
-            return []
-        return [Detection(track_id, bbox[0]-.5*bbox[2], bbox[1]-.5*bbox[3], bbox[2], bbox[3], 1, DetectionState.Confirmed, frame.index) for bbox, track_id in zip(results[0].boxes.xywh.cpu(), results[0].boxes.id.int().cpu().tolist())]
     
-    def _resnet_track(self, img, scale: float = 1) -> [Detection]:
+    def _resnet_track(self, frame: Frame, scale: float = 1) -> List[Detection]:
+        img = frame.img
         if scale != 1:
             dsize = (int(img.shape[1] * scale), int(img.shape[0] * scale))
             img = cv2.resize(img, dsize)
         detections = self._resnet_detect_persons(img)
-        tracks: [DeepsortTrack] = self.mot_tracker.update_tracks(detections, frame=img)
+        tracks: List[Detection] = self.mot_tracker.track_detections(detections, img, frame.index)
-        return [Detection.from_deepsort(t).get_scaled(1/scale) for t in tracks]
+        # active_tracks = [t for t in tracks if t.is_confirmed()]
+        return [d.get_scaled(1/scale) for d in tracks]
 
-    def _resnet_detect_persons(self, frame) -> [Detection]:
+    def _resnet_detect_persons(self, frame) -> List[Detection]:
         t = torch.from_numpy(cv2.cvtColor(frame, cv2.COLOR_BGR2RGB))
         # change axes of image loaded image to be compatilbe with torch.io.read_image (which has C,W,H format instead of W,H,C)
         t = t.permute(2, 0, 1)
@@ -293,8 +643,19 @@ class Tracker:
         mask = prediction['labels'] == 1 # if we want more than one label: np.isin(prediction['labels'], [1,86])
 
         scores = prediction['scores'][mask]
+        # print(scores, prediction['labels'])
         labels = prediction['labels'][mask]
         boxes = prediction['boxes'][mask]
+        # print(prediction['scores'])
+
+        if NON_MAXIMUM_SUPRESSION < 1:
+            nms_mask = torch.zeros(scores.shape[0]).bool()
+            nms_keep_ids = torchvision.ops.nms(boxes, scores, NON_MAXIMUM_SUPRESSION)
+            nms_mask[nms_keep_ids] = True
+            print(scores.shape[0], nms_keep_ids, nms_mask)
+            scores = scores[nms_mask]
+            labels = labels[nms_mask]
+            boxes = boxes[nms_mask]
         
         # TODO: introduce confidence and NMS supression: https://github.com/cfotache/pytorch_objectdetecttrack/blob/master/PyTorch_Object_Tracking.ipynb
         # (which I _think_ we better do after filtering)
@@ -302,7 +663,7 @@ class Tracker:
 
         #  dets - a numpy array of detections in the format [[x1,y1,x2,y2,score, label],[x1,y1,x2,y2,score, label],...]
         detections = np.array([np.append(bbox, [score, label]) for bbox, score, label in zip(boxes.cpu(), scores.cpu(), labels.cpu())])
-        detections = self.detect_persons_deepsort_wrapper(detections)
+        
         
         return detections
     
@@ -315,21 +676,30 @@ class Tracker:
         return [([d[0], d[1], d[2]-d[0], d[3]-d[1]], d[4], d[5]) for d in detections]
 
 
-def run_tracker(config: Namespace, is_running: Event):
+def run_tracker(config: Namespace, is_running: Event, timer_counter):
-    router = Tracker(config, is_running)
+    router = Tracker(config)
-    router.track()
+    router.track(is_running, timer_counter)
 
 
 
 class Smoother:
 
-    def __init__(self, window_len=2):
+    def __init__(self, window_len=6, convolution=False):
-        self.smoother = ConvolutionSmoother(window_len=window_len, window_type='ones', copy=None)
+        # for some reason this smoother messes the predictions. Probably skews the points too much??
+        if convolution:
+            self.smoother = ConvolutionSmoother(window_len=window_len, window_type='hanning', copy=None)
+        else:
+            # "Unlike Kalman filtering, which focuses on predicting and updating the current state using historical measurements, Kalman smoothing enhances the accuracy of past state values"
+            # see https://medium.com/@shahalkp1/kalman-smoothing-using-tsmoothie-0175260464e5
+            self.smoother = KalmanSmoother(component='level_trend',  component_noise={'level':0.02, 'season': .01, 'trend':0.02},n_seasons = 2, copy=None)
             
 
-    def smooth_frame_tracks(self, frame: Frame) -> Frame:
+    
-        new_tracks = []
+    def smooth(self, points: List[float]):
-        for track in frame.tracks.values():
+        self.smoother.smooth(points)
+        return self.smoother.smooth_data[0]
+    
+    def smooth_track(self, track: Track) -> Track:
         ls = [d.l for d in track.history]
         ts = [d.t for d in track.history]
         ws = [d.w for d in track.history]
@@ -342,8 +712,13 @@ class Smoother:
         ws = self.smoother.smooth_data[0]
         self.smoother.smooth(hs)
         hs = self.smoother.smooth_data[0]
-            new_history = [Detection(d.track_id, l, t, w, h, d.conf, d.state, d.frame_nr) for l, t, w, h, d in zip(ls,ts,ws,hs, track.history)]
+        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)
+        return Track(track.track_id, new_history, track.predictor_history, track.predictions, track.fps)
+    
+    def smooth_frame_tracks(self, frame: Frame) -> Frame:
+        new_tracks = []
+        for track in frame.tracks.values():
+            new_track = self.smooth_track(track)
             new_tracks.append(new_track)
         frame.tracks = {t.track_id: t for t in new_tracks}
         return frame

trap/utils.py

@@ -0,0 +1,175 @@
+# lerp & inverse lerp from https://gist.github.com/laundmo/b224b1f4c8ef6ca5fe47e132c8deab56
+import linecache
+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 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):
+    """Transform the given matrix so that it immediately converts
+    the points to img space"""
+    new_H = H.copy()
+    new_H[:2] = H[:2] * 100
+    return new_H
+
+def convert_world_points_to_img_points(points: Iterable):
+    """Transform the given matrix so that it immediately converts
+    the points to img space"""
+    if isinstance(points, np.ndarray):
+        return np.array(points) * 100
+    return [[p[0]*100, p[1]*100] 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)