851 lines
32 KiB
Python
851 lines
32 KiB
Python
|
#!/usr/bin/env python3
|
|||
|
|
# -*- coding: utf-8 -*-
|
|||
|
|
"""
|
|||
|
|
FemtoBolt深度相机管理器
|
|||
|
|
负责FemtoBolt深度相机的连接、配置和深度图像数据采集
|
|||
|
|
"""
|
|||
|
|
|
|||
|
|
import os
|
|||
|
|
import sys
|
|||
|
|
import threading
|
|||
|
|
import time
|
|||
|
|
import base64
|
|||
|
|
import numpy as np
|
|||
|
|
import cv2
|
|||
|
|
from typing import Optional, Dict, Any, Tuple
|
|||
|
|
import logging
|
|||
|
|
from collections import deque
|
|||
|
|
import gc
|
|||
|
|
|
|||
|
|
try:
|
|||
|
|
from .base_device import BaseDevice
|
|||
|
|
from .utils.socket_manager import SocketManager
|
|||
|
|
from .utils.config_manager import ConfigManager
|
|||
|
|
except ImportError:
|
|||
|
|
from base_device import BaseDevice
|
|||
|
|
from utils.socket_manager import SocketManager
|
|||
|
|
from utils.config_manager import ConfigManager
|
|||
|
|
|
|||
|
|
|
|||
|
|
class FemtoBoltManager(BaseDevice):
|
|||
|
|
"""FemtoBolt深度相机管理器"""
|
|||
|
|
|
|||
|
|
def __init__(self, socketio, config_manager: Optional[ConfigManager] = None):
|
|||
|
|
"""
|
|||
|
|
初始化FemtoBolt管理器
|
|||
|
|
|
|||
|
|
Args:
|
|||
|
|
socketio: SocketIO实例
|
|||
|
|
config_manager: 配置管理器实例
|
|||
|
|
"""
|
|||
|
|
# 配置管理
|
|||
|
|
self.config_manager = config_manager or ConfigManager()
|
|||
|
|
self.config = self.config_manager.get_device_config('femtobolt')
|
|||
|
|
|
|||
|
|
# 调用父类初始化
|
|||
|
|
super().__init__("femtobolt", self.config)
|
|||
|
|
|
|||
|
|
# 设置SocketIO实例
|
|||
|
|
self.set_socketio(socketio)
|
|||
|
|
|
|||
|
|
# 设备信息字典
|
|||
|
|
self.device_info = {}
|
|||
|
|
|
|||
|
|
# 设备ID
|
|||
|
|
self.device_id = "femtobolt_001"
|
|||
|
|
|
|||
|
|
# 性能统计
|
|||
|
|
self.performance_stats = {
|
|||
|
|
'fps': 0.0,
|
|||
|
|
'frame_count': 0,
|
|||
|
|
'dropped_frames': 0,
|
|||
|
|
'processing_time': 0.0
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
# FemtoBolt SDK相关
|
|||
|
|
self.femtobolt = None
|
|||
|
|
self.device_handle = None
|
|||
|
|
self.sdk_initialized = False
|
|||
|
|
|
|||
|
|
# 设备配置
|
|||
|
|
self.color_resolution = self.config.get('color_resolution', '1080P')
|
|||
|
|
self.depth_mode = self.config.get('depth_mode', 'NFOV_UNBINNED')
|
|||
|
|
self.fps = self.config.get('fps', 15)
|
|||
|
|
self.depth_range_min = self.config.get('depth_range_min', 500)
|
|||
|
|
self.depth_range_max = self.config.get('depth_range_max', 4500)
|
|||
|
|
self.synchronized_images_only = self.config.get('synchronized_images_only', False)
|
|||
|
|
|
|||
|
|
# 数据处理
|
|||
|
|
self.streaming_thread = None
|
|||
|
|
self.depth_frame_cache = deque(maxlen=10)
|
|||
|
|
self.color_frame_cache = deque(maxlen=10)
|
|||
|
|
self.last_depth_frame = None
|
|||
|
|
self.last_color_frame = None
|
|||
|
|
self.frame_count = 0
|
|||
|
|
|
|||
|
|
# 图像处理参数
|
|||
|
|
self.contrast_factor = 1.2
|
|||
|
|
self.gamma_value = 0.8
|
|||
|
|
self.use_pseudo_color = True
|
|||
|
|
|
|||
|
|
# 性能监控
|
|||
|
|
self.fps_counter = 0
|
|||
|
|
self.fps_start_time = time.time()
|
|||
|
|
self.actual_fps = 0
|
|||
|
|
self.dropped_frames = 0
|
|||
|
|
|
|||
|
|
# 重连机制
|
|||
|
|
self.max_reconnect_attempts = 3
|
|||
|
|
self.reconnect_delay = 3.0
|
|||
|
|
|
|||
|
|
self.logger.info("FemtoBolt管理器初始化完成")
|
|||
|
|
|
|||
|
|
def initialize(self) -> bool:
|
|||
|
|
"""
|
|||
|
|
初始化FemtoBolt设备
|
|||
|
|
|
|||
|
|
Returns:
|
|||
|
|
bool: 初始化是否成功
|
|||
|
|
"""
|
|||
|
|
try:
|
|||
|
|
self.logger.info("正在初始化FemtoBolt设备...")
|
|||
|
|
|
|||
|
|
# 初始化SDK
|
|||
|
|
if not self._initialize_sdk():
|
|||
|
|
raise Exception("SDK初始化失败")
|
|||
|
|
|
|||
|
|
# 配置设备
|
|||
|
|
if not self._configure_device():
|
|||
|
|
raise Exception("设备配置失败")
|
|||
|
|
|
|||
|
|
# 启动设备
|
|||
|
|
if not self._start_device():
|
|||
|
|
raise Exception("设备启动失败")
|
|||
|
|
|
|||
|
|
self.is_connected = True
|
|||
|
|
self.device_info.update({
|
|||
|
|
'color_resolution': self.color_resolution,
|
|||
|
|
'depth_mode': self.depth_mode,
|
|||
|
|
'fps': self.fps,
|
|||
|
|
'depth_range': f"{self.depth_range_min}-{self.depth_range_max}mm"
|
|||
|
|
})
|
|||
|
|
|
|||
|
|
self.logger.info("FemtoBolt初始化成功")
|
|||
|
|
return True
|
|||
|
|
|
|||
|
|
except Exception as e:
|
|||
|
|
self.logger.error(f"FemtoBolt初始化失败: {e}")
|
|||
|
|
self.is_connected = False
|
|||
|
|
self._cleanup_device()
|
|||
|
|
return False
|
|||
|
|
|
|||
|
|
def _initialize_sdk(self) -> bool:
|
|||
|
|
"""
|
|||
|
|
初始化FemtoBolt SDK (使用pykinect_azure)
|
|||
|
|
|
|||
|
|
Returns:
|
|||
|
|
bool: SDK初始化是否成功
|
|||
|
|
"""
|
|||
|
|
try:
|
|||
|
|
# 尝试导入pykinect_azure
|
|||
|
|
real_pykinect = None
|
|||
|
|
try:
|
|||
|
|
import pykinect_azure as pykinect
|
|||
|
|
real_pykinect = pykinect
|
|||
|
|
self.logger.info("成功导入pykinect_azure库")
|
|||
|
|
except ImportError as e:
|
|||
|
|
self.logger.warning(f"无法导入pykinect_azure库，使用模拟模式: {e}")
|
|||
|
|
self.pykinect = self._create_mock_pykinect()
|
|||
|
|
self.sdk_initialized = True
|
|||
|
|
return True
|
|||
|
|
|
|||
|
|
# 查找并初始化SDK路径
|
|||
|
|
sdk_initialized = False
|
|||
|
|
if real_pykinect and hasattr(real_pykinect, 'initialize_libraries'):
|
|||
|
|
sdk_paths = self._get_femtobolt_sdk_paths()
|
|||
|
|
for sdk_path in sdk_paths:
|
|||
|
|
if os.path.exists(sdk_path):
|
|||
|
|
try:
|
|||
|
|
real_pykinect.initialize_libraries(track_body=False, module_k4a_path=sdk_path)
|
|||
|
|
self.logger.info(f'✓ 成功使用FemtoBolt SDK: {sdk_path}')
|
|||
|
|
self.pykinect = real_pykinect
|
|||
|
|
sdk_initialized = True
|
|||
|
|
break
|
|||
|
|
except Exception as e:
|
|||
|
|
self.logger.warning(f'✗ FemtoBolt SDK路径失败: {sdk_path} - {e}')
|
|||
|
|
continue
|
|||
|
|
|
|||
|
|
if not sdk_initialized:
|
|||
|
|
self.logger.info('未找到真实SDK，使用模拟模式')
|
|||
|
|
self.pykinect = self._create_mock_pykinect()
|
|||
|
|
|
|||
|
|
self.sdk_initialized = True
|
|||
|
|
return True
|
|||
|
|
|
|||
|
|
except Exception as e:
|
|||
|
|
self.logger.error(f"SDK初始化失败: {e}")
|
|||
|
|
return False
|
|||
|
|
|
|||
|
|
def _get_femtobolt_sdk_paths(self) -> list:
|
|||
|
|
import platform
|
|||
|
|
sdk_paths = []
|
|||
|
|
if platform.system() == "Windows":
|
|||
|
|
# 优先使用Orbbec SDK K4A Wrapper（与azure_kinect_image_example.py一致）
|
|||
|
|
base_dir = os.path.dirname(os.path.abspath(__file__))
|
|||
|
|
dll_path = os.path.join(base_dir,"..", "dll","femtobolt","bin", "k4a.dll")
|
|||
|
|
self.logger.info(f"FemtoBolt SDK路径: {dll_path}")
|
|||
|
|
sdk_paths.append(dll_path)
|
|||
|
|
return sdk_paths
|
|||
|
|
|
|||
|
|
def _create_mock_pykinect(self):
|
|||
|
|
"""
|
|||
|
|
创建模拟pykinect_azure（用于测试）
|
|||
|
|
|
|||
|
|
Returns:
|
|||
|
|
Mock pykinect对象
|
|||
|
|
"""
|
|||
|
|
class MockPyKinect:
|
|||
|
|
def __init__(self):
|
|||
|
|
self.default_configuration = self._create_mock_config()
|
|||
|
|
|
|||
|
|
def initialize_libraries(self, track_body=False, module_k4a_path=None):
|
|||
|
|
pass
|
|||
|
|
|
|||
|
|
def start_device(self, config=None):
|
|||
|
|
return MockDevice()
|
|||
|
|
|
|||
|
|
def _create_mock_config(self):
|
|||
|
|
class MockConfig:
|
|||
|
|
def __init__(self):
|
|||
|
|
self.depth_mode = 'NFOV_UNBINNED'
|
|||
|
|
self.camera_fps = 15
|
|||
|
|
self.synchronized_images_only = False
|
|||
|
|
self.color_resolution = 0
|
|||
|
|
return MockConfig()
|
|||
|
|
|
|||
|
|
# 添加常量
|
|||
|
|
K4A_DEPTH_MODE_NFOV_UNBINNED = 'NFOV_UNBINNED'
|
|||
|
|
K4A_FRAMES_PER_SECOND_15 = 15
|
|||
|
|
|
|||
|
|
class MockDevice:
|
|||
|
|
def __init__(self):
|
|||
|
|
self.is_started = True
|
|||
|
|
|
|||
|
|
def update(self):
|
|||
|
|
return MockCapture()
|
|||
|
|
|
|||
|
|
def stop(self):
|
|||
|
|
self.is_started = False
|
|||
|
|
|
|||
|
|
def close(self):
|
|||
|
|
pass
|
|||
|
|
|
|||
|
|
class MockCapture:
|
|||
|
|
def __init__(self):
|
|||
|
|
pass
|
|||
|
|
|
|||
|
|
def get_depth_image(self):
|
|||
|
|
# 生成模拟深度图像
|
|||
|
|
height, width = 480, 640
|
|||
|
|
depth_image = np.full((height, width), 2000, dtype=np.uint16)
|
|||
|
|
|
|||
|
|
# 添加人体轮廓
|
|||
|
|
center_x = width // 2
|
|||
|
|
center_y = height // 2
|
|||
|
|
|
|||
|
|
# 头部
|
|||
|
|
cv2.circle(depth_image, (center_x, center_y - 100), 40, 1500, -1)
|
|||
|
|
# 身体
|
|||
|
|
cv2.rectangle(depth_image, (center_x - 50, center_y - 60),
|
|||
|
|
(center_x + 50, center_y + 100), 1600, -1)
|
|||
|
|
# 手臂
|
|||
|
|
cv2.rectangle(depth_image, (center_x - 80, center_y - 40),
|
|||
|
|
(center_x - 50, center_y + 20), 1700, -1)
|
|||
|
|
cv2.rectangle(depth_image, (center_x + 50, center_y - 40),
|
|||
|
|
(center_x + 80, center_y + 20), 1700, -1)
|
|||
|
|
|
|||
|
|
return True, depth_image
|
|||
|
|
|
|||
|
|
def get_color_image(self):
|
|||
|
|
return None
|
|||
|
|
|
|||
|
|
return MockPyKinect()
|
|||
|
|
|
|||
|
|
def _configure_device(self) -> bool:
|
|||
|
|
"""
|
|||
|
|
配置FemtoBolt设备
|
|||
|
|
|
|||
|
|
Returns:
|
|||
|
|
bool: 配置是否成功
|
|||
|
|
"""
|
|||
|
|
try:
|
|||
|
|
if not self.pykinect:
|
|||
|
|
return False
|
|||
|
|
|
|||
|
|
# 配置FemtoBolt设备参数
|
|||
|
|
self.femtobolt_config = self.pykinect.default_configuration
|
|||
|
|
self.femtobolt_config.depth_mode = self.pykinect.K4A_DEPTH_MODE_NFOV_UNBINNED
|
|||
|
|
self.femtobolt_config.camera_fps = self.pykinect.K4A_FRAMES_PER_SECOND_15
|
|||
|
|
self.femtobolt_config.synchronized_images_only = False
|
|||
|
|
self.femtobolt_config.color_resolution = 0
|
|||
|
|
|
|||
|
|
self.logger.info(f"FemtoBolt设备配置完成 - 深度模式: {self.depth_mode}, FPS: {self.fps}")
|
|||
|
|
return True
|
|||
|
|
|
|||
|
|
except Exception as e:
|
|||
|
|
self.logger.error(f"FemtoBolt设备配置失败: {e}")
|
|||
|
|
return False
|
|||
|
|
|
|||
|
|
def _start_device(self) -> bool:
|
|||
|
|
"""
|
|||
|
|
启动FemtoBolt设备
|
|||
|
|
|
|||
|
|
Returns:
|
|||
|
|
bool: 启动是否成功
|
|||
|
|
"""
|
|||
|
|
try:
|
|||
|
|
# 启动FemtoBolt设备
|
|||
|
|
self.logger.info(f'尝试启动FemtoBolt设备...')
|
|||
|
|
|
|||
|
|
if hasattr(self.pykinect, 'start_device'):
|
|||
|
|
# 真实设备模式
|
|||
|
|
self.device_handle = self.pykinect.start_device(config=self.femtobolt_config)
|
|||
|
|
if self.device_handle:
|
|||
|
|
self.logger.info('✓ FemtoBolt深度相机初始化成功!')
|
|||
|
|
else:
|
|||
|
|
raise Exception('设备启动返回None')
|
|||
|
|
else:
|
|||
|
|
# 模拟设备模式
|
|||
|
|
self.device_handle = self.pykinect.start_device(config=self.femtobolt_config)
|
|||
|
|
self.logger.info('✓ FemtoBolt深度相机模拟模式启动成功!')
|
|||
|
|
|
|||
|
|
# 等待设备稳定
|
|||
|
|
time.sleep(1.0)
|
|||
|
|
|
|||
|
|
# 测试捕获
|
|||
|
|
if not self._test_capture():
|
|||
|
|
raise Exception("设备捕获测试失败")
|
|||
|
|
|
|||
|
|
self.logger.info("FemtoBolt设备启动成功")
|
|||
|
|
return True
|
|||
|
|
|
|||
|
|
except Exception as e:
|
|||
|
|
self.logger.error(f"FemtoBolt设备启动失败: {e}")
|
|||
|
|
return False
|
|||
|
|
|
|||
|
|
def _test_capture(self) -> bool:
|
|||
|
|
"""
|
|||
|
|
测试设备捕获
|
|||
|
|
|
|||
|
|
Returns:
|
|||
|
|
bool: 测试是否成功
|
|||
|
|
"""
|
|||
|
|
try:
|
|||
|
|
for i in range(3):
|
|||
|
|
capture = self.device_handle.update()
|
|||
|
|
if capture:
|
|||
|
|
ret, depth_image = capture.get_depth_image()
|
|||
|
|
if ret and depth_image is not None:
|
|||
|
|
self.logger.info(f"FemtoBolt捕获测试成功 - 深度图像大小: {depth_image.shape}")
|
|||
|
|
return True
|
|||
|
|
time.sleep(0.1)
|
|||
|
|
|
|||
|
|
self.logger.error("FemtoBolt捕获测试失败")
|
|||
|
|
return False
|
|||
|
|
|
|||
|
|
except Exception as e:
|
|||
|
|
self.logger.error(f"FemtoBolt捕获测试异常: {e}")
|
|||
|
|
return False
|
|||
|
|
|
|||
|
|
def calibrate(self) -> bool:
|
|||
|
|
"""
|
|||
|
|
校准FemtoBolt设备
|
|||
|
|
|
|||
|
|
Returns:
|
|||
|
|
bool: 校准是否成功
|
|||
|
|
"""
|
|||
|
|
try:
|
|||
|
|
self.logger.info("开始FemtoBolt校准...")
|
|||
|
|
|
|||
|
|
if not self.is_connected:
|
|||
|
|
if not self.initialize():
|
|||
|
|
return False
|
|||
|
|
|
|||
|
|
# 对于FemtoBolt，校准主要是验证设备工作状态
|
|||
|
|
# 捕获几帧来确保设备稳定
|
|||
|
|
for i in range(10):
|
|||
|
|
capture = self.device_handle.get_capture()
|
|||
|
|
if capture:
|
|||
|
|
depth_image = capture.get_depth_image()
|
|||
|
|
if depth_image is not None:
|
|||
|
|
# 检查深度图像质量
|
|||
|
|
valid_pixels = np.sum((depth_image >= self.depth_range_min) &
|
|||
|
|
(depth_image <= self.depth_range_max))
|
|||
|
|
total_pixels = depth_image.size
|
|||
|
|
valid_ratio = valid_pixels / total_pixels
|
|||
|
|
|
|||
|
|
if valid_ratio > 0.1: # 至少10%的像素有效
|
|||
|
|
self.logger.info(f"校准帧 {i+1}: 有效像素比例 {valid_ratio:.2%}")
|
|||
|
|
else:
|
|||
|
|
self.logger.warning(f"校准帧 {i+1}: 有效像素比例过低 {valid_ratio:.2%}")
|
|||
|
|
|
|||
|
|
capture.release()
|
|||
|
|
else:
|
|||
|
|
self.logger.warning(f"校准时无法获取第{i+1}帧")
|
|||
|
|
|
|||
|
|
time.sleep(0.1)
|
|||
|
|
|
|||
|
|
self.logger.info("FemtoBolt校准完成")
|
|||
|
|
return True
|
|||
|
|
|
|||
|
|
except Exception as e:
|
|||
|
|
self.logger.error(f"FemtoBolt校准失败: {e}")
|
|||
|
|
return False
|
|||
|
|
|
|||
|
|
def start_streaming(self) -> bool:
|
|||
|
|
"""
|
|||
|
|
开始数据流推送
|
|||
|
|
|
|||
|
|
Returns:
|
|||
|
|
bool: 启动是否成功
|
|||
|
|
"""
|
|||
|
|
if self.is_streaming:
|
|||
|
|
self.logger.warning("FemtoBolt流已在运行")
|
|||
|
|
return True
|
|||
|
|
|
|||
|
|
if not self.is_connected:
|
|||
|
|
if not self.initialize():
|
|||
|
|
return False
|
|||
|
|
|
|||
|
|
try:
|
|||
|
|
self.is_streaming = True
|
|||
|
|
self.streaming_thread = threading.Thread(
|
|||
|
|
target=self._streaming_worker,
|
|||
|
|
name="FemtoBolt-Stream",
|
|||
|
|
daemon=True
|
|||
|
|
)
|
|||
|
|
self.streaming_thread.start()
|
|||
|
|
|
|||
|
|
self.logger.info("FemtoBolt流启动成功")
|
|||
|
|
return True
|
|||
|
|
|
|||
|
|
except Exception as e:
|
|||
|
|
self.logger.error(f"启动FemtoBolt流失败: {e}")
|
|||
|
|
self.is_streaming = False
|
|||
|
|
return False
|
|||
|
|
|
|||
|
|
def stop_streaming(self) -> bool:
|
|||
|
|
"""
|
|||
|
|
停止数据流推送
|
|||
|
|
|
|||
|
|
Returns:
|
|||
|
|
bool: 停止是否成功
|
|||
|
|
"""
|
|||
|
|
try:
|
|||
|
|
self.is_streaming = False
|
|||
|
|
|
|||
|
|
if self.streaming_thread and self.streaming_thread.is_alive():
|
|||
|
|
self.streaming_thread.join(timeout=5.0)
|
|||
|
|
|
|||
|
|
self.logger.info("FemtoBolt流已停止")
|
|||
|
|
return True
|
|||
|
|
|
|||
|
|
except Exception as e:
|
|||
|
|
self.logger.error(f"停止FemtoBolt流失败: {e}")
|
|||
|
|
return False
|
|||
|
|
|
|||
|
|
def _streaming_worker(self):
|
|||
|
|
"""
|
|||
|
|
流处理工作线程
|
|||
|
|
"""
|
|||
|
|
self.logger.info("FemtoBolt流工作线程启动")
|
|||
|
|
|
|||
|
|
frame_count = 0
|
|||
|
|
|
|||
|
|
try:
|
|||
|
|
while self.is_streaming:
|
|||
|
|
if self.device_handle and self._socketio:
|
|||
|
|
try:
|
|||
|
|
capture = self.device_handle.update()
|
|||
|
|
if capture is not None:
|
|||
|
|
ret, depth_image = capture.get_depth_image()
|
|||
|
|
if ret and depth_image is not None:
|
|||
|
|
|
|||
|
|
# 使用与device_manager.py相同的处理逻辑
|
|||
|
|
depth_image = depth_image.copy()
|
|||
|
|
|
|||
|
|
# === 生成灰色背景 + 白色网格 ===
|
|||
|
|
rows, cols = depth_image.shape[:2]
|
|||
|
|
background = np.ones((rows, cols, 3), dtype=np.uint8) * 128
|
|||
|
|
cell_size = 50
|
|||
|
|
grid_color = (255, 255, 255)
|
|||
|
|
grid_bg = np.zeros_like(background)
|
|||
|
|
for x in range(0, cols, cell_size):
|
|||
|
|
cv2.line(grid_bg, (x, 0), (x, rows), grid_color, 1)
|
|||
|
|
for y in range(0, rows, cell_size):
|
|||
|
|
cv2.line(grid_bg, (0, y), (cols, y), grid_color, 1)
|
|||
|
|
mask_grid = (grid_bg.sum(axis=2) > 0)
|
|||
|
|
background[mask_grid] = grid_bg[mask_grid]
|
|||
|
|
|
|||
|
|
# === 处理深度图满足区间的部分 ===
|
|||
|
|
depth_clipped = depth_image.copy()
|
|||
|
|
depth_clipped[depth_clipped < self.depth_range_min] = 0
|
|||
|
|
depth_clipped[depth_clipped > self.depth_range_max] = 0
|
|||
|
|
depth_normalized = np.clip(depth_clipped, self.depth_range_min, self.depth_range_max)
|
|||
|
|
depth_normalized = ((depth_normalized - self.depth_range_min) / (self.depth_range_max - self.depth_range_min) * 255).astype(np.uint8)
|
|||
|
|
|
|||
|
|
# 对比度和伽马校正
|
|||
|
|
alpha, beta, gamma = 1.5, 0, 0.8
|
|||
|
|
depth_normalized = cv2.convertScaleAbs(depth_normalized, alpha=alpha, beta=beta)
|
|||
|
|
lut = np.array([((i / 255.0) ** gamma) * 255 for i in range(256)]).astype("uint8")
|
|||
|
|
depth_normalized = cv2.LUT(depth_normalized, lut)
|
|||
|
|
|
|||
|
|
# 伪彩色
|
|||
|
|
depth_colored = cv2.applyColorMap(depth_normalized, cv2.COLORMAP_JET)
|
|||
|
|
|
|||
|
|
# 将有效深度覆盖到灰色背景上
|
|||
|
|
mask_valid = (depth_clipped > 0)
|
|||
|
|
for c in range(3):
|
|||
|
|
background[:, :, c][mask_valid] = depth_colored[:, :, c][mask_valid]
|
|||
|
|
|
|||
|
|
depth_colored_final = background
|
|||
|
|
|
|||
|
|
# 裁剪宽度
|
|||
|
|
height, width = depth_colored_final.shape[:2]
|
|||
|
|
target_width = height // 2
|
|||
|
|
if width > target_width:
|
|||
|
|
left = (width - target_width) // 2
|
|||
|
|
right = left + target_width
|
|||
|
|
depth_colored_final = depth_colored_final[:, left:right]
|
|||
|
|
|
|||
|
|
# 缓存图像
|
|||
|
|
self.last_depth_frame = depth_colored_final.copy()
|
|||
|
|
self.depth_frame_cache.append(depth_colored_final.copy())
|
|||
|
|
|
|||
|
|
# 推送SocketIO
|
|||
|
|
success, buffer = cv2.imencode('.jpg', depth_colored_final, [int(cv2.IMWRITE_JPEG_QUALITY), 80])
|
|||
|
|
if success and self._socketio:
|
|||
|
|
jpg_as_text = base64.b64encode(buffer).decode('utf-8')
|
|||
|
|
# 发送到femtobolt命名空间，使用前端期望的事件名和数据格式
|
|||
|
|
self._socketio.emit('femtobolt_frame', {
|
|||
|
|
'depth_image': jpg_as_text,
|
|||
|
|
'frame_count': frame_count,
|
|||
|
|
'timestamp': time.time(),
|
|||
|
|
'fps': self.actual_fps,
|
|||
|
|
'device_id': self.device_id,
|
|||
|
|
'depth_range': {
|
|||
|
|
'min': self.depth_range_min,
|
|||
|
|
'max': self.depth_range_max
|
|||
|
|
}
|
|||
|
|
}, namespace='/femtobolt')
|
|||
|
|
frame_count += 1
|
|||
|
|
|
|||
|
|
# 更新统计
|
|||
|
|
self._update_statistics()
|
|||
|
|
else:
|
|||
|
|
time.sleep(0.01)
|
|||
|
|
else:
|
|||
|
|
time.sleep(0.01)
|
|||
|
|
|
|||
|
|
except Exception as e:
|
|||
|
|
self.logger.error(f'FemtoBolt帧推送失败: {e}')
|
|||
|
|
time.sleep(0.1)
|
|||
|
|
|
|||
|
|
time.sleep(1/30) # 30 FPS
|
|||
|
|
|
|||
|
|
except Exception as e:
|
|||
|
|
self.logger.error(f"FemtoBolt流处理异常: {e}")
|
|||
|
|
finally:
|
|||
|
|
self.is_streaming = False
|
|||
|
|
self.logger.info("FemtoBolt流工作线程结束")
|
|||
|
|
|
|||
|
|
def _process_depth_image(self, depth_image) -> np.ndarray:
|
|||
|
|
"""
|
|||
|
|
处理深度图像
|
|||
|
|
|
|||
|
|
Args:
|
|||
|
|
depth_image: 原始深度图像
|
|||
|
|
|
|||
|
|
Returns:
|
|||
|
|
np.ndarray: 处理后的深度图像
|
|||
|
|
"""
|
|||
|
|
try:
|
|||
|
|
# 确保输入是numpy数组
|
|||
|
|
if not isinstance(depth_image, np.ndarray):
|
|||
|
|
self.logger.error(f"输入的深度图像不是numpy数组: {type(depth_image)}")
|
|||
|
|
return np.zeros((480, 640, 3), dtype=np.uint8)
|
|||
|
|
|
|||
|
|
# 深度范围过滤
|
|||
|
|
mask = (depth_image >= self.depth_range_min) & (depth_image <= self.depth_range_max)
|
|||
|
|
filtered_depth = np.where(mask, depth_image, 0)
|
|||
|
|
|
|||
|
|
# 归一化到0-255
|
|||
|
|
if np.max(filtered_depth) > 0:
|
|||
|
|
normalized = ((filtered_depth - self.depth_range_min) /
|
|||
|
|
(self.depth_range_max - self.depth_range_min) * 255).astype(np.uint8)
|
|||
|
|
else:
|
|||
|
|
normalized = np.zeros_like(filtered_depth, dtype=np.uint8)
|
|||
|
|
|
|||
|
|
# 对比度增强
|
|||
|
|
enhanced = cv2.convertScaleAbs(normalized, alpha=self.contrast_factor, beta=0)
|
|||
|
|
|
|||
|
|
# 伽马校正
|
|||
|
|
gamma_corrected = np.power(enhanced / 255.0, self.gamma_value) * 255
|
|||
|
|
gamma_corrected = gamma_corrected.astype(np.uint8)
|
|||
|
|
|
|||
|
|
# 伪彩色映射
|
|||
|
|
if self.use_pseudo_color:
|
|||
|
|
colored = cv2.applyColorMap(gamma_corrected, cv2.COLORMAP_JET)
|
|||
|
|
else:
|
|||
|
|
colored = cv2.cvtColor(gamma_corrected, cv2.COLOR_GRAY2BGR)
|
|||
|
|
|
|||
|
|
return colored
|
|||
|
|
|
|||
|
|
except Exception as e:
|
|||
|
|
self.logger.error(f"处理深度图像失败: {e}")
|
|||
|
|
return np.zeros((480, 640, 3), dtype=np.uint8)
|
|||
|
|
|
|||
|
|
def _send_depth_data(self, depth_image: np.ndarray, color_image: Optional[np.ndarray] = None):
|
|||
|
|
"""
|
|||
|
|
发送深度数据
|
|||
|
|
|
|||
|
|
Args:
|
|||
|
|
depth_image: 深度图像
|
|||
|
|
color_image: 彩色图像（可选）
|
|||
|
|
"""
|
|||
|
|
try:
|
|||
|
|
# 压缩深度图像
|
|||
|
|
encode_param = [int(cv2.IMWRITE_JPEG_QUALITY), 85]
|
|||
|
|
_, depth_buffer = cv2.imencode('.jpg', depth_image, encode_param)
|
|||
|
|
depth_data = base64.b64encode(depth_buffer).decode('utf-8')
|
|||
|
|
|
|||
|
|
# 准备发送数据
|
|||
|
|
send_data = {
|
|||
|
|
'timestamp': time.time(),
|
|||
|
|
'frame_count': self.frame_count,
|
|||
|
|
'depth_image': depth_data,
|
|||
|
|
'fps': self.actual_fps,
|
|||
|
|
'device_id': self.device_id,
|
|||
|
|
'depth_range': {
|
|||
|
|
'min': self.depth_range_min,
|
|||
|
|
'max': self.depth_range_max
|
|||
|
|
},
|
|||
|
|
'last_update': time.strftime('%H:%M:%S')
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
# 添加彩色图像（如果有）
|
|||
|
|
if color_image is not None:
|
|||
|
|
_, color_buffer = cv2.imencode('.jpg', color_image, encode_param)
|
|||
|
|
color_data = base64.b64encode(color_buffer).decode('utf-8')
|
|||
|
|
send_data['color_image'] = color_data
|
|||
|
|
|
|||
|
|
# 发送到SocketIO
|
|||
|
|
self._socketio.emit('femtobolt_frame', send_data, namespace='/femtobolt')
|
|||
|
|
|
|||
|
|
except Exception as e:
|
|||
|
|
self.logger.error(f"发送深度数据失败: {e}")
|
|||
|
|
|
|||
|
|
def _update_statistics(self):
|
|||
|
|
"""
|
|||
|
|
更新性能统计
|
|||
|
|
"""
|
|||
|
|
self.frame_count += 1
|
|||
|
|
self.fps_counter += 1
|
|||
|
|
|
|||
|
|
# 每秒计算一次实际FPS
|
|||
|
|
current_time = time.time()
|
|||
|
|
if current_time - self.fps_start_time >= 1.0:
|
|||
|
|
self.actual_fps = self.fps_counter / (current_time - self.fps_start_time)
|
|||
|
|
self.fps_counter = 0
|
|||
|
|
self.fps_start_time = current_time
|
|||
|
|
|
|||
|
|
# 更新性能统计
|
|||
|
|
self.performance_stats.update({
|
|||
|
|
'frames_processed': self.frame_count,
|
|||
|
|
'actual_fps': round(self.actual_fps, 2),
|
|||
|
|
'dropped_frames': self.dropped_frames
|
|||
|
|
})
|
|||
|
|
|
|||
|
|
def _reconnect(self) -> bool:
|
|||
|
|
"""
|
|||
|
|
重新连接FemtoBolt设备
|
|||
|
|
|
|||
|
|
Returns:
|
|||
|
|
bool: 重连是否成功
|
|||
|
|
"""
|
|||
|
|
try:
|
|||
|
|
self._cleanup_device()
|
|||
|
|
time.sleep(2.0) # 等待设备释放
|
|||
|
|
return self.initialize()
|
|||
|
|
|
|||
|
|
except Exception as e:
|
|||
|
|
self.logger.error(f"FemtoBolt重连失败: {e}")
|
|||
|
|
return False
|
|||
|
|
|
|||
|
|
def get_status(self) -> Dict[str, Any]:
|
|||
|
|
"""
|
|||
|
|
获取设备状态
|
|||
|
|
|
|||
|
|
Returns:
|
|||
|
|
Dict[str, Any]: 设备状态信息
|
|||
|
|
"""
|
|||
|
|
status = super().get_status()
|
|||
|
|
status.update({
|
|||
|
|
'color_resolution': self.color_resolution,
|
|||
|
|
'depth_mode': self.depth_mode,
|
|||
|
|
'target_fps': self.fps,
|
|||
|
|
'actual_fps': self.actual_fps,
|
|||
|
|
'frame_count': self.frame_count,
|
|||
|
|
'dropped_frames': self.dropped_frames,
|
|||
|
|
'depth_range': f"{self.depth_range_min}-{self.depth_range_max}mm",
|
|||
|
|
'has_depth_frame': self.last_depth_frame is not None,
|
|||
|
|
'has_color_frame': self.last_color_frame is not None
|
|||
|
|
})
|
|||
|
|
return status
|
|||
|
|
|
|||
|
|
def capture_body_image(self, save_path: Optional[str] = None) -> Optional[np.ndarray]:
|
|||
|
|
"""
|
|||
|
|
捕获身体图像
|
|||
|
|
|
|||
|
|
Args:
|
|||
|
|
save_path: 保存路径（可选）
|
|||
|
|
|
|||
|
|
Returns:
|
|||
|
|
Optional[np.ndarray]: 捕获的图像，失败返回None
|
|||
|
|
"""
|
|||
|
|
try:
|
|||
|
|
if not self.is_connected or not self.device_handle:
|
|||
|
|
self.logger.error("FemtoBolt设备未连接")
|
|||
|
|
return None
|
|||
|
|
|
|||
|
|
capture = self.device_handle.get_capture()
|
|||
|
|
if not capture:
|
|||
|
|
self.logger.error("无法获取FemtoBolt捕获")
|
|||
|
|
return None
|
|||
|
|
|
|||
|
|
depth_image = capture.get_depth_image()
|
|||
|
|
if depth_image is None:
|
|||
|
|
self.logger.error("无法获取深度图像")
|
|||
|
|
capture.release()
|
|||
|
|
return None
|
|||
|
|
|
|||
|
|
# 处理深度图像
|
|||
|
|
processed_image = self._process_depth_image(depth_image)
|
|||
|
|
|
|||
|
|
if save_path:
|
|||
|
|
cv2.imwrite(save_path, processed_image)
|
|||
|
|
self.logger.info(f"身体图像已保存到: {save_path}")
|
|||
|
|
|
|||
|
|
capture.release()
|
|||
|
|
return processed_image
|
|||
|
|
|
|||
|
|
except Exception as e:
|
|||
|
|
self.logger.error(f"捕获身体图像异常: {e}")
|
|||
|
|
return None
|
|||
|
|
|
|||
|
|
def get_latest_depth_frame(self) -> Optional[np.ndarray]:
|
|||
|
|
"""
|
|||
|
|
获取最新深度帧
|
|||
|
|
|
|||
|
|
Returns:
|
|||
|
|
Optional[np.ndarray]: 最新深度帧，无帧返回None
|
|||
|
|
"""
|
|||
|
|
return self.last_depth_frame.copy() if self.last_depth_frame is not None else None
|
|||
|
|
|
|||
|
|
def get_latest_color_frame(self) -> Optional[np.ndarray]:
|
|||
|
|
"""
|
|||
|
|
获取最新彩色帧
|
|||
|
|
|
|||
|
|
Returns:
|
|||
|
|
Optional[np.ndarray]: 最新彩色帧，无帧返回None
|
|||
|
|
"""
|
|||
|
|
return self.last_color_frame.copy() if self.last_color_frame is not None else None
|
|||
|
|
|
|||
|
|
def collect_body_pose_data(self) -> Optional[Dict[str, Any]]:
|
|||
|
|
"""
|
|||
|
|
采集身体姿态数据（兼容原接口）
|
|||
|
|
|
|||
|
|
Returns:
|
|||
|
|
Optional[Dict[str, Any]]: 身体姿态数据
|
|||
|
|
"""
|
|||
|
|
# 这里可以集成姿态估计算法
|
|||
|
|
# 目前返回模拟数据
|
|||
|
|
if not self.last_depth_frame is not None:
|
|||
|
|
return None
|
|||
|
|
|
|||
|
|
# 模拟身体姿态数据
|
|||
|
|
mock_keypoints = [
|
|||
|
|
{'name': 'head', 'x': 320, 'y': 100, 'confidence': 0.9},
|
|||
|
|
{'name': 'neck', 'x': 320, 'y': 150, 'confidence': 0.8},
|
|||
|
|
{'name': 'left_shoulder', 'x': 280, 'y': 160, 'confidence': 0.7},
|
|||
|
|
{'name': 'right_shoulder', 'x': 360, 'y': 160, 'confidence': 0.7},
|
|||
|
|
{'name': 'left_hip', 'x': 300, 'y': 300, 'confidence': 0.6},
|
|||
|
|
{'name': 'right_hip', 'x': 340, 'y': 300, 'confidence': 0.6}
|
|||
|
|
]
|
|||
|
|
|
|||
|
|
return {
|
|||
|
|
'timestamp': time.time(),
|
|||
|
|
'keypoints': mock_keypoints,
|
|||
|
|
'balance_score': np.random.uniform(0.7, 0.9),
|
|||
|
|
'center_of_mass': {'x': 320, 'y': 240},
|
|||
|
|
'device_id': self.device_id
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
def _cleanup_device(self):
|
|||
|
|
"""
|
|||
|
|
清理设备资源
|
|||
|
|
"""
|
|||
|
|
try:
|
|||
|
|
if self.device_handle:
|
|||
|
|
# 尝试停止设备（如果有stop方法）
|
|||
|
|
if hasattr(self.device_handle, 'stop'):
|
|||
|
|
try:
|
|||
|
|
self.device_handle.stop()
|
|||
|
|
self.logger.info("FemtoBolt设备已停止")
|
|||
|
|
except Exception as e:
|
|||
|
|
self.logger.warning(f"停止FemtoBolt设备时出现警告: {e}")
|
|||
|
|
|
|||
|
|
# 尝试关闭设备（如果有close方法）
|
|||
|
|
if hasattr(self.device_handle, 'close'):
|
|||
|
|
try:
|
|||
|
|
self.device_handle.close()
|
|||
|
|
self.logger.info("FemtoBolt设备连接已关闭")
|
|||
|
|
except Exception as e:
|
|||
|
|
self.logger.warning(f"关闭FemtoBolt设备时出现警告: {e}")
|
|||
|
|
|
|||
|
|
self.device_handle = None
|
|||
|
|
|
|||
|
|
except Exception as e:
|
|||
|
|
self.logger.error(f"清理FemtoBolt设备失败: {e}")
|
|||
|
|
|
|||
|
|
def disconnect(self):
|
|||
|
|
"""
|
|||
|
|
断开FemtoBolt设备连接
|
|||
|
|
"""
|
|||
|
|
try:
|
|||
|
|
self.stop_streaming()
|
|||
|
|
self._cleanup_device()
|
|||
|
|
self.is_connected = False
|
|||
|
|
self.logger.info("FemtoBolt设备已断开连接")
|
|||
|
|
|
|||
|
|
except Exception as e:
|
|||
|
|
self.logger.error(f"断开FemtoBolt设备连接失败: {e}")
|
|||
|
|
|
|||
|
|
def cleanup(self):
|
|||
|
|
"""
|
|||
|
|
清理资源
|
|||
|
|
"""
|
|||
|
|
try:
|
|||
|
|
self.stop_streaming()
|
|||
|
|
self._cleanup_device()
|
|||
|
|
|
|||
|
|
self.depth_frame_cache.clear()
|
|||
|
|
self.color_frame_cache.clear()
|
|||
|
|
self.last_depth_frame = None
|
|||
|
|
self.last_color_frame = None
|
|||
|
|
|
|||
|
|
super().cleanup()
|
|||
|
|
self.logger.info("FemtoBolt资源清理完成")
|
|||
|
|
|
|||
|
|
except Exception as e:
|
|||
|
|
self.logger.error(f"清理FemtoBolt资源失败: {e}")
|