Port complete CAN-aware risk pipeline to C++
This commit is contained in:
+18
-12
@@ -1,18 +1,24 @@
|
||||
cmake_minimum_required(VERSION 3.24)
|
||||
project(vision_anti_pinch_0923 LANGUAGES CXX CUDA)
|
||||
project(vision_anti_pinch LANGUAGES CXX CUDA)
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
||||
set(CMAKE_CUDA_STANDARD 17)
|
||||
set(CMAKE_POSITION_INDEPENDENT_CODE ON)
|
||||
set(CMAKE_CUDA_ARCHITECTURES 87)
|
||||
find_package(CUDAToolkit REQUIRED)
|
||||
find_package(OpenCV REQUIRED COMPONENTS core imgproc)
|
||||
find_package(OpenCV REQUIRED COMPONENTS core imgproc imgcodecs highgui videoio)
|
||||
find_package(Threads REQUIRED)
|
||||
find_path(TRT_INCLUDE NvInfer.h PATH_SUFFIXES aarch64-linux-gnu REQUIRED)
|
||||
find_library(TRT_LIB nvinfer REQUIRED)
|
||||
add_library(vision_native SHARED native/native_api.cpp native/v4l2_capture.cpp native/can_comm.cpp
|
||||
native/trt_engine.cpp native/pose_engine.cu
|
||||
native/gpu_preprocess.cu native/uyvy_converter.cu native/detector.cpp)
|
||||
target_include_directories(vision_native PRIVATE native/include ${TRT_INCLUDE} ${OpenCV_INCLUDE_DIRS})
|
||||
target_link_libraries(vision_native PRIVATE ${TRT_LIB} ${OpenCV_LIBS} Threads::Threads CUDA::cudart CUDA::nppc CUDA::nppig CUDA::nppidei CUDA::nppicc
|
||||
"${CMAKE_SOURCE_DIR}/orin/lib/libusbcanfd.so" "${CMAKE_SOURCE_DIR}/orin/lib/libusb-1.0.so")
|
||||
set_target_properties(vision_native PROPERTIES BUILD_WITH_INSTALL_RPATH TRUE INSTALL_RPATH "$ORIGIN/../lib;$ORIGIN/../orin/lib;/usr/local/cuda/lib64")
|
||||
find_path(TENSORRT_INCLUDE_DIR NvInfer.h PATH_SUFFIXES aarch64-linux-gnu REQUIRED)
|
||||
find_library(TENSORRT_LIBRARY nvinfer REQUIRED)
|
||||
add_library(camera_risk_core
|
||||
src/trt_engine.cpp src/pose_engine.cu src/gpu_preprocess.cu src/uyvy_converter.cu
|
||||
src/detector.cpp src/v4l2_capture.cpp src/camera.cpp src/can_comm.cpp src/can_state.cpp
|
||||
src/door_risk.cpp src/door_zone.cpp src/camera_pipeline.cpp src/config.cpp src/serialization.cpp)
|
||||
target_include_directories(camera_risk_core PUBLIC include third_party ${TENSORRT_INCLUDE_DIR} ${OpenCV_INCLUDE_DIRS})
|
||||
target_link_libraries(camera_risk_core PUBLIC ${TENSORRT_LIBRARY} ${OpenCV_LIBS} Threads::Threads CUDA::cudart CUDA::nppc CUDA::nppig CUDA::nppidei CUDA::nppicc
|
||||
"${CMAKE_SOURCE_DIR}/orin/lib/libusbcanfd.so" "${CMAKE_SOURCE_DIR}/orin/lib/libusb-1.0.so")
|
||||
target_compile_options(camera_risk_core PRIVATE $<$<COMPILE_LANGUAGE:CXX>:-Wall;-Wextra;-Wpedantic>)
|
||||
add_library(camera_risk_visualizer src/visualizer.cpp)
|
||||
target_link_libraries(camera_risk_visualizer PUBLIC camera_risk_core)
|
||||
add_executable(camera_risk src/main.cpp)
|
||||
target_link_libraries(camera_risk PRIVATE camera_risk_visualizer)
|
||||
set_target_properties(camera_risk PROPERTIES BUILD_WITH_INSTALL_RPATH TRUE INSTALL_RPATH "$ORIGIN/../lib;$ORIGIN/../orin/lib;/usr/local/cuda/lib64")
|
||||
|
||||
@@ -1,91 +1,96 @@
|
||||
# Vision Anti-Pinch — use-can
|
||||
# Vision Anti-Pinch — use-can（纯 C++）
|
||||
|
||||
V4L2 摄像头 + CAN 门状态 + YOLO/RTMW TensorRT 推理 + CMS 风险判断。
|
||||
默认持续推理，将最新完整结果保存在内存；可视化和文件保存均按需开启。
|
||||
本分支将 CMS `run_risk.py` 的处理流程移植到 C++，复用 0920 的 TensorRT 模型、CUDA/NPP 预处理及流水线实现。运行入口、时序融合、前后门归属、CAN 门级风险过滤和结果发布均在 C++ 进程中完成，不调用 Python。
|
||||
|
||||
## Orin 直接运行
|
||||
## 编译与运行
|
||||
|
||||
适用：Jetson AGX Orin，JetPack 5.1.2 / L4T R35.4.1、CUDA 11.4、TensorRT 8.5.x、系统 OpenCV 4.5.4；Python 3.8+，numpy、cv2。
|
||||
`orin/` 包含编译库、TensorRT 引擎、CAN 库、摄像头驱动、Python 运行代码和标定文件，可以单独复制到其他目录。
|
||||
目标环境：Jetson AGX Orin、JetPack 5.1.2 / L4T R35.4.1、CUDA 11.4、TensorRT 8.5.x、OpenCV 4.5.4。
|
||||
源码编译另需 CMake 3.24+ 和相应开发库。
|
||||
|
||||
```bash
|
||||
./scripts/build.sh # 或 make
|
||||
./scripts/initialize_camera.sh # 开机后摄像头尚未初始化时执行
|
||||
./run_camera.sh # 执行 build/camera_risk
|
||||
./run_camera.sh --visualize # 可选 OpenCV 本地窗口
|
||||
```
|
||||
|
||||
预编译 Orin 包：
|
||||
|
||||
```bash
|
||||
cd orin
|
||||
./scripts/initialize_camera.sh # 开机后无 /dev/video5 时初始化，需要 sudo
|
||||
./run.sh # 默认无可视化，结果保存在内存
|
||||
# 或开启浏览器预览：http://设备IP:8765
|
||||
./run.sh --visualize
|
||||
./scripts/initialize_camera.sh
|
||||
./run.sh # 脚本直接 exec C++ ELF 程序
|
||||
# 也可以直接执行：
|
||||
./build/camera_risk --config config/camera.json
|
||||
```
|
||||
|
||||
每次只运行一个采集进程；Ctrl+C / SIGTERM 停止。关闭浏览器不会停止推理。
|
||||
系统 CUDA/TensorRT/OpenCV/Python 运行环境由目标系统提供，不随包重复分发。TensorRT 引擎不能直接用于 Thor 或其他 JetPack/GPU 环境。
|
||||
默认无窗口、不保存文件；Ctrl+C / SIGTERM 停止。`--visualize` 使用 Orin 的图形桌面，关闭窗口仅停止显示、推理继续。没有桌面环境时保持无窗口推理。没有 Python HTTP 预览服务。
|
||||
|
||||
## 从源码编译
|
||||
## 流水线与结果变量
|
||||
|
||||
在目标 Orin 上执行，依赖 CMake 3.24+、CUDA/NPP、TensorRT、OpenCV 开发库：
|
||||
```
|
||||
V4L2 MMAP 采集 / NPP UYVY 转换（线程 1）
|
||||
→ YOLO TensorRT 检测（线程 2）
|
||||
→ RTMW TensorRT / 隔离推理 / C++ 时序融合（线程 3）
|
||||
→ 前后门多特征归属 / 因果中值 / mask 风险 / CAN 过滤
|
||||
→ shared_ptr<const FrameResult> 最新结果变量
|
||||
```
|
||||
|
||||
摄像头使用有界队列并丢弃积压旧帧；可视化独立读取最新结果。
|
||||
下游直接链接 `camera_risk_core`，在自己的线程中读取：
|
||||
|
||||
```cpp
|
||||
#include "camera_pipeline.hpp"
|
||||
#include "config.hpp"
|
||||
|
||||
cms::CameraPipeline pipeline(cms::loadConfig("config/camera.json"));
|
||||
pipeline.start();
|
||||
// 下游自己的循环中读取；首帧前 result 为空。
|
||||
auto result = pipeline.latest();
|
||||
if (result) {
|
||||
const auto& risk = result->risk;
|
||||
const auto& front = result->door_risks.at("front");
|
||||
const auto& rear = result->door_risks.at("rear");
|
||||
const auto& people = result->persons; // 每人 selected/smoothed/temporal 各 133 点
|
||||
const auto& assignments = result->door_assignments;
|
||||
const auto& can = result->can;
|
||||
// risk.risk_type / risk.risk_level / risk.risk_score（std::optional<double>）
|
||||
}
|
||||
// 退出时：
|
||||
pipeline.requestStop();
|
||||
pipeline.join();
|
||||
```
|
||||
|
||||
`latest()` 返回拥有数据的只读快照，读者互不消费结果，旧引用不会被新帧修改。慢读者可以跳过中间帧；异常通过 `join()` 传播。停止后的快照只代表最后结果，持续控制应检查运行状态和时间戳。
|
||||
公共结构定义见 `include/frame_result.hpp`，接口见 `include/camera_pipeline.hpp`。
|
||||
|
||||
## CAN 与风险
|
||||
|
||||
- 0x391 的 data[0..3]：左前、右前、左后、右后。
|
||||
- 默认左侧，`--can-side right` 切到右侧；值为 0 时对应门活跃。
|
||||
- 缺失或过期（默认 1000 ms）报文输出 `CAN_UNKNOWN`，分数为空，不能解释为 clear。
|
||||
- `--no-can --active-door both` 显式模拟门状态；也可选择 front/rear/none。
|
||||
- 优先使用有效手部凸包，无有效手部时用人体框兜底；transition 保留 unknown 语义。
|
||||
- 最终使用 `risk` 或 `door_risks`；`ungated_risk` 仅用于诊断。当前只接收门状态，不发送执行器指令。
|
||||
- mask 和前后门参数来自研发机位，默认 calibration_verified=false；装车后需要重新标定。
|
||||
|
||||
配置位于 `config/camera.json`，模型位于 `orin/models/engines/`。详细参数：
|
||||
|
||||
```bash
|
||||
./scripts/build.sh # 或 make
|
||||
./scripts/initialize_camera.sh # 摄像头尚未初始化时执行
|
||||
./run_camera.sh # 使用新编译的 build/libvision_native.so
|
||||
./run_camera.sh --help
|
||||
./run_camera.sh --duration 30
|
||||
./run_camera.sh --save-results /path/results.jsonl
|
||||
```
|
||||
|
||||
源码入口复用 `orin/models/engines/` 和 `orin/lib/`，不依赖旧版工程。
|
||||
修改源码后刷新直接运行包：
|
||||
显式保存时每帧写一行 JSON，核心字段保留 frame_index、track_ids、boxes、skeletons、door_assignments、door_risks、risk、ungated_risk、can；文件由用户指定路径管理。`--video` 可使用同一 C++ 流程处理视频文件，尺寸需匹配配置。
|
||||
|
||||
```bash
|
||||
./scripts/package_orin.sh # 或 make package
|
||||
cd orin && sha256sum -c SHA256SUMS
|
||||
```
|
||||
## 目录与打包
|
||||
|
||||
打包会替换运行代码，清除已删除的旧 Python 模块，并更新文件校验值。
|
||||
- `src/`：C++ / CUDA 源文件及 main.cpp。
|
||||
- `include/`：头文件及下游数据结构。
|
||||
- `third_party/nlohmann/`：C++ JSON 依赖及许可证。
|
||||
- `config/`、`annotations/`：运行配置与标定。
|
||||
- `scripts/`：编译、摄像头初始化、打包。
|
||||
- `orin/`：可单独复制的 ELF 程序、模型、驱动、CAN 库和配置。
|
||||
|
||||
## 下游读取结果
|
||||
|
||||
在同一个 Python 进程中创建流水线，然后读取单个最新只读快照：
|
||||
|
||||
```python
|
||||
from run_camera import CameraPipeline, parse_args
|
||||
|
||||
with CameraPipeline(parse_args([])) as pipeline:
|
||||
result = pipeline.wait_for_result(timeout=10)
|
||||
# 下游自己的循环/线程中读取；首帧前为 None：
|
||||
result = pipeline.latest_result # 等价于 pipeline.latest()
|
||||
if result is not None:
|
||||
print(result['frame_index'], result['risk'])
|
||||
door_risks = result['door_risks']
|
||||
skeletons = result['skeletons']
|
||||
```
|
||||
|
||||
结果字段：`frame_index`、`track_ids`、`boxes`、`skeletons`（每人 133 点）、`door_assignments`、`active_door`、`door_risks`、`risk`、`ungated_risk`、`can`、`capture`、`capture_to_result_ms`、`calibration_verified`。
|
||||
|
||||
- 读取不消费结果；多个读者可以持有同一个不可变快照，旧快照不会被下一帧改写。
|
||||
- 只保留最新结果，慢读者可以跳过帧；`wait_for_result(last_frame_index, timeout)` 可等待更新。
|
||||
- `request_stop()` 请求停止，`join()` 等待退出并传播异常；`with` 自动回收资源。
|
||||
- 停止后仍保留最后快照；下游应检查运行状态和帧时间，避免使用过期结果。
|
||||
- `result_to_dict(result)` 生成可修改、可序列化的副本。变量属于当前进程，不是跨进程共享内存。
|
||||
|
||||
## CAN、风险与输出选项
|
||||
|
||||
`0x391` 的 data[0..3] 依次为左前、右前、左后、右后。默认 `--can-side left`，右侧使用 `--can-side right`。值为 0 表示对应门活跃；报文缺失或超过默认 1000 ms 时，输出 `CAN_UNKNOWN`、分数 null。当前接口不发送防夹执行器指令。
|
||||
|
||||
优先用融合后的有效手部区域判断风险，无有效手部时使用人体框兜底；最终读取 `risk` / `door_risks`。`ungated_risk` 仅供诊断，`unknown` 不能当作 clear。
|
||||
标定文件来自研发机位，默认 `calibration_verified=false`，换机位须重新标定。
|
||||
|
||||
| 参数 | 作用 |
|
||||
|---|---|
|
||||
| `--visualize` | 开启独立绘制线程及 HTTP 预览；默认不绘制、不编码、不监听端口 |
|
||||
| `--save-results` | 保存逐帧 JSONL 及 latest.json，默认不保存 |
|
||||
| `--save-snapshots` | 每约 10 秒保存叠加图像 |
|
||||
| `--write-video` | 保存叠加视频，可配合 `--duration` 限时运行 |
|
||||
| `--output PATH` | 指定保存目录，本身不启用保存 |
|
||||
| `--no-can --active-door both` | 显式模拟门状态 |
|
||||
|
||||
运行 `./run_camera.sh --help` 或 `orin/run.sh --help` 查看全部选项。JSONL 每 32 MiB 轮换，保留两个历史文件；视频不轮换。开启可视化后可读取 `/result.json` 和 `/frame.jpg`。
|
||||
|
||||
## 目录
|
||||
|
||||
- `native/`：实际参与编译的摄像头/CAN、CUDA/NPP、TensorRT 源码及头文件。
|
||||
- `pipeline/`：运行时姿态融合、跟踪、遮挡处理、风险、可视化和结果变量。
|
||||
- `annotations/`：源码运行时的标定文件。
|
||||
- `scripts/`：构建、打包、摄像头初始化。
|
||||
- `orin/`：可单独复制并直接执行的运行包。
|
||||
修改源码后，在 Orin 重新编译并执行 `./scripts/package_orin.sh`，或执行 `make package`。运行、打包均不使用 Python。打包后可在 orin 目录运行 `sha256sum -c SHA256SUMS`。
|
||||
|
||||
@@ -0,0 +1,6 @@
|
||||
{
|
||||
"camera": {"device":"/dev/video5","width":1920,"height":1536,"fps":30,"buffers":4,"timeout_ms":2000},
|
||||
"inference": {"engine_directory":"../orin/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}
|
||||
}
|
||||
@@ -0,0 +1,42 @@
|
||||
#pragma once
|
||||
#include <condition_variable>
|
||||
#include <deque>
|
||||
#include <mutex>
|
||||
#include <optional>
|
||||
#include <stdexcept>
|
||||
|
||||
namespace cms {
|
||||
// FIFO with bounded ownership. Normal close drains; cancellation may discard.
|
||||
template<class T> class BoundedQueue {
|
||||
public:
|
||||
explicit BoundedQueue(std::size_t capacity):capacity_(capacity) {
|
||||
if(!capacity)throw std::invalid_argument("Queue capacity must be positive");
|
||||
}
|
||||
bool push(T item) {
|
||||
std::unique_lock<std::mutex> lock(mutex_);
|
||||
changed_.wait(lock,[&]{return closed_||items_.size()<capacity_;});
|
||||
if(closed_)return false;
|
||||
items_.push_back(std::move(item));changed_.notify_all();return true;
|
||||
}
|
||||
bool pushLatest(T item) {
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
if(closed_)return false;
|
||||
if(items_.size()>=capacity_)items_.pop_front();
|
||||
items_.push_back(std::move(item));changed_.notify_all();return true;
|
||||
}
|
||||
std::optional<T> pop() {
|
||||
std::unique_lock<std::mutex> lock(mutex_);
|
||||
changed_.wait(lock,[&]{return closed_||!items_.empty();});
|
||||
if(items_.empty())return {};
|
||||
T item=std::move(items_.front());items_.pop_front();changed_.notify_all();return item;
|
||||
}
|
||||
void close(bool discard=false) {
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
closed_=true;if(discard)items_.clear();changed_.notify_all();
|
||||
}
|
||||
private:
|
||||
const std::size_t capacity_;
|
||||
std::mutex mutex_;std::condition_variable changed_;
|
||||
std::deque<T> items_;bool closed_{};
|
||||
};
|
||||
}
|
||||
@@ -0,0 +1,32 @@
|
||||
#pragma once
|
||||
#include "image.hpp"
|
||||
#include <atomic>
|
||||
#include <memory>
|
||||
#include <optional>
|
||||
#include <string>
|
||||
|
||||
namespace cms {
|
||||
struct CameraConfig {
|
||||
std::string device{"/dev/video5"};
|
||||
std::string video;
|
||||
int width{1920},height{1536},fps{30},buffers{4},timeout_ms{2000};
|
||||
};
|
||||
struct CapturedFrame {
|
||||
CaptureMetadata metadata;
|
||||
std::shared_ptr<const BgrImage> image;
|
||||
};
|
||||
// The capture thread exclusively owns read(); requestStop() is thread safe.
|
||||
class Camera {
|
||||
public:
|
||||
explicit Camera(CameraConfig config);
|
||||
~Camera();
|
||||
Camera(Camera const&)=delete;
|
||||
Camera& operator=(Camera const&)=delete;
|
||||
std::optional<CapturedFrame> read();
|
||||
void requestStop() noexcept;
|
||||
private:
|
||||
struct Impl;
|
||||
std::unique_ptr<Impl> impl_;
|
||||
};
|
||||
std::uint64_t monotonicNs();
|
||||
}
|
||||
@@ -0,0 +1,48 @@
|
||||
#pragma once
|
||||
#include "camera.hpp"
|
||||
#include "frame_result.hpp"
|
||||
#include <filesystem>
|
||||
#include <memory>
|
||||
|
||||
namespace cms {
|
||||
struct PipelineConfig {
|
||||
CameraConfig camera;
|
||||
std::filesystem::path engines,door_mask,door_zone_config;
|
||||
bool can_enabled{true};
|
||||
std::string can_side{"left"},active_door{"can"};
|
||||
double can_timeout_ms{1000};
|
||||
std::uint64_t max_frames{};
|
||||
std::filesystem::path results_path,pose_records_path;
|
||||
double detector_confidence{0.30};
|
||||
int max_people{3};
|
||||
double warning_relative{0.30},release_relative{0.45};
|
||||
std::size_t queue_capacity{2};
|
||||
bool door_mask_calibration_verified{false};
|
||||
};
|
||||
struct PipelineStats {
|
||||
std::uint64_t captured{},published{},driver_sequence_gaps{},discarded_error_frames{};
|
||||
double mean_capture_to_result_ms{};
|
||||
};
|
||||
// One lifecycle per instance. start() warms engines before starting capture.
|
||||
// Runs continuously until requestStop() or a worker failure.
|
||||
// latest() reads the immutable result variable without removing or acknowledging it.
|
||||
// No result queue, consumer registration or frame-count termination.
|
||||
// Stop is safe from any thread;
|
||||
// destruction closes queues, interrupts capture and joins every worker.
|
||||
class CameraPipeline {
|
||||
public:
|
||||
explicit CameraPipeline(PipelineConfig config);
|
||||
~CameraPipeline();
|
||||
CameraPipeline(CameraPipeline const&)=delete;
|
||||
CameraPipeline& operator=(CameraPipeline const&)=delete;
|
||||
void start();
|
||||
FrameResultPtr latest() const;
|
||||
bool running() const noexcept;
|
||||
void requestStop() noexcept;
|
||||
void join();
|
||||
PipelineStats stats() const;
|
||||
private:
|
||||
struct Impl;
|
||||
std::unique_ptr<Impl> impl_;
|
||||
};
|
||||
}
|
||||
@@ -0,0 +1,22 @@
|
||||
#pragma once
|
||||
#include <array>
|
||||
#include <string>
|
||||
namespace cms {
|
||||
struct CanSnapshot {
|
||||
std::array<int,4> positions{{-1,-1,-1,-1}}; // LF, RF, LR, RR
|
||||
double age_ms{-1};
|
||||
bool valid{false};
|
||||
std::string source{"hardware"},side{"left"};
|
||||
};
|
||||
class CanInput {
|
||||
public:
|
||||
explicit CanInput(bool enabled);
|
||||
~CanInput();
|
||||
CanInput(CanInput const&)=delete;
|
||||
CanInput& operator=(CanInput const&)=delete;
|
||||
CanSnapshot snapshot(std::string const& side,double timeoutMs) const;
|
||||
private:
|
||||
bool opened_{};
|
||||
};
|
||||
std::string activeDoor(CanSnapshot const& state);
|
||||
}
|
||||
@@ -61,6 +61,15 @@ inline std::vector<std::uint8_t> readBinary(std::filesystem::path const& path) {
|
||||
return bytes;
|
||||
}
|
||||
|
||||
inline void writeBinary(std::filesystem::path const& path, void const* data, std::size_t size) {
|
||||
std::filesystem::create_directories(path.parent_path());
|
||||
std::ofstream stream(path, std::ios::binary);
|
||||
if (!stream) {
|
||||
throw std::runtime_error("Cannot write " + path.string());
|
||||
}
|
||||
stream.write(static_cast<char const*>(data), static_cast<std::streamsize>(size));
|
||||
}
|
||||
|
||||
inline std::size_t volume(nvinfer1::Dims const& dims) {
|
||||
std::size_t value = 1;
|
||||
for (int index = 0; index < dims.nbDims; ++index) {
|
||||
@@ -0,0 +1,5 @@
|
||||
#pragma once
|
||||
#include "camera_pipeline.hpp"
|
||||
namespace cms {
|
||||
PipelineConfig loadConfig(std::filesystem::path const& path);
|
||||
}
|
||||
@@ -14,4 +14,5 @@ private:
|
||||
std::unique_ptr<YoloPreprocess> preprocess_;
|
||||
std::vector<float> outputHost_;
|
||||
};
|
||||
cv::Mat maskOtherPeople(cv::Mat const& frame,std::vector<fangjia::Box> const& boxes,size_t target);
|
||||
}
|
||||
@@ -0,0 +1,35 @@
|
||||
#pragma once
|
||||
#include <nlohmann/json.hpp>
|
||||
#include <opencv2/core.hpp>
|
||||
#include <filesystem>
|
||||
#include <map>
|
||||
#include <set>
|
||||
using Json = nlohmann::ordered_json;
|
||||
namespace cms {
|
||||
struct DoorOptions {
|
||||
std::filesystem::path constant, directory;
|
||||
std::string format{"auto"};
|
||||
std::set<std::string> labels{"door", "车门"};
|
||||
int offset{};
|
||||
bool require{};
|
||||
double warning{0.30}, release{0.45};
|
||||
};
|
||||
class DoorRisk {
|
||||
public:
|
||||
DoorRisk(DoorOptions options, cv::Size size);
|
||||
void initialize() { load(0); }
|
||||
Json evaluate(int frame, Json& record);
|
||||
cv::Mat const& mask() const { return mask_; }
|
||||
private:
|
||||
void load(int frame);
|
||||
cv::Mat rasterize(std::filesystem::path const& path) const;
|
||||
DoorOptions options_;
|
||||
cv::Size size_;
|
||||
std::string format_, status_;
|
||||
std::filesystem::path loadedPath_;
|
||||
std::map<int, std::filesystem::path> paths_;
|
||||
cv::Mat mask_, distance_;
|
||||
bool cached_{};
|
||||
};
|
||||
double rounded(double value, int digits);
|
||||
}
|
||||
@@ -0,0 +1,22 @@
|
||||
#pragma once
|
||||
#include "door_risk.hpp"
|
||||
#include <deque>
|
||||
#include <unordered_map>
|
||||
|
||||
namespace cms {
|
||||
class DoorZoneAssigner {
|
||||
public:
|
||||
explicit DoorZoneAssigner(std::filesystem::path const& configuration);
|
||||
Json update(int frame, Json const& record, cv::Size size);
|
||||
private:
|
||||
std::string classify(double probability) const;
|
||||
Json features_;
|
||||
double frontMin_{},rearMax_{},minimumHeight_{};
|
||||
int minimumPoseFeatures_{},historyFrames_{},staleFrames_{};
|
||||
std::unordered_map<int,std::deque<double>> history_;
|
||||
std::unordered_map<int,int> lastSeen_;
|
||||
};
|
||||
struct DoorDecisions { Json doors; Json selected; };
|
||||
DoorDecisions buildDoorRisks(Json const& raw, Json const& assignments,
|
||||
std::string const& activeDoor, bool canValid);
|
||||
}
|
||||
@@ -0,0 +1,96 @@
|
||||
#pragma once
|
||||
// Public downstream contract. Owns its data; no GPU, JSON or network handles.
|
||||
#include "image.hpp"
|
||||
#include "can_state.hpp"
|
||||
#include <map>
|
||||
#include <array>
|
||||
#include <cstdint>
|
||||
#include <memory>
|
||||
#include <optional>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace cms {
|
||||
struct DoorMaskResult {
|
||||
std::string status;
|
||||
int annotation_index{};
|
||||
std::optional<std::string> path;
|
||||
std::string format;
|
||||
int area_px{};
|
||||
};
|
||||
struct RegionRisk {
|
||||
std::string status;
|
||||
std::optional<double> risk_score;
|
||||
std::string risk_level;
|
||||
std::string decision;
|
||||
std::string source;
|
||||
std::string door_zone;
|
||||
int track_id{};
|
||||
std::string side;
|
||||
std::optional<std::string> state;
|
||||
// Unavailable evidence has no geometric measurements, matching the reference.
|
||||
std::optional<int> region_area_px, door_overlap_px;
|
||||
std::optional<double> region_overlap_ratio, min_distance_px;
|
||||
std::optional<double> region_scale_px, distance_relative;
|
||||
};
|
||||
struct RiskResult {
|
||||
std::string risk_type;
|
||||
std::optional<std::string> door_id;
|
||||
std::string status;
|
||||
std::optional<double> risk_score;
|
||||
std::string risk_level;
|
||||
std::string decision;
|
||||
std::optional<std::string> source;
|
||||
std::optional<int> track_id;
|
||||
std::optional<std::string> side;
|
||||
DoorMaskResult door_mask;
|
||||
double warning_distance_relative{}, release_distance_relative{};
|
||||
std::vector<RegionRisk> regions;
|
||||
};
|
||||
struct PosePoint { double x{},y{},score{}; };
|
||||
struct HandState {
|
||||
std::string state, source;
|
||||
bool limb_consensus{}, distance_eligible{}, alarm_eligible{};
|
||||
};
|
||||
struct PersonResult {
|
||||
int track_id{};
|
||||
double confidence{};
|
||||
std::array<double,4> box{};
|
||||
std::array<PosePoint,133> selected{}, smoothed{}, temporal{};
|
||||
std::array<HandState,2> hands; // left, right
|
||||
};
|
||||
struct HandRegion {
|
||||
int person_id{},track_id{};
|
||||
std::string side;
|
||||
std::array<PosePoint,21> keypoints{};
|
||||
};
|
||||
struct StageTiming {
|
||||
double detector_ms{}, pose_ms{}, fusion_risk_ms{}, capture_to_result_ms{};
|
||||
};
|
||||
struct DoorAssignment {
|
||||
int track_id{}, person_index{}, pose_feature_count{}, history_frames{};
|
||||
std::string door_zone, raw_door_zone, evidence_source;
|
||||
double front_probability{}, smoothed_front_probability{}, confidence{};
|
||||
std::map<std::string,double> features, feature_frontness;
|
||||
};
|
||||
struct FrameResult {
|
||||
int frame_index{};
|
||||
std::vector<int> track_ids;
|
||||
std::vector<std::array<double,4>> boxes;
|
||||
RiskResult risk;
|
||||
RiskResult ungated_risk;
|
||||
std::map<std::string,RiskResult> door_risks;
|
||||
std::vector<DoorAssignment> door_assignments;
|
||||
std::string active_door;
|
||||
CanSnapshot can;
|
||||
CaptureMetadata capture;
|
||||
StageTiming timing;
|
||||
std::vector<PersonResult> persons;
|
||||
std::vector<HandRegion> hand_regions;
|
||||
// Both images belong to this result. The constant mask is shared across frames.
|
||||
std::shared_ptr<const BgrImage> image;
|
||||
std::shared_ptr<const BinaryMask> door_mask_pixels;
|
||||
bool door_mask_calibration_verified{};
|
||||
};
|
||||
using FrameResultPtr = std::shared_ptr<const FrameResult>;
|
||||
}
|
||||
@@ -0,0 +1,25 @@
|
||||
#pragma once
|
||||
#include <cstdint>
|
||||
#include <vector>
|
||||
|
||||
namespace cms {
|
||||
// Immutable when published. Stride is explicit; no borrowed camera/GPU memory.
|
||||
struct BgrImage {
|
||||
int width{}, height{}, stride{};
|
||||
std::vector<std::uint8_t> pixels;
|
||||
};
|
||||
struct BinaryMask {
|
||||
int width{}, height{}, stride{};
|
||||
std::vector<std::uint8_t> pixels;
|
||||
};
|
||||
struct CaptureMetadata {
|
||||
std::uint64_t frame_index{};
|
||||
std::uint32_t driver_sequence{};
|
||||
std::uint64_t driver_timestamp_ns{};
|
||||
std::uint64_t received_monotonic_ns{};
|
||||
std::uint64_t capture_monotonic_ns{};
|
||||
bool driver_timestamp_monotonic{};
|
||||
std::uint32_t skipped_driver_frames{};
|
||||
std::uint32_t discarded_error_frames{};
|
||||
};
|
||||
}
|
||||
@@ -0,0 +1,20 @@
|
||||
#pragma once
|
||||
#include "frame_result.hpp"
|
||||
#include <atomic>
|
||||
#include <stdexcept>
|
||||
|
||||
namespace cms {
|
||||
// One immutable result variable. Readers neither remove nor acknowledge results.
|
||||
class LatestResult {
|
||||
public:
|
||||
void publish(FrameResultPtr result) {
|
||||
if(!result)throw std::invalid_argument("Cannot publish a null result");
|
||||
std::atomic_store_explicit(&latest_,std::move(result),std::memory_order_release);
|
||||
}
|
||||
FrameResultPtr load() const {
|
||||
return std::atomic_load_explicit(&latest_,std::memory_order_acquire);
|
||||
}
|
||||
private:
|
||||
FrameResultPtr latest_;
|
||||
};
|
||||
}
|
||||
@@ -0,0 +1,100 @@
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <cstdint>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
namespace fangjia {
|
||||
|
||||
struct Box {
|
||||
float x1{};
|
||||
float y1{};
|
||||
float x2{};
|
||||
float y2{};
|
||||
float score{};
|
||||
int trackId{};
|
||||
};
|
||||
|
||||
struct Keypoint {
|
||||
float x{};
|
||||
float y{};
|
||||
float score{};
|
||||
};
|
||||
|
||||
struct PersonPose {
|
||||
Box box;
|
||||
std::array<Keypoint, 133> keypoints{};
|
||||
};
|
||||
|
||||
enum class LimbState {
|
||||
Observed,
|
||||
Provisional,
|
||||
Recovered,
|
||||
Reassigned,
|
||||
Interpolated,
|
||||
PredictedShort,
|
||||
ReachableMedium,
|
||||
ReachableLong,
|
||||
};
|
||||
|
||||
inline char const* limbStateName(LimbState state) {
|
||||
switch (state) {
|
||||
case LimbState::Observed: return "observed";
|
||||
case LimbState::Provisional: return "provisional";
|
||||
case LimbState::Recovered: return "recovered";
|
||||
case LimbState::Reassigned: return "reassigned";
|
||||
case LimbState::Interpolated: return "interpolated";
|
||||
case LimbState::PredictedShort: return "predicted_short";
|
||||
case LimbState::ReachableMedium: return "reachable_medium";
|
||||
case LimbState::ReachableLong: return "reachable_long";
|
||||
}
|
||||
return "unknown";
|
||||
}
|
||||
|
||||
struct LimbObservation {
|
||||
int trackId{};
|
||||
bool left{};
|
||||
LimbState state{LimbState::ReachableLong};
|
||||
Keypoint elbow{};
|
||||
Keypoint wrist{};
|
||||
std::array<Keypoint, 21> hand{};
|
||||
Box personBox{};
|
||||
bool handValid{};
|
||||
bool alarmEligible{};
|
||||
float handQuality{};
|
||||
};
|
||||
|
||||
// Exact pre-V2 hand evidence consumed by the Python risk stage. These points
|
||||
// come from raw/isolated fusion before temporal reconstruction.
|
||||
struct GlobalHand {
|
||||
int personId{};
|
||||
int trackId{};
|
||||
bool left{};
|
||||
std::string source;
|
||||
std::string state;
|
||||
bool distanceEligible{};
|
||||
bool alarmEligible{};
|
||||
float score{};
|
||||
float spanPx{};
|
||||
std::array<Keypoint, 21> keypoints{};
|
||||
std::vector<std::pair<int, bool>> aliases;
|
||||
};
|
||||
|
||||
struct FusionMeta {
|
||||
bool limbConsensus{}, handValid{}, distanceEligible{}, alarmEligible{};
|
||||
double handScore{};
|
||||
std::string handSource, handState;
|
||||
};
|
||||
|
||||
struct PoseFrameResult {
|
||||
std::vector<std::array<FusionMeta, 2>> metadata;
|
||||
std::vector<PersonPose> selectedPeople;
|
||||
std::vector<PersonPose> smoothedPeople;
|
||||
std::vector<PersonPose> v2People;
|
||||
std::vector<LimbObservation> limbs;
|
||||
std::vector<GlobalHand> globalHands;
|
||||
};
|
||||
|
||||
} // namespace fangjia
|
||||
@@ -0,0 +1,108 @@
|
||||
#pragma once
|
||||
|
||||
#include "pipeline_types.hpp"
|
||||
#include "trt_engine.hpp"
|
||||
|
||||
#include <opencv2/core.hpp>
|
||||
|
||||
#include <deque>
|
||||
#include <array>
|
||||
#include <optional>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
|
||||
namespace fangjia {
|
||||
|
||||
class BoxTracker {
|
||||
public:
|
||||
std::vector<int> update(std::vector<Box>& boxes, int frameIndex, int width, int height);
|
||||
|
||||
private:
|
||||
struct Track { Box box; int lastFrame{}; };
|
||||
int nextId_{1};
|
||||
std::unordered_map<int, Track> tracks_;
|
||||
};
|
||||
|
||||
class RtmwEngine {
|
||||
public:
|
||||
RtmwEngine(
|
||||
std::filesystem::path const& enginePath,
|
||||
TrtLogger& logger,
|
||||
int maxSourceWidth,
|
||||
int maxSourceHeight,
|
||||
int maxPeople = 3);
|
||||
~RtmwEngine();
|
||||
|
||||
RtmwEngine(RtmwEngine const&) = delete;
|
||||
RtmwEngine& operator=(RtmwEngine const&) = delete;
|
||||
|
||||
std::vector<PersonPose> infer(
|
||||
cv::Mat const& frame,
|
||||
std::vector<Box> const& boxes,
|
||||
cudaStream_t stream,
|
||||
float* preprocessMs = nullptr,
|
||||
float* inferenceMs = nullptr,
|
||||
float* postprocessMs = nullptr);
|
||||
|
||||
void warmup(cudaStream_t stream);
|
||||
|
||||
private:
|
||||
TrtEngine engine_;
|
||||
int maxSourceWidth_{};
|
||||
int maxSourceHeight_{};
|
||||
int maxPeople_{};
|
||||
std::uint8_t* sourceDevice_{};
|
||||
std::uint8_t* cropsDevice_{};
|
||||
float* networkDevice_{};
|
||||
std::vector<double> centersHost_;
|
||||
std::vector<double> scalesHost_;
|
||||
std::vector<float> simccXHost_;
|
||||
std::vector<float> simccYHost_;
|
||||
};
|
||||
|
||||
class PoseFusionTracker {
|
||||
public:
|
||||
PoseFrameResult update(
|
||||
std::vector<PersonPose> const& raw,
|
||||
std::vector<PersonPose> const& isolated,
|
||||
int frameIndex);
|
||||
|
||||
private:
|
||||
struct Vec2 { double x{}; double y{}; };
|
||||
struct SideFusion {
|
||||
std::array<Vec2, 3> limbPrevious{};
|
||||
std::array<Vec2, 3> limbVelocity{};
|
||||
std::array<Vec2, 21> handPrevious{};
|
||||
std::array<Vec2, 21> handVelocity{};
|
||||
bool hasLimbPrevious{};
|
||||
bool hasLimbVelocity{};
|
||||
bool hasHandPrevious{};
|
||||
bool hasHandVelocity{};
|
||||
std::string limbSource;
|
||||
std::string handSource;
|
||||
};
|
||||
struct Endpoint {
|
||||
bool hasWrist{};
|
||||
Vec2 wrist{};
|
||||
Vec2 velocity{};
|
||||
std::array<Vec2, 21> handShape{};
|
||||
bool hasHandShape{};
|
||||
int lastObservedFrame{-1};
|
||||
int missedFrames{};
|
||||
bool hasLastElbow{};
|
||||
Vec2 lastElbow{};
|
||||
bool hasLastDirection{};
|
||||
Vec2 lastDirection{};
|
||||
};
|
||||
struct PersonState {
|
||||
std::array<SideFusion, 2> fusion;
|
||||
std::array<Endpoint, 2> endpoints;
|
||||
std::array<std::deque<std::array<double, 2>>, 2> boneSamples;
|
||||
std::array<Vec2, 133> smoothed{};
|
||||
bool hasSmoothed{};
|
||||
int lastFrame{-1};
|
||||
};
|
||||
std::unordered_map<int, PersonState> states_;
|
||||
};
|
||||
|
||||
} // namespace fangjia
|
||||
@@ -0,0 +1,38 @@
|
||||
#pragma once
|
||||
#include "door_risk.hpp"
|
||||
#include "pipeline_types.hpp"
|
||||
namespace cms {
|
||||
inline Json xy(std::vector<fangjia::PersonPose> const& people, int digits=3) {
|
||||
Json a=Json::array(); for(auto const& p:people) { Json k=Json::array();for(auto const& v:p.keypoints) k.push_back({rounded(v.x,digits),rounded(v.y,digits)});a.push_back(k); } return a;
|
||||
}
|
||||
inline Json scores(std::vector<fangjia::PersonPose> const& people) {
|
||||
Json a=Json::array();for(auto const& p:people) {Json k=Json::array();for(auto const& v:p.keypoints) k.push_back(v.score);a.push_back(k);}return a;
|
||||
}
|
||||
inline Json makeRecord(int frame,std::vector<fangjia::PersonPose> const& raw,std::vector<fangjia::PersonPose> const& isolated,fangjia::PoseFrameResult const& fused, bool diagnostics=true, bool visual=true) {
|
||||
Json r={{"frame_index",frame},{"track_ids",Json::array()},{"boxes",Json::array()},{"det_confidences",Json::array()}};
|
||||
for(auto const&p:raw) {auto const&b=p.box;r["track_ids"].push_back(b.trackId);r["boxes"].push_back({rounded(b.x1,3),rounded(b.y1,3),rounded(b.x2,3),rounded(b.y2,3)});r["det_confidences"].push_back(rounded(b.score,4));}
|
||||
if(diagnostics) {
|
||||
r["raw_keypoints"]=xy(raw,9);r["raw_scores"]=scores(raw);r["isolated_keypoints"]=xy(isolated,9);r["isolated_scores"]=scores(isolated);
|
||||
r["inference_boxes"]=Json::array();for(auto const&p:raw)r["inference_boxes"].push_back({p.box.x1,p.box.y1,p.box.x2,p.box.y2});
|
||||
}
|
||||
if(visual||diagnostics) {
|
||||
r["temporal_selected_keypoints"]=xy(fused.selectedPeople);r["temporal_keypoints"]=xy(fused.smoothedPeople);r["v2_keypoints"]=xy(fused.v2People);
|
||||
r["temporal_metadata"]=Json::array();r["v2_metadata"]=Json::array();
|
||||
for(size_t p=0;p<fused.metadata.size();++p) {
|
||||
Json m=Json::object(),v=Json::object();
|
||||
for(int s=0;s<2;++s) {auto side=s==0?"left":"right";auto const&t=fused.metadata[p][s];
|
||||
m[side]={{"limb_state",t.limbConsensus?"consensus":"inferred"},{"hand_source",t.handSource},{"hand_state",t.handState},{"distance_eligible",t.distanceEligible},{"alarm_eligible",t.alarmEligible},{"selected_hand_valid",t.handValid},{"selected_hand_score",t.handScore}};
|
||||
v[side]={{"state",fangjia::limbStateName(fused.limbs[p*2+s].state)}};
|
||||
} r["temporal_metadata"].push_back(m);r["v2_metadata"].push_back(v);
|
||||
}
|
||||
}
|
||||
r["global_hands"]=Json::array();
|
||||
for(auto const& h:fused.globalHands) {
|
||||
Json points=Json::array(),aliases=Json::array();double cx=0,cy=0;
|
||||
for(auto const& k:h.keypoints) {points.push_back({rounded(k.x,3),rounded(k.y,3)});cx+=k.x;cy+=k.y;}
|
||||
for(auto const&a:h.aliases) aliases.push_back({{"person_id",a.first},{"side",a.second?"left":"right"}});
|
||||
r["global_hands"].push_back({{"person_id",h.personId},{"side",h.left?"left":"right"},{"source",h.source},{"state",h.state},{"distance_eligible",h.distanceEligible},{"alarm_eligible",h.alarmEligible},{"score",rounded(h.score,4)},{"center",{rounded(cx/21,3),rounded(cy/21,3)}},{"span_px",rounded(h.spanPx,3)},{"keypoints",points},{"aliases",aliases}});
|
||||
}
|
||||
return r;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,31 @@
|
||||
#pragma once
|
||||
#include "frame_result.hpp"
|
||||
#include "door_risk.hpp"
|
||||
namespace cms {
|
||||
template<class T> inline std::optional<T> optionalField(Json const& j,char const* key) {
|
||||
auto it=j.find(key);return it==j.end()||it->is_null()?std::nullopt:std::optional<T>(it->get<T>());
|
||||
}
|
||||
inline RiskResult parseRisk(Json const& j) {
|
||||
RiskResult r;r.status=j.at("status");r.risk_score=optionalField<double>(j,"risk_score");r.risk_level=j.at("risk_level");r.decision=j.at("decision");
|
||||
r.risk_type=j.value("risk_type",std::string());r.door_id=optionalField<std::string>(j,"door_id");
|
||||
r.source=optionalField<std::string>(j,"source");r.track_id=optionalField<int>(j,"track_id");r.side=optionalField<std::string>(j,"side");
|
||||
if(j.contains("door_mask")) {auto const& d=j.at("door_mask");r.door_mask={d.at("status"),d.at("annotation_index"),optionalField<std::string>(d,"path"),d.at("format"),d.at("area_px")};}
|
||||
r.warning_distance_relative=j.value("warning_distance_relative",.30);r.release_distance_relative=j.value("release_distance_relative",.45);
|
||||
for(auto const& x:j.at("regions")) {
|
||||
RegionRisk v;v.status=x.at("status");v.risk_score=optionalField<double>(x,"risk_score");v.risk_level=x.at("risk_level");v.decision=x.at("decision");v.source=x.at("source");
|
||||
v.track_id=x.at("track_id");v.side=x.at("side");v.state=optionalField<std::string>(x,"state");v.door_zone=x.value("door_zone",std::string());
|
||||
v.region_area_px=optionalField<int>(x,"region_area_px");v.door_overlap_px=optionalField<int>(x,"door_overlap_px");
|
||||
v.region_overlap_ratio=optionalField<double>(x,"region_overlap_ratio");v.min_distance_px=optionalField<double>(x,"min_distance_px");
|
||||
v.region_scale_px=optionalField<double>(x,"region_scale_px");v.distance_relative=optionalField<double>(x,"distance_relative");r.regions.push_back(std::move(v));
|
||||
}
|
||||
return r;
|
||||
}
|
||||
inline DoorAssignment parseAssignment(Json const& j) {
|
||||
DoorAssignment a;a.track_id=j.at("track_id");a.person_index=j.at("person_index");a.door_zone=j.at("door_zone");a.raw_door_zone=j.at("raw_door_zone");
|
||||
a.front_probability=j.at("front_probability");a.smoothed_front_probability=j.at("smoothed_front_probability");a.confidence=j.at("confidence");a.evidence_source=j.at("evidence_source");
|
||||
a.pose_feature_count=j.at("pose_feature_count");a.history_frames=j.at("history_frames");a.features=j.at("features").get<std::map<std::string,double>>();a.feature_frontness=j.at("feature_frontness").get<std::map<std::string,double>>();return a;
|
||||
}
|
||||
inline FrameResult makeFrameResult(Json const& record,Json const& raw,CaptureMetadata capture) {
|
||||
FrameResult r;r.frame_index=record.at("frame_index");r.track_ids=record.at("track_ids").get<std::vector<int>>();r.boxes=record.at("boxes").get<std::vector<std::array<double,4>>>();r.capture=capture;r.ungated_risk=parseRisk(raw);return r;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,6 @@
|
||||
#pragma once
|
||||
#include "frame_result.hpp"
|
||||
#include "door_risk.hpp"
|
||||
namespace cms {
|
||||
Json serializeResult(FrameResult const& result);
|
||||
}
|
||||
@@ -0,0 +1,36 @@
|
||||
#pragma once
|
||||
#include "camera_pipeline.hpp"
|
||||
#include <atomic>
|
||||
#include <csignal>
|
||||
#include <pthread.h>
|
||||
#include <stdexcept>
|
||||
#include <thread>
|
||||
|
||||
namespace cms {
|
||||
// Construct before starting workers so all threads inherit the signal mask.
|
||||
// Stop requests run in an ordinary thread, never an asynchronous signal handler.
|
||||
class StopSignals {
|
||||
public:
|
||||
explicit StopSignals(CameraPipeline& pipeline):pipeline_(pipeline) {
|
||||
sigemptyset(&signals_);sigaddset(&signals_,SIGINT);sigaddset(&signals_,SIGTERM);
|
||||
if(pthread_sigmask(SIG_BLOCK,&signals_,&old_)!=0)throw std::runtime_error("Cannot block termination signals");
|
||||
try {
|
||||
thread_=std::thread([this]{
|
||||
while(!done_) {
|
||||
timespec timeout{0,100000000};
|
||||
int signal=sigtimedwait(&signals_,nullptr,&timeout);
|
||||
if(signal==SIGINT||signal==SIGTERM){pipeline_.requestStop();break;}
|
||||
}
|
||||
});
|
||||
}catch(...){pthread_sigmask(SIG_SETMASK,&old_,nullptr);throw;}
|
||||
}
|
||||
~StopSignals(){done_=true;if(thread_.joinable())thread_.join();pthread_sigmask(SIG_SETMASK,&old_,nullptr);}
|
||||
StopSignals(StopSignals const&)=delete;
|
||||
StopSignals& operator=(StopSignals const&)=delete;
|
||||
private:
|
||||
CameraPipeline& pipeline_;
|
||||
sigset_t signals_{},old_{};
|
||||
std::atomic<bool> done_{false};
|
||||
std::thread thread_;
|
||||
};
|
||||
}
|
||||
@@ -16,6 +16,8 @@ public:
|
||||
|
||||
void setInputShape(std::string const& name, nvinfer1::Dims const& shape);
|
||||
void prepare();
|
||||
void bindExternal(std::string const& name, void* pointer, std::size_t bytes);
|
||||
void useOwned(std::string const& name);
|
||||
bool enqueue(cudaStream_t stream);
|
||||
|
||||
void* device(std::string const& name);
|
||||
@@ -31,6 +33,7 @@ private:
|
||||
void* active{};
|
||||
std::size_t capacity{};
|
||||
std::size_t required{};
|
||||
bool external{};
|
||||
};
|
||||
|
||||
Buffer& buffer(std::string const& name);
|
||||
@@ -0,0 +1,11 @@
|
||||
#pragma once
|
||||
#include "frame_result.hpp"
|
||||
#include <opencv2/core.hpp>
|
||||
|
||||
namespace cms {
|
||||
class CameraPipeline;
|
||||
// Pure drawing from the immutable result. Never re-runs inference.
|
||||
cv::Mat renderResult(FrameResult const& result);
|
||||
// Optional observer: closing the window returns without stopping the pipeline.
|
||||
void runLocalVisualization(CameraPipeline const& pipeline);
|
||||
}
|
||||
@@ -1,11 +0,0 @@
|
||||
#pragma once
|
||||
#include <cstdint>
|
||||
#include <vector>
|
||||
|
||||
namespace cms {
|
||||
// Immutable when published. Stride is explicit; no borrowed camera/GPU memory.
|
||||
struct BgrImage {
|
||||
int width{}, height{}, stride{};
|
||||
std::vector<std::uint8_t> pixels;
|
||||
};
|
||||
} // namespace cms
|
||||
@@ -1,31 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <cstdint>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
namespace fangjia {
|
||||
|
||||
struct Box {
|
||||
float x1{};
|
||||
float y1{};
|
||||
float x2{};
|
||||
float y2{};
|
||||
float score{};
|
||||
int trackId{};
|
||||
};
|
||||
|
||||
struct Keypoint {
|
||||
float x{};
|
||||
float y{};
|
||||
float score{};
|
||||
};
|
||||
|
||||
struct PersonPose {
|
||||
Box box;
|
||||
std::array<Keypoint, 133> keypoints{};
|
||||
};
|
||||
|
||||
} // namespace fangjia
|
||||
@@ -1,49 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include "pipeline_types.hpp"
|
||||
#include "trt_engine.hpp"
|
||||
|
||||
#include <opencv2/core.hpp>
|
||||
|
||||
#include <string>
|
||||
|
||||
namespace fangjia {
|
||||
|
||||
class RtmwEngine {
|
||||
public:
|
||||
RtmwEngine(
|
||||
std::filesystem::path const& enginePath,
|
||||
TrtLogger& logger,
|
||||
int maxSourceWidth,
|
||||
int maxSourceHeight,
|
||||
int maxPeople = 3);
|
||||
~RtmwEngine();
|
||||
|
||||
RtmwEngine(RtmwEngine const&) = delete;
|
||||
RtmwEngine& operator=(RtmwEngine const&) = delete;
|
||||
|
||||
std::vector<PersonPose> infer(
|
||||
cv::Mat const& frame,
|
||||
std::vector<Box> const& boxes,
|
||||
cudaStream_t stream,
|
||||
float* preprocessMs = nullptr,
|
||||
float* inferenceMs = nullptr,
|
||||
float* postprocessMs = nullptr);
|
||||
|
||||
void warmup(cudaStream_t stream);
|
||||
|
||||
private:
|
||||
TrtEngine engine_;
|
||||
int maxSourceWidth_{};
|
||||
int maxSourceHeight_{};
|
||||
int maxPeople_{};
|
||||
std::uint8_t* sourceDevice_{};
|
||||
std::uint8_t* cropsDevice_{};
|
||||
float* networkDevice_{};
|
||||
std::vector<double> centersHost_;
|
||||
std::vector<double> scalesHost_;
|
||||
std::vector<float> simccXHost_;
|
||||
std::vector<float> simccYHost_;
|
||||
};
|
||||
|
||||
} // namespace fangjia
|
||||
@@ -1,115 +0,0 @@
|
||||
// Baseline V4L2/CAN + 0920 TensorRT/NPP, with a narrow ctypes ABI.
|
||||
#include "v4l2_capture.h"
|
||||
#include "can_comm.h"
|
||||
#include "detector.hpp"
|
||||
#include "uyvy_converter.hpp"
|
||||
#include <linux/videodev2.h>
|
||||
#include <atomic>
|
||||
#include <chrono>
|
||||
#include <condition_variable>
|
||||
#include <cstring>
|
||||
#include <mutex>
|
||||
#include <thread>
|
||||
|
||||
namespace {
|
||||
using Clock=std::chrono::steady_clock;
|
||||
double nowMs(){return std::chrono::duration<double,std::milli>(Clock::now().time_since_epoch()).count();}
|
||||
thread_local std::string error;
|
||||
std::mutex canMutex;
|
||||
int positions[4]={-1,-1,-1,-1};
|
||||
double canTime=0;
|
||||
void receiveCan(int64_t, uint32_t id, const uint8_t* data, uint32_t len){
|
||||
if(id!=0x391||len<4)return;
|
||||
std::lock_guard<std::mutex> lock(canMutex);
|
||||
// Baseline comments specify bytes 1.0..4.7 (not overlapping bits 0,1,2,3).
|
||||
for(int i=0;i<4;++i)positions[i]=data[i];
|
||||
canTime=nowMs();
|
||||
}
|
||||
struct Engine {
|
||||
fangjia::TrtLogger logger;
|
||||
std::unique_ptr<cms::Detector> detector;
|
||||
std::unique_ptr<fangjia::RtmwEngine> pose;
|
||||
cudaStream_t stream{};
|
||||
int width,height,maxPeople;
|
||||
Engine(const char* dir,int w,int h,int n,float confidence):width(w),height(h),maxPeople(n){
|
||||
detector=std::make_unique<cms::Detector>(std::filesystem::path(dir)/"yolo11s.engine",logger,confidence,n);
|
||||
pose=std::make_unique<fangjia::RtmwEngine>(std::filesystem::path(dir)/"rtmw_l_384.engine",logger,w,h,n);
|
||||
CUDA_CHECK(cudaStreamCreateWithFlags(&stream,cudaStreamNonBlocking));
|
||||
try {pose->warmup(stream);detector->infer(cv::Mat(h,w,CV_8UC3,cv::Scalar()),stream);}catch(...){cudaStreamDestroy(stream);throw;}
|
||||
}
|
||||
~Engine(){if(stream)cudaStreamDestroy(stream);}
|
||||
};
|
||||
struct Camera {
|
||||
V4l2Capture capture;
|
||||
std::unique_ptr<cms::UyvyConverter> converter;
|
||||
std::thread worker;
|
||||
std::mutex mutex;
|
||||
std::condition_variable ready;
|
||||
std::atomic<bool> stop{false};
|
||||
std::shared_ptr<const cms::BgrImage> latest;
|
||||
uint64_t generation=0, consumed=0, sequence=0;
|
||||
double timestamp=0,driverTimestamp=0;
|
||||
int width,height;
|
||||
bool canEnabled=false;
|
||||
std::string failure;
|
||||
Camera(const char* device,int w,int h,int fps,bool can):width(w),height(h){
|
||||
if(!capture.open(device)||!capture.setFormat(w,h,V4L2_PIX_FMT_UYVY))throw std::runtime_error("Camera open/format failed");
|
||||
if(capture.width()!=uint32_t(w)||capture.height()!=uint32_t(h))throw std::runtime_error("Camera negotiated unexpected dimensions");
|
||||
capture.setFps(fps);
|
||||
if(!capture.initMmap(4)||!capture.startStreaming())throw std::runtime_error("Camera streaming failed");
|
||||
converter=std::make_unique<cms::UyvyConverter>(w,h);
|
||||
if(can){CanComm::set_msg_callback(receiveCan);canEnabled=CanComm::can_start();}
|
||||
try {worker=std::thread([this]{loop();});}catch(...){if(canEnabled)CanComm::can_stop();throw;}
|
||||
}
|
||||
~Camera(){stop=true;ready.notify_all();if(worker.joinable())worker.join();capture.close();if(canEnabled)CanComm::can_stop();}
|
||||
void loop(){
|
||||
try {
|
||||
int misses=0;
|
||||
while(!stop){
|
||||
V4l2Capture::Frame frame;
|
||||
if(!capture.getFrame(frame,1000)){if(++misses>=5)throw std::runtime_error("Camera timed out for 5 seconds");continue;}
|
||||
double received=nowMs();
|
||||
misses=0;
|
||||
auto stride=frame.bytes_per_line?frame.bytes_per_line:width*2;
|
||||
if(frame.size<size_t(stride)*height){capture.releaseFrame(frame);continue;}
|
||||
std::shared_ptr<const cms::BgrImage> image;
|
||||
try {image=converter->convert(frame.data,stride);}catch(...){capture.releaseFrame(frame);throw;}
|
||||
auto seq=frame.sequence;double driverTs=frame.timestamp_us/1000.0;
|
||||
capture.releaseFrame(frame);
|
||||
{std::lock_guard<std::mutex> lock(mutex);latest=std::move(image);sequence=seq;timestamp=received;driverTimestamp=driverTs;++generation;}
|
||||
ready.notify_one();
|
||||
}
|
||||
}catch(std::exception const&e){std::lock_guard<std::mutex> lock(mutex);failure=e.what();stop=true;ready.notify_all();}
|
||||
}
|
||||
};
|
||||
}
|
||||
extern "C" {
|
||||
const char* vap_error(){return error.c_str();}
|
||||
void* vap_engine(const char* dir,int w,int h,int n,float confidence){try {return new Engine(dir,w,h,n,confidence);}catch(std::exception const&e){error=e.what();return nullptr;}}
|
||||
void vap_engine_close(void* p){delete static_cast<Engine*>(p);}
|
||||
int vap_detect(void* p,unsigned char* pixels,int w,int h,float* out){try {
|
||||
auto& e=*static_cast<Engine*>(p);auto boxes=e.detector->infer(cv::Mat(h,w,CV_8UC3,pixels),e.stream);
|
||||
for(size_t i=0;i<boxes.size();++i){auto& b=boxes[i];float v[]={b.x1,b.y1,b.x2,b.y2,b.score};std::memcpy(out+i*5,v,sizeof(v));}
|
||||
return boxes.size();
|
||||
}catch(std::exception const&e){error=e.what();return -1;}}
|
||||
int vap_pose(void* p,unsigned char* pixels,int w,int h,const float* boxes,int count,float* out){try {
|
||||
auto& e=*static_cast<Engine*>(p);
|
||||
if(count<0||count>e.maxPeople||w>e.width||h>e.height)throw std::runtime_error("Pose input exceeds configured capacity");
|
||||
std::vector<fangjia::Box> input;for(int i=0;i<count;++i)input.push_back({boxes[i*4],boxes[i*4+1],boxes[i*4+2],boxes[i*4+3],1,0});
|
||||
auto poses=e.pose->infer(cv::Mat(h,w,CV_8UC3,pixels),input,e.stream);
|
||||
for(size_t i=0;i<poses.size();++i)for(int k=0;k<133;++k){auto& pt=poses[i].keypoints[k];auto* v=out+(i*133+k)*3;v[0]=pt.x;v[1]=pt.y;v[2]=pt.score;}
|
||||
return poses.size();
|
||||
}catch(std::exception const&e){error=e.what();return -1;}}
|
||||
void* vap_camera(const char* device,int w,int h,int fps,int can){try{return new Camera(device,w,h,fps,can);}catch(std::exception const&e){error=e.what();return nullptr;}}
|
||||
void vap_camera_close(void* p){delete static_cast<Camera*>(p);}
|
||||
int vap_read(void* p,unsigned char* out,double* meta){try {
|
||||
auto& c=*static_cast<Camera*>(p);std::shared_ptr<const cms::BgrImage> image;
|
||||
{std::unique_lock<std::mutex> lock(c.mutex);
|
||||
if(!c.ready.wait_for(lock,std::chrono::seconds(6),[&]{return c.generation>c.consumed||c.stop;}))throw std::runtime_error("No camera frame");
|
||||
if(!c.failure.empty())throw std::runtime_error(c.failure);
|
||||
if(c.stop)return 0;
|
||||
image=c.latest;meta[0]=c.sequence;meta[1]=c.timestamp;meta[2]=c.generation-c.consumed-1;meta[3]=c.driverTimestamp;c.consumed=c.generation;}
|
||||
std::memcpy(out,image->pixels.data(),image->pixels.size());return 1;
|
||||
}catch(std::exception const&e){error=e.what();return -1;}}
|
||||
void vap_can(double* out){std::lock_guard<std::mutex> lock(canMutex);for(int i=0;i<4;++i)out[i]=positions[i];out[4]=canTime?nowMs()-canTime:-1;}
|
||||
}
|
||||
@@ -1,175 +0,0 @@
|
||||
#include "pose_engine.hpp"
|
||||
#include "common.hpp"
|
||||
#include "gpu_preprocess.hpp"
|
||||
|
||||
#include <opencv2/imgproc.hpp>
|
||||
|
||||
#include <algorithm>
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
|
||||
namespace fangjia {
|
||||
namespace {
|
||||
|
||||
constexpr int kInputWidth = 288;
|
||||
constexpr int kInputHeight = 384;
|
||||
constexpr int kKeypoints = 133;
|
||||
constexpr int kSimccX = 576;
|
||||
constexpr int kSimccY = 768;
|
||||
|
||||
__global__ void poseNormalizeKernel(
|
||||
float* output, std::uint8_t const* input, int people, int inputStep) {
|
||||
int index = blockIdx.x * blockDim.x + threadIdx.x;
|
||||
int pixels = kInputWidth * kInputHeight;
|
||||
int total = people * pixels;
|
||||
if (index >= total) return;
|
||||
int person = index / pixels;
|
||||
int pixel = index - person * pixels;
|
||||
int y = pixel / kInputWidth;
|
||||
int x = pixel - y * kInputWidth;
|
||||
auto const* value = input + person * inputStep * kInputHeight + y * inputStep + x * 3;
|
||||
float* destination = output + static_cast<std::size_t>(person) * 3 * pixels;
|
||||
destination[pixel] = static_cast<float>((static_cast<double>(value[0]) - 123.675) / 58.395);
|
||||
destination[pixels + pixel] = static_cast<float>((static_cast<double>(value[1]) - 116.28) / 57.12);
|
||||
destination[2 * pixels + pixel] = static_cast<float>((static_cast<double>(value[2]) - 103.53) / 57.375);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
RtmwEngine::RtmwEngine(
|
||||
std::filesystem::path const& enginePath, TrtLogger& logger,
|
||||
int maxSourceWidth, int maxSourceHeight, int maxPeople)
|
||||
: engine_(enginePath, logger), maxSourceWidth_(maxSourceWidth),
|
||||
maxSourceHeight_(maxSourceHeight), maxPeople_(maxPeople),
|
||||
centersHost_(maxPeople * 2), scalesHost_(maxPeople * 2),
|
||||
simccXHost_(static_cast<std::size_t>(maxPeople) * kKeypoints * kSimccX),
|
||||
simccYHost_(static_cast<std::size_t>(maxPeople) * kKeypoints * kSimccY) {
|
||||
engine_.setInputShape("input", nvinfer1::Dims4{1,3,kInputHeight,kInputWidth});
|
||||
engine_.prepare();
|
||||
auto xs=engine_.shape("simcc_x"),ys=engine_.shape("simcc_y");
|
||||
if(xs.nbDims!=3||ys.nbDims!=3||xs.d[0]!=1||ys.d[0]!=1||xs.d[1]!=kKeypoints||ys.d[1]!=kKeypoints||xs.d[2]!=kSimccX||ys.d[2]!=kSimccY)
|
||||
throw std::runtime_error("RTMW engine I/O shape does not match the 133-point model");
|
||||
for(auto name:{"input","simcc_x","simcc_y"})if(engine_.dataType(name)!=nvinfer1::DataType::kFLOAT)
|
||||
throw std::runtime_error("RTMW requires float32 I/O bindings");
|
||||
std::size_t const values = static_cast<std::size_t>(maxPeople) * 3 * kInputWidth * kInputHeight;
|
||||
CUDA_CHECK(cudaMalloc(
|
||||
&cropsDevice_, static_cast<std::size_t>(maxPeople_) * kInputWidth * kInputHeight * 3));
|
||||
CUDA_CHECK(cudaMalloc(&networkDevice_, values * sizeof(float)));
|
||||
CUDA_CHECK(cudaMalloc(&sourceDevice_, static_cast<size_t>(maxSourceWidth_)*maxSourceHeight_*3));
|
||||
}
|
||||
|
||||
RtmwEngine::~RtmwEngine() {
|
||||
if (sourceDevice_) cudaFree(sourceDevice_);
|
||||
if (cropsDevice_) cudaFree(cropsDevice_);
|
||||
if (networkDevice_) cudaFree(networkDevice_);
|
||||
}
|
||||
|
||||
void RtmwEngine::warmup(cudaStream_t stream) {
|
||||
std::vector<Box> boxes{{0, 0, static_cast<float>(maxSourceWidth_), static_cast<float>(maxSourceHeight_), 1, 1}};
|
||||
cv::Mat black(maxSourceHeight_, maxSourceWidth_, CV_8UC3, cv::Scalar(0, 0, 0));
|
||||
for (int i = 0; i < 2; ++i) infer(black, boxes, stream);
|
||||
}
|
||||
|
||||
std::vector<PersonPose> RtmwEngine::infer(
|
||||
cv::Mat const& frame, std::vector<Box> const& boxes, cudaStream_t stream,
|
||||
float* preprocessMs, float* inferenceMs, float* postprocessMs) {
|
||||
int batch = std::min(static_cast<int>(boxes.size()), maxPeople_);
|
||||
if (batch <= 0) return {};
|
||||
auto const preprocessStarted = std::chrono::steady_clock::now();
|
||||
int const cropStep = kInputWidth * 3;
|
||||
int const pixels = kInputWidth * kInputHeight;
|
||||
if (frame.cols>maxSourceWidth_ || frame.rows>maxSourceHeight_) throw std::runtime_error("Pose input exceeds allocation");
|
||||
auto npp = cms::nppContext(stream);
|
||||
CUDA_CHECK(cudaMemcpy2DAsync(sourceDevice_, frame.cols*3, frame.data, frame.step, frame.cols*3, frame.rows, cudaMemcpyHostToDevice, stream));
|
||||
for (int i = 0; i < batch; ++i) {
|
||||
auto const& box = boxes[i];
|
||||
double width = (static_cast<double>(box.x2) - static_cast<double>(box.x1)) * 1.25;
|
||||
double height = (static_cast<double>(box.y2) - static_cast<double>(box.y1)) * 1.25;
|
||||
constexpr double aspect = static_cast<double>(kInputWidth) / kInputHeight;
|
||||
if (width > height * aspect) height = width / aspect;
|
||||
else width = height * aspect;
|
||||
centersHost_[i * 2] = 0.5 * (static_cast<double>(box.x1) + static_cast<double>(box.x2));
|
||||
centersHost_[i * 2 + 1] = 0.5 * (static_cast<double>(box.y1) + static_cast<double>(box.y2));
|
||||
scalesHost_[i * 2] = width;
|
||||
scalesHost_[i * 2 + 1] = height;
|
||||
|
||||
cv::Point2f source[3]{
|
||||
{static_cast<float>(centersHost_[i * 2]), static_cast<float>(centersHost_[i * 2 + 1])},
|
||||
{static_cast<float>(centersHost_[i * 2]), static_cast<float>(centersHost_[i * 2 + 1] - width * 0.5)},
|
||||
{static_cast<float>(centersHost_[i * 2] - width * 0.5), static_cast<float>(centersHost_[i * 2 + 1] - width * 0.5)}};
|
||||
cv::Point2f destination[3]{
|
||||
{kInputWidth * 0.5F, kInputHeight * 0.5F},
|
||||
{kInputWidth * 0.5F, kInputHeight * 0.5F - kInputWidth * 0.5F},
|
||||
{0.0F, kInputHeight * 0.5F - kInputWidth * 0.5F}};
|
||||
cv::Mat affine = cv::getAffineTransform(source, destination);
|
||||
auto* cropDevice=cropsDevice_+static_cast<size_t>(i)*cropStep*kInputHeight;
|
||||
Npp8u fill[3]={0,0,0};
|
||||
cms::nppCheck(nppiSet_8u_C3R_Ctx(fill,cropDevice,cropStep,{kInputWidth,kInputHeight},npp));
|
||||
double coeff[2][3];
|
||||
for(int row=0;row<2;++row) for(int col=0;col<3;++col) coeff[row][col]=affine.at<double>(row,col);
|
||||
cms::nppCheck(nppiWarpAffine_8u_C3R_Ctx(sourceDevice_,{frame.cols,frame.rows},frame.cols*3,{0,0,frame.cols,frame.rows},cropDevice,cropStep,{0,0,kInputWidth,kInputHeight},coeff,NPPI_INTER_LINEAR,npp));
|
||||
}
|
||||
int const totalPixels = batch * pixels;
|
||||
poseNormalizeKernel<<<(totalPixels + 255) / 256, 256, 0, stream>>>(
|
||||
networkDevice_, cropsDevice_, batch, cropStep);
|
||||
CUDA_CHECK(cudaGetLastError());
|
||||
CUDA_CHECK(cudaStreamSynchronize(stream));
|
||||
if (preprocessMs) {
|
||||
*preprocessMs = static_cast<float>(std::chrono::duration<double, std::milli>(
|
||||
std::chrono::steady_clock::now() - preprocessStarted).count());
|
||||
}
|
||||
|
||||
// RTMPose's Python API invokes the ONNX/TensorRT session once per person.
|
||||
// Preserve that execution contract instead of batching people together.
|
||||
engine_.setInputShape("input", nvinfer1::Dims4{1, 3, kInputHeight, kInputWidth});
|
||||
engine_.prepare();
|
||||
CudaEventTimer timer;
|
||||
float inferTime = 0.0F;
|
||||
std::size_t const inputValues = static_cast<std::size_t>(3) * pixels;
|
||||
std::size_t const xValues = static_cast<std::size_t>(kKeypoints) * kSimccX;
|
||||
std::size_t const yValues = static_cast<std::size_t>(kKeypoints) * kSimccY;
|
||||
for (int person = 0; person < batch; ++person) {
|
||||
CUDA_CHECK(cudaMemcpyAsync(
|
||||
engine_.device("input"), networkDevice_ + static_cast<std::size_t>(person) * inputValues,
|
||||
inputValues * sizeof(float), cudaMemcpyDeviceToDevice, stream));
|
||||
timer.start(stream);
|
||||
if (!engine_.enqueue(stream)) throw std::runtime_error("RTMW enqueue failed");
|
||||
inferTime += timer.stop(stream);
|
||||
CUDA_CHECK(cudaMemcpyAsync(
|
||||
simccXHost_.data() + static_cast<std::size_t>(person) * xValues,
|
||||
engine_.device("simcc_x"), xValues * sizeof(float), cudaMemcpyDeviceToHost, stream));
|
||||
CUDA_CHECK(cudaMemcpyAsync(
|
||||
simccYHost_.data() + static_cast<std::size_t>(person) * yValues,
|
||||
engine_.device("simcc_y"), yValues * sizeof(float), cudaMemcpyDeviceToHost, stream));
|
||||
}
|
||||
if (inferenceMs) *inferenceMs = inferTime;
|
||||
|
||||
auto const postprocessStarted = std::chrono::steady_clock::now();
|
||||
CUDA_CHECK(cudaStreamSynchronize(stream));
|
||||
std::vector<PersonPose> result(batch);
|
||||
for (int person = 0; person < batch; ++person) {
|
||||
result[person].box = boxes[person];
|
||||
for (int keypoint = 0; keypoint < kKeypoints; ++keypoint) {
|
||||
auto xBegin = simccXHost_.begin() + (person * kKeypoints + keypoint) * kSimccX;
|
||||
auto yBegin = simccYHost_.begin() + (person * kKeypoints + keypoint) * kSimccY;
|
||||
auto xIt = std::max_element(xBegin, xBegin + kSimccX);
|
||||
auto yIt = std::max_element(yBegin, yBegin + kSimccY);
|
||||
float score = 0.5F * (*xIt + *yIt);
|
||||
if(!std::isfinite(score))throw std::runtime_error("Non-finite RTMW output");
|
||||
float localX = score > 0.0F ? static_cast<float>(xIt - xBegin) * 0.5F : -0.5F;
|
||||
float localY = score > 0.0F ? static_cast<float>(yIt - yBegin) * 0.5F : -0.5F;
|
||||
double scaleX = scalesHost_[person * 2];
|
||||
double scaleY = scalesHost_[person * 2 + 1];
|
||||
result[person].keypoints[keypoint] = {
|
||||
static_cast<float>(static_cast<double>(localX) / kInputWidth * scaleX + centersHost_[person * 2] - scaleX * 0.5),
|
||||
static_cast<float>(static_cast<double>(localY) / kInputHeight * scaleY + centersHost_[person * 2 + 1] - scaleY * 0.5),
|
||||
score};
|
||||
}
|
||||
}
|
||||
float post = static_cast<float>(std::chrono::duration<double, std::milli>(
|
||||
std::chrono::steady_clock::now() - postprocessStarted).count());
|
||||
if (postprocessMs) *postprocessMs = post;
|
||||
return result;
|
||||
}
|
||||
|
||||
} // namespace fangjia
|
||||
+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()
|
||||
@@ -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
|
||||
-300
@@ -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()
|
||||
+1
-2
@@ -1,6 +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/orin/lib${LD_LIBRARY_PATH:+:$LD_LIBRARY_PATH}"
|
||||
exec "${PYTHON:-python3}" -u "$root/run_camera.py" "$@"
|
||||
exec "$root/build/camera_risk" --config "$root/config/camera.json" "$@"
|
||||
|
||||
+15
-23
@@ -1,26 +1,18 @@
|
||||
#!/usr/bin/env bash
|
||||
# Refresh the standalone runtime after compiling on the target Orin.
|
||||
# Assemble the C++ runtime; no Python interpreter is used for packaging or execution.
|
||||
set -euo pipefail
|
||||
root="$(cd "$(dirname "$0")/.." && pwd)"
|
||||
python3 - "$root" <<'PY'
|
||||
from pathlib import Path
|
||||
import hashlib,shutil,sys
|
||||
root=Path(sys.argv[1]);package=root/'orin'
|
||||
library=root/'build/libvision_native.so'
|
||||
if not library.is_file():raise SystemExit('Run scripts/build.sh on Orin first.')
|
||||
# Replace generated Python directories so removed modules cannot survive packaging.
|
||||
for name in ('pipeline','annotations'):
|
||||
target=package/name
|
||||
if target.exists():shutil.rmtree(target)
|
||||
shutil.copytree(root/name,target,ignore=shutil.ignore_patterns('__pycache__','*.pyc'))
|
||||
(package/'build').mkdir(exist_ok=True)
|
||||
shutil.copy2(library,package/'build/libvision_native.so')
|
||||
shutil.copy2(root/'run_camera.py',package/'run_camera.py')
|
||||
shutil.copy2(root/'scripts/initialize_camera.sh',package/'scripts/initialize_camera_impl.sh')
|
||||
for name in ('lib/libusbcanfd.so','lib/libusb-1.0.so','models/engines/yolo11s.engine','models/engines/rtmw_l_384.engine','bin/drivers/max9295.ko','bin/drivers/max9296.ko','bin/drivers/sgx-yuv-gmsl2.ko'):
|
||||
if not (package/name).is_file():raise SystemExit('Missing packaged dependency: '+name)
|
||||
ignored={'__pycache__','outputs','logs'}
|
||||
files=sorted(p for p in package.rglob('*') if p.is_file() and not p.is_symlink() and p.name!='SHA256SUMS' and not ignored.intersection(p.relative_to(package).parts))
|
||||
(package/'SHA256SUMS').write_text(''.join(hashlib.sha256(p.read_bytes()).hexdigest()+' '+p.relative_to(package).as_posix()+'\n' for p in files))
|
||||
print('Package ready:',package/'run.sh')
|
||||
PY
|
||||
package="$root/orin"
|
||||
[[ -x "$root/build/camera_risk" ]] || { echo 'Compile on Orin with scripts/build.sh first.' >&2; exit 1; }
|
||||
mkdir -p "$package/build" "$package/config" "$package/annotations" "$package/scripts"
|
||||
cp -p "$root/build/camera_risk" "$package/build/"
|
||||
cp -p "$root/annotations/"*.json "$package/annotations/"
|
||||
# Source and standalone configurations differ only in the engine directory.
|
||||
sed 's|../orin/models/engines|../models/engines|' "$root/config/camera.json" > "$package/config/camera.json"
|
||||
cp -p "$root/scripts/initialize_camera.sh" "$package/scripts/initialize_camera_impl.sh"
|
||||
(
|
||||
cd "$package"
|
||||
find . -type f ! -path './outputs/*' ! -path './logs/*' ! -name SHA256SUMS -print0 |
|
||||
sort -z | xargs -0 sha256sum > SHA256SUMS
|
||||
)
|
||||
echo "C++ runtime ready: $package/build/camera_risk"
|
||||
|
||||
@@ -0,0 +1,66 @@
|
||||
#include "camera.hpp"
|
||||
#include "v4l2_capture.h"
|
||||
#include "uyvy_converter.hpp"
|
||||
#include <linux/videodev2.h>
|
||||
#include <opencv2/videoio.hpp>
|
||||
#include <chrono>
|
||||
#include <cstring>
|
||||
#include <stdexcept>
|
||||
namespace cms {
|
||||
std::uint64_t monotonicNs(){return std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::steady_clock::now().time_since_epoch()).count();}
|
||||
struct Camera::Impl {
|
||||
CameraConfig config;
|
||||
V4l2Capture capture;
|
||||
cv::VideoCapture video;
|
||||
std::unique_ptr<UyvyConverter> converter;
|
||||
std::atomic<bool> stopped{false};
|
||||
std::uint64_t index{};
|
||||
std::uint32_t previousSequence{};
|
||||
bool haveSequence{};
|
||||
explicit Impl(CameraConfig c):config(std::move(c)) {
|
||||
if(!config.video.empty()) {
|
||||
if(!video.open(config.video))throw std::runtime_error("Cannot open video: "+config.video);
|
||||
if(int(video.get(cv::CAP_PROP_FRAME_WIDTH))!=config.width||int(video.get(cv::CAP_PROP_FRAME_HEIGHT))!=config.height)throw std::runtime_error("Video dimensions must match camera configuration");
|
||||
} else {
|
||||
if(!capture.open(config.device)||!capture.setFormat(config.width,config.height,V4L2_PIX_FMT_UYVY))throw std::runtime_error("Camera open/format failed");
|
||||
if(capture.width()!=unsigned(config.width)||capture.height()!=unsigned(config.height))throw std::runtime_error("Unexpected negotiated camera dimensions");
|
||||
capture.setFps(config.fps);
|
||||
if(!capture.initMmap(config.buffers)||!capture.startStreaming())throw std::runtime_error("Camera streaming failed");
|
||||
converter=std::make_unique<UyvyConverter>(config.width,config.height);
|
||||
}
|
||||
}
|
||||
std::optional<CapturedFrame> read() {
|
||||
if(stopped)return {};
|
||||
if(video.isOpened()) {
|
||||
cv::Mat bgr;if(!video.read(bgr))return {};
|
||||
auto image=std::make_shared<BgrImage>();image->width=bgr.cols;image->height=bgr.rows;image->stride=bgr.cols*3;
|
||||
image->pixels.resize(std::size_t(image->stride)*image->height);
|
||||
for(int y=0;y<bgr.rows;++y)std::memcpy(image->pixels.data()+y*image->stride,bgr.ptr(y),image->stride);
|
||||
CaptureMetadata m{};m.frame_index=index;m.driver_sequence=index++;m.capture_monotonic_ns=m.received_monotonic_ns=monotonicNs();return CapturedFrame{m,image};
|
||||
}
|
||||
auto started=monotonicNs();unsigned misses=0;
|
||||
while(!stopped) {
|
||||
V4l2Capture::Frame f;
|
||||
if(!capture.getFrame(f,100)) {
|
||||
++misses;
|
||||
if((monotonicNs()-started)*1e-6>=config.timeout_ms)throw std::runtime_error("Camera timeout or repeated invalid frames");
|
||||
continue;
|
||||
}
|
||||
CaptureMetadata m{};m.frame_index=index++;m.driver_sequence=f.sequence;
|
||||
m.received_monotonic_ns=m.capture_monotonic_ns=monotonicNs();m.driver_timestamp_ns=f.timestamp_us*1000;m.discarded_error_frames=misses;
|
||||
if(haveSequence){auto delta=std::uint32_t(f.sequence-previousSequence);if(delta>0&&delta<0x80000000U)m.skipped_driver_frames=delta-1;}
|
||||
previousSequence=f.sequence;haveSequence=true;
|
||||
auto stride=f.bytes_per_line?f.bytes_per_line:unsigned(config.width*2);
|
||||
try {
|
||||
if(f.size<std::size_t(stride)*config.height)throw std::runtime_error("Incomplete camera frame");
|
||||
auto image=converter->convert(f.data,stride);capture.releaseFrame(f);return CapturedFrame{m,std::move(image)};
|
||||
}catch(...){capture.releaseFrame(f);throw;}
|
||||
}
|
||||
return {};
|
||||
}
|
||||
};
|
||||
Camera::Camera(CameraConfig c):impl_(std::make_unique<Impl>(std::move(c))){}
|
||||
Camera::~Camera()=default;
|
||||
std::optional<CapturedFrame> Camera::read(){return impl_->read();}
|
||||
void Camera::requestStop()noexcept{impl_->stopped=true;}
|
||||
}
|
||||
@@ -0,0 +1,213 @@
|
||||
#include "camera_pipeline.hpp"
|
||||
#include "bounded_queue.hpp"
|
||||
#include "latest_result.hpp"
|
||||
#include "detector.hpp"
|
||||
#include "door_risk.hpp"
|
||||
#include "records.hpp"
|
||||
#include "result_conversion.hpp"
|
||||
#include "door_zone.hpp"
|
||||
#include "can_state.hpp"
|
||||
#include "serialization.hpp"
|
||||
#include <fstream>
|
||||
#include <opencv2/core.hpp>
|
||||
#include <atomic>
|
||||
#include <cmath>
|
||||
#include <cstring>
|
||||
#include <exception>
|
||||
#include <mutex>
|
||||
#include <limits>
|
||||
#include <thread>
|
||||
|
||||
namespace cms {
|
||||
namespace {
|
||||
struct Stream {
|
||||
cudaStream_t value{};
|
||||
Stream(){CUDA_CHECK(cudaStreamCreateWithFlags(&value,cudaStreamNonBlocking));}
|
||||
~Stream(){cudaStreamDestroy(value);}
|
||||
};
|
||||
cv::Mat imageView(BgrImage const& image) {
|
||||
return cv::Mat(image.height,image.width,CV_8UC3,const_cast<std::uint8_t*>(image.pixels.data()),image.stride);
|
||||
}
|
||||
double elapsedMs(std::uint64_t start){return (monotonicNs()-start)*1e-6;}
|
||||
void attachPoses(FrameResult& result,fangjia::PoseFrameResult const& fused) {
|
||||
for(std::size_t i=0;i<fused.selectedPeople.size();++i) {
|
||||
auto const& source=fused.selectedPeople[i];
|
||||
PersonResult person;person.track_id=source.box.trackId;person.confidence=source.box.score;
|
||||
person.box={source.box.x1,source.box.y1,source.box.x2,source.box.y2};
|
||||
for(std::size_t k=0;k<133;++k) {
|
||||
auto const&a=source.keypoints[k];auto const&b=fused.smoothedPeople[i].keypoints[k];auto const&c=fused.v2People[i].keypoints[k];
|
||||
person.selected[k]={a.x,a.y,a.score};person.smoothed[k]={b.x,b.y,b.score};person.temporal[k]={c.x,c.y,c.score};
|
||||
}
|
||||
for(int side=0;side<2;++side){auto const& m=fused.metadata[i][side];person.hands[side]={m.handState,m.handSource,m.limbConsensus,m.distanceEligible,m.alarmEligible};}
|
||||
result.persons.push_back(std::move(person));
|
||||
}
|
||||
for(auto const& source:fused.globalHands) {
|
||||
HandRegion hand;hand.person_id=source.personId;hand.track_id=source.trackId;hand.side=source.left?"left":"right";
|
||||
for(std::size_t k=0;k<21;++k)hand.keypoints[k]={source.keypoints[k].x,source.keypoints[k].y,source.keypoints[k].score};
|
||||
result.hand_regions.push_back(std::move(hand));
|
||||
}
|
||||
}
|
||||
}
|
||||
struct CameraPipeline::Impl {
|
||||
struct Detected {CapturedFrame frame;std::vector<fangjia::Box> boxes;double elapsed_ms{};};
|
||||
PipelineConfig config;
|
||||
BoundedQueue<CapturedFrame> captures;
|
||||
BoundedQueue<Detected> detections;
|
||||
LatestResult results;
|
||||
fangjia::TrtLogger logger;
|
||||
std::unique_ptr<Detector> detector;
|
||||
std::unique_ptr<fangjia::RtmwEngine> pose;
|
||||
std::unique_ptr<DoorRisk> risk;
|
||||
std::unique_ptr<DoorZoneAssigner> zone;
|
||||
std::unique_ptr<CanInput> can;
|
||||
std::ofstream output, poseOutput;
|
||||
std::shared_ptr<const BinaryMask> doorPixels;
|
||||
std::unique_ptr<Camera> camera;
|
||||
std::thread captureThread,detectorThread,inferenceThread;
|
||||
mutable std::mutex stateMutex;
|
||||
std::exception_ptr failure;
|
||||
bool started{};
|
||||
std::atomic<bool> stopped{false},active{false};
|
||||
std::atomic<std::uint64_t> captured{0},published{0},gaps{0},errorFrames{0};
|
||||
double totalLatency{};
|
||||
|
||||
explicit Impl(PipelineConfig c):config(std::move(c)),captures(config.queue_capacity),detections(config.queue_capacity) {
|
||||
if(config.can_side!="left"&&config.can_side!="right")throw std::invalid_argument("CAN side must be left or right");
|
||||
if(config.active_door!="can"&&config.active_door!="front"&&config.active_door!="rear"&&config.active_door!="both"&&config.active_door!="none")throw std::invalid_argument("Invalid active door");
|
||||
if(!config.can_enabled&&config.active_door=="can")throw std::invalid_argument("Disabled CAN requires an explicit simulated active door");
|
||||
if(config.can_timeout_ms<=0)throw std::invalid_argument("CAN timeout must be positive");
|
||||
if(config.queue_capacity>8)throw std::invalid_argument("Queue capacity must be between 1 and 8");
|
||||
if(config.max_people<1||config.max_people>16||!std::isfinite(config.detector_confidence)||config.detector_confidence<=0||config.detector_confidence>=1)
|
||||
throw std::invalid_argument("Invalid detector configuration");
|
||||
if(!std::filesystem::is_regular_file(config.door_mask))throw std::invalid_argument("Missing door mask: "+config.door_mask.string());
|
||||
}
|
||||
~Impl(){stop();joinWorkers();}
|
||||
void joinWorkers() noexcept {
|
||||
if(captureThread.joinable())captureThread.join();
|
||||
if(detectorThread.joinable())detectorThread.join();
|
||||
if(inferenceThread.joinable())inferenceThread.join();
|
||||
}
|
||||
void stop() noexcept {
|
||||
stopped=true;active=false;
|
||||
// Camera stays alive until all threads have joined. start()/destruction
|
||||
// are owner-thread operations; requestStop() only interrupts existing work.
|
||||
{std::lock_guard<std::mutex> lock(stateMutex);if(camera)camera->requestStop();}
|
||||
captures.close(true);detections.close(true);
|
||||
}
|
||||
void fail() noexcept {
|
||||
{std::lock_guard<std::mutex> lock(stateMutex);if(!failure)failure=std::current_exception();}
|
||||
stop();
|
||||
}
|
||||
void rethrowFailure() const {
|
||||
std::lock_guard<std::mutex> lock(stateMutex);if(failure)std::rethrow_exception(failure);
|
||||
}
|
||||
void start() {
|
||||
{std::lock_guard<std::mutex> lock(stateMutex);if(started||stopped)throw std::logic_error("CameraPipeline can only be started once");started=true;}
|
||||
try {
|
||||
detector=std::make_unique<Detector>(config.engines/"yolo11s.engine",logger,config.detector_confidence,config.max_people);
|
||||
pose=std::make_unique<fangjia::RtmwEngine>(config.engines/"rtmw_l_384.engine",logger,config.camera.width,config.camera.height,config.max_people);
|
||||
DoorOptions options;options.constant=config.door_mask;options.warning=config.warning_relative;options.release=config.release_relative;
|
||||
risk=std::make_unique<DoorRisk>(options,cv::Size(config.camera.width,config.camera.height));risk->initialize();
|
||||
if(risk->mask().empty()||cv::countNonZero(risk->mask())==0)throw std::runtime_error("Door mask has no usable region");
|
||||
auto pixels=std::make_shared<BinaryMask>();pixels->width=config.camera.width;pixels->height=config.camera.height;pixels->stride=config.camera.width;
|
||||
auto const& mask=risk->mask();pixels->pixels.resize(std::size_t(mask.cols)*mask.rows);
|
||||
for(int y=0;y<mask.rows;++y)std::memcpy(pixels->pixels.data()+std::size_t(y)*mask.cols,mask.ptr(y),mask.cols);
|
||||
doorPixels=std::move(pixels);
|
||||
zone=std::make_unique<DoorZoneAssigner>(config.door_zone_config);
|
||||
auto openOutput=[](std::ofstream& out,std::filesystem::path const& path) {
|
||||
if(path.empty())return;
|
||||
if(!path.parent_path().empty())std::filesystem::create_directories(path.parent_path());
|
||||
out.exceptions(std::ios::failbit|std::ios::badbit);out.open(path);
|
||||
};
|
||||
openOutput(output,config.results_path);openOutput(poseOutput,config.pose_records_path);
|
||||
Stream warmup;
|
||||
pose->warmup(warmup.value);
|
||||
detector->infer(cv::Mat(config.camera.height,config.camera.width,CV_8UC3,cv::Scalar()),warmup.value);
|
||||
{std::lock_guard<std::mutex> lock(stateMutex);camera=std::make_unique<Camera>(config.camera);if(stopped)camera->requestStop();}
|
||||
can=std::make_unique<CanInput>(config.can_enabled);
|
||||
active=!stopped.load();
|
||||
captureThread=std::thread([this]{try{captureLoop();}catch(...){fail();}});
|
||||
detectorThread=std::thread([this]{try{detectorLoop();}catch(...){fail();}});
|
||||
inferenceThread=std::thread([this]{try{inferenceLoop();}catch(...){fail();}});
|
||||
}catch(...){stop();joinWorkers();throw;}
|
||||
}
|
||||
void captureLoop() {
|
||||
while(!stopped) {
|
||||
auto frame=camera->read();if(!frame)break;
|
||||
gaps+=frame->metadata.skipped_driver_frames;
|
||||
errorFrames+=frame->metadata.discarded_error_frames;
|
||||
bool queued=config.camera.video.empty()?captures.pushLatest(std::move(*frame)):captures.push(std::move(*frame));
|
||||
if(!queued)break;
|
||||
++captured;
|
||||
}
|
||||
captures.close();
|
||||
}
|
||||
void detectorLoop() {
|
||||
Stream stream;
|
||||
while(!stopped) {
|
||||
auto frame=captures.pop();if(!frame)break;
|
||||
auto startedAt=monotonicNs();auto boxes=detector->infer(imageView(*frame->image),stream.value);
|
||||
Detected item{std::move(*frame),std::move(boxes),elapsedMs(startedAt)};
|
||||
bool queued=config.camera.video.empty()?detections.pushLatest(std::move(item)):detections.push(std::move(item));
|
||||
if(!queued)break;
|
||||
}
|
||||
detections.close();
|
||||
}
|
||||
void inferenceLoop() {
|
||||
Stream stream;fangjia::BoxTracker boxesTracker;fangjia::PoseFusionTracker fusion;std::uint64_t processed=0;
|
||||
while(!stopped) {
|
||||
auto detected=detections.pop();if(!detected)break;
|
||||
if(processed>static_cast<std::uint64_t>(std::numeric_limits<int>::max()))throw std::runtime_error("Frame index limit reached; restart pipeline");
|
||||
auto index=static_cast<int>(processed++);
|
||||
auto& boxes=detected->boxes;
|
||||
auto frame=imageView(*detected->frame.image);
|
||||
boxesTracker.update(boxes,index,frame.cols,frame.rows);
|
||||
auto poseStart=monotonicNs();
|
||||
auto raw=pose->infer(frame,boxes,stream.value);auto isolated=raw;
|
||||
for(std::size_t p=0;p<boxes.size();++p) {
|
||||
auto const& box=boxes[p];double area=std::max(double(box.x2-box.x1)*(box.y2-box.y1),1.0);
|
||||
bool needsIsolation=false;
|
||||
for(std::size_t q=0;q<boxes.size();++q)if(q!=p&&std::max(double(boxes[q].x2-boxes[q].x1)*(boxes[q].y2-boxes[q].y1),1.0)<0.5*area)needsIsolation=true;
|
||||
if(needsIsolation){auto masked=maskOtherPeople(frame,boxes,p);isolated[p]=pose->infer(masked,{box},stream.value).at(0);}
|
||||
}
|
||||
auto poseElapsed=elapsedMs(poseStart);auto fusionStart=monotonicNs();
|
||||
auto fused=fusion.update(raw,isolated,index);
|
||||
auto record=makeRecord(index,raw,isolated,fused,poseOutput.is_open(),true);
|
||||
auto riskData=risk->evaluate(index,record);
|
||||
auto result=std::make_shared<FrameResult>(makeFrameResult(record,riskData,detected->frame.metadata));
|
||||
auto assignments=zone->update(index,record,frame.size());
|
||||
result->can=can->snapshot(config.can_side,config.can_timeout_ms);
|
||||
result->active_door=activeDoor(result->can);
|
||||
if(config.active_door!="can") {result->active_door=config.active_door;result->can.source="simulation";result->can.valid=true;}
|
||||
auto decisions=buildDoorRisks(riskData,assignments,result->active_door,result->can.valid);
|
||||
result->risk=parseRisk(decisions.selected);
|
||||
for(auto const& entry:decisions.doors.items())result->door_risks[entry.key()]=parseRisk(entry.value());
|
||||
for(auto const& a:assignments)result->door_assignments.push_back(parseAssignment(a));
|
||||
attachPoses(*result,fused);
|
||||
result->image=std::move(detected->frame.image);result->door_mask_pixels=doorPixels;
|
||||
result->door_mask_calibration_verified=config.door_mask_calibration_verified;
|
||||
result->timing={detected->elapsed_ms,poseElapsed,elapsedMs(fusionStart),elapsedMs(result->capture.capture_monotonic_ns)};
|
||||
FrameResultPtr immutable=result;
|
||||
results.publish(std::move(immutable));
|
||||
{std::lock_guard<std::mutex> lock(stateMutex);totalLatency+=result->timing.capture_to_result_ms;++published;}
|
||||
if(output.is_open())output<<serializeResult(*result).dump()<<'\n';
|
||||
if(poseOutput.is_open())poseOutput<<record.dump()<<'\n';
|
||||
if(config.max_frames&&processed>=config.max_frames){stop();break;}
|
||||
}
|
||||
if(output.is_open())output.flush();
|
||||
if(poseOutput.is_open())poseOutput.flush();
|
||||
active=false;
|
||||
}
|
||||
};
|
||||
CameraPipeline::CameraPipeline(PipelineConfig config):impl_(std::make_unique<Impl>(std::move(config))){}
|
||||
CameraPipeline::~CameraPipeline()=default;
|
||||
void CameraPipeline::start(){impl_->start();}
|
||||
FrameResultPtr CameraPipeline::latest()const{return impl_->results.load();}
|
||||
bool CameraPipeline::running()const noexcept{return impl_->active.load();}
|
||||
void CameraPipeline::requestStop()noexcept{impl_->stop();}
|
||||
void CameraPipeline::join(){impl_->joinWorkers();{std::lock_guard<std::mutex> lock(impl_->stateMutex);impl_->camera.reset();impl_->can.reset();}impl_->rethrowFailure();}
|
||||
PipelineStats CameraPipeline::stats()const {
|
||||
std::lock_guard<std::mutex> lock(impl_->stateMutex);auto count=impl_->published.load();
|
||||
return {impl_->captured.load(),count,impl_->gaps.load(),impl_->errorFrames.load(),count?impl_->totalLatency/count:0};
|
||||
}
|
||||
}
|
||||
@@ -7,6 +7,7 @@
|
||||
#include <cstdio>
|
||||
#include <cstring>
|
||||
#include <pthread.h>
|
||||
#include <atomic>
|
||||
#include <unistd.h>
|
||||
|
||||
#include "zcan.h"
|
||||
@@ -31,26 +32,27 @@ typedef struct
|
||||
int dev_idx; // 设备索引
|
||||
int chn_idx; // 通道号
|
||||
int total; // 接收总数
|
||||
int stop; // 线程结束标志
|
||||
std::atomic<int> stop{0}; // 线程结束标志
|
||||
} THREAD_CTX;
|
||||
|
||||
/* CAN 运行标志, 由信号处理函数(Ctrl+C)或外部置位 */
|
||||
static volatile bool g_can_running = true;
|
||||
|
||||
static int g_receive_count = 0;
|
||||
static std::atomic<int> g_receive_count{0};
|
||||
|
||||
/* ===== 仅供 can_start/can_stop 共享的文件内静态变量 ===== */
|
||||
static int s_DevType = USBCANFD;
|
||||
static int s_DevIdx = 0;
|
||||
static THREAD_CTX s_rx_ctx[MAX_CHANNELS];
|
||||
static pthread_t s_rx_threads[MAX_CHANNELS];
|
||||
static bool s_thread_started[MAX_CHANNELS]{};
|
||||
static bool s_can_opened = false;
|
||||
|
||||
namespace CanComm {
|
||||
|
||||
bool is_open() { return s_can_opened; }
|
||||
|
||||
int receive_count() { return g_receive_count; }
|
||||
int receive_count() { return g_receive_count.load(); }
|
||||
|
||||
/* ===== 报文解析回调 (由 main.cpp 通过 set_msg_callback 注册) ===== */
|
||||
static MsgCallback s_msg_cb = nullptr;
|
||||
@@ -218,7 +220,7 @@ bool can_start()
|
||||
// 初始化,启动通道
|
||||
for (int i = 0; i < MAX_CHANNELS; i++)
|
||||
{
|
||||
ZCAN_INIT init; // 波特率结构体,数据根据zcanpro的波特率计算器得出
|
||||
ZCAN_INIT init{}; // 波特率结构体,数据根据zcanpro的波特率计算器得出
|
||||
init.clk = 60000000; // clock: 60M(V1.01) 80M(V1.03即以上)
|
||||
init.mode = 0; // 0-正常
|
||||
|
||||
@@ -237,7 +239,7 @@ bool can_start()
|
||||
if (!VCI_InitCAN(s_DevType, s_DevIdx, i, &init)) // 初始化通道
|
||||
{
|
||||
printf("InitCAN(%d) fail\n", i);
|
||||
return false;
|
||||
can_stop();return false;
|
||||
}
|
||||
printf("InitCAN(%d) success\n", i);
|
||||
|
||||
@@ -258,7 +260,7 @@ bool can_start()
|
||||
if (!VCI_StartCAN(s_DevType, s_DevIdx, i)) // 启动通道
|
||||
{
|
||||
printf("StartCAN(%d) fail\n", i);
|
||||
return false;
|
||||
can_stop();return false;
|
||||
}
|
||||
printf("StartCAN(%d) success\n", i);
|
||||
|
||||
@@ -270,12 +272,13 @@ bool can_start()
|
||||
|
||||
if (IsMerge && i == 0)
|
||||
{
|
||||
pthread_create(&s_rx_threads[i], NULL, rx_merge_thread, &s_rx_ctx[i]); // 合并接收线程
|
||||
s_thread_started[i]=pthread_create(&s_rx_threads[i], NULL, rx_merge_thread, &s_rx_ctx[i])==0; // 合并接收线程
|
||||
}
|
||||
else
|
||||
{
|
||||
pthread_create(&s_rx_threads[i], NULL, rx_thread, &s_rx_ctx[i]); // 创建接收线程
|
||||
s_thread_started[i]=pthread_create(&s_rx_threads[i], NULL, rx_thread, &s_rx_ctx[i])==0; // 创建接收线程
|
||||
}
|
||||
if(!s_thread_started[i]){can_stop();return false;}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
@@ -293,7 +296,7 @@ void can_stop()
|
||||
for (int i = 0; i < MAX_CHANNELS; i++)
|
||||
{
|
||||
s_rx_ctx[i].stop = 1;
|
||||
pthread_join(s_rx_threads[i], NULL); // 等待接收线程退出
|
||||
if(s_thread_started[i]){pthread_join(s_rx_threads[i], NULL);s_thread_started[i]=false;} // 等待接收线程退出
|
||||
|
||||
if (!VCI_ResetCAN(s_DevType, s_DevIdx, i)) // 复位通道
|
||||
printf("ResetCAN(%d) fail\n", i);
|
||||
@@ -0,0 +1,39 @@
|
||||
#include "can_state.hpp"
|
||||
#include "can_comm.h"
|
||||
#include "camera.hpp"
|
||||
#include <mutex>
|
||||
#include <stdexcept>
|
||||
namespace cms {
|
||||
namespace {
|
||||
std::mutex mutex;
|
||||
std::array<int,4> positions{{-1,-1,-1,-1}};
|
||||
std::uint64_t timestamp{};
|
||||
bool owned{};
|
||||
void received(int64_t,uint32_t id,uint8_t const* data,uint32_t size) {
|
||||
if(id!=0x391||size<4)return;
|
||||
std::lock_guard<std::mutex> lock(mutex);
|
||||
for(int i=0;i<4;++i)positions[i]=data[i];
|
||||
timestamp=monotonicNs();
|
||||
}
|
||||
}
|
||||
CanInput::CanInput(bool enabled) {
|
||||
{std::lock_guard<std::mutex> lock(mutex);if(owned)throw std::logic_error("Only one CAN input may run per process");owned=true;positions.fill(-1);timestamp=0;}
|
||||
if(enabled){CanComm::set_msg_callback(received);opened_=CanComm::can_start();}
|
||||
}
|
||||
CanInput::~CanInput() {
|
||||
if(opened_)CanComm::can_stop();
|
||||
CanComm::set_msg_callback(nullptr);
|
||||
std::lock_guard<std::mutex> lock(mutex);owned=false;
|
||||
}
|
||||
CanSnapshot CanInput::snapshot(std::string const& side,double timeoutMs) const {
|
||||
std::lock_guard<std::mutex> lock(mutex);CanSnapshot s;s.positions=positions;s.side=side;
|
||||
s.age_ms=timestamp?(monotonicNs()-timestamp)*1e-6:-1;
|
||||
int offset=side=="left"?0:1;
|
||||
s.valid=s.age_ms>=0&&s.age_ms<=timeoutMs&&s.positions[offset]>=0&&s.positions[offset+2]>=0;return s;
|
||||
}
|
||||
std::string activeDoor(CanSnapshot const& s) {
|
||||
if(!s.valid)return "none";
|
||||
int i=s.side=="left"?0:1;bool front=s.positions[i]==0,rear=s.positions[i+2]==0;
|
||||
return front&&rear?"both":front?"front":rear?"rear":"none";
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,29 @@
|
||||
#include "config.hpp"
|
||||
#include "door_risk.hpp"
|
||||
#include <fstream>
|
||||
#include <stdexcept>
|
||||
|
||||
namespace cms {
|
||||
PipelineConfig loadConfig(std::filesystem::path const& path) {
|
||||
std::ifstream file(path);
|
||||
if(!file)throw std::runtime_error("Cannot read configuration: "+path.string());
|
||||
auto j=Json::parse(file);PipelineConfig config;
|
||||
auto const& camera=j.at("camera");
|
||||
config.camera.device=camera.at("device");config.camera.width=camera.at("width");
|
||||
config.camera.height=camera.at("height");config.camera.fps=camera.at("fps");
|
||||
config.camera.buffers=camera.at("buffers");config.camera.timeout_ms=camera.at("timeout_ms");
|
||||
auto resolve=[&](std::string const& value){auto p=std::filesystem::path(value);return std::filesystem::absolute(p.is_absolute()?p:path.parent_path()/p);};
|
||||
auto const& inference=j.at("inference");
|
||||
config.engines=resolve(inference.at("engine_directory"));config.max_people=inference.at("max_people");
|
||||
config.detector_confidence=inference.at("detector_confidence");config.queue_capacity=inference.at("queue_capacity");
|
||||
auto const& door=j.at("door");config.door_mask=resolve(door.at("mask"));
|
||||
config.warning_relative=door.at("warning_relative");config.release_relative=door.at("release_relative");
|
||||
config.door_mask_calibration_verified=door.at("calibration_verified");
|
||||
config.door_zone_config=resolve(door.at("zone_config"));
|
||||
if(j.contains("can")) {
|
||||
auto const& can=j.at("can");config.can_enabled=can.value("enabled",true);
|
||||
config.can_side=can.value("side",std::string("left"));config.can_timeout_ms=can.value("timeout_ms",1000.0);
|
||||
}
|
||||
return config;
|
||||
}
|
||||
}
|
||||
@@ -56,4 +56,15 @@ std::vector<Box> Detector::infer(cv::Mat const& frame,cudaStream_t stream,int fi
|
||||
std::stable_sort(filtered.begin(),filtered.end(),[](auto const&a,auto const&b){return a.x1<b.x1;});
|
||||
return filtered;
|
||||
}
|
||||
cv::Mat maskOtherPeople(cv::Mat const& frame,std::vector<Box> const& boxes,size_t target) {
|
||||
auto masked=frame.clone(); auto const&t=boxes.at(target);
|
||||
double targetArea=std::max(double(t.x2-t.x1)*(t.y2-t.y1),1.0);
|
||||
for(size_t i=0;i<boxes.size();++i) {
|
||||
auto const& b=boxes[i]; if(i==target||std::max(double(b.x2-b.x1)*(b.y2-b.y1),1.0)>=0.5*targetArea) continue;
|
||||
int x1=std::clamp(int(std::nearbyint(b.x1)),0,frame.cols-1),x2=std::clamp(int(std::nearbyint(b.x2)),0,frame.cols-1);
|
||||
int y1=std::clamp(int(std::nearbyint(b.y1)),0,frame.rows-1),y2=std::clamp(int(std::nearbyint(b.y2)),0,frame.rows-1);
|
||||
if(x2>=x1&&y2>=y1) cv::rectangle(masked,{x1,y1},{x2,y2},{124,116,104},-1);
|
||||
}
|
||||
return masked;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,137 @@
|
||||
#include "door_risk.hpp"
|
||||
#include <opencv2/imgproc.hpp>
|
||||
#include <opencv2/imgcodecs.hpp>
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <fstream>
|
||||
#include <regex>
|
||||
#include <stdexcept>
|
||||
namespace cms {
|
||||
double rounded(double v, int d) { double s=std::pow(10.0,d); return std::nearbyint(v*s)/s; }
|
||||
static std::string lower(std::string s) { for(auto& c:s) if(static_cast<unsigned char>(c)<128) c=static_cast<char>(std::tolower(c)); return s; }
|
||||
static bool raster(std::filesystem::path const& p) { auto e=lower(p.extension().string()); return e==".png"||e==".bmp"||e==".tif"||e==".tiff"; }
|
||||
DoorRisk::DoorRisk(DoorOptions o, cv::Size size):options_(std::move(o)),size_(size) {
|
||||
if(options_.constant.empty()==options_.directory.empty()) throw std::runtime_error("Provide exactly one constant mask or door-mask directory");
|
||||
if(options_.warning<0 || options_.release<=options_.warning) throw std::runtime_error("Risk thresholds require 0 <= warning < release");
|
||||
format_=options_.format;
|
||||
if(!options_.constant.empty()) {
|
||||
if(!std::filesystem::is_regular_file(options_.constant)) throw std::runtime_error("Missing mask: "+options_.constant.string());
|
||||
if(format_=="auto") format_=lower(options_.constant.extension().string())==".json"?"xanylabeling":"raster";
|
||||
} else {
|
||||
std::vector<std::filesystem::path> files;
|
||||
for(auto const& e:std::filesystem::recursive_directory_iterator(options_.directory)) if(e.is_regular_file()) files.push_back(e.path());
|
||||
std::sort(files.begin(),files.end());
|
||||
if(format_=="auto") format_=std::any_of(files.begin(),files.end(),[](auto const& p){return p.extension()==".json";})?"xanylabeling":"raster";
|
||||
std::regex digits("[0-9]+");
|
||||
for(auto const& p:files) {
|
||||
if(format_=="xanylabeling"?p.extension()!=".json":!raster(p)) continue;
|
||||
int index=-1; std::string stem=p.stem().string();
|
||||
for(std::sregex_iterator i(stem.begin(),stem.end(),digits),end;i!=end;++i) index=std::stoi(i->str());
|
||||
if(index<0 && format_=="xanylabeling") { std::ifstream in(p); auto j=Json::parse(in); if(j.contains("frame_index")) index=j["frame_index"].get<int>(); else if(j.contains("frameIndex")) index=j["frameIndex"].get<int>(); }
|
||||
if(index<0) continue;
|
||||
if(!paths_.emplace(index,p).second) throw std::runtime_error("Duplicate mask frame: "+std::to_string(index));
|
||||
}
|
||||
if(paths_.empty()) throw std::runtime_error("No indexed door masks in "+options_.directory.string());
|
||||
}
|
||||
if(format_!="raster" && format_!="xanylabeling") throw std::runtime_error("Unknown door mask format");
|
||||
}
|
||||
cv::Mat DoorRisk::rasterize(std::filesystem::path const& path) const {
|
||||
cv::Mat mask(size_,CV_8U,cv::Scalar(0));
|
||||
if(format_=="raster") {
|
||||
auto img=cv::imread(path.string(),cv::IMREAD_UNCHANGED);
|
||||
if(img.empty()) throw std::runtime_error("Cannot read mask: "+path.string());
|
||||
if(img.channels()>1) { std::vector<cv::Mat> ch; cv::split(img,ch); img=ch[0]; for(size_t i=1;i<std::min(ch.size(),size_t(3));++i) cv::max(img,ch[i],img); }
|
||||
if(img.size()!=size_) cv::resize(img,img,size_,0,0,cv::INTER_NEAREST);
|
||||
return img>0;
|
||||
}
|
||||
std::ifstream in(path); auto j=Json::parse(in);
|
||||
double w=j.contains("imageWidth")&&!j["imageWidth"].is_null()?j["imageWidth"].get<double>():size_.width;
|
||||
double h=j.contains("imageHeight")&&!j["imageHeight"].is_null()?j["imageHeight"].get<double>():size_.height;
|
||||
if(w==0) w=size_.width;
|
||||
if(h==0) h=size_.height;
|
||||
float sx=static_cast<float>(size_.width/std::max(w,1.0)),sy=static_cast<float>(size_.height/std::max(h,1.0));
|
||||
for(auto const& s:j.value("shapes",Json::array())) {
|
||||
if(!options_.labels.count(lower(s.value("label",std::string())))) continue;
|
||||
std::vector<cv::Point> points;
|
||||
for(auto const& p:s.value("points",Json::array())) points.emplace_back(static_cast<int>(std::nearbyint(p[0].get<float>()*sx)),static_cast<int>(std::nearbyint(p[1].get<float>()*sy)));
|
||||
auto type=lower(s.value("shape_type",std::string("polygon")));
|
||||
if(points.size()<2) continue;
|
||||
if(type=="rectangle") cv::rectangle(mask,points[0],points[1],255,-1);
|
||||
else if(type=="circle") cv::circle(mask,points[0],static_cast<int>(std::nearbyint(cv::norm(points[1]-points[0]))),255,-1);
|
||||
else if(points.size()>=3) cv::fillPoly(mask,std::vector<std::vector<cv::Point>>{points},255);
|
||||
}
|
||||
return mask;
|
||||
}
|
||||
void DoorRisk::load(int frame) {
|
||||
if(cached_) return;
|
||||
loadedPath_.clear(); mask_.release(); distance_.release();
|
||||
if(!options_.constant.empty()) loadedPath_=options_.constant;
|
||||
else { auto it=paths_.find(frame+options_.offset); if(it!=paths_.end()) loadedPath_=it->second; }
|
||||
if(loadedPath_.empty()) {
|
||||
status_="missing";
|
||||
if(options_.require) throw std::runtime_error("No door mask for frame "+std::to_string(frame));
|
||||
return;
|
||||
}
|
||||
mask_=rasterize(loadedPath_);
|
||||
status_=cv::countNonZero(mask_)?"loaded":"empty";
|
||||
if(status_=="loaded") cv::distanceTransform(mask_==0,distance_,cv::DIST_L2,cv::DIST_MASK_PRECISE);
|
||||
cached_=!options_.constant.empty();
|
||||
}
|
||||
Json DoorRisk::evaluate(int frame, Json& record) {
|
||||
load(frame);
|
||||
Json regions=Json::array(),fallbacks=Json::array(),nearest=nullptr;
|
||||
std::set<int> withHands;
|
||||
auto measure=[&](cv::Mat const& region, std::string const& source,int track,std::string const& side,std::string const& state) {
|
||||
Json m={{"status","door_mask_"+status_},{"risk_score",nullptr},{"risk_level","unavailable"},{"decision","EVIDENCE_UNAVAILABLE"},{"source",source},{"track_id",track},{"side",side}};
|
||||
if(status_=="loaded") {
|
||||
int area=cv::countNonZero(region); cv::Mat intersection; cv::bitwise_and(region,mask_,intersection); int overlap=cv::countNonZero(intersection);
|
||||
double distance=0; cv::minMaxLoc(distance_,&distance,nullptr,nullptr,nullptr,region);
|
||||
double scale=std::max(std::sqrt(double(area)),1.0),relative=distance/scale;
|
||||
double score=overlap?1:std::clamp(1-relative/options_.release,0.0,1.0);
|
||||
std::string level=overlap?"danger":relative<=options_.warning?"warning":relative<=options_.release?"watch":"clear";
|
||||
std::string decision=overlap?"DANGER_INTERSECTION":relative<=options_.warning?"WARNING_NEAR":relative<=options_.release?"WATCH":"CLEAR";
|
||||
m={{"status","calculated"},{"risk_score",rounded(score,5)},{"risk_level",level},{"decision",decision},{"source",source},{"track_id",track},{"side",side},{"state",state},{"region_area_px",area},{"door_overlap_px",overlap},{"region_overlap_ratio",rounded(double(overlap)/std::max(area,1),6)},{"min_distance_px",rounded(distance,3)},{"region_scale_px",rounded(scale,3)},{"distance_relative",rounded(relative,6)}};
|
||||
if(nearest.is_null() || m["risk_score"].get<double>()>nearest["risk_score"].get<double>()) nearest=m;
|
||||
}
|
||||
regions.push_back(m); return m;
|
||||
};
|
||||
auto const ids=record.value("track_ids",Json::array());
|
||||
auto trackFor=[&](int pos){return pos>=0&&pos<int(ids.size())?ids[pos].get<int>():pos+1;};
|
||||
if(!record.contains("global_hands")) record["global_hands"]=Json::array();
|
||||
for(auto& hand:record["global_hands"]) {
|
||||
int pos=hand.value("person_id",0)-1; std::vector<cv::Point> points;
|
||||
bool finite=true;
|
||||
for(auto const& p:hand.value("keypoints",Json::array())) {
|
||||
float x=p[0].get<float>(),y=p[1].get<float>();
|
||||
if(!std::isfinite(x)||!std::isfinite(y)) {finite=false;break;}
|
||||
points.emplace_back(static_cast<int>(std::nearbyint(x)),static_cast<int>(std::nearbyint(y)));
|
||||
}
|
||||
if(!finite||points.size()<3) continue;
|
||||
std::vector<cv::Point> hull; cv::convexHull(points,hull);
|
||||
cv::Mat region(size_,CV_8U,cv::Scalar(0)); cv::fillConvexPoly(region,hull,255);
|
||||
if(!cv::countNonZero(region)) continue;
|
||||
withHands.insert(pos);
|
||||
hand["risk"]=measure(region,"RTMW_HAND_KEYPOINTS",trackFor(pos),hand.value("side",std::string("unknown")),hand.value("state",std::string("unknown")));
|
||||
}
|
||||
auto const boxes=record.value("boxes",Json::array());
|
||||
for(int pos=0;pos<int(boxes.size());++pos) {
|
||||
if(withHands.count(pos)) continue;
|
||||
auto const& b=boxes[pos]; if(b.size()<4) continue;
|
||||
int x1=std::clamp(int(std::nearbyint(b[0].get<double>())),0,size_.width-1),y1=std::clamp(int(std::nearbyint(b[1].get<double>())),0,size_.height-1);
|
||||
int x2=std::clamp(int(std::nearbyint(b[2].get<double>())),0,size_.width-1),y2=std::clamp(int(std::nearbyint(b[3].get<double>())),0,size_.height-1);
|
||||
if(x1>x2) std::swap(x1,x2);
|
||||
if(y1>y2) std::swap(y1,y2);
|
||||
if(x2<=x1||y2<=y1) continue;
|
||||
cv::Mat region(size_,CV_8U,cv::Scalar(0)); cv::rectangle(region,{x1,y1},{x2,y2},255,-1);
|
||||
Json fallback={{"track_id",trackFor(pos)},{"side","body"},{"state","rtmw_hand_unavailable"},{"body_zone_box",b}};
|
||||
fallback["risk"]=measure(region,"YOLO_BODY_BOX_FALLBACK",trackFor(pos),"body","rtmw_hand_unavailable"); fallbacks.push_back(fallback);
|
||||
}
|
||||
record["body_fallbacks"]=fallbacks;
|
||||
Json result={{"status",status_!="loaded"?"door_mask_"+status_:nearest.is_null()?"person_region_missing":"calculated"}};
|
||||
for(auto const* key:{"risk_score","risk_level","decision","source","track_id","side"}) result[key]=nearest.is_null()?Json(nullptr):nearest[key];
|
||||
if(nearest.is_null()) {result["risk_level"]="unavailable";result["decision"]="EVIDENCE_UNAVAILABLE";}
|
||||
result["door_mask"]={{"status",status_},{"annotation_index",options_.constant.empty()?frame+options_.offset:frame},{"path",loadedPath_.empty()?Json(nullptr):Json(std::filesystem::absolute(loadedPath_).string())},{"format",format_},{"area_px",mask_.empty()?0:cv::countNonZero(mask_)}};
|
||||
result["warning_distance_relative"]=options_.warning; result["release_distance_relative"]=options_.release; result["regions"]=regions;
|
||||
return result;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,94 @@
|
||||
#include "door_zone.hpp"
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <fstream>
|
||||
#include <map>
|
||||
|
||||
namespace cms {
|
||||
namespace {
|
||||
double median(std::deque<double> const& values) {
|
||||
std::vector<double> sorted(values.begin(),values.end());
|
||||
std::sort(sorted.begin(),sorted.end());auto n=sorted.size();
|
||||
return n%2?sorted[n/2]:(sorted[n/2-1]+sorted[n/2])*0.5;
|
||||
}
|
||||
std::string upper(std::string value) { for(auto& c:value)c=char(std::toupper(static_cast<unsigned char>(c)));return value; }
|
||||
Json features(Json const& record,std::size_t position,cv::Size size,int& poseCount) {
|
||||
auto const& b=record.at("boxes").at(position);
|
||||
double w=std::max(size.width,1),h=std::max(size.height,1);
|
||||
Json result={{"box_center_x",(b[0].get<double>()+b[2].get<double>())*.5/w},{"box_height",(b[3].get<double>()-b[1].get<double>())/h}};
|
||||
poseCount=0;
|
||||
if(!record.contains("temporal_selected_keypoints")||position>=record["temporal_selected_keypoints"].size())return result;
|
||||
auto const& points=record["temporal_selected_keypoints"][position];
|
||||
for(int k:{5,6,11,12})if(k>=int(points.size())||points[k].size()<2||!std::isfinite(points[k][0].get<double>())||!std::isfinite(points[k][1].get<double>()))return result;
|
||||
double sx=(points[5][0].get<double>()+points[6][0].get<double>())*.5;
|
||||
double sy=(points[5][1].get<double>()+points[6][1].get<double>())*.5;
|
||||
double hy=(points[11][1].get<double>()+points[12][1].get<double>())*.5;
|
||||
result["shoulder_center_x"]=sx/w;result["shoulder_center_y"]=sy/h;
|
||||
result["shoulder_width"]=std::hypot(points[5][0].get<double>()-points[6][0].get<double>(),points[5][1].get<double>()-points[6][1].get<double>())/w;
|
||||
result["torso_height"]=std::abs(hy-sy)/h;result["hip_center_y"]=hy/h;poseCount=5;return result;
|
||||
}
|
||||
Json baseRisk(std::string const& door,std::string const& level,Json score,std::string const& status,std::string const& decision,Json regions=Json::array()) {
|
||||
return {{"door_id",door},{"status",status},{"risk_score",score},{"risk_level",level},{"decision",decision},{"risk_type",upper(door)+"_"+upper(level)},{"source",nullptr},{"track_id",nullptr},{"side",nullptr},{"regions",regions}};
|
||||
}
|
||||
}
|
||||
DoorZoneAssigner::DoorZoneAssigner(std::filesystem::path const& configuration) {
|
||||
std::ifstream file(configuration);if(!file)throw std::runtime_error("Cannot read door-zone configuration: "+configuration.string());
|
||||
auto j=Json::parse(file);features_=j.at("features");
|
||||
frontMin_=j.value("front_probability_min",.72);rearMax_=j.value("rear_probability_max",.28);
|
||||
minimumHeight_=j.value("minimum_person_height",.4);minimumPoseFeatures_=j.value("minimum_pose_features",4);
|
||||
historyFrames_=j.value("history_frames",5);staleFrames_=j.value("stale_track_frames",120);
|
||||
if(historyFrames_<1||staleFrames_<0||rearMax_<0||frontMin_>1||rearMax_>=frontMin_)throw std::invalid_argument("Invalid door-zone configuration");
|
||||
}
|
||||
std::string DoorZoneAssigner::classify(double p) const {return p>=frontMin_?"front":p<=rearMax_?"rear":"transition";}
|
||||
Json DoorZoneAssigner::update(int frame,Json const& record,cv::Size size) {
|
||||
Json assignments=Json::array();auto const& boxes=record.at("boxes");auto ids=record.value("track_ids",Json::array());
|
||||
for(std::size_t i=0;i<boxes.size();++i) {
|
||||
int id=i<ids.size()?ids[i].get<int>():int(i)+1,count=0;
|
||||
auto f=features(record,i,size,count);double weighted=0,total=0;Json contributions=Json::object();
|
||||
for(auto const& entry:features_.items())if(f.contains(entry.key())) {
|
||||
auto const& c=entry.value();double rear=c.at("rear"),front=c.at("front"),weight=c.at("weight");
|
||||
double score=front==rear?.5:std::clamp((f[entry.key()].get<double>()-rear)/(front-rear),0.0,1.0);
|
||||
contributions[entry.key()]=rounded(score,6);weighted+=score*weight;total+=weight;
|
||||
}
|
||||
double probability=total?weighted/total:.5;std::string rawZone,zone;
|
||||
if(f["box_height"].get<double>()<minimumHeight_)rawZone=zone="unassigned";
|
||||
else if(count<minimumPoseFeatures_)rawZone=zone="transition";
|
||||
else {rawZone=classify(probability);auto& values=history_[id];values.push_back(probability);while(int(values.size())>historyFrames_)values.pop_front();zone=classify(median(values));}
|
||||
auto it=history_.find(id);double smoothed=it==history_.end()||it->second.empty()?probability:median(it->second);
|
||||
lastSeen_[id]=frame;for(auto& value:f.items())value.value()=rounded(value.value().get<double>(),6);
|
||||
assignments.push_back({{"track_id",id},{"person_index",i},{"door_zone",zone},{"raw_door_zone",rawZone},{"front_probability",rounded(probability,6)},{"smoothed_front_probability",rounded(smoothed,6)},{"confidence",rounded(std::abs(smoothed-.5)*2,6)},{"evidence_source","pose_primary_bbox_secondary"},{"pose_feature_count",count},{"features",f},{"feature_frontness",contributions},{"history_frames",it==history_.end()?0:it->second.size()}});
|
||||
}
|
||||
for(auto it=lastSeen_.begin();it!=lastSeen_.end();)if(frame-it->second>staleFrames_){history_.erase(it->first);it=lastSeen_.erase(it);}else ++it;
|
||||
return assignments;
|
||||
}
|
||||
DoorDecisions buildDoorRisks(Json const& raw,Json const& assignments,std::string const& active,bool canValid) {
|
||||
std::map<int,std::string> zones;for(auto const& a:assignments)zones[a.at("track_id").get<int>()]=a.at("door_zone");
|
||||
Json regions=Json::array();for(auto region:raw.value("regions",Json::array())){int id=region.value("track_id",-1);region["door_zone"]=zones.count(id)?zones[id]:"unknown";regions.push_back(std::move(region));}
|
||||
DoorDecisions result{Json::object(),Json::object()};
|
||||
for(std::string door:{"front","rear"}) {
|
||||
if(active!="both"&&active!=door){result.doors[door]=baseRisk(door,"inactive",nullptr,"inactive","DOOR_CAN_NOT_ACTIVE");continue;}
|
||||
Json exact=Json::array(),transition=Json::array(),nearest=nullptr;
|
||||
for(auto const& r:regions){if(r["door_zone"]==door){exact.push_back(r);if(!r.at("risk_score").is_null()&&(nearest.is_null()||r["risk_score"].get<double>()>nearest["risk_score"].get<double>()))nearest=r;}if(r["door_zone"]=="transition")transition.push_back(r);}
|
||||
if(!nearest.is_null()&&(transition.empty()||(nearest["risk_level"]!="clear"&&nearest["risk_level"]!="watch"))) {
|
||||
auto level=nearest["risk_level"].get<std::string>();auto r=baseRisk(door,level,nearest["risk_score"],"calculated",nearest["decision"],exact);
|
||||
for(auto key:{"source","track_id","side"}){r[key]=nearest[key];}
|
||||
result.doors[door]=std::move(r);
|
||||
} else if(!transition.empty())result.doors[door]=baseRisk(door,"unknown",nullptr,"door_zone_ambiguous","EVIDENCE_UNAVAILABLE_TRANSITION_ZONE",transition);
|
||||
else result.doors[door]=baseRisk(door,"clear",0.0,"calculated","CLEAR_NO_PERSON_ASSIGNED_TO_ACTIVE_DOOR");
|
||||
}
|
||||
if(active=="front"||active=="rear")result.selected=result.doors[active];
|
||||
else if(active=="none") {
|
||||
result.selected=baseRisk("none","inactive",nullptr,"inactive","NO_DOOR_CAN_ACTIVE");result.selected["door_id"]=nullptr;result.selected["risk_type"]="NO_ACTIVE_DOOR";
|
||||
} else {
|
||||
std::map<std::string,int> severity{{"danger",4},{"warning",3},{"watch",2},{"unknown",1},{"clear",0}};
|
||||
result.selected=result.doors["front"];
|
||||
if(severity[result.doors["rear"]["risk_level"]]>severity[result.selected["risk_level"]])result.selected=result.doors["rear"];
|
||||
}
|
||||
for(auto key:{"door_mask","warning_distance_relative","release_distance_relative"})result.selected[key]=raw.at(key);
|
||||
if(!canValid) {
|
||||
for(auto& entry:result.doors.items()){auto& r=entry.value();r["risk_type"]=upper(entry.key())+"_UNKNOWN";r["risk_level"]="unknown";r["risk_score"]=nullptr;r["decision"]="CAN_UNAVAILABLE_OR_STALE";}
|
||||
result.selected["risk_type"]="CAN_UNKNOWN";result.selected["risk_level"]="unknown";result.selected["risk_score"]=nullptr;result.selected["decision"]="CAN_UNAVAILABLE_OR_STALE";
|
||||
}
|
||||
return result;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,69 @@
|
||||
#include "camera_pipeline.hpp"
|
||||
#include "config.hpp"
|
||||
#include "stop_signals.hpp"
|
||||
#include "visualizer.hpp"
|
||||
#include <chrono>
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <cstdlib>
|
||||
#include <iostream>
|
||||
#include <thread>
|
||||
|
||||
int main(int argc,char** argv)try {
|
||||
auto root=std::filesystem::canonical("/proc/self/exe").parent_path().parent_path();
|
||||
auto configPath=root/"config/camera.json";
|
||||
for(int i=1;i<argc;++i)if(std::string(argv[i])=="--config") {
|
||||
if(++i>=argc)throw std::invalid_argument("--config needs a path");configPath=argv[i];
|
||||
}
|
||||
for(int i=1;i<argc;++i)if(std::string(argv[i])=="--help"||std::string(argv[i])=="-h") {
|
||||
std::cout<<"camera_risk [--config FILE] [--visualize] [--active-door can|front|rear|both|none]\n"
|
||||
<<" [--can-side left|right] [--no-can] [--duration SECONDS] [--max-frames N]\n"
|
||||
<<" [--video FILE] [--save-results FILE.jsonl] [--save-pose-records FILE.jsonl]\n"
|
||||
<<"Pure C++ inference; the latest immutable FrameResult is kept in memory.\n";return 0;
|
||||
}
|
||||
auto config=cms::loadConfig(configPath);bool visualize=false;double duration=0;
|
||||
auto value=[&](int& i){if(++i>=argc)throw std::invalid_argument("Missing option value");return std::string(argv[i]);};
|
||||
for(int i=1;i<argc;++i) {
|
||||
std::string arg=argv[i];
|
||||
if(arg=="--config")value(i);
|
||||
else if(arg=="--visualize")visualize=true;
|
||||
else if(arg=="--no-can")config.can_enabled=false;
|
||||
else if(arg=="--active-door")config.active_door=value(i);
|
||||
else if(arg=="--can-side")config.can_side=value(i);
|
||||
else if(arg=="--video")config.camera.video=value(i);
|
||||
else if(arg=="--save-results")config.results_path=value(i);
|
||||
else if(arg=="--save-pose-records")config.pose_records_path=value(i);
|
||||
else if(arg=="--max-frames"){auto n=std::stoll(value(i));if(n<0)throw std::invalid_argument("Negative frame count");config.max_frames=n;}
|
||||
else if(arg=="--duration"){duration=std::stod(value(i));if(!std::isfinite(duration)||duration<0)throw std::invalid_argument("Invalid duration");}
|
||||
else throw std::invalid_argument("Unknown option: "+arg);
|
||||
}
|
||||
cv::setNumThreads(2);
|
||||
cms::CameraPipeline pipeline(config);cms::StopSignals signals(pipeline);pipeline.start();
|
||||
auto started=std::chrono::steady_clock::now();std::atomic<bool> monitorStop{false};
|
||||
std::thread monitor([&] {
|
||||
auto report=started;
|
||||
while(!monitorStop&&pipeline.running()) {
|
||||
auto now=std::chrono::steady_clock::now();double elapsed=std::chrono::duration<double>(now-started).count();
|
||||
if(duration>0&&elapsed>=duration){pipeline.requestStop();break;}
|
||||
if(std::chrono::duration<double>(now-report).count()>=5) {
|
||||
auto s=pipeline.stats();auto r=pipeline.latest();
|
||||
std::cout<<"frames="<<s.published<<" fps="<<s.published/elapsed<<" risk="<<(r?r->risk.risk_type:"STARTING")<<std::endl;report=now;
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(50));
|
||||
}
|
||||
});
|
||||
try {
|
||||
if(visualize) {
|
||||
if(!std::getenv("DISPLAY")&&!std::getenv("WAYLAND_DISPLAY"))std::cerr<<"No display session; inference continues without visualization.\n";
|
||||
else try{cms::runLocalVisualization(pipeline);}catch(std::exception const& e){std::cerr<<"Visualization unavailable; inference continues: "<<e.what()<<std::endl;}
|
||||
}
|
||||
// Downstream C++ code reads this same variable from its own thread:
|
||||
// auto result = pipeline.latest(); // shared_ptr<const cms::FrameResult>
|
||||
// if(result) { use(result->risk, result->door_risks, result->persons); }
|
||||
pipeline.join();
|
||||
}catch(...){monitorStop=true;pipeline.requestStop();monitor.join();throw;}
|
||||
monitorStop=true;monitor.join();auto stats=pipeline.stats();
|
||||
auto seconds=std::chrono::duration<double>(std::chrono::steady_clock::now()-started).count();
|
||||
std::cout<<"frames="<<stats.published<<" elapsed_seconds="<<seconds<<" fps="<<stats.published/std::max(seconds,.001)<<" capture_to_result_ms="<<stats.mean_capture_to_result_ms<<std::endl;
|
||||
return 0;
|
||||
}catch(std::exception const& e){std::cerr<<"Camera pipeline failed: "<<e.what()<<std::endl;return 1;}
|
||||
@@ -0,0 +1,875 @@
|
||||
#include "pose_engine.hpp"
|
||||
#include "common.hpp"
|
||||
#include "gpu_preprocess.hpp"
|
||||
|
||||
#include <opencv2/imgproc.hpp>
|
||||
|
||||
#include <algorithm>
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <numeric>
|
||||
#include <set>
|
||||
#include <tuple>
|
||||
|
||||
namespace fangjia {
|
||||
namespace {
|
||||
|
||||
constexpr int kInputWidth = 288;
|
||||
constexpr int kInputHeight = 384;
|
||||
constexpr int kKeypoints = 133;
|
||||
constexpr int kSimccX = 576;
|
||||
constexpr int kSimccY = 768;
|
||||
|
||||
__global__ void poseNormalizeKernel(
|
||||
float* output, std::uint8_t const* input, int people, int inputStep) {
|
||||
int index = blockIdx.x * blockDim.x + threadIdx.x;
|
||||
int pixels = kInputWidth * kInputHeight;
|
||||
int total = people * pixels;
|
||||
if (index >= total) return;
|
||||
int person = index / pixels;
|
||||
int pixel = index - person * pixels;
|
||||
int y = pixel / kInputWidth;
|
||||
int x = pixel - y * kInputWidth;
|
||||
auto const* value = input + person * inputStep * kInputHeight + y * inputStep + x * 3;
|
||||
float* destination = output + static_cast<std::size_t>(person) * 3 * pixels;
|
||||
destination[pixel] = static_cast<float>((static_cast<double>(value[0]) - 123.675) / 58.395);
|
||||
destination[pixels + pixel] = static_cast<float>((static_cast<double>(value[1]) - 116.28) / 57.12);
|
||||
destination[2 * pixels + pixel] = static_cast<float>((static_cast<double>(value[2]) - 103.53) / 57.375);
|
||||
}
|
||||
|
||||
float iou(Box const& a, Box const& b) {
|
||||
float x1 = std::max(a.x1, b.x1);
|
||||
float y1 = std::max(a.y1, b.y1);
|
||||
float x2 = std::min(a.x2, b.x2);
|
||||
float y2 = std::min(a.y2, b.y2);
|
||||
float intersection = std::max(x2 - x1, 0.0F) * std::max(y2 - y1, 0.0F);
|
||||
float areaA = std::max(a.x2 - a.x1, 0.0F) * std::max(a.y2 - a.y1, 0.0F);
|
||||
float areaB = std::max(b.x2 - b.x1, 0.0F) * std::max(b.y2 - b.y1, 0.0F);
|
||||
return intersection / std::max(areaA + areaB - intersection, 1.0F);
|
||||
}
|
||||
|
||||
float distance(float x1, float y1, float x2, float y2) {
|
||||
return std::hypot(x1 - x2, y1 - y2);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
std::vector<int> BoxTracker::update(
|
||||
std::vector<Box>& boxes, int frameIndex, int width, int height) {
|
||||
// Tracks older than Python's max_age can never be matched again.
|
||||
// Reclaim them to bound memory during continuous camera operation.
|
||||
for(auto it=tracks_.begin();it!=tracks_.end();) {
|
||||
if(frameIndex-it->second.lastFrame>30)it=tracks_.erase(it);
|
||||
else ++it;
|
||||
}
|
||||
struct Pair { float score; int detection; int track; };
|
||||
std::vector<Pair> pairs;
|
||||
float imageDiagonal = std::hypot(static_cast<float>(width), static_cast<float>(height));
|
||||
for (int d = 0; d < static_cast<int>(boxes.size()); ++d) {
|
||||
float cx = 0.5F * (boxes[d].x1 + boxes[d].x2);
|
||||
float cy = 0.5F * (boxes[d].y1 + boxes[d].y2);
|
||||
for (auto const& [id, track] : tracks_) {
|
||||
if (frameIndex - track.lastFrame > 30) continue;
|
||||
float pcx = 0.5F * (track.box.x1 + track.box.x2);
|
||||
float pcy = 0.5F * (track.box.y1 + track.box.y2);
|
||||
float overlap = iou(boxes[d], track.box);
|
||||
float centerDistance = distance(cx, cy, pcx, pcy) / imageDiagonal;
|
||||
if (overlap >= 0.05F || centerDistance <= 0.18F) {
|
||||
pairs.push_back({2.0F * overlap + std::max(0.0F, 1.0F - centerDistance / 0.18F), d, id});
|
||||
}
|
||||
}
|
||||
}
|
||||
std::sort(pairs.begin(), pairs.end(), [](Pair const& a, Pair const& b) {
|
||||
if (a.score != b.score) return a.score > b.score;
|
||||
if (a.detection != b.detection) return a.detection > b.detection;
|
||||
return a.track > b.track;
|
||||
});
|
||||
std::vector<int> result(boxes.size(), 0);
|
||||
std::unordered_map<int, bool> assigned;
|
||||
for (auto const& pair : pairs) {
|
||||
if (result[pair.detection] || assigned[pair.track]) continue;
|
||||
result[pair.detection] = pair.track;
|
||||
assigned[pair.track] = true;
|
||||
}
|
||||
for (int d = 0; d < static_cast<int>(boxes.size()); ++d) {
|
||||
if (!result[d]) result[d] = nextId_++;
|
||||
boxes[d].trackId = result[d];
|
||||
tracks_[result[d]] = {boxes[d], frameIndex};
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
RtmwEngine::RtmwEngine(
|
||||
std::filesystem::path const& enginePath, TrtLogger& logger,
|
||||
int maxSourceWidth, int maxSourceHeight, int maxPeople)
|
||||
: engine_(enginePath, logger), maxSourceWidth_(maxSourceWidth),
|
||||
maxSourceHeight_(maxSourceHeight), maxPeople_(maxPeople),
|
||||
centersHost_(maxPeople * 2), scalesHost_(maxPeople * 2),
|
||||
simccXHost_(static_cast<std::size_t>(maxPeople) * kKeypoints * kSimccX),
|
||||
simccYHost_(static_cast<std::size_t>(maxPeople) * kKeypoints * kSimccY) {
|
||||
engine_.setInputShape("input", nvinfer1::Dims4{1,3,kInputHeight,kInputWidth});
|
||||
engine_.prepare();
|
||||
auto xs=engine_.shape("simcc_x"),ys=engine_.shape("simcc_y");
|
||||
if(xs.nbDims!=3||ys.nbDims!=3||xs.d[0]!=1||ys.d[0]!=1||xs.d[1]!=kKeypoints||ys.d[1]!=kKeypoints||xs.d[2]!=kSimccX||ys.d[2]!=kSimccY)
|
||||
throw std::runtime_error("RTMW engine I/O shape does not match the 133-point model");
|
||||
for(auto name:{"input","simcc_x","simcc_y"})if(engine_.dataType(name)!=nvinfer1::DataType::kFLOAT)
|
||||
throw std::runtime_error("RTMW requires float32 I/O bindings");
|
||||
std::size_t const values = static_cast<std::size_t>(maxPeople) * 3 * kInputWidth * kInputHeight;
|
||||
CUDA_CHECK(cudaMalloc(
|
||||
&cropsDevice_, static_cast<std::size_t>(maxPeople_) * kInputWidth * kInputHeight * 3));
|
||||
CUDA_CHECK(cudaMalloc(&networkDevice_, values * sizeof(float)));
|
||||
CUDA_CHECK(cudaMalloc(&sourceDevice_, static_cast<size_t>(maxSourceWidth_)*maxSourceHeight_*3));
|
||||
}
|
||||
|
||||
RtmwEngine::~RtmwEngine() {
|
||||
if (sourceDevice_) cudaFree(sourceDevice_);
|
||||
if (cropsDevice_) cudaFree(cropsDevice_);
|
||||
if (networkDevice_) cudaFree(networkDevice_);
|
||||
}
|
||||
|
||||
void RtmwEngine::warmup(cudaStream_t stream) {
|
||||
std::vector<Box> boxes{{0, 0, static_cast<float>(maxSourceWidth_), static_cast<float>(maxSourceHeight_), 1, 1}};
|
||||
cv::Mat black(maxSourceHeight_, maxSourceWidth_, CV_8UC3, cv::Scalar(0, 0, 0));
|
||||
for (int i = 0; i < 2; ++i) infer(black, boxes, stream);
|
||||
}
|
||||
|
||||
std::vector<PersonPose> RtmwEngine::infer(
|
||||
cv::Mat const& frame, std::vector<Box> const& boxes, cudaStream_t stream,
|
||||
float* preprocessMs, float* inferenceMs, float* postprocessMs) {
|
||||
int batch = std::min(static_cast<int>(boxes.size()), maxPeople_);
|
||||
if (batch <= 0) return {};
|
||||
auto const preprocessStarted = std::chrono::steady_clock::now();
|
||||
int const cropStep = kInputWidth * 3;
|
||||
int const pixels = kInputWidth * kInputHeight;
|
||||
if (frame.cols>maxSourceWidth_ || frame.rows>maxSourceHeight_) throw std::runtime_error("Pose input exceeds allocation");
|
||||
auto npp = cms::nppContext(stream);
|
||||
CUDA_CHECK(cudaMemcpy2DAsync(sourceDevice_, frame.cols*3, frame.data, frame.step, frame.cols*3, frame.rows, cudaMemcpyHostToDevice, stream));
|
||||
for (int i = 0; i < batch; ++i) {
|
||||
auto const& box = boxes[i];
|
||||
double width = (static_cast<double>(box.x2) - static_cast<double>(box.x1)) * 1.25;
|
||||
double height = (static_cast<double>(box.y2) - static_cast<double>(box.y1)) * 1.25;
|
||||
constexpr double aspect = static_cast<double>(kInputWidth) / kInputHeight;
|
||||
if (width > height * aspect) height = width / aspect;
|
||||
else width = height * aspect;
|
||||
centersHost_[i * 2] = 0.5 * (static_cast<double>(box.x1) + static_cast<double>(box.x2));
|
||||
centersHost_[i * 2 + 1] = 0.5 * (static_cast<double>(box.y1) + static_cast<double>(box.y2));
|
||||
scalesHost_[i * 2] = width;
|
||||
scalesHost_[i * 2 + 1] = height;
|
||||
|
||||
cv::Point2f source[3]{
|
||||
{static_cast<float>(centersHost_[i * 2]), static_cast<float>(centersHost_[i * 2 + 1])},
|
||||
{static_cast<float>(centersHost_[i * 2]), static_cast<float>(centersHost_[i * 2 + 1] - width * 0.5)},
|
||||
{static_cast<float>(centersHost_[i * 2] - width * 0.5), static_cast<float>(centersHost_[i * 2 + 1] - width * 0.5)}};
|
||||
cv::Point2f destination[3]{
|
||||
{kInputWidth * 0.5F, kInputHeight * 0.5F},
|
||||
{kInputWidth * 0.5F, kInputHeight * 0.5F - kInputWidth * 0.5F},
|
||||
{0.0F, kInputHeight * 0.5F - kInputWidth * 0.5F}};
|
||||
cv::Mat affine = cv::getAffineTransform(source, destination);
|
||||
auto* cropDevice=cropsDevice_+static_cast<size_t>(i)*cropStep*kInputHeight;
|
||||
Npp8u fill[3]={0,0,0};
|
||||
cms::nppCheck(nppiSet_8u_C3R_Ctx(fill,cropDevice,cropStep,{kInputWidth,kInputHeight},npp));
|
||||
double coeff[2][3];
|
||||
for(int row=0;row<2;++row) for(int col=0;col<3;++col) coeff[row][col]=affine.at<double>(row,col);
|
||||
cms::nppCheck(nppiWarpAffine_8u_C3R_Ctx(sourceDevice_,{frame.cols,frame.rows},frame.cols*3,{0,0,frame.cols,frame.rows},cropDevice,cropStep,{0,0,kInputWidth,kInputHeight},coeff,NPPI_INTER_LINEAR,npp));
|
||||
}
|
||||
int const totalPixels = batch * pixels;
|
||||
poseNormalizeKernel<<<(totalPixels + 255) / 256, 256, 0, stream>>>(
|
||||
networkDevice_, cropsDevice_, batch, cropStep);
|
||||
CUDA_CHECK(cudaGetLastError());
|
||||
CUDA_CHECK(cudaStreamSynchronize(stream));
|
||||
if (preprocessMs) {
|
||||
*preprocessMs = static_cast<float>(std::chrono::duration<double, std::milli>(
|
||||
std::chrono::steady_clock::now() - preprocessStarted).count());
|
||||
}
|
||||
|
||||
// RTMPose's Python API invokes the ONNX/TensorRT session once per person.
|
||||
// Preserve that execution contract instead of batching people together.
|
||||
engine_.setInputShape("input", nvinfer1::Dims4{1, 3, kInputHeight, kInputWidth});
|
||||
engine_.prepare();
|
||||
CudaEventTimer timer;
|
||||
float inferTime = 0.0F;
|
||||
std::size_t const inputValues = static_cast<std::size_t>(3) * pixels;
|
||||
std::size_t const xValues = static_cast<std::size_t>(kKeypoints) * kSimccX;
|
||||
std::size_t const yValues = static_cast<std::size_t>(kKeypoints) * kSimccY;
|
||||
for (int person = 0; person < batch; ++person) {
|
||||
CUDA_CHECK(cudaMemcpyAsync(
|
||||
engine_.device("input"), networkDevice_ + static_cast<std::size_t>(person) * inputValues,
|
||||
inputValues * sizeof(float), cudaMemcpyDeviceToDevice, stream));
|
||||
timer.start(stream);
|
||||
if (!engine_.enqueue(stream)) throw std::runtime_error("RTMW enqueue failed");
|
||||
inferTime += timer.stop(stream);
|
||||
CUDA_CHECK(cudaMemcpyAsync(
|
||||
simccXHost_.data() + static_cast<std::size_t>(person) * xValues,
|
||||
engine_.device("simcc_x"), xValues * sizeof(float), cudaMemcpyDeviceToHost, stream));
|
||||
CUDA_CHECK(cudaMemcpyAsync(
|
||||
simccYHost_.data() + static_cast<std::size_t>(person) * yValues,
|
||||
engine_.device("simcc_y"), yValues * sizeof(float), cudaMemcpyDeviceToHost, stream));
|
||||
}
|
||||
if (inferenceMs) *inferenceMs = inferTime;
|
||||
|
||||
auto const postprocessStarted = std::chrono::steady_clock::now();
|
||||
CUDA_CHECK(cudaStreamSynchronize(stream));
|
||||
std::vector<PersonPose> result(batch);
|
||||
for (int person = 0; person < batch; ++person) {
|
||||
result[person].box = boxes[person];
|
||||
for (int keypoint = 0; keypoint < kKeypoints; ++keypoint) {
|
||||
auto xBegin = simccXHost_.begin() + (person * kKeypoints + keypoint) * kSimccX;
|
||||
auto yBegin = simccYHost_.begin() + (person * kKeypoints + keypoint) * kSimccY;
|
||||
auto xIt = std::max_element(xBegin, xBegin + kSimccX);
|
||||
auto yIt = std::max_element(yBegin, yBegin + kSimccY);
|
||||
float score = 0.5F * (*xIt + *yIt);
|
||||
if(!std::isfinite(score))throw std::runtime_error("Non-finite RTMW output");
|
||||
float localX = score > 0.0F ? static_cast<float>(xIt - xBegin) * 0.5F : -0.5F;
|
||||
float localY = score > 0.0F ? static_cast<float>(yIt - yBegin) * 0.5F : -0.5F;
|
||||
double scaleX = scalesHost_[person * 2];
|
||||
double scaleY = scalesHost_[person * 2 + 1];
|
||||
result[person].keypoints[keypoint] = {
|
||||
static_cast<float>(static_cast<double>(localX) / kInputWidth * scaleX + centersHost_[person * 2] - scaleX * 0.5),
|
||||
static_cast<float>(static_cast<double>(localY) / kInputHeight * scaleY + centersHost_[person * 2 + 1] - scaleY * 0.5),
|
||||
score};
|
||||
}
|
||||
}
|
||||
float post = static_cast<float>(std::chrono::duration<double, std::milli>(
|
||||
std::chrono::steady_clock::now() - postprocessStarted).count());
|
||||
if (postprocessMs) *postprocessMs = post;
|
||||
return result;
|
||||
}
|
||||
|
||||
PoseFrameResult PoseFusionTracker::update(
|
||||
std::vector<PersonPose> const& rawPeople,
|
||||
std::vector<PersonPose> const& isolatedPeople,
|
||||
int frameIndex) {
|
||||
for(auto it=states_.begin();it!=states_.end();) {
|
||||
if(it->second.lastFrame>=0&&frameIndex-it->second.lastFrame>30)it=states_.erase(it);
|
||||
else ++it;
|
||||
}
|
||||
struct SideMeta {
|
||||
bool limbConsensus{};
|
||||
bool handValid{};
|
||||
bool distanceEligible{};
|
||||
bool alarmEligible{};
|
||||
double handScore{};
|
||||
double linkToSelectedLimb{};
|
||||
std::string handSource;
|
||||
std::string handState;
|
||||
};
|
||||
struct FusedPerson {
|
||||
PersonPose pose;
|
||||
std::array<SideMeta, 2> meta;
|
||||
};
|
||||
struct Measurement {
|
||||
std::array<Vec2, 21> hand{};
|
||||
bool precise{};
|
||||
bool provisional{};
|
||||
double quality{};
|
||||
std::string source{"model"};
|
||||
};
|
||||
auto vec = [](Keypoint const& point) { return Vec2{point.x, point.y}; };
|
||||
auto add = [](Vec2 a, Vec2 b) { return Vec2{a.x + b.x, a.y + b.y}; };
|
||||
auto sub = [](Vec2 a, Vec2 b) { return Vec2{a.x - b.x, a.y - b.y}; };
|
||||
auto mul = [](Vec2 a, double value) { return Vec2{a.x * value, a.y * value}; };
|
||||
auto norm = [](Vec2 a) { return std::hypot(a.x, a.y); };
|
||||
auto median = [](std::vector<double> values) {
|
||||
if (values.empty()) return 0.0;
|
||||
std::sort(values.begin(), values.end());
|
||||
auto middle = values.size() / 2;
|
||||
return values.size() % 2 ? values[middle] : 0.5 * (values[middle - 1] + values[middle]);
|
||||
};
|
||||
auto percentile90 = [](std::vector<double> values) {
|
||||
if (values.empty()) return 0.0;
|
||||
std::sort(values.begin(), values.end());
|
||||
double position = 0.9 * (values.size() - 1);
|
||||
auto lower = static_cast<std::size_t>(position);
|
||||
auto upper = std::min(lower + 1, values.size() - 1);
|
||||
return values[lower] * (upper - position) + values[upper] * (position - lower);
|
||||
};
|
||||
auto boxDiagonal = [](Box const& box) {
|
||||
return std::max(std::hypot(box.x2 - box.x1, box.y2 - box.y1), 1.0F);
|
||||
};
|
||||
auto normalized = [](Keypoint const& point, Box const& box) {
|
||||
return Vec2{
|
||||
(point.x - box.x1) / std::max(box.x2 - box.x1, 1.0F),
|
||||
(point.y - box.y1) / std::max(box.y2 - box.y1, 1.0F)};
|
||||
};
|
||||
auto denormalized = [](Vec2 point, Box const& box) {
|
||||
return Vec2{
|
||||
point.x * std::max(box.x2 - box.x1, 1.0F) + box.x1,
|
||||
point.y * std::max(box.y2 - box.y1, 1.0F) + box.y1};
|
||||
};
|
||||
auto solveTwoBoneElbow = [&](Vec2 shoulder, Vec2 wrist, double upperLength,
|
||||
double forearmLength, Vec2 reference) {
|
||||
Vec2 direction = sub(wrist, shoulder);
|
||||
double length = norm(direction);
|
||||
if (length < 1e-6) return reference;
|
||||
Vec2 unit = mul(direction, 1.0 / length);
|
||||
if (length >= upperLength + forearmLength) return add(shoulder, mul(unit, upperLength));
|
||||
if (length <= std::abs(upperLength - forearmLength)) {
|
||||
double sign = upperLength >= forearmLength ? 1.0 : -1.0;
|
||||
return add(shoulder, mul(unit, upperLength * sign));
|
||||
}
|
||||
double along = (upperLength * upperLength - forearmLength * forearmLength + length * length) /
|
||||
(2.0 * length);
|
||||
double height = std::sqrt(std::max(upperLength * upperLength - along * along, 0.0));
|
||||
Vec2 base = add(shoulder, mul(unit, along));
|
||||
Vec2 perpendicular{-unit.y, unit.x};
|
||||
Vec2 first = add(base, mul(perpendicular, height));
|
||||
Vec2 second = sub(base, mul(perpendicular, height));
|
||||
return norm(sub(first, reference)) <= norm(sub(second, reference)) ? first : second;
|
||||
};
|
||||
struct Candidate {
|
||||
PersonPose const* pose{};
|
||||
std::string source;
|
||||
std::array<Vec2, 3> limb{};
|
||||
std::array<Vec2, 21> hand{};
|
||||
double limbScore{};
|
||||
double handScore{};
|
||||
double handSpan{};
|
||||
double wristLink{};
|
||||
bool limbValid{};
|
||||
bool handValid{};
|
||||
};
|
||||
auto buildCandidate = [&](PersonPose const& pose, int side, std::string source) {
|
||||
int shoulder = side == 0 ? 5 : 6;
|
||||
int elbow = side == 0 ? 7 : 8;
|
||||
int wrist = side == 0 ? 9 : 10;
|
||||
int handStart = side == 0 ? 91 : 112;
|
||||
Candidate candidate;
|
||||
candidate.pose = &pose;
|
||||
candidate.source = std::move(source);
|
||||
candidate.limb = {normalized(pose.keypoints[shoulder], pose.box),
|
||||
normalized(pose.keypoints[elbow], pose.box),
|
||||
normalized(pose.keypoints[wrist], pose.box)};
|
||||
double minX = pose.keypoints[handStart].x, maxX = minX;
|
||||
double minY = pose.keypoints[handStart].y, maxY = minY;
|
||||
for (int index = 0; index < 21; ++index) {
|
||||
candidate.hand[index] = normalized(pose.keypoints[handStart + index], pose.box);
|
||||
minX = std::min<double>(minX, pose.keypoints[handStart + index].x);
|
||||
maxX = std::max<double>(maxX, pose.keypoints[handStart + index].x);
|
||||
minY = std::min<double>(minY, pose.keypoints[handStart + index].y);
|
||||
maxY = std::max<double>(maxY, pose.keypoints[handStart + index].y);
|
||||
candidate.handScore += pose.keypoints[handStart + index].score;
|
||||
}
|
||||
candidate.handScore /= 21.0;
|
||||
candidate.limbScore = (pose.keypoints[shoulder].score + pose.keypoints[elbow].score + pose.keypoints[wrist].score) / 3.0;
|
||||
double upper = norm(sub(candidate.limb[0], candidate.limb[1]));
|
||||
double forearm = norm(sub(candidate.limb[1], candidate.limb[2]));
|
||||
candidate.handSpan = std::hypot(maxX - minX, maxY - minY) / boxDiagonal(pose.box);
|
||||
candidate.wristLink = distance(
|
||||
pose.keypoints[wrist].x, pose.keypoints[wrist].y,
|
||||
pose.keypoints[handStart].x, pose.keypoints[handStart].y) / boxDiagonal(pose.box);
|
||||
candidate.limbValid = upper >= 0.025 && upper <= 0.8 && forearm >= 0.025 && forearm <= 0.8;
|
||||
candidate.handValid = candidate.handSpan >= 0.008 && candidate.handSpan <= 0.35 && candidate.wristLink <= 0.12;
|
||||
return candidate;
|
||||
};
|
||||
auto robustDistance = [&](auto const& points, auto const& target, bool available) {
|
||||
if (!available) return 0.0;
|
||||
std::vector<double> distances;
|
||||
for (std::size_t index = 0; index < points.size(); ++index) {
|
||||
distances.push_back(norm(sub(points[index], target[index])));
|
||||
}
|
||||
return median(std::move(distances));
|
||||
};
|
||||
|
||||
std::vector<FusedPerson> fused;
|
||||
fused.reserve(rawPeople.size());
|
||||
for (std::size_t personIndex = 0; personIndex < rawPeople.size(); ++personIndex) {
|
||||
auto const& raw = rawPeople[personIndex];
|
||||
auto const& isolated = personIndex < isolatedPeople.size() ? isolatedPeople[personIndex] : raw;
|
||||
auto& state = states_[raw.box.trackId];
|
||||
FusedPerson output;
|
||||
output.pose = raw;
|
||||
for (int side = 0; side < 2; ++side) {
|
||||
int shoulder = side == 0 ? 5 : 6;
|
||||
int elbow = side == 0 ? 7 : 8;
|
||||
int wrist = side == 0 ? 9 : 10;
|
||||
int handStart = side == 0 ? 91 : 112;
|
||||
auto& track = state.fusion[side];
|
||||
std::array<Candidate, 2> candidates{
|
||||
buildCandidate(raw, side, "raw"), buildCandidate(isolated, side, "isolated")};
|
||||
int limbChoice = 0;
|
||||
double bestLimbScore = -1e30;
|
||||
for (int choice = 0; choice < 2; ++choice) {
|
||||
std::array<Vec2, 3> target{};
|
||||
if (track.hasLimbPrevious) {
|
||||
for (int i = 0; i < 3; ++i) target[i] = add(track.limbPrevious[i], track.hasLimbVelocity ? mul(track.limbVelocity[i], 0.65) : Vec2{});
|
||||
}
|
||||
double temporal = robustDistance(candidates[choice].limb, target, track.hasLimbPrevious);
|
||||
double score = candidates[choice].limbScore + (candidates[choice].limbValid ? 1.2 : -5.0)
|
||||
+ (track.limbSource == candidates[choice].source ? 0.35 : 0.0) - 7.0 * temporal;
|
||||
if (score > bestLimbScore) { bestLimbScore = score; limbChoice = choice; }
|
||||
}
|
||||
int handChoice = 0;
|
||||
double bestHandScore = -1e30;
|
||||
double chosenTemporal = 0.0;
|
||||
double chosenLink = 1.0;
|
||||
for (int choice = 0; choice < 2; ++choice) {
|
||||
std::array<Vec2, 21> target{};
|
||||
if (track.hasHandPrevious) {
|
||||
for (int i = 0; i < 21; ++i) target[i] = add(track.handPrevious[i], track.hasHandVelocity ? mul(track.handVelocity[i], 0.65) : Vec2{});
|
||||
}
|
||||
double temporal = robustDistance(candidates[choice].hand, target, track.hasHandPrevious);
|
||||
auto const& limbPose = *candidates[limbChoice].pose;
|
||||
double link = distance(
|
||||
candidates[choice].pose->keypoints[handStart].x,
|
||||
candidates[choice].pose->keypoints[handStart].y,
|
||||
limbPose.keypoints[wrist].x, limbPose.keypoints[wrist].y) / boxDiagonal(raw.box);
|
||||
double linkPenalty = 1.5 * std::min(std::max(link - 0.12, 0.0), 0.2);
|
||||
double score = candidates[choice].handScore + (candidates[choice].handValid ? 3.0 : -7.0)
|
||||
+ (track.handSource == candidates[choice].source ? 0.45 : 0.0)
|
||||
+ (candidates[choice].source == candidates[limbChoice].source ? 0.15 : 0.0)
|
||||
- linkPenalty - 8.0 * std::min(temporal, 0.15);
|
||||
if (score > bestHandScore) {
|
||||
bestHandScore = score;
|
||||
handChoice = choice;
|
||||
chosenTemporal = temporal;
|
||||
chosenLink = link;
|
||||
}
|
||||
}
|
||||
double endpointAdvantage = candidates[handChoice].handScore - candidates[1 - handChoice].handScore;
|
||||
if (handChoice != limbChoice && candidates[handChoice].handValid && endpointAdvantage >= 0.6) {
|
||||
limbChoice = handChoice;
|
||||
// fuse_person() rewrites this link after its visible-endpoint
|
||||
// override. Temporal recovery consumes the rewritten value.
|
||||
chosenLink = candidates[handChoice].wristLink;
|
||||
}
|
||||
auto const& limb = candidates[limbChoice];
|
||||
auto const& hand = candidates[handChoice];
|
||||
for (int index : {shoulder, elbow, wrist}) output.pose.keypoints[index] = limb.pose->keypoints[index];
|
||||
for (int index = 0; index < 21; ++index) output.pose.keypoints[handStart + index] = hand.pose->keypoints[handStart + index];
|
||||
std::vector<double> limbDistances, handDistances;
|
||||
for (int index = 0; index < 3; ++index) limbDistances.push_back(norm(sub(candidates[0].limb[index], candidates[1].limb[index])));
|
||||
for (int index = 0; index < 21; ++index) handDistances.push_back(norm(sub(candidates[0].hand[index], candidates[1].hand[index])));
|
||||
double handDisagreement = percentile90(handDistances);
|
||||
double scoreAdvantage = candidates[0].handScore - candidates[1].handScore;
|
||||
auto& meta = output.meta[side];
|
||||
meta.limbConsensus = median(limbDistances) <= 0.05;
|
||||
meta.handValid = hand.handValid;
|
||||
meta.distanceEligible = hand.handValid && (
|
||||
handDisagreement <= 0.06 ||
|
||||
(hand.source == "raw" && scoreAdvantage >= 0.6 && chosenTemporal <= 0.08));
|
||||
meta.alarmEligible = meta.distanceEligible || (hand.handValid && hand.source == "raw" && scoreAdvantage >= 0.6);
|
||||
meta.handScore = std::nearbyint(hand.handScore * 1e4) / 1e4;
|
||||
meta.linkToSelectedLimb = std::nearbyint(chosenLink * 1e5) / 1e5;
|
||||
meta.handSource = hand.source;
|
||||
if (!hand.handValid) meta.handState = "rejected";
|
||||
else if (handDisagreement <= 0.06) meta.handState = "consensus";
|
||||
else if (hand.source == "raw" && scoreAdvantage >= 0.6 && chosenTemporal <= 0.08) meta.handState = "visible_endpoint";
|
||||
else if (hand.source == "isolated") meta.handState = "inferred";
|
||||
else meta.handState = "conflict";
|
||||
if (track.hasLimbPrevious) {
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
auto delta = sub(limb.limb[i], track.limbPrevious[i]);
|
||||
track.limbVelocity[i] = track.hasLimbVelocity ? add(mul(track.limbVelocity[i], 0.65), mul(delta, 0.35)) : delta;
|
||||
}
|
||||
track.hasLimbVelocity = true;
|
||||
}
|
||||
if (hand.handValid) {
|
||||
if (track.hasHandPrevious) {
|
||||
for (int i = 0; i < 21; ++i) {
|
||||
auto delta = sub(hand.hand[i], track.handPrevious[i]);
|
||||
track.handVelocity[i] = track.hasHandVelocity ? add(mul(track.handVelocity[i], 0.65), mul(delta, 0.35)) : delta;
|
||||
}
|
||||
track.hasHandVelocity = true;
|
||||
}
|
||||
track.handPrevious = hand.hand;
|
||||
track.hasHandPrevious = true;
|
||||
track.handSource = hand.source;
|
||||
}
|
||||
track.limbPrevious = limb.limb;
|
||||
track.hasLimbPrevious = true;
|
||||
track.limbSource = limb.source;
|
||||
}
|
||||
// Same-person duplicate suppression from the reference fusion.
|
||||
auto handCenter = [&](int side) {
|
||||
int start = side == 0 ? 91 : 112;
|
||||
Vec2 center{};
|
||||
for (int i = 0; i < 21; ++i) center = add(center, vec(output.pose.keypoints[start + i]));
|
||||
return mul(center, 1.0 / 21.0);
|
||||
};
|
||||
if (norm(sub(handCenter(0), handCenter(1))) / boxDiagonal(raw.box) < 0.08) {
|
||||
int weaker = output.meta[0].handScore <= output.meta[1].handScore ? 0 : 1;
|
||||
output.meta[weaker].handState = "duplicate";
|
||||
output.meta[weaker].distanceEligible = false;
|
||||
output.meta[weaker].alarmEligible = false;
|
||||
}
|
||||
// Motion-adaptive EMA in person-box coordinates.
|
||||
std::array<Vec2, 133> now{};
|
||||
for (int i = 0; i < 133; ++i) now[i] = normalized(output.pose.keypoints[i], raw.box);
|
||||
if (!state.hasSmoothed) { state.smoothed = now; state.hasSmoothed = true; }
|
||||
else {
|
||||
for (int i = 0; i < 133; ++i) {
|
||||
double displacement = norm(sub(now[i], state.smoothed[i]));
|
||||
double alpha = 0.32 + 0.58 * std::clamp(displacement / 0.08, 0.0, 1.0);
|
||||
state.smoothed[i] = add(mul(now[i], alpha), mul(state.smoothed[i], 1.0 - alpha));
|
||||
}
|
||||
}
|
||||
fused.push_back(std::move(output));
|
||||
}
|
||||
|
||||
// Exact build_global_hands() evidence: selected fusion output, stable
|
||||
// score ordering, then the same cross-person duplicate collapse.
|
||||
std::vector<GlobalHand> globalCandidates;
|
||||
for (std::size_t person = 0; person < fused.size(); ++person) {
|
||||
for (int side = 0; side < 2; ++side) {
|
||||
auto const& meta = fused[person].meta[side];
|
||||
if (!meta.alarmEligible) continue;
|
||||
int const start = side == 0 ? 91 : 112;
|
||||
GlobalHand hand;
|
||||
hand.personId = static_cast<int>(person) + 1;
|
||||
hand.trackId = fused[person].pose.box.trackId;
|
||||
hand.left = side == 0;
|
||||
hand.source = meta.handSource;
|
||||
hand.state = meta.handState;
|
||||
hand.distanceEligible = meta.distanceEligible;
|
||||
hand.alarmEligible = meta.alarmEligible;
|
||||
hand.score = static_cast<float>(meta.handScore);
|
||||
float minX = fused[person].pose.keypoints[start].x;
|
||||
float maxX = minX;
|
||||
float minY = fused[person].pose.keypoints[start].y;
|
||||
float maxY = minY;
|
||||
for (int point = 0; point < 21; ++point) {
|
||||
hand.keypoints[point] = fused[person].pose.keypoints[start + point];
|
||||
minX = std::min(minX, hand.keypoints[point].x);
|
||||
maxX = std::max(maxX, hand.keypoints[point].x);
|
||||
minY = std::min(minY, hand.keypoints[point].y);
|
||||
maxY = std::max(maxY, hand.keypoints[point].y);
|
||||
}
|
||||
hand.spanPx = std::hypot(maxX - minX, maxY - minY);
|
||||
globalCandidates.push_back(std::move(hand));
|
||||
}
|
||||
}
|
||||
std::stable_sort(globalCandidates.begin(), globalCandidates.end(), [](auto const& first, auto const& second) {
|
||||
return first.score > second.score;
|
||||
});
|
||||
std::vector<GlobalHand> globalHands;
|
||||
for (auto& candidate : globalCandidates) {
|
||||
Vec2 candidateCenter{};
|
||||
for (auto const& point : candidate.keypoints) candidateCenter = add(candidateCenter, vec(point));
|
||||
candidateCenter = mul(candidateCenter, 1.0 / 21.0);
|
||||
auto duplicate = globalHands.end();
|
||||
for (auto iterator = globalHands.begin(); iterator != globalHands.end(); ++iterator) {
|
||||
Vec2 existingCenter{};
|
||||
for (auto const& point : iterator->keypoints) existingCenter = add(existingCenter, vec(point));
|
||||
existingCenter = mul(existingCenter, 1.0 / 21.0);
|
||||
double const threshold = std::clamp(
|
||||
0.45 * std::max<double>(candidate.spanPx, iterator->spanPx), 18.0, 70.0);
|
||||
if (norm(sub(candidateCenter, existingCenter)) <= threshold) {
|
||||
duplicate = iterator;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (duplicate == globalHands.end()) globalHands.push_back(std::move(candidate));
|
||||
else duplicate->aliases.emplace_back(candidate.personId, candidate.left);
|
||||
}
|
||||
|
||||
// Build the model measurements, then collapse cross-person duplicates.
|
||||
std::vector<std::array<std::optional<Measurement>, 2>> measurements(fused.size());
|
||||
for (std::size_t person = 0; person < fused.size(); ++person) {
|
||||
for (int side = 0; side < 2; ++side) {
|
||||
if (!fused[person].meta[side].alarmEligible) continue;
|
||||
Measurement measurement;
|
||||
int start = side == 0 ? 91 : 112;
|
||||
for (int i = 0; i < 21; ++i) measurement.hand[i] = vec(fused[person].pose.keypoints[start + i]);
|
||||
measurement.precise = fused[person].meta[side].distanceEligible;
|
||||
measurement.provisional = !measurement.precise;
|
||||
measurement.quality = fused[person].meta[side].handScore;
|
||||
measurements[person][side] = measurement;
|
||||
}
|
||||
}
|
||||
auto handSpan = [&](Measurement const& measurement) {
|
||||
double minX = measurement.hand[0].x, maxX = minX, minY = measurement.hand[0].y, maxY = minY;
|
||||
for (auto point : measurement.hand) { minX = std::min(minX, point.x); maxX = std::max(maxX, point.x); minY = std::min(minY, point.y); maxY = std::max(maxY, point.y); }
|
||||
return std::hypot(maxX - minX, maxY - minY);
|
||||
};
|
||||
auto handCenter = [&](Measurement const& measurement) {
|
||||
Vec2 center{}; for (auto point : measurement.hand) center = add(center, point); return mul(center, 1.0 / 21.0);
|
||||
};
|
||||
for (std::size_t firstPerson = 0; firstPerson < fused.size(); ++firstPerson) {
|
||||
for (int firstSide = 0; firstSide < 2; ++firstSide) {
|
||||
if (!measurements[firstPerson][firstSide]) continue;
|
||||
for (std::size_t secondPerson = firstPerson + 1; secondPerson < fused.size(); ++secondPerson) {
|
||||
for (int secondSide = 0; secondSide < 2; ++secondSide) {
|
||||
if (!measurements[firstPerson][firstSide] || !measurements[secondPerson][secondSide]) continue;
|
||||
auto const& first = *measurements[firstPerson][firstSide];
|
||||
auto const& second = *measurements[secondPerson][secondSide];
|
||||
double firstSpan = handSpan(first), secondSpan = handSpan(second);
|
||||
if (norm(sub(handCenter(first), handCenter(second))) > std::max(70.0, 0.5 * std::max(firstSpan, secondSpan))) continue;
|
||||
double firstArea = std::max((fused[firstPerson].pose.box.x2 - fused[firstPerson].pose.box.x1) * (fused[firstPerson].pose.box.y2 - fused[firstPerson].pose.box.y1), 1.0F);
|
||||
double secondArea = std::max((fused[secondPerson].pose.box.x2 - fused[secondPerson].pose.box.x1) * (fused[secondPerson].pose.box.y2 - fused[secondPerson].pose.box.y1), 1.0F);
|
||||
if (firstArea >= 2.5 * secondArea &&
|
||||
(firstSpan >= 1.45 * secondSpan || first.quality >= second.quality + 0.6)) {
|
||||
measurements[secondPerson][secondSide].reset();
|
||||
states_[fused[secondPerson].pose.box.trackId].endpoints[secondSide] = {};
|
||||
} else if (secondArea >= 2.5 * firstArea &&
|
||||
(secondSpan >= 1.45 * firstSpan || second.quality >= first.quality + 0.6)) {
|
||||
measurements[firstPerson][firstSide].reset();
|
||||
states_[fused[firstPerson].pose.box.trackId].endpoints[firstSide] = {};
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Match resolve_global_hand_assignments(): a large foreground track may reclaim
|
||||
// a nearby hand that RTMW emitted only from a smaller background crop.
|
||||
struct Reassignment {
|
||||
double normalizedDistance{};
|
||||
std::size_t targetPerson{};
|
||||
int targetSide{};
|
||||
std::size_t sourcePerson{};
|
||||
int sourceSide{};
|
||||
Vec2 sourceCenter{};
|
||||
};
|
||||
std::vector<Reassignment> reassignments;
|
||||
for (std::size_t targetPerson = 0; targetPerson < fused.size(); ++targetPerson) {
|
||||
for (int targetSide = 0; targetSide < 2; ++targetSide) {
|
||||
if (measurements[targetPerson][targetSide]) continue;
|
||||
auto& targetEndpoint = states_[fused[targetPerson].pose.box.trackId].endpoints[targetSide];
|
||||
if (!targetEndpoint.hasWrist || !targetEndpoint.hasHandShape || targetEndpoint.missedFrames > 30) continue;
|
||||
double targetArea = std::max(
|
||||
(fused[targetPerson].pose.box.x2 - fused[targetPerson].pose.box.x1) *
|
||||
(fused[targetPerson].pose.box.y2 - fused[targetPerson].pose.box.y1), 1.0F);
|
||||
Vec2 shapeCenter{};
|
||||
for (auto point : targetEndpoint.handShape) shapeCenter = add(shapeCenter, point);
|
||||
shapeCenter = mul(shapeCenter, 1.0 / 21.0);
|
||||
Vec2 predictedCenter = add(targetEndpoint.wrist, shapeCenter);
|
||||
Measurement targetShape;
|
||||
for (int point = 0; point < 21; ++point) targetShape.hand[point] = targetEndpoint.handShape[point];
|
||||
double targetSpan = handSpan(targetShape);
|
||||
double gate = 0.20 * boxDiagonal(fused[targetPerson].pose.box);
|
||||
for (std::size_t sourcePerson = 0; sourcePerson < fused.size(); ++sourcePerson) {
|
||||
if (sourcePerson == targetPerson) continue;
|
||||
for (int sourceSide = 0; sourceSide < 2; ++sourceSide) {
|
||||
auto const& source = measurements[sourcePerson][sourceSide];
|
||||
if (!source || source->source != "model") continue;
|
||||
double sourceArea = std::max(
|
||||
(fused[sourcePerson].pose.box.x2 - fused[sourcePerson].pose.box.x1) *
|
||||
(fused[sourcePerson].pose.box.y2 - fused[sourcePerson].pose.box.y1), 1.0F);
|
||||
if (targetArea < 2.5 * sourceArea) continue;
|
||||
Vec2 sourceCenter = handCenter(*source);
|
||||
double sourceSpan = handSpan(*source);
|
||||
double candidateDistance = norm(sub(sourceCenter, predictedCenter));
|
||||
if (candidateDistance > gate || targetSpan < 1.45 * std::max(sourceSpan, 1.0)) continue;
|
||||
reassignments.push_back({
|
||||
candidateDistance / std::max(gate, 1e-6), targetPerson, targetSide,
|
||||
sourcePerson, sourceSide, sourceCenter});
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
std::sort(reassignments.begin(), reassignments.end(), [](auto const& first, auto const& second) {
|
||||
return std::tie(first.normalizedDistance, first.targetPerson, first.targetSide,
|
||||
first.sourcePerson, first.sourceSide) <
|
||||
std::tie(second.normalizedDistance, second.targetPerson, second.targetSide,
|
||||
second.sourcePerson, second.sourceSide);
|
||||
});
|
||||
std::set<std::pair<std::size_t, int>> usedTargets, usedSources;
|
||||
for (auto const& item : reassignments) {
|
||||
auto targetKey = std::make_pair(item.targetPerson, item.targetSide);
|
||||
auto sourceKey = std::make_pair(item.sourcePerson, item.sourceSide);
|
||||
if (usedTargets.count(targetKey) || usedSources.count(sourceKey) ||
|
||||
measurements[item.targetPerson][item.targetSide] ||
|
||||
!measurements[item.sourcePerson][item.sourceSide]) continue;
|
||||
auto& targetEndpoint = states_[fused[item.targetPerson].pose.box.trackId].endpoints[item.targetSide];
|
||||
Vec2 shapeCenter{};
|
||||
for (auto point : targetEndpoint.handShape) shapeCenter = add(shapeCenter, point);
|
||||
shapeCenter = mul(shapeCenter, 1.0 / 21.0);
|
||||
Vec2 reassignedWrist = sub(item.sourceCenter, shapeCenter);
|
||||
Measurement reassigned;
|
||||
for (int point = 0; point < 21; ++point) {
|
||||
reassigned.hand[point] = add(reassignedWrist, targetEndpoint.handShape[point]);
|
||||
}
|
||||
reassigned.provisional = true;
|
||||
reassigned.source = "global_reassignment";
|
||||
reassigned.quality = measurements[item.sourcePerson][item.sourceSide]->quality;
|
||||
measurements[item.targetPerson][item.targetSide] = reassigned;
|
||||
measurements[item.sourcePerson][item.sourceSide].reset();
|
||||
auto& sourceEndpoint = states_[fused[item.sourcePerson].pose.box.trackId].endpoints[item.sourceSide];
|
||||
sourceEndpoint = {};
|
||||
usedTargets.insert(targetKey);
|
||||
usedSources.insert(sourceKey);
|
||||
}
|
||||
|
||||
PoseFrameResult result;
|
||||
result.globalHands = std::move(globalHands);
|
||||
result.selectedPeople.reserve(fused.size());
|
||||
result.smoothedPeople.reserve(fused.size());
|
||||
result.v2People.reserve(fused.size());
|
||||
for (auto const& person : fused) {
|
||||
std::array<FusionMeta,2> meta{};
|
||||
for (int side=0;side<2;++side) {
|
||||
auto const& m=person.meta[side];
|
||||
meta[side]={m.limbConsensus,m.handValid,m.distanceEligible,m.alarmEligible,m.handScore,m.handSource,m.handState};
|
||||
}
|
||||
result.metadata.push_back(meta);
|
||||
result.selectedPeople.push_back(person.pose);
|
||||
PersonPose smoothed = person.pose;
|
||||
auto const& state = states_[person.pose.box.trackId];
|
||||
for (int keypoint = 0; keypoint < 133; ++keypoint) {
|
||||
Vec2 point = denormalized(state.smoothed[keypoint], person.pose.box);
|
||||
smoothed.keypoints[keypoint].x = static_cast<float>(point.x);
|
||||
smoothed.keypoints[keypoint].y = static_cast<float>(point.y);
|
||||
}
|
||||
result.smoothedPeople.push_back(smoothed);
|
||||
result.v2People.push_back(std::move(smoothed));
|
||||
}
|
||||
for (std::size_t person = 0; person < fused.size(); ++person) {
|
||||
auto& state = states_[fused[person].pose.box.trackId];
|
||||
int frameGap = state.lastFrame < 0 ? 0 : std::max(frameIndex - state.lastFrame - 1, 0);
|
||||
for (auto& endpoint : state.endpoints) endpoint.missedFrames += frameGap;
|
||||
state.lastFrame = frameIndex;
|
||||
for (int side = 0; side < 2; ++side) {
|
||||
if (!fused[person].meta[side].limbConsensus || !fused[person].meta[side].distanceEligible) continue;
|
||||
int shoulderIndex = side == 0 ? 5 : 6;
|
||||
int elbowIndex = side == 0 ? 7 : 8;
|
||||
int wristIndex = side == 0 ? 9 : 10;
|
||||
Vec2 shoulder = vec(fused[person].pose.keypoints[shoulderIndex]);
|
||||
Vec2 elbow = vec(fused[person].pose.keypoints[elbowIndex]);
|
||||
Vec2 wrist = vec(fused[person].pose.keypoints[wristIndex]);
|
||||
double diagonal = boxDiagonal(fused[person].pose.box);
|
||||
double upper = norm(sub(shoulder, elbow)) / diagonal;
|
||||
double forearm = norm(sub(elbow, wrist)) / diagonal;
|
||||
if (upper >= 0.04 && upper <= 0.65 && forearm >= 0.04 && forearm <= 0.65) {
|
||||
auto& samples = state.boneSamples[side];
|
||||
samples.push_back({upper, forearm});
|
||||
while (samples.size() > 120) samples.pop_front();
|
||||
}
|
||||
}
|
||||
for (int side = 0; side < 2; ++side) {
|
||||
int shoulderIndex = side == 0 ? 5 : 6;
|
||||
int elbowIndex = side == 0 ? 7 : 8;
|
||||
int wristIndex = side == 0 ? 9 : 10;
|
||||
int handStart = side == 0 ? 91 : 112;
|
||||
auto& endpoint = state.endpoints[side];
|
||||
auto measurement = measurements[person][side];
|
||||
// Reference temporal recovery gate.
|
||||
bool const recoverableHandState = fused[person].meta[side].handState == "conflict" ||
|
||||
fused[person].meta[side].handState == "inferred";
|
||||
if (!measurement && endpoint.hasWrist && endpoint.hasHandShape && endpoint.missedFrames <= 30 &&
|
||||
recoverableHandState && fused[person].meta[side].handValid &&
|
||||
fused[person].meta[side].handScore >= 2.0 &&
|
||||
fused[person].meta[side].linkToSelectedLimb <= 0.08) {
|
||||
Vec2 measured = vec(fused[person].pose.keypoints[handStart]);
|
||||
Vec2 prediction = add(endpoint.wrist, endpoint.velocity);
|
||||
double gate = std::min(0.20, 0.05 + 0.008 * std::max(endpoint.missedFrames, 1)) * boxDiagonal(fused[person].pose.box);
|
||||
if (norm(sub(measured, prediction)) <= gate) {
|
||||
Measurement recovered;
|
||||
for (int i = 0; i < 21; ++i) recovered.hand[i] = vec(fused[person].pose.keypoints[handStart + i]);
|
||||
recovered.provisional = true; recovered.quality = fused[person].meta[side].handScore; recovered.source = "temporal_recovery";
|
||||
measurement = recovered;
|
||||
}
|
||||
}
|
||||
LimbState outputState;
|
||||
std::array<Vec2, 21> outputHand{};
|
||||
bool hasOutput = false;
|
||||
bool returningAfterLong = measurement && endpoint.missedFrames > 12;
|
||||
if (measurement) {
|
||||
Vec2 measuredWrist = measurement->hand[0];
|
||||
std::array<Vec2, 21> measuredShape{};
|
||||
for (int i = 0; i < 21; ++i) measuredShape[i] = sub(measurement->hand[i], measuredWrist);
|
||||
if (!endpoint.hasWrist || endpoint.missedFrames > 12) {
|
||||
endpoint.wrist = measuredWrist; endpoint.velocity = {}; endpoint.handShape = measuredShape;
|
||||
} else {
|
||||
Vec2 prediction = add(endpoint.wrist, endpoint.velocity);
|
||||
Vec2 residual = sub(measuredWrist, prediction);
|
||||
double alpha = measurement->precise ? 0.78 : 0.88;
|
||||
double beta = measurement->precise ? 0.18 : 0.24;
|
||||
endpoint.wrist = add(prediction, mul(residual, alpha));
|
||||
endpoint.velocity = add(mul(endpoint.velocity, 0.82), mul(residual, beta));
|
||||
double shapeAlpha = measurement->precise ? 0.55 : 0.70;
|
||||
for (int i = 0; i < 21; ++i) endpoint.handShape[i] = add(mul(measuredShape[i], shapeAlpha), mul(endpoint.handShape[i], 1.0 - shapeAlpha));
|
||||
}
|
||||
endpoint.hasWrist = endpoint.hasHandShape = true;
|
||||
endpoint.lastObservedFrame = frameIndex;
|
||||
endpoint.missedFrames = 0;
|
||||
outputState = measurement->source == "global_reassignment" ? LimbState::Reassigned : measurement->source == "temporal_recovery" ? LimbState::Recovered : measurement->precise ? LimbState::Observed : measurement->provisional ? LimbState::Provisional : LimbState::Interpolated;
|
||||
for (int i = 0; i < 21; ++i) outputHand[i] = add(endpoint.wrist, endpoint.handShape[i]);
|
||||
hasOutput = true;
|
||||
} else {
|
||||
++endpoint.missedFrames;
|
||||
if (!endpoint.hasWrist || endpoint.missedFrames > 30) outputState = LimbState::ReachableLong;
|
||||
else if (endpoint.missedFrames <= 12) {
|
||||
endpoint.wrist = add(endpoint.wrist, endpoint.velocity);
|
||||
endpoint.velocity = mul(endpoint.velocity, 0.86);
|
||||
for (int i = 0; i < 21; ++i) outputHand[i] = add(endpoint.wrist, endpoint.handShape[i]);
|
||||
hasOutput = true; outputState = LimbState::PredictedShort;
|
||||
} else outputState = LimbState::ReachableMedium;
|
||||
}
|
||||
Vec2 smoothedShoulder = denormalized(state.smoothed[shoulderIndex], fused[person].pose.box);
|
||||
Vec2 smoothedElbow = denormalized(state.smoothed[elbowIndex], fused[person].pose.box);
|
||||
Keypoint shoulder = fused[person].pose.keypoints[shoulderIndex];
|
||||
shoulder.x = static_cast<float>(smoothedShoulder.x);
|
||||
shoulder.y = static_cast<float>(smoothedShoulder.y);
|
||||
Keypoint elbow = fused[person].pose.keypoints[elbowIndex];
|
||||
elbow.x = static_cast<float>(smoothedElbow.x);
|
||||
elbow.y = static_cast<float>(smoothedElbow.y);
|
||||
Keypoint wrist = result.smoothedPeople[person].keypoints[wristIndex];
|
||||
std::array<Keypoint, 21> hand{};
|
||||
for (int i = 0; i < 21; ++i) {
|
||||
hand[i] = result.smoothedPeople[person].keypoints[handStart + i];
|
||||
}
|
||||
if (hasOutput) {
|
||||
auto& ownSamples = state.boneSamples[side];
|
||||
auto& oppositeSamples = state.boneSamples[1 - side];
|
||||
auto const& samples = ownSamples.size() < 10 && oppositeSamples.size() >= 10
|
||||
? oppositeSamples : ownSamples;
|
||||
double upperRatio = 0.24, forearmRatio = 0.22;
|
||||
if (!samples.empty()) {
|
||||
std::vector<double> upperValues, forearmValues;
|
||||
upperValues.reserve(samples.size());
|
||||
forearmValues.reserve(samples.size());
|
||||
for (auto const& sample : samples) {
|
||||
upperValues.push_back(sample[0]);
|
||||
forearmValues.push_back(sample[1]);
|
||||
}
|
||||
upperRatio = median(std::move(upperValues));
|
||||
forearmRatio = median(std::move(forearmValues));
|
||||
}
|
||||
double diagonal = boxDiagonal(fused[person].pose.box);
|
||||
double upperLength = upperRatio * diagonal;
|
||||
double forearmLength = forearmRatio * diagonal;
|
||||
double totalLength = upperLength + forearmLength;
|
||||
if (totalLength > 0.95 * diagonal) {
|
||||
double lengthScale = 0.95 * diagonal / totalLength;
|
||||
upperLength *= lengthScale;
|
||||
forearmLength *= lengthScale;
|
||||
}
|
||||
Vec2 referenceElbow = returningAfterLong || !endpoint.hasLastElbow
|
||||
? vec(fused[person].pose.keypoints[elbowIndex]) : endpoint.lastElbow;
|
||||
if (!fused[person].meta[side].limbConsensus || outputState != LimbState::Observed) {
|
||||
Vec2 solved = solveTwoBoneElbow(
|
||||
smoothedShoulder, outputHand[0], upperLength, forearmLength, referenceElbow);
|
||||
elbow.x = static_cast<float>(solved.x);
|
||||
elbow.y = static_cast<float>(solved.y);
|
||||
}
|
||||
endpoint.lastElbow = vec(elbow);
|
||||
endpoint.hasLastElbow = true;
|
||||
Vec2 armDirection = sub(outputHand[0], smoothedShoulder);
|
||||
if (norm(armDirection) > 1e-6) {
|
||||
endpoint.lastDirection = mul(armDirection, 1.0 / norm(armDirection));
|
||||
endpoint.hasLastDirection = true;
|
||||
}
|
||||
wrist.x = outputHand[0].x; wrist.y = outputHand[0].y;
|
||||
for (int i = 0; i < 21; ++i) {
|
||||
hand[i].x = outputHand[i].x; hand[i].y = outputHand[i].y;
|
||||
}
|
||||
}
|
||||
auto& v2Pose = result.v2People[person];
|
||||
v2Pose.keypoints[shoulderIndex] = shoulder;
|
||||
v2Pose.keypoints[elbowIndex] = elbow;
|
||||
v2Pose.keypoints[wristIndex] = wrist;
|
||||
if (hasOutput) {
|
||||
for (int point = 0; point < 21; ++point) v2Pose.keypoints[handStart + point] = hand[point];
|
||||
}
|
||||
result.limbs.push_back({
|
||||
fused[person].pose.box.trackId, side == 0, outputState,
|
||||
elbow, wrist, hand, fused[person].pose.box,
|
||||
fused[person].meta[side].distanceEligible,
|
||||
fused[person].meta[side].alarmEligible,
|
||||
static_cast<float>(fused[person].meta[side].handScore)});
|
||||
}
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
} // namespace fangjia
|
||||
@@ -0,0 +1,38 @@
|
||||
#include "serialization.hpp"
|
||||
namespace cms {
|
||||
namespace {
|
||||
template<class T> Json optional(std::optional<T> const& x){return x?Json(*x):Json(nullptr);}
|
||||
Json riskJson(RiskResult const& r) {
|
||||
Json out={{"status",r.status},{"risk_score",optional(r.risk_score)},{"risk_level",r.risk_level},{"decision",r.decision},{"source",optional(r.source)},{"track_id",optional(r.track_id)},{"side",optional(r.side)},{"regions",Json::array()}};
|
||||
if(!r.risk_type.empty()){out["risk_type"]=r.risk_type;out["door_id"]=optional(r.door_id);}
|
||||
if(!r.door_mask.status.empty()) {
|
||||
auto const& d=r.door_mask;out["door_mask"]={{"status",d.status},{"annotation_index",d.annotation_index},{"path",optional(d.path)},{"format",d.format},{"area_px",d.area_px}};
|
||||
out["warning_distance_relative"]=r.warning_distance_relative;out["release_distance_relative"]=r.release_distance_relative;
|
||||
}
|
||||
for(auto const& v:r.regions) {
|
||||
Json j={{"status",v.status},{"risk_score",optional(v.risk_score)},{"risk_level",v.risk_level},{"decision",v.decision},{"source",v.source},{"track_id",v.track_id},{"side",v.side}};
|
||||
if(v.state)j["state"]=*v.state;
|
||||
if(!v.door_zone.empty())j["door_zone"]=v.door_zone;
|
||||
if(v.region_area_px) {
|
||||
j["region_area_px"]=optional(v.region_area_px);j["door_overlap_px"]=optional(v.door_overlap_px);j["region_overlap_ratio"]=optional(v.region_overlap_ratio);
|
||||
j["min_distance_px"]=optional(v.min_distance_px);j["region_scale_px"]=optional(v.region_scale_px);j["distance_relative"]=optional(v.distance_relative);
|
||||
}
|
||||
out["regions"].push_back(std::move(j));
|
||||
}
|
||||
return out;
|
||||
}
|
||||
}
|
||||
Json serializeResult(FrameResult const& r) {
|
||||
Json out={{"frame_index",r.frame_index},{"track_ids",r.track_ids},{"boxes",r.boxes},{"skeletons",Json::array()},{"active_door",r.active_door},{"door_assignments",Json::array()},{"door_risks",Json::object()},{"risk",riskJson(r.risk)},{"ungated_risk",riskJson(r.ungated_risk)}};
|
||||
for(std::size_t i=0;i<r.persons.size();++i) {
|
||||
auto const& p=r.persons[i];Json points=Json::array(),metadata=Json::object();for(auto const& k:p.selected)points.push_back({rounded(k.x,3),rounded(k.y,3)});
|
||||
for(int s=0;s<2;++s){auto const& h=p.hands[s];metadata[s==0?"left":"right"]={{"limb_state",h.limb_consensus?"consensus":"inferred"},{"hand_source",h.source},{"hand_state",h.state},{"distance_eligible",h.distance_eligible},{"alarm_eligible",h.alarm_eligible}};}
|
||||
out["skeletons"].push_back({{"track_id",p.track_id},{"person_index",i},{"layout","coco_wholebody_133"},{"keypoints",points},{"metadata",metadata}});
|
||||
}
|
||||
for(auto const& a:r.door_assignments)out["door_assignments"].push_back({{"track_id",a.track_id},{"person_index",a.person_index},{"door_zone",a.door_zone},{"raw_door_zone",a.raw_door_zone},{"front_probability",a.front_probability},{"smoothed_front_probability",a.smoothed_front_probability},{"confidence",a.confidence},{"evidence_source",a.evidence_source},{"pose_feature_count",a.pose_feature_count},{"history_frames",a.history_frames},{"features",a.features},{"feature_frontness",a.feature_frontness}});
|
||||
for(auto const& entry:r.door_risks)out["door_risks"][entry.first]=riskJson(entry.second);
|
||||
out["can"]={{"positions",r.can.positions},{"age_ms",r.can.age_ms},{"valid",r.can.valid},{"source",r.can.source},{"side",r.can.side}};
|
||||
out["capture"]={{"sequence",r.capture.driver_sequence},{"capture_monotonic_ms",r.capture.capture_monotonic_ns*1e-6},{"driver_timestamp_ms",r.capture.driver_timestamp_ns*1e-6}};
|
||||
out["capture_to_result_ms"]=rounded(r.timing.capture_to_result_ms,3);out["calibration_verified"]=r.door_mask_calibration_verified;return out;
|
||||
}
|
||||
}
|
||||
@@ -44,6 +44,12 @@ void TrtEngine::prepare() {
|
||||
}
|
||||
auto& item = buffer(name);
|
||||
item.required = volume(dims) * dataTypeSize(engine_->getTensorDataType(name.c_str()));
|
||||
if (item.external) {
|
||||
if (item.capacity < item.required) {
|
||||
throw std::runtime_error("External buffer is too small for " + name);
|
||||
}
|
||||
continue;
|
||||
}
|
||||
if (item.capacity < item.required) {
|
||||
if (item.owned) CUDA_CHECK(cudaFree(item.owned));
|
||||
CUDA_CHECK(cudaMalloc(&item.owned, item.required));
|
||||
@@ -53,6 +59,19 @@ void TrtEngine::prepare() {
|
||||
}
|
||||
}
|
||||
|
||||
void TrtEngine::bindExternal(std::string const& name, void* pointer, std::size_t bytesValue) {
|
||||
auto& item = buffer(name);
|
||||
item.active = pointer;
|
||||
item.capacity = bytesValue;
|
||||
item.external = true;
|
||||
}
|
||||
|
||||
void TrtEngine::useOwned(std::string const& name) {
|
||||
auto& item = buffer(name);
|
||||
item.external = false;
|
||||
item.active = item.owned;
|
||||
}
|
||||
|
||||
bool TrtEngine::enqueue(cudaStream_t stream) {
|
||||
for (int index = 0; index < engine_->getNbIOTensors(); ++index) {
|
||||
std::string const name = engine_->getIOTensorName(index);
|
||||
@@ -0,0 +1,99 @@
|
||||
#include "visualizer.hpp"
|
||||
#include "camera_pipeline.hpp"
|
||||
#include <opencv2/highgui.hpp>
|
||||
#include <opencv2/highgui/highgui_c.h>
|
||||
#include <opencv2/imgproc.hpp>
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <iomanip>
|
||||
#include <sstream>
|
||||
#include <stdexcept>
|
||||
|
||||
namespace cms {
|
||||
namespace {
|
||||
constexpr char windowName[]="CMS Camera Risk";
|
||||
cv::Point point(PosePoint const& p){return {int(std::nearbyint(p.x)),int(std::nearbyint(p.y))};}
|
||||
cv::Scalar color(std::string const& level) {
|
||||
if(level=="danger")return {40,40,255};
|
||||
if(level=="warning")return {0,160,255};
|
||||
if(level=="watch")return {0,240,240};
|
||||
if(level=="clear")return {80,220,90};
|
||||
return {190,190,190};
|
||||
}
|
||||
void segment(cv::Mat& image,cv::Point a,cv::Point b,cv::Scalar c,int thickness,bool dashed) {
|
||||
if(!dashed){cv::line(image,a,b,c,thickness,cv::LINE_AA);return;}
|
||||
cv::Point2d d=b-a;double length=cv::norm(d);if(length<1)return;d*=1.0/length;
|
||||
for(double x=0;x<length;x+=14)cv::line(image,cv::Point2d(a)+d*x,cv::Point2d(a)+d*std::min(x+8,length),c,thickness,cv::LINE_AA);
|
||||
}
|
||||
}
|
||||
cv::Mat renderResult(FrameResult const& r) {
|
||||
if(!r.image)throw std::invalid_argument("Result has no frame image");
|
||||
auto const& image=*r.image;
|
||||
if(image.width<=0||image.height<=0||image.stride<image.width*3||image.pixels.size()<std::size_t(image.stride)*image.height)
|
||||
throw std::invalid_argument("Malformed result image");
|
||||
cv::Mat view(image.height,image.width,CV_8UC3,const_cast<std::uint8_t*>(image.pixels.data()),image.stride);
|
||||
auto canvas=view.clone();
|
||||
if(r.door_mask_pixels) {
|
||||
auto const& m=*r.door_mask_pixels;
|
||||
if(m.width!=image.width||m.height!=image.height||m.stride<m.width||m.pixels.size()<std::size_t(m.stride)*m.height)
|
||||
throw std::invalid_argument("Result mask does not match image");
|
||||
cv::Mat mask(m.height,m.width,CV_8U,const_cast<std::uint8_t*>(m.pixels.data()),m.stride);
|
||||
auto tint=canvas.clone();tint.setTo(cv::Scalar(255,180,0),mask);
|
||||
cv::addWeighted(canvas,0.75,tint,0.25,0,canvas);
|
||||
std::vector<std::vector<cv::Point>> contours;cv::findContours(mask.clone(),contours,cv::RETR_EXTERNAL,cv::CHAIN_APPROX_SIMPLE);
|
||||
cv::drawContours(canvas,contours,-1,{255,180,0},2);
|
||||
}
|
||||
for(auto const& person:r.persons) {
|
||||
auto const& b=person.box;
|
||||
cv::rectangle(canvas,{int(b[0]),int(b[1])},{int(b[2]),int(b[3])},{0,220,255},2);
|
||||
std::ostringstream text;text<<"ID "<<person.track_id<<" "<<std::fixed<<std::setprecision(2)<<person.confidence;
|
||||
cv::putText(canvas,text.str(),{int(b[0])+4,std::max(180,int(b[1])+25)},cv::FONT_HERSHEY_SIMPLEX,.8,{0,220,255},2,cv::LINE_AA);
|
||||
for(auto const& a:r.door_assignments)if(a.track_id==person.track_id)
|
||||
cv::putText(canvas,a.door_zone,{int(b[0])+4,std::max(155,int(b[1])-8)},cv::FONT_HERSHEY_SIMPLEX,.7,{0,220,255},2,cv::LINE_AA);
|
||||
for(int s=0;s<2;++s) {
|
||||
auto const& state=person.hands[s];auto const& k=person.selected;
|
||||
cv::Scalar arm=s==0?cv::Scalar(40,40,255):cv::Scalar(30,230,30);
|
||||
for(auto e:std::vector<std::pair<int,int>>{{5+s,7+s},{7+s,9+s}}) {
|
||||
segment(canvas,point(k[e.first]),point(k[e.second]),state.limb_consensus?arm:cv::Scalar(0,140,255),3,!state.limb_consensus);
|
||||
cv::circle(canvas,point(k[e.second]),4,arm,-1,cv::LINE_AA);
|
||||
}
|
||||
if(state.state=="rejected")continue;
|
||||
int start=s==0?91:112;
|
||||
auto hand=state.distance_eligible?(s==0?cv::Scalar(255,80,220):cv::Scalar(255,190,40)):cv::Scalar(0,140,255);
|
||||
for(int f=0;f<5;++f)for(int j=0;j<4;++j){int a=j==0?start:start+f*4+j;int b=start+1+f*4+j;segment(canvas,point(k[a]),point(k[b]),hand,2,!state.distance_eligible);}
|
||||
for(int j=0;j<21;++j)cv::circle(canvas,point(k[start+j]),3,hand,-1,cv::LINE_AA);
|
||||
}
|
||||
}
|
||||
for(auto const& h:r.hand_regions) {
|
||||
std::vector<cv::Point> points,hull;for(auto const& p:h.keypoints)points.push_back(point(p));cv::convexHull(points,hull);
|
||||
auto risk=std::find_if(r.risk.regions.begin(),r.risk.regions.end(),[&](auto const& item){return item.track_id==h.track_id&&item.side==h.side;});
|
||||
auto c=color(risk==r.risk.regions.end()?"unavailable":risk->risk_level);
|
||||
cv::polylines(canvas,std::vector<std::vector<cv::Point>>{hull},true,c,2,cv::LINE_AA);
|
||||
}
|
||||
for(auto const& region:r.risk.regions)if(region.source=="YOLO_BODY_BOX_FALLBACK") {
|
||||
auto it=std::find(r.track_ids.begin(),r.track_ids.end(),region.track_id);
|
||||
if(it!=r.track_ids.end()){auto const& b=r.boxes.at(it-r.track_ids.begin());cv::putText(canvas,"BODY FALLBACK",{int(b[0])+4,int(b[1])+50},cv::FONT_HERSHEY_SIMPLEX,.65,{0,160,255},2,cv::LINE_AA);}
|
||||
}
|
||||
cv::rectangle(canvas,{0,0},{canvas.cols,148},{20,24,30},-1);
|
||||
std::ostringstream headline;headline<<r.risk.risk_type<<" ";if(r.risk.risk_score)headline<<std::fixed<<std::setprecision(3)<<*r.risk.risk_score;else headline<<"N/A";headline<<" "<<r.risk.risk_level;
|
||||
cv::putText(canvas,headline.str(),{24,40},cv::FONT_HERSHEY_SIMPLEX,1.0,color(r.risk.risk_level),2,cv::LINE_AA);
|
||||
std::ostringstream status;status<<"camera frame "<<r.frame_index<<" | seq "<<r.capture.driver_sequence<<" | latency "<<std::fixed<<std::setprecision(1)<<r.timing.capture_to_result_ms<<" ms";
|
||||
cv::putText(canvas,status.str(),{24,82},cv::FONT_HERSHEY_SIMPLEX,.72,{235,235,235},2,cv::LINE_AA);
|
||||
std::string calibration=r.door_mask_calibration_verified?"door calibration verified":"door calibration NOT verified (bring-up)";
|
||||
cv::putText(canvas,calibration+" | CAN "+r.can.source+" | q/Esc: close preview",{24,123},cv::FONT_HERSHEY_SIMPLEX,.70,{0,190,255},2,cv::LINE_AA);
|
||||
return canvas;
|
||||
}
|
||||
void runLocalVisualization(CameraPipeline const& pipeline) {
|
||||
struct Window {
|
||||
Window(){cv::namedWindow(windowName,cv::WINDOW_NORMAL);cv::resizeWindow(windowName,1000,800);}
|
||||
~Window(){try{if(cvGetWindowHandle(windowName))cv::destroyWindow(windowName);}catch(...) {}}
|
||||
} window;
|
||||
FrameResultPtr shown;
|
||||
while(pipeline.running()) {
|
||||
auto result=pipeline.latest();
|
||||
if(result&&result!=shown){cv::imshow(windowName,renderResult(*result));shown=std::move(result);}
|
||||
int key=cv::waitKey(16)&0xff;
|
||||
if(key==27||key=='q'||key=='Q'||cvGetWindowHandle(windowName)==nullptr)return;
|
||||
}
|
||||
}
|
||||
}
|
||||
Vendored
+21
@@ -0,0 +1,21 @@
|
||||
MIT License
|
||||
|
||||
Copyright (c) 2013-2022 Niels Lohmann
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
copies of the Software, and to permit persons to whom the Software is
|
||||
furnished to do so, subject to the following conditions:
|
||||
|
||||
The above copyright notice and this permission notice shall be included in all
|
||||
copies or substantial portions of the Software.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
SOFTWARE.
|
||||
Vendored
+24765
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user