Compare commits

..

10 Commits

Author SHA1 Message Date
ykcheungab 14fbfec62e remove hand.py, replaced by pipeline.py 2026-09-14 13:55:30 +08:00
ykcheungab 11962cdc34 配置没写的滤波键等于"关",不再回落到会滤波的默认值 - What: DEFAULT_FORCE_MEDIAN_WINDOW 3 -> 1、DEFAULT_FLOW_EMA_ALPHA 0.5 -> 1.0(DEFAULT_FORCE_LOWPASS_HZ 本就是 0.0),并把注释改成"配置文件没写的键就是关,而不是我们觉得合理的那一档"——原因写在注释里:否则设备会去滤一个没人要求滤的测量,而文件里没有任何线索;实测的延迟/降噪数据保留在注释中,作为 balanced/low_noise 预设的依据。docs/filters.md 第 2 节增加"缺省 = 关"这一条并说明没有 filters 段的配置什么都不滤,预设表里 balanced 的用途由"默认建议"改为"建议起步档"(它不再是默认值)。examples/filters/README.md 补一句"整段删掉等价于 off"。tests/test_filter_presets.py 新增 test_a_config_with_no_filters_block_filters_nothing:把 box bundle 的 filters 段整段删掉后构造真实 Sensor,断言 filters_state() 报 ema 1.0 / median 1 / cutoff 0.0 且 any_stage_applied 为 False。 - Why: 上一版只把"非法值"改成回落到关,但"键根本不写"仍会走 DEFAULT_* 从而把 EMA 0.5 + 中值 3 打开——这与"配置自描述、文件说什么就跑什么、出厂默认全关"矛盾,且现实里存在大量没有 filters 段的手写配置(例如 _perf/cfg_res_280x300.json),它们会在无人察觉的情况下继续滤波。缺省开是这条链上最后一个隐藏默认值。 - Verification: 删掉 filters 段后实测 filters_state() = (1.0, 1, 0.0) 且 any_stage_applied=False,出厂 box/mini/finger 三份同样为全关;pytest tests/test_filter_presets.py + tests/test_calibration.py → 33 passed;SDK 全量 336 passed / 15 skipped(唯一失败仍是需要 1200 帧而样例只有 577 帧的 longrun 测试);app 侧 studio 85 项与 shared 142 项测试、ui_check 554 项检查在升级后的 0.5.2 下全绿。 - Not in this change: 出厂 bundle 的三个显式值未动(本来就全关);_perf 的两个校验脚本不在仓库内,已就地补上显式 filters 段(balanced)以保持"开 vs 关"对照仍有意义;app 侧 wrapper/齿轮/CSV 17 列未动。 2026-09-14 10:53:33 +08:00
ykcheungab ae5bf0c208 断开连接不再因无头 OpenCV 抛错 - What: Sensor.disconnect() 里的 cv2.destroyAllWindows() 包上 try/except cv2.error:无头构建(opencv-python-headless)没有 highgui,destroyAllWindows 会抛 "The function is not implemented",此前会让 finally 里 disconnect() 的调用方(测试、采集退出路径)拿到一个与传感器无关的异常。 - Why: 跑 SDK 测试时发现 tests/test_calibration.py::test_sensor_cal_path_v2_applies_to_read_info 与 test_set_calibration_swaps_curve 在本机(cv2 4.14.0 headless)稳定失败,断言本身都过了,报错在 finally 的 disconnect() 第 1098 行;这不是滤波改动引入的,但会一直污染每次验证,且 SDK 明确支持无头/CI 使用(backend="cpu"、external-frame 模式)。 - Verification: 修前该模块 7 failed / 13 passed(含上述两个既有测试),修后 20 passed;SDK 全量 335 passed / 15 skipped(唯一失败为需要 1200 帧而 data/sample/s1_480p.mp4 只有 577 帧的 longrun 测试)。 - Not in this change: 未改 disconnect() 的其他分支;未引入 highgui 依赖探测,只在真正调用失败时忽略。 2026-09-14 10:44:13 +08:00
ykcheungab 7f8e5d0975 四个滤波预设落成机器可读的配置文件,并加上防漂移测试 - What: 新增 examples/filters/filters_{off,low_latency,balanced,low_noise}.json —— 每个都是完整可整块替换的 filters 段(五键齐全:flow_ema_alpha / force_median_window / force_lowpass_cutoff_hz / force_lowpass_order / force_lowpass_sample_rate_hz),数值即 FILTER_PRESETS 展开后的结果;examples/filters/README.md 说明这四份副本的存在与用途;新增 tests/test_filter_presets.py(12 项):预设名恰为四个、每个示例文件 == 默认块叠加该预设(改预设或改文件都会红)、每份都是含三个开关键的真实 filters 段、三个出厂 bundle(box/mini/finger)的 filters 段仍为全关、且用 external-frame 模式构造真实 Sensor 核对 filters_state() 报的也是全关(文件与设备必须一致)。 - Why: 之前的实现计划里写的是"四个 sensor_<preset>.json 整机配置",但整机配置要按传感器各复制一份、且必然随 bundle 漂移;README 里内嵌的 JSON 又无法被测试覆盖,等于把"建议用哪档滤波"这句话留在了一处会过时的散文里。改成"filters 片段 + 断言与 FILTER_PRESETS 逐值一致的测试",既不谎称是完整配置,也不会静默过时。 - Verification: pytest tests/test_filter_presets.py → 12 passed;SDK 全量 335 passed / 15 skipped(唯一失败仍是需要 1200 帧而样例只有 577 帧的 longrun 测试)。 - Not in this change: 未生成按传感器复制的整机配置;docs/filters.md 的预设表未纳入测试(只有这四个文件与代码常量对齐);app 侧未动。 2026-09-14 10:44:07 +08:00
ykcheungab 35190562e5 标定指纹改读生效值并按测量/读数分组 - What: calibration.py 新增 FILTER_MEASUREMENT_KEYS(flow_ema_alpha)与 FILTER_READOUT_KEYS(中值窗口、低通截止/阶数/采样率/rate_auto)、filters_fingerprint()(把 Sensor.filters_state() 归一化成 {measurement, readout, preset, source}:非法值丢弃、来源没写的键不出现而不是补默认、空映射返回 {} 表示"无记录")、filter_mismatches()(按组判读差异)与 _filters_from_config / _flat_filters_from(扁平旧格式与 preset 展开的兼容解析,显式键优先);pipeline_from_sensor_json() 新增 filters_state= 关键字——传了生效值就用生效值,否则读文件的 filters 段并标注 source="sensor.json.filters",返回结构化 filters 键,删除原先会凭空补出 0.5/3/10 的扁平键与 enabled 覆盖逻辑;Calibration.to_dict() 写 meta.filters(同时保留扁平键供旧读者按名取用);parse() 把 filters 读回 pipeline(此前 to_dict 写进 meta 的滤波键在读回时被整段丢弃,指纹实际是"只写不读");check() 改用 filter_mismatches 分开报告——measurement 组差异进 warnings 且措辞含 not comparable,readout 组差异进新增字段 CheckResult.notes(只记录、不拦截);build(extra_meta=…) 把 filters 与扁平滤波键折进结构化组。tests/test_calibration.py 新增 11 项测试(分组、preset 展开与显式键优先、enabled:false 全关、非法值为空而非默认、写入-读回往返、旧扁平文档读回、生效值压过文件、四个预设逐档核对)。docs/filters.md 新增第 7 节。 - Why: 用户决策 B 要求"Sensor.filters_state() 作为唯一生效值出口,calibration.py 指纹与 session.json 都读它、不读配置文件,指纹分 measurement/readout 两组";动手时确认旧实现三个问题叠加:来源错(读文件、被 filters.enabled 覆盖、文件没写就补默认 0.5/3/10),写进 meta 的指纹在读回时被丢弃,check() 因此永远比不出滤波差异——审计链是断的,不是不精确。 - Verification: SDK 全量 pytest 335 passed / 15 skipped,唯一失败 test_vnormal_longrun_no_outlier_jumps 需要 1200 帧而 data/sample/s1_480p.mp4 只有 577 帧(环境/数据,与本改动无关);external-frame 模式逐预设构造真实 Sensor,filters_fingerprint(filters_state()) 得 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);写入→读回往返后 measurement/readout/preset 完整保留且 to_dict 幂等;旧扁平 meta 文档仍读回同一指纹;.repro/verify_cal_filters.py 50 项全绿。 - Not in this change: 未动 app(app 的 meta_from_sensor_json 只取 4 个键,不受影响);未升级 venv 内 SDK;examples/filters 的四个机器可读预设片段与 app 侧 wrapper/齿轮/CSV 17 列未动。 2026-09-14 10:43:58 +08:00
ykcheungab b956d071ea 滤波说明落到文档与 examples:三级位置/性质、四个预设、可追溯性清单 - What: 新增 docs/filters.md(json.load 不支持注释,故建议以"代码预设 + 文档 + 可复制配置"承载:三级滤波的作用位置与性质对照——flow_ema_alpha 在 compute_flow/HHD 之前、改变测量故两次录制不可比,中值与低通是读数滤波;取值规则:每级按值开关、非法值一律关、filters.enabled 仅作废弃别名、显式键优先于预设;FILTER_PRESETS 四个档位的延迟与用途表;configure()/filters_state() 用法;force_triplets() 两个 buffer 的语义与尖峰实测 10/999;可追溯性五条清单)与 examples/filters/README.md(off/low_latency/balanced/low_noise 四段可直接整块替换的 filters 配置、preset 等价写法、"从预设出发只改一个键"的写法、复制后用 filters_state() 核对生效值的片段)。 - Why: 用户提出"在 json 里加 comment 说明建议的滤波,或在 examples 里写",查证 configs.py 使用 json.load 且环境无 json5/commentjson,加注释会导致加载失败;因此把建议做成可执行预设并在文档与 examples 中给出可复制版本。 - Verification: 文档中的四个预设与 FILTER_PRESETS 逐值一致;示例片段与 sensors/*/sensor.json 的 filters 段结构一致;filters_state()/force_triplets() 的行为已用 external-frame 模式实测(预设解析结果与尖峰 10/999 对照)。 - Not in this change: 未生成四个独立的 sensor_<preset>.json 整机配置文件(README 里给的是整块 filters 段);calibration.py 指纹尚未改读 filters_state();venv 内 SDK 未升级;app 侧 wrapper/齿轮与 CSV 17 列未动。 2026-09-14 10:36:00 +08:00
ykcheungab 0beb36a1af 滤波预设与生效值出口:FILTER_PRESETS + filters_state() + force_triplets() - What: 新增 FILTER_PRESETS(off/low_latency/balanced/low_noise,数值取自 _perf 的延迟表,low_noise 含 10 Hz 低通)与 available_filter_presets() 供 UI 取用;配置可写 filters.preset 且显式键优先于预设值;新增 Sensor.filters_state()(preset + measurement 组 flow_ema_alpha/flow_ema_applied/hhd_downsample/dis_mode + readout 组中值与低通的窗口/截止/阶数/采样率/是否 auto/是否生效 + sample_rate_source + any_stage_applied + source="sensor.filters_state")作为唯一"生效值"出口;新增 Sensor.force_triplets() 一次返回 {filtered:{raw,newton}, unfiltered:{raw,newton}},并在 _apply_force_filter 中于链后、标定前保存 _force_filtered_raw(修正:此前滤波后的 raw 计数没有保存,force_triplets 的 filtered.raw 实际返回的是输入);reset() 清空未滤波缓存。 - Why: 用户要求配置自描述(A)并把"生效值"作为审计唯一依据(B),同时要保留滤波前数据以便"两个 buffer、两个单位都保留";顺带证实并修掉了 filtered.raw 指向输入这一标签错误。 - Verification: external-frame 模式逐预设构造真实 Sensor 打印 filters_state:off/low_latency/balanced/low_noise 分别为 (ema 1.0 off, median 1 off, lp 0.0 off) / (1.0 off, 3 on, 0.0 off) / (0.5 on, 3 on, 0.0 off) / (0.3 on, 3 on, 10.0 on);注入 10,10,10,999,10,10 观察力三元组,filtered.raw 在尖峰帧为 10.0、unfiltered.raw 为 999.0,证明中值生效且未滤波 buffer 完整;compileall 通过。 - Not in this change: calibration.py 指纹尚未改读 filters_state(),examples/filters 与 docs/filters.md 未加,venv 里的 SDK 未升级(app 行为不变,仍硬编码 10 Hz 低通),app 侧 wrapper/齿轮/CSV 17 列未动。 2026-09-14 10:35:51 +08:00
ykcheungab 1adb1c30f0 滤波配置自描述:去掉覆盖式总开关,每级按自己的值生效 - What: sensor.py 删除 filters.enabled 这个覆盖层——flow_ema_alpha / force_median_window / force_lowpass_cutoff_hz 现在各自按值生效(0<α<1 才滤、≤1 关中值、≤0 关低通),非法值一律回落到"关"而不再回落到"默认开"(原实现里非法取值的 except 分支会把 cutoff 变回 10 Hz);enabled 仅作废弃别名保留(显式 false = 三级全关)并在 verbose 时提示弃用,true 不再有覆盖作用;_filters_enabled 改为派生量(任一级在滤才为真)供旧调用方查询。三份 sensors/{box,mini,finger}/sensor.json 的 filters 段改为自描述且默认全关(flow_ema_alpha 1.0 / force_median_window 1 / force_lowpass_cutoff_hz 0.0 / order 2 / sample_rate "auto")。 - Why: 用户问"flow_ema_alpha 是不是受 filter_enabled 控制、这样合适吗",查证后确认三级都被同一总开关覆盖,且出厂配置写 enabled:false 却仍列着 0.5/3,导致文件说的与设备实际跑的不一致(审计/指纹会误报);同时 flow_ema_alpha 位于 HHD 之前、改变的是测量本身,与中值/低通这类读数滤波性质不同,不应被一个开关捆绑。 - Verification: 用 SDK 的 external-frame 模式逐个 bundle 构造真实 Sensor 并读取解析结果:box/mini/finger 均为 ema=1.0、median=1、cutoff=0.0、lowpass_on=False、any_stage=False;三份 JSON 解析通过;compileall 通过;SDK 内 _filters_enabled 仅剩派生定义一处引用。 - Not in this change: 未动采集路径与 read_info/force_triplets(第 2 步),未动 calibration.py 指纹(第 2 步),未升级 venv 里的 SDK(第 5 步)——因此 app 当前行为不变(它仍在运行时硬编码 lowpass_cutoff_hz=10,压过本配置),"默认关"要到 app 改用 FilterSettings.apply_to(sensor) 后才在界面体现。 2026-09-14 10:26:39 +08:00
ykcheungab 514874e2f4 added 5 sensors multi-stream support, 5 finger reach > 100 fps stable using .mp4 video 2026-09-13 12:18:27 +08:00
ykcheungab 0d5d8ff70d update filter, flow ema 0.5 + med 3 2026-09-12 19:36:36 +08:00
30 changed files with 3540 additions and 81 deletions
+4 -1
View File
@@ -42,4 +42,7 @@ __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
View File
@@ -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` 是为此新增的字段。
+104
View File
@@ -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 节。
+7
View File
@@ -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"
}
+7
View File
@@ -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"
}
+7
View File
@@ -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"
}
+305
View File
@@ -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())
+7
View File
@@ -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,
+8 -1
View File
@@ -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,
+8 -1
View File
@@ -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,
+11
View File
@@ -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',
+252 -14
View File
@@ -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
p = Path(path)
pipeline["config_ref"] = str(p)
if not p.is_file():
return pipeline
try:
data = json.loads(p.read_text(encoding="utf-8"))
except Exception:
return pipeline
if not isinstance(data, dict):
return pipeline return pipeline
p = Path(path) if path is not None else None
if p is not None:
pipeline["config_ref"] = str(p)
data: dict[str, Any] = {}
if p is not None and p.is_file():
try:
loaded = json.loads(p.read_text(encoding="utf-8"))
except Exception:
loaded = None
if isinstance(loaded, dict):
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
+365
View File
@@ -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,
}
+9
View File
@@ -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,
+16 -1
View File
@@ -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:
View File
+45
View File
@@ -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
View File
@@ -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,
+5 -1
View File
@@ -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)
+465
View File
@@ -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
+562 -47
View File
@@ -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):
flow = self.flow def _apply_flow_ema(self, flow):
"""Temporally smooth the optical-flow field before HHD.
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.")
cv2.destroyAllWindows() try:
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:
rate = float(lowpass_sample_rate_hz) # Accept a positive number to pin the rate, or "auto"/<=0 to track
if rate <= 0: # the measured output rate (see _update_force_sample_rate).
raise ValueError("lowpass_sample_rate_hz must be > 0") auto = False
self._lowpass_sample_rate_hz = rate 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)
except (TypeError, ValueError):
auto = True
else:
auto = rate <= 0.0
self._lowpass_rate_auto = auto
if not auto:
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)
+114
View File
@@ -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
+21 -4
View File
@@ -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)
flow_out = flow.copy() if not getattr(self, "_Tracker__adaptive", False):
flow_out[~mask] = 0 # The non-adaptive path hands back the DIS state buffer, which feeds
return flow_out # the next frame as its initial guess; never mutate it.
flow_out = flow.copy()
flow_out[~mask] = 0
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
View File
@@ -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 = (
+187
View File
@@ -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)
+116
View File
@@ -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))
+281
View File
@@ -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"))
+117
View File
@@ -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
+329
View File
@@ -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