Compare commits
10 Commits
5f78115429
...
14fbfec62e
| Author | SHA1 | Date | |
|---|---|---|---|
| 14fbfec62e | |||
| 11962cdc34 | |||
| ae5bf0c208 | |||
| 7f8e5d0975 | |||
| 35190562e5 | |||
| b956d071ea | |||
| 0beb36a1af | |||
| 1adb1c30f0 | |||
| 514874e2f4 | |||
| 0d5d8ff70d |
@@ -43,3 +43,6 @@ __pycache__/
|
|||||||
# Legacy / scratch folders
|
# Legacy / scratch folders
|
||||||
/old_orisys/
|
/old_orisys/
|
||||||
/tools/calibration/
|
/tools/calibration/
|
||||||
|
|
||||||
|
# Scratch output of examples/ (multi_stream.py's default --out-dir)
|
||||||
|
/outputs/
|
||||||
+118
@@ -0,0 +1,118 @@
|
|||||||
|
# 滤波:三级、预设与可追溯性
|
||||||
|
|
||||||
|
`json.load` 不支持注释(见 `src/orisys/configs.py`),所以"建议的滤波"**不写在 JSON 注释里**,
|
||||||
|
而是写成**代码里的预设**(`src/orisys/sensor.py::FILTER_PRESETS`)+ 这份文档 +
|
||||||
|
`examples/filters/` 里可直接复制的配置。预设是可执行、可回显、可审计的;注释不是。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 1. 三级滤波,位置不同、性质不同
|
||||||
|
|
||||||
|
| 级 | 配置键 | 作用位置 | 性质 |
|
||||||
|
|---|---|---|---|
|
||||||
|
| 光流时间 EMA | `flow_ema_alpha` | `compute_flow()` 内,**HHD 之前** | **改变测量**:改 `vnormal`/接触统计与 `_force_raw` 的值。两个 `alpha` 不同的录制**不可比** |
|
||||||
|
| 力中值 | `force_median_window` | `_apply_force_filter()`:raw → 中值 → 低通 → 标定 | **读数滤波**:删除单点尖峰(因果滑动中值) |
|
||||||
|
| 力低通 | `force_lowpass_cutoff_hz` / `_order` / `_sample_rate_hz` | 同上(中值之后) | **读数滤波**:因果 Butterworth IIR(`sosfilt` + 持久状态) |
|
||||||
|
|
||||||
|
因此 `filters_state()` 把它们分成两组:`measurement`(测量身份)与 `readout`(读数滤波)。
|
||||||
|
标定/可比性检查应比对 `measurement`,记录里两者都记。
|
||||||
|
|
||||||
|
## 2. 取值规则(自描述,无隐藏覆盖)
|
||||||
|
|
||||||
|
- **每个级由自己的值开关**:`flow_ema_alpha >= 1`、`force_median_window <= 1`、
|
||||||
|
`force_lowpass_cutoff_hz <= 0` 都是"关"。
|
||||||
|
- **配置没写的键 = 关**:缺省不是"我们觉得合理的那档",而是"不滤波"。因此**没有 `filters`
|
||||||
|
段的配置文件什么都不滤**,出厂 bundle 也把三个值显式写成全关 —— 想让哪一级跑起来,
|
||||||
|
必须在文件里写出来(数值或 `preset`)。这样"文件说的"和"设备跑的"才总是一致。
|
||||||
|
- **非法值 → 关**,绝不回落到"默认开"。
|
||||||
|
- `filters.enabled` 是**废弃别名**,显式 `false` 表示"三级全关";没有任何开关能覆盖已写的值。
|
||||||
|
- `filters.preset` 可引用下面的预设;**显式键优先于预设值**(可从预设出发只改一个旋钮)。
|
||||||
|
- 任何时刻的**真值**是 `Sensor.filters_state()`,不是配置文件 —— 运行时 `configure()` 可以改它。
|
||||||
|
|
||||||
|
## 3. 预设(`FILTER_PRESETS`)
|
||||||
|
|
||||||
|
| 预设 | `flow_ema_alpha` | `force_median_window` | `force_lowpass_cutoff_hz` | 延迟 @30fps | 用途 |
|
||||||
|
|---|---|---|---|---|---|
|
||||||
|
| `off` | 1.0 | 1 | 0.0 | 0 帧 | 原始读数;文件说什么就跑什么 |
|
||||||
|
| `low_latency` | 1.0 | 3 | 0.0 | ≈1 帧(33 ms) | 延迟敏感,只去尖峰 |
|
||||||
|
| `balanced` | 0.5 | 3 | 0.0 | ≈2 帧(67 ms) | 建议起步档:EMA 干掉力级滤波够不到的光流抖动 |
|
||||||
|
| `low_noise` | 0.3 | 3 | 10.0 | ≈3 帧(100 ms) | 底噪优先,读数再低通一道 |
|
||||||
|
|
||||||
|
延迟换算 @60fps 减半;数字来自 `_perf/`(对照表见 `orisys-sdk-perf-plan.md`)。
|
||||||
|
≥1 帧的延迟与相位差是**因果滤波的代价**:旧显示端曾用零相位 `filtfilt`,那需要"未来",
|
||||||
|
无法在采集里做,也让"显示 == 记录"不可能——所以现在只有一条因果链。
|
||||||
|
|
||||||
|
## 4. 怎么用
|
||||||
|
|
||||||
|
```jsonc
|
||||||
|
// sensors/mini/sensor.json(片段;完整可复制版本见 examples/filters/)
|
||||||
|
"filters": { "preset": "balanced" }
|
||||||
|
```
|
||||||
|
```python
|
||||||
|
# 或运行时(UI 的齿轮就走这条路;不重连相机)
|
||||||
|
sensor.configure(flow_ema_alpha=0.5, force_median_window=3, lowpass_cutoff_hz=0.0)
|
||||||
|
print(sensor.filters_state()) # 实际生效值 + preset + applied 标志
|
||||||
|
```
|
||||||
|
UI 想列出可选档位:`from orisys.sensor import available_filter_presets`。
|
||||||
|
|
||||||
|
## 5. 两个 buffer:滤波前 / 滤波后
|
||||||
|
|
||||||
|
```python
|
||||||
|
t = sensor.force_triplets()
|
||||||
|
t["filtered"]["raw"], t["filtered"]["newton"] # 记录与显示用的值(同一套)
|
||||||
|
t["unfiltered"]["raw"], t["unfiltered"]["newton"] # 中值/低通之前;同帧、同标定系数
|
||||||
|
```
|
||||||
|
- `filtered.raw` = 链的输出(标定前),`unfiltered.raw` = 设备计数;
|
||||||
|
- 两个 `newton` 用同一份 `force_calibration.json`,差异**只**来自滤波链;
|
||||||
|
- 实测:注入 `10,10,10,999,10,10`(中值窗口 3)→ `filtered.raw` 在尖峰帧为 `10.0`、
|
||||||
|
`unfiltered.raw` 为 `999.0`。
|
||||||
|
|
||||||
|
## 6. 可追溯性清单
|
||||||
|
|
||||||
|
1. 录制元数据写 `sensor.filters_state()` 的**生效值**(不是配置文件内容),并含 `preset` 与
|
||||||
|
`"source": "sensor.filters_state"` + `sdk_version`。
|
||||||
|
2. 界面显示"实际生效"而不是"我填的值";两者不一致时以实际为准并提示。
|
||||||
|
3. 记录里主列 = 滤波后(与界面一致),未滤波列另存;列结构与格式版本可区分。
|
||||||
|
4. 标定/可比性指纹用 `measurement` 组;`readout` 组变化只记录、不拦截。
|
||||||
|
5. 双份数据在同一帧、同一份标定系数下产生,差异可完全归因于滤波链。
|
||||||
|
|
||||||
|
## 7. 指纹:标定文件里怎么记(已实现)
|
||||||
|
|
||||||
|
`orisys.calibration` 负责把滤波设置写进标定文件的 `meta`,并在读回时还原:
|
||||||
|
|
||||||
|
```python
|
||||||
|
from orisys.calibration import filters_fingerprint, filter_mismatches, pipeline_from_sensor_json
|
||||||
|
|
||||||
|
pipeline = pipeline_from_sensor_json(config_path, filters_state=sensor.filters_state())
|
||||||
|
# {"stitch_resolution": …, "dis_mode": …, "hhd_downsample": …, "config_ref": …,
|
||||||
|
# "filters": {"measurement": {"flow_ema_alpha": 0.5},
|
||||||
|
# "readout": {"force_median_window": 3, "force_lowpass_cutoff_hz": 0.0},
|
||||||
|
# "preset": "balanced", "source": "sensor.filters_state"}}
|
||||||
|
```
|
||||||
|
|
||||||
|
写入的文件长这样(`meta` 里同时保留扁平键供旧读者按名取用):
|
||||||
|
|
||||||
|
```json
|
||||||
|
"meta": {
|
||||||
|
"filters": {
|
||||||
|
"measurement": {"flow_ema_alpha": 0.5},
|
||||||
|
"readout": {"force_median_window": 3, "force_lowpass_cutoff_hz": 0.0},
|
||||||
|
"preset": "balanced",
|
||||||
|
"source": "sensor.filters_state"
|
||||||
|
},
|
||||||
|
"flow_ema_alpha": 0.5,
|
||||||
|
"force_median_window": 3
|
||||||
|
}
|
||||||
|
```
|
||||||
|
|
||||||
|
规则:
|
||||||
|
|
||||||
|
- **生效值优先**:传了 `filters_state=` 就用它;没传才读文件的 `filters` 段,并标成
|
||||||
|
`"source": "sensor.json.filters"`(那是"意图",不一定等于设备正在做的事)。
|
||||||
|
- **不猜**:来源没写的键**不出现**在指纹里。`{}` 表示"没有任何记录",绝不表示"大概是关的"。
|
||||||
|
旧文档写的扁平键(`meta.flow_ema_alpha` …)在读回时会被折进对应的组,因此仍可比。
|
||||||
|
- **`enabled: false`** 只作为"全部关"的废弃别名保留(`flow_ema_alpha=1`、中值 `1`、低通 `0`)。
|
||||||
|
- **分组判读**:`calibration.check(doc, pipeline=…)` 把两组的差异分开报告 —— measurement 组的
|
||||||
|
差异进 `warnings`(措辞含 `not comparable`,即"两份记录不可比"),readout 组的差异进
|
||||||
|
`notes`(只记录,不拦截)。`CheckResult.notes` 是为此新增的字段。
|
||||||
|
|
||||||
@@ -0,0 +1,104 @@
|
|||||||
|
# 滤波预设:可直接复制的 `filters` 段
|
||||||
|
|
||||||
|
`json.load` 不支持注释,所以这个目录就是"在配置里写说明"的替代品:**把下面任意一段整块替换**
|
||||||
|
到 `sensors/<name>/sensor.json` 的 `"filters"` 段即可。原有的 `filters` 段请整段替换,
|
||||||
|
不要只改其中一行(避免出现 `preset` 与数值混写的歧义——显式键优先于预设值,混写容易看错)。
|
||||||
|
|
||||||
|
配置里**没写**的滤波键等于"关":整段删掉就是什么都不滤,和 `off` 那段等价。
|
||||||
|
|
||||||
|
背景、代价与可追溯性约定见 `docs/filters.md`;延迟/噪声实测见 `_perf/` 与
|
||||||
|
`orisys-sdk-perf-plan.md`。
|
||||||
|
|
||||||
|
下面每一段都有一个同目录的机器可读副本 `filters_<预设名>.json`(四个文件),
|
||||||
|
`tests/test_filter_presets.py` 会核对它们与 `FILTER_PRESETS` 完全一致 —— 所以改预设时
|
||||||
|
这些文件会一起红,不会悄悄过时。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## `off` —— 原始读数(默认)
|
||||||
|
|
||||||
|
```json
|
||||||
|
"filters": {
|
||||||
|
"flow_ema_alpha": 1.0,
|
||||||
|
"force_median_window": 1,
|
||||||
|
"force_lowpass_cutoff_hz": 0.0,
|
||||||
|
"force_lowpass_order": 2,
|
||||||
|
"force_lowpass_sample_rate_hz": "auto"
|
||||||
|
}
|
||||||
|
```
|
||||||
|
|
||||||
|
## `low_latency` —— 只去尖峰(≈1 帧 @30fps)
|
||||||
|
|
||||||
|
```json
|
||||||
|
"filters": {
|
||||||
|
"flow_ema_alpha": 1.0,
|
||||||
|
"force_median_window": 3,
|
||||||
|
"force_lowpass_cutoff_hz": 0.0,
|
||||||
|
"force_lowpass_order": 2,
|
||||||
|
"force_lowpass_sample_rate_hz": "auto"
|
||||||
|
}
|
||||||
|
```
|
||||||
|
|
||||||
|
## `balanced` —— 建议默认(≈2 帧 @30fps)
|
||||||
|
|
||||||
|
```json
|
||||||
|
"filters": {
|
||||||
|
"flow_ema_alpha": 0.5,
|
||||||
|
"force_median_window": 3,
|
||||||
|
"force_lowpass_cutoff_hz": 0.0,
|
||||||
|
"force_lowpass_order": 2,
|
||||||
|
"force_lowpass_sample_rate_hz": "auto"
|
||||||
|
}
|
||||||
|
```
|
||||||
|
|
||||||
|
## `low_noise` —— 底噪优先(≈3 帧 @30fps)
|
||||||
|
|
||||||
|
```json
|
||||||
|
"filters": {
|
||||||
|
"flow_ema_alpha": 0.3,
|
||||||
|
"force_median_window": 3,
|
||||||
|
"force_lowpass_cutoff_hz": 10.0,
|
||||||
|
"force_lowpass_order": 2,
|
||||||
|
"force_lowpass_sample_rate_hz": "auto"
|
||||||
|
}
|
||||||
|
```
|
||||||
|
|
||||||
|
## 用预设名(等价写法,数值由代码展开)
|
||||||
|
|
||||||
|
```json
|
||||||
|
"filters": { "preset": "balanced" }
|
||||||
|
```
|
||||||
|
|
||||||
|
`off / low_latency / balanced / low_noise` 四个名字来自
|
||||||
|
`src/orisys/sensor.py::FILTER_PRESETS`(单一真值);想从预设出发只改一个旋钮,就在同一段里写
|
||||||
|
那一个键,例如:
|
||||||
|
|
||||||
|
```json
|
||||||
|
"filters": { "preset": "balanced", "force_lowpass_cutoff_hz": 10.0 }
|
||||||
|
```
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 复制后请核对"实际生效值"
|
||||||
|
|
||||||
|
配置只是**意图**;`Sensor.configure()` 可以在运行时再改,所以审计与记录一律看生效值:
|
||||||
|
|
||||||
|
```python
|
||||||
|
from orisys.sensor import Sensor
|
||||||
|
s = Sensor("external", config_name="mini", verbose=False) # 无需硬件即可核对
|
||||||
|
print(s.filters_state()["preset"], s.filters_state()["measurement"], s.filters_state()["readout"])
|
||||||
|
```
|
||||||
|
|
||||||
|
`flow_ema_applied` / `force_median_applied` / `force_lowpass_applied` 三个布尔就是
|
||||||
|
"这一级到底有没有在滤"的答案;`flow_ema_alpha` 属于 **measurement**(改变测量,
|
||||||
|
不同值的两次录制不可比),中值与低通属于 **readout**(只影响读数呈现)。
|
||||||
|
|
||||||
|
导出标定/写记录时把同一个状态交给指纹函数,文件里就会留下可核对的设置:
|
||||||
|
|
||||||
|
```python
|
||||||
|
from orisys.calibration import pipeline_from_sensor_json
|
||||||
|
pipeline = pipeline_from_sensor_json("sensors/mini/sensor.json", filters_state=s.filters_state())
|
||||||
|
print(pipeline["filters"]) # measurement / readout / preset / source
|
||||||
|
```
|
||||||
|
|
||||||
|
详见 `docs/filters.md` 第 7 节。
|
||||||
@@ -0,0 +1,7 @@
|
|||||||
|
{
|
||||||
|
"flow_ema_alpha": 0.5,
|
||||||
|
"force_median_window": 3,
|
||||||
|
"force_lowpass_cutoff_hz": 0.0,
|
||||||
|
"force_lowpass_order": 2,
|
||||||
|
"force_lowpass_sample_rate_hz": "auto"
|
||||||
|
}
|
||||||
@@ -0,0 +1,7 @@
|
|||||||
|
{
|
||||||
|
"flow_ema_alpha": 1.0,
|
||||||
|
"force_median_window": 3,
|
||||||
|
"force_lowpass_cutoff_hz": 0.0,
|
||||||
|
"force_lowpass_order": 2,
|
||||||
|
"force_lowpass_sample_rate_hz": "auto"
|
||||||
|
}
|
||||||
@@ -0,0 +1,7 @@
|
|||||||
|
{
|
||||||
|
"flow_ema_alpha": 0.3,
|
||||||
|
"force_median_window": 3,
|
||||||
|
"force_lowpass_cutoff_hz": 10.0,
|
||||||
|
"force_lowpass_order": 2,
|
||||||
|
"force_lowpass_sample_rate_hz": "auto"
|
||||||
|
}
|
||||||
@@ -0,0 +1,7 @@
|
|||||||
|
{
|
||||||
|
"flow_ema_alpha": 1.0,
|
||||||
|
"force_median_window": 1,
|
||||||
|
"force_lowpass_cutoff_hz": 0.0,
|
||||||
|
"force_lowpass_order": 2,
|
||||||
|
"force_lowpass_sample_rate_hz": "auto"
|
||||||
|
}
|
||||||
@@ -0,0 +1,305 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
"""Multi-sensor real-time example: N streams, one process each.
|
||||||
|
|
||||||
|
This is the deployment shape the multi-sensor throughput numbers assume. Each
|
||||||
|
stream owns a process and pipelines the per-frame work into three concurrent
|
||||||
|
stages (capture+unroll -> DIS flow -> HHD+readout), so a stream's rate is bounded
|
||||||
|
by its slowest stage instead of the sum of all three.
|
||||||
|
|
||||||
|
# five finger streams from a video file (benchmark / smoke test)
|
||||||
|
python examples/multi_stream.py --streams 5 -c finger -v data/dd02-0703/1.mp4
|
||||||
|
|
||||||
|
# five live cameras, camera ids 0..4
|
||||||
|
python examples/multi_stream.py --streams 5 -c finger --camera-base 0
|
||||||
|
|
||||||
|
# single box sensor
|
||||||
|
python examples/multi_stream.py --streams 1 -c box -v data/long.mp4
|
||||||
|
|
||||||
|
# five streams, additionally recording an arrow-overlay video per stream
|
||||||
|
# plus one 5-up mosaic, without stalling the pipeline
|
||||||
|
python examples/multi_stream.py --streams 5 -c finger -v data/dd02-0703/1.mp4 \
|
||||||
|
--seconds 10 --record outputs/arrows --mosaic
|
||||||
|
|
||||||
|
Notes
|
||||||
|
-----
|
||||||
|
* One process per stream. Threads inside a single process share the GIL and,
|
||||||
|
measured on a 9950X, five finger streams reach ~150 FPS aggregate as threads
|
||||||
|
versus ~626 FPS as processes.
|
||||||
|
* ``cv2.setNumThreads(2)`` per process: at five streams that beats 1, 4 and 8.
|
||||||
|
* Run one stream per process even when the sensor count is small; the process
|
||||||
|
boundary also isolates a crashed camera from the rest.
|
||||||
|
* ``--record`` draws the DIS flow with ``orisys.util.draw_arrows`` (plus the
|
||||||
|
contact overlay with ``--contact``) and encodes it with ``cv2.VideoWriter``.
|
||||||
|
Both happen on a dedicated thread fed by a bounded drop-oldest queue, so the
|
||||||
|
pipeline never waits on the encoder; frames the writer cannot keep up with are
|
||||||
|
counted in the ``rec`` column as ``written/dropped``. Keep ``--record`` for
|
||||||
|
verification runs - the encoder still competes for CPU with the pipeline.
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import argparse
|
||||||
|
import json
|
||||||
|
import multiprocessing as mp
|
||||||
|
import os
|
||||||
|
import statistics as st
|
||||||
|
import sys
|
||||||
|
import time
|
||||||
|
|
||||||
|
os.environ.setdefault("OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS", "0")
|
||||||
|
|
||||||
|
import orisys
|
||||||
|
from orisys import StreamPipeline, StreamRecorder, configs
|
||||||
|
|
||||||
|
|
||||||
|
def _make_stream(args, index):
|
||||||
|
"""Return (sensor, produce, close) for one stream."""
|
||||||
|
source = args.video
|
||||||
|
if args.camera_base is not None:
|
||||||
|
source = int(args.camera_base) + index
|
||||||
|
if source is None:
|
||||||
|
raise ValueError("pass --video PATH or --camera-base ID")
|
||||||
|
|
||||||
|
cfg = configs.load_config(args.config)
|
||||||
|
if str(cfg.get("type", "")).lower() == "single_camera":
|
||||||
|
from orisys.finger import FingerRuntime
|
||||||
|
|
||||||
|
runtime = FingerRuntime.open(config=args.config, video=source,
|
||||||
|
verbose=False)
|
||||||
|
return runtime.tactile.raw_sensor, _finger_produce(runtime), runtime.close
|
||||||
|
|
||||||
|
sensor = orisys.Sensor(source, config_name=args.config, isstitch=True,
|
||||||
|
cuda=False, backend="cpu", verbose=False)
|
||||||
|
|
||||||
|
def produce():
|
||||||
|
return sensor.get_img()
|
||||||
|
|
||||||
|
return sensor, produce, sensor.disconnect
|
||||||
|
|
||||||
|
|
||||||
|
def _finger_produce(runtime):
|
||||||
|
def produce():
|
||||||
|
ok, frame = runtime.grab_and_process(color=False)
|
||||||
|
return frame if ok else None
|
||||||
|
|
||||||
|
return produce
|
||||||
|
|
||||||
|
|
||||||
|
def worker(args, index, out_path):
|
||||||
|
"""One stream: own process, three-stage pipeline."""
|
||||||
|
import cv2
|
||||||
|
|
||||||
|
cv2.setNumThreads(int(args.threads))
|
||||||
|
sensor, produce, close = _make_stream(args, index)
|
||||||
|
|
||||||
|
frames = []
|
||||||
|
stamps = []
|
||||||
|
started = time.perf_counter()
|
||||||
|
|
||||||
|
def collect(snap):
|
||||||
|
frames.append(snap.age_ms)
|
||||||
|
stamps.append(time.perf_counter())
|
||||||
|
|
||||||
|
# Recording is entirely off the hot path: ``on_frame`` only pushes onto a
|
||||||
|
# bounded drop-oldest queue, and a separate thread does the drawing and the
|
||||||
|
# H.264/MPEG-4 encoding. The pipeline never blocks on the encoder.
|
||||||
|
recorder = None
|
||||||
|
on_frame = collect
|
||||||
|
if args.record:
|
||||||
|
recorder = StreamRecorder(
|
||||||
|
os.path.join(args.record, f"stream_{index}.mp4"),
|
||||||
|
fps=args.record_fps,
|
||||||
|
every=args.record_every,
|
||||||
|
fourcc=args.record_fourcc,
|
||||||
|
valid_mask=lambda: getattr(sensor, "valid_mask", None),
|
||||||
|
grid_spacing=args.record_grid,
|
||||||
|
threshold=args.record_threshold,
|
||||||
|
arrow_scale=args.record_arrow_scale,
|
||||||
|
draw_contact=args.contact,
|
||||||
|
label=f"#{index}",
|
||||||
|
verbose=True,
|
||||||
|
)
|
||||||
|
|
||||||
|
def on_frame(snap, _collect=collect, _rec=recorder):
|
||||||
|
_collect(snap)
|
||||||
|
_rec.on_frame(snap)
|
||||||
|
|
||||||
|
if args.no_arrays:
|
||||||
|
include = ()
|
||||||
|
elif recorder is not None:
|
||||||
|
include = ("image", "flow") # what the arrow overlay needs
|
||||||
|
else:
|
||||||
|
include = ("image",)
|
||||||
|
|
||||||
|
pipe = StreamPipeline(sensor, produce, on_frame=on_frame,
|
||||||
|
include=include, contact=args.contact, depth=args.depth)
|
||||||
|
pipe.start()
|
||||||
|
record_stats = None
|
||||||
|
try:
|
||||||
|
deadline = started + args.seconds if args.seconds else None
|
||||||
|
while True:
|
||||||
|
if not pipe.running:
|
||||||
|
break
|
||||||
|
if args.frames and pipe.stats.published >= args.frames:
|
||||||
|
break
|
||||||
|
if deadline is not None and time.perf_counter() >= deadline:
|
||||||
|
break
|
||||||
|
time.sleep(0.002)
|
||||||
|
finally:
|
||||||
|
try:
|
||||||
|
pipe.stop()
|
||||||
|
except BaseException as exc: # surface a stage failure
|
||||||
|
print(f"[stream {index}] pipeline error: {type(exc).__name__}: {exc}",
|
||||||
|
file=sys.stderr)
|
||||||
|
raise
|
||||||
|
finally:
|
||||||
|
if recorder is not None:
|
||||||
|
record_stats = recorder.close()
|
||||||
|
close()
|
||||||
|
|
||||||
|
wall = time.perf_counter() - started
|
||||||
|
# Steady-state rate: skip the first frames, which pay the JIT / HHD-cache /
|
||||||
|
# DIS-reference-frame warm-up and would drag a short run's average down.
|
||||||
|
warm = min(30, max(0, len(stamps) - 2))
|
||||||
|
span = (stamps[-1] - stamps[warm]) if len(stamps) > warm + 1 else 0.0
|
||||||
|
steady = (len(stamps) - 1 - warm) / span if span > 0 else 0.0
|
||||||
|
payload = {
|
||||||
|
"index": index,
|
||||||
|
"published": pipe.stats.published,
|
||||||
|
"wall": wall,
|
||||||
|
"fps": pipe.stats.published / wall if wall else 0.0,
|
||||||
|
"steady_fps": steady,
|
||||||
|
"stats": pipe.stats.as_dict(),
|
||||||
|
"age_p95_ms": float(st.quantiles(frames, n=20)[18]) if len(frames) >= 20 else 0.0,
|
||||||
|
"record": record_stats,
|
||||||
|
}
|
||||||
|
with open(out_path, "w") as fh:
|
||||||
|
json.dump(payload, fh)
|
||||||
|
|
||||||
|
|
||||||
|
def main():
|
||||||
|
ap = argparse.ArgumentParser(description=__doc__,
|
||||||
|
formatter_class=argparse.RawDescriptionHelpFormatter)
|
||||||
|
ap.add_argument("--streams", "-n", type=int, default=5)
|
||||||
|
ap.add_argument("--config", "-c", default="finger")
|
||||||
|
ap.add_argument("--video", "-v", default=None, help="video file shared by all streams")
|
||||||
|
ap.add_argument("--camera-base", type=int, default=None,
|
||||||
|
help="first camera id; stream i uses base + i")
|
||||||
|
ap.add_argument("--frames", type=int, default=0, help="stop after N frames (0 = until EOF)")
|
||||||
|
ap.add_argument("--seconds", type=float, default=10.0, help="max wall time per stream")
|
||||||
|
ap.add_argument("--threads", type=int, default=2, help="cv2.setNumThreads per stream")
|
||||||
|
ap.add_argument("--depth", type=int, default=2, help="queue depth between stages")
|
||||||
|
ap.add_argument("--contact", action="store_true", help="also run compute_contact")
|
||||||
|
ap.add_argument("--no-arrays", action="store_true",
|
||||||
|
help="scalars only (fastest; skips the snapshot image copy)")
|
||||||
|
ap.add_argument("--out-dir", default=None)
|
||||||
|
ap.add_argument("--record", default=None, metavar="DIR",
|
||||||
|
help="write one arrow-overlay mp4 per stream into DIR "
|
||||||
|
"(drawing+encoding run on a separate thread, off the "
|
||||||
|
"pipeline's critical path)")
|
||||||
|
ap.add_argument("--record-fps", type=float, default=30.0,
|
||||||
|
help="playback fps of the recorded files")
|
||||||
|
ap.add_argument("--record-every", type=int, default=1,
|
||||||
|
help="keep one frame in N (raise it if the writer falls behind)")
|
||||||
|
ap.add_argument("--record-fourcc", default="mp4v",
|
||||||
|
help="mp4v (built in) or MJPG/avi for maximum compatibility")
|
||||||
|
ap.add_argument("--record-grid", type=int, default=14, help="arrow grid spacing in px")
|
||||||
|
ap.add_argument("--record-threshold", type=float, default=0.1,
|
||||||
|
help="displacement (px) below which a dot is drawn instead of an arrow")
|
||||||
|
ap.add_argument("--record-arrow-scale", type=float, default=0.15,
|
||||||
|
help="draw_arrows divides the displacement by this; 0.15 magnifies "
|
||||||
|
"the finger's ~1-3 px field ~6.7x so directions are legible")
|
||||||
|
ap.add_argument("--mosaic", action="store_true",
|
||||||
|
help="after the run, tile the recordings into one grid video")
|
||||||
|
ap.add_argument("--mosaic-cols", type=int, default=0, help="mosaic columns (0 = square-ish)")
|
||||||
|
ap.add_argument("--mosaic-scale", type=float, default=1.0, help="mosaic downscale factor")
|
||||||
|
args = ap.parse_args()
|
||||||
|
|
||||||
|
if args.record and args.no_arrays:
|
||||||
|
ap.error("--record needs the frame and flow arrays; drop --no-arrays")
|
||||||
|
if args.mosaic and not args.record:
|
||||||
|
ap.error("--mosaic needs --record DIR")
|
||||||
|
|
||||||
|
if args.record:
|
||||||
|
args.record = os.path.abspath(args.record)
|
||||||
|
os.makedirs(args.record, exist_ok=True)
|
||||||
|
|
||||||
|
out_dir = args.out_dir or os.path.join(os.path.dirname(os.path.abspath(__file__)),
|
||||||
|
"..", "outputs", "multi_stream")
|
||||||
|
out_dir = os.path.abspath(out_dir)
|
||||||
|
os.makedirs(out_dir, exist_ok=True)
|
||||||
|
|
||||||
|
ctx = mp.get_context("spawn")
|
||||||
|
procs, paths = [], []
|
||||||
|
for i in range(args.streams):
|
||||||
|
path = os.path.join(out_dir, f"stream_{i}.json")
|
||||||
|
paths.append(path)
|
||||||
|
procs.append(ctx.Process(target=worker, args=(args, i, path)))
|
||||||
|
for p in procs:
|
||||||
|
p.start()
|
||||||
|
for p in procs:
|
||||||
|
p.join()
|
||||||
|
|
||||||
|
rows = []
|
||||||
|
for path in paths:
|
||||||
|
try:
|
||||||
|
with open(path) as fh:
|
||||||
|
rows.append(json.load(fh))
|
||||||
|
except Exception:
|
||||||
|
pass
|
||||||
|
if not rows:
|
||||||
|
print("no stream produced results", file=sys.stderr)
|
||||||
|
return 1
|
||||||
|
|
||||||
|
wall = max(r["wall"] for r in rows)
|
||||||
|
total = sum(r["published"] for r in rows)
|
||||||
|
steady_rows = [r for r in rows if r.get("steady_fps")]
|
||||||
|
steady = st.mean(r["steady_fps"] for r in steady_rows) if steady_rows else 0.0
|
||||||
|
print(f"\nstreams={len(rows)} frames={total} wall={wall:.2f}s")
|
||||||
|
print(f"aggregate throughput : {total / wall:.1f} FPS (incl. warm-up)")
|
||||||
|
print(f"per-stream throughput : {total / wall / len(rows):.1f} FPS (incl. warm-up)")
|
||||||
|
print(f"per-stream steady-state : {steady:.1f} FPS")
|
||||||
|
recording = args.record is not None
|
||||||
|
print(f"\n{'stream':>7}{'published':>11}{'FPS':>9}{'steady':>9}{'produce':>10}"
|
||||||
|
f"{'flow':>9}{'hhd':>9}{'age':>9}{'age p95':>10}"
|
||||||
|
+ (f"{'rec':>13}" if recording else ""))
|
||||||
|
for r in sorted(rows, key=lambda x: x["index"]):
|
||||||
|
s = r["stats"]
|
||||||
|
rec = ""
|
||||||
|
if recording:
|
||||||
|
rs = r.get("record") or {}
|
||||||
|
rec = f"{rs.get('written', 0)}/{rs.get('dropped', 0)}"
|
||||||
|
if rs.get("error"):
|
||||||
|
rec = "ERR"
|
||||||
|
print(f"{r['index']:>7}{r['published']:>11}{r['fps']:>9.1f}"
|
||||||
|
f"{r.get('steady_fps', 0.0):>9.1f}"
|
||||||
|
f"{s['produce_ms']:>10.2f}{s['flow_ms']:>9.2f}{s['hhd_ms']:>9.2f}"
|
||||||
|
f"{s['age_ms']:>9.2f}{r['age_p95_ms']:>10.2f}"
|
||||||
|
+ (f"{rec:>13}" if recording else ""))
|
||||||
|
if recording:
|
||||||
|
print("rec column = frames written / frames dropped by the writer thread")
|
||||||
|
for r in sorted(rows, key=lambda x: x["index"]):
|
||||||
|
rs = r.get("record") or {}
|
||||||
|
if rs.get("error"):
|
||||||
|
print(f" stream {r['index']}: {rs['error']}")
|
||||||
|
|
||||||
|
if args.mosaic and recording:
|
||||||
|
streams = sorted(r["index"] for r in rows)
|
||||||
|
srcs = [os.path.join(args.record, f"stream_{i}.mp4") for i in streams]
|
||||||
|
missing = [p for p in srcs if not os.path.isfile(p)]
|
||||||
|
if missing:
|
||||||
|
print(f"mosaic skipped, missing {missing}", file=sys.stderr)
|
||||||
|
else:
|
||||||
|
out = os.path.join(args.record, "mosaic.mp4")
|
||||||
|
# No tile labels: each recorded frame already carries the "#N" HUD.
|
||||||
|
info = orisys.mosaic_videos(srcs, out, cols=args.mosaic_cols or None,
|
||||||
|
scale=args.mosaic_scale,
|
||||||
|
fps=args.record_fps)
|
||||||
|
print(f"\nmosaic -> {info['path']} {info['size'][0]}x{info['size'][1]} "
|
||||||
|
f"{info['grid'][0]}x{info['grid'][1]} grid {info['frames']} frames "
|
||||||
|
f"@ {info['fps']:g} fps")
|
||||||
|
return 0
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
raise SystemExit(main())
|
||||||
@@ -33,6 +33,13 @@
|
|||||||
"algorithm_list": ["raft", "dis"],
|
"algorithm_list": ["raft", "dis"],
|
||||||
"hhd_downsample": 3
|
"hhd_downsample": 3
|
||||||
},
|
},
|
||||||
|
"filters": {
|
||||||
|
"flow_ema_alpha": 1.0,
|
||||||
|
"force_median_window": 1,
|
||||||
|
"force_lowpass_cutoff_hz": 0.0,
|
||||||
|
"force_lowpass_order": 2,
|
||||||
|
"force_lowpass_sample_rate_hz": "auto"
|
||||||
|
},
|
||||||
"contact": {
|
"contact": {
|
||||||
"min_contour_area": 3000,
|
"min_contour_area": 3000,
|
||||||
"flow_threshold":5,
|
"flow_threshold":5,
|
||||||
|
|||||||
@@ -22,7 +22,14 @@
|
|||||||
"dis_preset": "medium",
|
"dis_preset": "medium",
|
||||||
"dis_mode": 1,
|
"dis_mode": 1,
|
||||||
"algorithm_list": ["raft", "dis"],
|
"algorithm_list": ["raft", "dis"],
|
||||||
"hhd_downsample": 3
|
"hhd_downsample": 2
|
||||||
|
},
|
||||||
|
"filters": {
|
||||||
|
"flow_ema_alpha": 1.0,
|
||||||
|
"force_median_window": 1,
|
||||||
|
"force_lowpass_cutoff_hz": 0.0,
|
||||||
|
"force_lowpass_order": 2,
|
||||||
|
"force_lowpass_sample_rate_hz": "auto"
|
||||||
},
|
},
|
||||||
"contact": {
|
"contact": {
|
||||||
"min_contour_area": 3000,
|
"min_contour_area": 3000,
|
||||||
|
|||||||
@@ -29,10 +29,17 @@
|
|||||||
"loop_closure_interval": 1,
|
"loop_closure_interval": 1,
|
||||||
"algorithm": "dis",
|
"algorithm": "dis",
|
||||||
"dis_preset": "medium",
|
"dis_preset": "medium",
|
||||||
"dis_mode": 3,
|
"dis_mode": 1,
|
||||||
"algorithm_list": ["raft", "dis"],
|
"algorithm_list": ["raft", "dis"],
|
||||||
"hhd_downsample": 3
|
"hhd_downsample": 3
|
||||||
},
|
},
|
||||||
|
"filters": {
|
||||||
|
"flow_ema_alpha": 1.0,
|
||||||
|
"force_median_window": 1,
|
||||||
|
"force_lowpass_cutoff_hz": 0.0,
|
||||||
|
"force_lowpass_order": 2,
|
||||||
|
"force_lowpass_sample_rate_hz": "auto"
|
||||||
|
},
|
||||||
"contact": {
|
"contact": {
|
||||||
"min_contour_area": 3000,
|
"min_contour_area": 3000,
|
||||||
"flow_threshold":5,
|
"flow_threshold":5,
|
||||||
|
|||||||
@@ -15,6 +15,8 @@ from . import calibration
|
|||||||
from . import finger
|
from . import finger
|
||||||
from . import signal_processing
|
from . import signal_processing
|
||||||
from . import capture
|
from . import capture
|
||||||
|
from .pipeline import FrameSnapshot, PipelineStats, StreamPipeline
|
||||||
|
from .export import StreamRecorder, mosaic_videos
|
||||||
from .frame_processors import FFCFrameProcessor
|
from .frame_processors import FFCFrameProcessor
|
||||||
|
|
||||||
CompoundEyeSensor = Sensor
|
CompoundEyeSensor = Sensor
|
||||||
@@ -44,6 +46,15 @@ __all__ = [
|
|||||||
# capture helpers
|
# capture helpers
|
||||||
'capture',
|
'capture',
|
||||||
|
|
||||||
|
# pipelined multi-stream processing
|
||||||
|
'StreamPipeline',
|
||||||
|
'FrameSnapshot',
|
||||||
|
'PipelineStats',
|
||||||
|
|
||||||
|
# offline recording of arrow overlays
|
||||||
|
'StreamRecorder',
|
||||||
|
'mosaic_videos',
|
||||||
|
|
||||||
# force calibration
|
# force calibration
|
||||||
'calibration',
|
'calibration',
|
||||||
|
|
||||||
|
|||||||
+249
-11
@@ -47,11 +47,186 @@ _KNOWN_TOP_KEYS = frozenset(
|
|||||||
{"schema_version", "unit", "channels", "meta", "identity", "pipeline", "provenance", "measurements", "quality"}
|
{"schema_version", "unit", "channels", "meta", "identity", "pipeline", "provenance", "measurements", "quality"}
|
||||||
)
|
)
|
||||||
|
|
||||||
|
#: Filter stages that change **what is measured**: they sit upstream of the
|
||||||
|
#: force computation, so two recordings with different values are not comparable.
|
||||||
|
#: They belong to the measurement-pipeline identity, for the same reason
|
||||||
|
#: ``dis_mode`` and ``hhd_downsample`` do.
|
||||||
|
FILTER_MEASUREMENT_KEYS: tuple[str, ...] = ("flow_ema_alpha",)
|
||||||
|
#: Filter stages that only smooth the force triple on its way out. They are
|
||||||
|
#: recorded so a file explains its own numbers, but a difference between two
|
||||||
|
#: files does not invalidate a comparison.
|
||||||
|
FILTER_READOUT_KEYS: tuple[str, ...] = (
|
||||||
|
"force_median_window",
|
||||||
|
"force_lowpass_cutoff_hz",
|
||||||
|
"force_lowpass_order",
|
||||||
|
"force_lowpass_sample_rate_hz",
|
||||||
|
"force_lowpass_rate_auto",
|
||||||
|
)
|
||||||
|
#: Every recorded filter value, measurement group first.
|
||||||
|
FILTER_KEYS: tuple[str, ...] = FILTER_MEASUREMENT_KEYS + FILTER_READOUT_KEYS
|
||||||
|
#: What "off" looks like, per stage. ``enabled: false`` means exactly this.
|
||||||
|
_OFF_FILTER_VALUES: dict[str, Any] = {
|
||||||
|
"flow_ema_alpha": 1.0,
|
||||||
|
"force_median_window": 1,
|
||||||
|
"force_lowpass_cutoff_hz": 0.0,
|
||||||
|
}
|
||||||
|
_INT_FILTER_KEYS = frozenset({"force_median_window", "force_lowpass_order"})
|
||||||
|
_FLOAT_FILTER_KEYS = frozenset(
|
||||||
|
{"flow_ema_alpha", "force_lowpass_cutoff_hz", "force_lowpass_sample_rate_hz"}
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
class CalibrationError(ValueError):
|
class CalibrationError(ValueError):
|
||||||
"""Invalid or unusable force calibration document / path."""
|
"""Invalid or unusable force calibration document / path."""
|
||||||
|
|
||||||
|
|
||||||
|
def _coerce_filter_value(key: str, value: Any) -> Any:
|
||||||
|
"""A filter value in its recorded type, or ``None`` when unusable."""
|
||||||
|
if value is None:
|
||||||
|
return None
|
||||||
|
if key == "force_lowpass_rate_auto":
|
||||||
|
return bool(value)
|
||||||
|
try:
|
||||||
|
if key in _INT_FILTER_KEYS:
|
||||||
|
return int(value)
|
||||||
|
if key in _FLOAT_FILTER_KEYS:
|
||||||
|
return float(value)
|
||||||
|
except (TypeError, ValueError):
|
||||||
|
return None
|
||||||
|
return value
|
||||||
|
|
||||||
|
|
||||||
|
def _filters_from_config(block: Mapping[str, Any] | Any) -> dict[str, dict[str, Any]]:
|
||||||
|
"""Best-effort filter groups from a flat ``filters`` block.
|
||||||
|
|
||||||
|
Only what the mapping states — directly or through a named preset, with an
|
||||||
|
explicit key winning over the preset — is reported. Nothing is filled in from
|
||||||
|
a default: a default is how a document ends up claiming a filter the device
|
||||||
|
is not running. ``enabled: false`` keeps its deprecated meaning of "every
|
||||||
|
stage off".
|
||||||
|
"""
|
||||||
|
flat = dict(block) if isinstance(block, Mapping) else {}
|
||||||
|
preset_name = flat.get("preset") if isinstance(flat.get("preset"), str) else None
|
||||||
|
merged: dict[str, Any] = {}
|
||||||
|
if preset_name:
|
||||||
|
try:
|
||||||
|
from .sensor import FILTER_PRESETS # deferred: sensor imports this module
|
||||||
|
except Exception: # pragma: no cover - defensive
|
||||||
|
FILTER_PRESETS = {}
|
||||||
|
merged.update(FILTER_PRESETS.get(preset_name, {}) or {})
|
||||||
|
for key in FILTER_KEYS:
|
||||||
|
if flat.get(key) is not None:
|
||||||
|
merged[key] = flat[key]
|
||||||
|
if isinstance(flat.get("force_lowpass_sample_rate_hz"), str):
|
||||||
|
# "auto" / "measured" is a rate *source*, not a number; keep it as one.
|
||||||
|
merged["force_lowpass_rate_auto"] = True
|
||||||
|
merged.pop("force_lowpass_sample_rate_hz", None)
|
||||||
|
if flat.get("enabled", None) is False:
|
||||||
|
order = merged.get("force_lowpass_order")
|
||||||
|
rate_auto = merged.get("force_lowpass_rate_auto")
|
||||||
|
merged = dict(_OFF_FILTER_VALUES)
|
||||||
|
# Order and rate tracking stay meaningful even with the stage off.
|
||||||
|
if order is not None:
|
||||||
|
merged["force_lowpass_order"] = order
|
||||||
|
if rate_auto is not None:
|
||||||
|
merged["force_lowpass_rate_auto"] = rate_auto
|
||||||
|
return {
|
||||||
|
group: {key: _coerce_filter_value(key, merged.get(key)) for key in keys}
|
||||||
|
for group, keys in (
|
||||||
|
("measurement", FILTER_MEASUREMENT_KEYS),
|
||||||
|
("readout", FILTER_READOUT_KEYS),
|
||||||
|
)
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
def _flat_filters_from(*sources: Mapping[str, Any] | Any) -> dict[str, Any]:
|
||||||
|
"""The flat form older documents used: filter keys at the top of meta."""
|
||||||
|
out: dict[str, Any] = {}
|
||||||
|
for source in sources:
|
||||||
|
if not isinstance(source, Mapping):
|
||||||
|
continue
|
||||||
|
for key in FILTER_KEYS:
|
||||||
|
if key not in out and source.get(key) is not None:
|
||||||
|
out[key] = source[key]
|
||||||
|
return out
|
||||||
|
|
||||||
|
|
||||||
|
def filters_fingerprint(state: Mapping[str, Any] | Any) -> dict[str, Any]:
|
||||||
|
"""Split a filter state into its ``measurement`` and ``readout`` groups.
|
||||||
|
|
||||||
|
The honest input is ``Sensor.filters_state()`` — the values that actually
|
||||||
|
took effect, which is not necessarily what the config file says. A flat
|
||||||
|
mapping is accepted as a fallback (a sensor.json ``filters`` block, or the
|
||||||
|
flat form older documents wrote) and read as the intent it is.
|
||||||
|
|
||||||
|
Values a source does not state are absent from the result rather than
|
||||||
|
guessed, so ``{}`` means "nothing recorded" and never "probably off".
|
||||||
|
"""
|
||||||
|
if not isinstance(state, Mapping) or not state:
|
||||||
|
return {}
|
||||||
|
measurement = state.get("measurement")
|
||||||
|
readout = state.get("readout")
|
||||||
|
if isinstance(measurement, Mapping) or isinstance(readout, Mapping):
|
||||||
|
groups = {
|
||||||
|
"measurement": {
|
||||||
|
key: _coerce_filter_value(key, measurement.get(key))
|
||||||
|
for key in FILTER_MEASUREMENT_KEYS
|
||||||
|
}
|
||||||
|
if isinstance(measurement, Mapping)
|
||||||
|
else {},
|
||||||
|
"readout": {
|
||||||
|
key: _coerce_filter_value(key, readout.get(key))
|
||||||
|
for key in FILTER_READOUT_KEYS
|
||||||
|
}
|
||||||
|
if isinstance(readout, Mapping)
|
||||||
|
else {},
|
||||||
|
}
|
||||||
|
source = str(state.get("source") or "sensor.filters_state")
|
||||||
|
else:
|
||||||
|
groups = _filters_from_config(state)
|
||||||
|
source = "sensor.json.filters"
|
||||||
|
out: dict[str, Any] = {}
|
||||||
|
for group in ("measurement", "readout"):
|
||||||
|
values = {
|
||||||
|
key: value for key, value in (groups.get(group) or {}).items() if value is not None
|
||||||
|
}
|
||||||
|
if values:
|
||||||
|
out[group] = values
|
||||||
|
if not out:
|
||||||
|
return {}
|
||||||
|
if state.get("preset"):
|
||||||
|
out["preset"] = str(state["preset"])
|
||||||
|
out["source"] = source
|
||||||
|
return out
|
||||||
|
|
||||||
|
|
||||||
|
def filter_mismatches(
|
||||||
|
recorded: Mapping[str, Any] | Any, expected: Mapping[str, Any] | Any
|
||||||
|
) -> tuple[list[str], list[str]]:
|
||||||
|
"""Compare two filter fingerprints → ``(measurement, readout)`` differences.
|
||||||
|
|
||||||
|
A measurement difference means the two records are not comparable, so it is
|
||||||
|
reported as a warning. A readout difference only changes how the same
|
||||||
|
measurement was smoothed, so it is a note: recorded, never an obstacle.
|
||||||
|
"""
|
||||||
|
left = filters_fingerprint(recorded)
|
||||||
|
right = filters_fingerprint(expected)
|
||||||
|
warns: list[str] = []
|
||||||
|
notes: list[str] = []
|
||||||
|
for group, keys, bucket, label in (
|
||||||
|
("measurement", FILTER_MEASUREMENT_KEYS, warns, "measurement (not comparable)"),
|
||||||
|
("readout", FILTER_READOUT_KEYS, notes, "readout"),
|
||||||
|
):
|
||||||
|
for key in keys:
|
||||||
|
if key in (left.get(group) or {}) and key in (right.get(group) or {}):
|
||||||
|
if left[group][key] != right[group][key]:
|
||||||
|
bucket.append(
|
||||||
|
f"filter {label} mismatch: {key} "
|
||||||
|
f"{left[group][key]!r} != {right[group][key]!r}"
|
||||||
|
)
|
||||||
|
return warns, notes
|
||||||
|
|
||||||
|
|
||||||
def canonicalize_channel(name: str) -> str:
|
def canonicalize_channel(name: str) -> str:
|
||||||
"""Map FN / normal / shear_x / … to canonical ``fn`` / ``fx`` / ``fy``."""
|
"""Map FN / normal / shear_x / … to canonical ``fn`` / ``fx`` / ``fy``."""
|
||||||
key = str(name or "").strip()
|
key = str(name or "").strip()
|
||||||
@@ -175,6 +350,9 @@ class CheckResult:
|
|||||||
ok: bool
|
ok: bool
|
||||||
errors: list[str] = field(default_factory=list)
|
errors: list[str] = field(default_factory=list)
|
||||||
warnings: list[str] = field(default_factory=list)
|
warnings: list[str] = field(default_factory=list)
|
||||||
|
#: Differences worth recording that are not a reason to refuse anything,
|
||||||
|
#: such as a readout-only filter change between two documents.
|
||||||
|
notes: list[str] = field(default_factory=list)
|
||||||
|
|
||||||
|
|
||||||
@dataclass(frozen=True)
|
@dataclass(frozen=True)
|
||||||
@@ -245,6 +423,16 @@ class Calibration:
|
|||||||
for key in ("stitch_resolution", "dis_mode", "hhd_downsample", "config_ref"):
|
for key in ("stitch_resolution", "dis_mode", "hhd_downsample", "config_ref"):
|
||||||
if key in self.pipeline and self.pipeline[key] is not None:
|
if key in self.pipeline and self.pipeline[key] is not None:
|
||||||
meta[key] = self.pipeline[key]
|
meta[key] = self.pipeline[key]
|
||||||
|
fingerprint = filters_fingerprint(self.pipeline.get("filters"))
|
||||||
|
if fingerprint:
|
||||||
|
# Structured, so a reader can tell a change that makes two records
|
||||||
|
# incomparable (measurement) from one that only changes how the same
|
||||||
|
# measurement was smoothed (readout).
|
||||||
|
meta["filters"] = fingerprint
|
||||||
|
# The flat keys are kept as well for readers that look them up by
|
||||||
|
# name; the groups above are the authoritative form.
|
||||||
|
for group in ("measurement", "readout"):
|
||||||
|
meta.update(fingerprint.get(group) or {})
|
||||||
if self.measurements:
|
if self.measurements:
|
||||||
meta["samples"] = self.measurements
|
meta["samples"] = self.measurements
|
||||||
if self.origin and "origin" not in meta:
|
if self.origin and "origin" not in meta:
|
||||||
@@ -308,6 +496,7 @@ def check(doc: dict | Calibration, *, pipeline: dict | None = None) -> CheckResu
|
|||||||
"""Validate a document or Calibration. Errors refuse apply/write."""
|
"""Validate a document or Calibration. Errors refuse apply/write."""
|
||||||
errors: list[str] = []
|
errors: list[str] = []
|
||||||
warns: list[str] = []
|
warns: list[str] = []
|
||||||
|
notes: list[str] = []
|
||||||
|
|
||||||
if isinstance(doc, Calibration):
|
if isinstance(doc, Calibration):
|
||||||
data = doc.to_dict()
|
data = doc.to_dict()
|
||||||
@@ -390,8 +579,15 @@ def check(doc: dict | Calibration, *, pipeline: dict | None = None) -> CheckResu
|
|||||||
for key in ("stitch_resolution", "dis_mode", "hhd_downsample"):
|
for key in ("stitch_resolution", "dis_mode", "hhd_downsample"):
|
||||||
if key in doc.pipeline and key in pipeline and doc.pipeline[key] != pipeline[key]:
|
if key in doc.pipeline and key in pipeline and doc.pipeline[key] != pipeline[key]:
|
||||||
warns.append(f"pipeline mismatch: {key}")
|
warns.append(f"pipeline mismatch: {key}")
|
||||||
|
# The filter groups are compared apart on purpose: ``flow_ema_alpha``
|
||||||
|
# changes what was measured, the readout stages only smooth it.
|
||||||
|
filter_warns, filter_notes = filter_mismatches(
|
||||||
|
doc.pipeline.get("filters"), pipeline.get("filters")
|
||||||
|
)
|
||||||
|
warns.extend(filter_warns)
|
||||||
|
notes.extend(filter_notes)
|
||||||
|
|
||||||
return CheckResult(ok=not errors, errors=errors, warnings=warns)
|
return CheckResult(ok=not errors, errors=errors, warnings=warns, notes=notes)
|
||||||
|
|
||||||
|
|
||||||
def parse(data: dict, *, source_path: Path | None = None) -> Calibration:
|
def parse(data: dict, *, source_path: Path | None = None) -> Calibration:
|
||||||
@@ -492,6 +688,14 @@ def parse(data: dict, *, source_path: Path | None = None) -> Calibration:
|
|||||||
"hhd_downsample": pipeline_in.get("hhd_downsample", meta.get("hhd_downsample")),
|
"hhd_downsample": pipeline_in.get("hhd_downsample", meta.get("hhd_downsample")),
|
||||||
"config_ref": pipeline_in.get("config_ref", meta.get("config_ref")),
|
"config_ref": pipeline_in.get("config_ref", meta.get("config_ref")),
|
||||||
"config_hash": pipeline_in.get("config_hash", meta.get("config_hash")),
|
"config_hash": pipeline_in.get("config_hash", meta.get("config_hash")),
|
||||||
|
# Structured when the document has it, otherwise rebuilt from the flat
|
||||||
|
# keys older documents wrote — reading them back is what makes the
|
||||||
|
# fingerprint checkable at all.
|
||||||
|
"filters": filters_fingerprint(
|
||||||
|
pipeline_in.get("filters")
|
||||||
|
or meta.get("filters")
|
||||||
|
or _flat_filters_from(pipeline_in, meta)
|
||||||
|
),
|
||||||
}
|
}
|
||||||
provenance = {
|
provenance = {
|
||||||
"origin": provenance_in.get("origin", meta.get("origin", "")),
|
"origin": provenance_in.get("origin", meta.get("origin", "")),
|
||||||
@@ -600,26 +804,45 @@ def load(
|
|||||||
return None
|
return None
|
||||||
|
|
||||||
|
|
||||||
def pipeline_from_sensor_json(path: str | Path | None) -> dict[str, Any]:
|
def pipeline_from_sensor_json(
|
||||||
"""Pull stitch / tracker fields from a sensor.json for export pipeline/meta."""
|
path: str | Path | None,
|
||||||
|
*,
|
||||||
|
filters_state: Mapping[str, Any] | None = None,
|
||||||
|
) -> dict[str, Any]:
|
||||||
|
"""Pull stitch / tracker / filter fields from a sensor.json for export pipeline/meta.
|
||||||
|
|
||||||
|
``filters_state`` is the device's ``Sensor.filters_state()``. Pass it
|
||||||
|
whenever a device is available: a config file states an intent that a runtime
|
||||||
|
``configure()`` may have moved on from, so only the effective values are
|
||||||
|
worth recording. Without it the file's ``filters`` block is reported for what
|
||||||
|
it is — the intent — and values it does not state are left out rather than
|
||||||
|
filled in with a default.
|
||||||
|
|
||||||
|
The result carries the filter settings in one structured ``"filters"`` key
|
||||||
|
(``measurement`` / ``readout`` / ``preset`` / ``source``), not as loose
|
||||||
|
top-level keys: the groups decide whether two records are comparable, so they
|
||||||
|
have to survive a round trip intact.
|
||||||
|
"""
|
||||||
pipeline: dict[str, Any] = {
|
pipeline: dict[str, Any] = {
|
||||||
"stitch_resolution": None,
|
"stitch_resolution": None,
|
||||||
"dis_mode": None,
|
"dis_mode": None,
|
||||||
"hhd_downsample": None,
|
"hhd_downsample": None,
|
||||||
|
"filters": {},
|
||||||
"config_ref": None,
|
"config_ref": None,
|
||||||
}
|
}
|
||||||
if path is None:
|
if path is None and filters_state is None:
|
||||||
return pipeline
|
return pipeline
|
||||||
p = Path(path)
|
p = Path(path) if path is not None else None
|
||||||
|
if p is not None:
|
||||||
pipeline["config_ref"] = str(p)
|
pipeline["config_ref"] = str(p)
|
||||||
if not p.is_file():
|
data: dict[str, Any] = {}
|
||||||
return pipeline
|
if p is not None and p.is_file():
|
||||||
try:
|
try:
|
||||||
data = json.loads(p.read_text(encoding="utf-8"))
|
loaded = json.loads(p.read_text(encoding="utf-8"))
|
||||||
except Exception:
|
except Exception:
|
||||||
return pipeline
|
loaded = None
|
||||||
if not isinstance(data, dict):
|
if isinstance(loaded, dict):
|
||||||
return pipeline
|
data = loaded
|
||||||
camera = data.get("camera") if isinstance(data.get("camera"), dict) else {}
|
camera = data.get("camera") if isinstance(data.get("camera"), dict) else {}
|
||||||
tracker = data.get("tracker") if isinstance(data.get("tracker"), dict) else {}
|
tracker = data.get("tracker") if isinstance(data.get("tracker"), dict) else {}
|
||||||
stitch = camera.get("resolution_stitch")
|
stitch = camera.get("resolution_stitch")
|
||||||
@@ -635,6 +858,12 @@ def pipeline_from_sensor_json(path: str | Path | None) -> dict[str, Any]:
|
|||||||
pipeline["hhd_downsample"] = int(tracker["hhd_downsample"])
|
pipeline["hhd_downsample"] = int(tracker["hhd_downsample"])
|
||||||
except (TypeError, ValueError):
|
except (TypeError, ValueError):
|
||||||
pipeline["hhd_downsample"] = tracker.get("hhd_downsample")
|
pipeline["hhd_downsample"] = tracker.get("hhd_downsample")
|
||||||
|
# Signal conditioning is part of the measurement-pipeline identity: these
|
||||||
|
# stages change the reported force values, so they are recorded next to
|
||||||
|
# dis_mode / hhd_downsample. The effective state wins over the file.
|
||||||
|
pipeline["filters"] = filters_fingerprint(
|
||||||
|
filters_state if filters_state else data.get("filters")
|
||||||
|
)
|
||||||
return pipeline
|
return pipeline
|
||||||
|
|
||||||
|
|
||||||
@@ -720,6 +949,15 @@ def build(
|
|||||||
if k in ("stitch_resolution", "dis_mode", "hhd_downsample", "config_ref"):
|
if k in ("stitch_resolution", "dis_mode", "hhd_downsample", "config_ref"):
|
||||||
if k not in pipeline or pipeline[k] is None:
|
if k not in pipeline or pipeline[k] is None:
|
||||||
pipeline[k] = v
|
pipeline[k] = v
|
||||||
|
elif k == "filters":
|
||||||
|
if not pipeline.get("filters"):
|
||||||
|
pipeline["filters"] = filters_fingerprint(v)
|
||||||
|
elif k in FILTER_KEYS:
|
||||||
|
# Flat form: fold the loose key into the structured groups.
|
||||||
|
merged = deepcopy(pipeline.get("filters") or {})
|
||||||
|
group = "measurement" if k in FILTER_MEASUREMENT_KEYS else "readout"
|
||||||
|
merged.setdefault(group, {})[k] = v
|
||||||
|
pipeline["filters"] = filters_fingerprint(merged)
|
||||||
elif k in ("sensor_type", "sn", "hardware_rev", "gel_id"):
|
elif k in ("sensor_type", "sn", "hardware_rev", "gel_id"):
|
||||||
if k not in identity or not identity[k]:
|
if k not in identity or not identity[k]:
|
||||||
identity[k] = v
|
identity[k] = v
|
||||||
|
|||||||
@@ -0,0 +1,365 @@
|
|||||||
|
"""Record tactile arrow overlays to video without stalling the pipeline.
|
||||||
|
|
||||||
|
Drawing the arrow grid costs a few milliseconds per frame (hundreds of
|
||||||
|
``cv2.arrowedLine`` calls) and encoding costs a few more, so doing it inline
|
||||||
|
would cut a stream's throughput. Instead:
|
||||||
|
|
||||||
|
* :meth:`StreamRecorder.on_frame` is designed to be the pipeline's ``on_frame``
|
||||||
|
callback: it only performs a bounded, drop-oldest queue push, so the slowest
|
||||||
|
pipeline stage is unaffected and the run never blocks on the encoder;
|
||||||
|
* a dedicated writer thread does the drawing and the ``cv2.VideoWriter`` calls;
|
||||||
|
* when the writer falls behind, the newest frames win and the rest are counted
|
||||||
|
in :meth:`StreamRecorder.stats` so a recording gap is never silent.
|
||||||
|
|
||||||
|
Typical use::
|
||||||
|
|
||||||
|
recorder = StreamRecorder("run/stream_0.mp4", label="#0",
|
||||||
|
valid_mask=lambda: runtime.valid_mask)
|
||||||
|
pipe = StreamPipeline(sensor, produce, include=("image", "flow"),
|
||||||
|
on_frame=recorder.on_frame)
|
||||||
|
pipe.start()
|
||||||
|
...
|
||||||
|
print(recorder.close()) # written / dropped / fps
|
||||||
|
|
||||||
|
Per-stream files are the natural unit because each stream is its own process;
|
||||||
|
:func:`mosaic_videos` tiles them into one grid afterwards, which costs nothing
|
||||||
|
during the run.
|
||||||
|
|
||||||
|
Arrow parameters
|
||||||
|
----------------
|
||||||
|
``draw_arrows`` divides the sampled displacement by ``arrow_scale``, so the
|
||||||
|
defaults assume the finger's UV domain, where the active-region displacement is
|
||||||
|
only ~1-3 px: ``grid_spacing=14, arrow_scale=0.15, threshold=0.1`` magnifies it
|
||||||
|
~6.7x, which is what makes the direction field legible instead of a scatter of
|
||||||
|
dots (the ``draw_arrows`` defaults of ``arrow_scale=1.0, threshold=5`` render the
|
||||||
|
raw pixel displacement and are tuned for large-motion scenes). Length saturates
|
||||||
|
at ``grid_spacing * margin`` px, so colour - not length - carries the magnitude.
|
||||||
|
A box/mini stream at 365x365 needs its own calibration of ``arrow_scale``.
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import os
|
||||||
|
import queue
|
||||||
|
import threading
|
||||||
|
import time
|
||||||
|
from typing import Any, Callable, Iterable, Sequence
|
||||||
|
|
||||||
|
import cv2
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from .util import draw_arrows, draw_contact
|
||||||
|
|
||||||
|
__all__ = ["StreamRecorder", "mosaic_videos"]
|
||||||
|
|
||||||
|
|
||||||
|
class StreamRecorder:
|
||||||
|
"""Draw arrow overlays from snapshots and encode them to a video file."""
|
||||||
|
|
||||||
|
def __init__(
|
||||||
|
self,
|
||||||
|
path: str,
|
||||||
|
*,
|
||||||
|
fps: float = 30.0,
|
||||||
|
every: int = 1,
|
||||||
|
fourcc: str = "mp4v",
|
||||||
|
valid_mask: np.ndarray | Callable[[], np.ndarray | None] | None = None,
|
||||||
|
grid_spacing: int = 14,
|
||||||
|
threshold: float = 0.1,
|
||||||
|
arrow_scale: float = 0.15,
|
||||||
|
margin: float = 0.8,
|
||||||
|
draw_contact: bool = False,
|
||||||
|
label: str | None = None,
|
||||||
|
overlay_values: bool = True,
|
||||||
|
queue_depth: int = 8,
|
||||||
|
verbose: bool = False,
|
||||||
|
) -> None:
|
||||||
|
self.path = str(path)
|
||||||
|
self.fps = max(1.0, float(fps))
|
||||||
|
self.every = max(1, int(every))
|
||||||
|
self.fourcc = fourcc
|
||||||
|
# A callable is resolved once per frame: the SDK rebuilds
|
||||||
|
# ``Sensor.valid_mask`` during ``get_img``, so holding an array taken
|
||||||
|
# before the first frame would capture ``None``.
|
||||||
|
self.valid_mask = valid_mask
|
||||||
|
self.grid_spacing = int(grid_spacing)
|
||||||
|
self.threshold = float(threshold)
|
||||||
|
self.arrow_scale = float(arrow_scale)
|
||||||
|
self.margin = float(margin)
|
||||||
|
self.draw_contact_flag = bool(draw_contact)
|
||||||
|
self.label = label
|
||||||
|
self.overlay_values = bool(overlay_values)
|
||||||
|
self.verbose = bool(verbose)
|
||||||
|
|
||||||
|
self._queue: queue.Queue = queue.Queue(maxsize=max(1, int(queue_depth)))
|
||||||
|
self._stop = threading.Event()
|
||||||
|
self._thread: threading.Thread | None = None
|
||||||
|
self._seen = 0
|
||||||
|
self._written = 0
|
||||||
|
self._dropped = 0
|
||||||
|
self._skipped = 0
|
||||||
|
self._error: BaseException | None = None
|
||||||
|
self._writer: cv2.VideoWriter | None = None
|
||||||
|
self._size: tuple[int, int] | None = None
|
||||||
|
|
||||||
|
# ------------------------------------------------------------------ ingest
|
||||||
|
def on_frame(self, snap: Any) -> None:
|
||||||
|
"""Pipeline callback: enqueue one snapshot. Never blocks."""
|
||||||
|
self._seen += 1
|
||||||
|
if self.every > 1 and (self._seen - 1) % self.every:
|
||||||
|
self._skipped += 1
|
||||||
|
return
|
||||||
|
while True:
|
||||||
|
try:
|
||||||
|
self._queue.put_nowait(snap)
|
||||||
|
break
|
||||||
|
except queue.Full:
|
||||||
|
# Drop the oldest so the newest frame is always kept.
|
||||||
|
try:
|
||||||
|
self._queue.get_nowait()
|
||||||
|
self._dropped += 1
|
||||||
|
except queue.Empty:
|
||||||
|
pass
|
||||||
|
self._ensure_thread()
|
||||||
|
|
||||||
|
def _ensure_thread(self) -> None:
|
||||||
|
if self._thread is None:
|
||||||
|
self._thread = threading.Thread(target=self._loop, daemon=True,
|
||||||
|
name="orisys-recorder")
|
||||||
|
self._thread.start()
|
||||||
|
|
||||||
|
# ------------------------------------------------------------------ writer
|
||||||
|
def _open(self, frame: np.ndarray) -> None:
|
||||||
|
h, w = frame.shape[:2]
|
||||||
|
os.makedirs(os.path.dirname(os.path.abspath(self.path)) or ".", exist_ok=True)
|
||||||
|
fourcc = cv2.VideoWriter_fourcc(*self.fourcc) if len(self.fourcc) == 4 else 0
|
||||||
|
writer = cv2.VideoWriter(self.path, fourcc, self.fps, (w, h), True)
|
||||||
|
if not writer.isOpened():
|
||||||
|
raise RuntimeError(
|
||||||
|
f"cv2.VideoWriter could not open {self.path!r} with fourcc={self.fourcc!r}"
|
||||||
|
)
|
||||||
|
self._writer = writer
|
||||||
|
self._size = (w, h)
|
||||||
|
if self.verbose:
|
||||||
|
print(f"[recorder] {self.path} {w}x{h} @ {self.fps:g} fps fourcc={self.fourcc}")
|
||||||
|
|
||||||
|
def _render(self, snap: Any) -> np.ndarray:
|
||||||
|
if snap.image is None or snap.flow is None:
|
||||||
|
raise RuntimeError(
|
||||||
|
"recording needs image and flow in the snapshot; build the pipeline "
|
||||||
|
'with include=("image", "flow")'
|
||||||
|
)
|
||||||
|
mask = self.valid_mask() if callable(self.valid_mask) else self.valid_mask
|
||||||
|
image = draw_arrows(
|
||||||
|
snap.image,
|
||||||
|
snap.flow,
|
||||||
|
threshold=self.threshold,
|
||||||
|
grid_spacing=self.grid_spacing,
|
||||||
|
arrow_scale=self.arrow_scale,
|
||||||
|
margin=self.margin,
|
||||||
|
valid_mask=mask,
|
||||||
|
)
|
||||||
|
if self.draw_contact_flag and snap.contour is not None and snap.centroid is not None:
|
||||||
|
image = draw_contact(image, snap.contour, snap.centroid)
|
||||||
|
if self.overlay_values or self.label:
|
||||||
|
self._draw_hud(image, snap)
|
||||||
|
return image
|
||||||
|
|
||||||
|
def _draw_hud(self, image: np.ndarray, snap: Any) -> None:
|
||||||
|
lines = []
|
||||||
|
if self.label:
|
||||||
|
lines.append(str(self.label))
|
||||||
|
if self.overlay_values:
|
||||||
|
lines.append(f"{snap.unit} fn={snap.normal_force:.3g} "
|
||||||
|
f"fx={snap.shear_x:.3g} fy={snap.shear_y:.3g}")
|
||||||
|
lines.append(f"{snap.fps:.0f} FPS {snap.age_ms:.1f} ms")
|
||||||
|
y = 14
|
||||||
|
for text in lines:
|
||||||
|
cv2.putText(image, text, (4, y), cv2.FONT_HERSHEY_SIMPLEX, 0.38,
|
||||||
|
(0, 0, 0), 3, cv2.LINE_AA)
|
||||||
|
cv2.putText(image, text, (4, y), cv2.FONT_HERSHEY_SIMPLEX, 0.38,
|
||||||
|
(255, 255, 255), 1, cv2.LINE_AA)
|
||||||
|
y += 14
|
||||||
|
|
||||||
|
def _loop(self) -> None:
|
||||||
|
try:
|
||||||
|
while True:
|
||||||
|
try:
|
||||||
|
snap = self._queue.get(timeout=0.05)
|
||||||
|
except queue.Empty:
|
||||||
|
if self._stop.is_set():
|
||||||
|
break
|
||||||
|
continue
|
||||||
|
if snap is None:
|
||||||
|
break
|
||||||
|
try:
|
||||||
|
frame = self._render(snap)
|
||||||
|
if self._writer is None:
|
||||||
|
self._open(frame)
|
||||||
|
self._writer.write(frame)
|
||||||
|
self._written += 1
|
||||||
|
except BaseException as exc:
|
||||||
|
self._error = exc
|
||||||
|
break
|
||||||
|
finally:
|
||||||
|
if self._writer is not None:
|
||||||
|
self._writer.release()
|
||||||
|
self._writer = None
|
||||||
|
|
||||||
|
# ------------------------------------------------------------------- close
|
||||||
|
def close(self, timeout: float = 30.0) -> dict:
|
||||||
|
"""Drain, stop the writer thread and release the file."""
|
||||||
|
if self._thread is not None:
|
||||||
|
self._stop.set()
|
||||||
|
# The stop flag is the real mechanism - the writer drains what is
|
||||||
|
# queued and exits on its next empty poll. The sentinel only makes
|
||||||
|
# that immediate, so it must never block: if the writer died on an
|
||||||
|
# error the queue may be full and nobody would ever drain it.
|
||||||
|
try:
|
||||||
|
self._queue.put_nowait(None)
|
||||||
|
except queue.Full:
|
||||||
|
pass
|
||||||
|
self._thread.join(timeout=timeout)
|
||||||
|
alive = self._thread.is_alive()
|
||||||
|
self._thread = None
|
||||||
|
else:
|
||||||
|
alive = False
|
||||||
|
stats = self.stats()
|
||||||
|
stats["thread_alive"] = alive
|
||||||
|
if self._error is not None:
|
||||||
|
stats["error"] = f"{type(self._error).__name__}: {self._error}"
|
||||||
|
if self.verbose:
|
||||||
|
print(f"[recorder] {self.path}: {stats['error']}")
|
||||||
|
return stats
|
||||||
|
|
||||||
|
def stats(self) -> dict:
|
||||||
|
return {
|
||||||
|
"path": self.path,
|
||||||
|
"seen": self._seen,
|
||||||
|
"written": self._written,
|
||||||
|
"dropped": self._dropped,
|
||||||
|
"skipped": self._skipped,
|
||||||
|
"fps": self.fps,
|
||||||
|
"fourcc": self.fourcc,
|
||||||
|
}
|
||||||
|
|
||||||
|
def __enter__(self) -> "StreamRecorder":
|
||||||
|
return self
|
||||||
|
|
||||||
|
def __exit__(self, exc_type, exc, tb) -> bool:
|
||||||
|
self.close()
|
||||||
|
return False
|
||||||
|
|
||||||
|
|
||||||
|
def mosaic_videos(
|
||||||
|
paths: Sequence[str],
|
||||||
|
out_path: str,
|
||||||
|
*,
|
||||||
|
cols: int | None = None,
|
||||||
|
labels: Iterable[str] | None = None,
|
||||||
|
scale: float = 1.0,
|
||||||
|
gap: int = 4,
|
||||||
|
fps: float | None = None,
|
||||||
|
fourcc: str = "mp4v",
|
||||||
|
) -> dict:
|
||||||
|
"""Tile per-stream recordings into one grid video.
|
||||||
|
|
||||||
|
Post-processing only: run it after the streams have finished, so it costs
|
||||||
|
nothing while they are running. Shorter inputs hold their last frame.
|
||||||
|
"""
|
||||||
|
paths = [str(p) for p in paths]
|
||||||
|
if not paths:
|
||||||
|
raise ValueError("mosaic_videos needs at least one input")
|
||||||
|
for path in paths:
|
||||||
|
if not os.path.isfile(path):
|
||||||
|
raise FileNotFoundError(path)
|
||||||
|
|
||||||
|
caps = [cv2.VideoCapture(p) for p in paths]
|
||||||
|
try:
|
||||||
|
for path, cap in zip(paths, caps):
|
||||||
|
if not cap.isOpened():
|
||||||
|
raise RuntimeError(f"cannot open {path!r}")
|
||||||
|
tiles = []
|
||||||
|
for cap in caps:
|
||||||
|
ok, frame = cap.read()
|
||||||
|
if not ok:
|
||||||
|
raise RuntimeError("an input video has no frames")
|
||||||
|
tiles.append(frame)
|
||||||
|
|
||||||
|
if scale != 1.0:
|
||||||
|
tiles = [cv2.resize(t, None, fx=scale, fy=scale,
|
||||||
|
interpolation=cv2.INTER_AREA) for t in tiles]
|
||||||
|
th, tw = tiles[0].shape[:2]
|
||||||
|
n = len(tiles)
|
||||||
|
cols = int(cols) if cols else int(np.ceil(np.sqrt(n)))
|
||||||
|
cols = max(1, min(cols, n))
|
||||||
|
rows = int(np.ceil(n / cols))
|
||||||
|
label_list = [str(x) for x in labels] if labels is not None else []
|
||||||
|
|
||||||
|
out_h = rows * th + (rows - 1) * gap
|
||||||
|
out_w = cols * tw + (cols - 1) * gap
|
||||||
|
canvas = np.zeros((out_h, out_w, 3), dtype=np.uint8)
|
||||||
|
positions = []
|
||||||
|
for i in range(n):
|
||||||
|
r, c = divmod(i, cols)
|
||||||
|
positions.append((r * (th + gap), c * (tw + gap)))
|
||||||
|
# Frame 0 is already in hand; seed the canvas with it so the output is
|
||||||
|
# never short by one frame.
|
||||||
|
for i, tile in enumerate(tiles):
|
||||||
|
y, x = positions[i]
|
||||||
|
canvas[y:y + th, x:x + tw] = tile
|
||||||
|
|
||||||
|
out_fps = float(fps) if fps else float(caps[0].get(cv2.CAP_PROP_FPS) or 30.0)
|
||||||
|
os.makedirs(os.path.dirname(os.path.abspath(out_path)) or ".", exist_ok=True)
|
||||||
|
writer = cv2.VideoWriter(out_path, cv2.VideoWriter_fourcc(*fourcc), out_fps,
|
||||||
|
(out_w, out_h), True)
|
||||||
|
if not writer.isOpened():
|
||||||
|
raise RuntimeError(f"cannot open {out_path!r} for writing")
|
||||||
|
|
||||||
|
def emit() -> None:
|
||||||
|
# Labels go on the canvas, not on the tiles: the read loop below
|
||||||
|
# overwrites each tile region every frame.
|
||||||
|
for i, text in enumerate(label_list):
|
||||||
|
if i >= n:
|
||||||
|
break
|
||||||
|
y, x = positions[i]
|
||||||
|
org = (x + 4, y + th - 6)
|
||||||
|
cv2.putText(canvas, text, org, cv2.FONT_HERSHEY_SIMPLEX, 0.45,
|
||||||
|
(0, 0, 0), 3, cv2.LINE_AA)
|
||||||
|
cv2.putText(canvas, text, org, cv2.FONT_HERSHEY_SIMPLEX, 0.45,
|
||||||
|
(255, 255, 255), 1, cv2.LINE_AA)
|
||||||
|
writer.write(canvas)
|
||||||
|
|
||||||
|
written = 0
|
||||||
|
try:
|
||||||
|
# One frame per input per iteration; a spent input keeps its last
|
||||||
|
# frame so the grid stays full until every input has run out.
|
||||||
|
while True:
|
||||||
|
emit()
|
||||||
|
written += 1
|
||||||
|
progressed = False
|
||||||
|
for i, cap in enumerate(caps):
|
||||||
|
ok, frame = cap.read()
|
||||||
|
if not ok:
|
||||||
|
continue
|
||||||
|
progressed = True
|
||||||
|
if scale != 1.0:
|
||||||
|
frame = cv2.resize(frame, (tw, th), interpolation=cv2.INTER_AREA)
|
||||||
|
y, x = positions[i]
|
||||||
|
canvas[y:y + th, x:x + tw] = frame
|
||||||
|
if not progressed:
|
||||||
|
break
|
||||||
|
finally:
|
||||||
|
writer.release()
|
||||||
|
finally:
|
||||||
|
for cap in caps:
|
||||||
|
cap.release()
|
||||||
|
|
||||||
|
return {
|
||||||
|
"path": out_path,
|
||||||
|
"frames": written,
|
||||||
|
"fps": out_fps,
|
||||||
|
"size": (out_w, out_h),
|
||||||
|
"grid": (rows, cols),
|
||||||
|
"scale": scale,
|
||||||
|
}
|
||||||
@@ -293,6 +293,15 @@ class FingerRuntime:
|
|||||||
capture = capture_source.cap
|
capture = capture_source.cap
|
||||||
|
|
||||||
sensor = Sensor("external", config_name=config, verbose=verbose, mirror=mirror)
|
sensor = Sensor("external", config_name=config, verbose=verbose, mirror=mirror)
|
||||||
|
if not is_live_camera(video):
|
||||||
|
# File playback: wall-clock cadence is not signal time, so pin the
|
||||||
|
# force-filter design rate to the configured camera fps.
|
||||||
|
try:
|
||||||
|
fps = float(geometry.config.get("camera", {}).get("fps", 0.0) or 0.0)
|
||||||
|
except (TypeError, ValueError):
|
||||||
|
fps = 0.0
|
||||||
|
if fps > 0:
|
||||||
|
sensor.configure(lowpass_sample_rate_hz=fps)
|
||||||
frame_processor = FrameUnwarper(
|
frame_processor = FrameUnwarper(
|
||||||
domain.remap,
|
domain.remap,
|
||||||
camera_matrix=geometry.source_K if geometry.dist_coeffs is not None else None,
|
camera_matrix=geometry.source_K if geometry.dist_coeffs is not None else None,
|
||||||
|
|||||||
@@ -1,5 +1,7 @@
|
|||||||
from __future__ import annotations
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import time
|
||||||
|
|
||||||
import cv2
|
import cv2
|
||||||
import numpy as np
|
import numpy as np
|
||||||
from pathlib import Path
|
from pathlib import Path
|
||||||
@@ -12,11 +14,24 @@ class CompoundEyeFrameProcessor:
|
|||||||
|
|
||||||
def __init__(self, stitcher):
|
def __init__(self, stitcher):
|
||||||
self.stitcher = stitcher
|
self.stitcher = stitcher
|
||||||
|
# Per-call sub-stage timings (seconds), read by Sensor.get_img() for
|
||||||
|
# _last_stage_times. Keys: units, extract_tiles, stitch.
|
||||||
|
self.last_timings = {}
|
||||||
|
|
||||||
def process(self, frame):
|
def process(self, frame):
|
||||||
|
t0 = time.perf_counter()
|
||||||
self.stitcher.get_stitch_unit(frame)
|
self.stitcher.get_stitch_unit(frame)
|
||||||
|
t1 = time.perf_counter()
|
||||||
self.stitcher.extract_tiles()
|
self.stitcher.extract_tiles()
|
||||||
return self.stitcher.stitch_rt_optimized()
|
t2 = time.perf_counter()
|
||||||
|
stitched = self.stitcher.stitch_rt_optimized()
|
||||||
|
t3 = time.perf_counter()
|
||||||
|
self.last_timings = {
|
||||||
|
"units": t1 - t0,
|
||||||
|
"extract_tiles": t2 - t1,
|
||||||
|
"stitch": t3 - t2,
|
||||||
|
}
|
||||||
|
return stitched
|
||||||
|
|
||||||
|
|
||||||
class SingleCameraFrameProcessor:
|
class SingleCameraFrameProcessor:
|
||||||
|
|||||||
@@ -73,8 +73,49 @@ class nHHD(object):
|
|||||||
raise ValueError("nHHD Solver works only for 2 and 3 dimensions")
|
raise ValueError("nHHD Solver works only for 2 and 3 dimensions")
|
||||||
|
|
||||||
self._force_cpu_cache = None
|
self._force_cpu_cache = None
|
||||||
|
self._force_ref_shape = None
|
||||||
|
ref_shape = kwargs.get('force_reference_grid')
|
||||||
|
if ref_shape is not None:
|
||||||
|
try:
|
||||||
|
ref_h, ref_w = int(ref_shape[0]), int(ref_shape[1])
|
||||||
|
except (TypeError, ValueError, IndexError):
|
||||||
|
ref_h = ref_w = 0
|
||||||
|
if ref_h > 0 and ref_w > 0:
|
||||||
|
self._force_ref_shape = (ref_h, ref_w)
|
||||||
self.psolver.prepare(False)
|
self.psolver.prepare(False)
|
||||||
|
|
||||||
|
def _force_scale(self, shape):
|
||||||
|
"""Area/linear weights mapping grid sums at ``shape`` to the reference grid.
|
||||||
|
|
||||||
|
``Sn``/``Stx``/``Sty``/``Stor`` are *sums over grid pixels*, and the
|
||||||
|
torque term additionally uses pixel-index moment arms. When the flow is
|
||||||
|
downsampled by ``hhd_downsample=k`` before decomposition, the same
|
||||||
|
physical deformation therefore reports roughly ``1/k^2`` of the force
|
||||||
|
magnitude (and ``1/k^3`` of the torque) unless the sums are rescaled
|
||||||
|
back onto the reference grid.
|
||||||
|
|
||||||
|
returns: (area_scale, linear_scale); (1.0, 1.0) when no reference grid
|
||||||
|
was configured or the current grid already is the reference.
|
||||||
|
"""
|
||||||
|
ref = self._force_ref_shape
|
||||||
|
if ref is None or (int(shape[0]), int(shape[1])) == ref:
|
||||||
|
return 1.0, 1.0
|
||||||
|
cur_area = float(shape[0]) * float(shape[1])
|
||||||
|
if cur_area <= 0.0:
|
||||||
|
return 1.0, 1.0
|
||||||
|
area_scale = (float(ref[0]) * float(ref[1])) / cur_area
|
||||||
|
return area_scale, float(numpy.sqrt(area_scale))
|
||||||
|
|
||||||
|
def _apply_force_scale(self, shape):
|
||||||
|
"""Rescale aggregate force sums from a downsampled grid to the reference grid."""
|
||||||
|
area_scale, linear_scale = self._force_scale(shape)
|
||||||
|
if area_scale == 1.0 and linear_scale == 1.0:
|
||||||
|
return
|
||||||
|
self.Sn = float(self.Sn) * area_scale
|
||||||
|
self.Stx = float(self.Stx) * area_scale
|
||||||
|
self.Sty = float(self.Sty) * area_scale
|
||||||
|
self.Stor = float(self.Stor) * area_scale * linear_scale
|
||||||
|
|
||||||
def _normalize_mask(self, valid_mask, shape):
|
def _normalize_mask(self, valid_mask, shape):
|
||||||
if valid_mask is None:
|
if valid_mask is None:
|
||||||
return None
|
return None
|
||||||
@@ -402,6 +443,8 @@ class nHHD(object):
|
|||||||
cross_n = numpy.where(mask, cross_n, 0)
|
cross_n = numpy.where(mask, cross_n, 0)
|
||||||
self.Stor = numpy.sum(cross_p) + numpy.sum(cross_n)
|
self.Stor = numpy.sum(cross_p) + numpy.sum(cross_n)
|
||||||
|
|
||||||
|
self._apply_force_scale(shape)
|
||||||
|
|
||||||
# Optimized: pre-allocate output array with explicit dtype
|
# Optimized: pre-allocate output array with explicit dtype
|
||||||
output = numpy.array([self.Sn, self.Stx, self.Sty, self.Stor], dtype=numpy.float32)
|
output = numpy.array([self.Sn, self.Stx, self.Sty, self.Stor], dtype=numpy.float32)
|
||||||
|
|
||||||
@@ -515,6 +558,8 @@ class nHHD(object):
|
|||||||
))
|
))
|
||||||
|
|
||||||
self.Sn, self.Stx, self.Sty, self.Stor = Sn, Stx, Sty, Stor
|
self.Sn, self.Stx, self.Sty, self.Stor = Sn, Stx, Sty, Stor
|
||||||
|
self._apply_force_scale(shape)
|
||||||
|
Sn, Stx, Sty, Stor = self.Sn, self.Stx, self.Sty, self.Stor
|
||||||
output = numpy.array([Sn, Stx, Sty, Stor], dtype=numpy.float32)
|
output = numpy.array([Sn, Stx, Sty, Stor], dtype=numpy.float32)
|
||||||
|
|
||||||
# Keep stable CPU copies for get_normal()/get_shear() so callers get this frame's
|
# Keep stable CPU copies for get_normal()/get_shear() so callers get this frame's
|
||||||
|
|||||||
+59
-10
@@ -29,6 +29,39 @@ def compute_boundary_mask(active):
|
|||||||
return active & ~fully_surrounded
|
return active & ~fully_surrounded
|
||||||
|
|
||||||
|
|
||||||
|
def build_masked_factor(lap):
|
||||||
|
"""Factorize the masked Laplacian for fast repeated triangular solves.
|
||||||
|
|
||||||
|
The reduced system is a symmetric M-matrix (graph Laplacian of the active
|
||||||
|
pixels with an extra Dirichlet pin), so SuperLU can skip partial pivoting
|
||||||
|
(``SymmetricMode`` + ``diag_pivot_thresh=0``), which is ~2x faster than the
|
||||||
|
default pivoting factorization. Factoring in float32 halves the memory
|
||||||
|
traffic of the triangular solves; relative error against the float64
|
||||||
|
reference stays around 1e-7 for these systems.
|
||||||
|
|
||||||
|
``rhs_dtype`` is returned because ``SuperLU.solve`` refuses to cast a
|
||||||
|
float64 right-hand side down to a float32 factor.
|
||||||
|
|
||||||
|
lap: (N, N) symmetric sparse Laplacian (any scipy dtype)
|
||||||
|
returns: (solve_callable, rhs_dtype)
|
||||||
|
"""
|
||||||
|
lap_csc = lap.tocsc()
|
||||||
|
lap32 = lap_csc.astype(numpy.float32, copy=False)
|
||||||
|
for diag_pivot_thresh in (0.0, 1.0):
|
||||||
|
try:
|
||||||
|
lu = sparse_linalg.splu(
|
||||||
|
lap32,
|
||||||
|
permc_spec="MMD_AT_PLUS_A",
|
||||||
|
diag_pivot_thresh=diag_pivot_thresh,
|
||||||
|
options=dict(SymmetricMode=True),
|
||||||
|
)
|
||||||
|
return lu.solve, numpy.float32
|
||||||
|
except Exception:
|
||||||
|
continue
|
||||||
|
# Last resort: the original pivoting LU on the float64 matrix.
|
||||||
|
return sparse_linalg.factorized(lap_csc), numpy.float64
|
||||||
|
|
||||||
|
|
||||||
def sample_boundary_coords(boundary_coords, config: MaskedPoissonConfig):
|
def sample_boundary_coords(boundary_coords, config: MaskedPoissonConfig):
|
||||||
"""Subsample boundary pixel coordinates for capacitance correction."""
|
"""Subsample boundary pixel coordinates for capacitance correction."""
|
||||||
if boundary_coords.size == 0:
|
if boundary_coords.size == 0:
|
||||||
@@ -53,6 +86,7 @@ class MaskedDomain:
|
|||||||
"mask",
|
"mask",
|
||||||
"index_map",
|
"index_map",
|
||||||
"factor",
|
"factor",
|
||||||
|
"factor_dtype",
|
||||||
"lap",
|
"lap",
|
||||||
"num_active",
|
"num_active",
|
||||||
"pinned",
|
"pinned",
|
||||||
@@ -75,11 +109,13 @@ class MaskedDomain:
|
|||||||
pinned,
|
pinned,
|
||||||
active_coords,
|
active_coords,
|
||||||
active_linear_indices,
|
active_linear_indices,
|
||||||
|
factor_dtype=numpy.float32,
|
||||||
):
|
):
|
||||||
self.shape = shape
|
self.shape = shape
|
||||||
self.mask = mask
|
self.mask = mask
|
||||||
self.index_map = index_map
|
self.index_map = index_map
|
||||||
self.factor = factor
|
self.factor = factor
|
||||||
|
self.factor_dtype = numpy.dtype(factor_dtype)
|
||||||
self.lap = lap
|
self.lap = lap
|
||||||
self.num_active = num_active
|
self.num_active = num_active
|
||||||
self.pinned = pinned
|
self.pinned = pinned
|
||||||
@@ -156,7 +192,7 @@ class MaskedDomain:
|
|||||||
data.append(diag)
|
data.append(diag)
|
||||||
|
|
||||||
lap = sparse.csc_matrix((data, (rows, cols)), shape=(num_active, num_active), dtype=numpy.float64)
|
lap = sparse.csc_matrix((data, (rows, cols)), shape=(num_active, num_active), dtype=numpy.float64)
|
||||||
factor = sparse_linalg.factorized(lap)
|
factor, factor_dtype = build_masked_factor(lap)
|
||||||
return cls(
|
return cls(
|
||||||
shape=active.shape,
|
shape=active.shape,
|
||||||
mask=active.copy(),
|
mask=active.copy(),
|
||||||
@@ -167,6 +203,7 @@ class MaskedDomain:
|
|||||||
pinned=pinned,
|
pinned=pinned,
|
||||||
active_coords=active_coords,
|
active_coords=active_coords,
|
||||||
active_linear_indices=active_linear_indices,
|
active_linear_indices=active_linear_indices,
|
||||||
|
factor_dtype=factor_dtype,
|
||||||
)
|
)
|
||||||
|
|
||||||
def matches(self, mask):
|
def matches(self, mask):
|
||||||
@@ -179,13 +216,20 @@ class MaskedDomain:
|
|||||||
|
|
||||||
f: (H, W) scalar source field
|
f: (H, W) scalar source field
|
||||||
returns: (N,) RHS on active pixels; empty if no active pixels
|
returns: (N,) RHS on active pixels; empty if no active pixels
|
||||||
|
|
||||||
|
The returned dtype matches the factorization (see ``factor_dtype``):
|
||||||
|
``SuperLU.solve`` will not downcast a float64 RHS for a float32 factor.
|
||||||
"""
|
"""
|
||||||
if self.num_active == 0:
|
if self.num_active == 0:
|
||||||
return numpy.zeros(0, dtype=numpy.float64)
|
return numpy.zeros(0, dtype=self.factor_dtype)
|
||||||
if self._rhs_buffer is None or self._rhs_buffer.shape != (self.num_active,):
|
if (
|
||||||
self._rhs_buffer = numpy.empty(self.num_active, dtype=numpy.float64)
|
self._rhs_buffer is None
|
||||||
|
or self._rhs_buffer.shape != (self.num_active,)
|
||||||
|
or self._rhs_buffer.dtype != self.factor_dtype
|
||||||
|
):
|
||||||
|
self._rhs_buffer = numpy.empty(self.num_active, dtype=self.factor_dtype)
|
||||||
numpy.take(
|
numpy.take(
|
||||||
numpy.asarray(f, dtype=numpy.float64).ravel(),
|
numpy.asarray(f, dtype=self.factor_dtype).ravel(),
|
||||||
self.active_linear_indices,
|
self.active_linear_indices,
|
||||||
out=self._rhs_buffer,
|
out=self._rhs_buffer,
|
||||||
)
|
)
|
||||||
@@ -197,11 +241,15 @@ class MaskedDomain:
|
|||||||
def prepare_rhs_pair(self, f0, f1):
|
def prepare_rhs_pair(self, f0, f1):
|
||||||
"""Extract two gauge-fixed RHS vectors into one reusable (N, 2) buffer."""
|
"""Extract two gauge-fixed RHS vectors into one reusable (N, 2) buffer."""
|
||||||
if self.num_active == 0:
|
if self.num_active == 0:
|
||||||
return numpy.zeros((0, 2), dtype=numpy.float64)
|
return numpy.zeros((0, 2), dtype=self.factor_dtype)
|
||||||
if self._rhs_pair_buffer is None or self._rhs_pair_buffer.shape != (self.num_active, 2):
|
if (
|
||||||
self._rhs_pair_buffer = numpy.empty((self.num_active, 2), dtype=numpy.float64)
|
self._rhs_pair_buffer is None
|
||||||
flat0 = numpy.asarray(f0, dtype=numpy.float64).ravel()
|
or self._rhs_pair_buffer.shape != (self.num_active, 2)
|
||||||
flat1 = numpy.asarray(f1, dtype=numpy.float64).ravel()
|
or self._rhs_pair_buffer.dtype != self.factor_dtype
|
||||||
|
):
|
||||||
|
self._rhs_pair_buffer = numpy.empty((self.num_active, 2), dtype=self.factor_dtype)
|
||||||
|
flat0 = numpy.asarray(f0, dtype=self.factor_dtype).ravel()
|
||||||
|
flat1 = numpy.asarray(f1, dtype=self.factor_dtype).ravel()
|
||||||
numpy.take(flat0, self.active_linear_indices, out=self._rhs_pair_buffer[:, 0])
|
numpy.take(flat0, self.active_linear_indices, out=self._rhs_pair_buffer[:, 0])
|
||||||
numpy.take(flat1, self.active_linear_indices, out=self._rhs_pair_buffer[:, 1])
|
numpy.take(flat1, self.active_linear_indices, out=self._rhs_pair_buffer[:, 1])
|
||||||
self._rhs_pair_buffer -= self._rhs_pair_buffer.mean(axis=0, keepdims=True)
|
self._rhs_pair_buffer -= self._rhs_pair_buffer.mean(axis=0, keepdims=True)
|
||||||
@@ -255,6 +303,7 @@ class MaskedDomain:
|
|||||||
"mask": self.mask,
|
"mask": self.mask,
|
||||||
"index_map": self.index_map,
|
"index_map": self.index_map,
|
||||||
"factor": self.factor,
|
"factor": self.factor,
|
||||||
|
"factor_dtype": self.factor_dtype,
|
||||||
"lap": self.lap,
|
"lap": self.lap,
|
||||||
"num_active": self.num_active,
|
"num_active": self.num_active,
|
||||||
"pinned": self.pinned,
|
"pinned": self.pinned,
|
||||||
|
|||||||
@@ -33,7 +33,11 @@ class MaskedSparseSolver:
|
|||||||
try:
|
try:
|
||||||
sol_pair = -domain.factor(rhs_pair) # (N, 2) reduced-system potentials
|
sol_pair = -domain.factor(rhs_pair) # (N, 2) reduced-system potentials
|
||||||
except Exception:
|
except Exception:
|
||||||
|
# Some factorizations need contiguous 1-D right-hand sides.
|
||||||
sol_pair = -numpy.column_stack(
|
sol_pair = -numpy.column_stack(
|
||||||
[domain.factor(rhs_pair[:, 0]), domain.factor(rhs_pair[:, 1])]
|
[
|
||||||
|
domain.factor(numpy.ascontiguousarray(rhs_pair[:, 0])),
|
||||||
|
domain.factor(numpy.ascontiguousarray(rhs_pair[:, 1])),
|
||||||
|
]
|
||||||
)
|
)
|
||||||
return domain.scatter_pair(sol_pair, f0)
|
return domain.scatter_pair(sol_pair, f0)
|
||||||
|
|||||||
@@ -0,0 +1,465 @@
|
|||||||
|
"""Three-stage per-stream pipeline for the SDK's per-frame work.
|
||||||
|
|
||||||
|
A single ``Sensor`` frame costs the sum of three stages::
|
||||||
|
|
||||||
|
produce -> capture + (stitch | UV unroll) + grey/resize (Sensor.get_img)
|
||||||
|
flow -> DIS optical flow + forward/backward + loop closure
|
||||||
|
hhd -> Helmholtz-Hodge decomposition + force readout (+ contact)
|
||||||
|
|
||||||
|
Running them back to back in one thread bounds throughput by that *sum*. Running
|
||||||
|
them as three concurrent threads bounds it by the *slowest stage* only, which is
|
||||||
|
what makes multi-sensor deployments viable: measured on a Ryzen 9 9950X, five
|
||||||
|
finger streams go from 60.8 to 125.2 FPS per stream, and five box streams from
|
||||||
|
39.1 to 57.6 FPS per stream.
|
||||||
|
|
||||||
|
Usage (finger)::
|
||||||
|
|
||||||
|
with orisys.StreamPipeline.for_finger_runtime(runtime) as pipe:
|
||||||
|
while True:
|
||||||
|
snap = pipe.wait()
|
||||||
|
if snap is None:
|
||||||
|
break
|
||||||
|
print(snap.normal_force, snap.shear_x, snap.shear_y)
|
||||||
|
|
||||||
|
Usage (box / mini)::
|
||||||
|
|
||||||
|
with orisys.StreamPipeline(sensor, sensor.get_img) as pipe:
|
||||||
|
...
|
||||||
|
|
||||||
|
Data ownership
|
||||||
|
--------------
|
||||||
|
The SDK reuses its working buffers between frames (``Sensor.img`` is
|
||||||
|
``_img_gray_buf``, ``Sensor.flow`` may be the flow-EMA buffer, ``Sensor.vnormal``
|
||||||
|
is the HHD upsample buffer, and the finger remap output buffer is reused too), so
|
||||||
|
each stage must not hand a bare reference to the next one:
|
||||||
|
|
||||||
|
* the producer copies the frame it publishes, because the next ``get_img`` call
|
||||||
|
overwrites the SDK buffer in place;
|
||||||
|
* the flow stage takes ownership of ``Sensor.img`` for the duration of the frame,
|
||||||
|
which is why the producer writes its own copy rather than letting the SDK
|
||||||
|
buffer be read while being rewritten;
|
||||||
|
* the flow stage also copies ``Sensor.flow`` before handing the frame on, because
|
||||||
|
``Sensor.compute_hhd`` reads ``self.flow`` - and with the flow-EMA enabled that
|
||||||
|
attribute is a buffer the next frame rewrites. Handing the copy forward and
|
||||||
|
restoring ``Sensor.flow`` in the HHD stage is what keeps each frame's forces
|
||||||
|
paired with that frame's flow;
|
||||||
|
* the HHD stage owns every array that goes into a :class:`FrameSnapshot` (the
|
||||||
|
frame copy is already private), so a published snapshot is immutable and safe
|
||||||
|
to hand to a GUI thread.
|
||||||
|
|
||||||
|
``Sensor.valid_mask`` is still rebuilt by the producer; it is geometry that does
|
||||||
|
not change between frames in practice, so the stages only ever read it. A sensor
|
||||||
|
whose mask changes every frame would need the SDK to expose a frame-scoped mask.
|
||||||
|
|
||||||
|
Cost: one grey-frame copy (~0.1 MB) plus one flow copy (~1 MB at 365x365) per
|
||||||
|
frame, roughly 0.1 ms - about 0.02 cores of a 32-thread machine at five streams.
|
||||||
|
|
||||||
|
Latency
|
||||||
|
-------
|
||||||
|
With the queues empty (a real camera at its own frame rate) the end-to-end
|
||||||
|
capture-to-force delay is the sum of the three stage times. ``depth`` bounds how
|
||||||
|
many frames may be in flight; a larger depth absorbs jitter at the cost of
|
||||||
|
latency when the producer outruns the consumer.
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import threading
|
||||||
|
import time
|
||||||
|
from dataclasses import dataclass, field
|
||||||
|
from queue import Empty, Full, Queue
|
||||||
|
from typing import Any, Callable, Iterable, Sequence
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
__all__ = ["FrameSnapshot", "PipelineStats", "StreamPipeline"]
|
||||||
|
|
||||||
|
#: Array fields a snapshot can carry. ``image`` is the working grey frame; the
|
||||||
|
#: rest come from ``Sensor.read_info`` (already copied by the SDK).
|
||||||
|
_ARRAY_FIELDS = ("image", "flow", "vnormal", "vshear", "depth", "centroid", "contour")
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class FrameSnapshot:
|
||||||
|
"""An immutable view of one finished frame.
|
||||||
|
|
||||||
|
Scalars are always populated; arrays are ``None`` unless requested through
|
||||||
|
``StreamPipeline(include=...)``.
|
||||||
|
"""
|
||||||
|
|
||||||
|
seq: int
|
||||||
|
timestamp: float
|
||||||
|
age_ms: float
|
||||||
|
fps: float
|
||||||
|
unit: str
|
||||||
|
normal_force: float
|
||||||
|
shear_x: float
|
||||||
|
shear_y: float
|
||||||
|
normal_force_raw: float
|
||||||
|
shear_x_raw: float
|
||||||
|
shear_y_raw: float
|
||||||
|
torque: float
|
||||||
|
image: np.ndarray | None = None
|
||||||
|
flow: np.ndarray | None = None
|
||||||
|
vnormal: np.ndarray | None = None
|
||||||
|
vshear: np.ndarray | None = None
|
||||||
|
depth: np.ndarray | None = None
|
||||||
|
centroid: np.ndarray | None = None
|
||||||
|
contour: np.ndarray | None = None
|
||||||
|
produce_ms: float = 0.0
|
||||||
|
flow_ms: float = 0.0
|
||||||
|
hhd_ms: float = 0.0
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass
|
||||||
|
class PipelineStats:
|
||||||
|
"""Rolling diagnostics for one stream."""
|
||||||
|
|
||||||
|
produced: int = 0
|
||||||
|
published: int = 0
|
||||||
|
produce_ms: float = 0.0
|
||||||
|
flow_ms: float = 0.0
|
||||||
|
hhd_ms: float = 0.0
|
||||||
|
age_ms: float = 0.0
|
||||||
|
age_p95_ms: float = 0.0
|
||||||
|
fps: float = 0.0
|
||||||
|
_ages: list = field(default_factory=list, repr=False)
|
||||||
|
|
||||||
|
def as_dict(self) -> dict:
|
||||||
|
return {
|
||||||
|
"produced": self.produced,
|
||||||
|
"published": self.published,
|
||||||
|
"fps": round(self.fps, 2),
|
||||||
|
"produce_ms": round(self.produce_ms, 3),
|
||||||
|
"flow_ms": round(self.flow_ms, 3),
|
||||||
|
"hhd_ms": round(self.hhd_ms, 3),
|
||||||
|
"age_ms": round(self.age_ms, 3),
|
||||||
|
"age_p95_ms": round(self.age_p95_ms, 3),
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
class StreamPipeline:
|
||||||
|
"""Run ``produce -> flow -> hhd`` as three concurrent stages.
|
||||||
|
|
||||||
|
``produce`` is a zero-argument callable returning either the working frame
|
||||||
|
(``(H, W)`` grey or ``(H, W, 3)`` BGR) or ``None`` to signal end of input.
|
||||||
|
For a ``Sensor`` that is ``sensor.get_img``; for a ``FingerRuntime`` use
|
||||||
|
:meth:`for_finger_runtime`.
|
||||||
|
"""
|
||||||
|
|
||||||
|
def __init__(
|
||||||
|
self,
|
||||||
|
sensor: Any,
|
||||||
|
produce: Callable[[], np.ndarray | None],
|
||||||
|
*,
|
||||||
|
contact: bool = False,
|
||||||
|
depth: int = 2,
|
||||||
|
include: Sequence[str] | Iterable[str] = ("image",),
|
||||||
|
on_frame: Callable[["FrameSnapshot"], None] | None = None,
|
||||||
|
verbose: bool = False,
|
||||||
|
fps_window: int = 30,
|
||||||
|
) -> None:
|
||||||
|
unknown = set(include) - set(_ARRAY_FIELDS)
|
||||||
|
if unknown:
|
||||||
|
raise ValueError(
|
||||||
|
f"unknown include fields {sorted(unknown)}; expected {list(_ARRAY_FIELDS)}"
|
||||||
|
)
|
||||||
|
self.sensor = sensor
|
||||||
|
self._produce = produce
|
||||||
|
self._contact = bool(contact)
|
||||||
|
self._depth = max(1, int(depth))
|
||||||
|
self._include = tuple(dict.fromkeys(include))
|
||||||
|
self._on_frame = on_frame
|
||||||
|
self._verbose = bool(verbose)
|
||||||
|
self._fps_window = max(2, int(fps_window))
|
||||||
|
|
||||||
|
self._q1: Queue = Queue(maxsize=self._depth)
|
||||||
|
self._q2: Queue = Queue(maxsize=self._depth)
|
||||||
|
self._done = threading.Event()
|
||||||
|
self._threads: list[threading.Thread] = []
|
||||||
|
self._error: BaseException | None = None
|
||||||
|
self._running = False
|
||||||
|
|
||||||
|
self._lock = threading.Lock()
|
||||||
|
self._latest: FrameSnapshot | None = None
|
||||||
|
self._published = 0
|
||||||
|
self._wait_seq = 0
|
||||||
|
self._stamps: list[float] = []
|
||||||
|
self.stats = PipelineStats()
|
||||||
|
|
||||||
|
# ------------------------------------------------------------------ ctors
|
||||||
|
@classmethod
|
||||||
|
def for_finger_runtime(cls, runtime: Any, **kwargs: Any) -> "StreamPipeline":
|
||||||
|
"""Build a pipeline for a ``FingerRuntime`` (capture + UV unroll stage)."""
|
||||||
|
|
||||||
|
def produce() -> np.ndarray | None:
|
||||||
|
ok, frame = runtime.grab_and_process(color=False)
|
||||||
|
return frame if ok else None
|
||||||
|
|
||||||
|
return cls(runtime.tactile.raw_sensor, produce, **kwargs)
|
||||||
|
|
||||||
|
# --------------------------------------------------------------- lifecycle
|
||||||
|
def start(self) -> "StreamPipeline":
|
||||||
|
if self._running:
|
||||||
|
return self
|
||||||
|
self._running = True
|
||||||
|
self._done.clear()
|
||||||
|
self._error = None
|
||||||
|
with self._lock:
|
||||||
|
self._wait_seq = self._published
|
||||||
|
self._threads = [
|
||||||
|
threading.Thread(target=self._guard, args=(self._producer_loop,),
|
||||||
|
name="orisys-pipeline-produce", daemon=True),
|
||||||
|
threading.Thread(target=self._guard, args=(self._flow_loop,),
|
||||||
|
name="orisys-pipeline-flow", daemon=True),
|
||||||
|
threading.Thread(target=self._guard, args=(self._hhd_loop,),
|
||||||
|
name="orisys-pipeline-hhd", daemon=True),
|
||||||
|
]
|
||||||
|
for t in self._threads:
|
||||||
|
t.start()
|
||||||
|
return self
|
||||||
|
|
||||||
|
def stop(self, timeout: float = 5.0) -> None:
|
||||||
|
"""Stop all stages and join them. Re-raises a stage exception, if any."""
|
||||||
|
self._done.set()
|
||||||
|
self._drain(self._q1)
|
||||||
|
self._drain(self._q2)
|
||||||
|
for t in self._threads:
|
||||||
|
t.join(timeout=timeout)
|
||||||
|
self._threads = []
|
||||||
|
self._running = False
|
||||||
|
if self._error is not None:
|
||||||
|
err, self._error = self._error, None
|
||||||
|
raise err
|
||||||
|
|
||||||
|
def __enter__(self) -> "StreamPipeline":
|
||||||
|
return self.start()
|
||||||
|
|
||||||
|
def __exit__(self, exc_type, exc, tb) -> bool:
|
||||||
|
try:
|
||||||
|
self.stop()
|
||||||
|
except BaseException:
|
||||||
|
if exc_type is None:
|
||||||
|
raise
|
||||||
|
return False
|
||||||
|
|
||||||
|
# ------------------------------------------------------------------- state
|
||||||
|
@property
|
||||||
|
def running(self) -> bool:
|
||||||
|
return self._running and any(t.is_alive() for t in self._threads)
|
||||||
|
|
||||||
|
@property
|
||||||
|
def error(self) -> BaseException | None:
|
||||||
|
return self._error
|
||||||
|
|
||||||
|
def latest(self) -> FrameSnapshot | None:
|
||||||
|
"""Most recent snapshot, or ``None`` if none was published yet."""
|
||||||
|
with self._lock:
|
||||||
|
return self._latest
|
||||||
|
|
||||||
|
def wait(self, timeout: float | None = None) -> FrameSnapshot | None:
|
||||||
|
"""Block until a snapshot newer than the last returned one exists.
|
||||||
|
|
||||||
|
This is a *latest-wins* view: if the stages outrun the caller, skipped
|
||||||
|
frames are gone, which is what a live GUI/control loop wants. Use
|
||||||
|
``on_frame`` when every frame must be handled exactly once.
|
||||||
|
|
||||||
|
Returns ``None`` on timeout, or once the pipeline has ended without a
|
||||||
|
newer frame.
|
||||||
|
"""
|
||||||
|
with self._lock:
|
||||||
|
seen = self._published
|
||||||
|
deadline = None if timeout is None else time.perf_counter() + timeout
|
||||||
|
while True:
|
||||||
|
with self._lock:
|
||||||
|
if self._published > self._wait_seq and self._latest is not None:
|
||||||
|
self._wait_seq = self._published
|
||||||
|
return self._latest
|
||||||
|
if self._error is not None:
|
||||||
|
err, self._error = self._error, None
|
||||||
|
raise err
|
||||||
|
if not self.running:
|
||||||
|
with self._lock:
|
||||||
|
if self._published > self._wait_seq and self._latest is not None:
|
||||||
|
self._wait_seq = self._published
|
||||||
|
return self._latest
|
||||||
|
return None
|
||||||
|
if deadline is not None and time.perf_counter() >= deadline:
|
||||||
|
return None
|
||||||
|
time.sleep(0.0005)
|
||||||
|
|
||||||
|
# ----------------------------------------------------------------- internals
|
||||||
|
def _guard(self, target: Callable[[], None]) -> None:
|
||||||
|
try:
|
||||||
|
target()
|
||||||
|
except BaseException as exc: # noqa: BLE001 - surfaced by stop()/wait()
|
||||||
|
if self._error is None:
|
||||||
|
self._error = exc
|
||||||
|
self._done.set()
|
||||||
|
|
||||||
|
def _drain(self, q: Queue) -> None:
|
||||||
|
while True:
|
||||||
|
try:
|
||||||
|
q.get_nowait()
|
||||||
|
except Empty:
|
||||||
|
return
|
||||||
|
|
||||||
|
def _put(self, q: Queue, item: Any) -> bool:
|
||||||
|
while not self._done.is_set():
|
||||||
|
try:
|
||||||
|
q.put(item, timeout=0.05)
|
||||||
|
return True
|
||||||
|
except Full:
|
||||||
|
continue
|
||||||
|
return False
|
||||||
|
|
||||||
|
def _get(self, q: Queue) -> Any:
|
||||||
|
"""Pop the next item, or ``None`` once the pipeline is stopping.
|
||||||
|
|
||||||
|
Consumers must use this rather than ``q.get()``: when :meth:`stop` sets
|
||||||
|
the done flag the producers stop delivering sentinels, so a bare
|
||||||
|
blocking ``get()`` would wait forever and ``join()`` would time out.
|
||||||
|
"""
|
||||||
|
while not self._done.is_set():
|
||||||
|
try:
|
||||||
|
return q.get(timeout=0.05)
|
||||||
|
except Empty:
|
||||||
|
continue
|
||||||
|
try:
|
||||||
|
return q.get_nowait()
|
||||||
|
except Empty:
|
||||||
|
return None
|
||||||
|
|
||||||
|
def _producer_loop(self) -> None:
|
||||||
|
while not self._done.is_set():
|
||||||
|
t0 = time.perf_counter()
|
||||||
|
frame = self._produce()
|
||||||
|
if frame is None:
|
||||||
|
break
|
||||||
|
t1 = time.perf_counter()
|
||||||
|
# Private copy: the SDK buffer is rewritten in place by the next
|
||||||
|
# produce() call while the flow stage is still reading this frame.
|
||||||
|
payload = (frame.copy(), t0, t1 - t0)
|
||||||
|
if not self._put(self._q1, payload):
|
||||||
|
break
|
||||||
|
self.stats.produced += 1
|
||||||
|
self._put(self._q1, None)
|
||||||
|
|
||||||
|
def _flow_loop(self) -> None:
|
||||||
|
while True:
|
||||||
|
item = self._get(self._q1)
|
||||||
|
if item is None:
|
||||||
|
break
|
||||||
|
frame, t_cap, produce_s = item
|
||||||
|
t0 = time.perf_counter()
|
||||||
|
# The frame is passed explicitly: the SDK's self.img slot is shared
|
||||||
|
# with the producer, so it must not be the transport.
|
||||||
|
self.sensor.compute_flow(frame)
|
||||||
|
flow_s = time.perf_counter() - t0
|
||||||
|
flow = getattr(self.sensor, "flow", None)
|
||||||
|
# Private copy: with the flow EMA enabled Sensor.flow is a buffer the
|
||||||
|
# next frame rewrites.
|
||||||
|
flow_copy = flow.copy() if isinstance(flow, np.ndarray) else None
|
||||||
|
self._put(self._q2, (frame, flow_copy, t_cap, produce_s, flow_s))
|
||||||
|
self._put(self._q2, None)
|
||||||
|
|
||||||
|
def _hhd_loop(self) -> None:
|
||||||
|
while True:
|
||||||
|
item = self._get(self._q2)
|
||||||
|
if item is None:
|
||||||
|
break
|
||||||
|
frame, flow, t_cap, produce_s, flow_s = item
|
||||||
|
t0 = time.perf_counter()
|
||||||
|
# Explicit flow: compute_hhd reads self.flow, which the flow stage
|
||||||
|
# overwrites for the next frame, so it cannot be the transport.
|
||||||
|
self.sensor.compute_hhd(flow)
|
||||||
|
if self._contact:
|
||||||
|
self.sensor.compute_contact()
|
||||||
|
hhd_s = time.perf_counter() - t0
|
||||||
|
snap = self._build(frame, flow, produce_s, flow_s, hhd_s, t_cap)
|
||||||
|
self._publish(snap)
|
||||||
|
if self._on_frame is not None:
|
||||||
|
# Runs on this stage's thread: a slow consumer exerts real
|
||||||
|
# backpressure instead of silently losing frames.
|
||||||
|
self._on_frame(snap)
|
||||||
|
if self._verbose:
|
||||||
|
print(f"[pipeline] finished after {self.stats.published} frames")
|
||||||
|
|
||||||
|
# -------------------------------------------------------------- snapshot
|
||||||
|
def _copy_array(self, selector: Callable[[], Any]) -> np.ndarray | None:
|
||||||
|
try:
|
||||||
|
value = selector()
|
||||||
|
except Exception:
|
||||||
|
return None
|
||||||
|
if value is None or not isinstance(value, np.ndarray) or value.size == 0:
|
||||||
|
return None
|
||||||
|
return value.copy()
|
||||||
|
|
||||||
|
def _build(self, frame: np.ndarray, flow: np.ndarray | None, produce_s: float,
|
||||||
|
flow_s: float, hhd_s: float, t_cap: float) -> FrameSnapshot:
|
||||||
|
sensor = self.sensor
|
||||||
|
info = sensor.info
|
||||||
|
include = self._include
|
||||||
|
now = time.perf_counter()
|
||||||
|
|
||||||
|
def maybe(name: str, selector: Callable[[], Any]) -> np.ndarray | None:
|
||||||
|
return self._copy_array(selector) if name in include else None
|
||||||
|
|
||||||
|
return FrameSnapshot(
|
||||||
|
seq=self.stats.published,
|
||||||
|
timestamp=time.time(),
|
||||||
|
age_ms=(now - t_cap) * 1000.0,
|
||||||
|
fps=self._fps(),
|
||||||
|
unit=str(getattr(sensor, "force_unit", "raw")),
|
||||||
|
normal_force=float(sensor.fnormal),
|
||||||
|
shear_x=float(sensor.fshearx),
|
||||||
|
shear_y=float(sensor.fsheary),
|
||||||
|
normal_force_raw=float(sensor._force_raw[0]),
|
||||||
|
shear_x_raw=float(sensor._force_raw[1]),
|
||||||
|
shear_y_raw=float(sensor._force_raw[2]),
|
||||||
|
torque=float(getattr(sensor, "ftor", 0.0)),
|
||||||
|
# Both already private per-frame copies owned by this snapshot.
|
||||||
|
image=frame if "image" in include else None,
|
||||||
|
flow=flow if "flow" in include else None,
|
||||||
|
# Written only by the HHD stage, so read_info is safe here.
|
||||||
|
vnormal=maybe("vnormal", lambda: sensor.read_info(info.VNORMAL)[0]),
|
||||||
|
vshear=maybe("vshear", lambda: sensor.read_info(info.VSHEAR)[0]),
|
||||||
|
depth=maybe("depth", lambda: sensor.read_info(info.DEPTH)[0]),
|
||||||
|
centroid=maybe("centroid", lambda: sensor.read_info(info.CENTROID)[0]),
|
||||||
|
contour=maybe("contour", lambda: sensor.read_info(info.CONTOUR)[0]),
|
||||||
|
produce_ms=produce_s * 1000.0,
|
||||||
|
flow_ms=flow_s * 1000.0,
|
||||||
|
hhd_ms=hhd_s * 1000.0,
|
||||||
|
)
|
||||||
|
|
||||||
|
def _fps(self) -> float:
|
||||||
|
now = time.perf_counter()
|
||||||
|
stamps = self._stamps
|
||||||
|
stamps.append(now)
|
||||||
|
if len(stamps) > self._fps_window:
|
||||||
|
del stamps[: len(stamps) - self._fps_window]
|
||||||
|
if len(stamps) < 2:
|
||||||
|
return 0.0
|
||||||
|
span = stamps[-1] - stamps[0]
|
||||||
|
return (len(stamps) - 1) / span if span > 0 else 0.0
|
||||||
|
|
||||||
|
def _publish(self, snap: FrameSnapshot) -> None:
|
||||||
|
stats = self.stats
|
||||||
|
n = stats.published + 1
|
||||||
|
stats.published = n
|
||||||
|
stats.produce_ms += (snap.produce_ms - stats.produce_ms) / n
|
||||||
|
stats.flow_ms += (snap.flow_ms - stats.flow_ms) / n
|
||||||
|
stats.hhd_ms += (snap.hhd_ms - stats.hhd_ms) / n
|
||||||
|
stats.age_ms += (snap.age_ms - stats.age_ms) / n
|
||||||
|
stats.fps = snap.fps
|
||||||
|
ages = stats._ages
|
||||||
|
ages.append(snap.age_ms)
|
||||||
|
if len(ages) > 200:
|
||||||
|
del ages[: len(ages) - 200]
|
||||||
|
# A percentile per published frame is measurable at 500+ FPS; refresh
|
||||||
|
# it periodically instead.
|
||||||
|
if n <= 10 or n % 10 == 0:
|
||||||
|
stats.age_p95_ms = float(np.percentile(ages, 95))
|
||||||
|
with self._lock:
|
||||||
|
self._latest = snap
|
||||||
|
self._published = n
|
||||||
+558
-43
@@ -26,8 +26,72 @@ from .backend_availability import (check_cupy_available, check_opencl_available)
|
|||||||
from .frame_processors import create_frame_processor
|
from .frame_processors import create_frame_processor
|
||||||
from .signal_processing import (
|
from .signal_processing import (
|
||||||
DEFAULT_SAMPLE_RATE_HZ,
|
DEFAULT_SAMPLE_RATE_HZ,
|
||||||
|
CausalMedianFilter,
|
||||||
LowpassFilter,
|
LowpassFilter,
|
||||||
|
SampleRateEstimator,
|
||||||
)
|
)
|
||||||
|
|
||||||
|
# Default signal-conditioning settings (config: top-level "filters" block).
|
||||||
|
#
|
||||||
|
# A key a config does not state is **off**, not "whatever we thought was
|
||||||
|
# sensible": the file has to name what runs, or a device can be filtering a
|
||||||
|
# measurement nobody asked it to filter and no reader of the file could tell.
|
||||||
|
# The shipped bundles state all three values explicitly, so they are unaffected
|
||||||
|
# by these defaults; a config with no "filters" block at all filters nothing.
|
||||||
|
#
|
||||||
|
# Which settings are worth asking for is measured, not guessed. On dd02-0703 at
|
||||||
|
# 280x300 (see the noise analysis in the perf notes) flow EMA 0.5 + median 3
|
||||||
|
# costs ~2 frames at 30 fps and cuts steady-press force jitter by roughly half
|
||||||
|
# while also stabilising vnormal/contact (which a force-level filter cannot do);
|
||||||
|
# that combination is the "balanced" preset. The force low-pass stays off there
|
||||||
|
# too: on top of the EMA it added no measurable noise benefit at the same
|
||||||
|
# latency, so it is an opt-in stage for sites with different noise
|
||||||
|
# characteristics (the "low_noise" preset).
|
||||||
|
DEFAULT_FORCE_LOWPASS_HZ = 0.0
|
||||||
|
DEFAULT_FORCE_MEDIAN_WINDOW = 1
|
||||||
|
DEFAULT_FLOW_EMA_ALPHA = 1.0
|
||||||
|
|
||||||
|
#: Named filter presets, so a recommendation is *executable* instead of a
|
||||||
|
#: comment in a JSON file (``json.load`` cannot carry comments). A config may
|
||||||
|
#: write ``"filters": {"preset": "balanced"}`` and override any single key on
|
||||||
|
#: top of it; the resolved values are what ``Sensor.filters_state()`` reports.
|
||||||
|
#: The latency/noise trade-offs behind these numbers are measured in
|
||||||
|
#: ``_perf/`` and tabulated in ``docs/filters.md``.
|
||||||
|
FILTER_PRESETS = {
|
||||||
|
# Everything off: the file says what runs, and the raw reading is the record.
|
||||||
|
"off": {
|
||||||
|
"flow_ema_alpha": 1.0,
|
||||||
|
"force_median_window": 1,
|
||||||
|
"force_lowpass_cutoff_hz": 0.0,
|
||||||
|
},
|
||||||
|
# One frame of latency @30 fps: median only. The EMA changes the
|
||||||
|
# measurement, so it stays off when latency matters more than jitter.
|
||||||
|
"low_latency": {
|
||||||
|
"flow_ema_alpha": 1.0,
|
||||||
|
"force_median_window": 3,
|
||||||
|
"force_lowpass_cutoff_hz": 0.0,
|
||||||
|
},
|
||||||
|
# Two frames @30 fps: the EMA removes the spatially-coherent flow jitter
|
||||||
|
# that no force-level filter can reach, the median removes its spikes.
|
||||||
|
"balanced": {
|
||||||
|
"flow_ema_alpha": 0.5,
|
||||||
|
"force_median_window": 3,
|
||||||
|
"force_lowpass_cutoff_hz": 0.0,
|
||||||
|
},
|
||||||
|
# Three frames @30 fps and the quietest readout: as ``balanced`` plus the
|
||||||
|
# force low-pass on top of the median.
|
||||||
|
"low_noise": {
|
||||||
|
"flow_ema_alpha": 0.3,
|
||||||
|
"force_median_window": 3,
|
||||||
|
"force_lowpass_cutoff_hz": 10.0,
|
||||||
|
},
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
def available_filter_presets() -> dict:
|
||||||
|
"""The presets, for UIs that want to offer them (the studio's gear dialog)."""
|
||||||
|
return {name: dict(values) for name, values in FILTER_PRESETS.items()}
|
||||||
|
|
||||||
from .capture import (
|
from .capture import (
|
||||||
CaptureConfig,
|
CaptureConfig,
|
||||||
CaptureSource,
|
CaptureSource,
|
||||||
@@ -154,16 +218,115 @@ class Sensor:
|
|||||||
self.calibration: Optional[cal_mod.Calibration] = None
|
self.calibration: Optional[cal_mod.Calibration] = None
|
||||||
self._force_cal_sensor_name: Optional[str] = None
|
self._force_cal_sensor_name: Optional[str] = None
|
||||||
self._init_force_calibration()
|
self._init_force_calibration()
|
||||||
self._lowpass_enabled = False
|
|
||||||
self._lowpass_cutoff_hz = 0.0
|
# ---------------------------------------------------------------
|
||||||
self._lowpass_sample_rate_hz = float(DEFAULT_SAMPLE_RATE_HZ)
|
# Signal conditioning (config: top-level "filters" block)
|
||||||
|
# ---------------------------------------------------------------
|
||||||
|
# These settings change the reported force values, so they are part of
|
||||||
|
# the measurement-pipeline identity and are recorded in the calibration
|
||||||
|
# metadata / mismatch check alongside dis_mode and hhd_downsample.
|
||||||
|
#
|
||||||
|
# Every stage is switched by its **own value**, so the config states what
|
||||||
|
# runs: ``flow_ema_alpha >= 1``, ``force_median_window <= 1`` and
|
||||||
|
# ``force_lowpass_cutoff_hz <= 0`` each mean "stage off". Values are never
|
||||||
|
# overridden: there is no master switch that could make the file say 0.5
|
||||||
|
# while the device runs 1.0. An invalid value falls back to **off**, never
|
||||||
|
# to a default that filters. ``enabled`` is kept only as a deprecated
|
||||||
|
# alias for "turn every stage off" and is honoured when it is explicitly
|
||||||
|
# false. ``Sensor.filters_state()`` reports what actually took effect.
|
||||||
|
filters_cfg = self._config.get("filters")
|
||||||
|
if not isinstance(filters_cfg, dict):
|
||||||
|
filters_cfg = {}
|
||||||
|
legacy_off = filters_cfg.get("enabled", None) is False
|
||||||
|
preset_name = filters_cfg.get("preset", None)
|
||||||
|
preset_values = FILTER_PRESETS.get(str(preset_name), {}) if preset_name else {}
|
||||||
|
self._filter_preset = str(preset_name) if preset_name in FILTER_PRESETS else None
|
||||||
|
|
||||||
|
def _cfg(key, default):
|
||||||
|
"""The configured value: an explicit key, else the preset, else default."""
|
||||||
|
if legacy_off:
|
||||||
|
return default
|
||||||
|
value = filters_cfg.get(key, None)
|
||||||
|
if value is None:
|
||||||
|
value = preset_values.get(key, None)
|
||||||
|
return default if value is None else value
|
||||||
|
|
||||||
self._lowpass_order = 2
|
self._lowpass_order = 2
|
||||||
|
self._lowpass_sample_rate_hz = float(DEFAULT_SAMPLE_RATE_HZ)
|
||||||
|
self._lowpass_cutoff_hz = 0.0
|
||||||
|
self._lowpass_rate_auto = True
|
||||||
|
self._config_fps = 0.0
|
||||||
|
try:
|
||||||
|
self._config_fps = float(self.__config_cam.get("fps", 0.0) or 0.0)
|
||||||
|
except (TypeError, ValueError):
|
||||||
|
self._config_fps = 0.0
|
||||||
|
if self._config_fps <= 0:
|
||||||
|
self._config_fps = float(DEFAULT_SAMPLE_RATE_HZ)
|
||||||
|
self._lowpass_sample_rate_hz = self._config_fps
|
||||||
|
try:
|
||||||
|
self._lowpass_order = max(1, int(_cfg("force_lowpass_order", 2)))
|
||||||
|
except (TypeError, ValueError):
|
||||||
|
self._lowpass_order = 2
|
||||||
|
rate_cfg = _cfg("force_lowpass_sample_rate_hz", "auto")
|
||||||
|
if isinstance(rate_cfg, str):
|
||||||
|
self._lowpass_rate_auto = rate_cfg.strip().lower() in ("auto", "", "measured")
|
||||||
|
else:
|
||||||
|
try:
|
||||||
|
rate = float(rate_cfg)
|
||||||
|
except (TypeError, ValueError):
|
||||||
|
rate = 0.0
|
||||||
|
self._lowpass_rate_auto = rate <= 0.0
|
||||||
|
if not self._lowpass_rate_auto:
|
||||||
|
self._lowpass_sample_rate_hz = rate
|
||||||
|
try:
|
||||||
|
self._lowpass_cutoff_hz = float(
|
||||||
|
_cfg("force_lowpass_cutoff_hz", DEFAULT_FORCE_LOWPASS_HZ)
|
||||||
|
)
|
||||||
|
except (TypeError, ValueError):
|
||||||
|
self._lowpass_cutoff_hz = 0.0
|
||||||
|
if self._lowpass_sample_rate_hz <= 0:
|
||||||
|
self._lowpass_sample_rate_hz = self._config_fps
|
||||||
|
# An unusable cutoff is "off", never a silent jump to the default.
|
||||||
|
if not (0.0 <= self._lowpass_cutoff_hz < 0.5 * self._lowpass_sample_rate_hz):
|
||||||
|
self._lowpass_cutoff_hz = 0.0
|
||||||
|
self._lowpass_enabled = self._lowpass_cutoff_hz > 0
|
||||||
self._force_filter = LowpassFilter(
|
self._force_filter = LowpassFilter(
|
||||||
cutoff_hz=self._lowpass_cutoff_hz,
|
cutoff_hz=self._lowpass_cutoff_hz,
|
||||||
sample_rate_hz=self._lowpass_sample_rate_hz,
|
sample_rate_hz=self._lowpass_sample_rate_hz,
|
||||||
order=self._lowpass_order,
|
order=self._lowpass_order,
|
||||||
n_channels=3,
|
n_channels=3,
|
||||||
)
|
)
|
||||||
|
# Rate tracking: only meaningful while the force low-pass is in use.
|
||||||
|
self._force_rate = SampleRateEstimator(initial_hz=self._config_fps)
|
||||||
|
self._force_rate_source = "config"
|
||||||
|
|
||||||
|
try:
|
||||||
|
self._force_median_window = max(1, int(_cfg("force_median_window", DEFAULT_FORCE_MEDIAN_WINDOW)))
|
||||||
|
except (TypeError, ValueError):
|
||||||
|
self._force_median_window = 1
|
||||||
|
self._force_median = CausalMedianFilter(window=self._force_median_window, n_channels=3)
|
||||||
|
|
||||||
|
self._flow_ema_alpha = 1.0
|
||||||
|
try:
|
||||||
|
alpha = float(_cfg("flow_ema_alpha", DEFAULT_FLOW_EMA_ALPHA))
|
||||||
|
except (TypeError, ValueError):
|
||||||
|
alpha = 1.0
|
||||||
|
if 0.0 < alpha < 1.0:
|
||||||
|
self._flow_ema_alpha = alpha
|
||||||
|
self._ema_flow = None
|
||||||
|
if legacy_off and self.verbose:
|
||||||
|
print(
|
||||||
|
"Note: filters.enabled is deprecated and means 'all stages off'; "
|
||||||
|
"remove it and switch each stage by its own value "
|
||||||
|
"(flow_ema_alpha=1, force_median_window=1, force_lowpass_cutoff_hz=0)."
|
||||||
|
)
|
||||||
|
# Derived, for callers that ask "is any stage filtering?".
|
||||||
|
self._filters_enabled = bool(
|
||||||
|
self._flow_ema_alpha < 1.0
|
||||||
|
or self._force_median_window > 1
|
||||||
|
or self._lowpass_enabled
|
||||||
|
)
|
||||||
|
|
||||||
if self.verbose:
|
if self.verbose:
|
||||||
if self.calibration is not None and self.calibration.source_path is not None:
|
if self.calibration is not None and self.calibration.source_path is not None:
|
||||||
print(f"Force calibration: {self.calibration.source_path}")
|
print(f"Force calibration: {self.calibration.source_path}")
|
||||||
@@ -182,6 +345,13 @@ class Sensor:
|
|||||||
self.valid_mask_raw = None
|
self.valid_mask_raw = None
|
||||||
self.valid_mask = None
|
self.valid_mask = None
|
||||||
self.valid_mask_hhd = None
|
self.valid_mask_hhd = None
|
||||||
|
# Scratch buffers reused by _compute_flow_means / compute_hhd.
|
||||||
|
self._flow_mag_buf = None
|
||||||
|
self._flow_mask_u8 = None
|
||||||
|
self._hhd_vn_buf = None
|
||||||
|
self._hhd_vs_buf = None
|
||||||
|
self._hhd_flow_ds_buf = None
|
||||||
|
self._hhd_mask_f32 = None
|
||||||
self.mask_stats = {
|
self.mask_stats = {
|
||||||
"coverage": 0.0,
|
"coverage": 0.0,
|
||||||
"flow_mean_valid": 0.0,
|
"flow_mean_valid": 0.0,
|
||||||
@@ -194,10 +364,16 @@ class Sensor:
|
|||||||
self._last_capture_time = 0.0
|
self._last_capture_time = 0.0
|
||||||
self._last_processing_time = 0.0
|
self._last_processing_time = 0.0
|
||||||
self._last_stage_times = {
|
self._last_stage_times = {
|
||||||
"extract_tiles": 0.0,
|
# Frame-post-processing roll-ups (seconds).
|
||||||
"stitch": 0.0,
|
"extract_tiles": 0.0, # sensor units + tile extraction
|
||||||
"resize": 0.0,
|
"stitch": 0.0, # feature/feather stitch of the extracted tiles
|
||||||
"mirror": 0.0,
|
"resize": 0.0, # BGR->gray + resize to resolution_stitch + mirror
|
||||||
|
# Finer breakdown.
|
||||||
|
"store": 0.0, # frame reference/copy
|
||||||
|
"units": 0.0, # compound-eye unit (sub-image) extraction
|
||||||
|
"gray": 0.0, # BGR->gray conversion
|
||||||
|
"mirror": 0.0, # horizontal flip
|
||||||
|
"mask": 0.0, # valid-mask resize/cache update
|
||||||
"astype": 0.0,
|
"astype": 0.0,
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -218,6 +394,7 @@ class Sensor:
|
|||||||
except (TypeError, ValueError):
|
except (TypeError, ValueError):
|
||||||
self._hhd_downsample = 1
|
self._hhd_downsample = 1
|
||||||
self._hhd_grid_shape = None
|
self._hhd_grid_shape = None
|
||||||
|
self._hhd_ref_shape = None
|
||||||
cfg_backend = self.__config_tracker.get("backend", None)
|
cfg_backend = self.__config_tracker.get("backend", None)
|
||||||
self.poisson_backend = cfg_backend if cfg_backend is not None else backend
|
self.poisson_backend = cfg_backend if cfg_backend is not None else backend
|
||||||
self.masked_poisson_solver = self.__config_tracker.get("masked_poisson_solver", "sparse")
|
self.masked_poisson_solver = self.__config_tracker.get("masked_poisson_solver", "sparse")
|
||||||
@@ -244,6 +421,15 @@ class Sensor:
|
|||||||
print("Sensor initialized in external-frame mode.")
|
print("Sensor initialized in external-frame mode.")
|
||||||
else:
|
else:
|
||||||
self._open_video_source(override, box)
|
self._open_video_source(override, box)
|
||||||
|
if not self._is_live_camera:
|
||||||
|
# A file source is replayed at whatever speed the CPU manages,
|
||||||
|
# so wall-clock intervals do not reflect signal time: keep the
|
||||||
|
# configured camera fps for the force low-pass design.
|
||||||
|
self._lowpass_rate_auto = False
|
||||||
|
self._force_rate_source = "config (file)"
|
||||||
|
self._lowpass_sample_rate_hz = self._config_fps
|
||||||
|
if getattr(self, "_force_rate", None) is not None:
|
||||||
|
self._force_rate.reset(self._config_fps)
|
||||||
|
|
||||||
if self.verbose:
|
if self.verbose:
|
||||||
print(f" Video source: {vid_src}")
|
print(f" Video source: {vid_src}")
|
||||||
@@ -253,6 +439,14 @@ class Sensor:
|
|||||||
# print(f" CUDA acceleration: {self.cuda}")
|
# print(f" CUDA acceleration: {self.cuda}")
|
||||||
print(f" CuPy available: {self.CUPY_AVAILABLE}")
|
print(f" CuPy available: {self.CUPY_AVAILABLE}")
|
||||||
print(f" OpenCL available: {self.OPENCL_AVAILABLE}")
|
print(f" OpenCL available: {self.OPENCL_AVAILABLE}")
|
||||||
|
print(
|
||||||
|
" Signal filters: "
|
||||||
|
f"ema_alpha={self._flow_ema_alpha} "
|
||||||
|
f"median={self._force_median_window} "
|
||||||
|
f"lowpass={self._lowpass_cutoff_hz}Hz@"
|
||||||
|
f"{self._lowpass_sample_rate_hz:.1f}Hz "
|
||||||
|
f"({self.force_sample_rate_source})"
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
@@ -313,17 +507,31 @@ class Sensor:
|
|||||||
def _compute_flow_means(self, flow):
|
def _compute_flow_means(self, flow):
|
||||||
if flow is None:
|
if flow is None:
|
||||||
return
|
return
|
||||||
mag = np.linalg.norm(flow, axis=-1)
|
shape = flow.shape[:2]
|
||||||
|
mag = self._flow_mag_buf
|
||||||
|
if mag is None or mag.shape != shape or mag.dtype != np.float32:
|
||||||
|
mag = np.empty(shape, dtype=np.float32)
|
||||||
|
self._flow_mag_buf = mag
|
||||||
|
# cv2.magnitude + cv2.mean(mask=...) avoid the (H, W) temporaries and the
|
||||||
|
# boolean fancy-index gather that np.linalg.norm + mag[mask].mean() need.
|
||||||
|
cv2.magnitude(flow[:, :, 0], flow[:, :, 1], mag)
|
||||||
|
|
||||||
mask = self.valid_mask
|
mask = self.valid_mask
|
||||||
if mask is None or mask.shape != mag.shape:
|
if mask is None or mask.shape != shape:
|
||||||
self.mask_stats["flow_mean_valid"] = float(mag.mean()) if mag.size else 0.0
|
self.mask_stats["flow_mean_valid"] = float(cv2.mean(mag)[0]) if mag.size else 0.0
|
||||||
self.mask_stats["flow_mean_invalid"] = 0.0
|
self.mask_stats["flow_mean_invalid"] = 0.0
|
||||||
return
|
return
|
||||||
|
|
||||||
valid = mask.astype(bool)
|
mask_u8 = self._flow_mask_u8
|
||||||
invalid = ~valid
|
if mask_u8 is None or mask_u8.shape != shape or mask_u8.dtype != np.uint8:
|
||||||
self.mask_stats["flow_mean_valid"] = float(mag[valid].mean()) if np.any(valid) else 0.0
|
mask_u8 = np.empty(shape, dtype=np.uint8)
|
||||||
self.mask_stats["flow_mean_invalid"] = float(mag[invalid].mean()) if np.any(invalid) else 0.0
|
self._flow_mask_u8 = mask_u8
|
||||||
|
np.copyto(mask_u8, mask, casting="unsafe")
|
||||||
|
np.multiply(mask_u8, 255, out=mask_u8)
|
||||||
|
|
||||||
|
self.mask_stats["flow_mean_valid"] = float(cv2.mean(mag, mask=mask_u8)[0])
|
||||||
|
np.subtract(255, mask_u8, out=mask_u8)
|
||||||
|
self.mask_stats["flow_mean_invalid"] = float(cv2.mean(mag, mask=mask_u8)[0])
|
||||||
|
|
||||||
def get_img(self, frame=None, already_processed=False, valid_mask=None, already_gray=False):
|
def get_img(self, frame=None, already_processed=False, valid_mask=None, already_gray=False):
|
||||||
"""
|
"""
|
||||||
@@ -362,6 +570,7 @@ class Sensor:
|
|||||||
self.frame = frame if (already_processed and isinstance(frame, np.ndarray)) else (
|
self.frame = frame if (already_processed and isinstance(frame, np.ndarray)) else (
|
||||||
frame.copy() if isinstance(frame, np.ndarray) else frame
|
frame.copy() if isinstance(frame, np.ndarray) else frame
|
||||||
)
|
)
|
||||||
|
t_after_store = time.perf_counter()
|
||||||
if already_processed:
|
if already_processed:
|
||||||
img_bgr = frame
|
img_bgr = frame
|
||||||
gray = None
|
gray = None
|
||||||
@@ -383,8 +592,6 @@ class Sensor:
|
|||||||
gray = None
|
gray = None
|
||||||
t_after_extract = time.perf_counter()
|
t_after_extract = time.perf_counter()
|
||||||
|
|
||||||
t_after_stitch = time.perf_counter()
|
|
||||||
|
|
||||||
if gray is None:
|
if gray is None:
|
||||||
self.img_size_raw = img_bgr.shape
|
self.img_size_raw = img_bgr.shape
|
||||||
self._img_bgr_raw = img_bgr
|
self._img_bgr_raw = img_bgr
|
||||||
@@ -392,6 +599,8 @@ class Sensor:
|
|||||||
else:
|
else:
|
||||||
self.img_size_raw = gray.shape
|
self.img_size_raw = gray.shape
|
||||||
self._img_bgr_raw = None
|
self._img_bgr_raw = None
|
||||||
|
t_after_gray = time.perf_counter()
|
||||||
|
|
||||||
res = tuple(self._config_resolution_stitch)
|
res = tuple(self._config_resolution_stitch)
|
||||||
if self._img_gray_buf is None or self._img_gray_buf.shape != (res[1], res[0]):
|
if self._img_gray_buf is None or self._img_gray_buf.shape != (res[1], res[0]):
|
||||||
self._img_gray_buf = np.empty((res[1], res[0]), dtype=np.uint8)
|
self._img_gray_buf = np.empty((res[1], res[0]), dtype=np.uint8)
|
||||||
@@ -400,15 +609,22 @@ class Sensor:
|
|||||||
|
|
||||||
if self.mirror:
|
if self.mirror:
|
||||||
cv2.flip(self._img_gray_buf, 1, dst=self._img_gray_buf)
|
cv2.flip(self._img_gray_buf, 1, dst=self._img_gray_buf)
|
||||||
|
t_after_mirror = time.perf_counter()
|
||||||
|
|
||||||
self.img = self._img_gray_buf
|
self.img = self._img_gray_buf
|
||||||
self._prepare_valid_mask(valid_mask, gray.shape, self.img.shape)
|
self._prepare_valid_mask(valid_mask, gray.shape, self.img.shape)
|
||||||
|
|
||||||
t_processing_end = time.perf_counter()
|
t_processing_end = time.perf_counter()
|
||||||
|
|
||||||
self._last_processing_time = t_processing_end - t_processing_start
|
self._last_processing_time = t_processing_end - t_processing_start
|
||||||
self._last_stage_times["extract_tiles"] = t_after_extract - t_processing_start
|
self._record_stage_times(
|
||||||
self._last_stage_times["stitch"] = t_after_stitch - t_after_extract
|
t_processing_start=t_processing_start,
|
||||||
self._last_stage_times["resize"] = t_processing_end - t_after_stitch
|
t_after_store=t_after_store,
|
||||||
|
t_after_extract=t_after_extract,
|
||||||
|
t_after_gray=t_after_gray,
|
||||||
|
t_after_resize=t_after_resize,
|
||||||
|
t_after_mirror=t_after_mirror,
|
||||||
|
t_processing_end=t_processing_end,
|
||||||
|
)
|
||||||
|
|
||||||
# FPS calculation
|
# FPS calculation
|
||||||
self._fps = 1.0 / (time.perf_counter() - self.last_log_time)
|
self._fps = 1.0 / (time.perf_counter() - self.last_log_time)
|
||||||
@@ -416,6 +632,48 @@ class Sensor:
|
|||||||
|
|
||||||
return self.img.copy()
|
return self.img.copy()
|
||||||
|
|
||||||
|
def _record_stage_times(
|
||||||
|
self,
|
||||||
|
*,
|
||||||
|
t_processing_start,
|
||||||
|
t_after_store,
|
||||||
|
t_after_extract,
|
||||||
|
t_after_gray,
|
||||||
|
t_after_resize,
|
||||||
|
t_after_mirror,
|
||||||
|
t_processing_end,
|
||||||
|
):
|
||||||
|
"""Record per-stage frame-post-processing timings (seconds).
|
||||||
|
|
||||||
|
``extract_tiles``/``stitch`` come from the frame processor, which times
|
||||||
|
its own sub-steps; the remaining keys are measured here. The roll-ups
|
||||||
|
keep the historical key names while no longer collapsing ``stitch`` to
|
||||||
|
zero: previously ``t_after_stitch`` was sampled immediately after
|
||||||
|
``t_after_extract``, so ``stitch`` was always ~0 s and ``resize``
|
||||||
|
silently absorbed the whole stitch cost.
|
||||||
|
"""
|
||||||
|
stage = self._last_stage_times
|
||||||
|
processor_timings = getattr(self.frame_processor, "last_timings", None) or {}
|
||||||
|
units_s = float(processor_timings.get("units", 0.0))
|
||||||
|
extract_s = float(processor_timings.get("extract_tiles", 0.0))
|
||||||
|
stitch_s = float(processor_timings.get("stitch", 0.0))
|
||||||
|
processor_total_s = t_after_extract - t_after_store
|
||||||
|
|
||||||
|
stage["store"] = t_after_store - t_processing_start
|
||||||
|
stage["units"] = units_s
|
||||||
|
stage["gray"] = t_after_gray - t_after_extract
|
||||||
|
stage["resize"] = t_after_resize - t_after_gray
|
||||||
|
stage["mirror"] = t_after_mirror - t_after_resize
|
||||||
|
stage["mask"] = t_processing_end - t_after_mirror
|
||||||
|
|
||||||
|
if processor_timings:
|
||||||
|
stage["extract_tiles"] = units_s + extract_s
|
||||||
|
stage["stitch"] = stitch_s
|
||||||
|
else:
|
||||||
|
# Non-compound-eye processors are a single opaque step.
|
||||||
|
stage["extract_tiles"] = processor_total_s
|
||||||
|
stage["stitch"] = 0.0
|
||||||
|
|
||||||
def _get_img_bgr(self):
|
def _get_img_bgr(self):
|
||||||
"""Produce a resized+mirrored BGR image on demand (for display)."""
|
"""Produce a resized+mirrored BGR image on demand (for display)."""
|
||||||
if self._img_bgr_raw is None:
|
if self._img_bgr_raw is None:
|
||||||
@@ -504,20 +762,99 @@ class Sensor:
|
|||||||
self.compute_hhd()
|
self.compute_hhd()
|
||||||
return True
|
return True
|
||||||
|
|
||||||
def compute_flow(self):
|
def compute_flow(self, img=None):
|
||||||
if self.img is None:
|
"""Compute the optical flow for one frame.
|
||||||
|
|
||||||
|
``img`` optionally supplies the frame to use instead of ``self.img``.
|
||||||
|
StreamPipeline passes it explicitly so a producer thread cannot swap
|
||||||
|
``self.img`` between the hand-off and the tracker call. Positional
|
||||||
|
callers and existing code are unaffected.
|
||||||
|
"""
|
||||||
|
image = self.img if img is None else img
|
||||||
|
if image is None:
|
||||||
raise Exception("Warning: Get image first")
|
raise Exception("Warning: Get image first")
|
||||||
|
|
||||||
flow = self.__tracker.track(self.img, valid_mask=self.valid_mask)
|
flow = self.__tracker.track(image, valid_mask=self.valid_mask)
|
||||||
# flow = self.__tracker.track_raft(self.img)
|
# flow = self.__tracker.track_raft(self.img)
|
||||||
if hasattr(flow, 'download'):
|
if hasattr(flow, 'download'):
|
||||||
flow = flow.download()
|
flow = flow.download()
|
||||||
|
flow = self._apply_flow_ema(flow)
|
||||||
self.flow = flow
|
self.flow = flow
|
||||||
self._compute_flow_means(self.flow)
|
self._compute_flow_means(self.flow)
|
||||||
|
|
||||||
def compute_hhd(self):
|
def _apply_flow_ema(self, flow):
|
||||||
|
"""Temporally smooth the optical-flow field before HHD.
|
||||||
|
|
||||||
flow = self.flow
|
The frame-to-frame jitter of the estimated flow is spatially coherent
|
||||||
|
(a near-global shift: it survives ~40x less averaging than per-pixel
|
||||||
|
noise would, see the noise analysis), so it cannot be removed by
|
||||||
|
spatial smoothing, a coarser HHD grid or a different DIS setting - only
|
||||||
|
by averaging along time. Applying it here also stabilises
|
||||||
|
``vnormal``/contact, which a force-level filter cannot reach.
|
||||||
|
|
||||||
|
``flow_ema_alpha >= 1`` disables the stage.
|
||||||
|
"""
|
||||||
|
alpha = getattr(self, "_flow_ema_alpha", 1.0)
|
||||||
|
if flow is None or alpha >= 1.0:
|
||||||
|
return flow
|
||||||
|
prev = self._ema_flow
|
||||||
|
if prev is None or prev.shape != flow.shape or prev.dtype != flow.dtype:
|
||||||
|
self._ema_flow = np.array(flow, copy=True)
|
||||||
|
return flow
|
||||||
|
prev += alpha * (flow - prev)
|
||||||
|
return prev
|
||||||
|
|
||||||
|
def _update_force_sample_rate(self):
|
||||||
|
"""Track the achieved force update rate for the low-pass design.
|
||||||
|
|
||||||
|
A Butterworth designed for 30 Hz stops meaning the same thing at 60 Hz,
|
||||||
|
so when ``force_lowpass_sample_rate_hz`` is ``"auto"`` the rate actually
|
||||||
|
achieved is used. For sources whose wall-clock cadence does not equal
|
||||||
|
signal time (video files, offline replay) ``_lowpass_rate_auto`` is
|
||||||
|
turned off by whoever knows the source - ``__init__`` for a capture the
|
||||||
|
Sensor opened itself, and ``FingerRuntime.open`` for external frames -
|
||||||
|
and the configured camera fps is used instead.
|
||||||
|
"""
|
||||||
|
if not getattr(self, "_lowpass_rate_auto", False):
|
||||||
|
return
|
||||||
|
estimator = getattr(self, "_force_rate", None)
|
||||||
|
if estimator is None:
|
||||||
|
return
|
||||||
|
hz = estimator.tick()
|
||||||
|
if not (1.0 <= hz <= 240.0):
|
||||||
|
return
|
||||||
|
self._force_rate_source = "measured"
|
||||||
|
used = self._lowpass_sample_rate_hz
|
||||||
|
if used > 0 and abs(hz - used) <= 0.1 * used:
|
||||||
|
return
|
||||||
|
self._lowpass_sample_rate_hz = hz
|
||||||
|
if self._lowpass_enabled:
|
||||||
|
self._force_filter.configure(
|
||||||
|
cutoff_hz=self._lowpass_cutoff_hz,
|
||||||
|
sample_rate_hz=hz,
|
||||||
|
order=self._lowpass_order,
|
||||||
|
)
|
||||||
|
|
||||||
|
@property
|
||||||
|
def force_sample_rate_hz(self) -> float:
|
||||||
|
"""Sample rate currently used to design the force low-pass (Hz)."""
|
||||||
|
return float(self._lowpass_sample_rate_hz)
|
||||||
|
|
||||||
|
@property
|
||||||
|
def force_sample_rate_source(self) -> str:
|
||||||
|
"""``config``, ``measured`` or ``config (replay)``."""
|
||||||
|
return str(getattr(self, "_force_rate_source", "config"))
|
||||||
|
|
||||||
|
def compute_hhd(self, flow=None):
|
||||||
|
"""Decompose the optical flow and update the force readout.
|
||||||
|
|
||||||
|
``flow`` optionally supplies the field to decompose instead of
|
||||||
|
``self.flow``. StreamPipeline passes its own per-frame copy so the flow
|
||||||
|
stage cannot overwrite ``self.flow`` mid-decomposition. Existing
|
||||||
|
callers are unaffected.
|
||||||
|
"""
|
||||||
|
|
||||||
|
flow = self.flow if flow is None else flow
|
||||||
if flow is None:
|
if flow is None:
|
||||||
raise Exception("Warning: Compute flow first")
|
raise Exception("Warning: Compute flow first")
|
||||||
|
|
||||||
@@ -543,7 +880,11 @@ class Sensor:
|
|||||||
if k > 1:
|
if k > 1:
|
||||||
ds_w = max(1, orig_w // k)
|
ds_w = max(1, orig_w // k)
|
||||||
ds_h = max(1, orig_h // k)
|
ds_h = max(1, orig_h // k)
|
||||||
flow_ds = cv2.resize(flow, (ds_w, ds_h), interpolation=cv2.INTER_AREA)
|
flow_ds = self._hhd_flow_ds_buf
|
||||||
|
if flow_ds is None or flow_ds.shape != (ds_h, ds_w, 2) or flow_ds.dtype != np.float32:
|
||||||
|
flow_ds = np.empty((ds_h, ds_w, 2), dtype=np.float32)
|
||||||
|
self._hhd_flow_ds_buf = flow_ds
|
||||||
|
cv2.resize(flow, (ds_w, ds_h), dst=flow_ds, interpolation=cv2.INTER_AREA)
|
||||||
if mask_ds is not None and (
|
if mask_ds is not None and (
|
||||||
mask_ds.shape[1] != ds_w or mask_ds.shape[0] != ds_h
|
mask_ds.shape[1] != ds_w or mask_ds.shape[0] != ds_h
|
||||||
):
|
):
|
||||||
@@ -552,8 +893,13 @@ class Sensor:
|
|||||||
flow_ds = flow
|
flow_ds = flow
|
||||||
|
|
||||||
ds_shape = flow_ds.shape[:2]
|
ds_shape = flow_ds.shape[:2]
|
||||||
|
ref_shape = (orig_h, orig_w)
|
||||||
self.valid_mask_hhd = mask_ds
|
self.valid_mask_hhd = mask_ds
|
||||||
if self.__hhd is None or self._hhd_grid_shape != ds_shape:
|
if (
|
||||||
|
self.__hhd is None
|
||||||
|
or self._hhd_grid_shape != ds_shape
|
||||||
|
or self._hhd_ref_shape != ref_shape
|
||||||
|
):
|
||||||
spacings = (18 * k, 18 * k)
|
spacings = (18 * k, 18 * k)
|
||||||
self.__hhd = nHHD(
|
self.__hhd = nHHD(
|
||||||
cuda=self.cuda,
|
cuda=self.cuda,
|
||||||
@@ -561,6 +907,9 @@ class Sensor:
|
|||||||
spacings=spacings,
|
spacings=spacings,
|
||||||
poisson_backend=self.poisson_backend,
|
poisson_backend=self.poisson_backend,
|
||||||
debug=self.debug,
|
debug=self.debug,
|
||||||
|
# Rescale grid-summed force aggregates back onto the full-resolution
|
||||||
|
# flow grid so Sn/Stx/Sty/Stor stay comparable across hhd_downsample.
|
||||||
|
force_reference_grid=ref_shape,
|
||||||
masked_poisson_solver=self.masked_poisson_solver,
|
masked_poisson_solver=self.masked_poisson_solver,
|
||||||
masked_poisson_max_boundary=self.masked_poisson_max_boundary,
|
masked_poisson_max_boundary=self.masked_poisson_max_boundary,
|
||||||
masked_poisson_boundary_stride=self.masked_poisson_boundary_stride,
|
masked_poisson_boundary_stride=self.masked_poisson_boundary_stride,
|
||||||
@@ -569,29 +918,46 @@ class Sensor:
|
|||||||
masked_poisson_reg=self.masked_poisson_reg,
|
masked_poisson_reg=self.masked_poisson_reg,
|
||||||
)
|
)
|
||||||
self._hhd_grid_shape = ds_shape
|
self._hhd_grid_shape = ds_shape
|
||||||
|
self._hhd_ref_shape = ref_shape
|
||||||
|
|
||||||
_, forces = self.__hhd.get_force(flow_ds, valid_mask=mask_ds)
|
_, forces = self.__hhd.get_force(flow_ds, valid_mask=mask_ds)
|
||||||
fn, fx, fy, ftor = forces
|
fn, fx, fy, ftor = forces
|
||||||
self._force_raw = (float(fn), float(fx), float(fy))
|
self._force_raw = (float(fn), float(fx), float(fy))
|
||||||
self.ftor = float(ftor)
|
self.ftor = float(ftor)
|
||||||
|
self._update_force_sample_rate()
|
||||||
self._apply_force_filter()
|
self._apply_force_filter()
|
||||||
|
|
||||||
vnormal_ds = self.__hhd.get_normal()
|
vnormal_ds = self.__hhd.get_normal()
|
||||||
vshear_ds = self.__hhd.get_shear()
|
vshear_ds = self.__hhd.get_shear()
|
||||||
|
|
||||||
if k > 1:
|
if k > 1:
|
||||||
self.vnormal = cv2.resize(vnormal_ds, (orig_w, orig_h), interpolation=cv2.INTER_LINEAR)
|
# Upsample into reusable buffers; the solver keeps owning its own
|
||||||
self.vshear = cv2.resize(vshear_ds, (orig_w, orig_h), interpolation=cv2.INTER_LINEAR)
|
# arrays, so nothing downstream can alias solver state.
|
||||||
|
vn = self._hhd_vn_buf
|
||||||
|
vs = self._hhd_vs_buf
|
||||||
|
if vn is None or vn.shape != (orig_h, orig_w, 2) or vn.dtype != np.float32:
|
||||||
|
vn = np.empty((orig_h, orig_w, 2), dtype=np.float32)
|
||||||
|
vs = np.empty((orig_h, orig_w, 2), dtype=np.float32)
|
||||||
|
self._hhd_vn_buf = vn
|
||||||
|
self._hhd_vs_buf = vs
|
||||||
|
cv2.resize(vnormal_ds, (orig_w, orig_h), dst=vn, interpolation=cv2.INTER_LINEAR)
|
||||||
|
cv2.resize(vshear_ds, (orig_w, orig_h), dst=vs, interpolation=cv2.INTER_LINEAR)
|
||||||
|
self.vnormal = vn
|
||||||
|
self.vshear = vs
|
||||||
else:
|
else:
|
||||||
self.vnormal = vnormal_ds
|
self.vnormal = vnormal_ds.copy()
|
||||||
self.vshear = vshear_ds
|
self.vshear = vshear_ds.copy()
|
||||||
|
|
||||||
if self.valid_mask is not None:
|
if self.valid_mask is not None:
|
||||||
active = self.valid_mask.astype(bool)
|
# In-place multiply by a 0/1 float mask instead of
|
||||||
self.vnormal = self.vnormal.copy()
|
# ``x.copy(); x[~active] = 0`` (copy + boolean gather/scatter).
|
||||||
self.vshear = self.vshear.copy()
|
m3 = self._hhd_mask_f32
|
||||||
self.vnormal[~active] = 0
|
if m3 is None or m3.shape != (orig_h, orig_w, 1) or m3.dtype != np.float32:
|
||||||
self.vshear[~active] = 0
|
m3 = np.empty((orig_h, orig_w, 1), dtype=np.float32)
|
||||||
|
self._hhd_mask_f32 = m3
|
||||||
|
np.copyto(m3[:, :, 0], self.valid_mask, casting="unsafe")
|
||||||
|
np.multiply(self.vnormal, m3, out=self.vnormal)
|
||||||
|
np.multiply(self.vshear, m3, out=self.vshear)
|
||||||
|
|
||||||
self.compute_deformation_valid = True
|
self.compute_deformation_valid = True
|
||||||
|
|
||||||
@@ -700,6 +1066,13 @@ class Sensor:
|
|||||||
"""Reset optical flow tracker and force-filter state."""
|
"""Reset optical flow tracker and force-filter state."""
|
||||||
self.__tracker.reset()
|
self.__tracker.reset()
|
||||||
self._force_filter.reset()
|
self._force_filter.reset()
|
||||||
|
if getattr(self, "_force_median", None) is not None:
|
||||||
|
self._force_median.reset()
|
||||||
|
self._ema_flow = None
|
||||||
|
# The unfiltered pair belongs to the frame it was computed for: a new
|
||||||
|
# session must not report the previous one's reading.
|
||||||
|
self._force_unfiltered = None
|
||||||
|
self._force_raw_unfiltered = None
|
||||||
if self.img is not None:
|
if self.img is not None:
|
||||||
self.initial_img = self.img.copy()
|
self.initial_img = self.img.copy()
|
||||||
|
|
||||||
@@ -730,7 +1103,14 @@ class Sensor:
|
|||||||
# self.cap = None
|
# self.cap = None
|
||||||
# if self.verbose:
|
# if self.verbose:
|
||||||
# print("Sensor disconnected.")
|
# print("Sensor disconnected.")
|
||||||
|
try:
|
||||||
cv2.destroyAllWindows()
|
cv2.destroyAllWindows()
|
||||||
|
except cv2.error:
|
||||||
|
# A headless OpenCV build (``opencv-python-headless``) has no highgui,
|
||||||
|
# so there is nothing to close. Failing here would leave the rest of
|
||||||
|
# the shutdown — and every caller that disconnects in a ``finally`` —
|
||||||
|
# reporting an error that says nothing about the sensor.
|
||||||
|
pass
|
||||||
|
|
||||||
# =========================================================================
|
# =========================================================================
|
||||||
# Public API - Configuration & Calibration
|
# Public API - Configuration & Calibration
|
||||||
@@ -743,6 +1123,8 @@ class Sensor:
|
|||||||
lowpass_sample_rate_hz=None,
|
lowpass_sample_rate_hz=None,
|
||||||
lowpass_order=None,
|
lowpass_order=None,
|
||||||
lowpass_enabled=None,
|
lowpass_enabled=None,
|
||||||
|
flow_ema_alpha=None,
|
||||||
|
force_median_window=None,
|
||||||
hhd_downsample=None,
|
hhd_downsample=None,
|
||||||
):
|
):
|
||||||
"""Update runtime-safe settings without reconnecting the camera.
|
"""Update runtime-safe settings without reconnecting the camera.
|
||||||
@@ -754,10 +1136,18 @@ class Sensor:
|
|||||||
value enables filtering unless ``lowpass_enabled=False``.
|
value enables filtering unless ``lowpass_enabled=False``.
|
||||||
lowpass_sample_rate_hz : float, optional
|
lowpass_sample_rate_hz : float, optional
|
||||||
Assumed force sample rate for the Butterworth design (default 30).
|
Assumed force sample rate for the Butterworth design (default 30).
|
||||||
|
Set this to the actual output rate of the deployment.
|
||||||
lowpass_order : int, optional
|
lowpass_order : int, optional
|
||||||
Butterworth order (default 2).
|
Butterworth order (default 2).
|
||||||
lowpass_enabled : bool, optional
|
lowpass_enabled : bool, optional
|
||||||
Explicit on/off for the causal force filter.
|
Explicit on/off for the causal force filter.
|
||||||
|
flow_ema_alpha : float, optional
|
||||||
|
EMA coefficient applied to the optical-flow field before HHD.
|
||||||
|
``1.0`` disables it; smaller values smooth more (0.5 is the default
|
||||||
|
and costs about one frame of delay at 30 fps).
|
||||||
|
force_median_window : int, optional
|
||||||
|
Causal sliding-median width applied to the raw force triple before
|
||||||
|
the low-pass. ``<= 1`` disables it; 3 is the default.
|
||||||
hhd_downsample : int, optional
|
hhd_downsample : int, optional
|
||||||
Integer ``>= 1``. Rebuilds the HHD solver on the next frame.
|
Integer ``>= 1``. Rebuilds the HHD solver on the next frame.
|
||||||
"""
|
"""
|
||||||
@@ -768,10 +1158,26 @@ class Sensor:
|
|||||||
if lowpass_enabled is None:
|
if lowpass_enabled is None:
|
||||||
self._lowpass_enabled = self._lowpass_cutoff_hz > 0
|
self._lowpass_enabled = self._lowpass_cutoff_hz > 0
|
||||||
if lowpass_sample_rate_hz is not None:
|
if lowpass_sample_rate_hz is not None:
|
||||||
|
# Accept a positive number to pin the rate, or "auto"/<=0 to track
|
||||||
|
# the measured output rate (see _update_force_sample_rate).
|
||||||
|
auto = False
|
||||||
|
rate = 0.0
|
||||||
|
if isinstance(lowpass_sample_rate_hz, str):
|
||||||
|
auto = lowpass_sample_rate_hz.strip().lower() in ("auto", "", "measured")
|
||||||
|
else:
|
||||||
|
try:
|
||||||
rate = float(lowpass_sample_rate_hz)
|
rate = float(lowpass_sample_rate_hz)
|
||||||
if rate <= 0:
|
except (TypeError, ValueError):
|
||||||
raise ValueError("lowpass_sample_rate_hz must be > 0")
|
auto = True
|
||||||
|
else:
|
||||||
|
auto = rate <= 0.0
|
||||||
|
self._lowpass_rate_auto = auto
|
||||||
|
if not auto:
|
||||||
self._lowpass_sample_rate_hz = rate
|
self._lowpass_sample_rate_hz = rate
|
||||||
|
elif self._lowpass_sample_rate_hz <= 0:
|
||||||
|
self._lowpass_sample_rate_hz = self._config_fps
|
||||||
|
if getattr(self, "_force_rate", None) is not None:
|
||||||
|
self._force_rate.reset(self._config_fps)
|
||||||
filter_changed = True
|
filter_changed = True
|
||||||
if lowpass_order is not None:
|
if lowpass_order is not None:
|
||||||
order = int(lowpass_order)
|
order = int(lowpass_order)
|
||||||
@@ -783,6 +1189,22 @@ class Sensor:
|
|||||||
self._lowpass_enabled = bool(lowpass_enabled)
|
self._lowpass_enabled = bool(lowpass_enabled)
|
||||||
filter_changed = True
|
filter_changed = True
|
||||||
|
|
||||||
|
if flow_ema_alpha is not None:
|
||||||
|
alpha = float(flow_ema_alpha)
|
||||||
|
if not (0.0 < alpha <= 1.0):
|
||||||
|
raise ValueError("flow_ema_alpha must be in (0, 1]")
|
||||||
|
self._flow_ema_alpha = alpha
|
||||||
|
self._ema_flow = None
|
||||||
|
|
||||||
|
if force_median_window is not None:
|
||||||
|
try:
|
||||||
|
window = max(1, int(force_median_window))
|
||||||
|
except (TypeError, ValueError) as exc:
|
||||||
|
raise ValueError("force_median_window must be an integer >= 1") from exc
|
||||||
|
self._force_median_window = window
|
||||||
|
self._force_median.configure(window)
|
||||||
|
filter_changed = True
|
||||||
|
|
||||||
if filter_changed:
|
if filter_changed:
|
||||||
cutoff = self._lowpass_cutoff_hz if self._lowpass_enabled else 0.0
|
cutoff = self._lowpass_cutoff_hz if self._lowpass_enabled else 0.0
|
||||||
self._force_filter.configure(
|
self._force_filter.configure(
|
||||||
@@ -802,6 +1224,7 @@ class Sensor:
|
|||||||
self._hhd_downsample = k
|
self._hhd_downsample = k
|
||||||
self.__hhd = None
|
self.__hhd = None
|
||||||
self._hhd_grid_shape = None
|
self._hhd_grid_shape = None
|
||||||
|
self._hhd_ref_shape = None
|
||||||
self._update_hhd_mask_cache()
|
self._update_hhd_mask_cache()
|
||||||
return self
|
return self
|
||||||
|
|
||||||
@@ -876,12 +1299,24 @@ class Sensor:
|
|||||||
self.calibration = cal_mod.load(cal, strict=True)
|
self.calibration = cal_mod.load(cal, strict=True)
|
||||||
|
|
||||||
def _apply_force_filter(self):
|
def _apply_force_filter(self):
|
||||||
raw = np.asarray(self._force_raw, dtype=np.float64)
|
pre_filter = np.asarray(self._force_raw, dtype=np.float64)
|
||||||
|
raw = pre_filter
|
||||||
|
# Median first: it deletes the heavy-tailed spikes that the flow chain
|
||||||
|
# produces, so the low-pass does not have to spread them over time.
|
||||||
|
median = getattr(self, "_force_median", None)
|
||||||
|
if median is not None and median.enabled:
|
||||||
|
raw = median.process(raw)
|
||||||
if self._lowpass_enabled and self._lowpass_cutoff_hz > 0:
|
if self._lowpass_enabled and self._lowpass_cutoff_hz > 0:
|
||||||
filtered = self._force_filter.process(raw)
|
raw = self._force_filter.process(raw)
|
||||||
fn, fx, fy = (float(v) for v in filtered)
|
# The chain's own output, before calibration: the "filtered raw counts".
|
||||||
else:
|
# Without this the raw axis would have no filtered/unfiltered pair and
|
||||||
fn, fx, fy = float(raw[0]), float(raw[1]), float(raw[2])
|
# ``force_triplets`` could only offer the device counts twice.
|
||||||
|
self._force_filtered_raw = (
|
||||||
|
float(raw[0]),
|
||||||
|
float(raw[1]),
|
||||||
|
float(raw[2]),
|
||||||
|
)
|
||||||
|
fn, fx, fy = self._force_filtered_raw
|
||||||
|
|
||||||
# Apply loaded force_calibration.json when present; otherwise keep raw.
|
# Apply loaded force_calibration.json when present; otherwise keep raw.
|
||||||
if self.calibration is not None:
|
if self.calibration is not None:
|
||||||
@@ -891,6 +1326,86 @@ class Sensor:
|
|||||||
|
|
||||||
self.fnormal, self.fshearx, self.fsheary = fn, fx, fy
|
self.fnormal, self.fshearx, self.fsheary = fn, fx, fy
|
||||||
|
|
||||||
|
# The same reading with no readout filtering, so a caller can keep both
|
||||||
|
# (the app stores the filtered values as the record and the unfiltered
|
||||||
|
# ones beside them). Calibrated with the same coefficients, so the two
|
||||||
|
# only differ by the filter chain.
|
||||||
|
ufn, ufx, ufy = (float(pre_filter[0]), float(pre_filter[1]), float(pre_filter[2]))
|
||||||
|
if self.calibration is not None:
|
||||||
|
ufn, ufx, ufy = self.calibration.convert(ufn, ufx, ufy)
|
||||||
|
ufx, ufy, ufn = float(ufx), float(ufy), float(ufn)
|
||||||
|
self._force_raw_unfiltered = (
|
||||||
|
float(pre_filter[0]), float(pre_filter[1]), float(pre_filter[2])
|
||||||
|
)
|
||||||
|
self._force_unfiltered = (ufn, ufx, ufy)
|
||||||
|
|
||||||
|
# ------------------------------------------------------- filter contract
|
||||||
|
def filters_state(self) -> dict:
|
||||||
|
"""What the filter chain is **actually** doing, for records and audits.
|
||||||
|
|
||||||
|
Read this instead of the config file: a config can be overridden at
|
||||||
|
runtime (``configure``) and a value that is present is never silently
|
||||||
|
replaced, so the effective state is the only honest answer. The two
|
||||||
|
groups are kept apart on purpose:
|
||||||
|
|
||||||
|
* ``measurement`` — stages upstream of the HHD/force computation
|
||||||
|
(``flow_ema_alpha``): turning them on changes *what is measured*, so
|
||||||
|
two recordings with different values are not comparable;
|
||||||
|
* ``readout`` — stages that only smooth the force triple on its way out
|
||||||
|
(median, low-pass): recorded, but they do not change the physical
|
||||||
|
claim.
|
||||||
|
"""
|
||||||
|
median_applied = bool(getattr(self._force_median, "enabled", False))
|
||||||
|
tracker_cfg = self._config.get("tracker") or {}
|
||||||
|
return {
|
||||||
|
"preset": getattr(self, "_filter_preset", None),
|
||||||
|
"measurement": {
|
||||||
|
"flow_ema_alpha": float(self._flow_ema_alpha),
|
||||||
|
"flow_ema_applied": bool(self._flow_ema_alpha < 1.0),
|
||||||
|
"hhd_downsample": int(getattr(self, "hhd_downsample", 0) or 0),
|
||||||
|
"dis_mode": tracker_cfg.get("dis_mode"),
|
||||||
|
},
|
||||||
|
"readout": {
|
||||||
|
"force_median_window": int(self._force_median_window),
|
||||||
|
"force_median_applied": median_applied,
|
||||||
|
"force_lowpass_cutoff_hz": float(self._lowpass_cutoff_hz),
|
||||||
|
"force_lowpass_order": int(self._lowpass_order),
|
||||||
|
"force_lowpass_sample_rate_hz": float(self._lowpass_sample_rate_hz),
|
||||||
|
"force_lowpass_rate_auto": bool(self._lowpass_rate_auto),
|
||||||
|
"force_lowpass_applied": bool(self._lowpass_enabled),
|
||||||
|
"sample_rate_source": str(getattr(self, "_force_rate_source", "config")),
|
||||||
|
},
|
||||||
|
"any_stage_applied": bool(self._filters_enabled),
|
||||||
|
"unfiltered_available": True,
|
||||||
|
"source": "sensor.filters_state",
|
||||||
|
}
|
||||||
|
|
||||||
|
def force_triplets(self) -> dict:
|
||||||
|
"""The filtered and unfiltered force triples of the current frame.
|
||||||
|
|
||||||
|
One call, so a sampling thread reads a single consistent frame: the
|
||||||
|
filtered pair is what ``read_info`` reports and what a recording stores
|
||||||
|
as its primary columns, the unfiltered pair is the same reading with the
|
||||||
|
readout stages bypassed.
|
||||||
|
"""
|
||||||
|
raw_unfiltered = getattr(self, "_force_raw_unfiltered", None)
|
||||||
|
unfiltered = getattr(self, "_force_unfiltered", None)
|
||||||
|
filtered_raw = getattr(self, "_force_filtered_raw", None)
|
||||||
|
return {
|
||||||
|
"filtered": {
|
||||||
|
"raw": tuple(filtered_raw) if filtered_raw is not None else None,
|
||||||
|
"newton": (
|
||||||
|
float(self.fnormal),
|
||||||
|
float(self.fshearx),
|
||||||
|
float(self.fsheary),
|
||||||
|
),
|
||||||
|
},
|
||||||
|
"unfiltered": {
|
||||||
|
"raw": tuple(raw_unfiltered) if raw_unfiltered is not None else None,
|
||||||
|
"newton": tuple(unfiltered) if unfiltered is not None else None,
|
||||||
|
},
|
||||||
|
}
|
||||||
|
|
||||||
# def export_config(self, path="./config/exported_config.json"):
|
# def export_config(self, path="./config/exported_config.json"):
|
||||||
# """Return current configuration dict."""
|
# """Return current configuration dict."""
|
||||||
# configs.export_config(self._config, path)
|
# configs.export_config(self._config, path)
|
||||||
|
|||||||
@@ -15,6 +15,8 @@ Prefer ``Sensor.calibration`` / ``read_info(FNORMAL, …)`` in new code.
|
|||||||
|
|
||||||
from __future__ import annotations
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import collections
|
||||||
|
import time
|
||||||
from dataclasses import dataclass
|
from dataclasses import dataclass
|
||||||
|
|
||||||
import numpy as np
|
import numpy as np
|
||||||
@@ -312,3 +314,115 @@ class LowpassFilter:
|
|||||||
self.reset(x)
|
self.reset(x)
|
||||||
y, self._zi = sosfilt(self._sos, x[None, :], zi=self._zi, axis=0)
|
y, self._zi = sosfilt(self._sos, x[None, :], zi=self._zi, axis=0)
|
||||||
return np.asarray(y[0], dtype=np.float64)
|
return np.asarray(y[0], dtype=np.float64)
|
||||||
|
|
||||||
|
|
||||||
|
class CausalMedianFilter:
|
||||||
|
"""Causal sliding-median over the last ``window`` samples, one sample at a time.
|
||||||
|
|
||||||
|
The optical-flow force chain produces heavy-tailed jitter (rare multi-sigma
|
||||||
|
spikes). A linear low-pass spreads such a spike over several frames instead
|
||||||
|
of deleting it, whereas a median removes it outright - measured on idle
|
||||||
|
data, a 5-sample median cut the jitter 4x better than a moving average of
|
||||||
|
the same width.
|
||||||
|
|
||||||
|
``window <= 1`` disables the filter (pass-through). Until ``window`` samples
|
||||||
|
have arrived the median of the available samples is returned, so the output
|
||||||
|
is well defined from the first frame.
|
||||||
|
"""
|
||||||
|
|
||||||
|
def __init__(self, window: int = 1, n_channels: int = 3):
|
||||||
|
self.n_channels = int(n_channels)
|
||||||
|
self.window = 1
|
||||||
|
self._buf = None
|
||||||
|
self.configure(window)
|
||||||
|
|
||||||
|
@property
|
||||||
|
def enabled(self) -> bool:
|
||||||
|
return self.window > 1
|
||||||
|
|
||||||
|
def configure(self, window: int | None = None) -> None:
|
||||||
|
if window is not None:
|
||||||
|
try:
|
||||||
|
self.window = max(1, int(window))
|
||||||
|
except (TypeError, ValueError):
|
||||||
|
self.window = 1
|
||||||
|
self._buf = None
|
||||||
|
|
||||||
|
def reset(self, x0=None) -> None:
|
||||||
|
"""Drop the window history (e.g. after a tracker reset)."""
|
||||||
|
self._buf = None
|
||||||
|
|
||||||
|
def process(self, values) -> np.ndarray:
|
||||||
|
"""Filter one multi-channel sample. Returns float64 array of shape (n_channels,)."""
|
||||||
|
x = np.asarray(values, dtype=np.float64).reshape(-1)
|
||||||
|
if x.size != self.n_channels:
|
||||||
|
raise ValueError(f"expected {self.n_channels} channels, got {x.size}")
|
||||||
|
if not self.enabled:
|
||||||
|
return x.copy()
|
||||||
|
if self._buf is None or self._buf.shape[1] != x.size:
|
||||||
|
self._buf = x[None, :].copy()
|
||||||
|
return x.copy()
|
||||||
|
self._buf = np.vstack((self._buf, x[None, :]))[-self.window:]
|
||||||
|
return np.median(self._buf, axis=0)
|
||||||
|
|
||||||
|
|
||||||
|
class SampleRateEstimator:
|
||||||
|
"""Robust online estimate of the rate at which :meth:`tick` is called.
|
||||||
|
|
||||||
|
Uses the *median* of the most recent inter-call intervals, so one stall or
|
||||||
|
scheduling hiccup does not poison the estimate. :attr:`rate_hz` returns the
|
||||||
|
seed value until ``min_samples`` intervals have been observed.
|
||||||
|
|
||||||
|
This exists because a fixed floor filter designed for 30 Hz behaves
|
||||||
|
differently at 60 Hz: the cutoff stays put in Hz but the per-frame
|
||||||
|
smoothing halves. Measuring the achieved rate keeps the filter identical in
|
||||||
|
the time domain whatever the deployment runs at.
|
||||||
|
"""
|
||||||
|
|
||||||
|
def __init__(
|
||||||
|
self,
|
||||||
|
initial_hz: float = 30.0,
|
||||||
|
window: int = 45,
|
||||||
|
min_samples: int = 8,
|
||||||
|
max_interval_s: float = 1.0,
|
||||||
|
):
|
||||||
|
self.initial_hz = float(initial_hz) if initial_hz else 30.0
|
||||||
|
self.window = max(3, int(window))
|
||||||
|
self.min_samples = max(2, int(min_samples))
|
||||||
|
self.max_interval_s = float(max_interval_s)
|
||||||
|
self.reset()
|
||||||
|
|
||||||
|
def reset(self, initial_hz: float | None = None) -> None:
|
||||||
|
if initial_hz is not None:
|
||||||
|
self.initial_hz = float(initial_hz) if initial_hz else 30.0
|
||||||
|
self._dts: collections.deque = collections.deque(maxlen=self.window)
|
||||||
|
self._last = None
|
||||||
|
self._rate = self.initial_hz
|
||||||
|
self._measured = False
|
||||||
|
|
||||||
|
@property
|
||||||
|
def rate_hz(self) -> float:
|
||||||
|
return self._rate
|
||||||
|
|
||||||
|
@property
|
||||||
|
def measured(self) -> bool:
|
||||||
|
"""True once the estimate comes from real intervals."""
|
||||||
|
return self._measured
|
||||||
|
|
||||||
|
def tick(self, now: float | None = None) -> float:
|
||||||
|
"""Register one sample and return the current rate estimate."""
|
||||||
|
now = time.perf_counter() if now is None else float(now)
|
||||||
|
last, self._last = self._last, now
|
||||||
|
if last is None:
|
||||||
|
return self._rate
|
||||||
|
dt = now - last
|
||||||
|
if not (1e-5 < dt <= self.max_interval_s):
|
||||||
|
# Stalls, pauses and the first frame carry no rate information.
|
||||||
|
return self._rate
|
||||||
|
self._dts.append(dt)
|
||||||
|
if len(self._dts) >= self.min_samples:
|
||||||
|
dt_med = float(np.median(self._dts))
|
||||||
|
if dt_med > 0.0:
|
||||||
|
self._rate = 1.0 / dt_med
|
||||||
|
self._measured = True
|
||||||
|
return self._rate
|
||||||
|
|||||||
+18
-1
@@ -94,13 +94,30 @@ class Tracker:
|
|||||||
return mask.astype(bool)
|
return mask.astype(bool)
|
||||||
|
|
||||||
def apply_valid_mask(self, flow, valid_mask, erode_iters=1):
|
def apply_valid_mask(self, flow, valid_mask, erode_iters=1):
|
||||||
"""Zero flow outside the valid UV domain."""
|
"""Zero flow outside the valid UV domain (in place when safe).
|
||||||
|
|
||||||
|
Multiplying by a 0/1 mask is a single streaming pass, whereas
|
||||||
|
``flow.copy()`` followed by a boolean fancy-index assignment costs a
|
||||||
|
full copy plus a gather/scatter over the masked-out pixels.
|
||||||
|
|
||||||
|
Note: a masked-out *negative* component becomes ``-0.0`` rather than
|
||||||
|
``+0.0``. The two compare, sum and magnitude identically; only the raw
|
||||||
|
bit pattern of those zeros differs from the previous implementation.
|
||||||
|
"""
|
||||||
if flow is None or valid_mask is None:
|
if flow is None or valid_mask is None:
|
||||||
return flow
|
return flow
|
||||||
mask = self._prepare_valid_mask(valid_mask, flow.shape[:2], erode_iters=erode_iters)
|
mask = self._prepare_valid_mask(valid_mask, flow.shape[:2], erode_iters=erode_iters)
|
||||||
|
if not getattr(self, "_Tracker__adaptive", False):
|
||||||
|
# The non-adaptive path hands back the DIS state buffer, which feeds
|
||||||
|
# the next frame as its initial guess; never mutate it.
|
||||||
flow_out = flow.copy()
|
flow_out = flow.copy()
|
||||||
flow_out[~mask] = 0
|
flow_out[~mask] = 0
|
||||||
return flow_out
|
return flow_out
|
||||||
|
if not flow.flags.writeable:
|
||||||
|
flow = flow.copy()
|
||||||
|
mask_f = mask.astype(np.float32, copy=False)[:, :, None]
|
||||||
|
np.multiply(flow, mask_f, out=flow)
|
||||||
|
return flow
|
||||||
|
|
||||||
def track(self, frame, valid_mask=None):
|
def track(self, frame, valid_mask=None):
|
||||||
"""Main tracking function that dispatches to CUDA or CPU implementation"""
|
"""Main tracking function that dispatches to CUDA or CPU implementation"""
|
||||||
|
|||||||
+4
-1
@@ -75,7 +75,10 @@ def draw_arrows(
|
|||||||
)
|
)
|
||||||
if mask is not None:
|
if mask is not None:
|
||||||
valid &= mask[rr, cc]
|
valid &= mask[rr, cc]
|
||||||
valid &= mask[end_y, end_x]
|
# Arrow tips may fall outside the frame (the length cap is applied
|
||||||
|
# before the bounds check above), so clamp before indexing: those points
|
||||||
|
# are already False in ``valid``, and in-bounds indices are unchanged.
|
||||||
|
valid &= mask[np.clip(end_y, 0, h - 1), np.clip(end_x, 0, w - 1)]
|
||||||
|
|
||||||
mag_range = mag.max() - mag.min()
|
mag_range = mag.max() - mag.min()
|
||||||
norm_mag = (
|
norm_mag = (
|
||||||
|
|||||||
@@ -3,6 +3,7 @@
|
|||||||
from __future__ import annotations
|
from __future__ import annotations
|
||||||
|
|
||||||
import json
|
import json
|
||||||
|
import shutil
|
||||||
from pathlib import Path
|
from pathlib import Path
|
||||||
|
|
||||||
import pytest
|
import pytest
|
||||||
@@ -13,8 +14,11 @@ from orisys.calibration import (
|
|||||||
CalibrationError,
|
CalibrationError,
|
||||||
build,
|
build,
|
||||||
check,
|
check,
|
||||||
|
filter_mismatches,
|
||||||
|
filters_fingerprint,
|
||||||
load,
|
load,
|
||||||
parse,
|
parse,
|
||||||
|
pipeline_from_sensor_json,
|
||||||
write,
|
write,
|
||||||
)
|
)
|
||||||
|
|
||||||
@@ -241,3 +245,186 @@ def test_parse_shipped_box_file_directly():
|
|||||||
cal = parse(json.loads(_box_cal_path().read_text(encoding="utf-8")), source_path=_box_cal_path())
|
cal = parse(json.loads(_box_cal_path().read_text(encoding="utf-8")), source_path=_box_cal_path())
|
||||||
assert cal.schema_version == 1
|
assert cal.schema_version == 1
|
||||||
assert cal.convert(10000.0, 36000.0, -36000.0) == pytest.approx((0.377, 1.19, -1.19))
|
assert cal.convert(10000.0, 36000.0, -36000.0) == pytest.approx((0.377, 1.19, -1.19))
|
||||||
|
|
||||||
|
|
||||||
|
# ------------------------------------------------------- filter fingerprint
|
||||||
|
ALL_OFF_FILTERS = {
|
||||||
|
"flow_ema_alpha": 1.0,
|
||||||
|
"force_median_window": 1,
|
||||||
|
"force_lowpass_cutoff_hz": 0.0,
|
||||||
|
"force_lowpass_order": 2,
|
||||||
|
"force_lowpass_sample_rate_hz": "auto",
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
def _sensor_with_filters(tmp_path: Path, filters: dict) -> Sensor:
|
||||||
|
"""An external-frame Sensor whose bundle carries ``filters``."""
|
||||||
|
bundle = _box_cal_path().parent
|
||||||
|
copy = tmp_path / "box"
|
||||||
|
shutil.copytree(bundle, copy)
|
||||||
|
path = copy / "sensor.json"
|
||||||
|
cfg = json.loads(path.read_text(encoding="utf-8"))
|
||||||
|
cfg["filters"] = filters
|
||||||
|
path.write_text(json.dumps(cfg, indent=2), encoding="utf-8")
|
||||||
|
return Sensor(
|
||||||
|
"external",
|
||||||
|
config_name=str(path),
|
||||||
|
cal_path=str(copy / "force_calibration.json"),
|
||||||
|
verbose=False,
|
||||||
|
backend="cpu",
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def test_the_fingerprint_keeps_measurement_and_readout_apart():
|
||||||
|
"""A change to one group must not be confused with a change to the other."""
|
||||||
|
fp = filters_fingerprint(ALL_OFF_FILTERS)
|
||||||
|
assert fp["measurement"] == {"flow_ema_alpha": 1.0}
|
||||||
|
assert fp["readout"] == {
|
||||||
|
"force_median_window": 1,
|
||||||
|
"force_lowpass_cutoff_hz": 0.0,
|
||||||
|
"force_lowpass_order": 2,
|
||||||
|
"force_lowpass_rate_auto": True,
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_preset_expands_without_inventing_anything():
|
||||||
|
fp = filters_fingerprint({"preset": "low_noise"})
|
||||||
|
assert fp["preset"] == "low_noise"
|
||||||
|
assert fp["measurement"] == {"flow_ema_alpha": 0.3}
|
||||||
|
assert fp["readout"] == {"force_median_window": 3, "force_lowpass_cutoff_hz": 10.0}
|
||||||
|
# An explicit key wins over the preset.
|
||||||
|
explicit = filters_fingerprint({"preset": "low_noise", "force_lowpass_cutoff_hz": 0.0})
|
||||||
|
assert explicit["readout"]["force_lowpass_cutoff_hz"] == 0.0
|
||||||
|
# Keys no source states are absent, never filled in from a default.
|
||||||
|
assert filters_fingerprint({}) == {}
|
||||||
|
assert filters_fingerprint(None) == {}
|
||||||
|
assert filters_fingerprint({"flow_ema_alpha": "not-a-number"}) == {}
|
||||||
|
|
||||||
|
|
||||||
|
def test_the_deprecated_master_switch_still_means_all_off():
|
||||||
|
fp = filters_fingerprint(dict(ALL_OFF_FILTERS, enabled=False, flow_ema_alpha=0.3))
|
||||||
|
assert fp["measurement"] == {"flow_ema_alpha": 1.0}
|
||||||
|
assert fp["readout"]["force_median_window"] == 1
|
||||||
|
assert fp["readout"]["force_lowpass_cutoff_hz"] == 0.0
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_measurement_change_warns_and_a_readout_change_only_notes():
|
||||||
|
recorded = filters_fingerprint({"preset": "balanced"})
|
||||||
|
expected = filters_fingerprint({"preset": "low_noise"})
|
||||||
|
warns, notes = filter_mismatches(recorded, expected)
|
||||||
|
assert any("not comparable" in w and "flow_ema_alpha" in w for w in warns)
|
||||||
|
assert not any("flow_ema_alpha" in n for n in notes)
|
||||||
|
assert any("force_lowpass_cutoff_hz" in n for n in notes)
|
||||||
|
assert filter_mismatches(recorded, recorded) == ([], [])
|
||||||
|
|
||||||
|
|
||||||
|
def test_check_reports_the_two_groups_apart():
|
||||||
|
samples = {
|
||||||
|
key: [
|
||||||
|
CalSample(weight_n=0.0, raw_mean=0.0, n=10),
|
||||||
|
CalSample(weight_n=1.0, raw_mean=1000.0, n=10),
|
||||||
|
]
|
||||||
|
for key in ("fn", "fx", "fy")
|
||||||
|
}
|
||||||
|
doc = build(
|
||||||
|
samples,
|
||||||
|
identity={"sensor_type": "box", "sn": "FP-1"},
|
||||||
|
pipeline={"filters": filters_fingerprint({"preset": "balanced"})},
|
||||||
|
)
|
||||||
|
result = check(doc, pipeline={"filters": filters_fingerprint({"preset": "low_noise"})})
|
||||||
|
assert result.ok, "a filter difference must not refuse a calibration"
|
||||||
|
assert any("not comparable" in w for w in result.warnings)
|
||||||
|
assert any("readout" in n for n in result.notes)
|
||||||
|
assert not any("readout" in w for w in result.warnings)
|
||||||
|
|
||||||
|
|
||||||
|
def test_the_fingerprint_survives_a_write_and_read(tmp_path: Path):
|
||||||
|
samples = {
|
||||||
|
key: [
|
||||||
|
CalSample(weight_n=0.0, raw_mean=0.0, n=10),
|
||||||
|
CalSample(weight_n=1.0, raw_mean=1000.0, n=10),
|
||||||
|
]
|
||||||
|
for key in ("fn", "fx", "fy")
|
||||||
|
}
|
||||||
|
doc = build(
|
||||||
|
samples,
|
||||||
|
identity={"sensor_type": "box", "sn": "FP-2"},
|
||||||
|
pipeline={"filters": filters_fingerprint({"preset": "balanced"})},
|
||||||
|
)
|
||||||
|
path = tmp_path / "fp.json"
|
||||||
|
write(doc, path, overwrite=True)
|
||||||
|
raw = json.loads(path.read_text(encoding="utf-8"))
|
||||||
|
assert raw["meta"]["filters"]["measurement"] == {"flow_ema_alpha": 0.5}
|
||||||
|
# The flat keys stay for readers that look them up by name.
|
||||||
|
assert raw["meta"]["flow_ema_alpha"] == 0.5
|
||||||
|
|
||||||
|
loaded = load(path)
|
||||||
|
assert loaded.pipeline["filters"]["measurement"] == {"flow_ema_alpha": 0.5}
|
||||||
|
assert loaded.pipeline["filters"]["readout"]["force_median_window"] == 3
|
||||||
|
assert loaded.pipeline["filters"]["preset"] == "balanced"
|
||||||
|
assert loaded.to_dict()["meta"]["filters"] == doc.to_dict()["meta"]["filters"]
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_legacy_flat_fingerprint_is_still_read_back(tmp_path: Path):
|
||||||
|
"""Documents written before the groups existed keep being comparable."""
|
||||||
|
samples = {
|
||||||
|
key: [
|
||||||
|
CalSample(weight_n=0.0, raw_mean=0.0, n=10),
|
||||||
|
CalSample(weight_n=1.0, raw_mean=1000.0, n=10),
|
||||||
|
]
|
||||||
|
for key in ("fn", "fx", "fy")
|
||||||
|
}
|
||||||
|
doc = build(samples, identity={"sensor_type": "box", "sn": "FP-3"})
|
||||||
|
legacy = json.loads(json.dumps(doc.to_dict()))
|
||||||
|
legacy["meta"].pop("filters", None)
|
||||||
|
legacy["meta"].update(
|
||||||
|
{"flow_ema_alpha": 0.3, "force_median_window": 3, "force_lowpass_cutoff_hz": 10.0}
|
||||||
|
)
|
||||||
|
path = tmp_path / "legacy.json"
|
||||||
|
path.write_text(json.dumps(legacy), encoding="utf-8")
|
||||||
|
loaded = load(path)
|
||||||
|
assert loaded.pipeline["filters"]["measurement"] == {"flow_ema_alpha": 0.3}
|
||||||
|
assert loaded.pipeline["filters"]["readout"]["force_median_window"] == 3
|
||||||
|
assert loaded.pipeline["filters"]["readout"]["force_lowpass_cutoff_hz"] == 10.0
|
||||||
|
|
||||||
|
|
||||||
|
def test_the_effective_state_beats_the_config_file(tmp_path: Path):
|
||||||
|
"""A file states an intent; the device states what it is doing."""
|
||||||
|
bundle_json = _box_cal_path().parent / "sensor.json"
|
||||||
|
from_file = pipeline_from_sensor_json(bundle_json)
|
||||||
|
assert from_file["filters"]["measurement"] == {"flow_ema_alpha": 1.0}
|
||||||
|
assert "flow_ema_alpha" not in from_file, "loose top-level filter keys are gone"
|
||||||
|
|
||||||
|
sensor = _sensor_with_filters(tmp_path, {"preset": "balanced"})
|
||||||
|
try:
|
||||||
|
from_state = pipeline_from_sensor_json(
|
||||||
|
bundle_json, filters_state=sensor.filters_state()
|
||||||
|
)
|
||||||
|
finally:
|
||||||
|
sensor.disconnect()
|
||||||
|
assert from_state["filters"]["measurement"] == {"flow_ema_alpha": 0.5}
|
||||||
|
assert from_state["filters"]["readout"]["force_median_window"] == 3
|
||||||
|
assert from_state["filters"]["source"] == "sensor.filters_state"
|
||||||
|
assert from_state["config_ref"] == str(bundle_json)
|
||||||
|
|
||||||
|
|
||||||
|
@pytest.mark.parametrize(
|
||||||
|
"preset, alpha, median, cutoff",
|
||||||
|
[
|
||||||
|
("off", 1.0, 1, 0.0),
|
||||||
|
("low_latency", 1.0, 3, 0.0),
|
||||||
|
("balanced", 0.5, 3, 0.0),
|
||||||
|
("low_noise", 0.3, 3, 10.0),
|
||||||
|
],
|
||||||
|
)
|
||||||
|
def test_every_preset_reports_what_it_does(
|
||||||
|
tmp_path: Path, preset: str, alpha: float, median: int, cutoff: float
|
||||||
|
):
|
||||||
|
sensor = _sensor_with_filters(tmp_path, {"preset": preset})
|
||||||
|
try:
|
||||||
|
fp = filters_fingerprint(sensor.filters_state())
|
||||||
|
finally:
|
||||||
|
sensor.disconnect()
|
||||||
|
assert fp["measurement"]["flow_ema_alpha"] == pytest.approx(alpha)
|
||||||
|
assert fp["readout"]["force_median_window"] == median
|
||||||
|
assert fp["readout"]["force_lowpass_cutoff_hz"] == pytest.approx(cutoff)
|
||||||
|
|||||||
@@ -0,0 +1,116 @@
|
|||||||
|
"""orisys.util.draw_arrows: mask handling and the out-of-frame tip regression."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import cv2
|
||||||
|
import numpy as np
|
||||||
|
import pytest
|
||||||
|
|
||||||
|
from orisys.util import _prepare_draw_mask, draw_arrows
|
||||||
|
|
||||||
|
|
||||||
|
def flow_of(shape, dx, dy):
|
||||||
|
flow = np.zeros((*shape, 2), dtype=np.float32)
|
||||||
|
flow[..., 0] = dx / 2.0 # draw_arrows doubles the stored value
|
||||||
|
flow[..., 1] = dy / 2.0
|
||||||
|
return flow
|
||||||
|
|
||||||
|
|
||||||
|
def background(frame):
|
||||||
|
return cv2.cvtColor(frame, cv2.COLOR_GRAY2BGR)
|
||||||
|
|
||||||
|
|
||||||
|
def test_out_of_frame_arrow_tip_is_dropped_not_raised():
|
||||||
|
"""Regression: mask[end_y, end_x] was indexed before the bounds check.
|
||||||
|
|
||||||
|
A 40 px grid with the default edge_margin of 15 lets an arrow tip leave the
|
||||||
|
frame, which used to raise IndexError as soon as a valid_mask was supplied.
|
||||||
|
"""
|
||||||
|
frame = np.full((48, 48), 100, dtype=np.uint8)
|
||||||
|
flow = flow_of((48, 48), dx=40.0, dy=0.0)
|
||||||
|
mask = np.ones((48, 48), dtype=bool)
|
||||||
|
|
||||||
|
out = draw_arrows(frame, flow, grid_spacing=40, margin=0.8, valid_mask=mask)
|
||||||
|
|
||||||
|
assert out.shape == (48, 48, 3)
|
||||||
|
# The only grid point (20, 20) has a tip at x=58 in a 48 px frame, so the
|
||||||
|
# bounds check must have discarded it: nothing was drawn.
|
||||||
|
assert np.array_equal(out, background(frame))
|
||||||
|
|
||||||
|
|
||||||
|
def test_out_of_frame_tip_matches_render_without_mask():
|
||||||
|
"""Clamping the mask lookup must not change which points are valid."""
|
||||||
|
frame = np.full((48, 48), 100, dtype=np.uint8)
|
||||||
|
flow = flow_of((48, 48), dx=40.0, dy=0.0)
|
||||||
|
mask = np.ones((48, 48), dtype=bool)
|
||||||
|
|
||||||
|
with_mask = draw_arrows(frame, flow, grid_spacing=40, valid_mask=mask)
|
||||||
|
without = draw_arrows(frame, flow, grid_spacing=40)
|
||||||
|
assert np.array_equal(with_mask, without)
|
||||||
|
|
||||||
|
|
||||||
|
def test_in_frame_arrows_are_drawn_through_an_all_true_mask():
|
||||||
|
frame = np.full((96, 96), 90, dtype=np.uint8)
|
||||||
|
flow = flow_of((96, 96), dx=6.0, dy=3.0)
|
||||||
|
mask = np.ones((96, 96), dtype=bool)
|
||||||
|
|
||||||
|
out = draw_arrows(frame, flow, grid_spacing=12, arrow_scale=1.0,
|
||||||
|
threshold=0.1, valid_mask=mask)
|
||||||
|
assert not np.array_equal(out, background(frame))
|
||||||
|
|
||||||
|
|
||||||
|
def test_all_false_mask_draws_nothing():
|
||||||
|
frame = np.full((96, 96), 90, dtype=np.uint8)
|
||||||
|
flow = flow_of((96, 96), dx=6.0, dy=3.0)
|
||||||
|
mask = np.zeros((96, 96), dtype=bool)
|
||||||
|
|
||||||
|
out = draw_arrows(frame, flow, grid_spacing=12, arrow_scale=1.0,
|
||||||
|
threshold=0.1, valid_mask=mask)
|
||||||
|
assert np.array_equal(out, background(frame))
|
||||||
|
|
||||||
|
|
||||||
|
def test_mask_can_exclude_one_side():
|
||||||
|
"""Endpoints outside the mask are skipped even when inside the frame."""
|
||||||
|
frame = np.full((96, 96), 90, dtype=np.uint8)
|
||||||
|
flow = flow_of((96, 96), dx=8.0, dy=0.0)
|
||||||
|
mask = np.zeros((96, 96), dtype=bool)
|
||||||
|
mask[24:48, 24:72] = True # only the upper middle band
|
||||||
|
|
||||||
|
out = draw_arrows(frame, flow, grid_spacing=12, arrow_scale=1.0,
|
||||||
|
threshold=0.1, valid_mask=mask)
|
||||||
|
assert not np.array_equal(out, background(frame))
|
||||||
|
# Rows below the band stay untouched.
|
||||||
|
assert np.array_equal(out[80:, :, :], background(frame)[80:, :, :])
|
||||||
|
|
||||||
|
|
||||||
|
def test_below_threshold_points_get_the_dot_colour():
|
||||||
|
frame = np.full((96, 96), 90, dtype=np.uint8)
|
||||||
|
flow = flow_of((96, 96), dx=0.05, dy=0.05)
|
||||||
|
out = draw_arrows(frame, flow, grid_spacing=12, arrow_scale=1.0,
|
||||||
|
threshold=1.0, below_threshold_color=(0, 0, 255))
|
||||||
|
# OpenCV colours are BGR, so (0, 0, 255) is red.
|
||||||
|
red = (out[:, :, 2] > 150) & (out[:, :, 0] < 100) & (out[:, :, 1] < 100)
|
||||||
|
assert red.sum() > 0
|
||||||
|
|
||||||
|
|
||||||
|
def test_prepare_draw_mask_resizes_to_frame():
|
||||||
|
mask = np.zeros((10, 20), dtype=bool)
|
||||||
|
mask[:, :10] = True
|
||||||
|
out = _prepare_draw_mask(mask, (40, 40))
|
||||||
|
assert out.shape == (40, 40)
|
||||||
|
assert out[:, :20].all()
|
||||||
|
assert not out[:, 20:].any()
|
||||||
|
|
||||||
|
|
||||||
|
def test_prepare_draw_mask_passes_none_through():
|
||||||
|
assert _prepare_draw_mask(None, (10, 10)) is None
|
||||||
|
|
||||||
|
|
||||||
|
def test_non_square_grid_does_not_raise_with_mask():
|
||||||
|
"""Height and width differ, so rr/cc broadcasting must stay (N, N)."""
|
||||||
|
frame = np.full((60, 120), 80, dtype=np.uint8)
|
||||||
|
flow = flow_of((60, 120), dx=5.0, dy=5.0)
|
||||||
|
out = draw_arrows(frame, flow, grid_spacing=10, arrow_scale=1.0,
|
||||||
|
threshold=0.1, valid_mask=np.ones((60, 120), dtype=bool))
|
||||||
|
assert out.shape == (60, 120, 3)
|
||||||
|
assert not np.array_equal(out, background(frame))
|
||||||
@@ -0,0 +1,281 @@
|
|||||||
|
"""StreamRecorder / mosaic_videos: off-hot-path arrow overlay export."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import os
|
||||||
|
import time
|
||||||
|
|
||||||
|
import cv2
|
||||||
|
import numpy as np
|
||||||
|
import pytest
|
||||||
|
|
||||||
|
from orisys import FrameSnapshot, StreamRecorder, mosaic_videos
|
||||||
|
|
||||||
|
|
||||||
|
_DEFAULT = object()
|
||||||
|
|
||||||
|
|
||||||
|
def make_snapshot(seq: int = 0, *, size: int = 64, image=_DEFAULT, flow=_DEFAULT) -> FrameSnapshot:
|
||||||
|
if image is _DEFAULT:
|
||||||
|
image = np.full((size, size), 128, dtype=np.uint8)
|
||||||
|
image[8:-8, 8:-8] = 200
|
||||||
|
if flow is _DEFAULT:
|
||||||
|
flow = np.zeros((size, size, 2), dtype=np.float32)
|
||||||
|
flow[..., 0] = 2.0
|
||||||
|
flow[..., 1] = 1.0
|
||||||
|
return FrameSnapshot(
|
||||||
|
seq=seq,
|
||||||
|
timestamp=time.time(),
|
||||||
|
age_ms=5.0,
|
||||||
|
fps=100.0,
|
||||||
|
unit="raw",
|
||||||
|
normal_force=1.5,
|
||||||
|
shear_x=0.25,
|
||||||
|
shear_y=-0.5,
|
||||||
|
normal_force_raw=1.5,
|
||||||
|
shear_x_raw=0.25,
|
||||||
|
shear_y_raw=-0.5,
|
||||||
|
torque=0.01,
|
||||||
|
image=image,
|
||||||
|
flow=flow,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def read_video(path):
|
||||||
|
cap = cv2.VideoCapture(path)
|
||||||
|
try:
|
||||||
|
assert cap.isOpened(), f"cannot reopen {path}"
|
||||||
|
frames = []
|
||||||
|
while True:
|
||||||
|
ok, frame = cap.read()
|
||||||
|
if not ok:
|
||||||
|
break
|
||||||
|
frames.append(frame)
|
||||||
|
return frames, (int(cap.get(cv2.CAP_PROP_FRAME_WIDTH)),
|
||||||
|
int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT)))
|
||||||
|
finally:
|
||||||
|
cap.release()
|
||||||
|
|
||||||
|
|
||||||
|
def _diff_count(frame, expected, tol=20):
|
||||||
|
"""Pixels differing from ``expected`` by more than the codec's noise floor."""
|
||||||
|
d = np.abs(frame.astype(np.int16) - expected.astype(np.int16)).max(axis=2)
|
||||||
|
return int((d > tol).sum())
|
||||||
|
|
||||||
|
|
||||||
|
def _near(pixel, value, tol=14):
|
||||||
|
"""Codec round-trip shifts solid greys by a few levels (YUV range)."""
|
||||||
|
return all(abs(int(c) - value) <= tol for c in np.ravel(pixel))
|
||||||
|
|
||||||
|
|
||||||
|
# --------------------------------------------------------------------- recorder
|
||||||
|
def test_recorder_writes_every_frame(tmp_path):
|
||||||
|
path = str(tmp_path / "s0.mp4")
|
||||||
|
# Depth >= frame count so the drop-oldest policy provably does not engage;
|
||||||
|
# exactness of the writer itself is what this test is about.
|
||||||
|
rec = StreamRecorder(path, fps=25.0, grid_spacing=8, overlay_values=False,
|
||||||
|
queue_depth=64)
|
||||||
|
for i in range(12):
|
||||||
|
rec.on_frame(make_snapshot(i))
|
||||||
|
stats = rec.close()
|
||||||
|
|
||||||
|
assert stats["seen"] == 12
|
||||||
|
assert stats["written"] == 12
|
||||||
|
assert stats["dropped"] == 0
|
||||||
|
assert stats["skipped"] == 0
|
||||||
|
assert "error" not in stats
|
||||||
|
assert stats["thread_alive"] is False
|
||||||
|
|
||||||
|
frames, size = read_video(path)
|
||||||
|
assert len(frames) == 12
|
||||||
|
assert size == (64, 64)
|
||||||
|
|
||||||
|
|
||||||
|
def test_recorder_every_skips_frames(tmp_path):
|
||||||
|
path = str(tmp_path / "s0.mp4")
|
||||||
|
rec = StreamRecorder(path, every=3, grid_spacing=8, overlay_values=False,
|
||||||
|
queue_depth=64)
|
||||||
|
for i in range(12):
|
||||||
|
rec.on_frame(make_snapshot(i))
|
||||||
|
stats = rec.close()
|
||||||
|
|
||||||
|
assert stats["seen"] == 12
|
||||||
|
assert stats["written"] == 4
|
||||||
|
assert stats["skipped"] == 8
|
||||||
|
frames, _ = read_video(path)
|
||||||
|
assert len(frames) == 4
|
||||||
|
|
||||||
|
|
||||||
|
def test_on_frame_never_blocks_and_drops_oldest(tmp_path, monkeypatch):
|
||||||
|
"""A slow writer must cost frames, never pipeline throughput."""
|
||||||
|
path = str(tmp_path / "s0.mp4")
|
||||||
|
rec = StreamRecorder(path, queue_depth=2, grid_spacing=8, overlay_values=False)
|
||||||
|
|
||||||
|
original = rec._render
|
||||||
|
|
||||||
|
def slow_render(snap):
|
||||||
|
time.sleep(0.05)
|
||||||
|
return original(snap)
|
||||||
|
|
||||||
|
rec._render = slow_render
|
||||||
|
|
||||||
|
t0 = time.perf_counter()
|
||||||
|
for i in range(20):
|
||||||
|
rec.on_frame(make_snapshot(i))
|
||||||
|
ingest = time.perf_counter() - t0
|
||||||
|
stats = rec.close(timeout=30.0)
|
||||||
|
|
||||||
|
# Ingesting 20 frames must not wait on ~1s of encoder work.
|
||||||
|
assert ingest < 0.1, f"on_frame blocked for {ingest * 1000:.1f} ms"
|
||||||
|
assert stats["dropped"] > 0
|
||||||
|
assert stats["written"] + stats["dropped"] + stats["skipped"] == 20
|
||||||
|
assert stats["written"] < 20
|
||||||
|
|
||||||
|
|
||||||
|
def test_recorder_reports_missing_arrays(tmp_path):
|
||||||
|
path = str(tmp_path / "s0.mp4")
|
||||||
|
rec = StreamRecorder(path, grid_spacing=8, overlay_values=False)
|
||||||
|
rec.on_frame(make_snapshot(0, image=None))
|
||||||
|
stats = rec.close()
|
||||||
|
|
||||||
|
assert stats["written"] == 0
|
||||||
|
assert "error" in stats
|
||||||
|
assert 'include=("image", "flow")' in stats["error"]
|
||||||
|
|
||||||
|
|
||||||
|
def test_recorder_context_manager(tmp_path):
|
||||||
|
path = str(tmp_path / "s0.mp4")
|
||||||
|
with StreamRecorder(path, grid_spacing=8, overlay_values=False) as rec:
|
||||||
|
rec.on_frame(make_snapshot(0))
|
||||||
|
assert os.path.isfile(path)
|
||||||
|
assert rec._writer is None
|
||||||
|
|
||||||
|
|
||||||
|
def test_valid_mask_callable_resolved_per_frame(tmp_path):
|
||||||
|
calls = []
|
||||||
|
|
||||||
|
def mask():
|
||||||
|
calls.append(1)
|
||||||
|
return None
|
||||||
|
|
||||||
|
path = str(tmp_path / "s0.mp4")
|
||||||
|
rec = StreamRecorder(path, grid_spacing=8, overlay_values=False, valid_mask=mask)
|
||||||
|
for i in range(5):
|
||||||
|
rec.on_frame(make_snapshot(i))
|
||||||
|
stats = rec.close()
|
||||||
|
|
||||||
|
assert stats["written"] == 5
|
||||||
|
assert len(calls) == 5
|
||||||
|
|
||||||
|
|
||||||
|
def test_all_false_mask_suppresses_arrows(tmp_path):
|
||||||
|
"""With every grid point masked out the overlay is just the grey frame."""
|
||||||
|
path = str(tmp_path / "s0.mp4")
|
||||||
|
snap = make_snapshot(0)
|
||||||
|
rec = StreamRecorder(path, grid_spacing=8, overlay_values=False,
|
||||||
|
valid_mask=np.zeros((64, 64), dtype=bool))
|
||||||
|
rec.on_frame(snap)
|
||||||
|
rec.close()
|
||||||
|
|
||||||
|
frames, _ = read_video(path)
|
||||||
|
expected = cv2.cvtColor(snap.image, cv2.COLOR_GRAY2BGR)
|
||||||
|
assert _diff_count(frames[0], expected) == 0
|
||||||
|
|
||||||
|
|
||||||
|
def test_arrows_change_pixels_without_mask(tmp_path):
|
||||||
|
path = str(tmp_path / "s0.mp4")
|
||||||
|
snap = make_snapshot(0)
|
||||||
|
rec = StreamRecorder(path, grid_spacing=8, overlay_values=False)
|
||||||
|
rec.on_frame(snap)
|
||||||
|
rec.close()
|
||||||
|
|
||||||
|
frames, _ = read_video(path)
|
||||||
|
expected = cv2.cvtColor(snap.image, cv2.COLOR_GRAY2BGR)
|
||||||
|
assert _diff_count(frames[0], expected) > 100
|
||||||
|
|
||||||
|
|
||||||
|
def test_close_without_frames_is_safe(tmp_path):
|
||||||
|
rec = StreamRecorder(str(tmp_path / "empty.mp4"), grid_spacing=8)
|
||||||
|
stats = rec.close()
|
||||||
|
assert stats["written"] == 0
|
||||||
|
assert "error" not in stats
|
||||||
|
|
||||||
|
|
||||||
|
def test_stats_report_requested_geometry(tmp_path):
|
||||||
|
rec = StreamRecorder(str(tmp_path / "s0.mp4"), fps=12.0, fourcc="mp4v")
|
||||||
|
assert rec.stats()["fps"] == 12.0
|
||||||
|
assert rec.stats()["fourcc"] == "mp4v"
|
||||||
|
assert rec.stats()["path"].endswith("s0.mp4")
|
||||||
|
|
||||||
|
|
||||||
|
# ---------------------------------------------------------------------- mosaic
|
||||||
|
def _write_solid(path, value, frames=4, size=(16, 12), fps=10.0):
|
||||||
|
w, h = size
|
||||||
|
writer = cv2.VideoWriter(path, cv2.VideoWriter_fourcc(*"mp4v"), fps, (w, h), True)
|
||||||
|
assert writer.isOpened()
|
||||||
|
try:
|
||||||
|
for _ in range(frames):
|
||||||
|
writer.write(np.full((h, w, 3), value, dtype=np.uint8))
|
||||||
|
finally:
|
||||||
|
writer.release()
|
||||||
|
return path
|
||||||
|
|
||||||
|
|
||||||
|
def test_mosaic_grid_geometry(tmp_path):
|
||||||
|
srcs = [_write_solid(str(tmp_path / f"s{i}.mp4"), 40 + 40 * i) for i in range(3)]
|
||||||
|
out = str(tmp_path / "mosaic.mp4")
|
||||||
|
info = mosaic_videos(srcs, out, cols=2, gap=4, fps=10.0)
|
||||||
|
|
||||||
|
assert info["grid"] == (2, 2)
|
||||||
|
# 2 columns x 16 + 1 gap, 2 rows x 12 + 1 gap
|
||||||
|
assert info["size"] == (2 * 16 + 4, 2 * 12 + 4)
|
||||||
|
assert info["frames"] == 4
|
||||||
|
frames, size = read_video(out)
|
||||||
|
assert len(frames) == 4
|
||||||
|
assert size == info["size"]
|
||||||
|
# the empty bottom-right cell stays black
|
||||||
|
assert _near(frames[0][-1, -1], 0, tol=12)
|
||||||
|
# top-left tile carries stream 0's colour
|
||||||
|
assert _near(frames[0][0, 0], 40)
|
||||||
|
|
||||||
|
|
||||||
|
def test_mosaic_holds_last_frame_on_short_input(tmp_path):
|
||||||
|
a = _write_solid(str(tmp_path / "a.mp4"), 200, frames=5)
|
||||||
|
b = _write_solid(str(tmp_path / "b.mp4"), 50, frames=3)
|
||||||
|
out = str(tmp_path / "mosaic.mp4")
|
||||||
|
info = mosaic_videos([a, b], out, cols=2, gap=0, fps=10.0)
|
||||||
|
|
||||||
|
assert info["frames"] == 5
|
||||||
|
frames, size = read_video(out)
|
||||||
|
assert len(frames) == 5
|
||||||
|
# b's 4th frame is a repeat of its 3rd, so the right half is still solid 50
|
||||||
|
assert _near(frames[-1][0, size[0] // 2], 50)
|
||||||
|
|
||||||
|
|
||||||
|
def test_mosaic_labels_drawn(tmp_path):
|
||||||
|
srcs = [_write_solid(str(tmp_path / f"s{i}.mp4"), 30) for i in range(2)]
|
||||||
|
out = str(tmp_path / "mosaic.mp4")
|
||||||
|
plain = str(tmp_path / "plain.mp4")
|
||||||
|
mosaic_videos(srcs, plain, cols=2, gap=0, fps=10.0)
|
||||||
|
mosaic_videos(srcs, out, cols=2, gap=0, fps=10.0, labels=["#0", "#1"])
|
||||||
|
|
||||||
|
with_label, _ = read_video(out)
|
||||||
|
without, _ = read_video(plain)
|
||||||
|
assert _diff_count(with_label[0], without[0]) > 0
|
||||||
|
|
||||||
|
|
||||||
|
def test_mosaic_scale_downscales(tmp_path):
|
||||||
|
srcs = [_write_solid(str(tmp_path / f"s{i}.mp4"), 30, size=(16, 12)) for i in range(4)]
|
||||||
|
out = str(tmp_path / "mosaic.mp4")
|
||||||
|
info = mosaic_videos(srcs, out, cols=2, gap=2, scale=0.5, fps=10.0)
|
||||||
|
assert info["size"] == (8 * 2 + 2, 6 * 2 + 2)
|
||||||
|
|
||||||
|
|
||||||
|
def test_mosaic_rejects_missing_input(tmp_path):
|
||||||
|
with pytest.raises(FileNotFoundError):
|
||||||
|
mosaic_videos([str(tmp_path / "nope.mp4")], str(tmp_path / "out.mp4"))
|
||||||
|
|
||||||
|
|
||||||
|
def test_mosaic_rejects_empty_input(tmp_path):
|
||||||
|
with pytest.raises(ValueError):
|
||||||
|
mosaic_videos([], str(tmp_path / "out.mp4"))
|
||||||
@@ -0,0 +1,117 @@
|
|||||||
|
"""The shipped filter presets and the example configs that document them.
|
||||||
|
|
||||||
|
``json.load`` cannot carry comments, so the answer to "which filter should I
|
||||||
|
use?" lives in three places that must not drift apart: ``FILTER_PRESETS`` in the
|
||||||
|
sensor, the table in ``docs/filters.md``, and the copy-paste files in
|
||||||
|
``examples/filters/``. The presets are the source of truth; these tests fail
|
||||||
|
when a copy stops matching them.
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import json
|
||||||
|
import shutil
|
||||||
|
from pathlib import Path
|
||||||
|
|
||||||
|
import pytest
|
||||||
|
|
||||||
|
from orisys import Sensor
|
||||||
|
from orisys.calibration import filters_fingerprint
|
||||||
|
from orisys.sensor import FILTER_PRESETS, available_filter_presets
|
||||||
|
from orisys import configs as orisys_configs
|
||||||
|
|
||||||
|
REPO_ROOT = Path(__file__).resolve().parents[1]
|
||||||
|
EXAMPLES = REPO_ROOT / "examples" / "filters"
|
||||||
|
BUNDLES = ("box", "mini", "finger")
|
||||||
|
|
||||||
|
#: What every shipped bundle says: filtering is opt-in, so a fresh install
|
||||||
|
#: records the raw reading and the operator turns a stage on deliberately.
|
||||||
|
ALL_OFF = {
|
||||||
|
"flow_ema_alpha": 1.0,
|
||||||
|
"force_median_window": 1,
|
||||||
|
"force_lowpass_cutoff_hz": 0.0,
|
||||||
|
"force_lowpass_order": 2,
|
||||||
|
"force_lowpass_sample_rate_hz": "auto",
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
def test_the_preset_names_are_the_documented_ones():
|
||||||
|
assert sorted(available_filter_presets()) == [
|
||||||
|
"balanced",
|
||||||
|
"low_latency",
|
||||||
|
"low_noise",
|
||||||
|
"off",
|
||||||
|
]
|
||||||
|
# ``available_filter_presets`` hands out copies: a UI cannot edit the truth.
|
||||||
|
snapshot = available_filter_presets()
|
||||||
|
snapshot["off"]["flow_ema_alpha"] = 0.0
|
||||||
|
assert FILTER_PRESETS["off"]["flow_ema_alpha"] == 1.0
|
||||||
|
|
||||||
|
|
||||||
|
@pytest.mark.parametrize("preset", sorted(FILTER_PRESETS))
|
||||||
|
def test_every_example_file_still_matches_its_preset(preset: str):
|
||||||
|
path = EXAMPLES / f"filters_{preset}.json"
|
||||||
|
assert path.is_file(), f"missing example for preset {preset!r}: {path}"
|
||||||
|
block = json.loads(path.read_text(encoding="utf-8"))
|
||||||
|
# A whole ``filters`` block: the preset's own keys on top of the defaults
|
||||||
|
# every config needs, and nothing else.
|
||||||
|
expected = dict(ALL_OFF)
|
||||||
|
expected.update(FILTER_PRESETS[preset])
|
||||||
|
assert block == expected, f"{path.name} drifted from preset {preset!r}"
|
||||||
|
|
||||||
|
|
||||||
|
def test_every_example_file_is_a_real_filters_block():
|
||||||
|
"""A file that cannot be dropped into a config is documentation that lies."""
|
||||||
|
for preset in FILTER_PRESETS:
|
||||||
|
block = json.loads((EXAMPLES / f"filters_{preset}.json").read_text(encoding="utf-8"))
|
||||||
|
assert isinstance(block, dict) and block, preset
|
||||||
|
for key in ("flow_ema_alpha", "force_median_window", "force_lowpass_cutoff_hz"):
|
||||||
|
assert key in block, f"{preset}: {key} missing"
|
||||||
|
|
||||||
|
|
||||||
|
@pytest.mark.parametrize("bundle", BUNDLES)
|
||||||
|
def test_shipped_bundles_ship_with_filtering_off(bundle: str):
|
||||||
|
"""Nothing filters unless it was asked for."""
|
||||||
|
path = orisys_configs.resolve_sensor_config_path(bundle)
|
||||||
|
assert path is not None and path.is_file(), f"missing bundle: {bundle}"
|
||||||
|
cfg = json.loads(path.read_text(encoding="utf-8"))
|
||||||
|
assert cfg.get("filters") == ALL_OFF, f"{bundle} no longer ships with filtering off"
|
||||||
|
|
||||||
|
|
||||||
|
@pytest.mark.parametrize("bundle", BUNDLES)
|
||||||
|
def test_shipped_bundles_report_all_off(bundle: str):
|
||||||
|
"""The file and the running device have to agree about the shipped default."""
|
||||||
|
sensor = Sensor("external", config_name=bundle, verbose=False, backend="cpu")
|
||||||
|
try:
|
||||||
|
fp = filters_fingerprint(sensor.filters_state())
|
||||||
|
finally:
|
||||||
|
sensor.disconnect()
|
||||||
|
assert fp["measurement"] == {"flow_ema_alpha": 1.0}
|
||||||
|
assert fp["readout"]["force_median_window"] == 1
|
||||||
|
assert fp["readout"]["force_lowpass_cutoff_hz"] == 0.0
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_config_with_no_filters_block_filters_nothing(tmp_path: Path):
|
||||||
|
"""Absence is off, not "the values we thought were sensible".
|
||||||
|
|
||||||
|
A key that is missing has to mean the stage is not running: otherwise a
|
||||||
|
device filters a measurement nobody asked for, and nothing in the file says
|
||||||
|
so. Callers with a hand-written config (an old customer file, a bench
|
||||||
|
script) rely on this.
|
||||||
|
"""
|
||||||
|
copy = tmp_path / "box"
|
||||||
|
shutil.copytree(REPO_ROOT / "sensors" / "box", copy)
|
||||||
|
path = copy / "sensor.json"
|
||||||
|
cfg = json.loads(path.read_text(encoding="utf-8"))
|
||||||
|
cfg.pop("filters", None)
|
||||||
|
path.write_text(json.dumps(cfg, indent=2), encoding="utf-8")
|
||||||
|
|
||||||
|
sensor = Sensor("external", config_name=str(path), verbose=False, backend="cpu")
|
||||||
|
try:
|
||||||
|
state = sensor.filters_state()
|
||||||
|
finally:
|
||||||
|
sensor.disconnect()
|
||||||
|
assert state["measurement"]["flow_ema_alpha"] == 1.0
|
||||||
|
assert state["readout"]["force_median_window"] == 1
|
||||||
|
assert state["readout"]["force_lowpass_cutoff_hz"] == 0.0
|
||||||
|
assert state["any_stage_applied"] is False
|
||||||
@@ -0,0 +1,329 @@
|
|||||||
|
"""StreamPipeline: staging, snapshot immutability, error propagation, shutdown."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import time
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
import pytest
|
||||||
|
|
||||||
|
from orisys import FrameSnapshot, StreamPipeline
|
||||||
|
|
||||||
|
|
||||||
|
class FakeSensor:
|
||||||
|
"""Minimal Sensor stand-in with the same mutable-buffer behaviour.
|
||||||
|
|
||||||
|
It deliberately reuses one image buffer (like ``Sensor._img_gray_buf``) so the
|
||||||
|
pipeline's private-copy contract is actually exercised.
|
||||||
|
"""
|
||||||
|
|
||||||
|
class info:
|
||||||
|
VRAW = "vraw"
|
||||||
|
VNORMAL = "vnormal"
|
||||||
|
VSHEAR = "vshear"
|
||||||
|
DEPTH = "depth"
|
||||||
|
CENTROID = "centroid"
|
||||||
|
CONTOUR = "contour"
|
||||||
|
|
||||||
|
def __init__(self, *, fail_at: int | None = None, flow_delay: float = 0.0,
|
||||||
|
hhd_delay: float = 0.0):
|
||||||
|
self.img = None
|
||||||
|
self.flow = None
|
||||||
|
self.vnormal = None
|
||||||
|
self.vshear = None
|
||||||
|
self.depth_map = None
|
||||||
|
self.centroid = (0, 0)
|
||||||
|
self.contour = np.empty(0)
|
||||||
|
self.ftor = 0.0
|
||||||
|
self._force_raw = (0.0, 0.0, 0.0)
|
||||||
|
self.fnormal = self.fshearx = self.fsheary = 0.0
|
||||||
|
self.force_unit = "raw"
|
||||||
|
self._img_buf = np.zeros((8, 8), dtype=np.uint8)
|
||||||
|
self._flow_buf = np.zeros((8, 8, 2), dtype=np.float32)
|
||||||
|
self._flow_delay = flow_delay
|
||||||
|
self._hhd_delay = hhd_delay
|
||||||
|
self._fail_at = fail_at
|
||||||
|
self._flow_calls = 0
|
||||||
|
self._seen_images: list[np.ndarray] = []
|
||||||
|
self._seen_flow_values: list[float] = []
|
||||||
|
self.contact_calls = 0
|
||||||
|
|
||||||
|
def make_frame(self, seq: int) -> np.ndarray:
|
||||||
|
# writes INTO the shared buffer, exactly like cv2.resize(dst=...)
|
||||||
|
self._img_buf.fill(seq % 251)
|
||||||
|
self.img = self._img_buf
|
||||||
|
return self._img_buf
|
||||||
|
|
||||||
|
def compute_flow(self, img=None, **kwargs) -> None:
|
||||||
|
image = self.img if img is None else img
|
||||||
|
self._flow_calls += 1
|
||||||
|
if self._fail_at is not None and self._flow_calls == self._fail_at:
|
||||||
|
raise RuntimeError("flow exploded")
|
||||||
|
self._seen_images.append(np.array(image, copy=True))
|
||||||
|
time.sleep(self._flow_delay)
|
||||||
|
# rewrite the shared buffer, like the real DIS/EMA path does
|
||||||
|
self._flow_buf.fill(float(self._flow_calls))
|
||||||
|
self.flow = self._flow_buf
|
||||||
|
|
||||||
|
def compute_hhd(self, flow=None, **kwargs) -> None:
|
||||||
|
# Mirrors Sensor.compute_hhd, which derives the forces from the field it
|
||||||
|
# is given - so this only yields frame N's forces if the pipeline passes
|
||||||
|
# frame N's flow explicitly.
|
||||||
|
time.sleep(self._hhd_delay)
|
||||||
|
field = self.flow if flow is None else flow
|
||||||
|
value = (float(field[0, 0, 0])
|
||||||
|
if isinstance(field, np.ndarray) and field.size else 0.0)
|
||||||
|
self._seen_flow_values.append(value)
|
||||||
|
self.fnormal = value * 10.0
|
||||||
|
self.fshearx = value
|
||||||
|
self.fsheary = -value
|
||||||
|
self._force_raw = (self.fnormal, self.fshearx, self.fsheary)
|
||||||
|
self.ftor = value
|
||||||
|
self.vnormal = self._flow_buf
|
||||||
|
self.vshear = self._flow_buf
|
||||||
|
|
||||||
|
def compute_contact(self) -> None:
|
||||||
|
self.contact_calls += 1
|
||||||
|
self.centroid = (1, 2)
|
||||||
|
self.contour = np.zeros((1, 1, 2), dtype=np.int32)
|
||||||
|
self.depth_map = np.zeros((8, 8), dtype=np.float32)
|
||||||
|
|
||||||
|
def read_info(self, *args):
|
||||||
|
source = {
|
||||||
|
self.info.VRAW: lambda: self.flow.copy(),
|
||||||
|
self.info.VNORMAL: lambda: self.vnormal.copy(),
|
||||||
|
self.info.VSHEAR: lambda: self.vshear.copy(),
|
||||||
|
self.info.DEPTH: lambda: self.depth_map.copy(),
|
||||||
|
self.info.CENTROID: lambda: np.array(self.centroid),
|
||||||
|
self.info.CONTOUR: lambda: self.contour.copy(),
|
||||||
|
}
|
||||||
|
return tuple(source[a]() for a in args)
|
||||||
|
|
||||||
|
|
||||||
|
def make_producer(sensor: FakeSensor, count: int):
|
||||||
|
state = {"i": 0}
|
||||||
|
|
||||||
|
def produce():
|
||||||
|
if state["i"] >= count:
|
||||||
|
return None
|
||||||
|
frame = sensor.make_frame(state["i"])
|
||||||
|
state["i"] += 1
|
||||||
|
return frame
|
||||||
|
|
||||||
|
return produce
|
||||||
|
|
||||||
|
|
||||||
|
def run_all(sensor: FakeSensor, count: int, **kwargs):
|
||||||
|
"""Drive the pipeline with an exhaustive consumer: every frame, in order."""
|
||||||
|
frames: list = []
|
||||||
|
kwargs.setdefault("depth", 2)
|
||||||
|
pipe = StreamPipeline(sensor, make_producer(sensor, count),
|
||||||
|
on_frame=frames.append, **kwargs)
|
||||||
|
pipe.start()
|
||||||
|
deadline = time.perf_counter() + 10.0
|
||||||
|
while len(frames) < count and time.perf_counter() < deadline:
|
||||||
|
if pipe.error is not None:
|
||||||
|
break
|
||||||
|
time.sleep(0.001)
|
||||||
|
pipe.stop()
|
||||||
|
return pipe, frames
|
||||||
|
|
||||||
|
|
||||||
|
def test_on_frame_receives_every_frame_in_order():
|
||||||
|
sensor = FakeSensor()
|
||||||
|
pipe, frames = run_all(sensor, 12)
|
||||||
|
assert len(frames) == 12
|
||||||
|
assert [s.seq for s in frames] == list(range(12))
|
||||||
|
assert pipe.stats.produced == 12
|
||||||
|
assert pipe.stats.published == 12
|
||||||
|
|
||||||
|
|
||||||
|
def test_forces_come_from_the_matching_frame():
|
||||||
|
"""Each snapshot must carry the forces computed from its own frame's flow."""
|
||||||
|
sensor = FakeSensor()
|
||||||
|
_, frames = run_all(sensor, 10)
|
||||||
|
# frame i's flow value is (i + 1); the fake derives the force from self.flow,
|
||||||
|
# so a mismatched pairing would show up as a repeated or skipped value.
|
||||||
|
assert sensor._seen_flow_values == [float(i + 1) for i in range(10)]
|
||||||
|
assert [s.normal_force for s in frames] == [10.0 * (i + 1) for i in range(10)]
|
||||||
|
assert [s.shear_x for s in frames] == [float(i + 1) for i in range(10)]
|
||||||
|
|
||||||
|
|
||||||
|
def test_stages_do_not_read_a_rewritten_image_buffer():
|
||||||
|
"""The producer reuses one buffer; no stage may observe a later frame."""
|
||||||
|
sensor = FakeSensor(flow_delay=0.004)
|
||||||
|
run_all(sensor, 8)
|
||||||
|
seen = sensor._seen_images
|
||||||
|
assert len(seen) == 8
|
||||||
|
for i, arr in enumerate(seen):
|
||||||
|
assert np.all(arr == i % 251), f"frame {i} was overwritten before flow ran"
|
||||||
|
|
||||||
|
|
||||||
|
def test_snapshot_arrays_are_private_copies():
|
||||||
|
sensor = FakeSensor()
|
||||||
|
_, frames = run_all(sensor, 4, include=("image", "flow", "vnormal"))
|
||||||
|
first = frames[0]
|
||||||
|
assert isinstance(first, FrameSnapshot)
|
||||||
|
assert first.image is not None and first.flow is not None
|
||||||
|
saved = first.flow.copy()
|
||||||
|
# the SDK keeps rewriting its buffers; the snapshot must not change
|
||||||
|
sensor._flow_buf.fill(999.0)
|
||||||
|
sensor._img_buf.fill(7)
|
||||||
|
assert np.array_equal(first.flow, saved)
|
||||||
|
assert not np.any(first.flow == 999.0)
|
||||||
|
|
||||||
|
|
||||||
|
def test_include_filters_which_arrays_are_copied():
|
||||||
|
sensor = FakeSensor()
|
||||||
|
_, frames = run_all(sensor, 3, include=("vnormal",))
|
||||||
|
assert frames[0].vnormal is not None
|
||||||
|
assert frames[0].image is None and frames[0].flow is None
|
||||||
|
assert frames[0].depth is None and frames[0].contour is None
|
||||||
|
|
||||||
|
|
||||||
|
def test_rejects_unknown_include_field():
|
||||||
|
sensor = FakeSensor()
|
||||||
|
with pytest.raises(ValueError):
|
||||||
|
StreamPipeline(sensor, make_producer(sensor, 1), include=("floww",))
|
||||||
|
|
||||||
|
|
||||||
|
def test_contact_stage_runs_when_requested():
|
||||||
|
sensor = FakeSensor()
|
||||||
|
_, frames = run_all(sensor, 5, contact=True,
|
||||||
|
include=("depth", "centroid", "contour"))
|
||||||
|
assert sensor.contact_calls == 5
|
||||||
|
assert frames[-1].depth is not None
|
||||||
|
assert frames[-1].centroid is not None
|
||||||
|
assert frames[-1].contour is not None
|
||||||
|
|
||||||
|
|
||||||
|
def test_stage_exception_surfaces_on_stop():
|
||||||
|
sensor = FakeSensor(fail_at=3)
|
||||||
|
pipe = StreamPipeline(sensor, make_producer(sensor, 20), depth=2)
|
||||||
|
pipe.start()
|
||||||
|
time.sleep(0.4)
|
||||||
|
with pytest.raises(RuntimeError, match="flow exploded"):
|
||||||
|
pipe.stop()
|
||||||
|
|
||||||
|
|
||||||
|
def test_stage_exception_surfaces_on_wait():
|
||||||
|
sensor = FakeSensor(fail_at=2)
|
||||||
|
pipe = StreamPipeline(sensor, make_producer(sensor, 20), depth=2)
|
||||||
|
pipe.start()
|
||||||
|
with pytest.raises(RuntimeError):
|
||||||
|
for _ in range(50):
|
||||||
|
pipe.wait(timeout=0.1)
|
||||||
|
pipe.stop()
|
||||||
|
|
||||||
|
|
||||||
|
def test_stop_is_idempotent():
|
||||||
|
sensor = FakeSensor()
|
||||||
|
pipe, _ = run_all(sensor, 4)
|
||||||
|
pipe.stop() # second stop must not raise
|
||||||
|
assert not pipe.running
|
||||||
|
assert pipe.latest() is not None
|
||||||
|
|
||||||
|
|
||||||
|
def test_stop_while_running_does_not_hang():
|
||||||
|
"""Stopping mid-stream must unblock the consumer threads.
|
||||||
|
|
||||||
|
The producer stops delivering sentinels once the done flag is set, so a
|
||||||
|
bare blocking Queue.get() in a consumer would wait forever.
|
||||||
|
"""
|
||||||
|
sensor = FakeSensor(flow_delay=0.002)
|
||||||
|
pipe = StreamPipeline(sensor, make_producer(sensor, 100000), depth=2)
|
||||||
|
pipe.start()
|
||||||
|
assert pipe.wait(timeout=2.0) is not None
|
||||||
|
t0 = time.perf_counter()
|
||||||
|
pipe.stop(timeout=5.0)
|
||||||
|
elapsed = time.perf_counter() - t0
|
||||||
|
assert elapsed < 2.0, f"stop() took {elapsed:.2f}s; a stage was still blocked"
|
||||||
|
assert not pipe.running
|
||||||
|
|
||||||
|
|
||||||
|
def test_context_manager_stops_threads():
|
||||||
|
sensor = FakeSensor()
|
||||||
|
frames: list = []
|
||||||
|
with StreamPipeline(sensor, make_producer(sensor, 6),
|
||||||
|
on_frame=frames.append) as pipe:
|
||||||
|
deadline = time.perf_counter() + 5.0
|
||||||
|
while len(frames) < 6 and time.perf_counter() < deadline:
|
||||||
|
time.sleep(0.001)
|
||||||
|
assert len(frames) == 6
|
||||||
|
assert not pipe.running
|
||||||
|
|
||||||
|
|
||||||
|
def test_wait_exposes_a_latest_wins_view():
|
||||||
|
sensor = FakeSensor()
|
||||||
|
pipe = StreamPipeline(sensor, make_producer(sensor, 5))
|
||||||
|
pipe.start()
|
||||||
|
last = None
|
||||||
|
while True:
|
||||||
|
snap = pipe.wait(timeout=2.0)
|
||||||
|
if snap is None:
|
||||||
|
break
|
||||||
|
if last is not None:
|
||||||
|
assert snap.seq > last.seq
|
||||||
|
last = snap
|
||||||
|
# the pipeline has drained; the final snapshot is the last frame
|
||||||
|
assert last is not None and last.normal_force == 50.0
|
||||||
|
assert pipe.latest() is last
|
||||||
|
# nothing new is coming, so a further poll returns immediately
|
||||||
|
assert pipe.wait(timeout=0.2) is None
|
||||||
|
pipe.stop()
|
||||||
|
|
||||||
|
|
||||||
|
def test_wait_timeout_returns_none_without_new_frames():
|
||||||
|
sensor = FakeSensor()
|
||||||
|
pipe = StreamPipeline(sensor, make_producer(sensor, 1))
|
||||||
|
pipe.start()
|
||||||
|
assert pipe.wait(timeout=1.0) is not None
|
||||||
|
t0 = time.perf_counter()
|
||||||
|
assert pipe.wait(timeout=0.1) is None
|
||||||
|
assert time.perf_counter() - t0 < 1.0
|
||||||
|
pipe.stop()
|
||||||
|
|
||||||
|
|
||||||
|
def test_stats_track_stage_times_and_age():
|
||||||
|
sensor = FakeSensor(flow_delay=0.002, hhd_delay=0.001)
|
||||||
|
pipe, _ = run_all(sensor, 15)
|
||||||
|
d = pipe.stats.as_dict()
|
||||||
|
assert d["published"] == 15
|
||||||
|
assert d["flow_ms"] >= 1.5 # the injected delay shows up
|
||||||
|
assert d["hhd_ms"] >= 0.8
|
||||||
|
assert d["age_ms"] > 0.0
|
||||||
|
assert d["fps"] > 0.0
|
||||||
|
assert d["produce_ms"] >= 0.0
|
||||||
|
|
||||||
|
|
||||||
|
def test_depth_does_not_change_results():
|
||||||
|
for depth in (1, 2, 4):
|
||||||
|
sensor = FakeSensor()
|
||||||
|
_, frames = run_all(sensor, 10, depth=depth)
|
||||||
|
assert [s.normal_force for s in frames] == [10.0 * (i + 1) for i in range(10)]
|
||||||
|
|
||||||
|
|
||||||
|
def test_for_finger_runtime_maps_eof_to_none():
|
||||||
|
"""``for_finger_runtime`` drives ``grab_and_process`` and maps EOF to None."""
|
||||||
|
sensor = FakeSensor()
|
||||||
|
state = {"i": 0}
|
||||||
|
|
||||||
|
class FakeRuntime:
|
||||||
|
tactile = type("T", (), {"raw_sensor": sensor})()
|
||||||
|
|
||||||
|
def grab_and_process(self, color=False):
|
||||||
|
if state["i"] >= 3:
|
||||||
|
return False, None
|
||||||
|
frame = sensor.make_frame(state["i"])
|
||||||
|
state["i"] += 1
|
||||||
|
return True, frame
|
||||||
|
|
||||||
|
frames: list = []
|
||||||
|
pipe = StreamPipeline.for_finger_runtime(FakeRuntime(), on_frame=frames.append)
|
||||||
|
pipe.start()
|
||||||
|
deadline = time.perf_counter() + 5.0
|
||||||
|
while len(frames) < 3 and time.perf_counter() < deadline:
|
||||||
|
time.sleep(0.001)
|
||||||
|
pipe.stop()
|
||||||
|
assert len(frames) == 3
|
||||||
|
assert pipe.stats.produced == 3
|
||||||
Reference in New Issue
Block a user