From 21e83be26e4faf79f67d1dff4c9df61e89ad51f2 Mon Sep 17 00:00:00 2001 From: root Date: Sun, 2 Aug 2026 22:23:05 +0200 Subject: [PATCH] release: consolidate web simulation and real CV prototype Add cleaned RealSense/OpenCV CV under cv/ with secret-free example config, MQTT disabled by default, and canonical README coverage for web + CV relationship without claiming production integration. Co-authored-by: Cursor --- .gitignore | 7 +- README.md | 341 ++++++++-------- cv/.gitignore | 11 + cv/Dockerfile | 23 ++ cv/README.md | 176 +++++++++ cv/align_color.py | 139 +++++++ cv/calibrate.py | 188 +++++++++ cv/camera.py | 373 ++++++++++++++++++ cv/classify.py | 147 +++++++ cv/collect_log.py | 151 +++++++ cv/config.example.yaml | 90 +++++ cv/demo.py | 873 +++++++++++++++++++++++++++++++++++++++++ cv/demo.sh | 19 + cv/demo_hud.py | 302 ++++++++++++++ cv/docker-compose.yml | 25 ++ cv/journal.py | 47 +++ cv/main.py | 330 ++++++++++++++++ cv/measure.py | 719 +++++++++++++++++++++++++++++++++ cv/mqtt_bridge.py | 172 ++++++++ cv/requirements.txt | 6 + cv/run.sh | 17 + cv/stabilize.py | 306 +++++++++++++++ cv/test_classify.py | 86 ++++ cv/test_geometry.py | 384 ++++++++++++++++++ cv/tracker.py | 279 +++++++++++++ docs/ENGINEERING.md | 17 + 26 files changed, 5050 insertions(+), 178 deletions(-) create mode 100644 cv/.gitignore create mode 100644 cv/Dockerfile create mode 100644 cv/README.md create mode 100644 cv/align_color.py create mode 100644 cv/calibrate.py create mode 100644 cv/camera.py create mode 100644 cv/classify.py create mode 100644 cv/collect_log.py create mode 100644 cv/config.example.yaml create mode 100755 cv/demo.py create mode 100755 cv/demo.sh create mode 100644 cv/demo_hud.py create mode 100644 cv/docker-compose.yml create mode 100644 cv/journal.py create mode 100755 cv/main.py create mode 100644 cv/measure.py create mode 100644 cv/mqtt_bridge.py create mode 100644 cv/requirements.txt create mode 100755 cv/run.sh create mode 100644 cv/stabilize.py create mode 100755 cv/test_classify.py create mode 100644 cv/test_geometry.py create mode 100644 cv/tracker.py diff --git a/.gitignore b/.gitignore index 96623e5..b5b36aa 100644 --- a/.gitignore +++ b/.gitignore @@ -14,4 +14,9 @@ playwright-report/ blob-report/ .demo-preview.pid .demo-preview.log -__pycache__ +__pycache__/ +*.pyc +cv/.venv/ +cv/debug_frames/ +cv/logs/ +cv/config.yaml diff --git a/README.md b/README.md index 5df513a..26e91b2 100644 --- a/README.md +++ b/README.md @@ -1,111 +1,171 @@ # OZON Sorter Digital Twin -Web digital twin of an Ozon Tech Track 3 conveyor sorting line: continuous product playback, camera/classification stage, B/C/D routing, and author-CAD diverter motion. +Complete Track 3 Ozon Tech solution in one repository: a web digital twin of the sorting line, plus a real RealSense/OpenCV CV prototype. -**Production domain:** https://arhipovdan.ru +**Production:** https://arhipovdan.ru -## Current solution +## 1. Overview -Final web simulation lives on **`main`** (same commit as `dan_branch`): +This repository delivers a continuous web simulation of an Ozon conveyor sorter: CAD conveyor, product models, camera/measurement stage, B/C/D classification, diverters, and Rapier physics. The public demo runs on https://arhipovdan.ru with routes `/` (simulation) and `/documentation` (engineering status). -- continuous 3D conveyor digital twin (`/`); -- engineering documentation (`/documentation`); -- author CAD (`3d_models/conveer.FCStd`) + runtime GLB (`public/models/sorter/conveyor-clean.glb`); -- product STL assets (`public/models/*.stl`); -- camera / classification stage (digital twin rules, not live RealSense); -- B / C / D routing with CAD left/right diverters (−45° / +45°); -- Rapier-backed product physics (contact routing **not** fully validated). +Separately, `cv/` contains a **working hardware prototype** that reads Intel RealSense D415 depth, measures parcels with OpenCV, classifies B/C/D, and can publish results over MQTT. It is **not** wired into the live website. -Accepted release head: **`bb76963`** (content tree identical to stable baseline `13ce16b`). Later experimental junction/discharge commits were reverted by owner request and are **not** part of the submitted product. +Both paths share the same Track 3 classification domain (exclusive 10×10×10 … 450×320×320 mm, roundness K > 0.8). The web twin uses a digital sensor simulation; the CV folder uses real depth frames. -### Main functions +`main` is the canonical complete solution. Developers do not need other branches to run the web app or inspect/run the CV prototype. -1. Continuous SKU playback on a three-module CAD conveyor (clean → camera → sorter). -2. Measurement / classification stage with Track 3 B/C/D rules. -3. Diverter state machine and route visualization. -4. Desktop WebGL twin; mobile uses a lightweight presentation policy when WebGL budget requires it. -5. Canonical engineering status on `/documentation`. +Large submission artifacts (presentation, video, optional CAD/model mirrors) belong in team cloud storage; runtime assets required by deploy stay in Git. -## Technology stack +## 2. Submission components -| Layer | Packages | +| Component | Location | Notes | +|---|---|---| +| Web digital twin | `src/`, `public/` | Production-integrated | +| Real CV prototype | `cv/` | WORKING_PROTOTYPE, not live-integrated | +| Author CAD | `3d_models/conveer.FCStd` | FreeCAD source | +| Official materials | `input_info/`, `official_sources/` | PDFs/ZIPs cited by docs/code | +| Production domain | https://arhipovdan.ru | Docker + nginx | +| Presentation / video | cloud (owner) | Links TBD — see §17 | + +## 3. Production demo + +- **URL:** https://arhipovdan.ru +- **`/`** — continuous digital-twin simulation +- **`/documentation`** — canonical engineering status +- Unknown routes redirect to `/` + +Device behavior on current baseline: + +- **Desktop:** interactive WebGL 3D +- **Mobile:** lightweight **2D** fallback (not full WebGL 3D) + +Build identity: `/version.json`. + +## 4. Web capabilities + +- Three-module CAD conveyor (clean → camera → sorter) +- Camera / measurement simulation and Track 3 classifier +- B / C / D routing with CAD diverters (−45° / +45°) +- Rapier product physics (contact sorting **not** fully validated) +- Continuous playback HUD + documentation page + +## 5. Real CV prototype + +| Field | Value | |---|---| -| UI | React, react-router-dom | -| 3D | three, @react-three/fiber, @react-three/drei, @react-three/postprocessing | -| Physics | @react-three/rapier, @dimforge/rapier3d-compat | -| Build | Vite, TypeScript, @vitejs/plugin-react | -| Tests | Vitest, Playwright | +| Path | `cv/` | +| Origin | `drho1y-mvp_1` / `vision_classifier/` | +| Technology | RealSense D415 + OpenCV (depth segmentation + metrics) | +| Status | WORKING_PROTOTYPE | +| Integration | **Not** connected to production web runtime | -Exact versions: `package.json` / `package-lock.json`. +See **[cv/README.md](cv/README.md)** for install, demo, live camera, and MQTT. -## Dependencies / Node - -- Node.js **20+** -- npm (locked via `package-lock.json`) - -### Exact commands - -```bash -npm ci # install -npm run dev # development → http://127.0.0.1:3100 -npm test -- --run # unit tests -npm run build # production build → dist/ -npm run preview # serve build → http://127.0.0.1:3100 -npm run test:e2e # Playwright (set PLAYWRIGHT_BASE_URL if needed) -``` - -## Active routes - -| Route | Purpose | -|---|---| -| `/` | Continuous digital-twin simulation | -| `/documentation` | Canonical product / engineering status | -| `*` | Redirects to `/` | - -Removed routes (`/details`, `/device-test`) are not part of the product. - -## Project structure +## 6. Architecture ``` -src/ React app, domain logic, 3D twin, pages -public/ Runtime static assets (models, draco) - models/ Product STL + sorter/conveyor-clean.glb -3d_models/ Author CAD (conveer.FCStd) -docs/ Engineering notes (ENGINEERING.md) -e2e/ Playwright smoke / routes -input_info/ Official Ozon input packs (PDFs/ZIPs) -official_sources/ Classifier bounds PDF cited by code -Dockerfile Multi-stage Vite build + nginx +Real device path: + RealSense D415 → depth preprocess → segmentation → measurement + → B/C/D → optional MQTT / hardware + +Web path: + Digital product → simulated sensor → classifier + → physical digital twin → B/C/D receiver visualization +``` + +Shared: B/C/D semantics and official dimension/roundness rules. +Not shared today: live camera frames into the website. + +## 7. Repository structure + +``` +.github/ CI (build, unit, e2e) +3d_models/ Author CAD (conveer.FCStd) +cv/ Real CV prototype (Python) +docs/ Engineering notes +e2e/ Playwright smoke/routes +input_info/ Official Ozon input packs +official_sources/ Classifier bounds PDF +public/ Runtime static assets (GLB/STL/draco) +src/ React/Three web twin +Dockerfile Web production image docker-compose.server.yml nginx.conf -.github/workflows/ci.yml +package.json / lock +vite / vitest / playwright / tsconfig +README.md ``` -## Environment variables +No other top-level product directories are required to run or understand the solution. -No runtime secrets are required for local demo or production static hosting. +## 8. Requirements -Optional **build-time** identity (Docker / CI only; never commit secrets): +**Web:** Node.js 20+, npm (`package-lock.json`). +**CV:** Python 3.10+, ffmpeg, V4L2; RealSense D415 for live mode (`cv/requirements.txt`). +**Hardware (CV live / MQTT):** D415 USB3; optional MQTT broker + servo/motor controllers on site network. -| Variable | Purpose | +## 9. Web quick start + +```bash +npm ci +npm run dev # http://127.0.0.1:3100 +npm test -- --run +npm run build +npm run preview # http://127.0.0.1:3100 +``` + +## 10. CV quick start + +```bash +cd cv +./demo.sh # venv + deps; HUD on :8080 (needs D415 for live view) +# without camera: +python3 -m venv .venv && .venv/bin/pip install -r requirements.txt +.venv/bin/python test_classify.py +.venv/bin/python test_geometry.py +# live pipeline (hardware): +./run.sh --preview --no-mqtt --no-motor +``` + +`npm install` does **not** install CV dependencies. + +## 11. Configuration + +**Web (build-time, optional):** `VITE_BUILD_COMMIT`, `VITE_BUILD_BRANCH`, `VITE_BUILD_RELEASE` → `/version.json`. No runtime secrets. + +**CV:** copy `cv/config.example.yaml` → `cv/config.yaml` (gitignored). MQTT/motor/routing **disabled by default**. Never commit real passwords or broker credentials. + +## 12. Classification rules + +Verified in web (`src/domain/classifier.ts`) and CV (`cv/classify.py`): + +- dimensions strictly **> 10×10×10 mm** and **< 450×320×320 mm** +- circular when **K > 0.8** (web) / `circle_ratio ≥ 0.8` (CV) +- order: dimensions fail → **C**; else circular → **D**; else **B** + +Official citation: `official_sources/doc-1783095831.pdf` (present; not re-parsed on every doc pass). Missing extracted brief PDF is not claimed. + +## 13. Physics (accepted `main`) + +From current source (not superseded experimental branches): + +- belt speed **1.0 m/s** (`CONVEYOR_SPEED_MPS`) +- fixed timestep **1/60 s** (`PHYSICS_TIMESTEP_SEC`) +- CCD for light/thin product bodies +- diverters LEFT **−45°**, RIGHT **+45°** +- full contact-only junction sorting through CAD: **not fully validated** + +## 14. CAD and assets + +| Asset | Path | |---|---| -| `VITE_BUILD_COMMIT` | Short git commit in `/version.json` | -| `VITE_BUILD_BRANCH` | Branch name in `/version.json` | -| `VITE_BUILD_RELEASE` | Release label in `/version.json` | +| Author CAD | `3d_models/conveer.FCStd` | +| Runtime GLB | `public/models/sorter/conveyor-clean.glb` | +| Products | `public/models/*.stl` | -Do not commit `.env` files. Local `npm run build` derives identity from git when available. +Keep runtime assets in Git for deploy. Mirror large CAD/models/presentation/video to cloud for submission. -## Active assets - -| Path | Role | -|---|---| -| `3d_models/conveer.FCStd` | Author CAD (canonical source) | -| `public/models/sorter/conveyor-clean.glb` | Active runtime conveyor | -| `public/models/*.stl` | Product models | -| `input_info/*` | Official Ozon packs (PDFs/ZIPs) | -| `official_sources/doc-1783095831.pdf` | Official classifier bounds source cited by code | - -### Frozen checksums (SHA-256) +SHA-256 (frozen): ``` 3d_models/conveer.FCStd @@ -115,112 +175,39 @@ public/models/sorter/conveyor-clean.glb 1dc7a8d7891bfe756e277ad5368df74cb73410156b2fe0f92845afb8a56f285a ``` -## Classifier vs real CV - -**Web twin (`main`):** rule-based classification in the digital twin (`src/domain/classifier.ts`) using Track 3 bounds and roundness. Camera overlay / measurement in the twin is a **simulation**, not live RealSense inference. This is **not** a neural network in production. - -**Real CV (parallel prototype, not in the web app):** preserved on branch **`drho1y-mvp_1`** under `vision_classifier/` — OpenCV + Intel RealSense D415 depth → L×W×H + circle_ratio → B/C/D → MQTT. No PyTorch/YOLO weight files are stored in git. +## 15. Testing ```bash -git fetch origin -git switch drho1y-mvp_1 -cd vision_classifier -# see vision_classifier/README.md and START.md -./demo.sh -.venv/bin/python test_classify.py -./run.sh --preview # hardware path (Orange PI + RealSense) -``` - -Integration status: **WORKING_PROTOTYPE on `drho1y-mvp_1`**, **not integrated** into https://arhipovdan.ru. - -## Sorting logic - -**CURRENT_IMPLEMENTATION_VERIFIED_IN_CODE** (`src/domain/classifier.ts`, `src/domain/pusherMotion.ts`, unit tests). - -| Category | Physical route | Active CAD diverter | -|---|---|---| -| B | STRAIGHT | none | -| C | PHYSICAL_LEFT | LEFT, **−45°** | -| D | PHYSICAL_RIGHT | RIGHT, **+45°** | - -Frozen diverter timing / planes (verified in code + tests): - -- rotation duration: **0.50 s** -- opening safety margin: **0.15 s** -- contact plane S: **1.0538** -- clear plane S: **1.6000** - -Classifier bounds (code + `official_sources/doc-1783095831.pdf` reference): - -- dimensions strictly **> 10×10×10 mm** and **< 450×320×320 mm** -- circular when **K > 0.8** -- check order: dimensions → C, else circular → D, else B - -## Physical simulation - -- Belt target speed **1.0 m/s** (`CONVEYOR_SPEED_MPS`) -- Fixed physics timestep **1/60 s** (`PHYSICS_TIMESTEP_SEC`) -- CCD enabled for small/fast or thin product rigid bodies -- LEFT/RIGHT CAD diverters −45° / +45° -- Rapier world with gravity **[0, −9.81, 0]** -- Full contact-only junction sorting through CAD diverters is **not fully validated** - -Details: `docs/ENGINEERING.md`, `/documentation`. - -## Validation - -```bash -npm ci -npm test -- --run # currently 196 unit tests +npm test -- --run +# current release result: 196/196 npm run build -npx vite preview --host 127.0.0.1 --port 3101 & PLAYWRIGHT_BASE_URL=http://127.0.0.1:3101 npm run test:e2e -- e2e/routes.spec.ts e2e/smoke.spec.ts + +cd cv && .venv/bin/python test_classify.py && .venv/bin/python test_geometry.py ``` -## Production deploy (this host) +## 16. Deployment -Nginx terminates TLS for `arhipovdan.ru` and proxies to Docker `owl-web-1` on `127.0.0.1:3100` (`docker-compose.server.yml` + `Dockerfile`). +Nginx terminates TLS for `arhipovdan.ru` and proxies to Docker `owl-web-1` (`docker-compose.server.yml` + `Dockerfile`) on `127.0.0.1:3100`. Deploy from `main` with build-args for `/version.json`. CV is **not** part of the web container. -```bash -cd /opt/arhipovdan/app -COMMIT=$(git rev-parse --short HEAD) -BRANCH=$(git rev-parse --abbrev-ref HEAD) -RELEASE=$(date -u +%Y%m%d-%H%M%S) -docker compose -p owl -f docker-compose.server.yml build \ - --build-arg "BUILD_COMMIT=${COMMIT}" \ - --build-arg "BUILD_BRANCH=${BRANCH}" \ - --build-arg "BUILD_RELEASE=${RELEASE}" -docker compose -p owl -f docker-compose.server.yml up -d --force-recreate -curl -s https://arhipovdan.ru/version.json -``` +## 17. Submission materials -## Repository vs cloud materials +| Material | Status | +|---|---| +| Presentation URL | REQUIRED_FROM_OWNER | +| Video demo URL | REQUIRED_FROM_OWNER | +| Cloud folder URL | REQUIRED_FROM_OWNER | -| Material | In git | Cloud expected | -|---|---|---| -| Source, README, docs, configs, tests | yes | — | -| Runtime GLB/STL used by deploy | yes (required) | recommended mirror | -| Author CAD `conveer.FCStd` (~5 MB) | yes | recommended upload | -| Official PDFs/ZIPs (`input_info/`, `official_sources/`) | yes | optional mirror | -| Presentation / video demonstration | **not in repo** | **required for platform field** | -| ML weights | none (CV is classical OpenCV depth) | N/A | +Do not invent links. Runtime site assets remain in Git even when mirrored to cloud. -Confirmed public presentation / video / cloud-folder URLs are **not stored in this repository**. Owner must supply them in the platform submission field. +## 18. Known limitations -## Current limitations +- Mobile uses 2D lite fallback on current baseline +- CV is a prototype and is not live-integrated into arhipovdan.ru +- Simulation physics is engineering-derived; hardware calibration still required +- Contact routing through CAD diverters not fully validated +- Large presentation/video must be uploaded to cloud by owner -- Web classifier is a digital twin simulation, not live RealSense inference. -- Full contact-only sorting through CAD diverters is not fully validated. -- Belt surface-velocity physics (true tangential drive) is planned, not complete. -- Per-SKU mass / COM / friction profiles still need calibration. -- Author CAD horn / transmission incomplete in active GLB (`AUTHOR_CAD_INCOMPLETE`). -- Official compliance limited by missing extracted task PDF / PDF re-parse policy. -- Generated screenshots, videos, and stage folders are not canonical. +## 19. Branch history policy -## Repository policy - -- Keep author CAD, active runtime assets, official sources, build configs, and tests. -- Real CV remains on `drho1y-mvp_1`; do not force-merge into `main` without validated web integration. -- Rollback / accepted product baseline content: `13ce16b` (current `main` tree via `bb76963`). - -See also: `/documentation` in the running app, and `docs/ENGINEERING.md`. +**`main` is the canonical complete solution** (web + cleaned CV under `cv/`). Historical branches (`dan_branch`, `drho1y-mvp_1`, …) may remain for audit but are not required to run the product. diff --git a/cv/.gitignore b/cv/.gitignore new file mode 100644 index 0000000..f61c841 --- /dev/null +++ b/cv/.gitignore @@ -0,0 +1,11 @@ +.venv/ +__pycache__/ +*.pyc +debug_frames/ +logs/ +*.jpg +*.png +.pytest_cache/ +config.yaml +.env +.env.* diff --git a/cv/Dockerfile b/cv/Dockerfile new file mode 100644 index 0000000..cf91f5b --- /dev/null +++ b/cv/Dockerfile @@ -0,0 +1,23 @@ +# Vision classifier for Intel RealSense D415 on Orange PI (aarch64) +FROM python:3.11-slim-bookworm + +ENV DEBIAN_FRONTEND=noninteractive \ + PYTHONUNBUFFERED=1 \ + PIP_NO_CACHE_DIR=1 + +RUN apt-get update && apt-get install -y --no-install-recommends \ + libglib2.0-0 \ + libgl1 \ + libv4l-0 \ + v4l-utils \ + ffmpeg \ + && rm -rf /var/lib/apt/lists/* + +WORKDIR /app +COPY requirements.txt . +RUN pip install -r requirements.txt + +COPY . . + +# Камера пробрасывается через docker-compose (--device) +CMD ["python", "main.py", "-c", "config.yaml"] diff --git a/cv/README.md b/cv/README.md new file mode 100644 index 0000000..d4df229 --- /dev/null +++ b/cv/README.md @@ -0,0 +1,176 @@ +# Real CV prototype — RealSense D415 + OpenCV + +**Status:** WORKING_PROTOTYPE +**Production integrated:** NO (https://arhipovdan.ru does **not** consume this pipeline) +**Source:** consolidated from branch `drho1y-mvp_1` (`vision_classifier/`) into `cv/` + +Same Track 3 B/C/D rules as the web twin; different input path (real depth camera vs simulated sensor). + +## Purpose + +Measure parcels on a conveyor with an Intel RealSense D415 (depth + color), estimate L×W×H and circularity, classify into zones **B / C / D**, optionally publish results over MQTT for hardware routing. + +## Data flow + +``` +RealSense D415 (V4L2 depth + color) + → OpenCV segmentation on depth (optional RGB flat detect) + → measure L×W×H + circle_ratio + → stabilize (median / vote → LOCK) + → classify B/C/D + → optional MQTT (category, dimensions, servo/motor topics) +``` + +## Entrypoints (start here) + +| Command | Role | +|---|---| +| `./demo.sh` | Browser HUD demo on `:8080` (needs camera for live view) | +| `./run.sh --preview` | Live pipeline with JPEG preview frames | +| `./run.sh --once --no-mqtt --no-motor` | Single-shot / dry hardware | +| `.venv/bin/python test_classify.py` | Classifier unit checks **without camera** | +| `.venv/bin/python test_geometry.py` | Geometry helpers **without camera** | + +Primary modules: `main.py` (live), `demo.py` (HUD), `classify.py` (rules), `measure.py` (depth metrics), `camera.py` (V4L2 RealSense). + +## Classification rules (Track 3) + +1. Dimensions must be strictly **> 10×10×10 mm** and **< 450×320×320 mm** → else **C** +2. Else if `circle_ratio ≥ 0.8` → **D** +3. Else → **B** + +Stabilization: median window + vote → **LOCK**. Uncertain cases fall back to zone **C** after N frames. + +## File structure + +``` +cv/ + main.py # live pipeline entry + demo.py / demo.sh # browser demo + run.sh # venv bootstrap + main.py + camera.py # RealSense via V4L2 + ffmpeg depth + measure.py # segmentation + dimensions + classify.py # B/C/D rules + stabilize.py # temporal LOCK + mqtt_bridge.py # optional MQTT (disabled by default) + calibrate.py # fx/fy + belt height calibration + align_color.py # RGB↔depth alignment helper + tracker.py # multi-object tracking assist + journal.py # decisions JSONL writer + demo_hud.py # HUD rendering + collect_log.py # log helper + test_classify.py # no-camera tests + test_geometry.py # no-camera tests + config.example.yaml # safe defaults (commit) + config.yaml # local only (gitignored) + requirements.txt + Dockerfile / docker-compose.yml +``` + +## Dependencies + +**Software** + +- Python **3.10+** (3.11 recommended; Docker image uses 3.11) +- `opencv-python-headless`, `numpy`, `PyYAML`, `pillow`, `paho-mqtt` — see `requirements.txt` +- System: **ffmpeg**, V4L2 (`v4l-utils` useful) + +**Hardware (live mode)** + +- Intel **RealSense D415** on USB3 +- Linux host with `/dev/video*` depth+color nodes (Orange PI / x86) + +`npm` / Node packages are **not** used here. + +## Installation + +```bash +cd cv +python3 -m venv .venv +source .venv/bin/activate +pip install -U pip +pip install -r requirements.txt +cp config.example.yaml config.yaml # optional; scripts auto-copy +``` + +Or simply: + +```bash +cd cv +./demo.sh # creates .venv and config.yaml on first run +``` + +## Demo / tests without claiming live camera + +Classifier and geometry (no RealSense required): + +```bash +cd cv +python3 -m venv .venv && .venv/bin/pip install -r requirements.txt +.venv/bin/python test_classify.py +.venv/bin/python test_geometry.py +python3 -m compileall . +``` + +Live HUD (requires D415): + +```bash +./demo.sh +# open http://127.0.0.1:8080/ +``` + +Live pipeline: + +```bash +./run.sh --preview --no-mqtt --no-motor +# or full hardware once MQTT/routing configured in local config.yaml: +./run.sh --preview +``` + +## Configuration + +| File | Role | +|---|---| +| `config.example.yaml` | Committed safe defaults; **MQTT/motor/routing disabled** | +| `config.yaml` | Local overrides — **gitignored**; never commit credentials | + +Optional MQTT (enable only locally): + +```yaml +mqtt: + enabled: true + broker: "127.0.0.1" + port: 1883 + user: "" + password: "" +``` + +CLI overrides: `--no-mqtt`, `--no-motor`, `--dry-route`, `--once`, `--preview`. + +## Output schema (LOCK) + +- Zone: `B` | `C` | `D` +- Dimensions mm: L×W×H +- `circle_ratio` +- Optional MQTT topics (when enabled): `vision/feedback/category`, `…/dimensions`, `…/circle_ratio` +- Optional JSONL: `logs/decisions.jsonl` (local, gitignored) + +## Limitations + +- Not connected to the web digital twin runtime. +- Requires calibrated intrinsics / belt height for accurate mm. +- Live demo needs a physical D415; CI hosts usually lack it. +- MQTT/servo/motor path is optional and site-specific. + +## Troubleshooting + +| Symptom | Check | +|---|---| +| No `/dev/video*` | USB3, `lsusb`, `v4l2-ctl --list-devices` | +| Depth empty | ffmpeg installed; correct `depth_device` | +| Wrong sizes | run `calibrate.py --length … --width …` | +| MQTT offline | expected when `mqtt.enabled: false` | + +## Relation to web twin + +Web (`src/domain/classifier.ts`) and CV (`classify.py`) implement the **same official bounds**. The public site uses a **digital sensor simulation**; this folder is the **hardware prototype** for future integration. diff --git a/cv/align_color.py b/cv/align_color.py new file mode 100644 index 0000000..d979026 --- /dev/null +++ b/cv/align_color.py @@ -0,0 +1,139 @@ +#!/usr/bin/env python3 +""" +Автоподбор совмещения RGB↔depth для демо (сенсоры D415 разнесены). + +Положите на ленту коробку с чёткими краями и запустите: + .venv/bin/python align_color.py + +Скрипт ищет сдвиг (dx, dy) и масштаб цветного кадра, при которых края +на RGB совпадают с краями на карте глубины, и пишет результат в config.yaml. +""" + +from __future__ import annotations + +import argparse +import re +import sys +import time +from pathlib import Path + +import cv2 +import numpy as np +import yaml + +sys.path.insert(0, str(Path(__file__).resolve().parent)) + +from camera import RealSenseV4L2 +from demo_hud import align_color + + +def _edges_depth(depth_mm: np.ndarray) -> np.ndarray: + d = depth_mm.astype(np.float32) + d = cv2.medianBlur(d.astype(np.uint16), 5).astype(np.float32) + valid = d > 0 + if valid.sum() < 1000: + return np.zeros(depth_mm.shape, np.uint8) + lo, hi = np.percentile(d[valid], [2, 98]) + norm = np.clip((d - lo) / max(hi - lo, 1.0) * 255.0, 0, 255).astype(np.uint8) + return cv2.Canny(norm, 30, 90) + + +def _edges_color(color_bgr: np.ndarray) -> np.ndarray: + gray = cv2.cvtColor(color_bgr, cv2.COLOR_BGR2GRAY) + # тёмные сцены: выравниваем контраст перед Canny + gray = cv2.createCLAHE(clipLimit=3.0, tileGridSize=(8, 8)).apply(gray) + gray = cv2.GaussianBlur(gray, (5, 5), 0) + return cv2.Canny(gray, 40, 120) + + +def _score(color_edges: np.ndarray, depth_band: np.ndarray, dx: float, dy: float, s: float) -> float: + warped = align_color(color_edges[..., None].repeat(3, axis=2), dx, dy, s)[..., 0] + return float(np.count_nonzero((warped > 0) & (depth_band > 0))) + + +def main() -> int: + parser = argparse.ArgumentParser(description="Совмещение RGB и depth") + parser.add_argument("-c", "--config", default=str(Path(__file__).with_name("config.yaml"))) + args = parser.parse_args() + + cfg_path = Path(args.config) + cfg = yaml.safe_load(cfg_path.read_text(encoding="utf-8")) + cam_cfg = cfg["camera"] + + print("[align] открываю камеру… на ленте должна лежать коробка с чёткими краями") + cam = RealSenseV4L2( + depth_device=cam_cfg.get("depth_device", "/dev/video0"), + color_device=cam_cfg.get("color_device", "/dev/video4"), + width=int(cam_cfg.get("width", 640)), + height=int(cam_cfg.get("height", 480)), + fps=int(cam_cfg.get("fps", 30)), + use_color=True, + ) + try: + # прогрев RGB: первые кадры бывают пустыми, плюс автоэкспозиция + for _ in range(30): + cam.read() + time.sleep(0.05) + depth_acc, color_acc = [], [] + for _ in range(15): + pair = cam.read() + if pair is not None and not pair.color_is_depth_preview: + depth_acc.append(pair.depth_mm.astype(np.float32)) + color_acc.append(pair.color_bgr.astype(np.float32)) + time.sleep(0.06) + if len(color_acc) < 3: + print("[align] RGB не читается — проверьте use_color/USB") + return 1 + depth = np.median(np.stack(depth_acc), axis=0).astype(np.uint16) + color = np.clip(np.mean(np.stack(color_acc), axis=0), 0, 255).astype(np.uint8) + + de = _edges_depth(depth) + if np.count_nonzero(de) < 500: + print("[align] мало краёв на depth — положите коробку в центр кадра") + return 1 + band = cv2.dilate(de, cv2.getStructuringElement(cv2.MORPH_RECT, (7, 7))) + ce = _edges_color(color) + + # грубый перебор → уточнение + best = (0.0, 0.0, 1.0) + best_s = -1.0 + for s in np.arange(0.90, 1.16, 0.05): + for dx in range(-80, 81, 8): + for dy in range(-60, 61, 8): + sc = _score(ce, band, dx, dy, float(s)) + if sc > best_s: + best_s, best = sc, (float(dx), float(dy), float(s)) + bdx, bdy, bs = best + for s in np.arange(bs - 0.04, bs + 0.045, 0.01): + for dx in np.arange(bdx - 8, bdx + 9, 2): + for dy in np.arange(bdy - 8, bdy + 9, 2): + sc = _score(ce, band, float(dx), float(dy), float(s)) + if sc > best_s: + best_s, best = sc, (float(dx), float(dy), float(s)) + + base = _score(ce, band, 0, 0, 1.0) + dx, dy, s = best + print(f"[align] лучшее совмещение: dx={dx:.0f} dy={dy:.0f} scale={s:.2f} " + f"(совпадение краёв {best_s:.0f} против {base:.0f} без коррекции)") + + text = cfg_path.read_text(encoding="utf-8") + text = re.sub(r"(?m)^(\s*color_dx:\s*)-?[\d.]+", rf"\g<1>{dx:.1f}", text) + text = re.sub(r"(?m)^(\s*color_dy:\s*)-?[\d.]+", rf"\g<1>{dy:.1f}", text) + text = re.sub(r"(?m)^(\s*color_scale:\s*)-?[\d.]+", rf"\g<1>{s:.3f}", text) + cfg_path.write_text(text, encoding="utf-8") + print(f"[align] записано в {cfg_path}") + + # контрольная картинка + out = Path("debug_frames"); out.mkdir(exist_ok=True) + from camera import depth_colormap + aligned = align_color(color, dx, dy, s) + vis = cv2.addWeighted(aligned, 0.6, depth_colormap(depth), 0.4, 0) + cv2.imwrite(str(out / "align_check.jpg"), vis) + print(f"[align] проверка: debug_frames/align_check.jpg") + finally: + cam.release() + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/cv/calibrate.py b/cv/calibrate.py new file mode 100644 index 0000000..10847b3 --- /dev/null +++ b/cv/calibrate.py @@ -0,0 +1,188 @@ +#!/usr/bin/env python3 +""" +Калибровка камеры для точных габаритов (правила ТЗ: >10×10×10, <450×320×320 мм). + +Два шага: + 1) пустая лента → высота belt_distance_mm; + 2) коробка известного размера в центре → фокусное fx=fy. + +Запуск (пример для коробки 300×200 мм, высотой ≥ 30 мм): + .venv/bin/python calibrate.py --length 300 --width 200 + +Результат пишется прямо в config.yaml (fx, fy, belt_distance_mm). +""" + +from __future__ import annotations + +import argparse +import re +import sys +import time +from pathlib import Path + +import cv2 +import numpy as np +import yaml + +sys.path.insert(0, str(Path(__file__).resolve().parent)) + +from camera import RealSenseV4L2 +from measure import segment_object + + +def _wait_key(prompt: str) -> None: + """Ждёт одиночное нажатие: 1 — продолжить, q — выйти (Enter не нужен).""" + print(prompt + " [1 — продолжить, q — выйти]", flush=True) + + if not sys.stdin.isatty(): + # stdin не терминал (пайп/IDE) — читаем строку + line = sys.stdin.readline().strip().lower() + if line.startswith("q"): + raise KeyboardInterrupt + return + + import termios + import tty + + fd = sys.stdin.fileno() + old = termios.tcgetattr(fd) + try: + tty.setraw(fd) + while True: + ch = sys.stdin.read(1) + if ch in ("1", "\r", "\n"): # Enter тоже принимаем на всякий случай + return + if ch in ("q", "Q", "\x03"): # q или Ctrl+C + raise KeyboardInterrupt + finally: + termios.tcsetattr(fd, termios.TCSADRAIN, old) + + +def _collect_belt(cam: RealSenseV4L2, samples: int = 25) -> float: + vals = [] + for _ in range(samples): + pair = cam.read() + if pair is None: + time.sleep(0.05) + continue + d = pair.depth_mm + h, w = d.shape + roi = d[h // 4 : 3 * h // 4, w // 4 : 3 * w // 4] + valid = roi[(roi > 200) & (roi < 4000)] + if valid.size > 100: + vals.append(float(np.median(valid))) + time.sleep(0.04) + if not vals: + raise RuntimeError("Не вижу ленту: проверьте, что камера на 0.5–1.5 м над поверхностью") + return float(np.median(vals)) + + +def _collect_focal( + cam: RealSenseV4L2, + belt_mm: float, + known_length_mm: float, + known_width_mm: float, + samples: int = 40, +) -> float: + """fx=fy по площади minAreaRect в пикселях: f = z * sqrt(S_px / S_mm).""" + focals = [] + for _ in range(samples): + pair = cam.read() + if pair is None: + time.sleep(0.05) + continue + seg = segment_object(pair.depth_mm, belt_distance_mm=belt_mm, min_area_px=400) + if seg is None: + time.sleep(0.04) + continue + mask, contour = seg + ys, xs = np.where(mask > 0) + z = pair.depth_mm[ys, xs].astype(np.float32) + z = z[z > 0] + if z.size < 100: + continue + z_med = float(np.median(z)) + + rect = cv2.minAreaRect(contour) + pw, ph = rect[1] + if pw < 10 or ph < 10: + continue + f = z_med * float(np.sqrt((pw * ph) / (known_length_mm * known_width_mm))) + focals.append(f) + time.sleep(0.04) + if len(focals) < 10: + raise RuntimeError( + f"Стабильно вижу коробку только в {len(focals)} кадрах из {samples}. " + "Коробка должна быть высотой ≥ 30 мм и лежать в центре кадра." + ) + return float(np.median(focals)) + + +def _patch_config(path: Path, fx: float, belt_mm: float) -> None: + text = path.read_text(encoding="utf-8") + text = re.sub(r"(?m)^(\s*fx:\s*)[\d.]+", rf"\g<1>{fx:.1f}", text) + text = re.sub(r"(?m)^(\s*fy:\s*)[\d.]+", rf"\g<1>{fx:.1f}", text) + text = re.sub(r"(?m)^(belt_distance_mm:\s*)[\d.]+", rf"\g<1>{belt_mm:.0f}", text) + path.write_text(text, encoding="utf-8") + + +def main() -> int: + parser = argparse.ArgumentParser(description="Калибровка fx/fy и высоты ленты") + parser.add_argument("-c", "--config", default=str(Path(__file__).with_name("config.yaml"))) + parser.add_argument("--length", type=float, required=True, help="Длина коробки, мм (рулеткой)") + parser.add_argument("--width", type=float, required=True, help="Ширина коробки, мм (рулеткой)") + parser.add_argument("--yes", action="store_true", help="Не ждать Enter (сцена уже готова на каждом шаге)") + args = parser.parse_args() + + cfg_path = Path(args.config) + if not cfg_path.exists(): + example = cfg_path.with_name("config.example.yaml") + if not example.exists(): + raise SystemExit(f"Config not found: {cfg_path}") + cfg_path = example + print(f"[calib] using {cfg_path.name} (copy to config.yaml before saving results)") + cfg = yaml.safe_load(cfg_path.read_text(encoding="utf-8")) + cam_cfg = cfg["camera"] + + print("[calib] открываю RealSense D415…") + cam = RealSenseV4L2( + depth_device=cam_cfg.get("depth_device", "/dev/video0"), + color_device=cam_cfg.get("color_device", "/dev/video4"), + width=int(cam_cfg.get("width", 640)), + height=int(cam_cfg.get("height", 480)), + fps=int(cam_cfg.get("fps", 30)), + depth_scale_mm=float(cam_cfg.get("depth_scale_mm", 1.0)), + use_color=False, + ) + try: + if not args.yes: + _wait_key("[calib] Шаг 1/2: УБЕРИТЕ всё с ленты") + belt_mm = _collect_belt(cam) + print(f"[calib] высота до ленты: {belt_mm:.0f} мм") + + if not args.yes: + _wait_key( + f"[calib] Шаг 2/2: положите коробку {args.length:.0f}×{args.width:.0f} мм " + "в центр кадра" + ) + time.sleep(1.0) + fx = _collect_focal(cam, belt_mm, args.length, args.width) + old_fx = float(cam_cfg.get("fx", 0)) + print(f"[calib] фокусное fx=fy: {fx:.1f} (было {old_fx:.1f})") + if old_fx > 0: + k = fx / old_fx + print(f"[calib] габариты со старым fx были завышены/занижены в {k:.2f} раза") + + _patch_config(cfg_path, fx, belt_mm) + print(f"[calib] записано в {cfg_path}: fx=fy={fx:.1f}, belt_distance_mm={belt_mm:.0f}") + print("[calib] проверьте: .venv/bin/python demo.py — размеры LWH должны совпадать с рулеткой") + except KeyboardInterrupt: + print("\n[calib] отменено, config.yaml не изменён") + return 1 + finally: + cam.release() + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/cv/camera.py b/cv/camera.py new file mode 100644 index 0000000..8e99a6c --- /dev/null +++ b/cv/camera.py @@ -0,0 +1,373 @@ +"""Захват Depth (+опционально Color) с Intel RealSense D415 через V4L2/ffmpeg. + +Классификация по ТЗ опирается на depth (габариты + круг в сечении). +RGB у D415 через сырой V4L2 часто пустой без librealsense — +тогда для превью используется colorize(depth). +""" + +from __future__ import annotations + +import shutil +import subprocess +import threading +import time +from dataclasses import dataclass +from typing import Optional, Union + +import cv2 +import numpy as np + + +@dataclass +class FramePair: + color_bgr: np.ndarray + depth_mm: np.ndarray # uint16, миллиметры + timestamp_ms: float + color_is_depth_preview: bool = False + + +def _device_path(device: Union[str, int]) -> str: + if isinstance(device, int) or str(device).isdigit(): + return f"/dev/video{int(device)}" + return str(device) + + +def _v4l2_index(device: Union[str, int]) -> int: + if isinstance(device, int): + return device + s = str(device).strip() + if s.isdigit(): + return int(s) + if "video" in s: + return int(s.rsplit("video", 1)[-1]) + raise ValueError(f"Некорректный V4L2 device: {device}") + + +def find_realsense_color_device(preferred: Union[str, int, None] = None) -> Optional[str]: + """Найти RGB-ноду D415 (YUYV). Номера /dev/videoN плавают после переподключения.""" + import glob + import os + + def _formats(path: str) -> str: + try: + return subprocess.check_output( + ["v4l2-ctl", "-d", path, "--list-formats-ext"], + stderr=subprocess.DEVNULL, + text=True, + timeout=2, + ) + except (OSError, subprocess.SubprocessError): + return "" + + preferred_path = _device_path(preferred) if preferred is not None else "" + scored: list[tuple[int, str]] = [] + for path in sorted(glob.glob("/dev/video*")): + if not os.path.exists(path): + continue + fmt = _formats(path) + if "Z16" in fmt or "'GREY'" in fmt or "Greyscale" in fmt: + continue + score = 2 if ("YUYV" in fmt or "MJPG" in fmt or "Motion-JPEG" in fmt) else 0 + if path == preferred_path: + score += 5 + scored.append((score, path)) + scored.sort(key=lambda x: (-x[0], x[1])) + + for _, path in scored: + try: + idx = _v4l2_index(path) + except ValueError: + continue + cap = cv2.VideoCapture(idx, cv2.CAP_V4L2) + if not cap.isOpened(): + continue + ok_frame = None + for _ in range(12): + ok, frame = cap.read() + if not ok or frame is None: + continue + if frame.ndim == 2: + break + if frame.ndim == 3 and frame.shape[2] == 2: + frame = cv2.cvtColor(frame, cv2.COLOR_YUV2BGR_YUY2) + if frame.ndim == 3 and float(np.mean(frame)) > 8.0 and float(np.std(frame)) > 5.0: + ok_frame = frame + break + cap.release() + if ok_frame is not None: + return path + return None + + +def fill_depth_holes(depth_mm: np.ndarray, ksize: int = 5) -> np.ndarray: + """Простое заполнение дыр в depth.""" + d = depth_mm.copy() + mask = (d > 0).astype(np.uint8) * 255 + if mask.mean() < 1: + return d + kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (ksize, ksize)) + closed = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel) + holes = ((closed > 0) & (d == 0)).astype(np.uint8) * 255 + if holes.any(): + scale = max(float(d.max()), 1.0) + img8 = np.clip(d.astype(np.float32) / scale * 255.0, 0, 255).astype(np.uint8) + filled8 = cv2.inpaint(img8, holes, 3, cv2.INPAINT_TELEA) + filled = (filled8.astype(np.float32) / 255.0 * scale).astype(np.uint16) + d[holes > 0] = filled[holes > 0] + med = cv2.medianBlur(d, 3) + valid = d > 0 + d[valid] = med[valid] + return d + + +def depth_colormap(depth_mm: np.ndarray, max_mm: Optional[int] = None) -> np.ndarray: + valid = depth_mm[(depth_mm > 0) & (depth_mm < 10000)] + if max_mm is None: + max_mm = int(np.percentile(valid, 95)) if valid.size else 2000 + max_mm = max(max_mm, 500) + clipped = np.clip(depth_mm.astype(np.float32), 0, max_mm) + norm = np.zeros_like(clipped, dtype=np.uint8) + mask = depth_mm > 0 + norm[mask] = (clipped[mask] / max_mm * 255.0).astype(np.uint8) + return cv2.applyColorMap(norm, cv2.COLORMAP_JET) + + +class RealSenseV4L2: + """D415: depth=/dev/video0 (Z16 gray16le). Color опционален.""" + + def __init__( + self, + depth_device: Union[str, int] = "/dev/video0", + color_device: Union[str, int] = "/dev/video4", + width: int = 640, + height: int = 480, + fps: int = 30, + depth_scale_mm: float = 1.0, + use_color: bool = True, + ) -> None: + if shutil.which("ffmpeg") is None: + raise RuntimeError("Нужен ffmpeg для чтения depth Z16 с RealSense") + + self.depth_scale_mm = float(depth_scale_mm) + self.width = int(width) + self.height = int(height) + self.fps = int(fps) + self.use_color = bool(use_color) + self._frame_bytes = self.width * self.height * 2 + self.color_cap = None + self._depth_path = _device_path(depth_device) + self._lock = threading.Lock() + self._latest: Optional[np.ndarray] = None + self._stop = threading.Event() + self._ff: Optional[subprocess.Popen] = None + self._thread: Optional[threading.Thread] = None + + self._start_depth_worker() + + if self.use_color: + found = find_realsense_color_device(color_device) + if found is None: + print(f"[camera] RGB не найден (искали {color_device}) — в вебе будет colorize(depth)") + self.color_cap = None + else: + if _device_path(found) != _device_path(color_device): + print(f"[camera] RGB: {found} (в конфиге было {color_device})") + else: + print(f"[camera] RGB: {found}") + color_idx = _v4l2_index(found) + self.color_cap = cv2.VideoCapture(color_idx, cv2.CAP_V4L2) + if self.color_cap.isOpened(): + self.color_cap.set(cv2.CAP_PROP_FRAME_WIDTH, self.width) + self.color_cap.set(cv2.CAP_PROP_FRAME_HEIGHT, self.height) + self.color_cap.set(cv2.CAP_PROP_FPS, self.fps) + self.color_cap.set(cv2.CAP_PROP_CONVERT_RGB, 1) + # прогрев автоэкспозиции — иначе первые кадры чёрные/зелёные + for _ in range(20): + self.color_cap.read() + else: + print("[camera] RGB VideoCapture не открылся") + self.color_cap = None + + # Ждём первый кадр + deadline = time.time() + 5.0 + while time.time() < deadline: + with self._lock: + if self._latest is not None: + break + time.sleep(0.05) + else: + self.release() + raise RuntimeError( + f"Не удалось читать depth с {self._depth_path}. " + "Проверьте USB3, что камера не занята другим процессом." + ) + + valid_pct = float(((self._latest > 0) & (self._latest < 5000)).mean() * 100) + print(f"[camera] depth OK, valid≈{valid_pct:.1f}% (лучше >30%; высота камеры 0.5–1.5 м)") + + def _start_depth_worker(self) -> None: + self._ff = subprocess.Popen( + [ + "ffmpeg", + "-hide_banner", + "-loglevel", + "error", + "-fflags", + "nobuffer", + "-flags", + "low_delay", + "-f", + "v4l2", + "-video_size", + f"{self.width}x{self.height}", + "-framerate", + str(self.fps), + "-pixel_format", + "gray16le", + "-i", + self._depth_path, + "-f", + "rawvideo", + "-pix_fmt", + "gray16le", + "-", + ], + stdout=subprocess.PIPE, + stderr=subprocess.DEVNULL, + bufsize=self._frame_bytes * 8, + ) + self._thread = threading.Thread(target=self._depth_loop, name="rs-depth", daemon=True) + self._thread.start() + + def _depth_loop(self) -> None: + assert self._ff is not None and self._ff.stdout is not None + while not self._stop.is_set(): + raw = self._ff.stdout.read(self._frame_bytes) + if not raw or len(raw) != self._frame_bytes: + if self._ff.poll() is not None: + break + continue + depth = np.frombuffer(raw, dtype=" tuple[np.ndarray, bool]: + if self.color_cap is not None: + ok, color = self.color_cap.read() + if ok and color is not None: + if color.shape[:2] != (self.height, self.width): + color = cv2.resize(color, (self.width, self.height), interpolation=cv2.INTER_LINEAR) + if color.ndim == 3 and color.shape[2] == 2: + color = cv2.cvtColor(color, cv2.COLOR_YUV2BGR_YUY2) + elif color.ndim == 2: + color = cv2.cvtColor(color, cv2.COLOR_GRAY2BGR) + # пустой YUYV-кадр после конвертации — ровный зелёный (mean>5, + # но вариации нет) → проверяем и разброс пикселей + if float(np.mean(color)) > 5.0 and float(np.std(color)) > 4.0: + return color, False + return depth_colormap(depth_mm), True + + def read(self) -> Optional[FramePair]: + with self._lock: + depth = None if self._latest is None else self._latest.copy() + if depth is None: + return None + color, is_preview = self._read_color(depth) + return FramePair( + color_bgr=color, + depth_mm=depth, + timestamp_ms=time.time() * 1000.0, + color_is_depth_preview=is_preview, + ) + + def capture_background(self, samples: int = 15) -> np.ndarray: + """Медианная карта глубины пустой сцены (лента + платформы/борта). + + Позволяет сегментировать товар на неровном фоне и не сливать его + с накопителем: объект = то, что ближе фона на min_object_height_mm. + """ + frames = [] + deadline = time.time() + 12.0 + while len(frames) < samples and time.time() < deadline: + pair = self.read() + if pair is not None: + frames.append(pair.depth_mm.astype(np.float32)) + time.sleep(0.04) + if len(frames) < max(3, samples // 3): + raise RuntimeError("Не удалось накопить кадры для фоновой карты") + stack = np.stack(frames) + stack[stack <= 0] = np.nan + bg = np.nanmedian(stack, axis=0) + return np.nan_to_num(bg, nan=0.0).astype(np.uint16) + + def capture_background_rgb(self, samples: int = 10) -> Optional[np.ndarray]: + """Усреднённый RGB-кадр пустой сцены — для детекции плоских товаров + (телефон и т.п.), которые не видны в depth.""" + frames = [] + deadline = time.time() + 8.0 + while len(frames) < samples and time.time() < deadline: + pair = self.read() + if pair is not None and not pair.color_is_depth_preview: + frames.append(pair.color_bgr.astype(np.float32)) + time.sleep(0.04) + if len(frames) < 3: + return None + return np.clip(np.mean(np.stack(frames), axis=0), 0, 255).astype(np.uint8) + + def estimate_belt_distance_mm(self, samples: int = 30) -> float: + vals = [] + h, w = self.height, self.width + rois = [ + (h // 2 - 40, h // 2 + 40, w // 2 - 60, w // 2 + 60), + (h // 3 - 30, h // 3 + 30, w // 3 - 40, w // 3 + 40), + (2 * h // 3 - 30, 2 * h // 3 + 30, 2 * w // 3 - 40, 2 * w // 3 + 40), + (h // 4, 3 * h // 4, w // 4, 3 * w // 4), + ] + for _ in range(samples): + pair = self.read() + if pair is None: + time.sleep(0.03) + continue + for y0, y1, x0, x1 in rois: + roi = pair.depth_mm[y0:y1, x0:x1] + valid = roi[(roi > 200) & (roi < 4000)] + if valid.size >= 50: + vals.append(float(np.median(valid))) + break + else: + valid = pair.depth_mm[(pair.depth_mm > 200) & (pair.depth_mm < 4000)] + if valid.size >= 50: + vals.append(float(np.median(valid))) + time.sleep(0.03) + if not vals: + raise RuntimeError( + "Не удалось оценить belt_distance_mm. " + "Поставьте камеру на 0.5–1.5 м над лентой (USB3), задайте belt_distance_mm в config.yaml вручную." + ) + return float(np.median(vals)) + + def release(self) -> None: + self._stop.set() + if self.color_cap is not None: + self.color_cap.release() + self.color_cap = None + if self._ff is not None and self._ff.poll() is None: + self._ff.terminate() + try: + self._ff.wait(timeout=2) + except subprocess.TimeoutExpired: + self._ff.kill() + self._ff = None + if self._thread is not None: + self._thread.join(timeout=2) + self._thread = None + + def __enter__(self) -> "RealSenseV4L2": + return self + + def __exit__(self, *args) -> None: + self.release() diff --git a/cv/classify.py b/cv/classify.py new file mode 100644 index 0000000..2776575 --- /dev/null +++ b/cv/classify.py @@ -0,0 +1,147 @@ +"""Классификация строго по правилам ТЗ трека 3.""" + +from __future__ import annotations + +from dataclasses import dataclass +from enum import Enum +from typing import Sequence, Tuple + +from measure import ObjectMeasurement + + +class Category(str, Enum): + SUITABLE = "suitable" # Подходит для сортировки → B + OVERSIZE = "oversize" # Не подходит по габаритам → C + NEED_PACK = "need_pack" # Не подходит без доупаковки → D + + @property + def zone(self) -> str: + return { + Category.SUITABLE: "B", + Category.OVERSIZE: "C", + Category.NEED_PACK: "D", + }[self] + + @property + def ru_label(self) -> str: + return { + Category.SUITABLE: "Подходит для сортировки", + Category.OVERSIZE: "Не подходит для сортировки по габаритам", + Category.NEED_PACK: "Не подходит для сортировки без доупаковки", + }[self] + + @property + def short_label(self) -> str: + """Короткая метка для HUD и веб-статуса.""" + return { + Category.SUITABLE: "ГОТОВ К СОРТИРОВКЕ", + Category.OVERSIZE: "НЕГАБАРИТ", + Category.NEED_PACK: "ТРЕБУЕТ ДОУПАКОВКИ", + }[self] + + +@dataclass +class ClassificationResult: + category: Category + dims_sorted_mm: Tuple[float, float, float] + circle_ratio: float + passes_size: bool + is_circular: bool + reason: str + + +def _sorted_dims(length: float, width: float, height: float) -> Tuple[float, float, float]: + a, b, c = sorted([float(length), float(width), float(height)], reverse=True) + return a, b, c + + +def check_size( + dims_sorted: Sequence[float], + min_mm: Sequence[float], + max_mm: Sequence[float], +) -> bool: + """ + ТЗ: габариты строго больше минимума и строго меньше максимума + по сопоставленным сторонам после сортировки. + """ + min_s = sorted([float(x) for x in min_mm], reverse=True) + max_s = sorted([float(x) for x in max_mm], reverse=True) + d = [float(x) for x in dims_sorted] + return all(d[i] > min_s[i] for i in range(3)) and all(d[i] < max_s[i] for i in range(3)) + + +def classify( + measurement: ObjectMeasurement, + min_mm: Sequence[float] = (10, 10, 10), + max_mm: Sequence[float] = (450, 320, 320), + circle_ratio_threshold: float = 0.8, +) -> ClassificationResult: + """ + Порядок ТЗ: + 1) габариты → иначе C (приоритет над кругом) + 2) если r_in/r_out >= 0.8 в любом сечении → D + 3) иначе → B + """ + dims = _sorted_dims(measurement.length_mm, measurement.width_mm, measurement.height_mm) + passes = check_size(dims, min_mm, max_mm) + ratio = float(measurement.circle_ratio) + circular = ratio >= float(circle_ratio_threshold) + clipped = bool(getattr(measurement, "clipped_by_frame", False)) + + if not passes or clipped: + reason = ( + "объект обрезан краем кадра → габарит неполный, считаем негабаритом" + if clipped and passes + else "габариты вне допуска: нужно >10×10×10 и <450×320×320 мм" + ) + if clipped and not passes: + reason = "габариты вне допуска (в т.ч. обрезан кадром): нужно >10×10×10 и <450×320×320 мм" + return ClassificationResult( + category=Category.OVERSIZE, + dims_sorted_mm=dims, + circle_ratio=ratio, + passes_size=False, + is_circular=circular, + reason=reason, + ) + + if circular: + return ClassificationResult( + category=Category.NEED_PACK, + dims_sorted_mm=dims, + circle_ratio=ratio, + passes_size=True, + is_circular=True, + reason=f"круг в сечении: r_in/r_out={ratio:.3f} >= {circle_ratio_threshold}", + ) + + return ClassificationResult( + category=Category.SUITABLE, + dims_sorted_mm=dims, + circle_ratio=ratio, + passes_size=True, + is_circular=False, + reason=f"габариты OK, круга нет: r_in/r_out={ratio:.3f} < {circle_ratio_threshold}", + ) + + +def classify_from_dims( + length_mm: float, + width_mm: float, + height_mm: float, + circle_ratio: float, + min_mm: Sequence[float] = (10, 10, 10), + max_mm: Sequence[float] = (450, 320, 320), + circle_ratio_threshold: float = 0.8, +) -> ClassificationResult: + fake = ObjectMeasurement( + length_mm=length_mm, + width_mm=width_mm, + height_mm=height_mm, + circle_ratio=circle_ratio, + area_px=0, + centroid_px=(0, 0), + contour=None, # type: ignore + mask=None, # type: ignore + ) + return classify(fake, min_mm, max_mm, circle_ratio_threshold) diff --git a/cv/collect_log.py b/cv/collect_log.py new file mode 100644 index 0000000..d313d6b --- /dev/null +++ b/cv/collect_log.py @@ -0,0 +1,151 @@ +#!/usr/bin/env python3 +""" +Сбор логов классификации с камеры. +Кладите предметы по очереди — пишет CSV + печатает сводку. + + .venv/bin/python collect_log.py + # Ctrl+C — стоп +""" + +from __future__ import annotations + +import csv +import sys +import time +from datetime import datetime +from pathlib import Path + +import yaml + +sys.path.insert(0, str(Path(__file__).resolve().parent)) + +from camera import RealSenseV4L2 +from measure import measure_object, segment_object +from stabilize import DecisionStabilizer + + +def main() -> int: + cfg = yaml.safe_load(Path("config.yaml").read_text(encoding="utf-8")) + cam_cfg = cfg["camera"] + cls = cfg["classification"] + thr = float(cls.get("circle_ratio_threshold", 0.8)) + + out_dir = Path("debug_frames") + out_dir.mkdir(exist_ok=True) + stamp = datetime.now().strftime("%H%M%S") + csv_path = out_dir / f"log_{stamp}.csv" + + cam = RealSenseV4L2( + depth_device=cam_cfg.get("depth_device", "/dev/video0"), + color_device=cam_cfg.get("color_device", "/dev/video4"), + width=int(cam_cfg.get("width", 640)), + height=int(cam_cfg.get("height", 480)), + fps=int(cam_cfg.get("fps", 30)), + depth_scale_mm=float(cam_cfg.get("depth_scale_mm", 1.0)), + use_color=False, + ) + belt = float(cfg.get("belt_distance_mm") or 600) + print(f"[log] belt={belt:.0f} mm thr={thr} → {csv_path}") + print("[log] Кладите КРУГ / ПРЯМОУГОЛЬНИК. Ctrl+C — стоп.\n") + + stab = DecisionStabilizer(window=12, confirm_frames=8, lost_frames=12, enter_circle=thr) + fx, fy = float(cam_cfg["fx"]), float(cam_cfg["fy"]) + cx, cy = float(cam_cfg["cx"]), float(cam_cfg["cy"]) + + f = csv_path.open("w", newline="", encoding="utf-8") + w = csv.writer(f) + w.writerow( + [ + "t", + "present", + "L", + "W", + "H", + "top", + "sec", + "circle", + "raw_zone", + "lock", + "lock_zone", + "conf", + ] + ) + + last_print = 0.0 + n = 0 + try: + while True: + pair = cam.read() + if pair is None: + time.sleep(0.02) + continue + n += 1 + seg = segment_object( + pair.depth_mm, + belt_distance_mm=belt, + belt_tolerance_mm=float(cfg.get("belt_tolerance_mm", 25)), + min_object_height_mm=float(cfg.get("min_object_height_mm", 5)), + min_area_px=int(cfg.get("min_object_area_px", 400)), + ) + m = None + if seg is not None: + mask, contour = seg + m = measure_object( + pair.depth_mm, mask, contour, belt, fx, fy, cx, cy + ) + + d = stab.update( + m, + min_mm=cls.get("min_mm", [10, 10, 10]), + max_mm=cls.get("max_mm", [450, 320, 320]), + ) + + if m is None: + raw_zone = "-" + row = [time.time(), 0, "", "", "", "", "", "", raw_zone, int(d.locked), "", d.confidence_pct] + else: + dims = sorted([m.length_mm, m.width_mm, m.height_mm], reverse=True) + raw = "C" + if all(dims[i] > 10 and dims[i] < [450, 320, 320][i] for i in range(3)): + raw = "D" if m.circle_ratio >= thr else "B" + lz = d.result.category.zone if (d.locked and d.result) else "" + row = [ + time.time(), + 1, + round(dims[0], 1), + round(dims[1], 1), + round(dims[2], 1), + round(m.top_ratio, 3), + round(m.section_ratio, 3), + round(m.circle_ratio, 3), + raw, + int(d.locked), + lz, + d.confidence_pct, + ] + w.writerow(row) + if n % 5 == 0: + f.flush() + + now = time.time() + if now - last_print > 0.45: + last_print = now + if m is None: + print(f"[{n:05d}] пусто") + else: + lz = d.result.category.zone if (d.locked and d.result) else "…" + print( + f"[{n:05d}] raw={row[8]} lock={lz or '—':1s} conf={d.confidence_pct:3d}% | " + f"LWH={row[2]:.0f}×{row[3]:.0f}×{row[4]:.0f} | " + f"top={m.top_ratio:.3f} sec={m.section_ratio:.3f} circ={m.circle_ratio:.3f}" + ) + except KeyboardInterrupt: + print(f"\n[log] сохранено {csv_path}") + finally: + f.close() + cam.release() + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/cv/config.example.yaml b/cv/config.example.yaml new file mode 100644 index 0000000..101edc6 --- /dev/null +++ b/cv/config.example.yaml @@ -0,0 +1,90 @@ +# Vision classifier config EXAMPLE — copy to config.yaml and edit locally. +# Do not commit config.yaml (it may contain site-specific credentials). + +camera: + depth_device: /dev/video0 + color_device: /dev/video4 + width: 640 + height: 480 + fps: 30 + depth_scale_mm: 1.0 + # D415 @ 640x480 approximate intrinsics — calibrate on your setup + fx: 564.0 + fy: 564.0 + cx: 320.0 + cy: 240.0 + color_dx: -40.0 + color_dy: -8.0 + color_scale: 1.030 + +belt_distance_mm: 594 +belt_tolerance_mm: 25 +min_object_height_mm: 20 +min_object_area_px: 800 +roi_margin: + top: 0.12 + bottom: 0.02 + left: 0.05 + right: 0.12 +max_objects_in_frame: 3 +use_color: true +use_background_map: false +detect_flat_rgb: false +rgb_diff_threshold: 35 + +classification: + min_mm: [10, 10, 10] + max_mm: [450, 320, 320] + circle_ratio_threshold: 0.8 + uncertain_after_frames: 45 + uncertain_fallback_zone: C + +# MQTT is OPTIONAL and disabled by default for safe local runs. +mqtt: + broker: "127.0.0.1" + port: 1883 + user: "" + password: "" + client_id: "vision_classifier" + topic_result: "vision/feedback/category" + topic_dims: "vision/feedback/dimensions" + topic_circle: "vision/feedback/circle_ratio" + topic_debug: "vision/feedback/debug" + enabled: false + +motor: + enabled: false + rpm: -200 + current_percent: 50 + microsteps: 16 + stealthchop: true + disable_on_stop: false + +routing: + enabled: false + zones: + B: + servo: 0 + idle_angle: 0 + divert_angle: 0 + hold_ms: 500 + C: + servo: 1 + idle_angle: 0 + divert_angle: 90 + hold_ms: 800 + D: + servo: 2 + idle_angle: 0 + divert_angle: 90 + hold_ms: 800 + cooldown_ms: 1500 + +runtime: + show_preview: false + save_debug_frames: false + debug_dir: "debug_frames" + decisions_log: "logs/decisions.jsonl" + preview_every_n: 3 + process_every_n: 1 + confirm_frames: 8 diff --git a/cv/demo.py b/cv/demo.py new file mode 100755 index 0000000..e9cdf17 --- /dev/null +++ b/cv/demo.py @@ -0,0 +1,873 @@ +#!/usr/bin/env python3 +""" +Демо-режим хакатона с ползунками в браузере: + • высота до ленты (belt_distance_mm) + • порог уверенности (сколько кадров подряд одно и то же решение) + • мин. высота объекта, порог круга + +Без MQTT / мотора / серво. +""" + +from __future__ import annotations + +import argparse +import base64 +import json +import sys +import threading +import time +from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer +from pathlib import Path +from typing import Any, Dict, Optional +from urllib.parse import parse_qs, urlparse + +import cv2 +import numpy as np +import yaml + +from camera import RealSenseV4L2 +from classify import Category, ClassificationResult +from demo_hud import ContourSmoother, align_color, build_demo_frame +from journal import append_decision +from measure import ( + is_plausible_measurement, + measure_flat_object, + measure_object, + merge_overlapping_measurements, + segment_objects, + segment_rgb_objects, +) +from stabilize import DecisionStabilizer +from tracker import MultiObjectTracker, slot_key + +ZONE_TO_CATEGORY = { + "B": Category.SUITABLE, + "C": Category.OVERSIZE, + "D": Category.NEED_PACK, +} + + +HTML = r""" + + + + +Трек 3 — демо + + + +
+

