Port complete CAN-aware risk pipeline to C++
This commit is contained in:
+11
-22
@@ -1,33 +1,22 @@
|
||||
# Orin 直接运行包
|
||||
# Orin 纯 C++ 运行包
|
||||
|
||||
适用 Jetson AGX Orin / JetPack 5.1.2 (R35.4.1)、CUDA 11.4、TensorRT 8.5.x、OpenCV 4.5.4。
|
||||
系统需要 Python 3.8+、numpy、cv2。无需重新编译，可单独复制整个目录。
|
||||
适用 AGX Orin / JetPack 5.1.2 (R35.4.1)、CUDA 11.4、TensorRT 8.5.x、OpenCV 4.5.4。不依赖 Python。
|
||||
|
||||
```bash
|
||||
sha256sum -c SHA256SUMS
|
||||
./scripts/initialize_camera.sh # 开机后无 /dev/video5 时执行，需要 sudo
|
||||
./run.sh # 默认 CAN 模式，结果保存在内存
|
||||
./run.sh --visualize # 可选浏览器预览，http://设备IP:8765
|
||||
./run.sh # exec build/camera_risk，默认无可视化
|
||||
./run.sh --visualize # 可选 Orin 本地图形窗口
|
||||
# 直接执行 ELF：
|
||||
./build/camera_risk --config config/camera.json
|
||||
```
|
||||
|
||||
只运行一个摄像头采集进程，Ctrl+C / SIGTERM 停止。
|
||||
只运行一个摄像头进程。Ctrl+C / SIGTERM 停止；关闭可视化窗口不停止推理。无图形桌面时继续无窗口运行。
|
||||
|
||||
- data[0..3]：左前、右前、左后、右后；`--can-side left/right` 选择摄像头侧。
|
||||
- 未收到有效 CAN 或超时：CAN_UNKNOWN，score=null；不发送执行器控制报文。
|
||||
- `--save-results` 保存 JSONL；`--save-snapshots` 保存快照；`--write-video` 保存叠加视频。默认均关闭。
|
||||
- `--output PATH` 指定输出目录；`--duration 30` 限时运行。完整参数见 `./run.sh --help`。
|
||||
- 标定参数仍需匹配实际机位，默认 calibration_verified=false。
|
||||
CAN 0x391 data[0..3] 为左前、右前、左后、右后。默认左侧，`--can-side right` 切换右侧。缺失或超时报文输出 CAN_UNKNOWN，风险分数为空。
|
||||
|
||||
同进程下游示例（先停止其他采集进程）：
|
||||
`--no-can --active-door both` 模拟门状态；`--duration 30` 限时运行；`--save-results FILE.jsonl` 显式保存结果，默认不写文件。完整参数见 `./run.sh --help`。
|
||||
|
||||
```python
|
||||
from run_camera import CameraPipeline, parse_args
|
||||
with CameraPipeline(parse_args([])) as pipeline:
|
||||
result = pipeline.wait_for_result(timeout=10)
|
||||
result = pipeline.latest_result # 等价于 pipeline.latest()
|
||||
if result is not None:
|
||||
print(result['risk'], result['door_risks'], result['skeletons'])
|
||||
```
|
||||
最新结果保存在进程内的 `shared_ptr<const cms::FrameResult>`；C++ 下游调用 `pipeline.latest()` 读取。接口头文件位于仓库 include/，不是 Python 变量或跨进程共享内存。
|
||||
|
||||
变量是线程安全的最新只读快照，读取不会消费结果，不是跨进程共享变量。
|
||||
系统 CUDA/TensorRT/OpenCV 运行库由 JetPack 提供。引擎和驱动不能直接用于其他 GPU 或不匹配的内核。
|
||||
模型、驱动和编译产物绑定目标 GPU、TensorRT 与内核环境。标定文件需匹配实际机位，默认 calibration_verified=false。系统 CUDA/TensorRT/OpenCV 库由 JetPack 提供。
|
||||
|
||||
+15
-25
@@ -1,25 +1,15 @@
|
||||
b6295cf4b0705c5c3d83c9ba35e41b2a0b865ea67742c09cfd1690f263f556d0 README.md
|
||||
9ba768d559fd3b64e833b50c62c70f2adfd6c169620aea5fba32cfb1b4c5b0e9 annotations/constant_door_mask.json
|
||||
3544fb1c7b1b27b39db27d8f849d5e8652f5d09a950a59c5dd0c7703086b4f6b annotations/door_zone_multifeature.json
|
||||
6d5e874fd27fcfd75d303d7e35d9672028af71041f52dcc82a1e83676faefa65 bin/drivers/max9295.ko
|
||||
8a78d04ef8be44e9b62a5e4ee315d3d6d679e34aedff02a234bbf4d40e222933 bin/drivers/max9296.ko
|
||||
3da628587477cc35d5e3f8aee86e718e2ddf16808e893e00ba6e164b2307e843 bin/drivers/sgx-yuv-gmsl2.ko
|
||||
5ba7e4f0371a411cc1a6dfd1275e3b824032a14d07a9ccd7ca3d5a6630b322ef build/libvision_native.so
|
||||
8dae8c557deadbe6bd7241c0dcd7025cb007077ee39a79b115e953a2b22cd9c1 lib/libusb-1.0.so
|
||||
ab8b7f7249d5fb73d1d33d14fa7532a522088ffb6c25f37ba334daf4c6b4d313 lib/libusbcanfd.so
|
||||
ebcd09e8c2e35008adbb0c1d222e580b17e0df1c8093df3481ba3374cd9a8174 models/engines/rtmw_l_384.engine
|
||||
f6a65b926ac1144484f4aa93cd26d7997d32d62310ab93fe867f2bd4412222e9 models/engines/yolo11s.engine
|
||||
6d6d14a972bc4e5e9f8256514169e73ba0392a301cacd1221eaa26146aad00d2 pipeline/door_mask_risk.py
|
||||
46d70e1e90a1f36d544b7492ef1fbf08821377a1c460a6683f42d4d7006055c7 pipeline/door_zone_risk.py
|
||||
bcf2cc99dd27c8749801a5b743656767d9d64ef03d568d227cb654c573d70b00 pipeline/latest_result.py
|
||||
294cdc11aad0e5475275fd8dece2057e229b9bdedba69c26afa62176fd51a4e9 pipeline/native_backend.py
|
||||
8b8c5b8c9c76c9bd2e1efa6cba60be337a18c9366e8f748038465e9e2fba2672 pipeline/pose_engine.py
|
||||
93d3323ea431ed1d3c8397cfed7d800d65ec6d7700690487335861945040a486 pipeline/pose_fusion.py
|
||||
2ebf58114835ba7c4779d4b66aeff65b8a94107128989c0336de556a4cf95fda pipeline/pose_geometry.py
|
||||
f32fa5bebf893a3dc6b9ccfaa09a8e3a153ad5b36dde5373fdc561f10f9e526b pipeline/pose_occlusion.py
|
||||
1a3f1f55d7b8a3f0ac96e69bf77f02f82017e8970adef96181d2bfade26e5bd5 pipeline/pose_tracking.py
|
||||
dcf3cce5562ea52400ffa9842fdddaadac1efd672dabb0ed12ce34f20f779f37 pipeline/visualization.py
|
||||
8a10e035334ed97b7480f558aff7b0464c255ac6138dd99f55a08ade68853e0e run.sh
|
||||
23e9af5e82999889fe77c762a77c77036d1b1b699525ff2bd5ca8a418183ba50 run_camera.py
|
||||
11b5fbc3c17c0610ec0514a0a26cf5d92588d15c0e50e7086d9db5a5c0306e99 scripts/initialize_camera.sh
|
||||
bce5a22d49633454ed2c6bc92b608edf3b718af2424c283ed8f56ba93987200a scripts/initialize_camera_impl.sh
|
||||
1cff77e5547d14cd188b3bb38ba9332e9ae5904646f9248c2a3904a199318738 ./README.md
|
||||
9ba768d559fd3b64e833b50c62c70f2adfd6c169620aea5fba32cfb1b4c5b0e9 ./annotations/constant_door_mask.json
|
||||
3544fb1c7b1b27b39db27d8f849d5e8652f5d09a950a59c5dd0c7703086b4f6b ./annotations/door_zone_multifeature.json
|
||||
6d5e874fd27fcfd75d303d7e35d9672028af71041f52dcc82a1e83676faefa65 ./bin/drivers/max9295.ko
|
||||
8a78d04ef8be44e9b62a5e4ee315d3d6d679e34aedff02a234bbf4d40e222933 ./bin/drivers/max9296.ko
|
||||
3da628587477cc35d5e3f8aee86e718e2ddf16808e893e00ba6e164b2307e843 ./bin/drivers/sgx-yuv-gmsl2.ko
|
||||
d37bb5880f921ef8d5f937e5bf9dba38539929d95430f4fb8f575a2ede691a78 ./build/camera_risk
|
||||
718c43eba7c9a86178b272404b14e201445a3998dae81cd32b840f83018ba2b5 ./config/camera.json
|
||||
8dae8c557deadbe6bd7241c0dcd7025cb007077ee39a79b115e953a2b22cd9c1 ./lib/libusb-1.0.so
|
||||
ab8b7f7249d5fb73d1d33d14fa7532a522088ffb6c25f37ba334daf4c6b4d313 ./lib/libusbcanfd.so
|
||||
ebcd09e8c2e35008adbb0c1d222e580b17e0df1c8093df3481ba3374cd9a8174 ./models/engines/rtmw_l_384.engine
|
||||
f6a65b926ac1144484f4aa93cd26d7997d32d62310ab93fe867f2bd4412222e9 ./models/engines/yolo11s.engine
|
||||
4e3bd58314a9d70b6b243f14c0e26ffd1b1c6dcf8bcbf4c44629f1767f72a5ec ./run.sh
|
||||
11b5fbc3c17c0610ec0514a0a26cf5d92588d15c0e50e7086d9db5a5c0306e99 ./scripts/initialize_camera.sh
|
||||
bce5a22d49633454ed2c6bc92b608edf3b718af2424c283ed8f56ba93987200a ./scripts/initialize_camera_impl.sh
|
||||
|
||||
Executable
BIN
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,6 @@
|
||||
{
|
||||
"camera": {"device":"/dev/video5","width":1920,"height":1536,"fps":30,"buffers":4,"timeout_ms":2000},
|
||||
"inference": {"engine_directory":"../models/engines","detector_confidence":0.30,"max_people":3,"queue_capacity":2},
|
||||
"door": {"mask":"../annotations/constant_door_mask.json","zone_config":"../annotations/door_zone_multifeature.json","warning_relative":0.30,"release_relative":0.45,"calibration_verified":false},
|
||||
"can": {"enabled":true,"side":"left","timeout_ms":1000}
|
||||
}
|
||||
@@ -1,488 +0,0 @@
|
||||
"""Per-frame door-mask loading and YOLO/RTMW-only 2-D risk scoring."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import math
|
||||
import re
|
||||
import time
|
||||
from collections import Counter
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
|
||||
|
||||
RASTER_SUFFIXES = {".png", ".bmp", ".tif", ".tiff"}
|
||||
DEFAULT_DOOR_LABELS = ("door", "车门")
|
||||
|
||||
|
||||
@dataclass
|
||||
class LoadedDoorMask:
|
||||
status: str
|
||||
annotation_index: int
|
||||
path: Path | None
|
||||
mask: np.ndarray | None
|
||||
source_format: str | None
|
||||
|
||||
def metadata(self) -> dict:
|
||||
return {
|
||||
"status": self.status,
|
||||
"annotation_index": self.annotation_index,
|
||||
"path": str(self.path.resolve()) if self.path is not None else None,
|
||||
"format": self.source_format,
|
||||
"area_px": int(np.count_nonzero(self.mask)) if self.mask is not None else 0,
|
||||
}
|
||||
|
||||
|
||||
def _frame_index_from_path(path: Path) -> int | None:
|
||||
matches = re.findall(r"\d+", path.stem)
|
||||
return int(matches[-1]) if matches else None
|
||||
|
||||
|
||||
class DoorMaskSequence:
|
||||
"""Index a directory of X-AnyLabeling JSON polygons or raster masks."""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
directory: Path | None = None,
|
||||
mask_format: str = "auto",
|
||||
labels: tuple[str, ...] = DEFAULT_DOOR_LABELS,
|
||||
frame_offset: int = 0,
|
||||
require_every_frame: bool = False,
|
||||
constant_mask: Path | None = None,
|
||||
):
|
||||
if (directory is None) == (constant_mask is None):
|
||||
raise ValueError("Provide exactly one of directory or constant_mask")
|
||||
self.constant_path = (
|
||||
Path(constant_mask).expanduser().resolve()
|
||||
if constant_mask is not None
|
||||
else None
|
||||
)
|
||||
self.directory = (
|
||||
self.constant_path.parent if self.constant_path is not None else Path(directory)
|
||||
)
|
||||
if self.constant_path is not None and not self.constant_path.is_file():
|
||||
raise ValueError(f"Constant door mask does not exist: {self.constant_path}")
|
||||
if self.constant_path is None and not self.directory.is_dir():
|
||||
raise ValueError(f"Door mask directory does not exist: {self.directory}")
|
||||
self.labels = {label.casefold() for label in labels}
|
||||
self.frame_offset = int(frame_offset)
|
||||
self.require_every_frame = require_every_frame
|
||||
|
||||
if self.constant_path is not None:
|
||||
if mask_format == "auto":
|
||||
selected_format = (
|
||||
"xanylabeling"
|
||||
if self.constant_path.suffix.lower() == ".json"
|
||||
else "raster"
|
||||
)
|
||||
else:
|
||||
selected_format = mask_format
|
||||
self.source_format = selected_format
|
||||
self.paths = {0: self.constant_path}
|
||||
return
|
||||
|
||||
json_paths = sorted(self.directory.rglob("*.json"))
|
||||
raster_paths = sorted(
|
||||
path
|
||||
for path in self.directory.rglob("*")
|
||||
if path.is_file() and path.suffix.lower() in RASTER_SUFFIXES
|
||||
)
|
||||
if mask_format == "auto":
|
||||
selected_format = "xanylabeling" if json_paths else "raster"
|
||||
else:
|
||||
selected_format = mask_format
|
||||
candidates = json_paths if selected_format == "xanylabeling" else raster_paths
|
||||
if not candidates:
|
||||
expected = "JSON" if selected_format == "xanylabeling" else "PNG/BMP/TIFF"
|
||||
raise ValueError(f"No {expected} door masks found in {self.directory}")
|
||||
|
||||
self.source_format = selected_format
|
||||
self.paths: dict[int, Path] = {}
|
||||
for path in candidates:
|
||||
index = _frame_index_from_path(path)
|
||||
if index is None and selected_format == "xanylabeling":
|
||||
data = json.loads(path.read_text(encoding="utf-8-sig"))
|
||||
explicit = data.get("frame_index", data.get("frameIndex"))
|
||||
index = int(explicit) if explicit is not None else None
|
||||
if index is None:
|
||||
continue
|
||||
if index in self.paths:
|
||||
raise ValueError(
|
||||
f"Multiple door masks map to annotation frame {index}: "
|
||||
f"{self.paths[index]} and {path}"
|
||||
)
|
||||
self.paths[index] = path
|
||||
if not self.paths:
|
||||
raise ValueError(
|
||||
"Door mask filenames need a frame number, for example 000000.json"
|
||||
)
|
||||
|
||||
def load(self, video_frame_index: int, frame_shape: tuple[int, int]) -> LoadedDoorMask:
|
||||
if self.constant_path is not None:
|
||||
annotation_index = video_frame_index
|
||||
path = self.constant_path
|
||||
else:
|
||||
annotation_index = video_frame_index + self.frame_offset
|
||||
path = self.paths.get(annotation_index)
|
||||
if path is None:
|
||||
if self.require_every_frame:
|
||||
raise RuntimeError(
|
||||
f"No door mask for video frame {video_frame_index} "
|
||||
f"(expected annotation index {annotation_index})"
|
||||
)
|
||||
return LoadedDoorMask("missing", annotation_index, None, None, self.source_format)
|
||||
|
||||
if self.source_format == "xanylabeling":
|
||||
mask = self._load_xanylabeling(path, frame_shape)
|
||||
else:
|
||||
mask = self._load_raster(path, frame_shape)
|
||||
status = "loaded" if np.any(mask) else "empty"
|
||||
return LoadedDoorMask(status, annotation_index, path, mask, self.source_format)
|
||||
|
||||
@staticmethod
|
||||
def _load_raster(path: Path, frame_shape: tuple[int, int]) -> np.ndarray:
|
||||
image = cv2.imread(str(path), cv2.IMREAD_UNCHANGED)
|
||||
if image is None:
|
||||
raise RuntimeError(f"Cannot read door mask: {path}")
|
||||
if image.ndim == 3:
|
||||
image = np.max(image[:, :, :3], axis=2)
|
||||
height, width = frame_shape
|
||||
if image.shape != (height, width):
|
||||
image = cv2.resize(image, (width, height), interpolation=cv2.INTER_NEAREST)
|
||||
return image > 0
|
||||
|
||||
def _load_xanylabeling(
|
||||
self, path: Path, frame_shape: tuple[int, int]
|
||||
) -> np.ndarray:
|
||||
data = json.loads(path.read_text(encoding="utf-8-sig"))
|
||||
height, width = frame_shape
|
||||
source_width = float(data.get("imageWidth") or width)
|
||||
source_height = float(data.get("imageHeight") or height)
|
||||
scale = np.asarray(
|
||||
[width / max(source_width, 1.0), height / max(source_height, 1.0)],
|
||||
dtype=np.float32,
|
||||
)
|
||||
mask = np.zeros((height, width), dtype=np.uint8)
|
||||
for shape in data.get("shapes", []):
|
||||
if str(shape.get("label", "")).casefold() not in self.labels:
|
||||
continue
|
||||
points = np.asarray(shape.get("points", []), dtype=np.float32).reshape(-1, 2)
|
||||
if len(points) < 2:
|
||||
continue
|
||||
points = np.rint(points * scale).astype(np.int32)
|
||||
shape_type = str(shape.get("shape_type", "polygon")).lower()
|
||||
if shape_type == "rectangle" and len(points) >= 2:
|
||||
x1, y1 = points[0]
|
||||
x2, y2 = points[1]
|
||||
cv2.rectangle(mask, (x1, y1), (x2, y2), 1, -1)
|
||||
elif shape_type == "circle" and len(points) >= 2:
|
||||
radius = int(round(float(np.linalg.norm(points[1] - points[0]))))
|
||||
cv2.circle(mask, tuple(points[0]), radius, 1, -1)
|
||||
elif len(points) >= 3:
|
||||
cv2.fillPoly(mask, [points], 1)
|
||||
return mask > 0
|
||||
|
||||
|
||||
def _fit_mask(mask: np.ndarray, frame_shape: tuple[int, int]) -> np.ndarray:
|
||||
height, width = frame_shape
|
||||
result = np.asarray(mask) > 0
|
||||
if result.shape != (height, width):
|
||||
result = cv2.resize(
|
||||
result.astype(np.uint8), (width, height), interpolation=cv2.INTER_NEAREST
|
||||
) > 0
|
||||
return result
|
||||
|
||||
|
||||
def _box_mask(box: list[float] | None, frame_shape: tuple[int, int]) -> np.ndarray | None:
|
||||
if box is None or len(box) < 4:
|
||||
return None
|
||||
height, width = frame_shape
|
||||
x1, y1, x2, y2 = [int(round(float(value))) for value in box[:4]]
|
||||
x1, x2 = sorted((max(0, min(width - 1, x1)), max(0, min(width - 1, x2))))
|
||||
y1, y2 = sorted((max(0, min(height - 1, y1)), max(0, min(height - 1, y2))))
|
||||
if x2 <= x1 or y2 <= y1:
|
||||
return None
|
||||
mask = np.zeros((height, width), dtype=bool)
|
||||
mask[y1 : y2 + 1, x1 : x2 + 1] = True
|
||||
return mask
|
||||
|
||||
|
||||
def _hand_mask(points: list, frame_shape: tuple[int, int]) -> np.ndarray | None:
|
||||
hand = np.asarray(points, dtype=np.float32).reshape(-1, 2)
|
||||
if len(hand) < 3 or not np.isfinite(hand).all():
|
||||
return None
|
||||
hull = cv2.convexHull(np.rint(hand).astype(np.int32))
|
||||
mask = np.zeros(frame_shape, dtype=np.uint8)
|
||||
cv2.fillConvexPoly(mask, hull, 1)
|
||||
return mask > 0 if np.any(mask) else None
|
||||
|
||||
|
||||
def _decision(distance_relative: float, overlap_px: int, warning: float, release: float) -> tuple[str, str]:
|
||||
if overlap_px > 0:
|
||||
return "danger", "DANGER_INTERSECTION"
|
||||
if distance_relative <= warning:
|
||||
return "warning", "WARNING_NEAR"
|
||||
if distance_relative <= release:
|
||||
return "watch", "WATCH"
|
||||
return "clear", "CLEAR"
|
||||
|
||||
|
||||
class DoorRiskEvaluator:
|
||||
"""Score RTMW hand regions, with one YOLO body-box fallback per person."""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
sequence: DoorMaskSequence,
|
||||
warning_relative: float = 0.30,
|
||||
release_relative: float = 0.45,
|
||||
):
|
||||
if warning_relative < 0 or release_relative <= warning_relative:
|
||||
raise ValueError("Risk thresholds require 0 <= warning < release")
|
||||
self.sequence = sequence
|
||||
self.warning_relative = float(warning_relative)
|
||||
self.release_relative = float(release_relative)
|
||||
self.mask_status_counts: Counter = Counter()
|
||||
self.level_counts: Counter = Counter()
|
||||
self.source_counts: Counter = Counter()
|
||||
self.maximum_score = 0.0
|
||||
self.timing_seconds: Counter = Counter()
|
||||
self.timing_counts: Counter = Counter()
|
||||
self._constant_cache_shape: tuple[int, int] | None = None
|
||||
self._constant_loaded: LoadedDoorMask | None = None
|
||||
self._constant_distance_map: np.ndarray | None = None
|
||||
self.distance_map_cache_builds = 0
|
||||
|
||||
def _record_timing(self, name: str, started: float) -> None:
|
||||
self.timing_seconds[name] += time.perf_counter() - started
|
||||
self.timing_counts[name] += 1
|
||||
|
||||
def _measure_region(
|
||||
self,
|
||||
region: np.ndarray,
|
||||
door_mask: np.ndarray,
|
||||
distance_map: np.ndarray,
|
||||
source: str,
|
||||
track_id: int,
|
||||
side: str,
|
||||
state: str | None,
|
||||
) -> dict:
|
||||
area_px = int(np.count_nonzero(region))
|
||||
overlap_px = int(np.count_nonzero(region & door_mask))
|
||||
distances = distance_map[region]
|
||||
distance_px = float(np.min(distances)) if len(distances) else math.inf
|
||||
scale_px = max(math.sqrt(area_px), 1.0)
|
||||
distance_relative = distance_px / scale_px
|
||||
score = float(np.clip(1.0 - distance_relative / self.release_relative, 0.0, 1.0))
|
||||
if overlap_px:
|
||||
score = 1.0
|
||||
level, decision = _decision(
|
||||
distance_relative,
|
||||
overlap_px,
|
||||
self.warning_relative,
|
||||
self.release_relative,
|
||||
)
|
||||
result = {
|
||||
"status": "calculated",
|
||||
"risk_score": round(score, 5),
|
||||
"risk_level": level,
|
||||
"decision": decision,
|
||||
"source": source,
|
||||
"track_id": int(track_id),
|
||||
"side": side,
|
||||
"state": state,
|
||||
"region_area_px": area_px,
|
||||
"door_overlap_px": overlap_px,
|
||||
"region_overlap_ratio": round(overlap_px / max(area_px, 1), 6),
|
||||
"min_distance_px": round(distance_px, 3),
|
||||
"region_scale_px": round(scale_px, 3),
|
||||
"distance_relative": round(distance_relative, 6),
|
||||
}
|
||||
self.level_counts[level] += 1
|
||||
self.source_counts[source] += 1
|
||||
self.maximum_score = max(self.maximum_score, score)
|
||||
return result
|
||||
|
||||
@staticmethod
|
||||
def _unavailable(source: str, track_id: int, side: str, status: str) -> dict:
|
||||
return {
|
||||
"status": status,
|
||||
"risk_score": None,
|
||||
"risk_level": "unavailable",
|
||||
"decision": "EVIDENCE_UNAVAILABLE",
|
||||
"source": source,
|
||||
"track_id": int(track_id),
|
||||
"side": side,
|
||||
}
|
||||
|
||||
def evaluate(
|
||||
self,
|
||||
frame_index: int,
|
||||
record: dict,
|
||||
frame_shape: tuple[int, int],
|
||||
) -> tuple[dict, np.ndarray | None]:
|
||||
total_started = time.perf_counter()
|
||||
stage_started = time.perf_counter()
|
||||
use_constant_cache = self.sequence.constant_path is not None
|
||||
if use_constant_cache and self._constant_cache_shape == frame_shape:
|
||||
cached = self._constant_loaded
|
||||
assert cached is not None
|
||||
loaded = LoadedDoorMask(
|
||||
cached.status,
|
||||
frame_index,
|
||||
cached.path,
|
||||
cached.mask,
|
||||
cached.source_format,
|
||||
)
|
||||
else:
|
||||
loaded = self.sequence.load(frame_index, frame_shape)
|
||||
if use_constant_cache:
|
||||
self._constant_cache_shape = frame_shape
|
||||
self._constant_loaded = loaded
|
||||
self._constant_distance_map = None
|
||||
self._record_timing("door_mask_load", stage_started)
|
||||
self.mask_status_counts[loaded.status] += 1
|
||||
mask_ready = loaded.status == "loaded" and loaded.mask is not None
|
||||
door_mask = loaded.mask if mask_ready else None
|
||||
distance_map = None
|
||||
if door_mask is not None and use_constant_cache and self._constant_distance_map is not None:
|
||||
distance_map = self._constant_distance_map
|
||||
elif door_mask is not None:
|
||||
stage_started = time.perf_counter()
|
||||
distance_map = cv2.distanceTransform(
|
||||
(~door_mask).astype(np.uint8), cv2.DIST_L2, cv2.DIST_MASK_PRECISE
|
||||
)
|
||||
self._record_timing("distance_transform", stage_started)
|
||||
if use_constant_cache:
|
||||
self._constant_distance_map = distance_map
|
||||
self.distance_map_cache_builds += 1
|
||||
|
||||
stage_started = time.perf_counter()
|
||||
regions: list[tuple[dict, np.ndarray, str, int, str, str | None]] = []
|
||||
track_ids = [int(value) for value in record.get("track_ids", [])]
|
||||
person_positions_with_hands: set[int] = set()
|
||||
for hand in record.get("global_hands", []):
|
||||
position = int(hand.get("person_id", 0)) - 1
|
||||
track_id = track_ids[position] if 0 <= position < len(track_ids) else position + 1
|
||||
side = str(hand.get("side", "unknown"))
|
||||
region = _hand_mask(hand.get("keypoints", []), frame_shape)
|
||||
if region is None:
|
||||
continue
|
||||
person_positions_with_hands.add(position)
|
||||
regions.append(
|
||||
(
|
||||
hand,
|
||||
region,
|
||||
"RTMW_HAND_KEYPOINTS",
|
||||
track_id,
|
||||
side,
|
||||
str(hand.get("state", "unknown")),
|
||||
)
|
||||
)
|
||||
|
||||
boxes = record.get("boxes", [])
|
||||
body_fallbacks = []
|
||||
for position, box in enumerate(boxes):
|
||||
if position in person_positions_with_hands:
|
||||
continue
|
||||
region = _box_mask(box, frame_shape)
|
||||
if region is None:
|
||||
continue
|
||||
track_id = track_ids[position] if position < len(track_ids) else position + 1
|
||||
fallback = {
|
||||
"track_id": track_id,
|
||||
"side": "body",
|
||||
"state": "rtmw_hand_unavailable",
|
||||
"body_zone_box": box,
|
||||
}
|
||||
body_fallbacks.append(fallback)
|
||||
regions.append(
|
||||
(
|
||||
fallback,
|
||||
region,
|
||||
"YOLO_BODY_BOX_FALLBACK",
|
||||
track_id,
|
||||
"body",
|
||||
"rtmw_hand_unavailable",
|
||||
)
|
||||
)
|
||||
record["body_fallbacks"] = body_fallbacks
|
||||
self._record_timing("region_build", stage_started)
|
||||
|
||||
stage_started = time.perf_counter()
|
||||
measurements = []
|
||||
unavailable_status = f"door_mask_{loaded.status}"
|
||||
for owner, region, source, track_id, side, state in regions:
|
||||
if door_mask is None or distance_map is None:
|
||||
measurement = self._unavailable(
|
||||
source, track_id, side, unavailable_status
|
||||
)
|
||||
else:
|
||||
measurement = self._measure_region(
|
||||
region, door_mask, distance_map, source, track_id, side, state
|
||||
)
|
||||
owner["risk"] = measurement
|
||||
measurements.append(measurement)
|
||||
self._record_timing("region_measure", stage_started)
|
||||
|
||||
calculated = [item for item in measurements if item["risk_score"] is not None]
|
||||
nearest = max(calculated, key=lambda item: item["risk_score"]) if calculated else None
|
||||
if loaded.status != "loaded":
|
||||
status = unavailable_status
|
||||
elif not calculated:
|
||||
status = "person_region_missing"
|
||||
else:
|
||||
status = "calculated"
|
||||
frame_risk = {
|
||||
"status": status,
|
||||
"risk_score": nearest["risk_score"] if nearest else None,
|
||||
"risk_level": nearest["risk_level"] if nearest else "unavailable",
|
||||
"decision": nearest["decision"] if nearest else "EVIDENCE_UNAVAILABLE",
|
||||
"source": nearest["source"] if nearest else None,
|
||||
"track_id": nearest["track_id"] if nearest else None,
|
||||
"side": nearest["side"] if nearest else None,
|
||||
"door_mask": loaded.metadata(),
|
||||
"warning_distance_relative": self.warning_relative,
|
||||
"release_distance_relative": self.release_relative,
|
||||
"regions": measurements,
|
||||
}
|
||||
self._record_timing("risk_total", total_started)
|
||||
return frame_risk, loaded.mask
|
||||
|
||||
def summary(self) -> dict:
|
||||
return {
|
||||
"mask_directory": str(self.sequence.directory.resolve()),
|
||||
"constant_mask": (
|
||||
str(self.sequence.constant_path)
|
||||
if self.sequence.constant_path is not None
|
||||
else None
|
||||
),
|
||||
"constant_for_all_frames": self.sequence.constant_path is not None,
|
||||
"mask_format": self.sequence.source_format,
|
||||
"door_labels": sorted(self.sequence.labels),
|
||||
"frame_offset": self.sequence.frame_offset,
|
||||
"warning_distance_relative": self.warning_relative,
|
||||
"release_distance_relative": self.release_relative,
|
||||
"mask_status_counts": dict(self.mask_status_counts),
|
||||
"risk_level_counts": dict(self.level_counts),
|
||||
"source_counts": dict(self.source_counts),
|
||||
"maximum_risk_score": round(self.maximum_score, 5),
|
||||
"constant_distance_map_cached": self.sequence.constant_path is not None,
|
||||
"distance_map_cache_builds": self.distance_map_cache_builds,
|
||||
"timing_ms": {
|
||||
name: {
|
||||
"count": int(self.timing_counts[name]),
|
||||
"total": round(float(seconds) * 1000.0, 3),
|
||||
"mean": round(
|
||||
float(seconds) * 1000.0
|
||||
/ max(int(self.timing_counts[name]), 1),
|
||||
3,
|
||||
),
|
||||
}
|
||||
for name, seconds in self.timing_seconds.items()
|
||||
},
|
||||
"score_semantics": "2-D image overlap/proximity research score; not millimeters or safety certification",
|
||||
"region_policy": "RTMW alarm-eligible hand hulls; YOLO body box only when a person has no usable RTMW hand region",
|
||||
}
|
||||
@@ -1,358 +0,0 @@
|
||||
"""Multi-feature person-to-door assignment and active-CAN risk gating."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import math
|
||||
from collections import Counter, defaultdict, deque
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from statistics import median
|
||||
|
||||
|
||||
DOORS = ("front", "rear")
|
||||
POSE_FEATURES = {
|
||||
"shoulder_center_x",
|
||||
"shoulder_center_y",
|
||||
"shoulder_width",
|
||||
"torso_height",
|
||||
"hip_center_y",
|
||||
}
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class FeatureCalibration:
|
||||
rear: float
|
||||
front: float
|
||||
weight: float
|
||||
|
||||
def frontness(self, value: float) -> float:
|
||||
span = self.front - self.rear
|
||||
if span == 0:
|
||||
return 0.5
|
||||
return min(1.0, max(0.0, (value - self.rear) / span))
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class DoorZoneConfig:
|
||||
features: dict[str, FeatureCalibration]
|
||||
front_probability_min: float
|
||||
rear_probability_max: float
|
||||
minimum_person_height: float
|
||||
minimum_pose_features: int
|
||||
history_frames: int
|
||||
stale_track_frames: int
|
||||
source_path: Path | None = None
|
||||
|
||||
@classmethod
|
||||
def load(cls, path: Path) -> "DoorZoneConfig":
|
||||
source = Path(path).expanduser().resolve()
|
||||
data = json.loads(source.read_text(encoding="utf-8-sig"))
|
||||
features = {
|
||||
name: FeatureCalibration(
|
||||
rear=float(values["rear"]),
|
||||
front=float(values["front"]),
|
||||
weight=float(values["weight"]),
|
||||
)
|
||||
for name, values in data["features"].items()
|
||||
}
|
||||
return cls(
|
||||
features=features,
|
||||
front_probability_min=float(data.get("front_probability_min", 0.72)),
|
||||
rear_probability_max=float(data.get("rear_probability_max", 0.28)),
|
||||
minimum_person_height=float(data.get("minimum_person_height", 0.40)),
|
||||
minimum_pose_features=int(data.get("minimum_pose_features", 4)),
|
||||
history_frames=int(data.get("history_frames", 5)),
|
||||
stale_track_frames=int(data.get("stale_track_frames", 120)),
|
||||
source_path=source,
|
||||
)
|
||||
|
||||
def classify(self, probability: float) -> str:
|
||||
if probability >= self.front_probability_min:
|
||||
return "front"
|
||||
if probability <= self.rear_probability_max:
|
||||
return "rear"
|
||||
return "transition"
|
||||
|
||||
|
||||
def _point(keypoints: list, index: int) -> tuple[float, float] | None:
|
||||
if index >= len(keypoints) or len(keypoints[index]) < 2:
|
||||
return None
|
||||
x, y = float(keypoints[index][0]), float(keypoints[index][1])
|
||||
return (x, y) if math.isfinite(x) and math.isfinite(y) else None
|
||||
|
||||
|
||||
def extract_person_features(
|
||||
record: dict,
|
||||
position: int,
|
||||
frame_shape: tuple[int, int],
|
||||
) -> tuple[dict[str, float], int]:
|
||||
"""Extract fixed-camera pose features, with box geometry as low-weight fallback."""
|
||||
|
||||
height, width = frame_shape
|
||||
box = record.get("boxes", [])[position]
|
||||
x1, y1, x2, y2 = [float(value) for value in box[:4]]
|
||||
features = {
|
||||
"box_center_x": ((x1 + x2) * 0.5) / max(width, 1),
|
||||
"box_height": (y2 - y1) / max(height, 1),
|
||||
}
|
||||
|
||||
all_keypoints = record.get("temporal_selected_keypoints", [])
|
||||
if position >= len(all_keypoints):
|
||||
return features, 0
|
||||
keypoints = all_keypoints[position]
|
||||
left_shoulder = _point(keypoints, 5)
|
||||
right_shoulder = _point(keypoints, 6)
|
||||
left_hip = _point(keypoints, 11)
|
||||
right_hip = _point(keypoints, 12)
|
||||
if None in (left_shoulder, right_shoulder, left_hip, right_hip):
|
||||
return features, 0
|
||||
|
||||
shoulder_x = (left_shoulder[0] + right_shoulder[0]) * 0.5
|
||||
shoulder_y = (left_shoulder[1] + right_shoulder[1]) * 0.5
|
||||
hip_y = (left_hip[1] + right_hip[1]) * 0.5
|
||||
features.update(
|
||||
{
|
||||
"shoulder_center_x": shoulder_x / max(width, 1),
|
||||
"shoulder_center_y": shoulder_y / max(height, 1),
|
||||
"shoulder_width": math.hypot(
|
||||
left_shoulder[0] - right_shoulder[0],
|
||||
left_shoulder[1] - right_shoulder[1],
|
||||
)
|
||||
/ max(width, 1),
|
||||
"torso_height": abs(hip_y - shoulder_y) / max(height, 1),
|
||||
"hip_center_y": hip_y / max(height, 1),
|
||||
}
|
||||
)
|
||||
return features, len(POSE_FEATURES)
|
||||
|
||||
|
||||
class DoorZoneAssigner:
|
||||
"""Fuse pose-dominant evidence and smooth front probability per track."""
|
||||
|
||||
def __init__(self, config: DoorZoneConfig):
|
||||
self.config = config
|
||||
self.history: dict[int, deque[float]] = defaultdict(
|
||||
lambda: deque(maxlen=self.config.history_frames)
|
||||
)
|
||||
self.last_seen: dict[int, int] = {}
|
||||
self.zone_counts: Counter = Counter()
|
||||
|
||||
def _score(self, features: dict[str, float]) -> tuple[float, dict[str, float]]:
|
||||
weighted_sum = 0.0
|
||||
total_weight = 0.0
|
||||
contributions = {}
|
||||
for name, calibration in self.config.features.items():
|
||||
if name not in features:
|
||||
continue
|
||||
frontness = calibration.frontness(float(features[name]))
|
||||
contributions[name] = round(frontness, 6)
|
||||
weighted_sum += frontness * calibration.weight
|
||||
total_weight += calibration.weight
|
||||
probability = weighted_sum / total_weight if total_weight else 0.5
|
||||
return probability, contributions
|
||||
|
||||
def update(
|
||||
self,
|
||||
frame_index: int,
|
||||
record: dict,
|
||||
frame_shape: tuple[int, int],
|
||||
) -> list[dict]:
|
||||
track_ids = [int(value) for value in record.get("track_ids", [])]
|
||||
assignments = []
|
||||
for position, _box in enumerate(record.get("boxes", [])):
|
||||
track_id = track_ids[position] if position < len(track_ids) else position + 1
|
||||
features, pose_feature_count = extract_person_features(
|
||||
record, position, frame_shape
|
||||
)
|
||||
probability, contributions = self._score(features)
|
||||
enough_pose = pose_feature_count >= self.config.minimum_pose_features
|
||||
if features["box_height"] < self.config.minimum_person_height:
|
||||
raw_zone = "unassigned"
|
||||
door_zone = "unassigned"
|
||||
elif not enough_pose:
|
||||
raw_zone = "transition"
|
||||
door_zone = "transition"
|
||||
else:
|
||||
raw_zone = self.config.classify(probability)
|
||||
values = self.history[track_id]
|
||||
values.append(probability)
|
||||
door_zone = self.config.classify(float(median(values)))
|
||||
|
||||
values = self.history.get(track_id, ())
|
||||
smoothed_probability = (
|
||||
float(median(values)) if values else float(probability)
|
||||
)
|
||||
self.last_seen[track_id] = int(frame_index)
|
||||
self.zone_counts[door_zone] += 1
|
||||
assignments.append(
|
||||
{
|
||||
"track_id": track_id,
|
||||
"person_index": position,
|
||||
"door_zone": door_zone,
|
||||
"raw_door_zone": raw_zone,
|
||||
"front_probability": round(float(probability), 6),
|
||||
"smoothed_front_probability": round(smoothed_probability, 6),
|
||||
"confidence": round(abs(smoothed_probability - 0.5) * 2.0, 6),
|
||||
"evidence_source": "pose_primary_bbox_secondary",
|
||||
"pose_feature_count": pose_feature_count,
|
||||
"features": {name: round(value, 6) for name, value in features.items()},
|
||||
"feature_frontness": contributions,
|
||||
"history_frames": len(values),
|
||||
}
|
||||
)
|
||||
|
||||
expired = [
|
||||
track_id
|
||||
for track_id, seen_at in self.last_seen.items()
|
||||
if frame_index - seen_at > self.config.stale_track_frames
|
||||
]
|
||||
for track_id in expired:
|
||||
self.history.pop(track_id, None)
|
||||
self.last_seen.pop(track_id, None)
|
||||
return assignments
|
||||
|
||||
def summary(self) -> dict:
|
||||
return {
|
||||
"policy": "pose-primary weighted front probability with causal median",
|
||||
"config": str(self.config.source_path) if self.config.source_path else None,
|
||||
"history_frames": self.config.history_frames,
|
||||
"front_probability_min": self.config.front_probability_min,
|
||||
"rear_probability_max": self.config.rear_probability_max,
|
||||
"minimum_person_height": self.config.minimum_person_height,
|
||||
"minimum_pose_features": self.config.minimum_pose_features,
|
||||
"features": {
|
||||
name: {
|
||||
"rear": calibration.rear,
|
||||
"front": calibration.front,
|
||||
"weight": calibration.weight,
|
||||
}
|
||||
for name, calibration in self.config.features.items()
|
||||
},
|
||||
"zone_counts": dict(self.zone_counts),
|
||||
}
|
||||
|
||||
|
||||
def _inactive_risk(door: str) -> dict:
|
||||
return {
|
||||
"door_id": door,
|
||||
"status": "inactive",
|
||||
"risk_score": None,
|
||||
"risk_level": "inactive",
|
||||
"decision": "DOOR_CAN_NOT_ACTIVE",
|
||||
"risk_type": f"{door.upper()}_INACTIVE",
|
||||
"source": None,
|
||||
"track_id": None,
|
||||
"side": None,
|
||||
"regions": [],
|
||||
}
|
||||
|
||||
|
||||
def _clear_risk(door: str) -> dict:
|
||||
return {
|
||||
"door_id": door,
|
||||
"status": "calculated",
|
||||
"risk_score": 0.0,
|
||||
"risk_level": "clear",
|
||||
"decision": "CLEAR_NO_PERSON_ASSIGNED_TO_ACTIVE_DOOR",
|
||||
"risk_type": f"{door.upper()}_CLEAR",
|
||||
"source": None,
|
||||
"track_id": None,
|
||||
"side": None,
|
||||
"regions": [],
|
||||
}
|
||||
|
||||
|
||||
def _unknown_risk(door: str, transition_regions: list[dict]) -> dict:
|
||||
return {
|
||||
"door_id": door,
|
||||
"status": "door_zone_ambiguous",
|
||||
"risk_score": None,
|
||||
"risk_level": "unknown",
|
||||
"decision": "EVIDENCE_UNAVAILABLE_TRANSITION_ZONE",
|
||||
"risk_type": f"{door.upper()}_UNKNOWN",
|
||||
"source": None,
|
||||
"track_id": None,
|
||||
"side": None,
|
||||
"regions": transition_regions,
|
||||
}
|
||||
|
||||
|
||||
def build_door_risks(
|
||||
raw_risk: dict,
|
||||
assignments: list[dict],
|
||||
active_door: str,
|
||||
) -> tuple[dict[str, dict], dict]:
|
||||
"""Build per-door risk and select the CAN-active door result."""
|
||||
|
||||
zone_by_track = {
|
||||
int(assignment["track_id"]): str(assignment["door_zone"])
|
||||
for assignment in assignments
|
||||
}
|
||||
annotated_regions = []
|
||||
for original in raw_risk.get("regions", []):
|
||||
region = dict(original)
|
||||
region["door_zone"] = zone_by_track.get(int(region.get("track_id", -1)), "unknown")
|
||||
annotated_regions.append(region)
|
||||
|
||||
active_doors = set(DOORS) if active_door == "both" else {active_door}
|
||||
if active_door == "none":
|
||||
active_doors.clear()
|
||||
|
||||
door_risks: dict[str, dict] = {}
|
||||
for door in DOORS:
|
||||
if door not in active_doors:
|
||||
door_risks[door] = _inactive_risk(door)
|
||||
continue
|
||||
exact = [region for region in annotated_regions if region["door_zone"] == door]
|
||||
transition = [
|
||||
region for region in annotated_regions if region["door_zone"] == "transition"
|
||||
]
|
||||
calculated = [region for region in exact if region.get("risk_score") is not None]
|
||||
if calculated:
|
||||
nearest = max(calculated, key=lambda item: float(item["risk_score"]))
|
||||
level = str(nearest["risk_level"])
|
||||
if transition and level in ("clear", "watch"):
|
||||
door_risks[door] = _unknown_risk(door, transition)
|
||||
else:
|
||||
door_risks[door] = {
|
||||
"door_id": door,
|
||||
"status": "calculated",
|
||||
"risk_score": nearest["risk_score"],
|
||||
"risk_level": level,
|
||||
"decision": nearest["decision"],
|
||||
"risk_type": f"{door.upper()}_{level.upper()}",
|
||||
"source": nearest["source"],
|
||||
"track_id": nearest["track_id"],
|
||||
"side": nearest["side"],
|
||||
"regions": exact,
|
||||
}
|
||||
elif transition:
|
||||
door_risks[door] = _unknown_risk(door, transition)
|
||||
else:
|
||||
door_risks[door] = _clear_risk(door)
|
||||
|
||||
if len(active_doors) == 1:
|
||||
selected = dict(door_risks[next(iter(active_doors))])
|
||||
elif not active_doors:
|
||||
selected = {
|
||||
"door_id": None,
|
||||
"status": "inactive",
|
||||
"risk_score": None,
|
||||
"risk_level": "inactive",
|
||||
"decision": "NO_DOOR_CAN_ACTIVE",
|
||||
"risk_type": "NO_ACTIVE_DOOR",
|
||||
"source": None,
|
||||
"track_id": None,
|
||||
"side": None,
|
||||
"regions": [],
|
||||
}
|
||||
else:
|
||||
severity = {"danger": 4, "warning": 3, "watch": 2, "unknown": 1, "clear": 0}
|
||||
selected = dict(
|
||||
max(door_risks.values(), key=lambda item: severity.get(item["risk_level"], -1))
|
||||
)
|
||||
selected["door_mask"] = raw_risk.get("door_mask")
|
||||
selected["warning_distance_relative"] = raw_risk.get("warning_distance_relative")
|
||||
selected["release_distance_relative"] = raw_risk.get("release_distance_relative")
|
||||
return door_risks, selected
|
||||
@@ -1,64 +0,0 @@
|
||||
"""One owned, immutable latest result, mirroring the 0920 LatestResult contract."""
|
||||
from __future__ import annotations
|
||||
|
||||
from collections.abc import Mapping
|
||||
import threading
|
||||
from types import MappingProxyType
|
||||
|
||||
|
||||
def freeze(value):
|
||||
if isinstance(value, Mapping):
|
||||
return MappingProxyType({key: freeze(item) for key, item in value.items()})
|
||||
if isinstance(value, (list, tuple)):
|
||||
return tuple(freeze(item) for item in value)
|
||||
return value
|
||||
|
||||
|
||||
def result_to_dict(value):
|
||||
"""Return a mutable JSON-compatible copy when a consumer needs serialization."""
|
||||
if isinstance(value, Mapping):
|
||||
return {key: result_to_dict(item) for key, item in value.items()}
|
||||
if isinstance(value, tuple):
|
||||
return [result_to_dict(item) for item in value]
|
||||
return value
|
||||
|
||||
|
||||
class LatestResult:
|
||||
"""Readers share a stable snapshot; reading never removes or acknowledges it.
|
||||
|
||||
Only the latest result is retained here. Slow readers can skip frames, and
|
||||
references they already hold remain valid after subsequent publications.
|
||||
"""
|
||||
|
||||
def __init__(self):
|
||||
self._condition = threading.Condition()
|
||||
self._value = None
|
||||
self._closed = False
|
||||
|
||||
def publish(self, result):
|
||||
if result is None:
|
||||
raise ValueError("Cannot publish a null result")
|
||||
snapshot = freeze(result)
|
||||
with self._condition:
|
||||
if self._closed:
|
||||
raise RuntimeError("Result publisher is closed")
|
||||
self._value = snapshot
|
||||
self._condition.notify_all()
|
||||
|
||||
def load(self):
|
||||
with self._condition:
|
||||
return self._value
|
||||
|
||||
def wait(self, after_frame_index=-1, timeout=None):
|
||||
"""Wait for a newer result; return None on timeout/end without a new frame."""
|
||||
with self._condition:
|
||||
def has_new_frame():
|
||||
return self._value is not None and self._value['frame_index'] > after_frame_index
|
||||
|
||||
self._condition.wait_for(lambda: has_new_frame() or self._closed, timeout)
|
||||
return self._value if has_new_frame() else None
|
||||
|
||||
def close(self):
|
||||
with self._condition:
|
||||
self._closed = True
|
||||
self._condition.notify_all()
|
||||
@@ -1,66 +0,0 @@
|
||||
"""Native Jetson TensorRT adapter; temporal/risk policy stays in upstream Python."""
|
||||
from __future__ import annotations
|
||||
import ctypes as C
|
||||
from pathlib import Path
|
||||
import numpy as np
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
|
||||
class NativeBackend:
|
||||
def __init__(self, args):
|
||||
self.lib = C.CDLL(str(ROOT / 'build/libvision_native.so'))
|
||||
pointer = C.c_void_p
|
||||
signatures = {
|
||||
'vap_error': ([], C.c_char_p),
|
||||
'vap_engine': ([C.c_char_p,C.c_int,C.c_int,C.c_int,C.c_float],pointer),
|
||||
'vap_engine_close': ([pointer],None),
|
||||
'vap_detect': ([pointer,pointer,C.c_int,C.c_int,pointer],C.c_int),
|
||||
'vap_pose': ([pointer,pointer,C.c_int,C.c_int,pointer,C.c_int,pointer],C.c_int),
|
||||
'vap_camera': ([C.c_char_p,C.c_int,C.c_int,C.c_int,C.c_int],pointer),
|
||||
'vap_camera_close': ([pointer],None),
|
||||
'vap_read': ([pointer,pointer,pointer],C.c_int),
|
||||
'vap_can': ([pointer],None),
|
||||
}
|
||||
for name,(inputs,output) in signatures.items():
|
||||
fn=getattr(self.lib,name);fn.argtypes=inputs;fn.restype=output
|
||||
self.engine = self.lib.vap_engine(str(args.engines.resolve()).encode(),args.width,args.height,args.max_people,args.det_conf)
|
||||
if not self.engine: self.fail()
|
||||
self.camera=None
|
||||
self.args=args
|
||||
|
||||
def fail(self):
|
||||
raise RuntimeError(self.lib.vap_error().decode())
|
||||
|
||||
def open_camera(self):
|
||||
a=self.args
|
||||
self.camera=self.lib.vap_camera(a.camera.encode(),a.width,a.height,a.fps,not a.no_can)
|
||||
if not self.camera:self.fail()
|
||||
|
||||
def read(self):
|
||||
image=np.empty((self.args.height,self.args.width,3),np.uint8)
|
||||
meta=np.zeros(4,np.float64)
|
||||
result=self.lib.vap_read(self.camera,image.ctypes.data,meta.ctypes.data)
|
||||
if result<0:self.fail()
|
||||
if result==0:return None,None
|
||||
return image,{'sequence':int(meta[0]),'capture_monotonic_ms':float(meta[1]),'dropped_before_inference':int(meta[2]),'driver_timestamp_ms':float(meta[3]),'latency_clock':'host_monotonic_at_dequeue'}
|
||||
|
||||
def can_snapshot(self):
|
||||
data=np.zeros(5,np.float64);self.lib.vap_can(data.ctypes.data)
|
||||
return {'positions':[int(x) for x in data[:4]],'age_ms':float(data[4])}
|
||||
|
||||
def detect(self, frame):
|
||||
frame=np.ascontiguousarray(frame);out=np.empty((self.args.max_people,5),np.float32)
|
||||
n=self.lib.vap_detect(self.engine,frame.ctypes.data,frame.shape[1],frame.shape[0],out.ctypes.data)
|
||||
if n<0:self.fail()
|
||||
return out[:n,:4].tolist(),out[:n,4].tolist()
|
||||
|
||||
def pose(self,frame,bboxes):
|
||||
frame=np.ascontiguousarray(frame);boxes=np.ascontiguousarray(bboxes,dtype=np.float32)
|
||||
out=np.empty((len(boxes),133,3),np.float32)
|
||||
n=self.lib.vap_pose(self.engine,frame.ctypes.data,frame.shape[1],frame.shape[0],boxes.ctypes.data,len(boxes),out.ctypes.data)
|
||||
if n<0:self.fail()
|
||||
return out[:n,:,:2].astype(np.float64),out[:n,:,2].astype(np.float64)
|
||||
|
||||
def close(self):
|
||||
if self.camera:self.lib.vap_camera_close(self.camera);self.camera=None
|
||||
if self.engine:self.lib.vap_engine_close(self.engine);self.engine=None
|
||||
@@ -1,148 +0,0 @@
|
||||
"""Resident native TensorRT inference with causal pose fusion and tracking."""
|
||||
from __future__ import annotations
|
||||
import time
|
||||
from collections import defaultdict, deque
|
||||
import numpy as np
|
||||
from pose_tracking import BoxTracker
|
||||
|
||||
SIDES = ("left", "right")
|
||||
|
||||
class NativePoseEngine:
|
||||
def __init__(self, args, native):
|
||||
self.args = args
|
||||
self.native = native
|
||||
self.pose = native.pose
|
||||
self.box_tracker = BoxTracker(max_age=30)
|
||||
self.people = {}
|
||||
self.timings = defaultdict(lambda: deque(maxlen=3000))
|
||||
self.isolated_inferences = 0
|
||||
|
||||
def infer(self, frame: np.ndarray, frame_idx: int) -> dict:
|
||||
from pose_geometry import HAND_SLICES
|
||||
from pose_fusion import adaptive_smooth, build_global_hands, fuse_person
|
||||
from pose_occlusion import Measurement, resolve_global_hand_assignments
|
||||
from pose_tracking import (
|
||||
LivePersonState,
|
||||
mask_other_people,
|
||||
update_live_v2,
|
||||
)
|
||||
|
||||
infer_started = time.perf_counter()
|
||||
height, width = frame.shape[:2]
|
||||
started = time.perf_counter()
|
||||
detection_height = max(
|
||||
1, min(height, round(height * self.args.det_roi_y_max_ratio))
|
||||
)
|
||||
boxes, confidences = self.native.detect(frame[:detection_height])
|
||||
self.timings["detector_ms"].append((time.perf_counter() - started) * 1000)
|
||||
track_ids = self.box_tracker.update(boxes, frame_idx, (height, width))
|
||||
|
||||
# Retire disappeared people in long-running camera sessions.
|
||||
for stale_id in list(self.people):
|
||||
if stale_id not in self.box_tracker.tracks:
|
||||
del self.people[stale_id]
|
||||
if boxes:
|
||||
started = time.perf_counter()
|
||||
raw_keypoints, raw_scores = self.pose(frame, bboxes=boxes)
|
||||
self.timings["raw_pose_ms"].append((time.perf_counter() - started) * 1000)
|
||||
isolated_keypoints = raw_keypoints.copy()
|
||||
isolated_scores = raw_scores.copy()
|
||||
started = time.perf_counter()
|
||||
for person_idx, bbox in enumerate(boxes):
|
||||
target_area = max((bbox[2] - bbox[0]) * (bbox[3] - bbox[1]), 1.0)
|
||||
needs_isolation = any(
|
||||
idx != person_idx
|
||||
and max((other[2] - other[0]) * (other[3] - other[1]), 1.0)
|
||||
< 0.5 * target_area
|
||||
for idx, other in enumerate(boxes)
|
||||
)
|
||||
if not needs_isolation:
|
||||
continue
|
||||
masked = mask_other_people(frame, boxes, person_idx)
|
||||
keypoints, scores = self.pose(masked, bboxes=[bbox])
|
||||
isolated_keypoints[person_idx] = keypoints[0]
|
||||
isolated_scores[person_idx] = scores[0]
|
||||
self.isolated_inferences += 1
|
||||
self.timings["isolated_extra_ms"].append(
|
||||
(time.perf_counter() - started) * 1000
|
||||
)
|
||||
else:
|
||||
raw_keypoints = np.empty((0, 133, 2), dtype=np.float64)
|
||||
raw_scores = np.empty((0, 133), dtype=np.float64)
|
||||
isolated_keypoints = raw_keypoints.copy()
|
||||
isolated_scores = raw_scores.copy()
|
||||
|
||||
selected_list, smoothed_list, metadata_list, states = [], [], [], []
|
||||
for person_idx, (bbox, track_id) in enumerate(zip(boxes, track_ids)):
|
||||
state = self.people.setdefault(track_id, LivePersonState())
|
||||
selected, metadata = fuse_person(
|
||||
raw_keypoints[person_idx],
|
||||
raw_scores[person_idx],
|
||||
isolated_keypoints[person_idx],
|
||||
isolated_scores[person_idx],
|
||||
bbox,
|
||||
state.fusion,
|
||||
)
|
||||
selected_list.append(selected)
|
||||
smoothed_list.append(adaptive_smooth(selected, bbox, state.fusion))
|
||||
metadata_list.append(metadata)
|
||||
states.append(state)
|
||||
|
||||
measurements, endpoint_tracks = {}, {}
|
||||
for person_idx, (selected, metadata, state) in enumerate(
|
||||
zip(selected_list, metadata_list, states)
|
||||
):
|
||||
for side, (start, end) in HAND_SLICES.items():
|
||||
meta = metadata[side]
|
||||
measurements[(person_idx, side)] = (
|
||||
Measurement(
|
||||
keypoints=selected[start:end],
|
||||
precise=bool(meta["distance_eligible"]),
|
||||
provisional=not bool(meta["distance_eligible"]),
|
||||
quality=float(meta.get("selected_hand_score", 0.0)),
|
||||
)
|
||||
if meta["alarm_eligible"]
|
||||
else None
|
||||
)
|
||||
endpoint_tracks[(person_idx, side)] = state.endpoints[side]
|
||||
measurements, assignments = resolve_global_hand_assignments(
|
||||
measurements, endpoint_tracks, boxes, 30
|
||||
)
|
||||
|
||||
v2_list, v2_meta_list = [], []
|
||||
for person_idx, (bbox, selected, smoothed, metadata, state) in enumerate(
|
||||
zip(boxes, selected_list, smoothed_list, metadata_list, states)
|
||||
):
|
||||
v2, v2_meta = update_live_v2(
|
||||
frame_idx,
|
||||
bbox,
|
||||
selected,
|
||||
smoothed,
|
||||
metadata,
|
||||
state,
|
||||
{side: measurements[(person_idx, side)] for side in SIDES},
|
||||
{side: assignments.get((person_idx, side)) for side in SIDES},
|
||||
12,
|
||||
30,
|
||||
)
|
||||
v2_list.append(v2)
|
||||
v2_meta_list.append(v2_meta)
|
||||
|
||||
self.timings["pose_pipeline_total_ms"].append(
|
||||
(time.perf_counter() - infer_started) * 1000
|
||||
)
|
||||
return {
|
||||
"frame_index": frame_idx,
|
||||
"track_ids": track_ids,
|
||||
"boxes": np.round(np.asarray(boxes), 3).tolist() if boxes else [],
|
||||
"det_confidences": [round(value, 4) for value in confidences],
|
||||
"raw_keypoints": np.round(raw_keypoints, 3).tolist(),
|
||||
"temporal_selected_keypoints": np.round(
|
||||
np.asarray(selected_list), 3
|
||||
).tolist(),
|
||||
"temporal_keypoints": np.round(np.asarray(smoothed_list), 3).tolist(),
|
||||
"temporal_metadata": metadata_list,
|
||||
"v2_keypoints": np.round(np.asarray(v2_list), 3).tolist(),
|
||||
"v2_metadata": v2_meta_list,
|
||||
"global_hands": build_global_hands(selected_list, metadata_list),
|
||||
}
|
||||
@@ -1,530 +0,0 @@
|
||||
"""Runtime pose fusion helpers."""
|
||||
from __future__ import annotations
|
||||
import math
|
||||
from dataclasses import dataclass, field
|
||||
import cv2
|
||||
import numpy as np
|
||||
from pose_geometry import BODY_WRISTS, HAND_EDGES_LOCAL, HAND_SLICES, normalized_keypoints
|
||||
|
||||
|
||||
BODY_SIDE_INDICES = {"left": (5, 7, 9), "right": (6, 8, 10)}
|
||||
|
||||
|
||||
SIDE_COLORS = {"left": (40, 40, 255), "right": (30, 230, 30)}
|
||||
|
||||
|
||||
HAND_COLORS = {"left": (255, 80, 220), "right": (255, 190, 40)}
|
||||
|
||||
|
||||
@dataclass
|
||||
class Candidate:
|
||||
source: str
|
||||
keypoints: np.ndarray
|
||||
scores: np.ndarray
|
||||
normalized: np.ndarray
|
||||
limb_score: float
|
||||
hand_score: float
|
||||
upper_length: float
|
||||
forearm_length: float
|
||||
hand_span: float
|
||||
wrist_link: float
|
||||
limb_valid: bool
|
||||
hand_valid: bool
|
||||
|
||||
|
||||
@dataclass
|
||||
class SideTrack:
|
||||
limb_previous: np.ndarray | None = None
|
||||
limb_velocity: np.ndarray | None = None
|
||||
hand_previous: np.ndarray | None = None
|
||||
hand_velocity: np.ndarray | None = None
|
||||
limb_source: str | None = None
|
||||
hand_source: str | None = None
|
||||
previous_hand_eligible: bool = False
|
||||
|
||||
|
||||
@dataclass
|
||||
class PersonTrack:
|
||||
sides: dict[str, SideTrack] = field(
|
||||
default_factory=lambda: {"left": SideTrack(), "right": SideTrack()}
|
||||
)
|
||||
smoothed: np.ndarray | None = None
|
||||
|
||||
|
||||
def bbox_diagonal(bbox: list[float]) -> float:
|
||||
return max(math.hypot(bbox[2] - bbox[0], bbox[3] - bbox[1]), 1.0)
|
||||
|
||||
|
||||
def build_candidate(
|
||||
source: str,
|
||||
keypoints: np.ndarray,
|
||||
scores: np.ndarray,
|
||||
bbox: list[float],
|
||||
side: str,
|
||||
) -> Candidate:
|
||||
normalized = normalized_keypoints(keypoints, bbox)
|
||||
shoulder, elbow, wrist = BODY_SIDE_INDICES[side]
|
||||
hand_start, hand_end = HAND_SLICES[side]
|
||||
hand = keypoints[hand_start:hand_end]
|
||||
diagonal = bbox_diagonal(bbox)
|
||||
upper_length = float(np.linalg.norm(normalized[shoulder] - normalized[elbow]))
|
||||
forearm_length = float(np.linalg.norm(normalized[elbow] - normalized[wrist]))
|
||||
hand_span = math.hypot(float(np.ptp(hand[:, 0])), float(np.ptp(hand[:, 1]))) / diagonal
|
||||
wrist_link = float(np.linalg.norm(keypoints[wrist] - hand[0])) / diagonal
|
||||
limb_score = float(np.mean(scores[[shoulder, elbow, wrist]]))
|
||||
hand_score = float(np.mean(scores[hand_start:hand_end]))
|
||||
limb_valid = 0.025 <= upper_length <= 0.8 and 0.025 <= forearm_length <= 0.8
|
||||
hand_valid = 0.008 <= hand_span <= 0.35 and wrist_link <= 0.12
|
||||
return Candidate(
|
||||
source=source,
|
||||
keypoints=keypoints,
|
||||
scores=scores,
|
||||
normalized=normalized,
|
||||
limb_score=limb_score,
|
||||
hand_score=hand_score,
|
||||
upper_length=upper_length,
|
||||
forearm_length=forearm_length,
|
||||
hand_span=hand_span,
|
||||
wrist_link=wrist_link,
|
||||
limb_valid=limb_valid,
|
||||
hand_valid=hand_valid,
|
||||
)
|
||||
|
||||
|
||||
def predicted(previous: np.ndarray | None, velocity: np.ndarray | None) -> np.ndarray | None:
|
||||
if previous is None:
|
||||
return None
|
||||
if velocity is None:
|
||||
return previous
|
||||
return previous + 0.65 * velocity
|
||||
|
||||
|
||||
def robust_distance(points: np.ndarray, target: np.ndarray | None) -> float:
|
||||
if target is None:
|
||||
return 0.0
|
||||
distances = np.linalg.norm(points - target, axis=1)
|
||||
return float(np.median(distances))
|
||||
|
||||
|
||||
def select_limb(
|
||||
candidates: list[Candidate],
|
||||
side: str,
|
||||
track: SideTrack | None,
|
||||
) -> tuple[Candidate, dict[str, float]]:
|
||||
indices = np.asarray(BODY_SIDE_INDICES[side])
|
||||
target = predicted(track.limb_previous, track.limb_velocity) if track else None
|
||||
scored: list[tuple[float, Candidate, float]] = []
|
||||
for candidate in candidates:
|
||||
temporal_cost = robust_distance(candidate.normalized[indices], target)
|
||||
geometry_bonus = 1.2 if candidate.limb_valid else -5.0
|
||||
switch_bonus = 0.35 if track and candidate.source == track.limb_source else 0.0
|
||||
score = candidate.limb_score + geometry_bonus + switch_bonus - 7.0 * temporal_cost
|
||||
scored.append((score, candidate, temporal_cost))
|
||||
score, selected, temporal_cost = max(scored, key=lambda item: item[0])
|
||||
return selected, {"score": score, "temporal_cost": temporal_cost}
|
||||
|
||||
|
||||
def hand_link_to_limb(
|
||||
candidate: Candidate,
|
||||
selected_limb: Candidate,
|
||||
bbox: list[float],
|
||||
side: str,
|
||||
) -> float:
|
||||
hand_start, _ = HAND_SLICES[side]
|
||||
wrist_idx = BODY_WRISTS[side]
|
||||
return float(
|
||||
np.linalg.norm(candidate.keypoints[hand_start] - selected_limb.keypoints[wrist_idx])
|
||||
) / bbox_diagonal(bbox)
|
||||
|
||||
|
||||
def select_hand(
|
||||
candidates: list[Candidate],
|
||||
selected_limb: Candidate,
|
||||
bbox: list[float],
|
||||
side: str,
|
||||
track: SideTrack | None,
|
||||
) -> tuple[Candidate, bool, dict[str, float]]:
|
||||
start, end = HAND_SLICES[side]
|
||||
target = predicted(track.hand_previous, track.hand_velocity) if track else None
|
||||
scored: list[tuple[float, Candidate, bool, float, float]] = []
|
||||
for candidate in candidates:
|
||||
temporal_cost = robust_distance(candidate.normalized[start:end], target)
|
||||
link = hand_link_to_limb(candidate, selected_limb, bbox, side)
|
||||
valid = candidate.hand_valid
|
||||
geometry_bonus = 3.0 if valid else -7.0
|
||||
switch_bonus = 0.45 if track and candidate.source == track.hand_source else 0.0
|
||||
limb_source_bonus = 0.15 if candidate.source == selected_limb.source else 0.0
|
||||
# A newly visible endpoint may disagree with the currently selected
|
||||
# (occlusion-driven) limb. Keep this as a soft cost so strong image
|
||||
# evidence can switch both the hand and its limb together.
|
||||
link_penalty = 1.5 * min(max(link - 0.12, 0.0), 0.2)
|
||||
score = (
|
||||
candidate.hand_score
|
||||
+ geometry_bonus
|
||||
+ switch_bonus
|
||||
+ limb_source_bonus
|
||||
- link_penalty
|
||||
- 8.0 * min(temporal_cost, 0.15)
|
||||
)
|
||||
scored.append((score, candidate, valid, temporal_cost, link))
|
||||
score, selected, valid, temporal_cost, link = max(scored, key=lambda item: item[0])
|
||||
return selected, valid, {
|
||||
"score": score,
|
||||
"temporal_cost": temporal_cost,
|
||||
"link_to_selected_limb": link,
|
||||
}
|
||||
|
||||
|
||||
def candidate_disagreement(
|
||||
raw: Candidate,
|
||||
isolated: Candidate,
|
||||
side: str,
|
||||
) -> tuple[float, float]:
|
||||
shoulder, elbow, wrist = BODY_SIDE_INDICES[side]
|
||||
start, end = HAND_SLICES[side]
|
||||
limb = float(
|
||||
np.median(
|
||||
np.linalg.norm(
|
||||
raw.normalized[[shoulder, elbow, wrist]]
|
||||
- isolated.normalized[[shoulder, elbow, wrist]],
|
||||
axis=1,
|
||||
)
|
||||
)
|
||||
)
|
||||
hand_distances = np.linalg.norm(
|
||||
raw.normalized[start:end] - isolated.normalized[start:end], axis=1
|
||||
)
|
||||
hand = float(np.percentile(hand_distances, 90))
|
||||
return limb, hand
|
||||
|
||||
|
||||
def suppress_duplicate_hands(
|
||||
fused: np.ndarray,
|
||||
metadata: dict,
|
||||
bbox: list[float],
|
||||
) -> None:
|
||||
left_start, left_end = HAND_SLICES["left"]
|
||||
right_start, right_end = HAND_SLICES["right"]
|
||||
left_center = np.mean(fused[left_start:left_end], axis=0)
|
||||
right_center = np.mean(fused[right_start:right_end], axis=0)
|
||||
separation = float(np.linalg.norm(left_center - right_center)) / bbox_diagonal(bbox)
|
||||
if separation >= 0.08:
|
||||
return
|
||||
weaker = min(
|
||||
("left", "right"),
|
||||
key=lambda side: metadata[side]["selected_hand_score"],
|
||||
)
|
||||
metadata[weaker]["hand_state"] = "duplicate"
|
||||
metadata[weaker]["distance_eligible"] = False
|
||||
metadata[weaker]["alarm_eligible"] = False
|
||||
metadata[weaker]["duplicate_hand_separation"] = round(separation, 5)
|
||||
|
||||
|
||||
def fuse_person(
|
||||
raw_keypoints: np.ndarray,
|
||||
raw_scores: np.ndarray,
|
||||
isolated_keypoints: np.ndarray,
|
||||
isolated_scores: np.ndarray,
|
||||
bbox: list[float],
|
||||
track: PersonTrack | None,
|
||||
) -> tuple[np.ndarray, dict]:
|
||||
fused = raw_keypoints.copy()
|
||||
metadata: dict[str, dict] = {}
|
||||
|
||||
for side in ("left", "right"):
|
||||
side_track = track.sides[side] if track else None
|
||||
raw = build_candidate("raw", raw_keypoints, raw_scores, bbox, side)
|
||||
isolated = build_candidate(
|
||||
"isolated", isolated_keypoints, isolated_scores, bbox, side
|
||||
)
|
||||
candidates = [raw, isolated]
|
||||
limb, limb_detail = select_limb(candidates, side, side_track)
|
||||
hand, hand_valid, hand_detail = select_hand(
|
||||
candidates, limb, bbox, side, side_track
|
||||
)
|
||||
|
||||
other_hand = isolated if hand.source == "raw" else raw
|
||||
endpoint_advantage = hand.hand_score - other_hand.hand_score
|
||||
if hand.source != limb.source and hand_valid and endpoint_advantage >= 0.6:
|
||||
limb = hand
|
||||
limb_detail["visible_endpoint_override"] = 1.0
|
||||
hand_detail["link_to_selected_limb"] = hand.wrist_link
|
||||
|
||||
body_indices = np.asarray(BODY_SIDE_INDICES[side])
|
||||
hand_start, hand_end = HAND_SLICES[side]
|
||||
fused[body_indices] = limb.keypoints[body_indices]
|
||||
fused[hand_start:hand_end] = hand.keypoints[hand_start:hand_end]
|
||||
|
||||
limb_disagreement, hand_disagreement = candidate_disagreement(raw, isolated, side)
|
||||
if limb_disagreement <= 0.05:
|
||||
limb_state = "consensus"
|
||||
else:
|
||||
limb_state = "inferred"
|
||||
|
||||
score_advantage = raw.hand_score - isolated.hand_score
|
||||
raw_temporally_stable = hand_detail["temporal_cost"] <= 0.08
|
||||
if not hand_valid:
|
||||
hand_state = "rejected"
|
||||
distance_eligible = False
|
||||
elif hand_disagreement <= 0.06:
|
||||
hand_state = "consensus"
|
||||
distance_eligible = True
|
||||
elif (
|
||||
hand.source == "raw"
|
||||
and score_advantage >= 0.6
|
||||
and raw_temporally_stable
|
||||
):
|
||||
hand_state = "visible_endpoint"
|
||||
distance_eligible = True
|
||||
elif hand.source == "isolated":
|
||||
hand_state = "inferred"
|
||||
distance_eligible = False
|
||||
else:
|
||||
hand_state = "conflict"
|
||||
distance_eligible = False
|
||||
fast_alarm_endpoint = (
|
||||
hand_valid and hand.source == "raw" and score_advantage >= 0.6
|
||||
)
|
||||
alarm_eligible = distance_eligible or fast_alarm_endpoint
|
||||
|
||||
metadata[side] = {
|
||||
"limb_source": limb.source,
|
||||
"hand_source": hand.source,
|
||||
"limb_state": limb_state,
|
||||
"hand_state": hand_state,
|
||||
"distance_eligible": distance_eligible,
|
||||
"alarm_eligible": alarm_eligible,
|
||||
"limb_disagreement": round(limb_disagreement, 5),
|
||||
"hand_disagreement": round(hand_disagreement, 5),
|
||||
"raw_hand_score": round(raw.hand_score, 4),
|
||||
"isolated_hand_score": round(isolated.hand_score, 4),
|
||||
"selected_hand_span": round(hand.hand_span, 5),
|
||||
"selected_hand_score": round(hand.hand_score, 4),
|
||||
"selected_hand_valid": hand_valid,
|
||||
"limb_selection": {key: round(value, 5) for key, value in limb_detail.items()},
|
||||
"hand_selection": {key: round(value, 5) for key, value in hand_detail.items()},
|
||||
}
|
||||
|
||||
if side_track is not None:
|
||||
limb_now = limb.normalized[body_indices]
|
||||
hand_now = hand.normalized[hand_start:hand_end]
|
||||
if side_track.limb_previous is not None:
|
||||
delta = limb_now - side_track.limb_previous
|
||||
side_track.limb_velocity = (
|
||||
delta
|
||||
if side_track.limb_velocity is None
|
||||
else 0.65 * side_track.limb_velocity + 0.35 * delta
|
||||
)
|
||||
if hand_valid:
|
||||
if side_track.hand_previous is not None:
|
||||
delta = hand_now - side_track.hand_previous
|
||||
side_track.hand_velocity = (
|
||||
delta
|
||||
if side_track.hand_velocity is None
|
||||
else 0.65 * side_track.hand_velocity + 0.35 * delta
|
||||
)
|
||||
side_track.hand_previous = hand_now
|
||||
side_track.hand_source = hand.source
|
||||
side_track.limb_previous = limb_now
|
||||
side_track.limb_source = limb.source
|
||||
side_track.previous_hand_eligible = distance_eligible
|
||||
|
||||
suppress_duplicate_hands(fused, metadata, bbox)
|
||||
return fused, metadata
|
||||
|
||||
|
||||
def adaptive_smooth(
|
||||
keypoints: np.ndarray,
|
||||
bbox: list[float],
|
||||
track: PersonTrack,
|
||||
) -> np.ndarray:
|
||||
normalized = normalized_keypoints(keypoints, bbox)
|
||||
if track.smoothed is None:
|
||||
track.smoothed = normalized
|
||||
else:
|
||||
displacement = np.linalg.norm(normalized - track.smoothed, axis=1)
|
||||
alpha = 0.32 + 0.58 * np.clip(displacement / 0.08, 0.0, 1.0)
|
||||
track.smoothed = alpha[:, None] * normalized + (1.0 - alpha[:, None]) * track.smoothed
|
||||
|
||||
x1, y1, x2, y2 = bbox
|
||||
result = track.smoothed.copy()
|
||||
result[:, 0] = result[:, 0] * max(x2 - x1, 1.0) + x1
|
||||
result[:, 1] = result[:, 1] * max(y2 - y1, 1.0) + y1
|
||||
return result
|
||||
|
||||
|
||||
def build_global_hands(
|
||||
keypoints: list[np.ndarray],
|
||||
metadata: list[dict],
|
||||
) -> list[dict]:
|
||||
candidates = []
|
||||
for person_idx, (person_keypoints, person_meta) in enumerate(
|
||||
zip(keypoints, metadata), start=1
|
||||
):
|
||||
for side, (start, end) in HAND_SLICES.items():
|
||||
meta = person_meta[side]
|
||||
if not meta["alarm_eligible"]:
|
||||
continue
|
||||
hand = person_keypoints[start:end]
|
||||
span_px = math.hypot(float(np.ptp(hand[:, 0])), float(np.ptp(hand[:, 1])))
|
||||
candidates.append(
|
||||
{
|
||||
"person_id": person_idx,
|
||||
"side": side,
|
||||
"source": meta["hand_source"],
|
||||
"state": meta["hand_state"],
|
||||
"distance_eligible": meta["distance_eligible"],
|
||||
"alarm_eligible": meta["alarm_eligible"],
|
||||
"score": meta["selected_hand_score"],
|
||||
"center": np.mean(hand, axis=0),
|
||||
"span_px": span_px,
|
||||
"keypoints": hand,
|
||||
"aliases": [],
|
||||
}
|
||||
)
|
||||
|
||||
kept: list[dict] = []
|
||||
for candidate in sorted(candidates, key=lambda item: item["score"], reverse=True):
|
||||
duplicate = None
|
||||
for existing in kept:
|
||||
threshold = float(np.clip(0.45 * max(candidate["span_px"], existing["span_px"]), 18, 70))
|
||||
if float(np.linalg.norm(candidate["center"] - existing["center"])) <= threshold:
|
||||
duplicate = existing
|
||||
break
|
||||
if duplicate is None:
|
||||
kept.append(candidate)
|
||||
else:
|
||||
duplicate["aliases"].append(
|
||||
{"person_id": candidate["person_id"], "side": candidate["side"]}
|
||||
)
|
||||
|
||||
return [
|
||||
{
|
||||
"person_id": item["person_id"],
|
||||
"side": item["side"],
|
||||
"source": item["source"],
|
||||
"state": item["state"],
|
||||
"distance_eligible": item["distance_eligible"],
|
||||
"alarm_eligible": item["alarm_eligible"],
|
||||
"score": item["score"],
|
||||
"center": np.round(item["center"], 3).tolist(),
|
||||
"span_px": round(item["span_px"], 3),
|
||||
"keypoints": np.round(item["keypoints"], 3).tolist(),
|
||||
"aliases": item["aliases"],
|
||||
}
|
||||
for item in kept
|
||||
]
|
||||
|
||||
|
||||
def draw_dashed_line(
|
||||
image: np.ndarray,
|
||||
start: np.ndarray,
|
||||
end: np.ndarray,
|
||||
color: tuple[int, int, int],
|
||||
thickness: int = 2,
|
||||
) -> None:
|
||||
a = np.asarray(start, dtype=np.float64)
|
||||
b = np.asarray(end, dtype=np.float64)
|
||||
length = float(np.linalg.norm(b - a))
|
||||
if length < 1.0:
|
||||
return
|
||||
direction = (b - a) / length
|
||||
for offset in np.arange(0.0, length, 14.0):
|
||||
segment_start = a + direction * offset
|
||||
segment_end = a + direction * min(offset + 8.0, length)
|
||||
cv2.line(
|
||||
image,
|
||||
tuple(np.round(segment_start).astype(int)),
|
||||
tuple(np.round(segment_end).astype(int)),
|
||||
color,
|
||||
thickness,
|
||||
cv2.LINE_AA,
|
||||
)
|
||||
|
||||
|
||||
def draw_fused(
|
||||
image: np.ndarray,
|
||||
boxes: list[list[float]],
|
||||
keypoints: list[np.ndarray],
|
||||
metadata: list[dict],
|
||||
title: str,
|
||||
) -> np.ndarray:
|
||||
canvas = image.copy()
|
||||
for person_idx, (bbox, kpts, person_meta) in enumerate(
|
||||
zip(boxes, keypoints, metadata), start=1
|
||||
):
|
||||
x1, y1, x2, y2 = (int(round(value)) for value in bbox)
|
||||
cv2.rectangle(canvas, (x1, y1), (x2, y2), (0, 220, 255), 2)
|
||||
cv2.putText(
|
||||
canvas,
|
||||
f"P{person_idx}",
|
||||
(x1 + 4, max(22, y1 + 24)),
|
||||
cv2.FONT_HERSHEY_SIMPLEX,
|
||||
0.72,
|
||||
(0, 220, 255),
|
||||
2,
|
||||
cv2.LINE_AA,
|
||||
)
|
||||
for side in ("left", "right"):
|
||||
shoulder, elbow, wrist = BODY_SIDE_INDICES[side]
|
||||
side_meta = person_meta[side]
|
||||
color = SIDE_COLORS[side]
|
||||
for a, b in ((shoulder, elbow), (elbow, wrist)):
|
||||
if side_meta["limb_state"] == "consensus":
|
||||
cv2.line(
|
||||
canvas,
|
||||
tuple(np.round(kpts[a]).astype(int)),
|
||||
tuple(np.round(kpts[b]).astype(int)),
|
||||
color,
|
||||
3,
|
||||
cv2.LINE_AA,
|
||||
)
|
||||
else:
|
||||
draw_dashed_line(canvas, kpts[a], kpts[b], (0, 140, 255), 3)
|
||||
|
||||
start, end = HAND_SLICES[side]
|
||||
hand = kpts[start:end]
|
||||
if side_meta["hand_state"] != "rejected":
|
||||
hand_color = HAND_COLORS[side]
|
||||
if not side_meta["distance_eligible"]:
|
||||
hand_color = (0, 140, 255)
|
||||
for a, b in HAND_EDGES_LOCAL:
|
||||
if side_meta["distance_eligible"]:
|
||||
cv2.line(
|
||||
canvas,
|
||||
tuple(np.round(hand[a]).astype(int)),
|
||||
tuple(np.round(hand[b]).astype(int)),
|
||||
hand_color,
|
||||
2,
|
||||
cv2.LINE_AA,
|
||||
)
|
||||
else:
|
||||
draw_dashed_line(canvas, hand[a], hand[b], hand_color, 2)
|
||||
label = (
|
||||
f"{side[0].upper()}:{side_meta['hand_state'][:4]}"
|
||||
f"/{side_meta['hand_source'][0]}"
|
||||
)
|
||||
label_y = max(45, y1 + (48 if side == "left" else 70))
|
||||
cv2.putText(
|
||||
canvas,
|
||||
label,
|
||||
(x1 + 4, label_y),
|
||||
cv2.FONT_HERSHEY_SIMPLEX,
|
||||
0.5,
|
||||
HAND_COLORS[side] if side_meta["distance_eligible"] else (0, 140, 255),
|
||||
1,
|
||||
cv2.LINE_AA,
|
||||
)
|
||||
|
||||
cv2.rectangle(canvas, (0, 0), (650, 45), (0, 0, 0), -1)
|
||||
cv2.putText(
|
||||
canvas,
|
||||
title,
|
||||
(10, 31),
|
||||
cv2.FONT_HERSHEY_SIMPLEX,
|
||||
0.8,
|
||||
(255, 255, 255),
|
||||
2,
|
||||
cv2.LINE_AA,
|
||||
)
|
||||
return canvas
|
||||
@@ -1,28 +0,0 @@
|
||||
"""Runtime pose geometry helpers."""
|
||||
from __future__ import annotations
|
||||
import numpy as np
|
||||
|
||||
|
||||
HAND_EDGES_LOCAL = (
|
||||
(0, 1), (1, 2), (2, 3), (3, 4),
|
||||
(0, 5), (5, 6), (6, 7), (7, 8),
|
||||
(0, 9), (9, 10), (10, 11), (11, 12),
|
||||
(0, 13), (13, 14), (14, 15), (15, 16),
|
||||
(0, 17), (17, 18), (18, 19), (19, 20),
|
||||
)
|
||||
|
||||
|
||||
HAND_SLICES = {"left": (91, 112), "right": (112, 133)}
|
||||
|
||||
|
||||
BODY_WRISTS = {"left": 9, "right": 10}
|
||||
|
||||
|
||||
def normalized_keypoints(kpts: np.ndarray, bbox: list[float]) -> np.ndarray:
|
||||
x1, y1, x2, y2 = bbox
|
||||
width = max(x2 - x1, 1.0)
|
||||
height = max(y2 - y1, 1.0)
|
||||
result = kpts.copy().astype(np.float64)
|
||||
result[:, 0] = (result[:, 0] - x1) / width
|
||||
result[:, 1] = (result[:, 1] - y1) / height
|
||||
return result
|
||||
@@ -1,513 +0,0 @@
|
||||
"""Runtime pose occlusion helpers."""
|
||||
from __future__ import annotations
|
||||
import math
|
||||
from dataclasses import dataclass
|
||||
import numpy as np
|
||||
from pose_geometry import HAND_SLICES
|
||||
|
||||
|
||||
@dataclass
|
||||
class Measurement:
|
||||
keypoints: np.ndarray
|
||||
precise: bool
|
||||
provisional: bool
|
||||
source: str = "model"
|
||||
quality: float = 0.0
|
||||
|
||||
|
||||
@dataclass
|
||||
class EndpointTrack:
|
||||
wrist: np.ndarray | None = None
|
||||
velocity: np.ndarray | None = None
|
||||
hand_shape: np.ndarray | None = None
|
||||
last_observed_frame: int | None = None
|
||||
missed_frames: int = 0
|
||||
last_elbow: np.ndarray | None = None
|
||||
last_direction: np.ndarray | None = None
|
||||
|
||||
|
||||
def bbox_diagonal(bbox: list[float]) -> float:
|
||||
return max(math.hypot(bbox[2] - bbox[0], bbox[3] - bbox[1]), 1.0)
|
||||
|
||||
|
||||
def solve_two_bone_elbow(
|
||||
shoulder: np.ndarray,
|
||||
wrist: np.ndarray,
|
||||
upper_length: float,
|
||||
forearm_length: float,
|
||||
reference_elbow: np.ndarray,
|
||||
) -> np.ndarray:
|
||||
direction = wrist - shoulder
|
||||
distance = float(np.linalg.norm(direction))
|
||||
if distance < 1e-6:
|
||||
return reference_elbow.copy()
|
||||
unit = direction / distance
|
||||
|
||||
if distance >= upper_length + forearm_length:
|
||||
return shoulder + unit * upper_length
|
||||
if distance <= abs(upper_length - forearm_length):
|
||||
sign = 1.0 if upper_length >= forearm_length else -1.0
|
||||
return shoulder + unit * upper_length * sign
|
||||
|
||||
along = (
|
||||
upper_length * upper_length
|
||||
- forearm_length * forearm_length
|
||||
+ distance * distance
|
||||
) / (2.0 * distance)
|
||||
height = math.sqrt(max(upper_length * upper_length - along * along, 0.0))
|
||||
base = shoulder + unit * along
|
||||
perpendicular = np.asarray([-unit[1], unit[0]])
|
||||
candidates = (base + perpendicular * height, base - perpendicular * height)
|
||||
return min(candidates, key=lambda candidate: float(np.linalg.norm(candidate - reference_elbow)))
|
||||
|
||||
|
||||
def update_endpoint_track(
|
||||
track: EndpointTrack,
|
||||
measurement: Measurement | None,
|
||||
frame_idx: int,
|
||||
short_occlusion_frames: int,
|
||||
medium_occlusion_frames: int,
|
||||
) -> tuple[np.ndarray | None, np.ndarray | None, str]:
|
||||
if measurement is not None:
|
||||
measured_wrist = measurement.keypoints[0]
|
||||
measured_shape = measurement.keypoints - measured_wrist
|
||||
if track.wrist is None or track.missed_frames > short_occlusion_frames:
|
||||
track.wrist = measured_wrist.copy()
|
||||
track.velocity = np.zeros(2, dtype=np.float64)
|
||||
track.hand_shape = measured_shape.copy()
|
||||
else:
|
||||
prediction = track.wrist + track.velocity
|
||||
residual = measured_wrist - prediction
|
||||
alpha = 0.78 if measurement.precise else 0.88
|
||||
beta = 0.18 if measurement.precise else 0.24
|
||||
track.wrist = prediction + alpha * residual
|
||||
track.velocity = 0.82 * track.velocity + beta * residual
|
||||
shape_alpha = 0.55 if measurement.precise else 0.7
|
||||
track.hand_shape = (
|
||||
shape_alpha * measured_shape + (1.0 - shape_alpha) * track.hand_shape
|
||||
)
|
||||
track.last_observed_frame = frame_idx
|
||||
track.missed_frames = 0
|
||||
if measurement.source == "global_reassignment":
|
||||
state = "reassigned"
|
||||
elif measurement.source == "temporal_recovery":
|
||||
state = "recovered"
|
||||
elif measurement.precise:
|
||||
state = "observed"
|
||||
elif measurement.provisional:
|
||||
state = "provisional"
|
||||
else:
|
||||
state = "interpolated"
|
||||
return track.wrist.copy(), track.wrist + track.hand_shape, state
|
||||
|
||||
track.missed_frames += 1
|
||||
if track.wrist is None:
|
||||
return None, None, "reachable_long"
|
||||
if track.missed_frames <= short_occlusion_frames:
|
||||
track.wrist = track.wrist + track.velocity
|
||||
track.velocity *= 0.86
|
||||
return track.wrist.copy(), track.wrist + track.hand_shape, "predicted_short"
|
||||
if track.missed_frames <= medium_occlusion_frames:
|
||||
return None, None, "reachable_medium"
|
||||
return None, None, "reachable_long"
|
||||
|
||||
|
||||
def temporal_recovery_measurement(
|
||||
keypoints: np.ndarray,
|
||||
metadata: dict,
|
||||
side: str,
|
||||
bbox: list[float],
|
||||
track: EndpointTrack,
|
||||
medium_occlusion_frames: int,
|
||||
) -> Measurement | None:
|
||||
"""Recover a weak current-frame hand only when history strongly supports it."""
|
||||
if track.wrist is None or track.missed_frames > medium_occlusion_frames:
|
||||
return None
|
||||
if metadata.get("hand_state") not in {"conflict", "inferred"}:
|
||||
return None
|
||||
if not metadata.get("selected_hand_valid", False):
|
||||
return None
|
||||
if float(metadata.get("selected_hand_score", 0.0)) < 2.0:
|
||||
return None
|
||||
link = float(metadata.get("hand_selection", {}).get("link_to_selected_limb", 1.0))
|
||||
if link > 0.08:
|
||||
return None
|
||||
|
||||
start, end = HAND_SLICES[side]
|
||||
hand = np.asarray(keypoints[start:end], dtype=np.float64)
|
||||
if hand.shape != (end - start, 2) or not np.isfinite(hand).all():
|
||||
return None
|
||||
prediction = track.wrist + track.velocity
|
||||
diagonal = bbox_diagonal(bbox)
|
||||
gate = min(0.20, 0.05 + 0.008 * max(track.missed_frames, 1)) * diagonal
|
||||
if float(np.linalg.norm(hand[0] - prediction)) > gate:
|
||||
return None
|
||||
return Measurement(
|
||||
keypoints=hand,
|
||||
precise=False,
|
||||
provisional=True,
|
||||
source="temporal_recovery",
|
||||
quality=float(metadata.get("selected_hand_score", 0.0)),
|
||||
)
|
||||
|
||||
|
||||
def hand_span_px(hand: np.ndarray) -> float:
|
||||
return math.hypot(float(np.ptp(hand[:, 0])), float(np.ptp(hand[:, 1])))
|
||||
|
||||
|
||||
def bbox_area(bbox: list[float]) -> float:
|
||||
return max((bbox[2] - bbox[0]) * (bbox[3] - bbox[1]), 1.0)
|
||||
|
||||
|
||||
def reset_endpoint_track(track: EndpointTrack) -> None:
|
||||
track.wrist = None
|
||||
track.velocity = None
|
||||
track.hand_shape = None
|
||||
track.last_observed_frame = None
|
||||
track.missed_frames = 0
|
||||
track.last_elbow = None
|
||||
track.last_direction = None
|
||||
|
||||
|
||||
def resolve_global_hand_assignments(
|
||||
frame_measurements: dict[tuple[int, str], Measurement | None],
|
||||
tracks: dict[tuple[int, str], EndpointTrack],
|
||||
boxes: list[list[float]],
|
||||
medium_occlusion_frames: int,
|
||||
) -> tuple[dict[tuple[int, str], Measurement | None], dict[tuple[int, str], dict]]:
|
||||
"""Resolve overlapping-person hands before binding them to body sides.
|
||||
|
||||
Large foreground top-down crops may include a smaller background person.
|
||||
The same physical hand can therefore be emitted for both people, or only by
|
||||
the smaller crop even though it belongs to the foreground person. Direct
|
||||
duplicates are kept on the foreground track when their apparent scales
|
||||
agree. A missing foreground side can then reclaim a nearby candidate using
|
||||
its recent hand trajectory and historical hand scale.
|
||||
"""
|
||||
resolved = dict(frame_measurements)
|
||||
events: dict[tuple[int, str], dict] = {}
|
||||
keys = list(resolved)
|
||||
|
||||
# First collapse direct cross-person duplicates. Keep this deliberately
|
||||
# asymmetric: a substantially larger foreground crop must also carry the
|
||||
# substantially larger historical hand shape.
|
||||
for index, first_key in enumerate(keys):
|
||||
first = resolved[first_key]
|
||||
if first is None:
|
||||
continue
|
||||
for second_key in keys[index + 1 :]:
|
||||
second = resolved[second_key]
|
||||
if second is None or first_key[0] == second_key[0]:
|
||||
continue
|
||||
first_hand = first.keypoints
|
||||
second_hand = second.keypoints
|
||||
first_span = hand_span_px(first_hand)
|
||||
second_span = hand_span_px(second_hand)
|
||||
center_distance = float(
|
||||
np.linalg.norm(np.mean(first_hand, axis=0) - np.mean(second_hand, axis=0))
|
||||
)
|
||||
if center_distance > max(70.0, 0.5 * max(first_span, second_span)):
|
||||
continue
|
||||
|
||||
first_area = bbox_area(boxes[first_key[0]])
|
||||
second_area = bbox_area(boxes[second_key[0]])
|
||||
if first_area >= 2.5 * second_area and (
|
||||
first_span >= 1.45 * second_span
|
||||
or first.quality >= second.quality + 0.6
|
||||
):
|
||||
owner_key, released_key = first_key, second_key
|
||||
elif second_area >= 2.5 * first_area and (
|
||||
second_span >= 1.45 * first_span
|
||||
or second.quality >= first.quality + 0.6
|
||||
):
|
||||
owner_key, released_key = second_key, first_key
|
||||
else:
|
||||
continue
|
||||
resolved[released_key] = None
|
||||
reset_endpoint_track(tracks[released_key])
|
||||
events[owner_key] = {
|
||||
"method": "foreground_duplicate_owner",
|
||||
"released_person_id": released_key[0] + 1,
|
||||
"released_side": released_key[1],
|
||||
}
|
||||
events[released_key] = {
|
||||
"method": "reassigned_away",
|
||||
"assigned_person_id": owner_key[0] + 1,
|
||||
"assigned_side": owner_key[1],
|
||||
}
|
||||
|
||||
# Then recover missing foreground hands from another crop. The candidate
|
||||
# must fall inside the target's recent motion gate and be much smaller than
|
||||
# the target's historical foreground hand shape.
|
||||
available_sources = [key for key, value in resolved.items() if value is not None]
|
||||
target_candidates = []
|
||||
for target_key, target_measurement in resolved.items():
|
||||
if target_measurement is not None:
|
||||
continue
|
||||
target_track = tracks[target_key]
|
||||
if (
|
||||
target_track.wrist is None
|
||||
or target_track.hand_shape is None
|
||||
or target_track.missed_frames > medium_occlusion_frames
|
||||
):
|
||||
continue
|
||||
target_area = bbox_area(boxes[target_key[0]])
|
||||
predicted_center = target_track.wrist + np.mean(target_track.hand_shape, axis=0)
|
||||
target_span = hand_span_px(target_track.hand_shape)
|
||||
gate = 0.20 * bbox_diagonal(boxes[target_key[0]])
|
||||
for source_key in available_sources:
|
||||
if source_key[0] == target_key[0]:
|
||||
continue
|
||||
source = resolved[source_key]
|
||||
if source is None or source.source != "model":
|
||||
continue
|
||||
source_area = bbox_area(boxes[source_key[0]])
|
||||
if target_area < 2.5 * source_area:
|
||||
continue
|
||||
source_center = np.mean(source.keypoints, axis=0)
|
||||
source_span = hand_span_px(source.keypoints)
|
||||
distance = float(np.linalg.norm(source_center - predicted_center))
|
||||
if distance > gate or target_span < 1.45 * max(source_span, 1.0):
|
||||
continue
|
||||
target_candidates.append(
|
||||
(distance / gate, target_key, source_key, source_center)
|
||||
)
|
||||
|
||||
used_targets = set()
|
||||
used_sources = set()
|
||||
for _, target_key, source_key, source_center in sorted(target_candidates):
|
||||
if target_key in used_targets or source_key in used_sources:
|
||||
continue
|
||||
target_track = tracks[target_key]
|
||||
shape_center = np.mean(target_track.hand_shape, axis=0)
|
||||
reassigned_wrist = source_center - shape_center
|
||||
reassigned_hand = reassigned_wrist + target_track.hand_shape
|
||||
resolved[target_key] = Measurement(
|
||||
keypoints=reassigned_hand,
|
||||
precise=False,
|
||||
provisional=True,
|
||||
source="global_reassignment",
|
||||
quality=resolved[source_key].quality,
|
||||
)
|
||||
resolved[source_key] = None
|
||||
reset_endpoint_track(tracks[source_key])
|
||||
events[target_key] = {
|
||||
"method": "foreground_temporal_reassignment",
|
||||
"source_person_id": source_key[0] + 1,
|
||||
"source_side": source_key[1],
|
||||
"observed_center": np.round(source_center, 3).tolist(),
|
||||
}
|
||||
events[source_key] = {
|
||||
"method": "reassigned_away",
|
||||
"assigned_person_id": target_key[0] + 1,
|
||||
"assigned_side": target_key[1],
|
||||
}
|
||||
used_targets.add(target_key)
|
||||
used_sources.add(source_key)
|
||||
return resolved, events
|
||||
|
||||
|
||||
def track_direction(track: EndpointTrack) -> np.ndarray:
|
||||
if track.last_direction is not None:
|
||||
return track.last_direction / max(float(np.linalg.norm(track.last_direction)), 1e-6)
|
||||
if track.velocity is not None and float(np.linalg.norm(track.velocity)) > 1e-6:
|
||||
return track.velocity / np.linalg.norm(track.velocity)
|
||||
return np.asarray([1.0, 0.0], dtype=np.float64)
|
||||
|
||||
|
||||
def ellipse_polygon(
|
||||
center: np.ndarray,
|
||||
major_axis: float,
|
||||
minor_axis: float,
|
||||
direction: np.ndarray,
|
||||
point_count: int = 64,
|
||||
) -> np.ndarray:
|
||||
perpendicular = np.asarray([-direction[1], direction[0]], dtype=np.float64)
|
||||
angles = np.linspace(0.0, 2.0 * math.pi, point_count, endpoint=False)
|
||||
return (
|
||||
center
|
||||
+ np.cos(angles)[:, None] * major_axis * direction
|
||||
+ np.sin(angles)[:, None] * minor_axis * perpendicular
|
||||
)
|
||||
|
||||
|
||||
def clip_polygon_from_anchor_to_circle(
|
||||
points: np.ndarray,
|
||||
anchor: np.ndarray,
|
||||
circle_center: np.ndarray,
|
||||
radius: float,
|
||||
) -> np.ndarray:
|
||||
"""Clip radial polygon samples to a shoulder-centered anatomical limit."""
|
||||
clipped = []
|
||||
offset = anchor - circle_center
|
||||
for point in points:
|
||||
if float(np.linalg.norm(point - circle_center)) <= radius:
|
||||
clipped.append(point)
|
||||
continue
|
||||
ray = point - anchor
|
||||
a = float(np.dot(ray, ray))
|
||||
b = 2.0 * float(np.dot(offset, ray))
|
||||
c = float(np.dot(offset, offset) - radius * radius)
|
||||
discriminant = max(b * b - 4.0 * a * c, 0.0)
|
||||
roots = [
|
||||
value
|
||||
for value in (
|
||||
(-b - math.sqrt(discriminant)) / max(2.0 * a, 1e-6),
|
||||
(-b + math.sqrt(discriminant)) / max(2.0 * a, 1e-6),
|
||||
)
|
||||
if 0.0 <= value <= 1.0
|
||||
]
|
||||
clipped.append(anchor + (max(roots) if roots else 0.0) * ray)
|
||||
return np.asarray(clipped, dtype=np.float64)
|
||||
|
||||
|
||||
def medium_motion_region(
|
||||
shoulder: np.ndarray,
|
||||
upper_length: float,
|
||||
forearm_length: float,
|
||||
bbox: list[float],
|
||||
track: EndpointTrack,
|
||||
short_occlusion_frames: int,
|
||||
) -> dict:
|
||||
diagonal = bbox_diagonal(bbox)
|
||||
direction = track_direction(track)
|
||||
hand_shape = (
|
||||
track.hand_shape
|
||||
if track.hand_shape is not None
|
||||
else np.zeros((21, 2), dtype=np.float64)
|
||||
)
|
||||
shape_center = np.mean(hand_shape, axis=0)
|
||||
centered_shape = hand_shape - shape_center
|
||||
perpendicular = np.asarray([-direction[1], direction[0]], dtype=np.float64)
|
||||
hand_major = float(np.max(np.abs(centered_shape @ direction)))
|
||||
hand_minor = float(np.max(np.abs(centered_shape @ perpendicular)))
|
||||
|
||||
extra_frames = max(track.missed_frames - short_occlusion_frames, 1)
|
||||
motion_major = min(0.20, 0.04 + 0.007 * extra_frames) * diagonal
|
||||
motion_minor = min(0.13, 0.03 + 0.0045 * extra_frames) * diagonal
|
||||
major_axis = max(hand_major + motion_major, 0.08 * diagonal)
|
||||
minor_axis = max(hand_minor + motion_minor, 0.06 * diagonal)
|
||||
|
||||
center = track.wrist + shape_center
|
||||
tracked_hand_extent = float(np.max(np.linalg.norm(centered_shape, axis=1)))
|
||||
hand_reach = max(tracked_hand_extent, 0.10 * diagonal)
|
||||
anatomical_reach = upper_length + forearm_length + hand_reach + 0.02 * diagonal
|
||||
shoulder_to_center = center - shoulder
|
||||
center_distance = float(np.linalg.norm(shoulder_to_center))
|
||||
if center_distance > 0.96 * anatomical_reach:
|
||||
center = shoulder + shoulder_to_center / center_distance * 0.96 * anatomical_reach
|
||||
|
||||
polygon = ellipse_polygon(center, major_axis, minor_axis, direction)
|
||||
polygon = clip_polygon_from_anchor_to_circle(
|
||||
polygon,
|
||||
center,
|
||||
shoulder,
|
||||
anatomical_reach,
|
||||
)
|
||||
direction_deg = math.degrees(math.atan2(float(direction[1]), float(direction[0])))
|
||||
return {
|
||||
"geometry": "motion_corridor",
|
||||
"center": np.round(center, 3).tolist(),
|
||||
"predicted_wrist": np.round(track.wrist, 3).tolist(),
|
||||
"major_axis_px": round(major_axis, 3),
|
||||
"minor_axis_px": round(minor_axis, 3),
|
||||
"outer_radius_px": round(major_axis, 3),
|
||||
"direction_deg": round(direction_deg, 3),
|
||||
"max_reach_from_shoulder_px": round(anatomical_reach, 3),
|
||||
"polygon": np.round(polygon, 3).tolist(),
|
||||
"requires_door_intersection": True,
|
||||
"render_full_region": True,
|
||||
"distance_lower_bound_rule": "minimum distance from motion-corridor polygon to door_edge",
|
||||
}
|
||||
|
||||
|
||||
def long_reachable_region(
|
||||
shoulder: np.ndarray,
|
||||
upper_length: float,
|
||||
forearm_length: float,
|
||||
bbox: list[float],
|
||||
track: EndpointTrack,
|
||||
medium_occlusion_frames: int,
|
||||
) -> dict:
|
||||
diagonal = bbox_diagonal(bbox)
|
||||
height = max(bbox[3] - bbox[1], 1.0)
|
||||
arm_reach = upper_length + forearm_length
|
||||
hand_reach = min(0.13 * diagonal, 0.17 * height)
|
||||
safety_margin = 0.025 * diagonal
|
||||
outer_radius = arm_reach + hand_reach + safety_margin
|
||||
inner_radius = max(abs(upper_length - forearm_length) - 0.04 * diagonal, 0.0)
|
||||
if track.last_direction is None:
|
||||
direction_deg = 0.0
|
||||
half_angle_deg = 180.0
|
||||
else:
|
||||
direction = track_direction(track)
|
||||
direction_deg = math.degrees(math.atan2(float(direction[1]), float(direction[0])))
|
||||
half_angle_deg = min(
|
||||
180.0,
|
||||
65.0 + 2.0 * max(track.missed_frames - medium_occlusion_frames, 0),
|
||||
)
|
||||
return {
|
||||
"geometry": "shoulder_sector",
|
||||
"center": np.round(shoulder, 3).tolist(),
|
||||
"inner_radius_px": round(inner_radius, 3),
|
||||
"outer_radius_px": round(outer_radius, 3),
|
||||
"arm_reach_px": round(arm_reach, 3),
|
||||
"hand_reach_px": round(hand_reach, 3),
|
||||
"safety_margin_px": round(safety_margin, 3),
|
||||
"direction_deg": round(direction_deg, 3),
|
||||
"half_angle_deg": round(half_angle_deg, 3),
|
||||
"requires_door_intersection": True,
|
||||
"render_full_region": False,
|
||||
"distance_lower_bound_rule": "intersect shoulder sector with door danger band before alarming",
|
||||
}
|
||||
|
||||
|
||||
def reachable_region(
|
||||
shoulder: np.ndarray,
|
||||
upper_length: float,
|
||||
forearm_length: float,
|
||||
bbox: list[float],
|
||||
track: EndpointTrack,
|
||||
state: str = "reachable_long",
|
||||
short_occlusion_frames: int = 12,
|
||||
medium_occlusion_frames: int = 30,
|
||||
) -> dict:
|
||||
if state == "reachable_medium" and track.wrist is not None:
|
||||
return medium_motion_region(
|
||||
shoulder,
|
||||
upper_length,
|
||||
forearm_length,
|
||||
bbox,
|
||||
track,
|
||||
short_occlusion_frames,
|
||||
)
|
||||
return long_reachable_region(
|
||||
shoulder,
|
||||
upper_length,
|
||||
forearm_length,
|
||||
bbox,
|
||||
track,
|
||||
medium_occlusion_frames,
|
||||
)
|
||||
|
||||
|
||||
def calibrated_bone_lengths(
|
||||
upper_ratio: float,
|
||||
forearm_ratio: float,
|
||||
bbox: list[float],
|
||||
) -> tuple[float, float]:
|
||||
diagonal = bbox_diagonal(bbox)
|
||||
height = max(bbox[3] - bbox[1], 1.0)
|
||||
upper = upper_ratio * diagonal
|
||||
forearm = forearm_ratio * diagonal
|
||||
total = upper + forearm
|
||||
# Keep only a loose sanity cap. The previous 0.55 * bbox-height cap was
|
||||
# unsafe for close, clipped CMS views: a valid projected arm can occupy
|
||||
# almost the full detection height. The ratios are already robust medians,
|
||||
# so this cap only limits catastrophic calibration outliers.
|
||||
maximum_total = 0.95 * diagonal
|
||||
if total > maximum_total:
|
||||
scale = maximum_total / total
|
||||
upper *= scale
|
||||
forearm *= scale
|
||||
return upper, forearm
|
||||
@@ -1,304 +0,0 @@
|
||||
"""Runtime pose tracking helpers."""
|
||||
from __future__ import annotations
|
||||
import math
|
||||
from collections import deque
|
||||
from dataclasses import dataclass, field
|
||||
import numpy as np
|
||||
from pose_geometry import HAND_SLICES
|
||||
from pose_fusion import BODY_SIDE_INDICES, PersonTrack
|
||||
from pose_occlusion import EndpointTrack, Measurement, calibrated_bone_lengths, reachable_region, solve_two_bone_elbow, temporal_recovery_measurement, update_endpoint_track
|
||||
|
||||
|
||||
def bbox_iou(a: np.ndarray, b: np.ndarray) -> float:
|
||||
x1 = max(float(a[0]), float(b[0]))
|
||||
y1 = max(float(a[1]), float(b[1]))
|
||||
x2 = min(float(a[2]), float(b[2]))
|
||||
y2 = min(float(a[3]), float(b[3]))
|
||||
intersection = max(x2 - x1, 0.0) * max(y2 - y1, 0.0)
|
||||
union = (
|
||||
max(float(a[2] - a[0]), 0.0) * max(float(a[3] - a[1]), 0.0)
|
||||
+ max(float(b[2] - b[0]), 0.0) * max(float(b[3] - b[1]), 0.0)
|
||||
- intersection
|
||||
)
|
||||
return intersection / max(union, 1.0)
|
||||
|
||||
|
||||
class BoxTracker:
|
||||
def __init__(self, max_age: int = 30):
|
||||
self.max_age = max_age
|
||||
self.next_id = 1
|
||||
self.tracks: dict[int, dict] = {}
|
||||
|
||||
def update(
|
||||
self,
|
||||
boxes: list[list[float]],
|
||||
frame_idx: int,
|
||||
image_shape: tuple[int, int],
|
||||
) -> list[int]:
|
||||
height, width = image_shape
|
||||
image_diagonal = math.hypot(width, height)
|
||||
active_ids = [
|
||||
track_id
|
||||
for track_id, track in self.tracks.items()
|
||||
if frame_idx - track["last_frame"] <= self.max_age
|
||||
]
|
||||
self.tracks = {track_id: self.tracks[track_id] for track_id in active_ids}
|
||||
pairs = []
|
||||
for detection_idx, box in enumerate(boxes):
|
||||
box_array = np.asarray(box, dtype=np.float64)
|
||||
center = (box_array[:2] + box_array[2:]) * 0.5
|
||||
for track_id in active_ids:
|
||||
prior = np.asarray(self.tracks[track_id]["box"], dtype=np.float64)
|
||||
prior_center = (prior[:2] + prior[2:]) * 0.5
|
||||
iou = bbox_iou(box_array, prior)
|
||||
center_distance = float(np.linalg.norm(center - prior_center)) / image_diagonal
|
||||
if iou >= 0.05 or center_distance <= 0.18:
|
||||
score = 2.0 * iou + max(0.0, 1.0 - center_distance / 0.18)
|
||||
pairs.append((score, detection_idx, track_id))
|
||||
|
||||
assigned_detections = set()
|
||||
assigned_tracks = set()
|
||||
result: list[int | None] = [None] * len(boxes)
|
||||
for _, detection_idx, track_id in sorted(pairs, reverse=True):
|
||||
if detection_idx in assigned_detections or track_id in assigned_tracks:
|
||||
continue
|
||||
assigned_detections.add(detection_idx)
|
||||
assigned_tracks.add(track_id)
|
||||
result[detection_idx] = track_id
|
||||
|
||||
for detection_idx, box in enumerate(boxes):
|
||||
track_id = result[detection_idx]
|
||||
if track_id is None:
|
||||
track_id = self.next_id
|
||||
self.next_id += 1
|
||||
result[detection_idx] = track_id
|
||||
self.tracks[track_id] = {"box": box, "last_frame": frame_idx}
|
||||
return [int(track_id) for track_id in result]
|
||||
|
||||
|
||||
@dataclass
|
||||
class LivePersonState:
|
||||
fusion: PersonTrack = field(default_factory=PersonTrack)
|
||||
endpoints: dict[str, EndpointTrack] = field(
|
||||
default_factory=lambda: {"left": EndpointTrack(), "right": EndpointTrack()}
|
||||
)
|
||||
bone_samples: dict[str, deque] = field(
|
||||
default_factory=lambda: {
|
||||
"left": deque(maxlen=120),
|
||||
"right": deque(maxlen=120),
|
||||
}
|
||||
)
|
||||
last_frame: int = -1
|
||||
|
||||
|
||||
def bbox_diagonal(bbox: list[float]) -> float:
|
||||
return max(math.hypot(bbox[2] - bbox[0], bbox[3] - bbox[1]), 1.0)
|
||||
|
||||
|
||||
def update_bone_samples(
|
||||
state: LivePersonState,
|
||||
keypoints: np.ndarray,
|
||||
metadata: dict,
|
||||
bbox: list[float],
|
||||
) -> None:
|
||||
diagonal = bbox_diagonal(bbox)
|
||||
for side, (shoulder, elbow, wrist) in BODY_SIDE_INDICES.items():
|
||||
meta = metadata[side]
|
||||
if meta["limb_state"] != "consensus" or not meta["distance_eligible"]:
|
||||
continue
|
||||
upper = float(np.linalg.norm(keypoints[shoulder] - keypoints[elbow])) / diagonal
|
||||
forearm = float(np.linalg.norm(keypoints[elbow] - keypoints[wrist])) / diagonal
|
||||
if 0.04 <= upper <= 0.65 and 0.04 <= forearm <= 0.65:
|
||||
state.bone_samples[side].append((upper, forearm))
|
||||
|
||||
|
||||
def current_bone_ratios(state: LivePersonState, side: str) -> tuple[float, float]:
|
||||
samples = state.bone_samples[side]
|
||||
if len(samples) < 10:
|
||||
opposite = "right" if side == "left" else "left"
|
||||
if len(state.bone_samples[opposite]) >= 10:
|
||||
samples = state.bone_samples[opposite]
|
||||
if not samples:
|
||||
return 0.24, 0.22
|
||||
values = np.asarray(samples, dtype=np.float64)
|
||||
return float(np.median(values[:, 0])), float(np.median(values[:, 1]))
|
||||
|
||||
|
||||
def update_live_v2(
|
||||
frame_idx: int,
|
||||
bbox: list[float],
|
||||
selected_keypoints: np.ndarray,
|
||||
smoothed_keypoints: np.ndarray,
|
||||
fusion_metadata: dict,
|
||||
state: LivePersonState,
|
||||
assigned_measurements: dict[str, Measurement | None] | None,
|
||||
identity_assignments: dict[str, dict] | None,
|
||||
short_occlusion_frames: int,
|
||||
medium_occlusion_frames: int,
|
||||
) -> tuple[np.ndarray, dict]:
|
||||
frame_gap = 0 if state.last_frame < 0 else max(frame_idx - state.last_frame - 1, 0)
|
||||
for endpoint in state.endpoints.values():
|
||||
endpoint.missed_frames += frame_gap
|
||||
state.last_frame = frame_idx
|
||||
update_bone_samples(state, selected_keypoints, fusion_metadata, bbox)
|
||||
|
||||
output = smoothed_keypoints.copy()
|
||||
output_metadata = {}
|
||||
diagonal = bbox_diagonal(bbox)
|
||||
for side, (shoulder_idx, elbow_idx, wrist_idx) in BODY_SIDE_INDICES.items():
|
||||
endpoint = state.endpoints[side]
|
||||
meta = fusion_metadata[side]
|
||||
start, end = HAND_SLICES[side]
|
||||
measurement = (
|
||||
assigned_measurements.get(side)
|
||||
if assigned_measurements is not None
|
||||
else None
|
||||
)
|
||||
if assigned_measurements is None and meta["alarm_eligible"]:
|
||||
measurement = Measurement(
|
||||
keypoints=selected_keypoints[start:end],
|
||||
precise=bool(meta["distance_eligible"]),
|
||||
provisional=not bool(meta["distance_eligible"]),
|
||||
quality=float(meta.get("selected_hand_score", 0.0)),
|
||||
)
|
||||
if measurement is None:
|
||||
measurement = temporal_recovery_measurement(
|
||||
selected_keypoints,
|
||||
meta,
|
||||
side,
|
||||
bbox,
|
||||
endpoint,
|
||||
medium_occlusion_frames,
|
||||
)
|
||||
returning_after_long = (
|
||||
measurement is not None
|
||||
and endpoint.missed_frames > short_occlusion_frames
|
||||
)
|
||||
wrist, hand, endpoint_state = update_endpoint_track(
|
||||
endpoint,
|
||||
measurement,
|
||||
frame_idx,
|
||||
short_occlusion_frames,
|
||||
medium_occlusion_frames,
|
||||
)
|
||||
shoulder = smoothed_keypoints[shoulder_idx]
|
||||
upper_ratio, forearm_ratio = current_bone_ratios(state, side)
|
||||
upper_length, forearm_length = calibrated_bone_lengths(
|
||||
upper_ratio, forearm_ratio, bbox
|
||||
)
|
||||
region = None
|
||||
if wrist is not None and hand is not None:
|
||||
reference_elbow = (
|
||||
selected_keypoints[elbow_idx]
|
||||
if returning_after_long or endpoint.last_elbow is None
|
||||
else endpoint.last_elbow
|
||||
)
|
||||
use_ik = (
|
||||
meta["limb_state"] != "consensus"
|
||||
or endpoint_state
|
||||
in ("provisional", "recovered", "reassigned", "predicted_short")
|
||||
)
|
||||
elbow = (
|
||||
solve_two_bone_elbow(
|
||||
shoulder,
|
||||
wrist,
|
||||
upper_length,
|
||||
forearm_length,
|
||||
reference_elbow,
|
||||
)
|
||||
if use_ik
|
||||
else smoothed_keypoints[elbow_idx]
|
||||
)
|
||||
endpoint.last_elbow = elbow.copy()
|
||||
direction = wrist - shoulder
|
||||
if float(np.linalg.norm(direction)) > 1e-6:
|
||||
endpoint.last_direction = direction / np.linalg.norm(direction)
|
||||
output[shoulder_idx] = shoulder
|
||||
output[elbow_idx] = elbow
|
||||
output[wrist_idx] = wrist
|
||||
output[start:end] = hand
|
||||
uncertainty = (
|
||||
0.025 * diagonal
|
||||
if endpoint_state == "observed"
|
||||
else (0.04 + 0.015 * endpoint.missed_frames) * diagonal
|
||||
)
|
||||
precise = endpoint_state == "observed"
|
||||
else:
|
||||
region = reachable_region(
|
||||
shoulder,
|
||||
upper_length,
|
||||
forearm_length,
|
||||
bbox,
|
||||
endpoint,
|
||||
endpoint_state,
|
||||
short_occlusion_frames,
|
||||
medium_occlusion_frames,
|
||||
)
|
||||
uncertainty = region["outer_radius_px"]
|
||||
precise = False
|
||||
output_metadata[side] = {
|
||||
"state": endpoint_state,
|
||||
"precise_distance": precise,
|
||||
"fast_alarm_candidate": True,
|
||||
"risk_mode": (
|
||||
"precise_hand"
|
||||
if endpoint_state == "observed"
|
||||
else (
|
||||
"motion_corridor_intersection"
|
||||
if endpoint_state == "reachable_medium"
|
||||
else (
|
||||
"door_intersection_only"
|
||||
if endpoint_state == "reachable_long"
|
||||
else (
|
||||
"reassigned_hand"
|
||||
if endpoint_state == "reassigned"
|
||||
else "temporal_hand"
|
||||
)
|
||||
)
|
||||
)
|
||||
),
|
||||
"requires_door_intersection": endpoint_state
|
||||
in ("reachable_medium", "reachable_long"),
|
||||
"identity_assignment": (
|
||||
identity_assignments.get(side)
|
||||
if identity_assignments is not None
|
||||
else None
|
||||
),
|
||||
"missed_frames": endpoint.missed_frames,
|
||||
"uncertainty_radius_px": round(float(uncertainty), 3),
|
||||
"bone_lengths_px": {
|
||||
"upper": round(upper_length, 3),
|
||||
"forearm": round(forearm_length, 3),
|
||||
},
|
||||
"reachable_region": region,
|
||||
}
|
||||
return output, output_metadata
|
||||
|
||||
|
||||
def mask_other_people(
|
||||
frame: np.ndarray,
|
||||
boxes: list[list[float]],
|
||||
target_idx: int,
|
||||
) -> np.ndarray:
|
||||
masked = frame.copy()
|
||||
height, width = frame.shape[:2]
|
||||
mask = np.zeros((height, width), dtype=bool)
|
||||
target = boxes[target_idx]
|
||||
target_area = max((target[2] - target[0]) * (target[3] - target[1]), 1.0)
|
||||
for idx, bbox in enumerate(boxes):
|
||||
if idx == target_idx:
|
||||
continue
|
||||
other_area = max((bbox[2] - bbox[0]) * (bbox[3] - bbox[1]), 1.0)
|
||||
# The failure in this video is the small background person being
|
||||
# absorbed by the much larger foreground person's 3:4 pose crop.
|
||||
# Mask only markedly smaller people. Masking a larger occluder while
|
||||
# estimating a small person removes genuine target pixels at overlap.
|
||||
if other_area >= 0.5 * target_area:
|
||||
continue
|
||||
x1, y1, x2, y2 = (int(round(value)) for value in bbox)
|
||||
x1, x2 = max(0, x1), min(width - 1, x2)
|
||||
y1, y2 = max(0, y1), min(height - 1, y2)
|
||||
mask[y1:y2 + 1, x1:x2 + 1] = True
|
||||
|
||||
masked[mask] = (124, 116, 104)
|
||||
return masked
|
||||
@@ -1,116 +0,0 @@
|
||||
"""Runtime visualization helpers."""
|
||||
from __future__ import annotations
|
||||
import cv2
|
||||
import numpy as np
|
||||
|
||||
|
||||
def percentile(values: list[float], p: float) -> float | None:
|
||||
return round(float(np.percentile(values, p)), 3) if values else None
|
||||
|
||||
|
||||
def timing_stats(values: list[float]) -> dict:
|
||||
return {
|
||||
"count": len(values),
|
||||
"total": round(float(np.sum(values)), 3) if values else 0.0,
|
||||
"mean": round(float(np.mean(values)), 3) if values else None,
|
||||
"median": percentile(values, 50),
|
||||
"p95": percentile(values, 95),
|
||||
}
|
||||
|
||||
|
||||
def visualization_skeletons(record: dict) -> list[dict]:
|
||||
"""Return the exact fused keypoints and metadata consumed by draw_fused."""
|
||||
|
||||
boxes = record.get("boxes", [])
|
||||
track_ids = [int(value) for value in record.get("track_ids", [])]
|
||||
keypoints = record.get("temporal_selected_keypoints", [])
|
||||
metadata = record.get("temporal_metadata", [])
|
||||
if not (len(boxes) == len(keypoints) == len(metadata)):
|
||||
return []
|
||||
return [
|
||||
{
|
||||
"track_id": track_ids[position] if position < len(track_ids) else position + 1,
|
||||
"person_index": position,
|
||||
"layout": "coco_wholebody_133",
|
||||
"keypoints": person_keypoints,
|
||||
"metadata": metadata[position],
|
||||
}
|
||||
for position, person_keypoints in enumerate(keypoints)
|
||||
]
|
||||
|
||||
|
||||
def render_frame(
|
||||
frame: np.ndarray,
|
||||
record: dict,
|
||||
frame_risk: dict,
|
||||
door_mask: np.ndarray | None,
|
||||
door_assignments: list[dict],
|
||||
) -> np.ndarray:
|
||||
canvas = frame.copy()
|
||||
boxes = record.get("boxes", [])
|
||||
keypoints = record.get("temporal_selected_keypoints", [])
|
||||
metadata = record.get("temporal_metadata", [])
|
||||
if len(boxes) == len(keypoints) == len(metadata) and boxes:
|
||||
from pose_fusion import draw_fused
|
||||
|
||||
canvas = draw_fused(canvas, boxes, keypoints, metadata, "YOLO + RTMW risk")
|
||||
else:
|
||||
for position, box in enumerate(boxes):
|
||||
x1, y1, x2, y2 = [int(round(float(value))) for value in box[:4]]
|
||||
cv2.rectangle(canvas, (x1, y1), (x2, y2), (0, 220, 255), 2)
|
||||
cv2.putText(
|
||||
canvas,
|
||||
f"P{position + 1}",
|
||||
(x1 + 4, max(22, y1 + 24)),
|
||||
cv2.FONT_HERSHEY_SIMPLEX,
|
||||
0.7,
|
||||
(0, 220, 255),
|
||||
2,
|
||||
cv2.LINE_AA,
|
||||
)
|
||||
zone_colors = {
|
||||
"front": (0, 220, 255),
|
||||
"rear": (255, 180, 0),
|
||||
"transition": (0, 140, 255),
|
||||
"unassigned": (150, 150, 150),
|
||||
}
|
||||
for assignment in door_assignments:
|
||||
position = int(assignment["person_index"])
|
||||
if not 0 <= position < len(boxes):
|
||||
continue
|
||||
box = boxes[position]
|
||||
x1, y1 = int(round(float(box[0]))), int(round(float(box[1])))
|
||||
zone = str(assignment["door_zone"])
|
||||
cv2.putText(
|
||||
canvas,
|
||||
f"T{assignment['track_id']} {zone.upper()}",
|
||||
(max(0, x1 + 4), max(24, y1 - 8)),
|
||||
cv2.FONT_HERSHEY_SIMPLEX,
|
||||
0.68,
|
||||
zone_colors.get(zone, (160, 160, 160)),
|
||||
2,
|
||||
cv2.LINE_AA,
|
||||
)
|
||||
if door_mask is not None and np.any(door_mask):
|
||||
overlay = np.zeros_like(canvas)
|
||||
overlay[door_mask > 0] = (255, 180, 0)
|
||||
canvas = cv2.addWeighted(canvas, 1.0, overlay, 0.22, 0)
|
||||
|
||||
score = frame_risk.get("risk_score")
|
||||
score_text = "N/A" if score is None else f"{float(score):.3f}"
|
||||
level = str(frame_risk.get("risk_level", "unavailable"))
|
||||
color = (0, 0, 255) if level == "danger" else (0, 180, 255)
|
||||
if level in ("clear", "unavailable", "inactive", "unknown"):
|
||||
color = (40, 210, 80) if level == "clear" else (160, 160, 160)
|
||||
cv2.rectangle(canvas, (12, 55), (490, 95), (0, 0, 0), -1)
|
||||
cv2.putText(
|
||||
canvas,
|
||||
f"{frame_risk.get('risk_type', 'DOOR_RISK')} score={score_text} level={level}",
|
||||
(24, 83),
|
||||
cv2.FONT_HERSHEY_SIMPLEX,
|
||||
0.68,
|
||||
color,
|
||||
2,
|
||||
cv2.LINE_AA,
|
||||
)
|
||||
return canvas
|
||||
+1
-3
@@ -1,7 +1,5 @@
|
||||
#!/usr/bin/env bash
|
||||
set -euo pipefail
|
||||
root="$(cd "$(dirname "$0")" && pwd)"
|
||||
export OPENBLAS_NUM_THREADS=1 OMP_NUM_THREADS=2
|
||||
export LD_LIBRARY_PATH="$root/lib${LD_LIBRARY_PATH:+:$LD_LIBRARY_PATH}"
|
||||
export PYTHONDONTWRITEBYTECODE=1
|
||||
exec "${PYTHON:-python3}" -u "$root/run_camera.py" "$@"
|
||||
exec "$root/build/camera_risk" --config "$root/config/camera.json" "$@"
|
||||
|
||||
@@ -1,300 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Live baseline V4L2/CAN capture + native TensorRT + CMS v9.10 risk."""
|
||||
from __future__ import annotations
|
||||
import argparse
|
||||
from collections import Counter, deque
|
||||
from contextlib import ExitStack
|
||||
import copy
|
||||
import json
|
||||
import signal
|
||||
import sys
|
||||
import threading
|
||||
import time
|
||||
from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer
|
||||
from pathlib import Path
|
||||
import cv2
|
||||
import numpy as np
|
||||
ROOT=Path(__file__).resolve().parent
|
||||
sys.path.insert(0,str(ROOT/'pipeline'))
|
||||
from native_backend import NativeBackend
|
||||
from pose_engine import NativePoseEngine
|
||||
from latest_result import LatestResult, result_to_dict
|
||||
from door_mask_risk import DoorMaskSequence, DoorRiskEvaluator
|
||||
from door_zone_risk import DoorZoneConfig, DoorZoneAssigner, build_door_risks
|
||||
from visualization import render_frame, visualization_skeletons, timing_stats
|
||||
|
||||
|
||||
def select_active_door(mode, snapshot, side='left', timeout_ms=1000):
|
||||
if mode!='can': return mode, True
|
||||
p=snapshot['positions'];offset=0 if side=='left' else 1
|
||||
if snapshot['age_ms']<0 or snapshot['age_ms']>timeout_ms or min(p[offset],p[offset+2])<0:
|
||||
return 'none',False
|
||||
front,rear=p[offset]==0,p[offset+2]==0
|
||||
return ('both' if front and rear else 'front' if front else 'rear' if rear else 'none'),True
|
||||
|
||||
|
||||
def gate_risks(raw, assignments, active, can_valid):
|
||||
doors,risk=build_door_risks(raw,assignments,active)
|
||||
if not can_valid:
|
||||
for door,item in doors.items():
|
||||
item.update(risk_type=door.upper()+'_UNKNOWN',risk_level='unknown',risk_score=None,decision='CAN_UNAVAILABLE_OR_STALE')
|
||||
risk=dict(risk,risk_type='CAN_UNKNOWN',risk_level='unknown',risk_score=None,decision='CAN_UNAVAILABLE_OR_STALE')
|
||||
return doors,risk
|
||||
|
||||
|
||||
class JsonlWriter:
|
||||
"""Bound the disk usage of continuous camera runs (current + two backups)."""
|
||||
def __init__(self,path,max_bytes):
|
||||
self.path=path;self.max_bytes=max_bytes;self.size=0
|
||||
self.handle=path.open('w',encoding='utf-8',buffering=1)
|
||||
def __enter__(self):return self
|
||||
def __exit__(self,*args):self.close()
|
||||
def close(self):self.handle.close()
|
||||
def write(self,text):
|
||||
size=len(text.encode('utf-8'))
|
||||
if self.max_bytes and self.size and self.size+size>self.max_bytes:
|
||||
self.handle.close()
|
||||
backup=self.path.with_suffix(self.path.suffix+'.1')
|
||||
if backup.exists():backup.replace(self.path.with_suffix(self.path.suffix+'.2'))
|
||||
self.path.replace(backup)
|
||||
self.handle=self.path.open('w',encoding='utf-8',buffering=1);self.size=0
|
||||
self.handle.write(text);self.size+=size
|
||||
|
||||
|
||||
def render_output(frame, record, risk, mask, assignments, status):
|
||||
canvas = render_frame(frame, record, risk, mask, assignments)
|
||||
cv2.putText(canvas,status,(24,frame.shape[0]-30),cv2.FONT_HERSHEY_SIMPLEX,.8,(0,180,255),2)
|
||||
return canvas
|
||||
|
||||
|
||||
class Preview:
|
||||
def __init__(self,host,port,results):
|
||||
self.jpeg=None;self.results=results;self.lock=threading.Lock()
|
||||
self.pending=threading.Condition();self.packet=None;self.stopped=False
|
||||
outer=self
|
||||
class Handler(BaseHTTPRequestHandler):
|
||||
def do_GET(self):
|
||||
with outer.lock:
|
||||
if self.path.split('?')[0]=='/frame.jpg': payload=outer.jpeg or b'';mime='image/jpeg'
|
||||
elif self.path=='/result.json':payload=json.dumps(result_to_dict(outer.results.load()) or {},ensure_ascii=False,allow_nan=False).encode();mime='application/json'
|
||||
else:
|
||||
payload=b'''<!doctype html><meta charset="utf-8"><title>Vision anti-pinch 0923</title><body style="background:#101820;color:white;font:18px sans-serif"><h2>Vision anti-pinch 0923 / Live camera</h2><p id="state"></p><img id="cam" style="max-width:100%;max-height:83vh"><script>let img=document.querySelector('#cam'); function next(){setTimeout(()=>img.src='/frame.jpg?t='+Date.now(),150)}img.onload=next;img.onerror=next;next();setInterval(async()=>{let r=await(await fetch('/result.json')).json();document.querySelector('#state').textContent=JSON.stringify({frame:r.frame_index,active_door:r.active_door,risk:r.risk,can:r.can,calibration_verified:r.calibration_verified})},1000)</script>''';mime='text/html; charset=utf-8'
|
||||
self.send_response(200 if payload else 503);self.send_header('Content-Type',mime);self.send_header('Cache-Control','no-store');self.send_header('Content-Length',str(len(payload)));self.end_headers()
|
||||
try:self.wfile.write(payload)
|
||||
except (BrokenPipeError,ConnectionResetError):pass
|
||||
def log_message(self,*args):pass
|
||||
self.server=ThreadingHTTPServer((host,port),Handler)
|
||||
self.thread=threading.Thread(target=self.server.serve_forever,daemon=True);self.thread.start()
|
||||
self.renderer=threading.Thread(target=self._render_loop,name='risk-preview',daemon=True)
|
||||
self.renderer.start()
|
||||
def submit(self, frame, record, risk, mask, assignments, status):
|
||||
# All inputs belong to a finished frame; inference never modifies them
|
||||
# after submission. Retain only one pending frame, replacing older work.
|
||||
with self.pending:
|
||||
if not self.stopped:
|
||||
self.packet=(frame,record,risk,mask,assignments,status)
|
||||
self.pending.notify()
|
||||
|
||||
def _render_loop(self):
|
||||
try:
|
||||
while True:
|
||||
with self.pending:
|
||||
self.pending.wait_for(lambda:self.packet is not None or self.stopped)
|
||||
if self.stopped:return
|
||||
packet=self.packet;self.packet=None
|
||||
self.update(render_output(*packet))
|
||||
except Exception as exc:
|
||||
with self.pending:self.stopped=True;self.packet=None
|
||||
with self.lock:self.jpeg=None
|
||||
print('Visualization disabled; inference continues: '+str(exc),file=sys.stderr,flush=True)
|
||||
|
||||
def update(self,image):
|
||||
w=960;h=round(image.shape[0]*w/image.shape[1]);image=cv2.resize(image,(w,h))
|
||||
ok,jpeg=cv2.imencode('.jpg',image,[cv2.IMWRITE_JPEG_QUALITY,80])
|
||||
if ok:
|
||||
with self.lock:self.jpeg=jpeg.tobytes()
|
||||
def close(self):
|
||||
with self.pending:self.stopped=True;self.packet=None;self.pending.notify_all()
|
||||
self.renderer.join()
|
||||
self.server.shutdown();self.server.server_close();self.thread.join()
|
||||
|
||||
|
||||
def parse_args(argv=None):
|
||||
p=argparse.ArgumentParser(description=__doc__)
|
||||
p.add_argument('--video',type=Path,help='Diagnostic video replay through the same TensorRT/risk chain');p.add_argument('--camera',default='/dev/video5');p.add_argument('--width',type=int,default=1920);p.add_argument('--height',type=int,default=1536);p.add_argument('--fps',type=int,default=30)
|
||||
p.add_argument('--engines',type=Path,default=ROOT/'orin/models/engines' if (ROOT/'orin').is_dir() else ROOT/'models/engines')
|
||||
p.add_argument('--constant-door-mask',type=Path,default=ROOT/'annotations/constant_door_mask.json')
|
||||
p.add_argument('--door-zone-config',type=Path,default=ROOT/'annotations/door_zone_multifeature.json')
|
||||
p.add_argument('--active-door',choices=['can','front','rear','both','none'],default='can')
|
||||
p.add_argument('--can-side',choices=['left','right'],default='left');p.add_argument('--can-timeout-ms',type=float,default=1000);p.add_argument('--no-can',action='store_true')
|
||||
p.add_argument('--max-people',type=int,default=3);p.add_argument('--det-conf',type=float,default=.30)
|
||||
p.add_argument('--max-frames',type=int,default=0);p.add_argument('--duration',type=float,default=0)
|
||||
p.add_argument('--jsonl-max-mb',type=float,default=32,help='Rotate each JSONL at this size; keep two backups; 0 disables rotation');p.add_argument('--output',type=Path,default=ROOT/'outputs'/time.strftime('%Y%m%d-%H%M%S'))
|
||||
p.add_argument('--visualize',action='store_true',help='Enable optional HTTP visualization; disabled by default')
|
||||
p.add_argument('--save-results',action='store_true',help='Save per-frame JSONL and periodic latest.json')
|
||||
p.add_argument('--save-snapshots',action='store_true',help='Render and save latest.jpg every 10 seconds')
|
||||
p.add_argument('--preview-port',type=int,default=8765);p.add_argument('--preview-host',default='0.0.0.0');p.add_argument('--preview-every',type=int,default=3)
|
||||
p.add_argument('--write-video',action='store_true');p.add_argument('--save-pose-records',action='store_true');p.add_argument('--calibration-verified',action='store_true')
|
||||
a=p.parse_args(argv)
|
||||
if not 1<=a.max_people<=16 or not 0<a.det_conf<1 or min(a.width,a.height,a.fps,a.preview_every)<=0 or a.can_timeout_ms<=0 or a.jsonl_max_mb<0:p.error('Invalid camera/inference configuration')
|
||||
if a.no_can and a.active_door=='can':p.error('--no-can requires explicit --active-door front/rear/both/none')
|
||||
if not 0<=a.preview_port<=65535 or (a.visualize and a.preview_port==0):p.error('--visualize requires a port between 1 and 65535')
|
||||
if a.max_frames<0 or a.duration<0:p.error('Frame count and duration cannot be negative')
|
||||
a.det_roi_y_max_ratio=1.;return a
|
||||
|
||||
|
||||
def run_pipeline(a, results, stop, summary_sink=None):
|
||||
"""Produce results independently of optional visualization and disk outputs."""
|
||||
save_any = a.save_results or a.save_snapshots or a.save_pose_records or a.write_video
|
||||
if save_any:
|
||||
a.output.mkdir(parents=True,exist_ok=True)
|
||||
cv2.setNumThreads(2)
|
||||
native=None;preview=None;writer=None;video=None;count=0;dropped=0;levels=Counter();latencies=deque(maxlen=3000);pose_ms=deque(maxlen=3000);risk_ms=deque(maxlen=3000);ages=deque(maxlen=3000)
|
||||
started=time.monotonic();run_started=None;error=None
|
||||
try:
|
||||
if a.video:
|
||||
video=cv2.VideoCapture(str(a.video))
|
||||
if not video.isOpened():raise RuntimeError('Cannot open video: '+str(a.video))
|
||||
a.width=int(video.get(cv2.CAP_PROP_FRAME_WIDTH));a.height=int(video.get(cv2.CAP_PROP_FRAME_HEIGHT))
|
||||
evaluator=DoorRiskEvaluator(DoorMaskSequence(constant_mask=a.constant_door_mask,require_every_frame=True))
|
||||
assigner=DoorZoneAssigner(DoorZoneConfig.load(a.door_zone_config))
|
||||
# Fail before acquiring camera if calibration mask is invalid.
|
||||
initial=evaluator.sequence.load(0,(a.height,a.width))
|
||||
if initial.status!='loaded' or initial.mask is None or not np.any(initial.mask):raise ValueError('Door mask has no usable pixels')
|
||||
native=NativeBackend(a);pose=NativePoseEngine(a,native)
|
||||
if a.visualize:
|
||||
try:preview=Preview(a.preview_host,a.preview_port,results)
|
||||
except Exception as exc:
|
||||
print('Visualization unavailable; inference continues: '+str(exc),file=sys.stderr,flush=True)
|
||||
if not video:native.open_camera()
|
||||
run_started=time.monotonic();last_report=run_started;last_snapshot=0
|
||||
if a.write_video:
|
||||
writer=cv2.VideoWriter(str(a.output/'rtmw_risk_overlay.mp4'),cv2.VideoWriter_fourcc(*'mp4v'),a.fps,(a.width,a.height))
|
||||
if not writer.isOpened():raise RuntimeError('Video writer could not open')
|
||||
with ExitStack() as output_stack:
|
||||
out=output_stack.enter_context(JsonlWriter(a.output/'causal_results.jsonl',int(a.jsonl_max_mb*1024*1024))) if a.save_results else None
|
||||
pose_out=JsonlWriter(a.output/'pose_records.jsonl',int(a.jsonl_max_mb*1024*1024)) if a.save_pose_records else None
|
||||
try:
|
||||
while not stop.is_set():
|
||||
if (a.max_frames and count>=a.max_frames) or (a.duration and time.monotonic()-run_started>=a.duration):break
|
||||
if video:
|
||||
ok,frame=video.read()
|
||||
if not ok:break
|
||||
capture={'sequence':count,'capture_monotonic_ms':time.monotonic()*1000,'dropped_before_inference':0,'latency_clock':'video_decode_complete'}
|
||||
else:frame,capture=native.read()
|
||||
if frame is None:break
|
||||
tick=time.monotonic();record=pose.infer(frame,count);pose_ms.append((time.monotonic()-tick)*1000)
|
||||
rt=time.monotonic();snapshot=native.can_snapshot();active,valid=select_active_door(a.active_door,snapshot,a.can_side,a.can_timeout_ms)
|
||||
assignments=assigner.update(count,record,(a.height,a.width));raw,mask=evaluator.evaluate(count,record,(a.height,a.width))
|
||||
doors,risk=gate_risks(raw,assignments,active,valid);risk_ms.append((time.monotonic()-rt)*1000)
|
||||
result={'frame_index':count,'track_ids':record['track_ids'],'boxes':record['boxes'],'skeletons':visualization_skeletons(record),'active_door':active,'door_assignments':assignments,'door_risks':doors,'risk':risk,'ungated_risk':raw,'capture':capture,'can':dict(snapshot,valid=valid,source='hardware' if a.active_door=='can' else 'simulation',side=a.can_side),'calibration_verified':a.calibration_verified}
|
||||
result['capture_to_result_ms']=round(time.monotonic()*1000-capture['capture_monotonic_ms'],3)
|
||||
results.publish(result)
|
||||
if out:out.write(json.dumps(result,ensure_ascii=False,allow_nan=False)+'\n')
|
||||
if pose_out:pose_out.write(json.dumps(record,ensure_ascii=False,allow_nan=False)+'\n')
|
||||
now=time.monotonic()
|
||||
snapshot_due=now-last_snapshot>=10
|
||||
if a.save_results and snapshot_due:
|
||||
(a.output/'latest.json').write_text(json.dumps(result,ensure_ascii=False,indent=2))
|
||||
status=('CAN LIVE' if a.active_door=='can' else 'CAN SIMULATION: '+active)+' | '+('CALIBRATED' if a.calibration_verified else 'CALIBRATION UNVERIFIED')
|
||||
if preview and count%a.preview_every==0:
|
||||
preview.submit(frame,record,risk,mask,assignments,status)
|
||||
if writer or (a.save_snapshots and snapshot_due):
|
||||
canvas=render_output(frame,record,risk,mask,assignments,status)
|
||||
if writer:writer.write(canvas)
|
||||
if a.save_snapshots and snapshot_due:
|
||||
cv2.imwrite(str(a.output/'latest.jpg'),canvas)
|
||||
if snapshot_due:last_snapshot=now
|
||||
count+=1;dropped+=capture['dropped_before_inference'];levels[risk['risk_level']]+=1;ages.append(result['capture_to_result_ms']);latencies.append((time.monotonic()-tick)*1000)
|
||||
if now-last_report>=5:
|
||||
print(json.dumps({'frames':count,'fps':round(count/(now-run_started),2),'people':len(record['boxes']),'risk':risk['risk_type'],'latency_ms':round(float(np.mean(latencies)),1),'capture_age_ms':round(float(np.mean(ages)),1),'can_valid':valid}),flush=True);last_report=now
|
||||
finally:
|
||||
if pose_out:pose_out.close()
|
||||
except Exception as exc:
|
||||
error=str(exc);raise
|
||||
finally:
|
||||
ended=time.monotonic()
|
||||
if video:video.release()
|
||||
if native:native.close()
|
||||
if writer:writer.release()
|
||||
if preview:preview.close()
|
||||
elapsed=ended-(run_started or started)
|
||||
summary={'mode':'baseline_v4l2_can_native_trt_cms_v9_10','camera':a.camera if not a.video else None,'video':str(a.video) if a.video else None,'frames':count,'elapsed_seconds':round(elapsed,3),'throughput_fps':round(count/max(elapsed,.001),3),'dropped_latest_frames':dropped,'risk_levels':dict(levels),'active_door_mode':a.active_door,'can_side':a.can_side,'calibration_verified':a.calibration_verified,'visualize_requested':a.visualize,'save_results':a.save_results,'save_snapshots':a.save_snapshots,'latency_window_frames':3000,'latency_ms':{'total':timing_stats(list(latencies)),'pose':timing_stats(list(pose_ms)),'risk':timing_stats(list(risk_ms)),'capture_to_result':timing_stats(list(ages))},'error':error}
|
||||
if summary_sink is not None:summary_sink.update(summary)
|
||||
if save_any:(a.output/'summary.json').write_text(json.dumps(summary,ensure_ascii=False,indent=2))
|
||||
print(json.dumps(summary,ensure_ascii=False),flush=True)
|
||||
|
||||
class CameraPipeline:
|
||||
"""One lifecycle per instance, with the 0920 start/latest/stop/join pattern.
|
||||
|
||||
The worker owns inference. Downstream threads read the latest immutable
|
||||
snapshot directly, without JSON files, HTTP, callbacks or a result queue.
|
||||
"""
|
||||
def __init__(self, args=None):
|
||||
self.args = copy.deepcopy(args if args is not None else parse_args([]))
|
||||
self._results = LatestResult()
|
||||
self._stop = threading.Event()
|
||||
self._lifecycle_lock = threading.Lock()
|
||||
self._thread = None
|
||||
self._error = None
|
||||
self.summary = {}
|
||||
|
||||
@property
|
||||
def latest_result(self):
|
||||
"""The latest complete result variable, or None before the first frame."""
|
||||
return self._results.load()
|
||||
|
||||
def latest(self):
|
||||
return self.latest_result
|
||||
|
||||
def wait_for_result(self, after_frame_index=-1, timeout=None):
|
||||
return self._results.wait(after_frame_index, timeout)
|
||||
|
||||
def start(self):
|
||||
with self._lifecycle_lock:
|
||||
if self._thread is not None:
|
||||
raise RuntimeError('CameraPipeline can only be started once')
|
||||
self._thread = threading.Thread(target=self._run, name='vision-risk')
|
||||
self._thread.start()
|
||||
return self
|
||||
|
||||
def _run(self):
|
||||
try:
|
||||
run_pipeline(self.args, self._results, self._stop, self.summary)
|
||||
except BaseException as exc:
|
||||
self._error = exc
|
||||
finally:
|
||||
self._results.close()
|
||||
|
||||
def running(self):
|
||||
return self._thread is not None and self._thread.is_alive()
|
||||
|
||||
def request_stop(self):
|
||||
self._stop.set()
|
||||
|
||||
def join(self, timeout=None):
|
||||
if self._thread is not None:
|
||||
self._thread.join(timeout)
|
||||
if not self._thread.is_alive() and self._error is not None:
|
||||
raise self._error
|
||||
|
||||
def __enter__(self):
|
||||
return self.start()
|
||||
|
||||
def __exit__(self, *args):
|
||||
self.request_stop()
|
||||
self.join()
|
||||
|
||||
|
||||
def main():
|
||||
pipeline=CameraPipeline(parse_args())
|
||||
for sig in (signal.SIGINT,signal.SIGTERM):
|
||||
signal.signal(sig,lambda *_:pipeline.request_stop())
|
||||
pipeline.start()
|
||||
# Periodic joins allow Python to deliver stop signals on the main thread.
|
||||
while pipeline.running():
|
||||
pipeline.join(.2)
|
||||
pipeline.join()
|
||||
|
||||
|
||||
if __name__=='__main__':main()
|
||||
Reference in New Issue
Block a user