Compare commits
4 Commits
feature/se
...
a37207b6ff
| Author | SHA1 | Date | |
|---|---|---|---|
| a37207b6ff | |||
| 3b471992f2 | |||
| 24aaae6ff2 | |||
| 62931e26ff |
5
.gitignore
vendored
5
.gitignore
vendored
@@ -1 +1,6 @@
|
||||
.worktrees/
|
||||
.agent/
|
||||
.claude/
|
||||
graphify-out/
|
||||
firmware/.pio/
|
||||
*.log
|
||||
|
||||
106
README.md
106
README.md
@@ -1,6 +1,8 @@
|
||||
# DoorCounter
|
||||
|
||||
Retail door traffic counter using M5Stack TimerCamera-F (ESP32 + OV3660). Counts entries/exits via overhead camera CV, passively scans BLE foot traffic, and reports hourly to `logs.research.bike`.
|
||||
Retail door traffic counter using M5Stack TimerCamera-F (ESP32 + OV3660). Counts walker traversals via overhead camera CV, passively scans BLE foot traffic, and reports hourly to `logs.research.bike`.
|
||||
|
||||
> **Known limitation — directional accuracy.** This firmware reports counts as `{entries, exits}` for API compatibility, but **per-walk direction labelling is not reliable at the current mount (7' overhead, straight down).** In bench testing, event detection was 100% (8/8 walks detected) while per-walk direction matched the physical walk only ~50% of the time — the centroid trajectories produced by entries and exits were nearly indistinguishable. **The number to trust is gross traffic: `entries + exits` ≈ total walkers through the doorway.** The directional split is an unreliable best-effort heuristic. See [Directional counting](#directional-counting) for why.
|
||||
|
||||
## Hardware
|
||||
|
||||
@@ -21,7 +23,8 @@ pio run -t upload --upload-port /dev/ttyUSB0
|
||||
|
||||
| Module | Behavior |
|
||||
|--------|----------|
|
||||
| CV pipeline | 5 fps, 96×96 grayscale, blob tracking, line-crossing count |
|
||||
| CV pipeline | 5 fps, 96×96 grayscale, event-based walker detector (foreground-count state machine; centroid-trajectory direction heuristic) with post-fire refractory period |
|
||||
| Detection LED | Single blink on entry, double blink on exit (preserves upload/no-WiFi status LED) |
|
||||
| BLE scanner | Continuous passive scan; deinits during hourly upload to free heap |
|
||||
| Reporter | Hourly HMAC-signed POST; 60s boot report for fast connectivity check |
|
||||
| Provisioning | Captive portal AP on first boot for WiFi setup |
|
||||
@@ -32,6 +35,71 @@ pio run -t upload --upload-port /dev/ttyUSB0
|
||||
- **First report**: 60 seconds after NTP sync (connectivity check)
|
||||
- **Subsequent reports**: every 3600 seconds
|
||||
|
||||
### Counting model — event-based walker detector
|
||||
|
||||
The CV pipeline is a **single event state machine** (no per-blob tracking
|
||||
for counting). Per-frame foreground pixel count gates event start and end;
|
||||
centroid trajectory within the active event decides direction.
|
||||
|
||||
**Event lifecycle:**
|
||||
1. **Idle → Active**: `fg_count ≥ CV_EVENT_ENTER_THRESH` (250 px) fires event start.
|
||||
Background updates freeze while the event is active so the walker does
|
||||
not get absorbed into the baseline.
|
||||
2. **Active accumulation**: every frame updates `first_c` (once), `min_c`,
|
||||
`max_c`, `last_c`, `min_y_seen`, `max_y_seen`, and the frame count.
|
||||
3. **Active → End** (either):
|
||||
- **Quiet exit**: `fg_count < CV_EVENT_EXIT_THRESH` (150 px) for
|
||||
`CV_EVENT_QUIET_FRAMES` (3) consecutive frames — walker has left.
|
||||
- **Timeout**: `event_frame_count > CV_EVENT_MAX_FRAMES` (25 frames ≈ 5s).
|
||||
4. On end, the event is finalized: gated by minimum duration, vertical
|
||||
extent (must span a large fraction of the frame), and minimum centroid
|
||||
trajectory magnitude. Background snaps to the current frame.
|
||||
5. A **refractory period** (`CV_EVENT_REFRACTORY_FRAMES` = 10 ≈ 2s) after
|
||||
a fire blocks a new event from starting — absorbs residual lingering
|
||||
motion that would otherwise double-count.
|
||||
|
||||
**Direction heuristic** (applied only if the event passes all gates):
|
||||
- `up_score = first_c − min_c` (how far centroid excursed upward)
|
||||
- `down_score = max_c − first_c` (how far it excursed downward)
|
||||
- Quiet-exit events: `is_entry = (up_score ≥ down_score)`
|
||||
- Timeout events: `is_entry = (last_c < first_c)` — net displacement is
|
||||
more reliable than excursion when the walker is still in frame at timeout.
|
||||
|
||||
Per-mount convention: centroid moving **up through the frame** (y decreasing)
|
||||
= **entry** into the store.
|
||||
|
||||
### Directional counting — known limitation
|
||||
|
||||
**Per-walk direction labelling is unreliable at the current mount.** In
|
||||
bench testing (8 alternating entry/exit walks at 4s intervals, 7' overhead
|
||||
mount pointing straight down):
|
||||
|
||||
- **Event detection**: 8/8 (100%) — every walk produced exactly one event.
|
||||
- **Aggregate split**: 4 entries + 4 exits — matches the 4+4 ground truth.
|
||||
- **Per-walk direction**: 4/8 (50%) — essentially a coin flip.
|
||||
|
||||
At this mount, entries and exits produce nearly identical centroid
|
||||
trajectories: both begin near mid-frame (walker is already large when
|
||||
`fg_count` crosses 250), both reach a peak excursion toward the top, and
|
||||
both end near mid-frame (walker's tail is still visible when `fg_count`
|
||||
drops below 150). No heuristic over the recorded centroid statistics
|
||||
separates them with better than ~50% accuracy on alternating walks.
|
||||
|
||||
**What we ship, and what the server should trust:**
|
||||
- **Gross traffic (`entries + exits`) is accurate.** This is the number
|
||||
downstream analytics should use as "people through the door this hour."
|
||||
- **Directional split is reported but unreliable.** Treat individual
|
||||
`entries` and `exits` values as a best-effort labelling. Do not infer
|
||||
net flow or dwell from them.
|
||||
|
||||
To actually recover per-walk direction would require either a physical
|
||||
change (raise or tilt the camera so walkers enter/leave through the frame
|
||||
edges) or a richer signal than centroid statistics (e.g. time-resolved
|
||||
optical flow, or a second sensor). That work is out of scope for v1.
|
||||
|
||||
See `firmware/lib/cv/cv.h` for tuning constants and `cv.cpp` for the
|
||||
finalize logic.
|
||||
|
||||
## Operator Setup
|
||||
|
||||
### 1. Flash firmware
|
||||
@@ -55,6 +123,12 @@ python tools/flash_device.py \
|
||||
|
||||
WiFi credentials are optional — if omitted, device starts captive portal on boot.
|
||||
|
||||
> **Re-provision after firmware uploads.** Flashing firmware via
|
||||
> `pio run -t upload` may clear the NVS partition on this board. If the device
|
||||
> boots into a ~1 Hz LED blink (the "not provisioned" fatal state) after a
|
||||
> firmware update, re-run `flash_device.py` with the same credentials. See
|
||||
> [Troubleshooting](#troubleshooting).
|
||||
|
||||
### 3. OTA updates
|
||||
|
||||
```bash
|
||||
@@ -70,7 +144,7 @@ python tools/ota_push.py \
|
||||
3. Connect phone to `DoorCounter-Setup` WiFi
|
||||
4. Browser opens automatically → enter store WiFi password → done
|
||||
|
||||
**LED indicators**: Red = no WiFi · Blue = counting · Yellow = uploading
|
||||
**LED indicators**: Red = no WiFi · Blue = counting · Yellow = uploading · Brief flash (×1) on entry · Brief flash (×2) on exit
|
||||
|
||||
## API
|
||||
|
||||
@@ -90,7 +164,9 @@ All requests are HMAC-SHA256 signed. See [design spec](docs/superpowers/specs/20
|
||||
DoorCounter/
|
||||
├── firmware/
|
||||
│ ├── platformio.ini
|
||||
│ ├── lib/hmac/ — HMAC-SHA256 signing library
|
||||
│ ├── lib/
|
||||
│ │ ├── cv/ — CV pipeline (event state machine, centroid-trajectory direction)
|
||||
│ │ └── hmac/ — HMAC-SHA256 signing library
|
||||
│ └── src/
|
||||
│ ├── main.cpp — FreeRTOS tasks, boot sequence
|
||||
│ ├── config.* — NVS read/write
|
||||
@@ -100,8 +176,24 @@ DoorCounter/
|
||||
│ └── reporter.* — hourly batch POST + local buffer
|
||||
├── tools/
|
||||
│ ├── flash_device.py — NVS provisioning script
|
||||
│ └── ota_push.py — OTA push script
|
||||
├── docs/superpowers/specs/
|
||||
│ └── 2026-04-13-door-counter-design.md
|
||||
│ ├── ota_push.py — OTA push script
|
||||
│ └── serial_monitor.py — reset + read serial with timestamps (diagnostic)
|
||||
├── docs/
|
||||
│ ├── server-prompt-crossing-cooldown.md — server-side coordination notes
|
||||
│ └── superpowers/specs/2026-04-13-door-counter-design.md
|
||||
└── server/ — API server (separate deployment)
|
||||
```
|
||||
|
||||
## Troubleshooting
|
||||
|
||||
| Symptom | Likely cause | Remedy |
|
||||
|---------|--------------|--------|
|
||||
| ~1 Hz LED blink after boot, no serial beyond `esp_core_dump_flash: No core dump partition found!` | NVS missing `device_id` / `location_id` / `hmac_secret`. Commonly triggered by a firmware upload wiping NVS. | Re-run `flash_device.py` with the device's known credentials. |
|
||||
| Device stays on `DoorCounter-Setup` AP instead of joining customer WiFi | SSID/password in NVS wrong, or network out of range. | Connect phone to `DoorCounter-Setup` → captive portal → re-enter WiFi. Or reflash NVS with correct `--wifi-ssid` / `--wifi-password`. |
|
||||
| No entries/exits counted for a known-walking doorway | WiFi captive portal still up (camera task starts only after connect); or camera blocked/unfocused. | Check LED: solid on = booting/uploading, off = counting. Run `serial_monitor.py` to see `[CV] entry/exit` log lines. |
|
||||
|
||||
Capture a boot log with timestamps:
|
||||
|
||||
```bash
|
||||
python tools/serial_monitor.py --port /dev/ttyUSB0 --reset --timestamp --seconds 30
|
||||
```
|
||||
|
||||
78
docs/server-prompt-crossing-cooldown.md
Normal file
78
docs/server-prompt-crossing-cooldown.md
Normal file
@@ -0,0 +1,78 @@
|
||||
# Server-Side Prompt — Crossing Cooldown Awareness
|
||||
|
||||
> Give this to your server-side agent after the firmware commit that introduces
|
||||
> `CV_CROSSING_COOLDOWN_FRAMES` in `firmware/lib/cv/cv.h` has been flashed to devices.
|
||||
|
||||
## Context
|
||||
|
||||
The DoorCounter firmware now enforces a **per-direction crossing cooldown** in
|
||||
its CV pipeline. After a counted entry, subsequent entries within 5 frames
|
||||
(~1 second at 5 fps) are silently dropped on-device. Exits follow the same
|
||||
rule independently. This is a device-side fix for the known track-churn bug
|
||||
(single person producing 5+ counts per visit when their blob briefly drops
|
||||
below the minimum-blob-pixel threshold).
|
||||
|
||||
Constants live in `firmware/lib/cv/cv.h`:
|
||||
|
||||
```c++
|
||||
static const uint32_t CV_CROSSING_COOLDOWN_FRAMES = 5; // per-direction
|
||||
```
|
||||
|
||||
Cooldown is **fixed at compile time**. It is **not** currently a server-
|
||||
pushable tuning parameter.
|
||||
|
||||
## What the server should do
|
||||
|
||||
1. **Expect lower per-hour entry/exit counts** from devices running this
|
||||
firmware compared to their historical baseline. This reflects suppression
|
||||
of false positives, not a device regression. Do not alert on the drop.
|
||||
|
||||
2. **If you have a per-device tuning-config push mechanism** (the one planned
|
||||
in `.agent/plan.md` — server-push CV config via heartbeat response), add
|
||||
`cooldown_frames` to the sendable tuning set with:
|
||||
- Default: `5`
|
||||
- Valid range: `0..60` (0 disables cooldown; 60 ≈ 12s at 5 fps)
|
||||
- Semantics: per-direction, applied to both entries and exits
|
||||
- Persist alongside other CV tuning keys in NVS under a new key `cv_cool`.
|
||||
- Bump the `cfg_version` scheme accordingly.
|
||||
|
||||
**Do not ship this server-side change yet** — the firmware change in this
|
||||
commit keeps the cooldown as a compile-time constant. A future firmware
|
||||
commit on `feature/server-push-config` will expose it as a runtime
|
||||
tunable and bump `cfg_version`. Coordinate the rollout: firmware first,
|
||||
then server.
|
||||
|
||||
3. **Dashboard**: if you render device CV parameters in a per-device settings
|
||||
view, add a read-only row "Crossing cooldown (frames): 5" sourced from
|
||||
the firmware's compiled default. Mark it editable only once the firmware
|
||||
exposes it as a tunable.
|
||||
|
||||
4. **Telemetry (optional, low priority)**: consider adding a field
|
||||
`suppressed_crossings_delta` to the heartbeat or camera-events payload
|
||||
so operators can see how often cooldown is kicking in. This would require
|
||||
a firmware change; flag it as future work only if churn continues.
|
||||
|
||||
## What NOT to do
|
||||
|
||||
- Do not attempt to push `cooldown_frames` via the existing config channel
|
||||
today — the firmware will ignore unknown fields, which is fine, but
|
||||
shipping server changes that assume the firmware-side plumbing exists
|
||||
will break the integration contract.
|
||||
- Do not "correct" the lower counts via server-side multiplication. The
|
||||
cooldown is the correct behavior; old counts were inflated by the churn
|
||||
bug.
|
||||
|
||||
## Verification checklist
|
||||
|
||||
- [ ] Historical counts chart annotated with "firmware v{N} deployed"
|
||||
marker on the rollout date.
|
||||
- [ ] Per-device tuning view renders cooldown row (read-only for now).
|
||||
- [ ] No alert fires on the per-device count drop post-rollout.
|
||||
|
||||
## Reference
|
||||
|
||||
- Firmware change: `firmware/lib/cv/cv.h` (`CV_CROSSING_COOLDOWN_FRAMES`),
|
||||
`firmware/lib/cv/cv.cpp` (suppression logic in `cv_process`).
|
||||
- Design spec: `docs/superpowers/specs/2026-04-13-door-counter-design.md`
|
||||
§ 3.1 "Counting logic".
|
||||
- Unit test: `firmware/test/test_cv/test_cv.cpp::test_cooldown_suppresses_rapid_re_entry`.
|
||||
@@ -13,7 +13,7 @@
|
||||
[TimerCamera-F Device]
|
||||
├── Provisioning module — captive portal AP on first boot
|
||||
├── Config store — NVS: device_id, location_id, HMAC secret, WiFi creds, line_offset
|
||||
├── Camera + CV module — captures frames, runs line-crossing counter
|
||||
├── Camera + CV module — captures frames, runs event-based walker detector
|
||||
├── BLE scanner — continuous passive scan (WiFi coexistence mode)
|
||||
├── Report buffer — accumulates counts in RAM, flushes hourly
|
||||
└── HTTP client — HMAC-signed POSTs to logs.research.bike
|
||||
@@ -89,15 +89,38 @@ Capture → Grayscale → Downscale 96×96 → Frame diff → Threshold → Blob
|
||||
| Downscale | Bilinear to 96×96 (~11× compute reduction) |
|
||||
| Frame diff | Absolute difference against rolling background (updated every ~2s when no motion) |
|
||||
| Threshold | Pixels > 30 intensity delta = foreground |
|
||||
| Blob detect | Connected components; blobs < 8×8 px discarded as noise |
|
||||
| Centroid track | Nearest-centroid matching frame-to-frame (max 15px), tracks persist up to 10 missed frames |
|
||||
| Line crossing | Virtual horizontal line at configurable vertical position (default: 50% of frame height) |
|
||||
| Event state machine | Single global state machine (not per-blob). Per-frame `fg_count` (total foreground pixels) gates event start and end. |
|
||||
| Event start | `fg_count ≥ CV_EVENT_ENTER_THRESH` (250 px) → event becomes active. Background updates freeze for the event's duration so the walker does not blend into the baseline. |
|
||||
| Event accumulation | Each frame records `first_c` (centroid_y at start), running `min_c` / `max_c` / `last_c`, vertical extents (`min_y_seen`, `max_y_seen`), and frame count. |
|
||||
| Event end | Either **quiet exit** (`fg_count < CV_EVENT_EXIT_THRESH` (150 px) for `CV_EVENT_QUIET_FRAMES` (3) consecutive frames) or **timeout** (`event_frame_count > CV_EVENT_MAX_FRAMES` (25)). On end, background snaps to the current frame. |
|
||||
| Fire gates | Duration ≥ `CV_EVENT_MIN_FRAMES` (5), `min_y_seen ≤ CV_EVENT_EXTENT_TOP` (25) AND `max_y_seen ≥ CV_EVENT_EXTENT_BOT` (50) — event must span a large fraction of the frame — AND `max(up_score, down_score) ≥ CV_EVENT_MIN_TRAJ` (5) |
|
||||
| Refractory | `CV_EVENT_REFRACTORY_FRAMES` (10 ≈ 2s) after a fire, the machine refuses to start a new event — absorbs lingering motion of the just-counted walker. |
|
||||
|
||||
**Counting logic:**
|
||||
- Centroid crosses line top→bottom = **entry**
|
||||
- Centroid crosses line bottom→top = **exit**
|
||||
**Direction heuristic (applied after fire gates pass):**
|
||||
- `up_score = first_c − min_c` (peak upward centroid excursion)
|
||||
- `down_score = max_c − first_c` (peak downward centroid excursion)
|
||||
- **Quiet-exit fires**: `is_entry = (up_score ≥ down_score)`
|
||||
- **Timeout fires**: `is_entry = (last_c < first_c)` — walker is still in frame at timeout, so net displacement is a better signal than excursion.
|
||||
|
||||
Counts accumulate as `{entries, exits}` in RAM and reset each hour on report.
|
||||
Per-mount convention: centroid moving **up through the frame** (y decreasing) = **entry** into the store.
|
||||
|
||||
**Counting surface**: `{entries, exits}` accumulate in RAM and reset each hour on report.
|
||||
|
||||
**Directional accuracy is best-effort, not guaranteed.** In bench testing at the intended 7' overhead straight-down mount:
|
||||
|
||||
| Metric | Result |
|
||||
|--------|--------|
|
||||
| Event detection | 8/8 walks (100%) |
|
||||
| Aggregate entry/exit split | 4+4 vs ground-truth 4+4 (matches) |
|
||||
| Per-walk direction labelling | 4/8 (50%) — no better than chance |
|
||||
|
||||
At this mount, entries and exits produce nearly identical centroid trajectories: the walker is already large when `fg_count` crosses 250 (so `first_c` is always near mid-frame), their tail is still visible when `fg_count` drops below 150 (so `last_c` is always near mid-frame), and the excursion in between peaks upward for both directions. No statistic computable from (`first_c`, `min_c`, `max_c`, `last_c`, duration) separates them reliably.
|
||||
|
||||
**Contract with downstream consumers (API and analytics):**
|
||||
- **`entries + exits` is the trustworthy number** — it is the count of walkers through the doorway in the hour. Use this as "foot traffic."
|
||||
- **Individual `entries` and `exits` are reported for API shape compatibility, but should not be relied on for net flow, dwell, or any per-direction analysis.**
|
||||
|
||||
Recovering true direction requires either a physical change (tilt or raise the camera so walkers pass fully through the frame edges) or a richer signal (time-resolved centroid trajectory, optical flow, secondary sensor). Both are out of scope for v1.
|
||||
|
||||
---
|
||||
|
||||
|
||||
@@ -5,8 +5,21 @@
|
||||
#include <algorithm>
|
||||
#include <vector>
|
||||
|
||||
static void event_reset(CVState& s) {
|
||||
s.event_active = false;
|
||||
s.event_start_frame = 0;
|
||||
s.event_frame_count = 0;
|
||||
s.event_peak_n = 0;
|
||||
s.event_first_c = -1.0f;
|
||||
s.event_last_c = -1.0f;
|
||||
s.event_min_c = (float)CV_H;
|
||||
s.event_max_c = -1.0f;
|
||||
s.event_min_y_seen = CV_H;
|
||||
s.event_max_y_seen = -1;
|
||||
s.event_quiet_count = 0;
|
||||
}
|
||||
|
||||
void cv_init(CVState& state) {
|
||||
// Initialize members directly — avoid CVState{} temporary which puts 9KB on stack
|
||||
memset(state.background, 0, sizeof(state.background));
|
||||
state.bg_valid = false;
|
||||
state.last_motion_frame = 0;
|
||||
@@ -15,6 +28,8 @@ void cv_init(CVState& state) {
|
||||
state.tracks.clear();
|
||||
state.entries = 0;
|
||||
state.exits = 0;
|
||||
state.last_fire_frame = 0;
|
||||
event_reset(state);
|
||||
}
|
||||
|
||||
void cv_reset_counts(CVState& state) {
|
||||
@@ -24,9 +39,6 @@ void cv_reset_counts(CVState& state) {
|
||||
|
||||
struct Point { int x, y; };
|
||||
|
||||
// Note: queue may grow to CV_PIXELS entries (~72KB) on large blobs.
|
||||
// Requires PSRAM (enabled via -DBOARD_HAS_PSRAM in platformio.ini).
|
||||
// BFS flood fill. Marks visited pixels (sets fg to 0). Returns {-1,-1} if blob < CV_MIN_BLOB_PX.
|
||||
static std::pair<float,float> extract_blob(uint8_t* fg, int start_x, int start_y) {
|
||||
std::vector<Point> queue;
|
||||
queue.reserve(512);
|
||||
@@ -58,7 +70,7 @@ static std::pair<float,float> extract_blob(uint8_t* fg, int start_x, int start_y
|
||||
|
||||
static std::vector<std::pair<float,float>> find_centroids(const uint8_t* fg) {
|
||||
std::vector<std::pair<float,float>> result;
|
||||
static uint8_t fg_copy[CV_PIXELS]; // static to avoid 9KB stack allocation
|
||||
static uint8_t fg_copy[CV_PIXELS];
|
||||
memcpy(fg_copy, fg, CV_PIXELS);
|
||||
|
||||
for (int y = 0; y < CV_H; y++) {
|
||||
@@ -80,8 +92,62 @@ static void frame_diff(const uint8_t* frame, const uint8_t* bg,
|
||||
}
|
||||
}
|
||||
|
||||
CVResult cv_process(CVState& state, const uint8_t* frame, uint8_t line_pct) {
|
||||
CVResult result = {0, 0};
|
||||
// Decide whether the just-ended event should fire and in which direction.
|
||||
// Up-through-frame (centroid excursion from high y toward low y) maps to
|
||||
// ENTRY per mount convention.
|
||||
static void finalize_event(CVState& s, CVResult& result) {
|
||||
if (s.event_frame_count < CV_EVENT_MIN_FRAMES) return;
|
||||
// Note: no MAX_FRAMES rejection here. An event that runs the full duration
|
||||
// may still be a valid walker whose fg_count stayed above EXIT_THRESH due
|
||||
// to a stale bg or an AEC-driven lighting shift. Extent + MIN_TRAJ gates
|
||||
// below already reject stationary-person / wobble events.
|
||||
if (s.event_min_y_seen > CV_EVENT_EXTENT_TOP) return;
|
||||
if (s.event_max_y_seen < CV_EVENT_EXTENT_BOT) return;
|
||||
|
||||
// Direction from centroid excursion relative to event start.
|
||||
// up_score: how far centroid excursed upward (smaller y) from first_c.
|
||||
// down_score: how far it excursed downward (larger y) from first_c.
|
||||
float up_score = s.event_first_c - s.event_min_c;
|
||||
float down_score = s.event_max_c - s.event_first_c;
|
||||
|
||||
float winning = (up_score >= down_score) ? up_score : down_score;
|
||||
if (winning < CV_EVENT_MIN_TRAJ) return;
|
||||
|
||||
// Timeout-aware direction. Quiet-exit events (fg fell below EXIT_THRESH)
|
||||
// have walker fully out of frame → min/max excursion bracket the true
|
||||
// traversal and up/down scores are reliable. Timeout events (event hit
|
||||
// MAX_FRAMES while still elevated) captured both an approach and a
|
||||
// departure within the window, so excursion measures the walker's
|
||||
// *range in frame* rather than direction — an entry walker who paused
|
||||
// near the top, then drifted back toward the middle before timeout
|
||||
// gets (wrongly) called an entry by up-score even though net motion is
|
||||
// mixed. For those, the net first→last centroid displacement is a
|
||||
// better direction signal (it's where the walker ended up, not just
|
||||
// where they peaked).
|
||||
bool timed_out = (s.event_frame_count > CV_EVENT_MAX_FRAMES);
|
||||
bool is_entry;
|
||||
if (timed_out) {
|
||||
is_entry = (s.event_last_c < s.event_first_c);
|
||||
} else {
|
||||
is_entry = (up_score >= down_score);
|
||||
}
|
||||
if (is_entry) {
|
||||
s.entries++;
|
||||
result.entries_delta++;
|
||||
} else {
|
||||
s.exits++;
|
||||
result.exits_delta++;
|
||||
}
|
||||
s.last_fire_frame = s.frame_index;
|
||||
result.fire_first_c = s.event_first_c;
|
||||
result.fire_min_c = s.event_min_c;
|
||||
result.fire_max_c = s.event_max_c;
|
||||
result.fire_last_c = s.event_last_c;
|
||||
result.fire_duration = s.event_frame_count;
|
||||
}
|
||||
|
||||
CVResult cv_process(CVState& state, const uint8_t* frame, uint8_t /*line_pct*/) {
|
||||
CVResult result = {0, 0, 0, -1, -1, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0};
|
||||
state.frame_index++;
|
||||
|
||||
if (!state.bg_valid) {
|
||||
@@ -90,85 +156,147 @@ CVResult cv_process(CVState& state, const uint8_t* frame, uint8_t line_pct) {
|
||||
return result;
|
||||
}
|
||||
|
||||
static uint8_t fg[CV_PIXELS]; // static: avoids 9KB on task stack
|
||||
static uint8_t fg[CV_PIXELS];
|
||||
frame_diff(frame, state.background, fg, CV_PIXELS);
|
||||
|
||||
int fg_count = 0;
|
||||
for (int i = 0; i < CV_PIXELS; i++) fg_count += fg[i];
|
||||
|
||||
bool motion = fg_count > CV_MIN_BLOB_PX;
|
||||
if (!motion) {
|
||||
if (state.frame_index - state.last_motion_frame > 10) {
|
||||
memcpy(state.background, frame, CV_PIXELS);
|
||||
// Running-average background blend: bg = (31*bg + frame)/32. Adapts to
|
||||
// slow scene drift during idle periods. Frozen during an active event so
|
||||
// the walker's signature is never absorbed — otherwise bg retains a
|
||||
// "ghost" of the walker for ~30 frames after they leave, keeping fg_count
|
||||
// elevated and preventing subsequent walkers from producing a clean
|
||||
// trajectory.
|
||||
if (!state.event_active) {
|
||||
for (int i = 0; i < CV_PIXELS; i++) {
|
||||
state.background[i] = (uint8_t)(((uint16_t)state.background[i] * 31 + frame[i]) >> 5);
|
||||
}
|
||||
}
|
||||
|
||||
int fg_count = 0;
|
||||
int min_y = CV_H, max_y = -1;
|
||||
long sum_y = 0;
|
||||
for (int y = 0; y < CV_H; y++) {
|
||||
const uint8_t* row = &fg[y * CV_W];
|
||||
int row_count = 0;
|
||||
for (int x = 0; x < CV_W; x++) row_count += row[x];
|
||||
if (row_count > 0) {
|
||||
if (y < min_y) min_y = y;
|
||||
if (y > max_y) max_y = y;
|
||||
sum_y += (long)row_count * y;
|
||||
fg_count += row_count;
|
||||
}
|
||||
}
|
||||
result.fg_count = fg_count;
|
||||
result.fg_min_y = (fg_count > 0) ? min_y : -1;
|
||||
result.fg_max_y = (fg_count > 0) ? max_y : -1;
|
||||
result.fg_centroid_y = (fg_count > 0) ? ((float)sum_y / fg_count) : -1.0f;
|
||||
|
||||
// Hard self-heal: if more than half the frame is fg, bg is catastrophically
|
||||
// wrong. Snap and skip the event machine this frame.
|
||||
if (fg_count > CV_PIXELS / 2) {
|
||||
memcpy(state.background, frame, CV_PIXELS);
|
||||
state.last_motion_frame = state.frame_index;
|
||||
if (state.event_active) event_reset(state);
|
||||
return result;
|
||||
}
|
||||
|
||||
// Diagnostic track management (no effect on counting).
|
||||
bool motion = fg_count > CV_MIN_BLOB_PX;
|
||||
if (motion) {
|
||||
state.last_motion_frame = state.frame_index;
|
||||
auto centroids = find_centroids(fg);
|
||||
std::vector<bool> centroid_matched(centroids.size(), false);
|
||||
for (auto& track : state.tracks) {
|
||||
float best_dist = CV_MAX_MOVE * CV_MAX_MOVE;
|
||||
int best_idx = -1;
|
||||
for (int i = 0; i < (int)centroids.size(); i++) {
|
||||
if (centroid_matched[i]) continue;
|
||||
float dx = centroids[i].first - track.x;
|
||||
float dy = centroids[i].second - track.y;
|
||||
float d2 = dx*dx + dy*dy;
|
||||
if (d2 < best_dist) { best_dist = d2; best_idx = i; }
|
||||
}
|
||||
if (best_idx >= 0) {
|
||||
centroid_matched[best_idx] = true;
|
||||
track.x = centroids[best_idx].first;
|
||||
track.y = centroids[best_idx].second;
|
||||
track.missed = 0;
|
||||
} else {
|
||||
track.missed++;
|
||||
}
|
||||
}
|
||||
state.tracks.erase(
|
||||
std::remove_if(state.tracks.begin(), state.tracks.end(),
|
||||
[](const CVTrack& t){ return t.missed > CV_MAX_MISSED; }),
|
||||
state.tracks.end());
|
||||
for (int i = 0; i < (int)centroids.size(); i++) {
|
||||
if (centroid_matched[i]) continue;
|
||||
CVTrack t;
|
||||
t.id = state.next_id++;
|
||||
t.x = centroids[i].first;
|
||||
t.y = centroids[i].second;
|
||||
t.spawn_y = t.y;
|
||||
t.missed = 0;
|
||||
state.tracks.push_back(t);
|
||||
}
|
||||
} else {
|
||||
for (auto& t : state.tracks) t.missed++;
|
||||
state.tracks.erase(
|
||||
std::remove_if(state.tracks.begin(), state.tracks.end(),
|
||||
[](const CVTrack& t){ return t.missed > CV_MAX_MISSED; }),
|
||||
state.tracks.end());
|
||||
return result;
|
||||
}
|
||||
|
||||
state.last_motion_frame = state.frame_index;
|
||||
// Event state machine. Refractory period after a fire blocks new events
|
||||
// for CV_EVENT_REFRACTORY_FRAMES frames — absorbs lingering-walker motion
|
||||
// that would otherwise re-trigger a second count.
|
||||
bool in_refractory = state.last_fire_frame != 0 &&
|
||||
(state.frame_index - state.last_fire_frame) < CV_EVENT_REFRACTORY_FRAMES;
|
||||
|
||||
auto centroids = find_centroids(fg);
|
||||
|
||||
std::vector<bool> centroid_matched(centroids.size(), false);
|
||||
|
||||
for (auto& track : state.tracks) {
|
||||
float best_dist = CV_MAX_MOVE * CV_MAX_MOVE;
|
||||
int best_idx = -1;
|
||||
|
||||
for (int i = 0; i < (int)centroids.size(); i++) {
|
||||
if (centroid_matched[i]) continue;
|
||||
float dx = centroids[i].first - track.x;
|
||||
float dy = centroids[i].second - track.y;
|
||||
float d2 = dx*dx + dy*dy;
|
||||
if (d2 < best_dist) { best_dist = d2; best_idx = i; }
|
||||
if (!state.event_active) {
|
||||
if (!in_refractory && fg_count >= CV_EVENT_ENTER_THRESH) {
|
||||
state.event_active = true;
|
||||
state.event_start_frame = state.frame_index;
|
||||
state.event_frame_count = 1;
|
||||
state.event_peak_n = fg_count;
|
||||
state.event_first_c = result.fg_centroid_y;
|
||||
state.event_last_c = result.fg_centroid_y;
|
||||
state.event_min_c = result.fg_centroid_y;
|
||||
state.event_max_c = result.fg_centroid_y;
|
||||
state.event_min_y_seen = min_y;
|
||||
state.event_max_y_seen = max_y;
|
||||
state.event_quiet_count = 0;
|
||||
}
|
||||
|
||||
if (best_idx >= 0) {
|
||||
centroid_matched[best_idx] = true;
|
||||
track.x = centroids[best_idx].first;
|
||||
track.y = centroids[best_idx].second;
|
||||
track.missed = 0;
|
||||
} else {
|
||||
state.event_frame_count++;
|
||||
if (fg_count > state.event_peak_n) state.event_peak_n = fg_count;
|
||||
if (fg_count > 0) {
|
||||
state.event_last_c = result.fg_centroid_y;
|
||||
if (result.fg_centroid_y < state.event_min_c) state.event_min_c = result.fg_centroid_y;
|
||||
if (result.fg_centroid_y > state.event_max_c) state.event_max_c = result.fg_centroid_y;
|
||||
if (min_y < state.event_min_y_seen) state.event_min_y_seen = min_y;
|
||||
if (max_y > state.event_max_y_seen) state.event_max_y_seen = max_y;
|
||||
}
|
||||
if (fg_count < CV_EVENT_EXIT_THRESH) {
|
||||
state.event_quiet_count++;
|
||||
if (state.event_quiet_count >= CV_EVENT_QUIET_FRAMES) {
|
||||
finalize_event(state, result);
|
||||
event_reset(state);
|
||||
memcpy(state.background, frame, CV_PIXELS);
|
||||
}
|
||||
} else {
|
||||
track.missed++;
|
||||
}
|
||||
}
|
||||
|
||||
state.tracks.erase(
|
||||
std::remove_if(state.tracks.begin(), state.tracks.end(),
|
||||
[](const CVTrack& t){ return t.missed > CV_MAX_MISSED; }),
|
||||
state.tracks.end());
|
||||
|
||||
float line_y = (line_pct / 100.0f) * CV_H;
|
||||
for (int i = 0; i < (int)centroids.size(); i++) {
|
||||
if (centroid_matched[i]) continue;
|
||||
CVTrack t;
|
||||
t.id = state.next_id++;
|
||||
t.x = centroids[i].first;
|
||||
t.y = centroids[i].second;
|
||||
t.above_line = (t.y < line_y);
|
||||
t.missed = 0;
|
||||
state.tracks.push_back(t);
|
||||
}
|
||||
// Line crossing check
|
||||
for (auto& track : state.tracks) {
|
||||
if (track.missed > 0) continue; // only check tracks matched this frame
|
||||
bool now_above = (track.y < line_y);
|
||||
if (now_above != track.above_line) {
|
||||
if (!now_above) {
|
||||
// was above, now below → entry
|
||||
state.entries++;
|
||||
result.entries_delta++;
|
||||
} else {
|
||||
// was below, now above → exit
|
||||
state.exits++;
|
||||
result.exits_delta++;
|
||||
state.event_quiet_count = 0;
|
||||
if (state.event_frame_count > CV_EVENT_MAX_FRAMES) {
|
||||
// Timeout end: fg still elevated. Snap bg anyway — in practice
|
||||
// a stuck-high event means bg is stale (walker has merged
|
||||
// with stale bg, or AEC shifted). Leaving bg stale permanently
|
||||
// poisons subsequent events. If a walker truly is mid-frame
|
||||
// they'll get absorbed into bg, but that's a rare corner
|
||||
// beaten by the common case of stale bg chaining events.
|
||||
finalize_event(state, result);
|
||||
event_reset(state);
|
||||
memcpy(state.background, frame, CV_PIXELS);
|
||||
}
|
||||
}
|
||||
track.above_line = now_above;
|
||||
}
|
||||
|
||||
return result;
|
||||
|
||||
@@ -12,10 +12,63 @@ static const int CV_MIN_BLOB_PX = 64;
|
||||
static const float CV_MAX_MOVE = 15.0f;
|
||||
static const int CV_MAX_MISSED = 10;
|
||||
|
||||
// Event-based walker detector. Per-frame zone-flip approaches were direction-
|
||||
// blind at realistic mounts: a walker traversing top-to-bottom and a walker
|
||||
// traversing bottom-to-top produced identical zone-dominance sequences
|
||||
// (geometric artifact of asymmetric zones + body spanning the line). The
|
||||
// event approach buffers a whole walker event, then decides direction from
|
||||
// the centroid trajectory: sign(first_centroid_y - peak_centroid_y) > 0 means
|
||||
// the centroid moved upward through the frame during the event.
|
||||
//
|
||||
// Per-mount convention: UP through frame == ENTRY into store. Flip the camera
|
||||
// mount or invert the mapping in cv_process if the physical install differs.
|
||||
|
||||
// fg_count thresholds that gate event start/end. Tuned against a real
|
||||
// 8-walk isolated test (see .agent/walk_isolated_8walks.log). Lower than
|
||||
// initial guesses because the 7' overhead mount produces smaller centroid
|
||||
// excursions than we originally modelled.
|
||||
static const int CV_EVENT_ENTER_THRESH = 250;
|
||||
static const int CV_EVENT_EXIT_THRESH = 150;
|
||||
|
||||
// Number of consecutive sub-EXIT frames required to end an event.
|
||||
static const int CV_EVENT_QUIET_FRAMES = 3;
|
||||
|
||||
// Min/max event duration in frames. Below min = too brief to be a walker
|
||||
// (noise burst). Above max = stationary object or stuck detection.
|
||||
static const int CV_EVENT_MIN_FRAMES = 5;
|
||||
// MAX bounds the event duration. Too low (15) cut events off while walker
|
||||
// was still physically in frame — every fire hit dur=MAX+1 and bg snapped
|
||||
// with a walker-ghost baked in, corrupting the next walk. Too high (40)
|
||||
// merged multiple walkers. 25 frames (5s) lets a single walker reach the
|
||||
// quiet-exit path (fg drops below EXIT_THRESH) before timeout, so bg snaps
|
||||
// on a clean empty frame.
|
||||
static const int CV_EVENT_MAX_FRAMES = 25;
|
||||
|
||||
// Required vertical extent: during the event, fg must have reached near the
|
||||
// top of the frame (min_y <= TOP) AND near the bottom (max_y >= BOT). At a
|
||||
// 7' overhead mount real walkers span fg y≈0..70, not 0..95 — the original
|
||||
// 10/85 gates rejected most real walks. Relaxed to catch them while still
|
||||
// filtering small local motion that doesn't span the doorway.
|
||||
static const int CV_EVENT_EXTENT_TOP = 25;
|
||||
static const int CV_EVENT_EXTENT_BOT = 50;
|
||||
|
||||
// Minimum centroid excursion (max of up_score/down_score) for a valid
|
||||
// trajectory. At overhead mount walker centroid traverses ~15-40 pixels;
|
||||
// 15 was too aggressive and dropped clean walks. 5 still filters wobble.
|
||||
static const float CV_EVENT_MIN_TRAJ = 5.0f;
|
||||
|
||||
// Refractory period after a fire. Shorter than originally chosen — at 5 fps
|
||||
// a second walker can arrive within 2s of the first, especially at busy
|
||||
// doorways. 10 frames = 2s of back-pressure, tuned to match the gap between
|
||||
// consecutive isolated walks in the test log.
|
||||
static const uint32_t CV_EVENT_REFRACTORY_FRAMES = 10;
|
||||
|
||||
// Diagnostic only: tracks are kept for spawn logging. Counting does NOT
|
||||
// depend on tracks.
|
||||
struct CVTrack {
|
||||
int id;
|
||||
float x, y;
|
||||
bool above_line;
|
||||
float spawn_y;
|
||||
int missed;
|
||||
};
|
||||
|
||||
@@ -28,11 +81,36 @@ struct CVState {
|
||||
std::vector<CVTrack> tracks;
|
||||
int entries;
|
||||
int exits;
|
||||
|
||||
// Event state machine.
|
||||
bool event_active;
|
||||
uint32_t event_start_frame;
|
||||
int event_frame_count;
|
||||
int event_peak_n;
|
||||
float event_first_c;
|
||||
float event_last_c;
|
||||
float event_min_c; // min centroid_y observed during event
|
||||
float event_max_c; // max centroid_y observed during event
|
||||
int event_min_y_seen;
|
||||
int event_max_y_seen;
|
||||
int event_quiet_count;
|
||||
uint32_t last_fire_frame; // 0 = never; frame of last counted fire
|
||||
};
|
||||
|
||||
struct CVResult {
|
||||
int entries_delta;
|
||||
int exits_delta;
|
||||
// Per-frame foreground diagnostics (populated every call).
|
||||
int fg_count;
|
||||
int fg_min_y;
|
||||
int fg_max_y;
|
||||
float fg_centroid_y;
|
||||
// Populated only on a fire frame; zeroed otherwise.
|
||||
float fire_first_c;
|
||||
float fire_min_c;
|
||||
float fire_max_c;
|
||||
float fire_last_c;
|
||||
int fire_duration;
|
||||
};
|
||||
|
||||
void cv_init(CVState& state);
|
||||
|
||||
@@ -7,6 +7,7 @@ platform = espressif32@6.6.0
|
||||
board = m5stack-timer-cam
|
||||
framework = arduino
|
||||
board_build.partitions = partitions_4mb_ota.csv
|
||||
build_src_filter = +<*> -<main_capture.cpp>
|
||||
build_flags =
|
||||
-DBOARD_HAS_PSRAM
|
||||
-mfix-esp32-psram-cache-issue
|
||||
@@ -23,6 +24,25 @@ lib_deps =
|
||||
h2zero/NimBLE-Arduino@^1.4.2
|
||||
espressif/esp32-camera
|
||||
|
||||
; Frame-capture build. Strips WiFi/BLE/CV/reporter; streams raw 96x96 frames
|
||||
; over serial at 921600 baud for offline algorithm iteration.
|
||||
[env:timercam-capture]
|
||||
platform = espressif32@6.6.0
|
||||
board = m5stack-timer-cam
|
||||
framework = arduino
|
||||
board_build.partitions = partitions_4mb_ota.csv
|
||||
build_flags =
|
||||
-DBOARD_HAS_PSRAM
|
||||
-mfix-esp32-psram-cache-issue
|
||||
-DCORE_DEBUG_LEVEL=0
|
||||
-DCONFIG_SPIRAM_USE_MALLOC=1
|
||||
build_src_filter = -<*> +<main_capture.cpp> +<camera.cpp>
|
||||
monitor_speed = 460800
|
||||
upload_speed = 115200
|
||||
upload_flags = --no-stub
|
||||
lib_deps =
|
||||
espressif/esp32-camera
|
||||
|
||||
[env:native]
|
||||
platform = native
|
||||
test_framework = unity
|
||||
|
||||
@@ -24,9 +24,22 @@ static DeviceConfig g_cfg;
|
||||
static CVState g_cv;
|
||||
static SemaphoreHandle_t s_cv_mutex = nullptr;
|
||||
|
||||
// LED: simple on/off — blink patterns can be added later
|
||||
static void led_set(bool on) { digitalWrite(LED_PIN, on ? HIGH : LOW); }
|
||||
|
||||
// Non-blocking-ish detection blink. Saves and restores the current LED state
|
||||
// so it doesn't clobber upload/no-wifi indicators. Total duration: ~60ms per
|
||||
// pulse + 80ms gap between pulses.
|
||||
static void led_blink_pattern(int pulses) {
|
||||
bool prev = digitalRead(LED_PIN);
|
||||
for (int i = 0; i < pulses; i++) {
|
||||
led_set(true);
|
||||
vTaskDelay(pdMS_TO_TICKS(60));
|
||||
led_set(false);
|
||||
if (i < pulses - 1) vTaskDelay(pdMS_TO_TICKS(80));
|
||||
}
|
||||
led_set(prev);
|
||||
}
|
||||
|
||||
static void check_factory_reset() {
|
||||
if (digitalRead(BUTTON_PIN) != LOW) return;
|
||||
uint32_t held = millis();
|
||||
@@ -42,13 +55,30 @@ static void check_factory_reset() {
|
||||
// Camera + CV task — runs on core 1 at 5 fps
|
||||
static void task_camera(void*) {
|
||||
static uint8_t frame[CV_PIXELS]; // static: avoids 9KB on task stack
|
||||
int last_logged_track_id = 0; // diagnostic: log each new track once
|
||||
while (true) {
|
||||
if (camera_capture_96(frame)) {
|
||||
if (xSemaphoreTake(s_cv_mutex, pdMS_TO_TICKS(100)) == pdTRUE) {
|
||||
CVResult r = cv_process(g_cv, frame, g_cfg.line_offset);
|
||||
if (r.entries_delta) Serial.printf("[CV] entry +%d (total %d)\n", r.entries_delta, g_cv.entries);
|
||||
if (r.exits_delta) Serial.printf("[CV] exit +%d (total %d)\n", r.exits_delta, g_cv.exits);
|
||||
for (const auto& t : g_cv.tracks) {
|
||||
if (t.id > last_logged_track_id) {
|
||||
last_logged_track_id = t.id;
|
||||
Serial.printf("[CV] spawn id=%d y=%.1f\n", t.id, t.spawn_y);
|
||||
}
|
||||
}
|
||||
if (r.fg_count > 0) {
|
||||
Serial.printf("[F] n=%d y=%d..%d c=%.1f\n",
|
||||
r.fg_count, r.fg_min_y, r.fg_max_y, r.fg_centroid_y);
|
||||
}
|
||||
if (r.entries_delta) Serial.printf("[CV] entry +%d (total %d) first=%.1f min=%.1f max=%.1f last=%.1f dur=%d\n",
|
||||
r.entries_delta, g_cv.entries,
|
||||
r.fire_first_c, r.fire_min_c, r.fire_max_c, r.fire_last_c, r.fire_duration);
|
||||
if (r.exits_delta) Serial.printf("[CV] exit +%d (total %d) first=%.1f min=%.1f max=%.1f last=%.1f dur=%d\n",
|
||||
r.exits_delta, g_cv.exits,
|
||||
r.fire_first_c, r.fire_min_c, r.fire_max_c, r.fire_last_c, r.fire_duration);
|
||||
xSemaphoreGive(s_cv_mutex);
|
||||
if (r.entries_delta) led_blink_pattern(1);
|
||||
if (r.exits_delta) led_blink_pattern(2);
|
||||
}
|
||||
}
|
||||
vTaskDelay(pdMS_TO_TICKS(CAM_INTERVAL_MS));
|
||||
|
||||
64
firmware/src/main_capture.cpp
Normal file
64
firmware/src/main_capture.cpp
Normal file
@@ -0,0 +1,64 @@
|
||||
// firmware/src/main_capture.cpp
|
||||
//
|
||||
// Frame-dump firmware. Replaces main.cpp when building env:timercam-capture.
|
||||
// Streams raw 96x96 grayscale frames at 5 fps over serial (921600 baud) for
|
||||
// offline algorithm iteration.
|
||||
//
|
||||
// Wire format per frame (little-endian):
|
||||
// magic uint32 0xDC0FC0DE
|
||||
// frame_ix uint32 monotonic counter
|
||||
// millis uint32 ms since boot
|
||||
// pixels byte[9216] raw grayscale 96x96, row-major
|
||||
//
|
||||
// No WiFi, no BLE, no CV. Just camera → serial.
|
||||
|
||||
#include <Arduino.h>
|
||||
#include "camera.h"
|
||||
#include "cv.h" // for CV_PIXELS
|
||||
|
||||
#define LED_PIN 2
|
||||
#define CAM_FPS 5
|
||||
#define CAM_INTERVAL_MS (1000 / CAM_FPS)
|
||||
|
||||
// Magic chosen from bytes that commonly survive; 'FRM1' ascii.
|
||||
// Avoid high bytes 0xA0-AF / 0xD0-DF — observed missing from the CH9102 stream.
|
||||
static const uint32_t FRAME_MAGIC = 0x314D5246; // 'FRM1' little-endian on wire
|
||||
|
||||
void setup() {
|
||||
Serial.begin(460800);
|
||||
pinMode(LED_PIN, OUTPUT);
|
||||
digitalWrite(LED_PIN, HIGH);
|
||||
|
||||
delay(500);
|
||||
Serial.println("# capture-mode: 460800 baud, 96x96 gray @ 5fps");
|
||||
Serial.flush();
|
||||
|
||||
if (!camera_init()) {
|
||||
Serial.println("# FATAL: camera init failed");
|
||||
while (true) {
|
||||
digitalWrite(LED_PIN, !digitalRead(LED_PIN));
|
||||
delay(200);
|
||||
}
|
||||
}
|
||||
|
||||
digitalWrite(LED_PIN, LOW);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
static uint8_t frame[CV_PIXELS];
|
||||
static uint32_t frame_ix = 0;
|
||||
uint32_t t0 = millis();
|
||||
|
||||
if (camera_capture_96(frame)) {
|
||||
uint32_t ms = millis();
|
||||
Serial.write((uint8_t*)&FRAME_MAGIC, 4);
|
||||
Serial.write((uint8_t*)&frame_ix, 4);
|
||||
Serial.write((uint8_t*)&ms, 4);
|
||||
Serial.write(frame, CV_PIXELS);
|
||||
frame_ix++;
|
||||
digitalWrite(LED_PIN, frame_ix & 1);
|
||||
}
|
||||
|
||||
uint32_t elapsed = millis() - t0;
|
||||
if (elapsed < CAM_INTERVAL_MS) delay(CAM_INTERVAL_MS - elapsed);
|
||||
}
|
||||
@@ -7,160 +7,290 @@ static void fill_frame(uint8_t* f, uint8_t val) {
|
||||
memset(f, val, CV_PIXELS);
|
||||
}
|
||||
|
||||
// Draw a rectangular walker-blob spanning rows [y0, y1], columns [cx-hw, cx+hw].
|
||||
// Pixel value 200 over background 100 -> frame_diff threshold (30) is cleared.
|
||||
static void draw_walker(uint8_t* f, int y0, int y1, int cx, int hw) {
|
||||
fill_frame(f, 100);
|
||||
for (int y = y0; y <= y1; y++) {
|
||||
if (y < 0 || y >= CV_H) continue;
|
||||
for (int x = cx - hw; x <= cx + hw; x++) {
|
||||
if (x < 0 || x >= CV_W) continue;
|
||||
f[y * CV_W + x] = 200;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void prime_bg(CVState& state) {
|
||||
uint8_t bg[CV_PIXELS];
|
||||
fill_frame(bg, 100);
|
||||
cv_process(state, bg, 50);
|
||||
}
|
||||
|
||||
// Let the event state machine see QUIET_FRAMES+1 empty frames so any active
|
||||
// event finalizes before the next test assertion.
|
||||
static void quiesce(CVState& state) {
|
||||
uint8_t bg[CV_PIXELS]; fill_frame(bg, 100);
|
||||
for (int i = 0; i < CV_EVENT_QUIET_FRAMES + 1; i++) cv_process(state, bg, 50);
|
||||
}
|
||||
|
||||
void setUp(void) {}
|
||||
void tearDown(void) {}
|
||||
|
||||
void test_frame_diff_no_change_gives_no_fg() {
|
||||
CVState state;
|
||||
cv_init(state);
|
||||
|
||||
uint8_t frame[CV_PIXELS];
|
||||
fill_frame(frame, 128);
|
||||
|
||||
void test_no_change_no_event() {
|
||||
CVState state; cv_init(state);
|
||||
uint8_t frame[CV_PIXELS]; fill_frame(frame, 128);
|
||||
CVResult r1 = cv_process(state, frame, 50);
|
||||
TEST_ASSERT_EQUAL_INT(0, r1.entries_delta);
|
||||
|
||||
CVResult r2 = cv_process(state, frame, 50);
|
||||
TEST_ASSERT_EQUAL_INT(0, r2.entries_delta);
|
||||
TEST_ASSERT_EQUAL_INT(0, r2.exits_delta);
|
||||
}
|
||||
|
||||
void test_frame_diff_large_change_detected_no_crash() {
|
||||
CVState state;
|
||||
cv_init(state);
|
||||
|
||||
uint8_t bg[CV_PIXELS], fg_frame[CV_PIXELS];
|
||||
fill_frame(bg, 100);
|
||||
fill_frame(fg_frame, 200);
|
||||
|
||||
cv_process(state, bg, 50);
|
||||
CVResult r = cv_process(state, fg_frame, 50);
|
||||
|
||||
// Tracking not yet implemented — just verify no crash and result is zero
|
||||
TEST_ASSERT_EQUAL_INT(0, r.entries_delta);
|
||||
TEST_ASSERT_EQUAL_INT(0, r.exits_delta);
|
||||
}
|
||||
|
||||
void test_cv_init_clears_state() {
|
||||
CVState state;
|
||||
state.entries = 99; state.exits = 88;
|
||||
state.entries = 99; state.exits = 88; state.event_active = true;
|
||||
cv_init(state);
|
||||
TEST_ASSERT_EQUAL_INT(0, state.entries);
|
||||
TEST_ASSERT_EQUAL_INT(0, state.exits);
|
||||
TEST_ASSERT_FALSE(state.bg_valid);
|
||||
TEST_ASSERT_FALSE(state.event_active);
|
||||
}
|
||||
|
||||
void test_cv_reset_counts() {
|
||||
CVState state;
|
||||
cv_init(state);
|
||||
state.entries = 5;
|
||||
state.exits = 3;
|
||||
CVState state; cv_init(state);
|
||||
state.entries = 5; state.exits = 3;
|
||||
cv_reset_counts(state);
|
||||
TEST_ASSERT_EQUAL_INT(0, state.entries);
|
||||
TEST_ASSERT_EQUAL_INT(0, state.exits);
|
||||
}
|
||||
|
||||
void test_tracking_spawns_track_for_new_blob() {
|
||||
CVState state;
|
||||
cv_init(state);
|
||||
void test_walker_up_through_frame_is_entry() {
|
||||
// Simulate a walker traversing from bottom to top of frame.
|
||||
// Per-frame fg_count and centroid (11-wide column, height H -> n=11*H):
|
||||
// t0 y=60..95 n=396 c=77 <- event starts (n >= ENTER=300)
|
||||
// t1 y=30..95 n=726 c=62
|
||||
// t2 y=0..95 n=1056 c=47
|
||||
// t3 y=0..60 n=671 c=30
|
||||
// t4 y=0..25 n=286 c=12 (below EXIT=200, quiet=1)
|
||||
// t5 y=0..10 n=121 c=5 (below EXIT, quiet=2)
|
||||
// t6 empty quiet=3 -> finalize
|
||||
CVState state; cv_init(state);
|
||||
prime_bg(state);
|
||||
|
||||
uint8_t bg[CV_PIXELS];
|
||||
fill_frame(bg, 100);
|
||||
cv_process(state, bg, 50); // init background
|
||||
|
||||
// Frame with a bright 30x30 blob in top-left quadrant
|
||||
uint8_t blob_frame[CV_PIXELS];
|
||||
fill_frame(blob_frame, 100);
|
||||
for (int y = 5; y < 35; y++)
|
||||
for (int x = 5; x < 35; x++)
|
||||
blob_frame[y * CV_W + x] = 200;
|
||||
|
||||
cv_process(state, blob_frame, 50);
|
||||
|
||||
TEST_ASSERT_EQUAL_INT(1, (int)state.tracks.size());
|
||||
TEST_ASSERT_FLOAT_WITHIN(5.0f, 20.0f, state.tracks[0].x);
|
||||
TEST_ASSERT_FLOAT_WITHIN(5.0f, 20.0f, state.tracks[0].y);
|
||||
}
|
||||
|
||||
static void make_blob_frame(uint8_t* f, int cx, int cy) {
|
||||
fill_frame(f, 100);
|
||||
for (int y = cy - 12; y <= cy + 12; y++)
|
||||
for (int x = cx - 12; x <= cx + 12; x++)
|
||||
if (y >= 0 && y < CV_H && x >= 0 && x < CV_W)
|
||||
f[y * CV_W + x] = 200;
|
||||
}
|
||||
|
||||
void test_blob_crossing_line_top_to_bottom_is_entry() {
|
||||
CVState state;
|
||||
cv_init(state);
|
||||
|
||||
// Line at 50% = y=48; step ≤14px per frame to stay within CV_MAX_MOVE
|
||||
uint8_t bg[CV_PIXELS];
|
||||
fill_frame(bg, 100);
|
||||
cv_process(state, bg, 50); // init background
|
||||
|
||||
// Walk blob from y=20 toward line; crossing occurs at y=48 (above→below)
|
||||
// Stop at crossing frame and assert its result
|
||||
int setup[] = {20, 34};
|
||||
for (int i = 0; i < 2; i++) {
|
||||
uint8_t f[CV_PIXELS]; make_blob_frame(f, 48, setup[i]);
|
||||
int rows[][2] = {{60,95},{30,95},{0,95},{0,60},{0,25},{0,10}};
|
||||
for (int i = 0; i < 6; i++) {
|
||||
uint8_t f[CV_PIXELS]; draw_walker(f, rows[i][0], rows[i][1], 48, 5);
|
||||
cv_process(state, f, 50);
|
||||
}
|
||||
uint8_t fcross[CV_PIXELS]; make_blob_frame(fcross, 48, 48);
|
||||
CVResult r = cv_process(state, fcross, 50);
|
||||
quiesce(state);
|
||||
|
||||
TEST_ASSERT_EQUAL_INT(1, r.entries_delta);
|
||||
TEST_ASSERT_EQUAL_INT(0, r.exits_delta);
|
||||
TEST_ASSERT_EQUAL_INT(1, state.entries);
|
||||
TEST_ASSERT_EQUAL_INT(0, state.exits);
|
||||
}
|
||||
|
||||
void test_walker_down_through_frame_is_exit() {
|
||||
CVState state; cv_init(state);
|
||||
prime_bg(state);
|
||||
|
||||
int rows[][2] = {{0,35},{0,65},{0,95},{35,95},{70,95},{85,95}};
|
||||
for (int i = 0; i < 6; i++) {
|
||||
uint8_t f[CV_PIXELS]; draw_walker(f, rows[i][0], rows[i][1], 48, 5);
|
||||
cv_process(state, f, 50);
|
||||
}
|
||||
quiesce(state);
|
||||
|
||||
TEST_ASSERT_EQUAL_INT(0, state.entries);
|
||||
TEST_ASSERT_EQUAL_INT(1, state.exits);
|
||||
}
|
||||
|
||||
void test_approach_retreat_without_full_extent_does_not_fire() {
|
||||
// Walker approaches from bottom, reaches y=30, retreats, never reaches top.
|
||||
// Extent gate requires min_y_seen <= 10; this event tops out at y=30 so
|
||||
// extent never clears and no fire occurs regardless of trajectory score.
|
||||
CVState state; cv_init(state);
|
||||
prime_bg(state);
|
||||
|
||||
int rows[][2] = {{60,95},{40,95},{30,95},{40,95},{60,95},{80,95}};
|
||||
for (int i = 0; i < 6; i++) {
|
||||
uint8_t f[CV_PIXELS]; draw_walker(f, rows[i][0], rows[i][1], 48, 5);
|
||||
cv_process(state, f, 50);
|
||||
}
|
||||
quiesce(state);
|
||||
|
||||
TEST_ASSERT_EQUAL_INT(0, state.entries);
|
||||
TEST_ASSERT_EQUAL_INT(0, state.exits);
|
||||
}
|
||||
|
||||
void test_brief_burst_below_min_duration_does_not_fire() {
|
||||
// One frame of large fg, then gone. Event starts, immediately quiesces,
|
||||
// duration ends up below CV_EVENT_MIN_FRAMES.
|
||||
CVState state; cv_init(state);
|
||||
prime_bg(state);
|
||||
|
||||
uint8_t f[CV_PIXELS]; draw_walker(f, 0, 95, 48, 5);
|
||||
cv_process(state, f, 50);
|
||||
quiesce(state);
|
||||
|
||||
TEST_ASSERT_EQUAL_INT(0, state.entries);
|
||||
TEST_ASSERT_EQUAL_INT(0, state.exits);
|
||||
}
|
||||
|
||||
void test_stationary_large_blob_does_not_fire() {
|
||||
// Static large blob in frame for many frames, then removed. Centroid
|
||||
// never moves -> MIN_TRAJ gate blocks fire.
|
||||
CVState state; cv_init(state);
|
||||
prime_bg(state);
|
||||
|
||||
for (int i = 0; i < 10; i++) {
|
||||
uint8_t f[CV_PIXELS]; draw_walker(f, 0, 95, 48, 5);
|
||||
cv_process(state, f, 50);
|
||||
}
|
||||
quiesce(state);
|
||||
|
||||
TEST_ASSERT_EQUAL_INT(0, state.entries);
|
||||
TEST_ASSERT_EQUAL_INT(0, state.exits);
|
||||
}
|
||||
|
||||
// Wait out the refractory period with bg-only frames so the next walker
|
||||
// event is accepted.
|
||||
static void wait_refractory(CVState& state) {
|
||||
uint8_t bg[CV_PIXELS]; fill_frame(bg, 100);
|
||||
for (uint32_t i = 0; i < CV_EVENT_REFRACTORY_FRAMES + 2; i++) {
|
||||
cv_process(state, bg, 50);
|
||||
}
|
||||
}
|
||||
|
||||
void test_two_sequential_walkers_count_twice() {
|
||||
CVState state; cv_init(state);
|
||||
prime_bg(state);
|
||||
|
||||
int rows[][2] = {{60,95},{30,95},{0,95},{0,60},{0,25},{0,10}};
|
||||
for (int i = 0; i < 6; i++) {
|
||||
uint8_t f[CV_PIXELS]; draw_walker(f, rows[i][0], rows[i][1], 48, 5);
|
||||
cv_process(state, f, 50);
|
||||
}
|
||||
quiesce(state);
|
||||
wait_refractory(state);
|
||||
|
||||
for (int i = 0; i < 6; i++) {
|
||||
uint8_t f[CV_PIXELS]; draw_walker(f, rows[i][0], rows[i][1], 48, 5);
|
||||
cv_process(state, f, 50);
|
||||
}
|
||||
quiesce(state);
|
||||
|
||||
TEST_ASSERT_EQUAL_INT(2, state.entries);
|
||||
TEST_ASSERT_EQUAL_INT(0, state.exits);
|
||||
}
|
||||
|
||||
void test_full_reversal_counts_entry_then_exit() {
|
||||
CVState state; cv_init(state);
|
||||
prime_bg(state);
|
||||
|
||||
int up_rows[][2] = {{60,95},{30,95},{0,95},{0,60},{0,25},{0,10}};
|
||||
int down_rows[][2] = {{0,35},{0,65},{0,95},{35,95},{70,95},{85,95}};
|
||||
|
||||
for (int i = 0; i < 6; i++) {
|
||||
uint8_t f[CV_PIXELS]; draw_walker(f, up_rows[i][0], up_rows[i][1], 48, 5);
|
||||
cv_process(state, f, 50);
|
||||
}
|
||||
quiesce(state);
|
||||
wait_refractory(state);
|
||||
|
||||
for (int i = 0; i < 6; i++) {
|
||||
uint8_t f[CV_PIXELS]; draw_walker(f, down_rows[i][0], down_rows[i][1], 48, 5);
|
||||
cv_process(state, f, 50);
|
||||
}
|
||||
quiesce(state);
|
||||
|
||||
TEST_ASSERT_EQUAL_INT(1, state.entries);
|
||||
TEST_ASSERT_EQUAL_INT(1, state.exits);
|
||||
}
|
||||
|
||||
void test_refractory_suppresses_back_to_back_fire() {
|
||||
// After a fire, a second event attempted within CV_EVENT_REFRACTORY_FRAMES
|
||||
// is suppressed. Simulates walker lingering / ghost re-triggering.
|
||||
CVState state; cv_init(state);
|
||||
prime_bg(state);
|
||||
|
||||
int rows[][2] = {{60,95},{30,95},{0,95},{0,60},{0,25},{0,10}};
|
||||
for (int i = 0; i < 6; i++) {
|
||||
uint8_t f[CV_PIXELS]; draw_walker(f, rows[i][0], rows[i][1], 48, 5);
|
||||
cv_process(state, f, 50);
|
||||
}
|
||||
quiesce(state);
|
||||
TEST_ASSERT_EQUAL_INT(1, state.entries);
|
||||
|
||||
// Immediate second walker within refractory window — should NOT count.
|
||||
for (int i = 0; i < 6; i++) {
|
||||
uint8_t f[CV_PIXELS]; draw_walker(f, rows[i][0], rows[i][1], 48, 5);
|
||||
cv_process(state, f, 50);
|
||||
}
|
||||
quiesce(state);
|
||||
TEST_ASSERT_EQUAL_INT(1, state.entries);
|
||||
}
|
||||
|
||||
void test_blob_crossing_line_bottom_to_top_is_exit() {
|
||||
CVState state;
|
||||
cv_init(state);
|
||||
void test_event_counts_after_refractory_expires() {
|
||||
CVState state; cv_init(state);
|
||||
prime_bg(state);
|
||||
|
||||
uint8_t bg[CV_PIXELS]; fill_frame(bg, 100);
|
||||
cv_process(state, bg, 50);
|
||||
|
||||
// Walk blob from y=76 toward line; crossing occurs at y=34 (below→above)
|
||||
// Stop at crossing frame and assert its result
|
||||
int setup[] = {76, 62, 48};
|
||||
for (int i = 0; i < 3; i++) {
|
||||
uint8_t f[CV_PIXELS]; make_blob_frame(f, 48, setup[i]);
|
||||
int rows[][2] = {{60,95},{30,95},{0,95},{0,60},{0,25},{0,10}};
|
||||
for (int i = 0; i < 6; i++) {
|
||||
uint8_t f[CV_PIXELS]; draw_walker(f, rows[i][0], rows[i][1], 48, 5);
|
||||
cv_process(state, f, 50);
|
||||
}
|
||||
uint8_t fcross[CV_PIXELS]; make_blob_frame(fcross, 48, 34);
|
||||
CVResult r = cv_process(state, fcross, 50);
|
||||
quiesce(state);
|
||||
TEST_ASSERT_EQUAL_INT(1, state.entries);
|
||||
|
||||
TEST_ASSERT_EQUAL_INT(0, r.entries_delta);
|
||||
TEST_ASSERT_EQUAL_INT(1, r.exits_delta);
|
||||
// Wait out the refractory period.
|
||||
uint8_t bg[CV_PIXELS]; fill_frame(bg, 100);
|
||||
for (uint32_t i = 0; i < CV_EVENT_REFRACTORY_FRAMES + 2; i++) {
|
||||
cv_process(state, bg, 50);
|
||||
}
|
||||
|
||||
// Second walker — should now count.
|
||||
for (int i = 0; i < 6; i++) {
|
||||
uint8_t f[CV_PIXELS]; draw_walker(f, rows[i][0], rows[i][1], 48, 5);
|
||||
cv_process(state, f, 50);
|
||||
}
|
||||
quiesce(state);
|
||||
TEST_ASSERT_EQUAL_INT(2, state.entries);
|
||||
}
|
||||
|
||||
void test_no_crossing_same_side_no_count() {
|
||||
CVState state;
|
||||
cv_init(state);
|
||||
void test_noise_below_enter_thresh_does_not_start_event() {
|
||||
// Tiny 5x5 blob (25 px) never crosses ENTER=300, event never starts.
|
||||
CVState state; cv_init(state);
|
||||
prime_bg(state);
|
||||
|
||||
uint8_t bg[CV_PIXELS]; fill_frame(bg, 100);
|
||||
cv_process(state, bg, 50);
|
||||
auto small = [](uint8_t* f, int cy) {
|
||||
fill_frame(f, 100);
|
||||
for (int y = cy-2; y <= cy+2; y++)
|
||||
for (int x = 46; x <= 50; x++)
|
||||
if (y>=0 && y<CV_H && x>=0 && x<CV_W) f[y*CV_W+x] = 200;
|
||||
};
|
||||
for (int cy = 10; cy <= 90; cy += 8) {
|
||||
uint8_t f[CV_PIXELS]; small(f, cy);
|
||||
cv_process(state, f, 50);
|
||||
}
|
||||
quiesce(state);
|
||||
|
||||
uint8_t f1[CV_PIXELS]; make_blob_frame(f1, 48, 20); // above line
|
||||
cv_process(state, f1, 50);
|
||||
|
||||
uint8_t f2[CV_PIXELS]; make_blob_frame(f2, 48, 30); // still above line, moved closer
|
||||
CVResult r = cv_process(state, f2, 50);
|
||||
|
||||
TEST_ASSERT_EQUAL_INT(0, r.entries_delta);
|
||||
TEST_ASSERT_EQUAL_INT(0, r.exits_delta);
|
||||
TEST_ASSERT_EQUAL_INT(0, state.entries);
|
||||
TEST_ASSERT_EQUAL_INT(0, state.exits);
|
||||
}
|
||||
|
||||
int main() {
|
||||
UNITY_BEGIN();
|
||||
RUN_TEST(test_frame_diff_no_change_gives_no_fg);
|
||||
RUN_TEST(test_frame_diff_large_change_detected_no_crash);
|
||||
RUN_TEST(test_no_change_no_event);
|
||||
RUN_TEST(test_cv_init_clears_state);
|
||||
RUN_TEST(test_cv_reset_counts);
|
||||
RUN_TEST(test_tracking_spawns_track_for_new_blob);
|
||||
RUN_TEST(test_blob_crossing_line_top_to_bottom_is_entry);
|
||||
RUN_TEST(test_blob_crossing_line_bottom_to_top_is_exit);
|
||||
RUN_TEST(test_no_crossing_same_side_no_count);
|
||||
RUN_TEST(test_walker_up_through_frame_is_entry);
|
||||
RUN_TEST(test_walker_down_through_frame_is_exit);
|
||||
RUN_TEST(test_approach_retreat_without_full_extent_does_not_fire);
|
||||
RUN_TEST(test_brief_burst_below_min_duration_does_not_fire);
|
||||
RUN_TEST(test_stationary_large_blob_does_not_fire);
|
||||
RUN_TEST(test_two_sequential_walkers_count_twice);
|
||||
RUN_TEST(test_full_reversal_counts_entry_then_exit);
|
||||
RUN_TEST(test_refractory_suppresses_back_to_back_fire);
|
||||
RUN_TEST(test_event_counts_after_refractory_expires);
|
||||
RUN_TEST(test_noise_below_enter_thresh_does_not_start_event);
|
||||
return UNITY_END();
|
||||
}
|
||||
|
||||
105
tools/capture_frames.py
Normal file
105
tools/capture_frames.py
Normal file
@@ -0,0 +1,105 @@
|
||||
#!/usr/bin/env python3
|
||||
# tools/capture_frames.py
|
||||
#
|
||||
# Read framed 96x96 grayscale frames from the capture-mode firmware over serial
|
||||
# and write them to a .bin file for offline replay.
|
||||
#
|
||||
# Wire format per frame (little-endian):
|
||||
# magic u32 0xDC0FC0DE
|
||||
# frame_ix u32
|
||||
# millis u32
|
||||
# pixels 9216 bytes
|
||||
#
|
||||
# Output file is the raw concatenation of frames (same layout as the wire),
|
||||
# so replay_frames.py can stream it with identical parsing.
|
||||
#
|
||||
# Usage: python tools/capture_frames.py --port /dev/ttyUSB0 --out walk.bin --duration 60
|
||||
|
||||
import argparse
|
||||
import serial
|
||||
import struct
|
||||
import sys
|
||||
import time
|
||||
|
||||
MAGIC = 0x314D5246 # 'FRM1' — ascii bytes that survive the CH9102 stream
|
||||
FRAME_PIXELS = 96 * 96
|
||||
HEADER_LEN = 12
|
||||
FRAME_LEN = HEADER_LEN + FRAME_PIXELS
|
||||
|
||||
|
||||
def read_exact(ser, n):
|
||||
buf = bytearray()
|
||||
while len(buf) < n:
|
||||
chunk = ser.read(n - len(buf))
|
||||
if not chunk:
|
||||
return None
|
||||
buf.extend(chunk)
|
||||
return bytes(buf)
|
||||
|
||||
|
||||
def find_magic(ser):
|
||||
"""Scan serial byte-by-byte until we see the 4-byte MAGIC."""
|
||||
window = bytearray()
|
||||
magic_bytes = struct.pack('<I', MAGIC)
|
||||
while True:
|
||||
b = ser.read(1)
|
||||
if not b:
|
||||
return False
|
||||
window.extend(b)
|
||||
if len(window) > 4:
|
||||
del window[0]
|
||||
if bytes(window) == magic_bytes:
|
||||
return True
|
||||
|
||||
|
||||
def main():
|
||||
ap = argparse.ArgumentParser()
|
||||
ap.add_argument('--port', required=True)
|
||||
ap.add_argument('--baud', type=int, default=460800)
|
||||
ap.add_argument('--out', required=True)
|
||||
ap.add_argument('--duration', type=float, default=60.0,
|
||||
help='Seconds to capture (default 60)')
|
||||
args = ap.parse_args()
|
||||
|
||||
ser = serial.Serial(args.port, args.baud, timeout=1.0)
|
||||
print(f'# listening on {args.port} @ {args.baud} for {args.duration}s...',
|
||||
file=sys.stderr)
|
||||
|
||||
# Drain boot banner lines.
|
||||
deadline_banner = time.time() + 2.0
|
||||
while time.time() < deadline_banner:
|
||||
line = ser.readline()
|
||||
if line.startswith(b'#'):
|
||||
print(line.decode(errors='replace').rstrip(), file=sys.stderr)
|
||||
if b'capture-mode' in line:
|
||||
break
|
||||
|
||||
deadline = time.time() + args.duration
|
||||
frames = 0
|
||||
last_ix = None
|
||||
dropped = 0
|
||||
|
||||
with open(args.out, 'wb') as f:
|
||||
while time.time() < deadline:
|
||||
if not find_magic(ser):
|
||||
continue
|
||||
body = read_exact(ser, 8 + FRAME_PIXELS)
|
||||
if body is None:
|
||||
break
|
||||
frame_ix, ms = struct.unpack('<II', body[:8])
|
||||
if last_ix is not None and frame_ix != last_ix + 1:
|
||||
dropped += frame_ix - last_ix - 1
|
||||
last_ix = frame_ix
|
||||
f.write(struct.pack('<I', MAGIC))
|
||||
f.write(body)
|
||||
frames += 1
|
||||
if frames % 25 == 0:
|
||||
print(f'# {frames} frames, last ix={frame_ix} ms={ms} '
|
||||
f'dropped={dropped}', file=sys.stderr)
|
||||
|
||||
print(f'# done: {frames} frames written to {args.out} '
|
||||
f'({dropped} dropped)', file=sys.stderr)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
211
tools/replay_frames.py
Normal file
211
tools/replay_frames.py
Normal file
@@ -0,0 +1,211 @@
|
||||
#!/usr/bin/env python3
|
||||
# tools/replay_frames.py
|
||||
#
|
||||
# Offline Python port of the event-based CV detector (firmware/lib/cv/cv.cpp).
|
||||
# Reads a .bin file produced by capture_frames.py and prints events.
|
||||
#
|
||||
# Purpose: iterate algorithm changes in seconds instead of minutes. All
|
||||
# constants match cv.h so baseline behavior matches firmware.
|
||||
#
|
||||
# Usage:
|
||||
# python tools/replay_frames.py walk.bin
|
||||
# python tools/replay_frames.py walk.bin --enter 250 --exit 150 --max 25
|
||||
#
|
||||
# Output: one line per frame with fg diagnostics, plus [ENTRY]/[EXIT] lines
|
||||
# when the detector fires.
|
||||
|
||||
import argparse
|
||||
import struct
|
||||
import sys
|
||||
|
||||
import numpy as np
|
||||
|
||||
MAGIC = 0x314D5246 # 'FRM1'
|
||||
W = H = 96
|
||||
PIXELS = W * H
|
||||
HEADER = 12
|
||||
FRAME_LEN = HEADER + PIXELS
|
||||
|
||||
|
||||
class Detector:
|
||||
"""Mirror of firmware CV state machine. Single walker events, centroid
|
||||
trajectory direction. Only per-frame fg_count + min/max y + centroid y
|
||||
feed the decision — per-blob tracks are diagnostic in firmware, dropped
|
||||
here."""
|
||||
|
||||
def __init__(self, args):
|
||||
self.a = args
|
||||
self.bg = None
|
||||
self.ev_active = False
|
||||
self.ev_frames = 0
|
||||
self.ev_first_c = -1.0
|
||||
self.ev_last_c = -1.0
|
||||
self.ev_min_c = float(H)
|
||||
self.ev_max_c = -1.0
|
||||
self.ev_min_y = H
|
||||
self.ev_max_y = -1
|
||||
self.ev_quiet = 0
|
||||
self.last_fire = 0
|
||||
self.frame_ix = 0
|
||||
self.entries = 0
|
||||
self.exits = 0
|
||||
|
||||
def _reset_event(self):
|
||||
self.ev_active = False
|
||||
self.ev_frames = 0
|
||||
self.ev_first_c = self.ev_last_c = -1.0
|
||||
self.ev_min_c = float(H)
|
||||
self.ev_max_c = -1.0
|
||||
self.ev_min_y = H
|
||||
self.ev_max_y = -1
|
||||
self.ev_quiet = 0
|
||||
|
||||
def _finalize(self):
|
||||
a = self.a
|
||||
if self.ev_frames < a.min_frames: return None
|
||||
if self.ev_min_y > a.extent_top: return None
|
||||
if self.ev_max_y < a.extent_bot: return None
|
||||
up = self.ev_first_c - self.ev_min_c
|
||||
down = self.ev_max_c - self.ev_first_c
|
||||
winning = max(up, down)
|
||||
if winning < a.min_traj: return None
|
||||
is_entry = up >= down
|
||||
self.last_fire = self.frame_ix
|
||||
info = dict(
|
||||
kind='ENTRY' if is_entry else 'EXIT',
|
||||
first=self.ev_first_c, min=self.ev_min_c,
|
||||
max=self.ev_max_c, last=self.ev_last_c,
|
||||
dur=self.ev_frames,
|
||||
)
|
||||
if is_entry: self.entries += 1
|
||||
else: self.exits += 1
|
||||
return info
|
||||
|
||||
def step(self, frame):
|
||||
"""frame: uint8 array of shape (H, W). Returns list of fire dicts."""
|
||||
self.frame_ix += 1
|
||||
fires = []
|
||||
|
||||
if self.bg is None:
|
||||
self.bg = frame.astype(np.int16)
|
||||
return fires
|
||||
|
||||
bg = self.bg.astype(np.int16)
|
||||
diff = np.abs(frame.astype(np.int16) - bg)
|
||||
fg = (diff > self.a.diff_thresh).astype(np.uint8)
|
||||
|
||||
# Running-avg bg blend, frozen during active event.
|
||||
if not self.ev_active:
|
||||
self.bg = ((self.bg * 31 + frame.astype(np.int16)) >> 5)
|
||||
|
||||
fg_count = int(fg.sum())
|
||||
if fg_count > 0:
|
||||
row_counts = fg.sum(axis=1)
|
||||
ys = np.where(row_counts > 0)[0]
|
||||
min_y = int(ys.min())
|
||||
max_y = int(ys.max())
|
||||
centroid_y = float((row_counts * np.arange(H)).sum() / fg_count)
|
||||
else:
|
||||
min_y, max_y, centroid_y = -1, -1, -1.0
|
||||
|
||||
# Self-heal on catastrophic bg mismatch.
|
||||
if fg_count > PIXELS // 2:
|
||||
self.bg = frame.astype(np.int16)
|
||||
if self.ev_active: self._reset_event()
|
||||
return fires
|
||||
|
||||
a = self.a
|
||||
in_refractory = (self.last_fire != 0 and
|
||||
(self.frame_ix - self.last_fire) < a.refractory)
|
||||
|
||||
if not self.ev_active:
|
||||
if not in_refractory and fg_count >= a.enter_thresh:
|
||||
self.ev_active = True
|
||||
self.ev_frames = 1
|
||||
self.ev_first_c = centroid_y
|
||||
self.ev_last_c = centroid_y
|
||||
self.ev_min_c = centroid_y
|
||||
self.ev_max_c = centroid_y
|
||||
self.ev_min_y = min_y
|
||||
self.ev_max_y = max_y
|
||||
self.ev_quiet = 0
|
||||
else:
|
||||
self.ev_frames += 1
|
||||
if fg_count > 0:
|
||||
self.ev_last_c = centroid_y
|
||||
if centroid_y < self.ev_min_c: self.ev_min_c = centroid_y
|
||||
if centroid_y > self.ev_max_c: self.ev_max_c = centroid_y
|
||||
if min_y < self.ev_min_y: self.ev_min_y = min_y
|
||||
if max_y > self.ev_max_y: self.ev_max_y = max_y
|
||||
|
||||
ended = False
|
||||
if fg_count < a.exit_thresh:
|
||||
self.ev_quiet += 1
|
||||
if self.ev_quiet >= a.quiet_frames:
|
||||
ended = True
|
||||
else:
|
||||
self.ev_quiet = 0
|
||||
if self.ev_frames > a.max_frames:
|
||||
ended = True
|
||||
|
||||
if ended:
|
||||
fire = self._finalize()
|
||||
if fire: fires.append(fire)
|
||||
self._reset_event()
|
||||
self.bg = frame.astype(np.int16)
|
||||
|
||||
return fires, fg_count, min_y, max_y, centroid_y
|
||||
|
||||
|
||||
def iter_frames(path):
|
||||
with open(path, 'rb') as f:
|
||||
data = f.read()
|
||||
n = len(data) // FRAME_LEN
|
||||
for i in range(n):
|
||||
off = i * FRAME_LEN
|
||||
magic, ix, ms = struct.unpack('<III', data[off:off + HEADER])
|
||||
if magic != MAGIC:
|
||||
raise RuntimeError(f'bad magic at frame {i}: 0x{magic:08x}')
|
||||
frame = np.frombuffer(data, dtype=np.uint8,
|
||||
count=PIXELS, offset=off + HEADER).reshape(H, W)
|
||||
yield ix, ms, frame
|
||||
|
||||
|
||||
def main():
|
||||
ap = argparse.ArgumentParser()
|
||||
ap.add_argument('path')
|
||||
ap.add_argument('--diff-thresh', dest='diff_thresh', type=int, default=30)
|
||||
ap.add_argument('--enter', dest='enter_thresh', type=int, default=300)
|
||||
ap.add_argument('--exit', dest='exit_thresh', type=int, default=200)
|
||||
ap.add_argument('--quiet', dest='quiet_frames', type=int, default=3)
|
||||
ap.add_argument('--min', dest='min_frames', type=int, default=5)
|
||||
ap.add_argument('--max', dest='max_frames', type=int, default=25)
|
||||
ap.add_argument('--extent-top', dest='extent_top', type=int, default=10)
|
||||
ap.add_argument('--extent-bot', dest='extent_bot', type=int, default=85)
|
||||
ap.add_argument('--min-traj', dest='min_traj', type=float, default=15.0)
|
||||
ap.add_argument('--refractory', dest='refractory', type=int, default=15)
|
||||
ap.add_argument('--quiet-log', action='store_true',
|
||||
help='Suppress per-frame fg lines')
|
||||
args = ap.parse_args()
|
||||
|
||||
det = Detector(args)
|
||||
total = 0
|
||||
for ix, ms, frame in iter_frames(args.path):
|
||||
total += 1
|
||||
out = det.step(frame)
|
||||
if out == []:
|
||||
if not args.quiet_log:
|
||||
print(f'[{ix:4d}] bg init')
|
||||
continue
|
||||
fires, fg, miny, maxy, cy = out
|
||||
if not args.quiet_log and fg > 0:
|
||||
print(f'[{ix:4d}] n={fg:4d} y={miny:2d}..{maxy:2d} c={cy:5.1f}')
|
||||
for fire in fires:
|
||||
print(f' >>> {fire["kind"]} first={fire["first"]:.1f} '
|
||||
f'min={fire["min"]:.1f} max={fire["max"]:.1f} '
|
||||
f'last={fire["last"]:.1f} dur={fire["dur"]}')
|
||||
print(f'\n# {total} frames entries={det.entries} exits={det.exits}')
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
186
tools/replay_logs.py
Normal file
186
tools/replay_logs.py
Normal file
@@ -0,0 +1,186 @@
|
||||
#!/usr/bin/env python3
|
||||
# tools/replay_logs.py
|
||||
#
|
||||
# Replay the event state machine against text serial logs captured from the
|
||||
# production firmware. Input lines of the form:
|
||||
# [F] n=<fg_count> y=<min_y>..<max_y> c=<centroid_y>
|
||||
#
|
||||
# Those four values are exactly what the firmware's event state machine
|
||||
# consumes — so we can iterate event-level params (thresholds, max_frames,
|
||||
# extent gates, trajectory cutoffs, refractory) offline without needing raw
|
||||
# frames or the device.
|
||||
#
|
||||
# Usage:
|
||||
# python tools/replay_logs.py walk.log
|
||||
# python tools/replay_logs.py walk.log --enter 250 --exit 100 --max 30 --min-traj 10
|
||||
# cat walk.log | python tools/replay_logs.py - --ground-truth 12
|
||||
|
||||
import argparse
|
||||
import re
|
||||
import sys
|
||||
|
||||
|
||||
LINE_RE = re.compile(
|
||||
r'\[F\]\s+n=(?P<n>\d+)\s+y=(?P<miny>-?\d+)\.\.(?P<maxy>-?\d+)\s+c=(?P<c>-?\d+\.\d+)'
|
||||
)
|
||||
|
||||
|
||||
def parse_frames(text):
|
||||
"""Yield (fg_count, min_y, max_y, centroid_y) per [F] line, in order."""
|
||||
for line in text.splitlines():
|
||||
m = LINE_RE.search(line)
|
||||
if not m:
|
||||
continue
|
||||
yield int(m['n']), int(m['miny']), int(m['maxy']), float(m['c'])
|
||||
|
||||
|
||||
class Detector:
|
||||
"""Mirror of firmware event state machine. Only uses per-frame diagnostic
|
||||
values — the same inputs the firmware feeds it."""
|
||||
|
||||
def __init__(self, a):
|
||||
self.a = a
|
||||
self.ev = False
|
||||
self.ev_n = 0
|
||||
self.ev_first = self.ev_last = -1.0
|
||||
self.ev_min = 1e9
|
||||
self.ev_max = -1.0
|
||||
self.ev_miny = 1e9
|
||||
self.ev_maxy = -1
|
||||
self.ev_quiet = 0
|
||||
self.last_fire = -10**9
|
||||
self.ix = 0
|
||||
self.entries = 0
|
||||
self.exits = 0
|
||||
self.fires = []
|
||||
|
||||
def _reset(self):
|
||||
self.ev = False
|
||||
self.ev_n = 0
|
||||
self.ev_first = self.ev_last = -1.0
|
||||
self.ev_min = 1e9; self.ev_max = -1.0
|
||||
self.ev_miny = 1e9; self.ev_maxy = -1
|
||||
self.ev_quiet = 0
|
||||
|
||||
def _finalize(self):
|
||||
a = self.a
|
||||
if self.ev_n < a.min_frames:
|
||||
return ('reject_short', None)
|
||||
if self.ev_miny > a.extent_top:
|
||||
return ('reject_extent_top', None)
|
||||
if self.ev_maxy < a.extent_bot:
|
||||
return ('reject_extent_bot', None)
|
||||
up = self.ev_first - self.ev_min
|
||||
down = self.ev_max - self.ev_first
|
||||
winning = max(up, down)
|
||||
if winning < a.min_traj:
|
||||
return ('reject_traj', None)
|
||||
timed_out = self.ev_n > a.max_frames
|
||||
if timed_out:
|
||||
is_entry = self.ev_last < self.ev_first
|
||||
else:
|
||||
is_entry = up >= down
|
||||
kind = 'ENTRY' if is_entry else 'EXIT'
|
||||
self.last_fire = self.ix
|
||||
info = dict(kind=kind, first=self.ev_first, min=self.ev_min,
|
||||
max=self.ev_max, last=self.ev_last, dur=self.ev_n,
|
||||
up=up, down=down, ix=self.ix)
|
||||
if is_entry: self.entries += 1
|
||||
else: self.exits += 1
|
||||
self.fires.append(info)
|
||||
return ('fire', info)
|
||||
|
||||
def step(self, n, miny, maxy, c):
|
||||
self.ix += 1
|
||||
a = self.a
|
||||
refractory = (self.ix - self.last_fire) < a.refractory
|
||||
|
||||
if not self.ev:
|
||||
if not refractory and n >= a.enter_thresh:
|
||||
self.ev = True
|
||||
self.ev_n = 1
|
||||
self.ev_first = self.ev_last = c
|
||||
self.ev_min = c; self.ev_max = c
|
||||
self.ev_miny = miny; self.ev_maxy = maxy
|
||||
self.ev_quiet = 0
|
||||
return None
|
||||
|
||||
self.ev_n += 1
|
||||
if n > 0:
|
||||
self.ev_last = c
|
||||
if c < self.ev_min: self.ev_min = c
|
||||
if c > self.ev_max: self.ev_max = c
|
||||
if miny < self.ev_miny: self.ev_miny = miny
|
||||
if maxy > self.ev_maxy: self.ev_maxy = maxy
|
||||
|
||||
ended = False
|
||||
if n < a.exit_thresh:
|
||||
self.ev_quiet += 1
|
||||
if self.ev_quiet >= a.quiet_frames:
|
||||
ended = True
|
||||
reason = 'quiet'
|
||||
else:
|
||||
self.ev_quiet = 0
|
||||
if self.ev_n > a.max_frames:
|
||||
ended = True
|
||||
reason = 'timeout'
|
||||
|
||||
if ended:
|
||||
result = self._finalize()
|
||||
self._reset()
|
||||
return (reason, result)
|
||||
return None
|
||||
|
||||
|
||||
def main():
|
||||
ap = argparse.ArgumentParser()
|
||||
ap.add_argument('path', help='log file, or - for stdin')
|
||||
ap.add_argument('--enter', dest='enter_thresh', type=int, default=300)
|
||||
ap.add_argument('--exit', dest='exit_thresh', type=int, default=200)
|
||||
ap.add_argument('--quiet', dest='quiet_frames', type=int, default=3)
|
||||
ap.add_argument('--min', dest='min_frames', type=int, default=5)
|
||||
ap.add_argument('--max', dest='max_frames', type=int, default=25)
|
||||
ap.add_argument('--extent-top', dest='extent_top', type=int, default=10)
|
||||
ap.add_argument('--extent-bot', dest='extent_bot', type=int, default=85)
|
||||
ap.add_argument('--min-traj', dest='min_traj', type=float, default=15.0)
|
||||
ap.add_argument('--refractory', dest='refractory', type=int, default=15)
|
||||
ap.add_argument('--ground-truth', type=int, default=0,
|
||||
help='Total expected walks for accuracy calc')
|
||||
ap.add_argument('-v', '--verbose', action='store_true',
|
||||
help='Print every event end, including rejections')
|
||||
args = ap.parse_args()
|
||||
|
||||
text = sys.stdin.read() if args.path == '-' else open(args.path).read()
|
||||
|
||||
det = Detector(args)
|
||||
rejects = {}
|
||||
for n, miny, maxy, c in parse_frames(text):
|
||||
out = det.step(n, miny, maxy, c)
|
||||
if out is None:
|
||||
continue
|
||||
reason, result = out
|
||||
if result is None:
|
||||
continue
|
||||
kind, info = result
|
||||
if kind == 'fire':
|
||||
print(f' {info["kind"]:5} first={info["first"]:5.1f} '
|
||||
f'min={info["min"]:5.1f} max={info["max"]:5.1f} '
|
||||
f'last={info["last"]:5.1f} dur={info["dur"]:2d} '
|
||||
f'exit={reason}')
|
||||
else:
|
||||
rejects[kind] = rejects.get(kind, 0) + 1
|
||||
if args.verbose:
|
||||
print(f' [drop {kind}]')
|
||||
|
||||
total = det.entries + det.exits
|
||||
print(f'\n=== entries={det.entries} exits={det.exits} total={total} ===')
|
||||
print(f'rejected events: {rejects}')
|
||||
if args.ground_truth:
|
||||
gt = args.ground_truth
|
||||
acc = min(total, gt) / gt * 100
|
||||
over = max(0, total - gt)
|
||||
print(f'accuracy vs gt={gt}: {acc:.0f}% (over={over})')
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
56
tools/serial_monitor.py
Executable file
56
tools/serial_monitor.py
Executable file
@@ -0,0 +1,56 @@
|
||||
#!/usr/bin/env python3
|
||||
# Serial monitor for ESP32. Optionally pulses RTS/DTR to reset the device
|
||||
# so we capture boot output. Prefixes each line with elapsed seconds.
|
||||
import serial, sys, time, argparse
|
||||
|
||||
def main():
|
||||
ap = argparse.ArgumentParser()
|
||||
ap.add_argument("--port", default="/dev/ttyUSB0")
|
||||
ap.add_argument("--baud", type=int, default=115200)
|
||||
ap.add_argument("--seconds", type=int, default=20)
|
||||
ap.add_argument("--reset", action="store_true",
|
||||
help="Pulse RTS/DTR to reset the ESP32 before reading")
|
||||
ap.add_argument("--timestamp", action="store_true",
|
||||
help="Prefix each line with elapsed seconds since boot")
|
||||
args = ap.parse_args()
|
||||
|
||||
try:
|
||||
s = serial.Serial(args.port, args.baud, timeout=0.2,
|
||||
rtscts=False, dsrdtr=False)
|
||||
except Exception as e:
|
||||
print(f"[open-fail] {e}", flush=True)
|
||||
sys.exit(2)
|
||||
|
||||
if args.reset:
|
||||
s.setDTR(False)
|
||||
s.setRTS(True)
|
||||
time.sleep(0.1)
|
||||
s.setRTS(False)
|
||||
s.reset_input_buffer()
|
||||
|
||||
t0 = time.time()
|
||||
end = t0 + args.seconds
|
||||
buf = b""
|
||||
while time.time() < end:
|
||||
chunk = s.read(512)
|
||||
if chunk:
|
||||
buf += chunk
|
||||
while b"\n" in buf:
|
||||
line, buf = buf.split(b"\n", 1)
|
||||
text = line.decode("utf-8", errors="replace").rstrip("\r")
|
||||
if args.timestamp:
|
||||
sys.stdout.write(f"[{time.time()-t0:5.1f}s] {text}\n")
|
||||
else:
|
||||
sys.stdout.write(text + "\n")
|
||||
sys.stdout.flush()
|
||||
if buf:
|
||||
text = buf.decode("utf-8", errors="replace")
|
||||
if args.timestamp:
|
||||
sys.stdout.write(f"[{time.time()-t0:5.1f}s] {text}\n")
|
||||
else:
|
||||
sys.stdout.write(text)
|
||||
sys.stdout.flush()
|
||||
s.close()
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Reference in New Issue
Block a user