Трек 3 — демо классификации (B / C / D)

+

Ползунки СВЕРХУ (отдельная панель). Картинка только камера. Выход: Ctrl+C в терминале. Обновите страницу Ctrl+F5.

+
+
+ + +
+
+ + +
+
+ + +
+
+ + +
+
+ + +
+
+
+
Загрузка…
+ + +
+
+
+
+ camera +
+ +
+ + + +""" + + +class Params: + def __init__(self) -> None: + self.lock = threading.Lock() + self.belt_mm: float = 800.0 + self.confidence_pct: int = 75 # порог фиксации + self.min_object_height_mm: float = 8.0 + self.circle_threshold: float = 0.80 + self.min_area_px: int = 400 + # runtime status + self.confidence_now: int = 0 + self.zone: Optional[str] = None + self.locked: bool = False + self.uncertain: bool = False + self.circle_ratio: Optional[float] = None + self.dims: Optional[tuple] = None + self.reason: str = "" + self.objects: list = [] # [{id, zone, label, dims, ratio, locked, uncertain, conf, crop}] + self.stats: dict = {} # счётчики за сессию + self.feed: list = [] # лента LOCK-событий (новые в конце) + self.feed_seq: int = 0 # версия ленты — клиент тянет только при изменении + self.feed_slots: set = set() # slot_key — один товар = одна карточка в ленте + self.crop_cache: dict = {} # track_id → base64, фиксируется при LOCK + self.jpeg: bytes = b"" + self.last_print: str = "" + self.cam: Any = None + self.request_autocalib: bool = False + self.autocalib_result: Optional[Dict[str, Any]] = None + self.background: Optional[np.ndarray] = None # карта глубины пустой сцены + self.color_background: Optional[np.ndarray] = None # RGB пустой сцены (плоские товары) + + +STATE = Params() + + +def make_handler() -> type: + class Handler(BaseHTTPRequestHandler): + def log_message(self, fmt: str, *args) -> None: + return + + def _json(self, code: int, obj: Dict[str, Any]) -> None: + body = json.dumps(obj, ensure_ascii=False).encode("utf-8") + self.send_response(code) + self.send_header("Content-Type", "application/json; charset=utf-8") + self.send_header("Cache-Control", "no-store") + self.send_header("Content-Length", str(len(body))) + self.end_headers() + self.wfile.write(body) + + def do_GET(self) -> None: + path = urlparse(self.path).path + if path.startswith("/frame.jpg"): + with STATE.lock: + data = STATE.jpeg + if not data: + self.send_error(503, "no frame yet") + return + self.send_response(200) + self.send_header("Content-Type", "image/jpeg") + self.send_header("Cache-Control", "no-store") + self.send_header("Content-Length", str(len(data))) + self.end_headers() + self.wfile.write(data) + elif path == "/api/status": + with STATE.lock: + self._json( + 200, + { + "belt_mm": round(STATE.belt_mm), + "confidence_pct": STATE.confidence_pct, + "confidence_now": STATE.confidence_now, + "min_object_height_mm": STATE.min_object_height_mm, + "circle_threshold": STATE.circle_threshold, + "min_area_px": STATE.min_area_px, + "zone": STATE.zone, + "locked": STATE.locked, + "uncertain": STATE.uncertain, + "circle_ratio": STATE.circle_ratio, + "dims": STATE.dims, + "reason": STATE.reason, + "objects": STATE.objects, + "stats": STATE.stats, + "feed_seq": STATE.feed_seq, + "feed": STATE.feed, + }, + ) + elif path == "/api/feed": + with STATE.lock: + self._json(200, {"seq": STATE.feed_seq, "items": STATE.feed}) + else: + body = HTML.encode("utf-8") + self.send_response(200) + self.send_header("Content-Type", "text/html; charset=utf-8") + self.send_header("Content-Length", str(len(body))) + self.end_headers() + self.wfile.write(body) + + def do_POST(self) -> None: + path = urlparse(self.path).path + length = int(self.headers.get("Content-Length", 0)) + raw = self.rfile.read(length) if length else b"{}" + try: + data = json.loads(raw.decode("utf-8") or "{}") + except json.JSONDecodeError: + data = {} + + if path == "/api/params": + with STATE.lock: + if "belt_mm" in data: + new_belt = float(np.clip(float(data["belt_mm"]), 200, 3000)) + # ручная правка высоты → фоновая карта устарела + if abs(new_belt - STATE.belt_mm) > 2.0: + STATE.background = None + STATE.belt_mm = new_belt + if "confidence_pct" in data: + STATE.confidence_pct = int(np.clip(int(data["confidence_pct"]), 10, 100)) + if "min_object_height_mm" in data: + STATE.min_object_height_mm = float(np.clip(float(data["min_object_height_mm"]), 1, 200)) + if "circle_threshold" in data: + STATE.circle_threshold = float(np.clip(float(data["circle_threshold"]), 0.4, 0.99)) + if "min_area_px" in data: + STATE.min_area_px = int(np.clip(int(data["min_area_px"]), 50, 20000)) + self._json(200, {"ok": True}) + elif path == "/api/autocalib": + with STATE.lock: + STATE.request_autocalib = True + STATE.autocalib_result = None + # ждём результат от цикла камеры + for _ in range(80): + time.sleep(0.1) + with STATE.lock: + if STATE.autocalib_result is not None: + self._json(200, STATE.autocalib_result) + return + self._json(500, {"ok": False, "error": "timeout"}) + else: + self.send_error(404) + + return Handler + + +def resolve_config_path(path: Path) -> Path: + if path.exists(): + return path + example = path.with_name("config.example.yaml") + if example.exists(): + return example + raise FileNotFoundError(f"Config not found: {path} (and no config.example.yaml)") + + +def load_config(path: Path) -> Dict[str, Any]: + resolved = resolve_config_path(Path(path)) + with open(resolved, "r", encoding="utf-8") as f: + return yaml.safe_load(f) + + +def frames_needed(confidence_pct: int) -> int: + # 10% → 5, 100% → 12 кадров одной зоны после прогрева окна + return max(5, int(round(5 + (confidence_pct / 100.0) * 7))) + + +def crop_b64(img: np.ndarray, contour: np.ndarray, pad: int = 14, max_w: int = 260) -> Optional[str]: + """Кроп объекта по bounding box контура → JPEG base64 для веб-панели.""" + x, y, w, h = cv2.boundingRect(contour) + H, W = img.shape[:2] + x0, y0 = max(0, x - pad), max(0, y - pad) + x1, y1 = min(W, x + w + pad), min(H, y + h + pad) + if x1 - x0 < 4 or y1 - y0 < 4: + return None + crop = img[y0:y1, x0:x1] + if crop.shape[1] > max_w: + s = max_w / crop.shape[1] + crop = cv2.resize(crop, (max_w, max(1, int(crop.shape[0] * s)))) + ok, buf = cv2.imencode(".jpg", crop, [int(cv2.IMWRITE_JPEG_QUALITY), 78]) + return base64.b64encode(buf.tobytes()).decode("ascii") if ok else None + + +def main() -> int: + parser = argparse.ArgumentParser(description="Демо классификации с ползунками") + parser.add_argument("-c", "--config", default=str(Path(__file__).with_name("config.yaml"))) + parser.add_argument("--host", default="0.0.0.0") + parser.add_argument("--port", type=int, default=8080) + args = parser.parse_args() + + cfg = load_config(Path(args.config)) + cam_cfg = cfg["camera"] + cls_cfg = cfg["classification"] + min_mm = cls_cfg.get("min_mm", [10, 10, 10]) + max_mm = cls_cfg.get("max_mm", [450, 320, 320]) + + out_dir = Path(cfg.get("runtime", {}).get("debug_dir", "debug_frames")) + out_dir.mkdir(parents=True, exist_ok=True) + out_jpg = out_dir / "demo_live.jpg" + + print("[demo] открываю RealSense D415…") + cam = RealSenseV4L2( + depth_device=cam_cfg.get("depth_device", "/dev/video0"), + color_device=cam_cfg.get("color_device", "/dev/video4"), + width=int(cam_cfg.get("width", 640)), + height=int(cam_cfg.get("height", 480)), + fps=int(cam_cfg.get("fps", 30)), + depth_scale_mm=float(cam_cfg.get("depth_scale_mm", 1.0)), + use_color=bool(cfg.get("use_color", False)), + ) + STATE.cam = cam + + belt0 = float(cfg.get("belt_distance_mm") or 0) + if belt0 <= 0: + print("[demo] калибровка ленты — уберите объекты…") + belt0 = cam.estimate_belt_distance_mm() + with STATE.lock: + STATE.belt_mm = belt0 + STATE.circle_threshold = float(cls_cfg.get("circle_ratio_threshold", 0.8)) + STATE.min_object_height_mm = float(cfg.get("min_object_height_mm", 8)) + STATE.min_area_px = int(cfg.get("min_object_area_px", 400)) + STATE.confidence_pct = 75 + print(f"[demo] belt_distance_mm = {belt0:.0f}") + max_objects = int(cfg.get("max_objects_in_frame", 3)) + detect_flat_rgb = bool(cfg.get("detect_flat_rgb", False)) + print(f"[demo] max_objects={max_objects}, flat_rgb={'ON' if detect_flat_rgb else 'OFF'}") + print("[demo] фоновая карта: кнопка «Авто-высота» на пустой ленте") + + server = ThreadingHTTPServer((args.host, args.port), make_handler()) + threading.Thread(target=server.serve_forever, daemon=True).start() + print(f"[demo] браузер → http://127.0.0.1:{args.port}/") + print("[demo] ползунки СВЕРХУ страницы (не на картинке)") + print("[demo] зона только после LOCK (медиана 12 кадров + голосование)") + print("[demo] Ctrl+C — выход\n") + + fx, fy = float(cam_cfg["fx"]), float(cam_cfg["fy"]) + cx, cy = float(cam_cfg["cx"]), float(cam_cfg["cy"]) + fallback_zone = str(cls_cfg.get("uncertain_fallback_zone", "C")).upper() + thr0 = float(cls_cfg.get("circle_ratio_threshold", 0.8)) + settings = {"confirm": frames_needed(75), "circ": thr0} + + def make_stabilizer() -> DecisionStabilizer: + return DecisionStabilizer( + window=12, + confirm_frames=settings["confirm"], + lost_frames=12, + enter_circle=settings["circ"], + exit_circle=settings["circ"] - 0.08, + uncertain_after=int(cls_cfg.get("uncertain_after_frames", 45)), + fallback=ZONE_TO_CATEGORY.get(fallback_zone, Category.OVERSIZE), + ) + + # lost_frames=30 ≈ 1.5–2 с: глянцевые/тёмные предметы (мышка) дают + # кратковременные выпадения depth — трек не должен умирать от них + tracker = MultiObjectTracker(make_stabilizer, max_dist_px=120, lost_frames=30) + contour_smoother = ContourSmoother(alpha=0.3) + color_align = ( + float(cam_cfg.get("color_dx", 0.0)), + float(cam_cfg.get("color_dy", 0.0)), + float(cam_cfg.get("color_scale", 1.0)), + ) + decisions_log = Path(cfg.get("runtime", {}).get("decisions_log", "logs/decisions.jsonl")) + frame_i = 0 + last_conf_setting = 75 + last_circ = thr0 + + try: + while True: + # автокалибровка по запросу из UI + with STATE.lock: + need_auto = STATE.request_autocalib + if need_auto: + STATE.request_autocalib = False + if need_auto: + try: + print("[demo] автокалибровка: снимаю фоновую карту (сцена должна быть пустой)…") + bg = cam.capture_background(samples=15) + color_bg = cam.capture_background_rgb(samples=10) + h, w = bg.shape + center = bg[h // 4 : 3 * h // 4, w // 4 : 3 * w // 4].astype(np.float32) + center = center[(center > 200) & (center < 4000)] + new_belt = float(np.median(center)) if center.size > 100 else cam.estimate_belt_distance_mm(samples=10) + with STATE.lock: + STATE.belt_mm = new_belt + STATE.background = bg + STATE.color_background = color_bg + STATE.autocalib_result = {"ok": True, "belt_mm": new_belt} + tracker.reset() + rgb_tag = "RGB-фон есть" if color_bg is not None else "RGB-фон недоступен" + print(f"[demo] высота = {new_belt:.0f} mm, фоновая карта активна, {rgb_tag}") + except Exception as exc: + with STATE.lock: + STATE.autocalib_result = {"ok": False, "error": str(exc)} + + pair = cam.read() + if pair is None: + time.sleep(0.02) + continue + frame_i += 1 + + with STATE.lock: + belt_mm = STATE.belt_mm + conf_pct = STATE.confidence_pct + hmin = STATE.min_object_height_mm + circ_thr = STATE.circle_threshold + min_area = STATE.min_area_px + background = STATE.background + color_background = STATE.color_background + + if conf_pct != last_conf_setting: + settings["confirm"] = frames_needed(conf_pct) + tracker.reset() + last_conf_setting = conf_pct + if abs(circ_thr - last_circ) > 1e-6: + settings["circ"] = float(circ_thr) + tracker.reset() + last_circ = circ_thr + + # несколько объектов в кадре → трекер с ID + measurements = [] + depth_union = None + seg_n = 0 + for mask, contour in segment_objects( + pair.depth_mm, + belt_distance_mm=belt_mm, + belt_tolerance_mm=float(cfg.get("belt_tolerance_mm", 25)), + min_object_height_mm=hmin, + min_area_px=min_area, + background_mm=background, + max_objects=max_objects, + ): + seg_n += 1 + depth_union = mask if depth_union is None else cv2.bitwise_or(depth_union, mask) + m = measure_object( + pair.depth_mm, + mask, + contour, + belt_distance_mm=belt_mm, + fx=fx, + fy=fy, + cx=cx, + cy=cy, + background_mm=background, + min_object_height_mm=hmin, + ) + if m is not None and is_plausible_measurement(m): + measurements.append(m) + + # плоские товары — только если detect_flat_rgb: true (иначе тени → ложные C) + if ( + detect_flat_rgb + and color_background is not None + and not pair.color_is_depth_preview + ): + for mask, contour in segment_rgb_objects( + pair.color_bgr, + color_background, + min_area_px=min_area, + diff_threshold=int(cfg.get("rgb_diff_threshold", 35)), + max_objects=max(0, max_objects - len(measurements)), + exclude_mask=depth_union, + ): + m = measure_flat_object( + pair.depth_mm, + mask, + contour, + belt_distance_mm=belt_mm, + fx=fx, + fy=fy, + cx=cx, + cy=cy, + background_mm=background, + color_bgr=pair.color_bgr, + color_bg_bgr=color_background, + ) + if m is not None and is_plausible_measurement(m): + measurements.append(m) + + measurements = merge_overlapping_measurements(measurements, overlap_thr=0.5) + tracks, events = tracker.update(measurements, min_mm=min_mm, max_mm=max_mm) + + rgb_available = not pair.color_is_depth_preview + if rgb_available and float(np.std(pair.color_bgr)) > 4.0: + base_img = pair.color_bgr + if base_img.shape[:2] != pair.depth_mm.shape[:2]: + base_img = cv2.resize(base_img, (pair.depth_mm.shape[1], pair.depth_mm.shape[0])) + base_img = align_color(base_img, *color_align) + else: + # без живого RGB — colorize(depth), иначе веб был бы чёрным + from camera import depth_colormap + base_img = depth_colormap(pair.depth_mm) + rgb_available = False + + crops: Dict[int, Optional[str]] = {} + for tr in tracks: + if tr.measurement is not None: + crops[tr.track_id] = crop_b64(base_img, tr.measurement.contour) + + crop_updates: Dict[int, str] = {} + feed_add = [] + for ev in events: + r = ev.decision.result + tag = "UNCERTAIN→" if ev.decision.uncertain else "LOCK " + print( + f"[demo] #{ev.track_id} {tag}{r.category.zone} | {r.category.short_label} | " + f"LWH={tuple(round(x, 1) for x in r.dims_sorted_mm)} | circle={r.circle_ratio:.3f}" + ) + append_decision( + decisions_log, r, + uncertain=ev.decision.uncertain, source="demo", track_id=ev.track_id, + ) + L, W, H = r.dims_sorted_mm + tr_ev = next((t for t in tracks if t.track_id == ev.track_id), None) + cx, cy = (tr_ev.centroid if tr_ev else (0, 0)) + sk = slot_key(cx, cy, L, W, H, r.category.zone) + crop = crops.get(ev.track_id) + if crop: + crop_updates[ev.track_id] = crop + feed_add.append({ + "slot": sk, + "id": ev.track_id, + "time": time.strftime("%H:%M:%S"), + "zone": r.category.zone, + "label": r.category.short_label, + "dims": [round(x, 1) for x in r.dims_sorted_mm], + "ratio": round(r.circle_ratio, 3), + "uncertain": bool(ev.decision.uncertain), + "crop": crop, + }) + + with STATE.lock: + STATE.crop_cache.update(crop_updates) + crop_cache = dict(STATE.crop_cache) + + # статус для веба: список объектов + «главный» (первый залоченный) + objects_json = [] + primary = None + for tr in tracks: + d = tr.decision + m = tr.measurement + if d is None or m is None: + continue + is_locked = bool(d.locked and d.result is not None) + if is_locked: + obj = { + "id": tr.track_id, + "locked": True, + "uncertain": bool(d.uncertain), + "conf": 100, + "zone": d.result.category.zone, + "label": d.result.category.short_label, + "dims": [round(x, 1) for x in d.result.dims_sorted_mm], + "ratio": round(d.result.circle_ratio, 3), + "reason": d.result.reason, + "crop": crop_cache.get(tr.track_id), + } + else: + obj = { + "id": tr.track_id, + "locked": False, + "uncertain": False, + "conf": d.confidence_pct, + "zone": None, + "label": "анализ…", + "dims": [round(m.length_mm, 1), round(m.width_mm, 1), round(m.height_mm, 1)], + "ratio": round(m.circle_ratio, 3), + "reason": "", + "crop": None, + } + objects_json.append(obj) + if primary is None or (obj["locked"] and not primary["locked"]): + primary = obj + + with STATE.lock: + STATE.objects = objects_json + STATE.stats = dict(tracker.stats) + if feed_add: + fresh = [it for it in feed_add if it["slot"] not in STATE.feed_slots] + for it in fresh: + STATE.feed_slots.add(it["slot"]) + if fresh: + STATE.feed.extend(fresh) + STATE.feed = STATE.feed[-20:] + STATE.feed_seq += 1 + if primary is not None: + STATE.confidence_now = primary["conf"] + STATE.locked = primary["locked"] + STATE.uncertain = primary["uncertain"] + STATE.zone = primary["zone"] + STATE.circle_ratio = primary["ratio"] + STATE.dims = tuple(primary["dims"]) + STATE.reason = primary["reason"] or ( + f"накопление {primary['conf']}% → ждём LOCK" if not primary["locked"] else "" + ) + else: + STATE.confidence_now = 0 + STATE.locked = False + STATE.uncertain = False + STATE.zone = None + STATE.circle_ratio = None + STATE.dims = None + STATE.reason = "" + + hud = build_demo_frame( + base_img, + pair.depth_mm, + tracks, + belt_mm, + stats=tracker.stats, + confidence_pct=conf_pct, + rgb_available=rgb_available, + background_active=background is not None, + color_align=(0.0, 0.0, 1.0), + contour_smoother=contour_smoother, + ) + ok, buf = cv2.imencode(".jpg", hud, [int(cv2.IMWRITE_JPEG_QUALITY), 80]) + if ok: + jpeg = buf.tobytes() + with STATE.lock: + STATE.jpeg = jpeg + if frame_i % 3 == 0: + out_jpg.write_bytes(jpeg) + + if seg_n > len(measurements) and STATE.last_print != "seg_drop": + print(f"[demo] depth: контуров {seg_n}, измерено {len(measurements)} " + f"(часть отфильтрована: низкая высота < {hmin:.0f} мм или шум)") + STATE.last_print = "seg_drop" + elif not tracks and STATE.last_print != "empty": + print("[demo] объектов нет") + STATE.last_print = "empty" + elif tracks and STATE.last_print in ("seg_drop", "empty"): + STATE.last_print = "" + + time.sleep(0.03) + except KeyboardInterrupt: + print("\n[demo] stop") + finally: + server.shutdown() + cam.release() + return 0 + + +if __name__ == "__main__": + sys.path.insert(0, str(Path(__file__).resolve().parent)) + raise SystemExit(main()) diff --git a/cv/demo.sh b/cv/demo.sh new file mode 100755 index 0000000..c1decbf --- /dev/null +++ b/cv/demo.sh @@ -0,0 +1,19 @@ +#!/usr/bin/env bash +set -euo pipefail +DIR="$(cd "$(dirname "$0")" && pwd)" +cd "$DIR" + +if [[ ! -f config.yaml && -f config.example.yaml ]]; then + cp config.example.yaml config.yaml + echo "[cv] created config.yaml from config.example.yaml (MQTT disabled)" +fi + +if [[ ! -d .venv ]]; then + python3 -m venv .venv + .venv/bin/pip install -U pip + .venv/bin/pip install -r requirements.txt +fi + +echo "Демо классификации (без моторов/серво)" +echo "Браузер: http://127.0.0.1:8080/" +exec .venv/bin/python demo.py "$@" diff --git a/cv/demo_hud.py b/cv/demo_hud.py new file mode 100644 index 0000000..e6635f1 --- /dev/null +++ b/cv/demo_hud.py @@ -0,0 +1,302 @@ +"""HUD демо: RGB-подложка + depth, несколько объектов с ID, кириллица через PIL.""" + +from __future__ import annotations + +from functools import lru_cache +from typing import Dict, List, Optional, Tuple + +import cv2 +import numpy as np +from PIL import Image, ImageDraw, ImageFont + +ZONE_COLOR = { # BGR + "B": (40, 180, 40), + "C": (40, 40, 220), + "D": (0, 165, 255), +} +UNCERTAIN_COLOR = (0, 130, 250) # оранжевый +PENDING_COLOR = (0, 255, 255) # жёлтый — идёт накопление + +_FONT_CANDIDATES = [ + "/usr/share/fonts/noto/NotoSans-Bold.ttf", + "/usr/share/fonts/noto/NotoSans-Regular.ttf", + "/usr/share/fonts/TTF/DejaVuSans-Bold.ttf", + "/usr/share/fonts/truetype/dejavu/DejaVuSans-Bold.ttf", +] + + +@lru_cache(maxsize=8) +def _font(size: int) -> ImageFont.FreeTypeFont: + for path in _FONT_CANDIDATES: + try: + return ImageFont.truetype(path, size) + except OSError: + continue + return ImageFont.load_default() + + +def _draw_texts( + img_bgr: np.ndarray, + texts: List[Tuple[int, int, str, Tuple[int, int, int], int]], +) -> np.ndarray: + """texts: (x, y, строка, цвет BGR, размер). Кириллица через PIL.""" + if not texts: + return img_bgr + pil = Image.fromarray(cv2.cvtColor(img_bgr, cv2.COLOR_BGR2RGB)) + draw = ImageDraw.Draw(pil) + for x, y, s, bgr, size in texts: + rgb = (bgr[2], bgr[1], bgr[0]) + draw.text((x, y), s, font=_font(size), fill=rgb, stroke_width=2, stroke_fill=(0, 0, 0)) + return cv2.cvtColor(np.array(pil), cv2.COLOR_RGB2BGR) + + +class DepthSmoother: + """Временное сглаживание depth только для отображения (не для измерений).""" + + def __init__(self, alpha: float = 0.25) -> None: + self.alpha = float(alpha) + self._acc: Optional[np.ndarray] = None + + def update(self, depth_mm: np.ndarray) -> np.ndarray: + d = depth_mm.astype(np.float32) + if self._acc is None or self._acc.shape != d.shape: + self._acc = d.copy() + valid = d > 0 + self._acc[valid] = (1.0 - self.alpha) * self._acc[valid] + self.alpha * d[valid] + out = self._acc.astype(np.uint16) + out[~valid & (self._acc <= 0)] = 0 + return out + + +class ContourSmoother: + """Стабильная окантовка: EMA маски по каждому треку + аппроксимация контура, + плюс «примагничивание» контура к краям объекта на RGB (снимает остаточный + параллакс depth↔color и распухание depth-маски). + + Только для отрисовки — измерения идут по сырому контуру. + """ + + def __init__(self, alpha: float = 0.3, snap_alpha: float = 0.35) -> None: + self.alpha = float(alpha) + self.snap_alpha = float(snap_alpha) + self._acc: Dict[int, np.ndarray] = {} + self._snap: Dict[int, Tuple[float, float, float]] = {} # tid -> (dx, dy, shrink) + + def smooth( + self, track_id: int, mask: np.ndarray, edge_img: Optional[np.ndarray] = None + ) -> Optional[np.ndarray]: + m = mask.astype(np.float32) / 255.0 + acc = self._acc.get(track_id) + if acc is None or acc.shape != m.shape: + acc = m.copy() + else: + acc = (1.0 - self.alpha) * acc + self.alpha * m + self._acc[track_id] = acc + + soft = cv2.GaussianBlur((acc * 255.0).astype(np.uint8), (11, 11), 0) + _, binm = cv2.threshold(soft, 127, 255, cv2.THRESH_BINARY) + contours, _ = cv2.findContours(binm, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + if not contours: + return None + contour = max(contours, key=cv2.contourArea) + + pts = contour.reshape(-1, 2).astype(np.float32) + if edge_img is not None and pts.shape[0] >= 8: + pts = self._snap_to_edges(track_id, pts, edge_img) + + contour = pts.reshape(-1, 1, 2).astype(np.int32) + eps = 0.008 * cv2.arcLength(contour, True) + return cv2.approxPolyDP(contour, eps, True) + + def _snap_to_edges( + self, track_id: int, pts: np.ndarray, edge: np.ndarray + ) -> np.ndarray: + """Локальный поиск сдвига (±14 px) и поджатия контура, при которых под + контуром максимум RGB-краёв. Найденная поправка сглаживается по времени.""" + H, W = edge.shape[:2] + xs, ys = pts[:, 0], pts[:, 1] + + def score(dx: float, dy: float, f: float, c: np.ndarray) -> float: + x = np.clip((c[0] + f * (xs - c[0]) + dx).astype(np.int32), 0, W - 1) + y = np.clip((c[1] + f * (ys - c[1]) + dy).astype(np.int32), 0, H - 1) + return float(edge[y, x].mean()) - 0.6 * float(np.hypot(dx, dy)) + + c = pts.mean(axis=0) + best_dx, best_dy, best_s = 0.0, 0.0, score(0, 0, 1.0, c) + for dy in range(-14, 15, 2): + for dx in range(-14, 15, 2): + s = score(dx, dy, 1.0, c) + if s > best_s: + best_s, best_dx, best_dy = s, float(dx), float(dy) + best_f = 1.0 + # только сдвиг и лёгкое РАСШИРЕНИЕ — поджатие (f<1) отрезало часть объекта + for f in (1.0, 1.04, 1.08): + s = score(best_dx, best_dy, f, c) + if s > best_s: + best_s, best_f = s, f + + prev = self._snap.get(track_id, (0.0, 0.0, 1.0)) + a = self.snap_alpha + sm = ( + (1 - a) * prev[0] + a * best_dx, + (1 - a) * prev[1] + a * best_dy, + max(1.0, (1 - a) * prev[2] + a * best_f), + ) + self._snap[track_id] = sm + out = pts.copy() + out[:, 0] = c[0] + sm[2] * (xs - c[0]) + sm[0] + out[:, 1] = c[1] + sm[2] * (ys - c[1]) + sm[1] + return out + + def drop_missing(self, alive_ids: set) -> None: + for tid in list(self._acc.keys()): + if tid not in alive_ids: + del self._acc[tid] + self._snap.pop(tid, None) + + +def align_color(color_bgr: np.ndarray, dx: float, dy: float, scale: float) -> np.ndarray: + """Совмещение RGB с depth: сдвиг+масштаб (у D415 сенсоры разнесены).""" + if abs(dx) < 0.5 and abs(dy) < 0.5 and abs(scale - 1.0) < 1e-3: + return color_bgr + h, w = color_bgr.shape[:2] + M = np.float32([ + [scale, 0, dx + (1.0 - scale) * w / 2.0], + [0, scale, dy + (1.0 - scale) * h / 2.0], + ]) + return cv2.warpAffine(color_bgr, M, (w, h), flags=cv2.INTER_LINEAR) + + +def build_demo_frame( + color_bgr: np.ndarray, + depth_mm: np.ndarray, + tracks: list, # List[tracker.Track] + belt_mm: float, + stats: Optional[Dict[str, int]] = None, + confidence_pct: int = 75, + rgb_available: bool = False, + background_active: bool = False, + color_align: Tuple[float, float, float] = (0.0, 0.0, 1.0), + contour_smoother: Optional[ContourSmoother] = None, +) -> np.ndarray: + from camera import depth_colormap + + # RGB для веба; если цвет недоступен — colorize(depth), НЕ чёрный экран + if rgb_available and color_bgr is not None and float(np.std(color_bgr)) > 4.0: + base = color_bgr + if base.shape[:2] != depth_mm.shape[:2]: + base = cv2.resize(base, (depth_mm.shape[1], depth_mm.shape[0])) + base = align_color(base, color_align[0], color_align[1], color_align[2]) + view = base.copy() + rgb_ok = True + else: + view = depth_colormap(depth_mm) + view = cv2.medianBlur(view, 3) + base = view + rgb_ok = False + h, w = view.shape[:2] + + # мягкое поле RGB-краёв для «примагничивания» контуров + edge_field: Optional[np.ndarray] = None + if rgb_ok and contour_smoother is not None and tracks: + gray = cv2.cvtColor(base, cv2.COLOR_BGR2GRAY) + gray = cv2.createCLAHE(clipLimit=3.0, tileGridSize=(8, 8)).apply(gray) + edge_field = cv2.Canny(cv2.GaussianBlur(gray, (5, 5), 0), 40, 120) + edge_field = cv2.GaussianBlur(edge_field, (13, 13), 0) + + texts: List[Tuple[int, int, str, Tuple[int, int, int], int]] = [] + max_conf_pending = 0 + all_locked = bool(tracks) + alive_ids = set() + + for tr in tracks: + m = tr.measurement + d = tr.decision + if m is None or d is None: + continue + alive_ids.add(tr.track_id) + locked = d.locked and d.result is not None + if locked: + color = UNCERTAIN_COLOR if d.uncertain else ZONE_COLOR.get(d.result.category.zone, PENDING_COLOR) + else: + color = PENDING_COLOR + all_locked = False + max_conf_pending = max(max_conf_pending, d.confidence_pct) + + # bbox / класс — по сырому контуру измерения; сглаживание только для окантовки «анализ» + raw_contour = m.contour + draw_contour = raw_contour + if not locked and contour_smoother is not None: + sm = contour_smoother.smooth(tr.track_id, m.mask, edge_img=edge_field) + if sm is not None: + draw_contour = sm + if locked: + bx, by, bw, bh = cv2.boundingRect(raw_contour) + # небольшой запас, чтобы рамка не обрезала края + pad = 4 + bx, by = max(0, bx - pad), max(0, by - pad) + bw = min(w - bx, bw + 2 * pad) + bh = min(h - by, bh + 2 * pad) + cv2.rectangle(view, (bx, by), (bx + bw, by + bh), color, 2, lineType=cv2.LINE_AA) + cl = max(8, min(bw, bh) // 5) + for px, py, sx, sy in ((bx, by, 1, 1), (bx + bw, by, -1, 1), + (bx, by + bh, 1, -1), (bx + bw, by + bh, -1, -1)): + cv2.line(view, (px, py), (px + sx * cl, py), color, 4, lineType=cv2.LINE_AA) + cv2.line(view, (px, py), (px, py + sy * cl), color, 4, lineType=cv2.LINE_AA) + contour = raw_contour + else: + cv2.drawContours(view, [draw_contour], -1, color, 2, lineType=cv2.LINE_AA) + contour = draw_contour + ccx, ccy = contour.reshape(-1, 2).mean(axis=0) + cv2.circle(view, (int(ccx), int(ccy)), 4, (0, 0, 255), -1, lineType=cv2.LINE_AA) + + x0, y0, _, _ = cv2.boundingRect(contour) + tx = int(np.clip(x0, 4, w - 220)) + ty = int(np.clip(y0 - 46, 4, h - 46)) + if locked: + label = d.result.category.short_label + if d.uncertain: + label = "НЕУВЕРЕННО → " + label + texts.append((tx, ty, f"#{tr.track_id} {label}", color, 20)) + else: + texts.append((tx, ty, f"#{tr.track_id} анализ… {d.confidence_pct}%", color, 20)) + dims = d.result.dims_sorted_mm if (locked and d.result) else (m.length_mm, m.width_mm, m.height_mm) + ratio = d.result.circle_ratio if (locked and d.result) else m.circle_ratio + src = " · RGB" if getattr(m, "source", "depth") == "rgb" else "" + texts.append( + (tx, ty + 24, f"{dims[0]:.0f}×{dims[1]:.0f}×{dims[2]:.0f} мм · круг {ratio:.2f}{src}", (235, 235, 235), 15) + ) + + if contour_smoother is not None: + contour_smoother.drop_missing(alive_ids) + + view = _draw_texts(view, texts) + + # надписи — в отдельной полосе НАД кадром, чтобы не закрывать камеру + top_bar = np.full((36, w, 3), 18, np.uint8) + st = stats or {} + stats_line = ( + f"ГОТОВ {st.get('B', 0)} · НЕГАБАРИТ {st.get('C', 0)} · ДОУПАК {st.get('D', 0)}" + + (f" · неувер. {st['uncertain']}" if st.get("uncertain") else "") + ) + bg_tag = " · фон:карта" if background_active else "" + left = "объектов нет" if not tracks else f"объектов: {len(alive_ids)}" + top_texts = [ + (10, 6, left, (150, 150, 255) if not tracks else (200, 230, 200), 17), + (max(200, w - 440), 8, f"{stats_line} | h={belt_mm:.0f}мм{bg_tag}", (200, 230, 200), 14), + ] + top_bar = _draw_texts(top_bar, top_texts) + view = np.vstack([top_bar, view]) + h = view.shape[0] + + # прогресс уверенности внизу + bar_y = h - 8 + cv2.rectangle(view, (0, bar_y), (w, h), (40, 40, 40), -1) + conf_show = 100 if (all_locked and tracks) else max_conf_pending + fill = int(w * min(1.0, conf_show / 100.0)) + col = (40, 200, 40) if (all_locked and tracks) else (0, 200, 255) + cv2.rectangle(view, (0, bar_y), (fill, h), col, -1) + thr = int(w * confidence_pct / 100.0) + cv2.line(view, (thr, bar_y), (thr, h), (255, 255, 255), 1) + + return view diff --git a/cv/docker-compose.yml b/cv/docker-compose.yml new file mode 100644 index 0000000..ed25f19 --- /dev/null +++ b/cv/docker-compose.yml @@ -0,0 +1,25 @@ +services: + vision: + build: . + container_name: realsense_vision + restart: unless-stopped + network_mode: host + privileged: true + devices: + - /dev/video0:/dev/video0 + - /dev/video1:/dev/video1 + - /dev/video2:/dev/video2 + - /dev/video3:/dev/video3 + - /dev/video4:/dev/video4 + - /dev/video5:/dev/video5 + volumes: + # Copy config.example.yaml → config.yaml locally before enabling MQTT/hardware. + - ./config.yaml:/app/config.yaml:ro + - ./debug_frames:/app/debug_frames + # Prefer: command with --no-mqtt until credentials are configured. + group_add: + - video + environment: + - QT_X11_NO_MITSHM=1 + # Без GUI в контейнере по умолчанию; превью — через native run + command: ["python", "main.py", "-c", "config.yaml"] diff --git a/cv/journal.py b/cv/journal.py new file mode 100644 index 0000000..29520c9 --- /dev/null +++ b/cv/journal.py @@ -0,0 +1,47 @@ +"""JSONL-журнал решений классификатора — метрики для отчёта и защиты. + +Каждая строка — одно зафиксированное решение (LOCK): +время, зона, габариты, circle_ratio, флаг «неуверенно», причина. + +Анализ (корректность, доля неуверенных, распределение зон): + .venv/bin/python -c " + import json; + rows=[json.loads(l) for l in open('logs/decisions.jsonl')]; + from collections import Counter; + print(Counter(r['zone'] for r in rows)); + print('uncertain:', sum(r['uncertain'] for r in rows), '/', len(rows))" +""" + +from __future__ import annotations + +import json +import time +from pathlib import Path + +from classify import ClassificationResult + + +def append_decision( + path: str | Path, + result: ClassificationResult, + uncertain: bool = False, + source: str = "main", + track_id: int | None = None, +) -> None: + entry = { + "track_id": track_id, + "ts": time.strftime("%Y-%m-%dT%H:%M:%S"), + "ts_ms": int(time.time() * 1000), + "zone": result.category.zone, + "category": result.category.value, + "label_ru": result.category.ru_label, + "dims_mm": [round(float(x), 1) for x in result.dims_sorted_mm], + "circle_ratio": round(float(result.circle_ratio), 4), + "uncertain": bool(uncertain), + "reason": result.reason, + "source": source, + } + p = Path(path) + p.parent.mkdir(parents=True, exist_ok=True) + with open(p, "a", encoding="utf-8") as f: + f.write(json.dumps(entry, ensure_ascii=False) + "\n") diff --git a/cv/main.py b/cv/main.py new file mode 100755 index 0000000..4fff93d --- /dev/null +++ b/cv/main.py @@ -0,0 +1,330 @@ +#!/usr/bin/env python3 +""" +Алгоритмическая часть трека 3: Intel RealSense D415 на Orange PI. + +Пайплайн: + depth+color → сегментация объекта на ленте → габариты L×W×H + circle_ratio + → классификация (B/C/D) → MQTT → сервоприводы Arduino (без правок arduino_code). +""" + +from __future__ import annotations + +import argparse +import os +import sys +import time +from pathlib import Path +from typing import Any, Dict, Optional + +import cv2 +import yaml + +from camera import RealSenseV4L2, depth_colormap +from classify import Category, ClassificationResult +from journal import append_decision +from measure import measure_flat_object, measure_object, segment_objects, segment_rgb_objects, is_plausible_measurement +from mqtt_bridge import MqttBridge +from stabilize import DecisionStabilizer + +ZONE_TO_CATEGORY = { + "B": Category.SUITABLE, + "C": Category.OVERSIZE, + "D": Category.NEED_PACK, +} + + +def resolve_config_path(path: Path) -> Path: + if path.exists(): + return path + example = path.with_name("config.example.yaml") + if example.exists(): + return example + raise FileNotFoundError(f"Config not found: {path} (and no config.example.yaml)") + + +def load_config(path: Path) -> Dict[str, Any]: + resolved = resolve_config_path(Path(path)) + with open(resolved, "r", encoding="utf-8") as f: + return yaml.safe_load(f) + + +def draw_overlay( + color_bgr, + depth_mm, + measurement, + result: Optional[ClassificationResult], + belt_mm: float, +): + vis = color_bgr.copy() + depth_vis = depth_colormap(depth_mm) + if measurement is not None: + cv2.drawContours(vis, [measurement.contour], -1, (0, 255, 0), 2) + cx, cy = measurement.centroid_px + cv2.circle(vis, (cx, cy), 4, (0, 0, 255), -1) + lines = [ + f"L={measurement.length_mm:.0f} W={measurement.width_mm:.0f} H={measurement.height_mm:.0f} mm", + f"circle_ratio={measurement.circle_ratio:.3f}", + ] + if result is not None: + lines.append(f"{result.category.zone}: {result.category.ru_label}") + y = 24 + for line in lines: + cv2.putText(vis, line, (10, y), cv2.FONT_HERSHEY_SIMPLEX, 0.55, (20, 20, 20), 3, cv2.LINE_AA) + cv2.putText(vis, line, (10, y), cv2.FONT_HERSHEY_SIMPLEX, 0.55, (0, 255, 255), 1, cv2.LINE_AA) + y += 22 + cv2.putText( + vis, + f"belt={belt_mm:.0f}mm", + (10, vis.shape[0] - 12), + cv2.FONT_HERSHEY_SIMPLEX, + 0.5, + (200, 200, 200), + 1, + cv2.LINE_AA, + ) + return vis, depth_vis + + +def main() -> int: + parser = argparse.ArgumentParser(description="RealSense D415 classifier for hackathon track 3") + parser.add_argument( + "-c", + "--config", + default=str(Path(__file__).with_name("config.yaml")), + help="Путь к config.yaml", + ) + parser.add_argument("--once", action="store_true", help="Один кадр и выход") + parser.add_argument("--no-mqtt", action="store_true", help="Не публиковать в MQTT") + parser.add_argument( + "--preview", + action="store_true", + help="Живое превью в debug_frames/live_*.jpg (без GTK-окон)", + ) + parser.add_argument("--dry-route", action="store_true", help="Не двигать серво") + parser.add_argument("--no-motor", action="store_true", help="Не включать шаговик ленты") + args = parser.parse_args() + + cfg = load_config(Path(args.config)) + cam_cfg = cfg["camera"] + cls_cfg = cfg["classification"] + rt = cfg.get("runtime", {}) + mqtt_cfg = dict(cfg.get("mqtt", {})) + routing_cfg = dict(cfg.get("routing", {})) + motor_cfg = dict(cfg.get("motor", {})) + if args.no_mqtt: + mqtt_cfg["enabled"] = False + if args.dry_route: + routing_cfg["enabled"] = False + if args.no_motor: + motor_cfg["enabled"] = False + mqtt_cfg["_routing"] = routing_cfg + mqtt_cfg["_motor"] = motor_cfg + + show_preview = args.preview or bool(rt.get("show_preview", False)) + save_debug = bool(rt.get("save_debug_frames", False)) or show_preview + debug_dir = Path(rt.get("debug_dir", "debug_frames")) + if save_debug or show_preview: + debug_dir.mkdir(parents=True, exist_ok=True) + live_color = debug_dir / "live_color.jpg" + live_depth = debug_dir / "live_depth.jpg" + preview_every = max(1, int(rt.get("preview_every_n", 3))) + + print("[vision] открываю RealSense D415…") + cam = RealSenseV4L2( + depth_device=cam_cfg.get("depth_device", "/dev/video0"), + color_device=cam_cfg.get("color_device", "/dev/video4"), + width=int(cam_cfg.get("width", 640)), + height=int(cam_cfg.get("height", 480)), + fps=int(cam_cfg.get("fps", 30)), + depth_scale_mm=float(cam_cfg.get("depth_scale_mm", 1.0)), + use_color=bool(cfg.get("use_color", False)), + ) + + belt_mm = float(cfg.get("belt_distance_mm") or 0) + if belt_mm <= 0: + print("[vision] калибровка плоскости ленты (уберите объекты)…") + belt_mm = cam.estimate_belt_distance_mm() + print(f"[vision] belt_distance_mm ≈ {belt_mm:.1f}") + else: + print(f"[vision] belt_distance_mm из конфига: {belt_mm:.1f}") + + background = None + color_background = None + if bool(cfg.get("use_background_map", False)): + print("[vision] снимаю фоновую карту сцены — лента должна быть ПУСТОЙ…") + try: + background = cam.capture_background(samples=15) + print("[vision] фоновая карта активна (сегментация относительно фона)") + except RuntimeError as exc: + print(f"[vision] фоновая карта не снята ({exc}), работаю по скалярной высоте") + color_background = cam.capture_background_rgb(samples=10) + if color_background is not None: + print("[vision] RGB-фон снят — плоские товары (телефон) будут детектироваться") + + bridge = MqttBridge(mqtt_cfg) + print(f"[vision] MQTT: {'OK' if bridge.connected else 'offline/disabled'}") + if bridge.connected: + bridge.start_conveyor() + if show_preview: + print(f"[vision] превью → {live_color} и {live_depth} (обновляются на лету)") + print("[vision] откройте файлы в IDE/файловом менеджере или: eog debug_frames/live_color.jpg") + + confirm_need = int(rt.get("confirm_frames", 8)) + process_every_n = max(1, int(rt.get("process_every_n", 1))) + frame_i = 0 + thr = float(cls_cfg.get("circle_ratio_threshold", 0.8)) + fallback_zone = str(cls_cfg.get("uncertain_fallback_zone", "C")).upper() + stabilizer = DecisionStabilizer( + window=12, + confirm_frames=confirm_need, + lost_frames=12, + enter_circle=thr, + exit_circle=thr - 0.08, + uncertain_after=int(cls_cfg.get("uncertain_after_frames", 45)), + fallback=ZONE_TO_CATEGORY.get(fallback_zone, Category.OVERSIZE), + ) + last_routed_zone: Optional[str] = None + decisions_log = Path(rt.get("decisions_log", "logs/decisions.jsonl")) + + fx, fy = float(cam_cfg["fx"]), float(cam_cfg["fy"]) + cx, cy = float(cam_cfg["cx"]), float(cam_cfg["cy"]) + + try: + while True: + pair = cam.read() + if pair is None: + print("[vision] нет кадра", file=sys.stderr) + time.sleep(0.05) + continue + + frame_i += 1 + measurement = None + result = None + + if frame_i % process_every_n == 0: + candidates = segment_objects( + pair.depth_mm, + belt_distance_mm=belt_mm, + belt_tolerance_mm=float(cfg.get("belt_tolerance_mm", 25)), + min_object_height_mm=float(cfg.get("min_object_height_mm", 5)), + min_area_px=int(cfg.get("min_object_area_px", 800)), + background_mm=background, + max_objects=int(cfg.get("max_objects_in_frame", 3)), + roi_margin=cfg.get("roi_margin"), + ) + seg = None + best = None # (score, mask, contour, measurement) + for mask, contour in candidates: + m_try = measure_object( + pair.depth_mm, + mask, + contour, + belt_distance_mm=belt_mm, + fx=fx, + fy=fy, + cx=cx, + cy=cy, + background_mm=background, + min_object_height_mm=float(cfg.get("min_object_height_mm", 8)), + roi_margin=cfg.get("roi_margin"), + ) + if m_try is None or not is_plausible_measurement(m_try): + continue + # приоритет: круглый и более высокий товар над шумом ленты + score = float(m_try.circle_ratio) * 2.0 + min(float(m_try.height_mm), 200.0) / 100.0 + if best is None or score > best[0]: + best = (score, mask, contour, m_try) + if best is not None: + _, mask, contour, measurement = best + seg = (mask, contour) + else: + measurement = None + + # depth ничего не видит → плоский товар (телефон) ищем по RGB + if ( + measurement is None + and bool(cfg.get("detect_flat_rgb", False)) + and color_background is not None + and not pair.color_is_depth_preview + ): + rgb_objs = segment_rgb_objects( + pair.color_bgr, + color_background, + min_area_px=int(cfg.get("min_object_area_px", 800)), + diff_threshold=int(cfg.get("rgb_diff_threshold", 35)), + max_objects=1, + exclude_mask=seg[0] if seg is not None else None, + ) + if rgb_objs: + mask, contour = rgb_objs[0] + measurement = measure_flat_object( + pair.depth_mm, + mask, + contour, + belt_distance_mm=belt_mm, + fx=fx, + fy=fy, + cx=cx, + cy=cy, + background_mm=background, + color_bgr=pair.color_bgr, + color_bg_bgr=color_background, + ) + + if measurement is not None and not is_plausible_measurement(measurement): + measurement = None + + decision = stabilizer.update( + measurement, + min_mm=cls_cfg.get("min_mm", [10, 10, 10]), + max_mm=cls_cfg.get("max_mm", [450, 320, 320]), + ) + if decision.locked and decision.result is not None: + result = decision.result + zone = result.category.zone + if zone != last_routed_zone: + tag = "UNCERTAIN→" if decision.uncertain else "LOCK " + print( + f"[vision] {tag}{zone} | {result.category.ru_label} | " + f"dims={result.dims_sorted_mm} | ratio={result.circle_ratio:.3f} | {result.reason}" + ) + append_decision(decisions_log, result, uncertain=decision.uncertain, source="main") + bridge.publish_result(result) + bridge.route(result.category) + last_routed_zone = zone + elif not decision.present: + last_routed_zone = None + + if show_preview or save_debug: + vis, depth_vis = draw_overlay(pair.color_bgr, pair.depth_mm, measurement, result, belt_mm) + if save_debug and result is not None and not show_preview: + out = debug_dir / f"frame_{frame_i:06d}_{result.category.value}.jpg" + cv2.imwrite(str(out), vis) + # headless OpenCV: пишем JPEG вместо cv2.imshow + if show_preview and frame_i % preview_every == 0: + cv2.imwrite(str(live_color), vis) + cv2.imwrite(str(live_depth), depth_vis) + + if args.once: + if result is not None: + print(result) + if show_preview: + vis, depth_vis = draw_overlay(pair.color_bgr, pair.depth_mm, measurement, result, belt_mm) + cv2.imwrite(str(live_color), vis) + cv2.imwrite(str(live_depth), depth_vis) + print(f"[vision] кадр сохранён: {live_color}") + break + + except KeyboardInterrupt: + print("\n[vision] stop") + finally: + bridge.close() + cam.release() + + return 0 + + +if __name__ == "__main__": + # Чтобы импорты работали и как пакет, и как скрипт + sys.path.insert(0, str(Path(__file__).resolve().parent)) + raise SystemExit(main()) diff --git a/cv/measure.py b/cv/measure.py new file mode 100644 index 0000000..fbf8951 --- /dev/null +++ b/cv/measure.py @@ -0,0 +1,719 @@ +"""Сегментация и измерение по ТЗ трека 3 — без эвристик «подкрутки».""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import List, Optional, Tuple + +import cv2 +import numpy as np + + +@dataclass +class ObjectMeasurement: + length_mm: float + width_mm: float + height_mm: float + circle_ratio: float # итог для классификатора: max устойчивых сечений + area_px: int + centroid_px: Tuple[int, int] + contour: np.ndarray + mask: np.ndarray + top_ratio: float = 0.0 # rin/rout вида сверху + section_ratio: float = 0.0 # лучший устойчивый 3D-срез + source: str = "depth" # "depth" | "rgb" (плоский товар, найден по RGB) + clipped_by_frame: bool = False # объект упирается в край кадра → габарит неполный + + +def is_plausible_measurement( + m: ObjectMeasurement, + min_footprint_mm: float = 15.0, +) -> bool: + """Отсев шума depth/RGB до классификации (не путать с ТЗ-негабаритом). + + Мелкие пятна и «0×0×0 мм» не должны попадать в трекер — иначе + check_size(<10 мм) даёт ложный класс C. + """ + L, W, H = float(m.length_mm), float(m.width_mm), float(m.height_mm) + if L <= 1.5 or W <= 1.5: + return False + if int(m.area_px) < 120: + return False + a, b, c = sorted((L, W, H), reverse=True) + if a < min_footprint_mm: + return False + if c <= 0.5: + return False + # RGB-шум: нет высоты и крошечное пятно на ленте + if getattr(m, "source", "depth") == "rgb" and H < 2.0 and a < 45.0: + return False + return True + + +def mask_iou(a: np.ndarray, b: np.ndarray) -> float: + """Классический IoU масок.""" + inter = int(np.count_nonzero((a > 0) & (b > 0))) + if inter == 0: + return 0.0 + ua = int(np.count_nonzero(a > 0)) + ub = int(np.count_nonzero(b > 0)) + return inter / float(ua + ub - inter) + + +def mask_overlap_min(a: np.ndarray, b: np.ndarray) -> float: + """Доля пересечения относительно меньшей маски (0..1). + + ≥0.5 ≈ «хотя бы половина одного объекта лежит на другом». + Удобнее IoU, когда кусок намного меньше целого. + """ + inter = int(np.count_nonzero((a > 0) & (b > 0))) + if inter == 0: + return 0.0 + ua = int(np.count_nonzero(a > 0)) + ub = int(np.count_nonzero(b > 0)) + return inter / float(max(1, min(ua, ub))) + + +def merge_overlapping_masks( + items: List[Tuple[np.ndarray, np.ndarray]], + overlap_thr: float = 0.5, + near_gap_px: int = 14, +) -> List[Tuple[np.ndarray, np.ndarray]]: + """Склеить контуры при IoU/пересечении ≥ thr или узкой дыре depth (near_gap).""" + if len(items) <= 1: + return items + items = sorted(items, key=lambda ic: cv2.contourArea(ic[1]), reverse=True) + used = [False] * len(items) + out: List[Tuple[np.ndarray, np.ndarray]] = [] + + def _near(a: np.ndarray, b: np.ndarray) -> bool: + xa, ya, wa, ha = cv2.boundingRect(a) + xb, yb, wb, hb = cv2.boundingRect(b) + g = int(near_gap_px) + return not ( + xa + wa + g < xb or xb + wb + g < xa or ya + ha + g < yb or yb + hb + g < ya + ) + + for i, (mask_i, _) in enumerate(items): + if used[i]: + continue + merged = mask_i.copy() + used[i] = True + changed = True + while changed: + changed = False + for j, (mask_j, _) in enumerate(items): + if used[j]: + continue + hit = ( + mask_overlap_min(merged, mask_j) >= overlap_thr + or mask_iou(merged, mask_j) >= overlap_thr + or _near(merged, mask_j) + ) + if hit: + merged = cv2.bitwise_or(merged, mask_j) + used[j] = True + changed = True + k = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5)) + merged = cv2.morphologyEx(merged, cv2.MORPH_CLOSE, k, iterations=1) + contours, _ = cv2.findContours(merged, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + if not contours: + continue + contour = max(contours, key=cv2.contourArea) + clean = np.zeros_like(merged) + cv2.drawContours(clean, [contour], -1, 255, thickness=-1) + out.append((clean, contour)) + return out + + +def merge_overlapping_measurements( + measurements: List[ObjectMeasurement], + overlap_thr: float = 0.5, +) -> List[ObjectMeasurement]: + """Склеить измерения с пересекающимися масками (оставить более крупное).""" + if len(measurements) <= 1: + return measurements + ms = sorted(measurements, key=lambda m: m.area_px, reverse=True) + kept: List[ObjectMeasurement] = [] + for m in ms: + drop = False + for k in kept: + if m.mask.shape != k.mask.shape: + continue + if mask_overlap_min(m.mask, k.mask) >= overlap_thr or mask_iou(m.mask, k.mask) >= overlap_thr: + drop = True + break + if not drop: + kept.append(m) + return kept + + +def apply_roi_margin( + mask: np.ndarray, + margin: Optional[dict] = None, +) -> np.ndarray: + """Обнулить края кадра (ролики, борта, плата), доли 0..1 от H/W.""" + if not margin: + return mask + h, w = mask.shape[:2] + top = int(h * float(margin.get("top", 0))) + bottom = int(h * float(margin.get("bottom", 0))) + left = int(w * float(margin.get("left", 0))) + right = int(w * float(margin.get("right", 0))) + out = mask.copy() + if top > 0: + out[:top, :] = 0 + if bottom > 0: + out[h - bottom :, :] = 0 + if left > 0: + out[:, :left] = 0 + if right > 0: + out[:, w - right :] = 0 + return out + + +def contour_touches_border( + contour: np.ndarray, + shape: Tuple[int, ...], + margin_px: int = 3, + roi_margin: Optional[dict] = None, +) -> bool: + """True если контур упирается в край кадра/ROI — реальный размер может быть больше.""" + h, w = int(shape[0]), int(shape[1]) + top = int(h * float((roi_margin or {}).get("top", 0))) + bottom = int(h * float((roi_margin or {}).get("bottom", 0))) + left = int(w * float((roi_margin or {}).get("left", 0))) + right = int(w * float((roi_margin or {}).get("right", 0))) + y0, y1 = top + margin_px, h - bottom - 1 - margin_px + x0, x1 = left + margin_px, w - right - 1 - margin_px + pts = contour.reshape(-1, 2) + xs, ys = pts[:, 0], pts[:, 1] + return bool( + (xs <= x0).any() + or (xs >= x1).any() + or (ys <= y0).any() + or (ys >= y1).any() + ) + + +def segment_objects( + depth_mm: np.ndarray, + belt_distance_mm: float, + belt_tolerance_mm: float = 25.0, + min_object_height_mm: float = 5.0, + min_area_px: int = 800, + background_mm: Optional[np.ndarray] = None, + max_objects: int = 3, + roi_margin: Optional[dict] = None, +) -> List[Tuple[np.ndarray, np.ndarray]]: + """Все объекты в кадре (крупнейшие первыми), до max_objects штук. + + Порог высоты — как раньше (строгий): мягкий «ореол» раздувал маску на ленту + и ломал габариты/круг → путаница B/C/D. + """ + valid = (depth_mm > 50) & (depth_mm < 5000) + hmin = float(min_object_height_mm) + if background_mm is not None: + bg = background_mm.astype(np.float32) + d = depth_mm.astype(np.float32) + raised = valid & (bg > 50) & (d < bg - hmin) + near_belt_band = d > (bg - 520.0) + else: + raised = valid & (depth_mm < (belt_distance_mm - hmin)) + near_belt_band = depth_mm > (belt_distance_mm - 450) + mask = (raised & near_belt_band).astype(np.uint8) * 255 + mask = apply_roi_margin(mask, roi_margin) + + k_open = cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3)) + k_close = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5)) + mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, k_open, iterations=1) + mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, k_close, iterations=2) + + contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + frame_area = mask.shape[0] * mask.shape[1] + raw: List[Tuple[np.ndarray, np.ndarray]] = [] + for contour in sorted(contours, key=cv2.contourArea, reverse=True): + area = cv2.contourArea(contour) + # крупные товары могут занимать почти весь кадр — не отсекать как шум + if area < min_area_px or area > frame_area * 0.92: + continue + clean = np.zeros_like(mask) + cv2.drawContours(clean, [contour], -1, 255, thickness=-1) + raw.append((clean, contour)) + + # только явное пересечение / узкая щель — не склеивать соседние товары + merged = merge_overlapping_masks(raw, overlap_thr=0.5, near_gap_px=14) + return merged[:max_objects] + + +def segment_object( + depth_mm: np.ndarray, + belt_distance_mm: float, + belt_tolerance_mm: float = 25.0, + min_object_height_mm: float = 5.0, + min_area_px: int = 800, + background_mm: Optional[np.ndarray] = None, + roi_margin: Optional[dict] = None, +) -> Optional[Tuple[np.ndarray, np.ndarray]]: + """Крупнейший объект (для main.py/calibrate.py — один товар в накопителе).""" + objs = segment_objects( + depth_mm, + belt_distance_mm, + belt_tolerance_mm=belt_tolerance_mm, + min_object_height_mm=min_object_height_mm, + min_area_px=min_area_px, + background_mm=background_mm, + max_objects=1, + roi_margin=roi_margin, + ) + return objs[0] if objs else None + + +def _pixel_to_xy_mm( + u: float, v: float, z_mm: float, fx: float, fy: float, cx: float, cy: float +) -> Tuple[float, float]: + return (u - cx) * z_mm / fx, (v - cy) * z_mm / fy + + +def measure_object( + depth_mm: np.ndarray, + mask: np.ndarray, + contour: np.ndarray, + belt_distance_mm: float, + fx: float, + fy: float, + cx: float, + cy: float, + background_mm: Optional[np.ndarray] = None, + min_object_height_mm: float = 5.0, + roi_margin: Optional[dict] = None, +) -> Optional[ObjectMeasurement]: + ys, xs = np.where(mask > 0) + if xs.size < 50: + return None + + z_vals = depth_mm[ys, xs].astype(np.float32) + z_vals = z_vals[z_vals > 0] + if z_vals.size < 50: + return None + + z_med = float(np.median(z_vals)) + # высота относительно локального фона (платформа/лента под объектом) + belt_local = belt_distance_mm + if background_mm is not None: + bg_vals = background_mm[ys, xs].astype(np.float32) + bg_vals = bg_vals[bg_vals > 50] + if bg_vals.size >= 50: + belt_local = float(np.median(bg_vals)) + height_mm = max(0.0, belt_local - z_med) + + pts_mm = [] + for p in contour.reshape(-1, 2): + u, v = float(p[0]), float(p[1]) + x, y = _pixel_to_xy_mm(u, v, z_med, fx, fy, cx, cy) + pts_mm.append([x, y]) + pts_mm = np.asarray(pts_mm, dtype=np.float32) + if pts_mm.shape[0] < 5: + return None + + rect = cv2.minAreaRect(pts_mm.reshape(-1, 1, 2)) + rw, rh = rect[1] + length_mm = float(max(rw, rh)) + width_mm = float(min(rw, rh)) + + touches_edge = contour_touches_border(contour, mask.shape, roi_margin=roi_margin) + # Негабарит «не влезает в кадр» только если реально занимает большую долю FOV. + # Лежачая бутылка может чуть касаться ROI — это не повод форсировать 500 мм. + span_x = float(xs.max() - xs.min()) + span_y = float(ys.max() - ys.min()) + mh = float((roi_margin or {}).get("top", 0.0)) + float((roi_margin or {}).get("bottom", 0.0)) + mw = float((roi_margin or {}).get("left", 0.0)) + float((roi_margin or {}).get("right", 0.0)) + usable_w = mask.shape[1] * max(0.5, 1.0 - mw) + usable_h = mask.shape[0] * max(0.5, 1.0 - mh) + spans_frame = (span_x >= 0.72 * usable_w) or (span_y >= 0.72 * usable_h) + clipped = bool(touches_edge and spans_frame) + + # отсев шума ленты / руки на краю (низкий «холм» большой площади → не товар) + # но не отсекаем крупные обрезанные объекты — они уйдут в C + if not clipped and height_mm < 30.0 and max(length_mm, width_mm) > 150.0: + return None + # жёсткий пол — иначе складки ленты дают ложный C + if height_mm < max(20.0, float(min_object_height_mm)): + return None + # раньше >520 отбрасывали → ложный B на негабарите в FOV; + # оставляем измерение: classify отправит в C (>450) + if max(length_mm, width_mm) > 2000.0 and not clipped: + return None + + # объект не помещается в кадр → габарит занижен; форсируем > max ТЗ + if clipped: + length_mm = max(length_mm, 500.0) + + top_ratio = rin_rout(pts_mm) + section_ratio = 0.0 + cloud = _point_cloud(depth_mm, mask, fx, fy, cx, cy) + if cloud is not None: + section_ratio = robust_section_ratio(cloud) + + # ТЗ: круг в любом сечении. Один шумный 3D-срез не считаем: + # D только если top>=0.8 ИЛИ ≥2 среза >=0.8 (внутри robust_section_ratio). + circle_ratio = max(top_ratio, section_ratio) + if clipped: + # обрезанный негабарит не классифицируем по кругу + circle_ratio = min(circle_ratio, 0.5) + + m = cv2.moments(contour) + if m["m00"] > 0: + cx_px = int(m["m10"] / m["m00"]) + cy_px = int(m["m01"] / m["m00"]) + else: + cx_px, cy_px = int(xs.mean()), int(ys.mean()) + + return ObjectMeasurement( + length_mm=length_mm, + width_mm=width_mm, + height_mm=height_mm, + circle_ratio=float(circle_ratio), + area_px=int(xs.size), + centroid_px=(cx_px, cy_px), + contour=contour, + mask=mask, + top_ratio=float(top_ratio), + section_ratio=float(section_ratio), + clipped_by_frame=bool(clipped), + ) + + +def segment_rgb_objects( + color_bgr: np.ndarray, + color_bg_bgr: np.ndarray, + min_area_px: int = 800, + diff_threshold: int = 35, + max_objects: int = 3, + exclude_mask: Optional[np.ndarray] = None, +) -> List[Tuple[np.ndarray, np.ndarray]]: + """Плоские товары (телефон и т.п.) по разнице с RGB-фоном пустой ленты. + + exclude_mask — зоны, уже найденные по depth (не дублируем объекты). + Тени (пропорциональное затемнение каналов) отбрасываются. + """ + if color_bgr.shape != color_bg_bgr.shape: + return [] + fg = color_bgr.astype(np.float32) + bg = color_bg_bgr.astype(np.float32) + gray = np.max(np.abs(fg - bg), axis=2).astype(np.uint8) + _, m = cv2.threshold(gray, int(diff_threshold), 255, cv2.THRESH_BINARY) + + ratio = (fg + 8.0) / (bg + 8.0) + r_med = np.median(ratio, axis=2) + r_spread = np.max(ratio, axis=2) - np.min(ratio, axis=2) + is_shadow = (r_med < 0.93) & (r_med > 0.38) & (r_spread < 0.14) + m[is_shadow] = 0 + + k = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5)) + m = cv2.morphologyEx(m, cv2.MORPH_OPEN, k, iterations=1) + m = cv2.morphologyEx(m, cv2.MORPH_CLOSE, k, iterations=2) + + if exclude_mask is not None: + excl = cv2.dilate(exclude_mask, cv2.getStructuringElement(cv2.MORPH_RECT, (31, 31))) + m[excl > 0] = 0 + + contours, _ = cv2.findContours(m, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + out: List[Tuple[np.ndarray, np.ndarray]] = [] + for contour in sorted(contours, key=cv2.contourArea, reverse=True): + if cv2.contourArea(contour) < min_area_px or len(out) >= max_objects: + break + clean = np.zeros_like(m) + cv2.drawContours(clean, [contour], -1, 255, thickness=-1) + out.append((clean, contour)) + return out + + +def measure_flat_object( + depth_mm: np.ndarray, + mask: np.ndarray, + contour: np.ndarray, + belt_distance_mm: float, + fx: float, + fy: float, + cx: float, + cy: float, + background_mm: Optional[np.ndarray] = None, + color_bgr: Optional[np.ndarray] = None, + color_bg_bgr: Optional[np.ndarray] = None, +) -> Optional[ObjectMeasurement]: + """Измерение товара, найденного по RGB: без отсева по минимальной высоте.""" + ys, xs = np.where(mask > 0) + if xs.size < 50: + return None + + z_plane = belt_distance_mm + if background_mm is not None: + bg_vals = background_mm[ys, xs].astype(np.float32) + bg_vals = bg_vals[bg_vals > 50] + if bg_vals.size >= 50: + z_plane = float(np.median(bg_vals)) + + z_vals = depth_mm[ys, xs].astype(np.float32) + z_vals = z_vals[z_vals > 0] + height_mm = max(0.0, z_plane - float(np.median(z_vals))) if z_vals.size >= 50 else 0.0 + + pts_mm = [] + for p in contour.reshape(-1, 2): + x, y = _pixel_to_xy_mm(float(p[0]), float(p[1]), z_plane, fx, fy, cx, cy) + pts_mm.append([x, y]) + pts_mm = np.asarray(pts_mm, dtype=np.float32) + if pts_mm.shape[0] < 3: + return None + + rect = cv2.minAreaRect(pts_mm.reshape(-1, 1, 2)) + rw, rh = rect[1] + length_mm = float(max(rw, rh)) + width_mm = float(min(rw, rh)) + if max(length_mm, width_mm) > 520.0 or max(length_mm, width_mm) < 5.0: + return None + + top_ratio = rin_rout(pts_mm) + + m = cv2.moments(contour) + if m["m00"] > 0: + cx_px, cy_px = int(m["m10"] / m["m00"]), int(m["m01"] / m["m00"]) + else: + cx_px, cy_px = int(xs.mean()), int(ys.mean()) + + return ObjectMeasurement( + length_mm=length_mm, + width_mm=width_mm, + height_mm=float(height_mm), + circle_ratio=float(top_ratio), + area_px=int(xs.size), + centroid_px=(cx_px, cy_px), + contour=contour, + mask=mask, + top_ratio=float(top_ratio), + section_ratio=0.0, + source="rgb", + ) + + +def rin_rout(pts_xy: np.ndarray) -> float: + """ТЗ: r_in / r_out по выпуклой оболочке сечения.""" + pts = np.asarray(pts_xy, dtype=np.float32).reshape(-1, 2) + if pts.shape[0] < 3: + return 0.0 + + hull = cv2.convexHull(pts.reshape(-1, 1, 2)) + hull_pts = hull.reshape(-1, 2) + if hull_pts.shape[0] < 3: + return 0.0 + + (_center, r_out) = cv2.minEnclosingCircle(hull) + r_out = float(r_out) + if r_out < 1e-6: + return 0.0 + + r_in = _inscribed_radius_mm(hull_pts) + if r_in <= 0: + return 0.0 + return float(np.clip(r_in / r_out, 0.0, 1.0)) + + +def robust_section_ratio(cloud: np.ndarray, thr: float = 0.8) -> float: + """ + Поперечные срезы. Из логов: у круга часто 1–2 среза ≥0.85, иногда медиана падает. + - ≥1 срез с score≥0.85 → принимаем (уверенный круг/дуга); + - иначе ≥2 среза ≥0.8 → max; + - иначе медиана (антишум для коробки). + """ + ratios = _section_ratios_3d(cloud) + if not ratios: + return 0.0 + very = [r for r in ratios if r >= 0.85] + if very: + return float(max(very)) + strong = [r for r in ratios if r >= thr] + if len(strong) >= 2: + return float(max(strong)) + return float(np.median(ratios)) + + +def _point_cloud( + depth_mm: np.ndarray, + mask: np.ndarray, + fx: float, + fy: float, + cx: float, + cy: float, +) -> Optional[np.ndarray]: + ys, xs = np.where(mask > 0) + if xs.size < 150: + return None + z = depth_mm[ys, xs].astype(np.float32) + ok = (z > 50) & (z < 5000) + xs, ys, z = xs[ok], ys[ok], z[ok] + if xs.size < 150: + return None + # детерминированный даунсэмпл (без random) + if xs.size > 4000: + step = int(np.ceil(xs.size / 4000)) + xs, ys, z = xs[::step], ys[::step], z[::step] + X = (xs.astype(np.float32) - cx) * z / fx + Y = (ys.astype(np.float32) - cy) * z / fy + return np.column_stack([X, Y, z]).astype(np.float32) + + +def _section_ratios_3d(cloud: np.ndarray) -> List[float]: + mean = cloud.mean(axis=0) + centered = cloud - mean + try: + _u, s, vt = np.linalg.svd(centered, full_matrices=False) + except np.linalg.LinAlgError: + return [] + + out: List[float] = [] + # только вдоль самой длинной оси — поперечные сечения цилиндра/коробки + for axis_i in range(min(1, vt.shape[0])): + if float(s[axis_i]) < 1e-6: + continue + axis = vt[axis_i] + axis = axis / (np.linalg.norm(axis) + 1e-9) + along = centered @ axis + + ref = np.array([0.0, 0.0, 1.0], dtype=np.float32) + if abs(float(np.dot(axis, ref))) > 0.9: + ref = np.array([1.0, 0.0, 0.0], dtype=np.float32) + u = np.cross(axis, ref) + u /= np.linalg.norm(u) + 1e-9 + v = np.cross(axis, u) + + a0, a1 = float(np.percentile(along, 15)), float(np.percentile(along, 85)) + if a1 - a0 < 10.0: + continue + + for t in (0.2, 0.35, 0.5, 0.65, 0.8): + ca = a0 + t * (a1 - a0) + half = max(4.0, 0.06 * (a1 - a0)) + band = np.abs(along - ca) <= half + if int(band.sum()) < 40: + continue + pts = centered[band] + sec = np.column_stack([pts @ u, pts @ v]).astype(np.float32) + out.append(_section_score(sec)) + return out + + +def _section_score(sec: np.ndarray) -> float: + """ + Чистый rin/rout. Для дуги лежачего цилиндра (depth видит полкруга) + допускаем score 0.85 только при жёстком circle-fit: + малый residual, почти равные радиусы, покрытие ≥200°, bbox не «палка». + Прямоугольное сечение fit не проходит → остаётся rin/rout < 0.8. + """ + direct = rin_rout(sec) + if direct >= 0.8: + return float(direct) + + fit = _fit_circle_arc(sec) + if fit is None: + return float(direct) + return float(max(direct, 0.85)) + + +def _fit_circle_arc(pts: np.ndarray) -> Optional[Tuple[np.ndarray, float]]: + pts = np.asarray(pts, dtype=np.float64).reshape(-1, 2) + if pts.shape[0] < 35: + return None + + c0 = pts.mean(axis=0) + x0 = pts - c0 + try: + _u, s, _vt = np.linalg.svd(x0, full_matrices=False) + except np.linalg.LinAlgError: + return None + if s.shape[0] < 2 or float(s[0]) < 1e-6: + return None + # сечение не должно быть линией + if float(s[1] / (s[0] + 1e-9)) < 0.45: + return None + + x, y = pts[:, 0], pts[:, 1] + A = np.column_stack([2 * x, 2 * y, np.ones_like(x)]) + b = x * x + y * y + try: + sol, *_ = np.linalg.lstsq(A, b, rcond=None) + except np.linalg.LinAlgError: + return None + cx_, cy_, c = sol + r2 = c + cx_ * cx_ + cy_ * cy_ + if r2 <= 1.0: + return None + r = float(np.sqrt(r2)) + rad = np.sqrt((x - cx_) ** 2 + (y - cy_) ** 2) + rel = float(np.sqrt(np.mean((rad - r) ** 2)) / (r + 1e-9)) + rad_cv = float(rad.std() / (rad.mean() + 1e-9)) + if rel > 0.04 or rad_cv > 0.04: + return None + + bw = float(x.max() - x.min()) + bh = float(y.max() - y.min()) + aspect = max(bw, bh) / max(min(bw, bh), 1e-6) + # у круга/полукруга bbox близок к квадрату; у прямоугольника 2:1 — нет + if aspect > 1.45: + return None + if r < 0.40 * max(bw, bh) or r > 0.70 * max(bw, bh): + return None + + ang = np.arctan2(y - cy_, x - cx_) + ang = np.sort(ang) + gaps = np.diff(ang) + gaps = np.append(gaps, ang[0] + 2 * np.pi - ang[-1]) + coverage = float(2 * np.pi - gaps.max()) + if coverage < np.deg2rad(200.0): + return None + + return np.array([cx_, cy_], dtype=np.float32), r + + +def _inscribed_radius_mm(pts_xy_mm: np.ndarray, grid: int = 192) -> float: + x_min, y_min = pts_xy_mm.min(axis=0) + x_max, y_max = pts_xy_mm.max(axis=0) + span = max(float(x_max - x_min), float(y_max - y_min), 1.0) + pad = 8 + inner = grid - 2 * pad + if inner < 16: + return 0.0 + scale = inner / span + + img = np.zeros((grid, grid), dtype=np.uint8) + pts_px = ((pts_xy_mm - np.array([x_min, y_min], dtype=np.float32)) * scale).astype(np.int32) + pts_px[:, 0] = np.clip(pts_px[:, 0] + pad, 0, grid - 1) + pts_px[:, 1] = np.clip(pts_px[:, 1] + pad, 0, grid - 1) + cv2.fillPoly(img, [pts_px], 255) + if img.max() == 0 or float((img > 0).mean()) > 0.98: + return 0.0 + dist = cv2.distanceTransform(img, cv2.DIST_L2, 5) + return float(dist.max()) / scale + + +def circularity_ratio(pts_xy_mm: np.ndarray) -> float: + return rin_rout(pts_xy_mm) + + +# совместимость со старыми вызовами +def max_section_circle_ratio( + depth_mm: np.ndarray, + mask: np.ndarray, + top_pts_mm: np.ndarray, + fx: float, + fy: float, + cx: float, + cy: float, +) -> float: + top = rin_rout(top_pts_mm) + cloud = _point_cloud(depth_mm, mask, fx, fy, cx, cy) + sec = robust_section_ratio(cloud) if cloud is not None else 0.0 + return float(max(top, sec)) + + +def section_rin_rout(sec: np.ndarray) -> float: + return rin_rout(sec) diff --git a/cv/mqtt_bridge.py b/cv/mqtt_bridge.py new file mode 100644 index 0000000..f351993 --- /dev/null +++ b/cv/mqtt_bridge.py @@ -0,0 +1,172 @@ +"""MQTT: публикация категории + команды серво/мотору (существующий API Arduino).""" + +from __future__ import annotations + +import json +import threading +import time +from typing import Any, Dict, Optional + +import paho.mqtt.client as mqtt + +from classify import Category, ClassificationResult + + +class MqttBridge: + def __init__(self, cfg: Dict[str, Any]) -> None: + self.cfg = cfg + self.enabled = bool(cfg.get("enabled", False)) + self.routing_cfg = cfg.get("_routing", {}) + self.motor_cfg = cfg.get("_motor", {}) + self._last_route_ts = 0.0 + self._last_category: Optional[str] = None + + self.client = mqtt.Client( + mqtt.CallbackAPIVersion.VERSION2, + client_id=cfg.get("client_id", "vision_classifier_opi"), + ) + user = cfg.get("user") + password = cfg.get("password") + if user: + self.client.username_pw_set(user, password) + + self._connected = False + if self.enabled: + try: + self.client.connect(cfg["broker"], int(cfg.get("port", 1883)), 30) + self.client.loop_start() + # короткая проверка + time.sleep(0.3) + self._connected = True + print(f"[mqtt] подключено к {cfg['broker']}:{cfg.get('port', 1883)}") + except Exception as exc: + print(f"[mqtt] не удалось подключиться: {exc}") + self._connected = False + + @property + def connected(self) -> bool: + return self._connected + + def start_conveyor(self) -> None: + """Включить шаговик ленты через уже существующие топики motor/control/*.""" + m = self.motor_cfg + if not m.get("enabled", False): + return + if not self.enabled or not self._connected: + return + rpm = int(m.get("rpm", 200)) + current = int(m.get("current_percent", 50)) + microsteps = int(m.get("microsteps", 16)) + self.client.publish("motor/control/driver", "on", qos=1) + self.client.publish("motor/control/tmc/enable", "on", qos=1) + self.client.publish("motor/control/tmc/current_percent", str(current), qos=1) + self.client.publish("motor/control/tmc/microsteps", str(microsteps), qos=1) + if m.get("stealthchop", True): + self.client.publish("motor/control/tmc/stealthchop", "on", qos=1) + self.client.publish("motor/control/rpm", str(rpm), qos=1) + print(f"[mqtt] конвейер: driver ON, rpm={rpm}") + + def stop_conveyor(self) -> None: + m = self.motor_cfg + if not m.get("enabled", False): + return + if not self.enabled or not self._connected: + return + self.client.publish("motor/control/rpm", "0", qos=1) + if m.get("disable_on_stop", False): + self.client.publish("motor/control/driver", "off", qos=1) + print("[mqtt] конвейер: rpm=0") + + def publish_result(self, result: ClassificationResult) -> None: + if not self.enabled or not self._connected: + return + l, w, h = result.dims_sorted_mm + self.client.publish( + self.cfg.get("topic_result", "vision/feedback/category"), + result.category.value, + qos=1, + ) + self.client.publish( + self.cfg.get("topic_dims", "vision/feedback/dimensions"), + f"{l:.1f},{w:.1f},{h:.1f}", + qos=0, + ) + self.client.publish( + self.cfg.get("topic_circle", "vision/feedback/circle_ratio"), + f"{result.circle_ratio:.4f}", + qos=0, + ) + payload = { + "category": result.category.value, + "zone": result.category.zone, + "label_ru": result.category.ru_label, + "dims_mm": [round(l, 1), round(w, 1), round(h, 1)], + "circle_ratio": round(result.circle_ratio, 4), + "reason": result.reason, + } + self.client.publish( + self.cfg.get("topic_debug", "vision/feedback/debug"), + json.dumps(payload, ensure_ascii=False), + qos=0, + ) + + def route(self, category: Category) -> None: + """Отправка команды серво через servo/control/{ch}/angle|enable.""" + routing = self.routing_cfg + if not routing.get("enabled", True): + return + if not self.enabled or not self._connected: + return + + now = time.time() * 1000.0 + cooldown = float(routing.get("cooldown_ms", 1500)) + if category.value == self._last_category and (now - self._last_route_ts) < cooldown: + return + + zones = routing.get("zones", {}) + zone_key = category.zone + zone = zones.get(zone_key) + if not zone: + return + + for zk, zcfg in zones.items(): + ch = int(zcfg["servo"]) + idle = int(zcfg.get("idle_angle", 0)) + if zk == zone_key: + continue + self._set_servo(ch, idle, enable=True) + + ch = int(zone["servo"]) + divert = int(zone.get("divert_angle", 90)) + idle = int(zone.get("idle_angle", 0)) + hold_ms = int(zone.get("hold_ms", 800)) + + if category == Category.SUITABLE and divert == idle: + self._set_servo(ch, idle, enable=True) + print(f"[mqtt] зона B — пропуск (servo {ch} idle)") + else: + self._set_servo(ch, divert, enable=True) + print(f"[mqtt] зона {zone_key} — divert servo {ch} → {divert}°") + + def _return_idle(channel: int = ch, angle: int = idle, delay_s: float = hold_ms / 1000.0) -> None: + time.sleep(delay_s) + self._set_servo(channel, angle, enable=True) + + threading.Thread(target=_return_idle, daemon=True).start() + + self._last_category = category.value + self._last_route_ts = now + + def _set_servo(self, channel: int, angle: int, enable: bool = True) -> None: + base = f"servo/control/{channel}" + self.client.publish(f"{base}/enable", "on" if enable else "off", qos=1) + self.client.publish(f"{base}/angle", str(int(angle)), qos=1) + + def close(self) -> None: + try: + self.stop_conveyor() + except Exception: + pass + if self._connected: + self.client.loop_stop() + self.client.disconnect() diff --git a/cv/requirements.txt b/cv/requirements.txt new file mode 100644 index 0000000..ca80870 --- /dev/null +++ b/cv/requirements.txt @@ -0,0 +1,6 @@ +opencv-python-headless>=4.8 +numpy>=1.24 +paho-mqtt>=2.0 +PyYAML>=6.0 +pillow>=10.0 # кириллица на HUD (demo_hud.py) +# ffmpeg должен быть в системе (pacman/apt: ffmpeg) — depth Z16 читается через него \ No newline at end of file diff --git a/cv/run.sh b/cv/run.sh new file mode 100755 index 0000000..a5e549f --- /dev/null +++ b/cv/run.sh @@ -0,0 +1,17 @@ +#!/usr/bin/env bash +set -euo pipefail +DIR="$(cd "$(dirname "$0")" && pwd)" +cd "$DIR" + +if [[ ! -f config.yaml && -f config.example.yaml ]]; then + cp config.example.yaml config.yaml + echo "[cv] created config.yaml from config.example.yaml (MQTT disabled)" +fi + +if [[ ! -d .venv ]]; then + python3 -m venv .venv + .venv/bin/pip install -U pip + .venv/bin/pip install -r requirements.txt +fi + +exec .venv/bin/python main.py "$@" diff --git a/cv/stabilize.py b/cv/stabilize.py new file mode 100644 index 0000000..6d326cc --- /dev/null +++ b/cv/stabilize.py @@ -0,0 +1,306 @@ +""" +Стабильный консенсус по кадрам. + +Из логов RealSense: круг даёт sec≈0.82–0.87, но early LOCK на B +залипал навсегда. Поэтому: + • классификация по медиане окна; + • LOCK после прогрева; + • апгрейд B→D при устойчивом круге (≥ confirm кадров подряд); + • D→B и смена зоны после LOCK запрещены (пока объект не исчез). +""" + +from __future__ import annotations + +from collections import Counter, deque +from dataclasses import dataclass +from typing import Deque, Optional, Sequence + +from classify import Category, ClassificationResult, check_size +from measure import ObjectMeasurement + + +@dataclass +class StableDecision: + result: Optional[ClassificationResult] + locked: bool + confidence_pct: int + ratio_smooth: float + present: bool + uncertain: bool = False # LOCK по правилу «нет консенсуса → безопасная зона» + + +class DecisionStabilizer: + def __init__( + self, + window: int = 12, + confirm_frames: int = 8, + lost_frames: int = 12, + enter_circle: float = 0.80, + exit_circle: float = 0.72, + uncertain_after: int = 45, + fallback: Category = Category.OVERSIZE, + ) -> None: + self.window = max(5, int(window)) + self.confirm_frames = max(3, int(confirm_frames)) + self.lost_frames = max(3, int(lost_frames)) + self.enter_circle = float(enter_circle) + self.exit_circle = float(exit_circle) + # нет консенсуса за uncertain_after кадров → безопасная зона (ТЗ: + # неоднозначные товары не должны идти в основной поток) + self.uncertain_after = max(self.window + 5, int(uncertain_after)) + self.fallback = fallback + + self._ratios: Deque[float] = deque(maxlen=self.window) + self._tops: Deque[float] = deque(maxlen=self.window) + self._secs: Deque[float] = deque(maxlen=self.window) + self._Ls: Deque[float] = deque(maxlen=self.window) + self._Ws: Deque[float] = deque(maxlen=self.window) + self._Hs: Deque[float] = deque(maxlen=self.window) + self._zones: Deque[str] = deque(maxlen=self.window) + + self._pending_zone: Optional[str] = None + self._pending_count: int = 0 + self._upgrade_count: int = 0 + self._locked: Optional[ClassificationResult] = None + self._miss: int = 0 + self._frames_seen: int = 0 + self._uncertain_locked: bool = False + + def reset(self) -> None: + self._ratios.clear() + self._tops.clear() + self._secs.clear() + self._Ls.clear() + self._Ws.clear() + self._Hs.clear() + self._zones.clear() + self._pending_zone = None + self._pending_count = 0 + self._upgrade_count = 0 + self._locked = None + self._miss = 0 + self._frames_seen = 0 + self._uncertain_locked = False + + def update( + self, + measurement: Optional[ObjectMeasurement], + min_mm: Sequence[float] = (10, 10, 10), + max_mm: Sequence[float] = (450, 320, 320), + ) -> StableDecision: + if measurement is None: + self._miss += 1 + if self._miss >= self.lost_frames: + self.reset() + return StableDecision(None, False, 0, 0.0, False) + if self._locked is not None: + return StableDecision( + self._locked, True, 100, self._locked.circle_ratio, True, + uncertain=self._uncertain_locked, + ) + return StableDecision(None, False, 0, 0.0, False) + + self._miss = 0 + self._frames_seen += 1 + self._ratios.append(float(measurement.circle_ratio)) + self._tops.append(float(getattr(measurement, "top_ratio", measurement.circle_ratio))) + self._secs.append(float(getattr(measurement, "section_ratio", 0.0))) + self._Ls.append(float(measurement.length_mm)) + self._Ws.append(float(measurement.width_mm)) + self._Hs.append(float(measurement.height_mm)) + + ratio = _median(self._ratios) + top_m = _median(self._tops) + sec_m = _median(self._secs) + ratio_p75 = _percentile(self._ratios, 75) + # устойчивый круг: медиана >= 0.8 ИЛИ (медиана сечений >= 0.8 и ≥ половины окна сильные) + sec_strong = ( + sum(1 for x in self._secs if x >= self.enter_circle) / max(1, len(self._secs)) + ) + circular = ratio >= self.enter_circle or ( + sec_m >= self.enter_circle and sec_strong >= 0.55 + ) + ratio_show = max(ratio, sec_m) if circular else ratio + + L, W, H = _median(self._Ls), _median(self._Ws), _median(self._Hs) + dims = tuple(sorted([L, W, H], reverse=True)) + passes = check_size(dims, min_mm, max_mm) + clipped = bool(getattr(measurement, "clipped_by_frame", False)) + if clipped: + # неполный габарит из-за края кадра → безопасный негабарит + passes = False + + if not passes: + instant = ClassificationResult( + category=Category.OVERSIZE, + dims_sorted_mm=(dims[0], dims[1], dims[2]), + circle_ratio=ratio_show, + passes_size=False, + is_circular=circular, + reason=( + "объект обрезан краем кадра → габарит неполный, считаем негабаритом" + if clipped + else "габариты вне допуска: нужно >10×10×10 и <450×320×320 мм" + ), + ) + elif circular: + instant = ClassificationResult( + category=Category.NEED_PACK, + dims_sorted_mm=(dims[0], dims[1], dims[2]), + circle_ratio=ratio_show, + passes_size=True, + is_circular=True, + reason=( + f"круг: med={ratio:.3f} p75={ratio_p75:.3f} " + f"top={top_m:.3f} sec={sec_m:.3f} strong={sec_strong:.0%} " + f">= {self.enter_circle}" + ), + ) + else: + instant = ClassificationResult( + category=Category.SUITABLE, + dims_sorted_mm=(dims[0], dims[1], dims[2]), + circle_ratio=ratio_show, + passes_size=True, + is_circular=False, + reason=( + f"не круг: med={ratio:.3f} sec={sec_m:.3f} " + f"strong={sec_strong:.0%} < {self.enter_circle}" + ), + ) + + zone = instant.category.zone + self._zones.append(zone) + + # --- уже есть LOCK --- + if self._locked is not None: + prev = self._locked.category + cur = instant.category + locked_dims = self._locked.dims_sorted_mm + + # новый объект (габариты сильно сменились) — сброс LOCK и набор заново + if _dims_changed(locked_dims, dims, rel=0.28): + self._locked = None + self._uncertain_locked = False + self._pending_zone = zone + self._pending_count = 1 + self._upgrade_count = 0 + self._frames_seen = 1 + conf = int(min(99, round(100.0 * 1 / max(1, self.confirm_frames)))) + return StableDecision(instant, False, conf, ratio_show, True) + + can_upgrade = ( + (prev == Category.OVERSIZE and cur in (Category.SUITABLE, Category.NEED_PACK)) + or (prev == Category.SUITABLE and cur == Category.NEED_PACK) + ) + if can_upgrade: + self._upgrade_count += 1 + need_up = max(5, self.confirm_frames // 2) + if self._upgrade_count >= need_up: + self._locked = instant + self._upgrade_count = 0 + self._uncertain_locked = False # появился консенсус + else: + self._upgrade_count = 0 + return StableDecision( + self._locked, True, 100, ratio_show, True, + uncertain=self._uncertain_locked, + ) + + # нет консенсуса слишком долго → «неуверенно», безопасная зона + if self._frames_seen >= self.uncertain_after: + fallback_result = ClassificationResult( + category=self.fallback, + dims_sorted_mm=(dims[0], dims[1], dims[2]), + circle_ratio=ratio_show, + passes_size=passes, + is_circular=circular, + reason="НЕУВЕРЕННО → безопасная зона: " + self._uncertain_reason( + ratio, dims, min_mm, max_mm + ), + ) + self._locked = fallback_result + self._uncertain_locked = True + return StableDecision(fallback_result, True, 100, ratio_show, True, uncertain=True) + + # прогрев окна + if len(self._ratios) < self.window: + conf = int(min(99, round(100.0 * len(self._ratios) / self.window))) + return StableDecision(instant, False, conf, ratio_show, True) + + votes = Counter(self._zones) + winner, win_n = votes.most_common(1)[0] + if winner != zone: + self._pending_zone = None + self._pending_count = 0 + conf = int(round(100.0 * win_n / len(self._zones))) + return StableDecision(instant, False, conf, ratio_show, True) + + need = max(self.confirm_frames, (self.window * 2 + 2) // 3) + if zone == self._pending_zone: + self._pending_count += 1 + else: + self._pending_zone = zone + self._pending_count = 1 + + conf = int(min(100, round(100.0 * max(self._pending_count, win_n) / need))) + if self._pending_count >= need and win_n >= need: + self._locked = instant + self._uncertain_locked = False + return StableDecision(instant, True, 100, ratio_show, True) + + return StableDecision(instant, False, conf, ratio_show, True) + + def _uncertain_reason( + self, + ratio: float, + dims: Sequence[float], + min_mm: Sequence[float], + max_mm: Sequence[float], + ) -> str: + votes = Counter(self._zones) + parts = [ + f"нет консенсуса {self._frames_seen} кадров", + "голоса " + " ".join(f"{z}:{n}" for z, n in votes.most_common()), + ] + if abs(ratio - self.enter_circle) <= 0.06: + parts.append(f"ratio {ratio:.3f} у порога {self.enter_circle}") + max_s = sorted([float(x) for x in max_mm], reverse=True) + min_s = sorted([float(x) for x in min_mm], reverse=True) + for d, mx, mn in zip(dims, max_s, min_s): + if abs(d - mx) <= 0.05 * mx: + parts.append(f"сторона {d:.0f} мм у лимита {mx:.0f}") + elif mn > 0 and abs(d - mn) <= max(3.0, 0.3 * mn): + parts.append(f"сторона {d:.0f} мм у минимума {mn:.0f}") + return "; ".join(parts) + + +def _median(vals: Deque[float]) -> float: + return _percentile(vals, 50) + + +def _dims_changed( + a: Sequence[float], b: Sequence[float], rel: float = 0.28 +) -> bool: + """True если хотя бы одна сторона изменилась больше чем на rel (новый объект).""" + if len(a) < 3 or len(b) < 3: + return False + for x, y in zip(a[:3], b[:3]): + base = max(abs(float(x)), abs(float(y)), 1.0) + if abs(float(x) - float(y)) / base > rel: + return True + return False + + +def _percentile(vals: Deque[float], q: float) -> float: + arr = sorted(vals) + n = len(arr) + if n == 0: + return 0.0 + if n == 1: + return float(arr[0]) + pos = (q / 100.0) * (n - 1) + lo = int(pos) + hi = min(lo + 1, n - 1) + frac = pos - lo + return float(arr[lo] * (1 - frac) + arr[hi] * frac) diff --git a/cv/test_classify.py b/cv/test_classify.py new file mode 100755 index 0000000..c18645b --- /dev/null +++ b/cv/test_classify.py @@ -0,0 +1,86 @@ +#!/usr/bin/env python3 +"""Юнит-тесты правил классификации (без камеры).""" + +from __future__ import annotations + +import sys +from pathlib import Path + +sys.path.insert(0, str(Path(__file__).resolve().parent)) + +from classify import Category, classify_from_dims + + +def test_suitable_box(): + r = classify_from_dims(120, 80, 40, circle_ratio=0.5) + assert r.category == Category.SUITABLE + assert r.category.zone == "B" + + +def test_oversize_priority_over_circle(): + # Большой цилиндр: габариты важнее круга + r = classify_from_dims(500, 100, 100, circle_ratio=0.95) + assert r.category == Category.OVERSIZE + assert r.category.zone == "C" + + +def test_too_small(): + r = classify_from_dims(5, 5, 5, circle_ratio=0.2) + assert r.category == Category.OVERSIZE + + +def test_need_pack_cylinder(): + r = classify_from_dims(100, 50, 50, circle_ratio=0.85) + assert r.category == Category.NEED_PACK + assert r.category.zone == "D" + + +def test_lying_bottle_must_be_D_not_B(): + """ТЗ: круг в ЛЮБОМ сечении. Лежачая бутылка сверху не круг, но сечение круглое.""" + # имитация: top-view низкий, но итоговый circle_ratio после 3D-срезов высокий + r = classify_from_dims(220, 70, 70, circle_ratio=0.86) + assert r.category == Category.NEED_PACK + assert r.category.zone == "D" + + +def test_border_circle(): + r = classify_from_dims(100, 50, 50, circle_ratio=0.8) + assert r.category == Category.NEED_PACK + r2 = classify_from_dims(100, 50, 50, circle_ratio=0.799) + assert r2.category == Category.SUITABLE + + +def test_border_dims_strict(): + """ТЗ: строго больше 10×10×10 и строго меньше 450×320×320.""" + # ровно на максимуме → C + assert classify_from_dims(450, 320, 320, 0.3).category == Category.OVERSIZE + # ровно на минимуме → C + assert classify_from_dims(10, 10, 10, 0.3).category == Category.OVERSIZE + # чуть внутри границ → B + assert classify_from_dims(449, 319, 319, 0.3).category == Category.SUITABLE + assert classify_from_dims(11, 11, 11, 0.3).category == Category.SUITABLE + # 321 мм влезает вдоль оси 450 → B (сопоставление после сортировки) + assert classify_from_dims(100, 321, 100, 0.3).category == Category.SUITABLE + # а вот две стороны > 320 уже не влезают → C + assert classify_from_dims(400, 330, 100, 0.3).category == Category.OVERSIZE + + +def test_dims_order_independent(): + """Стороны сопоставляются после сортировки — порядок L/W/H не важен.""" + assert classify_from_dims(319, 449, 318, 0.3).category == Category.SUITABLE + assert classify_from_dims(318, 319, 449, 0.3).category == Category.SUITABLE + # ровно 320 по строгому правилу «меньше» → C, в любом порядке + assert classify_from_dims(320, 449, 319, 0.3).category == Category.OVERSIZE + assert classify_from_dims(319, 320, 449, 0.3).category == Category.OVERSIZE + + +if __name__ == "__main__": + test_suitable_box() + test_oversize_priority_over_circle() + test_too_small() + test_need_pack_cylinder() + test_lying_bottle_must_be_D_not_B() + test_border_circle() + test_border_dims_strict() + test_dims_order_independent() + print("OK: all classification tests passed") diff --git a/cv/test_geometry.py b/cv/test_geometry.py new file mode 100644 index 0000000..b87be26 --- /dev/null +++ b/cv/test_geometry.py @@ -0,0 +1,384 @@ +#!/usr/bin/env python3 +"""Геометрические тесты: прямоугольник → B, круг → D (без камеры).""" + +from __future__ import annotations + +import sys +from pathlib import Path + +import numpy as np +import cv2 + +sys.path.insert(0, str(Path(__file__).resolve().parent)) + +from measure import ( + ObjectMeasurement, + _fit_circle_arc, + _section_score, + measure_object, + rin_rout, + section_rin_rout, + segment_object, +) +from stabilize import DecisionStabilizer + + +def _circle(n=64, r=40.0): + a = np.linspace(0, 2 * np.pi, n, endpoint=False) + return np.column_stack([50 + r * np.cos(a), 50 + r * np.sin(a)]).astype(np.float32) + + +def _rect(w=150.0, h=90.0, n=20): + pts = ( + [[x, 0] for x in np.linspace(0, w, n)] + + [[w, y] for y in np.linspace(0, h, n)] + + [[x, h] for x in np.linspace(w, 0, n)] + + [[0, y] for y in np.linspace(h, 0, n)] + ) + return np.array(pts, dtype=np.float32) + + +def _round_rect(w=150.0, h=90.0, rad=8.0, n=12, ne=15): + pts = [] + pts += [[x, 0] for x in np.linspace(rad, w - rad, ne)] + pts += [[w, y] for y in np.linspace(rad, h - rad, ne)] + pts += [[x, h] for x in np.linspace(w - rad, rad, ne)] + pts += [[0, y] for y in np.linspace(h - rad, rad, ne)] + corners = [ + (rad, rad, np.pi, 1.5 * np.pi), + (w - rad, rad, 1.5 * np.pi, 2 * np.pi), + (w - rad, h - rad, 0, 0.5 * np.pi), + (rad, h - rad, 0.5 * np.pi, np.pi), + ] + for cx, cy, a0, a1 in corners: + for a in np.linspace(a0, a1, n): + pts.append([cx + rad * np.cos(a), cy + rad * np.sin(a)]) + return np.array(pts, dtype=np.float32) + + +def _arc(span_deg=252.0, r=35.0, n=80): + a0 = -np.deg2rad(span_deg) / 2 + a1 = np.deg2rad(span_deg) / 2 + a = np.linspace(a0, a1, n) + return np.column_stack([50 + r * np.cos(a), 50 + r * np.sin(a)]).astype(np.float32) + + +def test_geometry_ratios(): + assert rin_rout(_circle()) >= 0.8, "круг сверху должен быть D" + assert rin_rout(_rect()) < 0.8, "прямоугольник должен быть B" + assert rin_rout(_round_rect()) < 0.8, "скруглённый прямоугольник — B" + # квадрат < 0.8 (теоретически ~0.707) + sq = np.array([[0, 0], [100, 0], [100, 100], [0, 100]], dtype=np.float32) + assert rin_rout(sq) < 0.8 + + # дуга цилиндра: fit → 0.85 + arc = _arc() + score = _section_score(arc) + assert score >= 0.8, f"дуга цилиндра должна давать >=0.8, got {score}" + assert _fit_circle_arc(_rect()) is None, "прямоугольник не должен проходить circle-fit" + + +def test_stabilizer_box_vs_cylinder(): + dummy = np.zeros((10, 1, 2), np.int32) + mask = np.zeros((10, 10), np.uint8) + + # коробка с редкими ложными пиками → LOCK B + s = DecisionStabilizer(window=8, confirm_frames=5, enter_circle=0.8) + locked_zone = None + seq = [0.55, 0.84, 0.56, 0.58, 0.57, 0.59, 0.55, 0.56, 0.58, 0.57, 0.55, 0.56, 0.57, 0.58] + for r in seq: + m = ObjectMeasurement(120, 80, 40, r, 1000, (1, 1), dummy, mask, top_ratio=r, section_ratio=0.5) + d = s.update(m) + if d.locked and d.result: + locked_zone = d.result.category.zone + assert locked_zone == "B", f"коробка должна LOCK B, got {locked_zone}" + + # цилиндр → LOCK D + s2 = DecisionStabilizer(window=8, confirm_frames=5, enter_circle=0.8) + locked_zone = None + for r in [0.90] * 16: + m = ObjectMeasurement(100, 50, 50, r, 1000, (1, 1), dummy, mask, top_ratio=r, section_ratio=0.88) + d = s2.update(m) + if d.locked and d.result: + locked_zone = d.result.category.zone + assert locked_zone == "D", f"цилиндр должен LOCK D, got {locked_zone}" + + # после LOCK D не прыгаем в B + for r in [0.4] * 10: + m = ObjectMeasurement(100, 50, 50, r, 1000, (1, 1), dummy, mask, top_ratio=r, section_ratio=0.4) + d = s2.update(m) + assert d.locked and d.result and d.result.category.zone == "D" + + # early B, then устойчивый круг → апгрейд в D (те же габариты) + s3 = DecisionStabilizer(window=8, confirm_frames=6, enter_circle=0.8) + for r in [0.55] * 20: + m = ObjectMeasurement(100, 50, 50, r, 1000, (1, 1), dummy, mask, top_ratio=r, section_ratio=0.5) + d = s3.update(m) + assert d.locked and d.result.category.zone == "B" + for r in [0.86] * 16: + m = ObjectMeasurement(100, 50, 50, r, 1000, (1, 1), dummy, mask, top_ratio=0.72, section_ratio=0.86) + d = s3.update(m) + assert d.locked and d.result.category.zone == "D", f"ожидали апгрейд B→D, got {d.result.category.zone}" + + # смена объекта круг→коробка по габаритам → новый LOCK B + saw_reset = False + locked_b = False + for r in [0.45] * 25: + m = ObjectMeasurement(240, 120, 50, r, 1000, (1, 1), dummy, mask, top_ratio=0.45, section_ratio=0.27) + d = s3.update(m) + if d.present and not d.locked: + saw_reset = True + if d.locked and d.result and d.result.category.zone == "B": + locked_b = True + assert saw_reset, "при смене габаритов должен быть сброс LOCK" + assert locked_b, "коробка после смены должна LOCK B" + + +def test_background_map_splits_object_from_platform(): + """Цилиндр на платформе: скалярная высота сливает их, фоновая карта — нет.""" + H, W = 480, 640 + belt = 600 + bg = np.full((H, W), belt, np.uint16) + bg[100:340, 130:470] = belt - 80 # платформа 80 мм — часть фона + + depth = bg.copy() + yy, xx = np.ogrid[:H, :W] + circ = (yy - 220) ** 2 + (xx - 300) ** 2 <= 70**2 + depth[circ] = belt - 80 - 60 # круглый предмет 60 мм на платформе + + # старый способ: платформа+цилиндр в одном контуре (большая площадь) + seg_old = segment_object(depth, belt_distance_mm=belt, min_area_px=400) + assert seg_old is not None + area_old = int((seg_old[0] > 0).sum()) + assert area_old > 240 * 340 * 0.8, "скалярный способ должен захватить платформу" + + # с фоновой картой: только цилиндр + seg_bg = segment_object(depth, belt_distance_mm=belt, min_area_px=400, background_mm=bg) + assert seg_bg is not None + mask, contour = seg_bg + area = int((mask > 0).sum()) + circle_area = np.pi * 70 * 70 + assert abs(area - circle_area) / circle_area < 0.15, f"площадь {area} vs круг {circle_area:.0f}" + + m = measure_object( + depth, mask, contour, + belt_distance_mm=belt, fx=670, fy=670, cx=320, cy=240, + background_mm=bg, + ) + assert m is not None + assert abs(m.height_mm - 60) < 8, f"высота от платформы должна быть ~60, got {m.height_mm:.1f}" + assert m.top_ratio >= 0.8, f"вид сверху круг, got {m.top_ratio:.3f}" + + +def test_uncertain_fallback_to_safe_zone(): + """Нет консенсуса (ratio скачет у порога) → «неуверенно» → безопасная зона C.""" + from classify import Category + + dummy = np.zeros((10, 1, 2), np.int32) + mask = np.zeros((10, 10), np.uint8) + + s = DecisionStabilizer(window=8, confirm_frames=6, uncertain_after=25, fallback=Category.OVERSIZE) + seq = ([0.70] * 6 + [0.90] * 6) * 5 # блоками вокруг порога 0.8 + final = None + for i, r in enumerate(seq): + m = ObjectMeasurement(100, 50, 50, r, 1000, (1, 1), dummy, mask, top_ratio=r, section_ratio=r) + d = s.update(m) + if d.locked: + final = (i + 1, d) + break + assert final is not None, "fallback должен сработать" + n_frames, d = final + assert d.uncertain, "LOCK должен быть помечен как неуверенный" + assert d.result is not None and d.result.category.zone == "C" + assert "НЕУВЕРЕННО" in d.result.reason + assert n_frames <= 30, f"fallback должен сработать около 25 кадров, got {n_frames}" + + # уверенная коробка не должна помечаться «неуверенно» + s2 = DecisionStabilizer(window=8, confirm_frames=6, uncertain_after=25) + got = None + for r in [0.5] * 20: + m = ObjectMeasurement(120, 80, 40, r, 1000, (1, 1), dummy, mask, top_ratio=r, section_ratio=r) + d2 = s2.update(m) + if d2.locked: + got = d2 + break + assert got is not None and not got.uncertain + assert got.result is not None and got.result.category.zone == "B" + + +def test_tracker_two_objects_ids_and_stats(): + """Коробка + цилиндр одновременно: два ID, статистика считает каждого один раз.""" + from tracker import MultiObjectTracker + + dummy = np.zeros((10, 1, 2), np.int32) + mask = np.zeros((10, 10), np.uint8) + + def factory(): + return DecisionStabilizer(window=8, confirm_frames=5, enter_circle=0.8) + + t = MultiObjectTracker(factory, max_dist_px=120, lost_frames=8) + all_events = [] + for _ in range(20): + box = ObjectMeasurement(120, 80, 40, 0.55, 1000, (100, 100), dummy, mask, + top_ratio=0.55, section_ratio=0.5) + cyl = ObjectMeasurement(100, 50, 50, 0.90, 900, (500, 300), dummy, mask, + top_ratio=0.90, section_ratio=0.88) + tracks, events = t.update([box, cyl]) + all_events.extend(events) + + assert len(tracks) == 2, f"должно быть 2 трека, got {len(tracks)}" + ids = sorted(tr.track_id for tr in tracks) + assert ids == [1, 2], f"ID должны быть 1 и 2, got {ids}" + zones = sorted(ev.decision.result.category.zone for ev in all_events) + assert zones == ["B", "D"], f"события LOCK для B и D, got {zones}" + assert t.stats["B"] == 1 and t.stats["D"] == 1 and t.stats["total"] == 2 + + # объекты убрали → треки умирают (залоченные живут дольше), статистика остаётся + for _ in range(40): + tracks, _ = t.update([]) + assert not tracks + assert t.stats["total"] == 2 + + # тот же товар на том же месте вернулся → без нового LOCK и без роста счётчика + events_back = [] + for _ in range(15): + box = ObjectMeasurement(120, 80, 40, 0.55, 1000, (100, 100), dummy, mask, + top_ratio=0.55, section_ratio=0.5) + tracks, events = t.update([box]) + events_back.extend(events) + assert not events_back, "повторный захват уже учтённого товара не должен давать LOCK" + assert tracks and tracks[0].track_id == 1 + assert tracks[0].frozen is not None and tracks[0].decision.locked + assert t.stats["B"] == 1 and t.stats["total"] == 2 + + # новый объект в другом месте получает следующий ID + box2 = ObjectMeasurement(200, 100, 60, 0.5, 1200, (300, 200), dummy, mask, + top_ratio=0.5, section_ratio=0.4) + for _ in range(20): + tracks, events = t.update([box2]) + assert tracks and any(tr.track_id == 3 for tr in tracks) + assert t.stats["B"] == 2 and t.stats["total"] == 3 + + +def test_tracker_slot_dedup_same_object(): + """Тот же товар на том же месте с новым track_id — без повторного LOCK.""" + from tracker import MultiObjectTracker, slot_key + + dummy = np.zeros((10, 1, 2), np.int32) + mask = np.zeros((10, 10), np.uint8) + + def factory(): + return DecisionStabilizer(window=8, confirm_frames=5, enter_circle=0.8) + + t = MultiObjectTracker(factory, max_dist_px=120, lost_frames=8) + box = ObjectMeasurement(120, 80, 40, 0.55, 1000, (100, 100), dummy, mask, + top_ratio=0.55, section_ratio=0.5) + all_events = [] + for _ in range(20): + tracks, events = t.update([box]) + all_events.extend(events) + assert all_events, "первый LOCK должен быть" + sk = slot_key(100, 100, 120, 80, 40, "B") + assert sk in t._seen_slots + + # новый track_id, та же позиция — события нет, счётчик не растёт + for _ in range(40): + tracks, _ = t.update([]) + box2 = ObjectMeasurement(118, 82, 41, 0.56, 1000, (102, 98), dummy, mask, + top_ratio=0.56, section_ratio=0.5) + extra = [] + for _ in range(20): + tracks, events = t.update([box2]) + extra.extend(events) + assert not extra, "повтор того же слота не должен давать LOCK" + assert t.stats["total"] == 1 + + +def test_shadow_not_detected_as_object(): + """Тень на ленте (затемнение без смены цвета) не должна давать RGB-объект.""" + from measure import segment_rgb_objects + + H, W = 480, 640 + bg = np.full((H, W, 3), (90, 130, 160), np.uint8) # коричневая лента BGR + color = bg.copy() + # мягкая тень: все каналы ×0.55 — типичная тень от коробки + color[150:300, 200:420] = (bg[150:300, 200:420].astype(np.float32) * 0.55).astype(np.uint8) + objs = segment_rgb_objects(color, bg, min_area_px=400, diff_threshold=35) + assert not objs, f"тень не должна детектироваться, got {len(objs)}" + + +def test_merge_overlapping_halves(): + """Две половины одного объекта с пересечением ≥50% → один контур.""" + from measure import merge_overlapping_masks, mask_overlap_min + + H, W = 100, 100 + a = np.zeros((H, W), np.uint8) + b = np.zeros((H, W), np.uint8) + a[20:60, 20:60] = 255 # 40×40 + b[20:60, 35:75] = 255 # перекрытие 40×25 = 1000 / 1600 = 0.625 + assert mask_overlap_min(a, b) >= 0.5 + ca, _ = cv2.findContours(a, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + cb, _ = cv2.findContours(b, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + merged = merge_overlapping_masks([(a, ca[0]), (b, cb[0])], overlap_thr=0.5) + assert len(merged) == 1, f"ожидали 1 объект, got {len(merged)}" + + +def test_noise_measurement_rejected(): + """Шум 0×0×0 / мелкие RGB-пятна не должны классифицироваться как негабарит.""" + from measure import ObjectMeasurement, is_plausible_measurement + + dummy = np.zeros((10, 1, 2), np.int32) + mask = np.zeros((10, 10), np.uint8) + noise = ObjectMeasurement(0.0, 0.0, 0.0, 0.1, 80, (10, 10), dummy, mask, source="rgb") + assert not is_plausible_measurement(noise) + speckle = ObjectMeasurement(12.0, 8.0, 0.0, 0.2, 500, (50, 50), dummy, mask, source="rgb") + assert not is_plausible_measurement(speckle) + ok = ObjectMeasurement(110.0, 70.0, 28.0, 0.6, 1200, (100, 100), dummy, mask) + assert is_plausible_measurement(ok) + + +def test_flat_phone_via_rgb(): + """Телефон 8 мм: depth не видит (порог 12 мм), RGB-фон находит → класс C.""" + from classify import Category, classify + from measure import measure_flat_object, segment_rgb_objects + + H, W = 480, 640 + belt = 534 + bg_color = np.full((H, W, 3), 120, np.uint8) # серая лента + color = bg_color.copy() + x0, y0, pw, ph = 250, 180, 172, 80 # ≈160×75 мм при fx=564, z=534 + color[y0 : y0 + ph, x0 : x0 + pw] = (30, 30, 30) # тёмный телефон + + depth = np.full((H, W), belt, np.uint16) + depth[y0 : y0 + ph, x0 : x0 + pw] = belt - 8 # всего 8 мм над лентой + rng = np.random.default_rng(0) + depth = (depth.astype(np.int32) + rng.integers(-3, 4, size=depth.shape)).astype(np.uint16) + + objs = segment_rgb_objects(color, bg_color, min_area_px=400) + assert objs, "телефон должен найтись по RGB-фону" + mask, contour = objs[0] + m = measure_flat_object( + depth, mask, contour, + belt_distance_mm=belt, fx=564.0, fy=564.0, cx=320, cy=240, + background_mm=np.full((H, W), belt, np.uint16), + ) + assert m is not None and m.source == "rgb" + assert abs(m.length_mm - 160) < 15, f"длина ~160, got {m.length_mm:.0f}" + assert abs(m.width_mm - 75) < 12, f"ширина ~75, got {m.width_mm:.0f}" + assert m.height_mm < 12, f"высота должна быть маленькой, got {m.height_mm:.0f}" + r = classify(m) + assert r.category == Category.OVERSIZE, "тоньше 10 мм → C по ТЗ (меньше минимума)" + + +if __name__ == "__main__": + test_geometry_ratios() + test_stabilizer_box_vs_cylinder() + test_background_map_splits_object_from_platform() + test_uncertain_fallback_to_safe_zone() + test_tracker_two_objects_ids_and_stats() + test_tracker_slot_dedup_same_object() + test_noise_measurement_rejected() + test_merge_overlapping_halves() + test_shadow_not_detected_as_object() + test_flat_phone_via_rgb() + print("OK: geometry + stabilizer tests passed") diff --git a/cv/tracker.py b/cv/tracker.py new file mode 100644 index 0000000..d9a1dbd --- /dev/null +++ b/cv/tracker.py @@ -0,0 +1,279 @@ +"""Трекинг нескольких объектов в кадре: ID + статистика зон за сессию. + +Сопоставление кадр-к-кадру по ближайшему центроиду. На каждый трек — +свой DecisionStabilizer. LOCK в ленту/MQTT — один раз на товар: после +фиксации запоминаем «отпечаток» (центр+габариты); если depth кратковременно +пропал и объект нашёлся снова рядом — трек возрождается без нового LOCK. +""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import Callable, Dict, List, Optional, Sequence, Tuple + +from classify import ClassificationResult +from measure import ObjectMeasurement +from stabilize import DecisionStabilizer, StableDecision + + +@dataclass +class Track: + track_id: int + stabilizer: DecisionStabilizer + centroid: Tuple[int, int] + miss: int = 0 + counted_zone: Optional[str] = None + counted_uncertain: bool = False + measurement: Optional[ObjectMeasurement] = None + decision: Optional[StableDecision] = None + reported: bool = False # уже ушёл в ленту — повторно не пишем + frozen: Optional[StableDecision] = None # снимок первого LOCK (для возрождения) + + +@dataclass +class LockEvent: + track_id: int + decision: StableDecision + + +@dataclass +class _Fingerprint: + """Товар уже зафиксирован — не считаем повторно при перезахвате.""" + track_id: int + cx: int + cy: int + length_mm: float + width_mm: float + height_mm: float + zone: str + uncertain: bool + result: ClassificationResult + age: int = 0 # кадров без детекции + + +def slot_key(cx: int, cy: int, L: float, W: float, H: float, zone: str) -> str: + """Стабильный ключ товара на ленте — один физический предмет = одна запись.""" + a = sorted((L, W, H)) + return f"{cx // 35}_{cy // 35}_{int(a[0] // 12)}_{int(a[1] // 12)}_{int(a[2] // 8)}_{zone}" + + +class MultiObjectTracker: + def __init__( + self, + stabilizer_factory: Callable[[], DecisionStabilizer], + max_dist_px: int = 120, + lost_frames: int = 12, + fingerprint_ttl: int = 450, # ~15–20 с помнить «уже учтён» + ) -> None: + self._factory = stabilizer_factory + self.max_dist_px = int(max_dist_px) + self.lost_frames = int(lost_frames) + self.fingerprint_ttl = int(fingerprint_ttl) + self._tracks: Dict[int, Track] = {} + self._fps: List[_Fingerprint] = [] + self._seen_slots: Dict[str, _Fingerprint] = {} # уже учтённые товары (позиция+габариты) + self._next_id = 1 + self.stats: Dict[str, int] = {"B": 0, "C": 0, "D": 0, "total": 0, "uncertain": 0} + + def reset(self) -> None: + self._tracks.clear() + self._fps.clear() + + def update( + self, + measurements: List[ObjectMeasurement], + min_mm: Sequence[float] = (10, 10, 10), + max_mm: Sequence[float] = (450, 320, 320), + ) -> Tuple[List[Track], List[LockEvent]]: + free_meas = list(range(len(measurements))) + assigned: Dict[int, int] = {} + pairs = [] + for tid, tr in self._tracks.items(): + for mi in free_meas: + m = measurements[mi] + d2 = (tr.centroid[0] - m.centroid_px[0]) ** 2 + (tr.centroid[1] - m.centroid_px[1]) ** 2 + pairs.append((d2, tid, mi)) + for d2, tid, mi in sorted(pairs): + if tid in assigned or mi not in free_meas: + continue + if d2 > self.max_dist_px**2: + continue + assigned[tid] = mi + free_meas.remove(mi) + + events: List[LockEvent] = [] + + for tid in list(self._tracks.keys()): + tr = self._tracks[tid] + # залоченный трек держим дольше — глянец даёт короткие выпадения depth + kill_after = self.lost_frames * 3 if tr.counted_zone is not None else self.lost_frames + if tid in assigned: + m = measurements[assigned[tid]] + tr.centroid = m.centroid_px + tr.miss = 0 + tr.measurement = m + tr.decision = tr.stabilizer.update(m, min_mm=min_mm, max_mm=max_mm) + # после возрождения стабилизатор ещё «холодный» — держим прошлый LOCK на экране + if tr.reported and tr.frozen is not None and not (tr.decision and tr.decision.locked): + tr.decision = tr.frozen + self._account(tr, events) + if tr.reported: + self._touch_fp(tr) + else: + tr.miss += 1 + tr.measurement = None + tr.decision = tr.stabilizer.update(None, min_mm=min_mm, max_mm=max_mm) + if tr.reported and tr.frozen is not None and not (tr.decision and tr.decision.locked): + tr.decision = tr.frozen + if tr.miss >= kill_after: + if tr.reported and tr.frozen is not None and tr.frozen.result is not None: + self._remember(tr) + del self._tracks[tid] + + # старение отпечатков + for fp in self._fps: + fp.age += 1 + self._fps = [fp for fp in self._fps if fp.age < self.fingerprint_ttl] + + for mi in free_meas: + m = measurements[mi] + fp = self._match_fp(m) + if fp is not None: + # тот же товар вернулся после выпадения depth — без нового LOCK + fp.age = 0 + fp.cx, fp.cy = m.centroid_px + frozen = StableDecision( + fp.result, True, 100, fp.result.circle_ratio, True, uncertain=fp.uncertain, + ) + tr = Track( + track_id=fp.track_id, + stabilizer=self._factory(), + centroid=m.centroid_px, + measurement=m, + counted_zone=fp.zone, + counted_uncertain=fp.uncertain, + reported=True, + frozen=frozen, + decision=frozen, + ) + self._tracks[tr.track_id] = tr + self._fps = [x for x in self._fps if x.track_id != fp.track_id] + continue + + tr = Track( + track_id=self._next_id, + stabilizer=self._factory(), + centroid=m.centroid_px, + measurement=m, + ) + self._next_id += 1 + tr.decision = tr.stabilizer.update(m, min_mm=min_mm, max_mm=max_mm) + self._tracks[tr.track_id] = tr + self._account(tr, events) + + alive = sorted(self._tracks.values(), key=lambda t: t.track_id) + return [t for t in alive if t.measurement is not None or t.miss < ( + self.lost_frames * 3 if t.counted_zone else self.lost_frames + )], events + + def _account(self, tr: Track, events: List[LockEvent]) -> None: + d = tr.decision + if d is None or not d.locked or d.result is None: + return + zone = d.result.category.zone + L, W, H = d.result.dims_sorted_mm + sk = slot_key(tr.centroid[0], tr.centroid[1], L, W, H, zone) + if tr.counted_zone is None: + if sk in self._seen_slots: + # тот же товар уже был в ленте/статистике — только показываем на экране + prev = self._seen_slots[sk] + tr.counted_zone = prev.zone + tr.counted_uncertain = prev.uncertain + tr.frozen = StableDecision( + prev.result, True, 100, prev.result.circle_ratio, True, uncertain=prev.uncertain, + ) + tr.reported = True + tr.decision = tr.frozen + return + self.stats[zone] = self.stats.get(zone, 0) + 1 + self.stats["total"] += 1 + if d.uncertain: + self.stats["uncertain"] += 1 + tr.counted_zone = zone + tr.counted_uncertain = d.uncertain + tr.frozen = StableDecision( + d.result, True, 100, d.result.circle_ratio, True, uncertain=d.uncertain, + ) + self._seen_slots[sk] = _Fingerprint( + track_id=tr.track_id, + cx=tr.centroid[0], + cy=tr.centroid[1], + length_mm=float(L), + width_mm=float(W), + height_mm=float(H), + zone=zone, + uncertain=d.uncertain, + result=d.result, + ) + if not tr.reported: + tr.reported = True + events.append(LockEvent(tr.track_id, tr.frozen)) + elif tr.counted_zone != zone: + # смена зоны на экране/в счётчиках — в ленту повторно не пишем + self.stats[tr.counted_zone] = max(0, self.stats.get(tr.counted_zone, 0) - 1) + self.stats[zone] = self.stats.get(zone, 0) + 1 + if tr.counted_uncertain and not d.uncertain: + self.stats["uncertain"] = max(0, self.stats["uncertain"] - 1) + tr.counted_uncertain = d.uncertain + tr.counted_zone = zone + tr.frozen = StableDecision( + d.result, True, 100, d.result.circle_ratio, True, uncertain=d.uncertain, + ) + + def _remember(self, tr: Track) -> None: + assert tr.frozen is not None and tr.frozen.result is not None + r = tr.frozen.result + L, W, H = r.dims_sorted_mm + self._fps = [fp for fp in self._fps if fp.track_id != tr.track_id] + self._fps.append( + _Fingerprint( + track_id=tr.track_id, + cx=tr.centroid[0], + cy=tr.centroid[1], + length_mm=float(L), + width_mm=float(W), + height_mm=float(H), + zone=tr.counted_zone or r.category.zone, + uncertain=tr.counted_uncertain, + result=r, + age=0, + ) + ) + + def _touch_fp(self, tr: Track) -> None: + for fp in self._fps: + if fp.track_id == tr.track_id: + fp.age = 0 + fp.cx, fp.cy = tr.centroid + + def _match_fp(self, m: ObjectMeasurement) -> Optional[_Fingerprint]: + best: Optional[_Fingerprint] = None + best_d2 = self.max_dist_px**2 + for fp in self._fps: + d2 = (fp.cx - m.centroid_px[0]) ** 2 + (fp.cy - m.centroid_px[1]) ** 2 + if d2 > best_d2: + continue + if not _dims_close(fp.length_mm, fp.width_mm, fp.height_mm, m.length_mm, m.width_mm, m.height_mm): + continue + best, best_d2 = fp, d2 + return best + + +def _dims_close(L0: float, W0: float, H0: float, L1: float, W1: float, H1: float, tol: float = 0.40) -> bool: + """Габариты «похожи» (порядок осей уже отсортирован в classify, здесь — сырые L×W×H).""" + a = sorted((L0, W0, H0)) + b = sorted((L1, W1, H1)) + for x, y in zip(a, b): + if abs(x - y) / max(x, y, 1.0) > tol: + return False + return True diff --git a/docs/ENGINEERING.md b/docs/ENGINEERING.md index 49e137f..105fecc 100644 --- a/docs/ENGINEERING.md +++ b/docs/ENGINEERING.md @@ -61,6 +61,23 @@ CCD for light/thin items exists in runtime/sim; that alone is **not** full conta - Missing: `input_info/extracted/Постановка_Задача_3_сжато_2.pdf` — never claim it is available. - Internal engineering criteria are not automatic Ozon pass/fail. +## Соответствие подтверждённым требованиям + +Grounded only in present sources + current `main` code/tests (not a full official scorecard): + +| Area | Official source present | Current implementation | Evidence | Status | +|---|---|---|---|---| +| B/C/D bounds & roundness | `official_sources/doc-1783095831.pdf` | web `classifier.ts`, CV `cv/classify.py` | unit tests | PARTIAL (PDF not re-parsed each pass) | +| Digital twin demo | workspace PDFs in `input_info/` | `/` + `/documentation` | e2e smoke/routes, production | PASS (demo present) | +| CAD conveyor | author pack / STL references | `3d_models/`, `conveyor-clean.glb` | checksums in README | PASS (assets present) | +| Real measurement CV | Track 3 camera intent in briefs | `cv/` RealSense+OpenCV | `cv/README.md`, `test_classify.py` | PARTIAL (prototype, not live web) | +| Physical industrial line | scoring/workspace PDFs | web physics + optional MQTT CV | code; contact not fully validated | PARTIAL | +| Presentation / video | platform rules | cloud links | README §17 | NOT_VERIFIED (links missing) | + +## Real CV prototype + +Path `cv/` — OpenCV + RealSense D415 depth pipeline. Same B/C/D domain as the web twin. **Not** consumed by https://arhipovdan.ru. Details: `cv/README.md`. + ## Layout drawing `docs/engineering/work-area-layout-source.png` — workspace layout provenance image retained for engineering reference.