1103 lines
48 KiB
Python
1103 lines
48 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
|
||
from matplotlib.colors import LinearSegmentedColormap
|
||
import matplotlib.pyplot as plt
|
||
import matplotlib
|
||
from scipy import ndimage
|
||
from scipy.interpolate import griddata
|
||
import io
|
||
import importlib
|
||
from typing import Optional
|
||
import subprocess
|
||
import tempfile
|
||
|
||
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
|
||
# 新增：记录已使用的k4a.dll路径，供子进程探测使用
|
||
self.k4a_dll_path: Optional[str] = None
|
||
|
||
# 设备配置
|
||
self.algorithm_type = self.config.get('algorithm_type', 'opencv')
|
||
self.color_resolution = self.config.get('color_resolution', '1080P')
|
||
self.depth_mode = self.config.get('depth_mode', 'NFOV_2X2BINNED')
|
||
self.color_format = self.config.get('color_format', 'COLOR_BGRA32')
|
||
self.fps = self.config.get('camera_fps', 20)
|
||
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.background = None
|
||
self.output_buffer = None
|
||
self._depth_filtered = None # 用于复用深度图过滤结果
|
||
self._blur_buffer = None # 用于复用高斯模糊结果
|
||
self._current_gamma = None
|
||
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.send_fps = self.config.get('send_fps', 15) # 默认20FPS发送
|
||
self._min_send_interval = 1.0 / self.send_fps if self.send_fps > 0 else 0.05
|
||
self._last_send_time = 0
|
||
|
||
# 编码参数缓存（避免每帧创建数组）- 为plt模式优化编码质量
|
||
jpeg_quality = 40 if self.algorithm_type == 'plt' else int(self.config.get('jpeg_quality', 60))
|
||
self._encode_param = [int(cv2.IMWRITE_JPEG_QUALITY), jpeg_quality]
|
||
|
||
# 预计算伽马LUT（避免每帧计算）
|
||
self._gamma_lut = None
|
||
self._current_gamma = None
|
||
self._update_gamma_lut()
|
||
|
||
# 预分配缓冲区和变量（减少内存分配）
|
||
self._grid_background = None
|
||
self._reusable_buffer = None
|
||
self._crop_params = None # 缓存裁剪参数
|
||
|
||
# 帧预测和插值机制
|
||
self._last_frame = None
|
||
self._frame_history = deque(maxlen=3) # 保存最近3帧用于预测
|
||
self._prediction_enabled = True if self.algorithm_type == 'plt' else False
|
||
self._grid_bg = None
|
||
self._grid_size = (480, 640) # 默认尺寸
|
||
self.background = None # 用于缓存等高线渲染的背景
|
||
|
||
# 自定义彩虹色 colormap（参考testfemtobolt.py）
|
||
colors = [
|
||
'fuchsia', 'red', 'yellow', 'lime', 'cyan', 'blue',
|
||
'fuchsia', 'red', 'yellow', 'lime', 'cyan', 'blue',
|
||
'fuchsia', 'red', 'yellow', 'lime', 'cyan', 'blue',
|
||
'fuchsia', 'red', 'yellow', 'lime', 'cyan', 'blue',
|
||
'fuchsia', 'red', 'yellow', 'lime', 'cyan', 'blue',
|
||
'fuchsia', 'red', 'yellow', 'lime', 'cyan', 'blue']
|
||
self.custom_cmap = LinearSegmentedColormap.from_list("custom_cmap", colors)
|
||
|
||
# 设置matplotlib为非交互模式
|
||
matplotlib.use('Agg')
|
||
|
||
# 创建matplotlib图形对象（复用以提高性能）
|
||
self.fig, self.ax = plt.subplots(figsize=(9, 6))
|
||
self.ax.set_aspect('equal')
|
||
plt.subplots_adjust(left=0, right=1, top=1, bottom=0)
|
||
self.logger.info(f"FemtoBolt设备配置完成 - 算法类型: {self.algorithm_type}, 深度模式: {self.depth_mode}, FPS: {self.fps}")
|
||
|
||
|
||
def _update_gamma_lut(self):
|
||
"""更新伽马校正查找表"""
|
||
if self._current_gamma != self.gamma_value:
|
||
self._gamma_lut = np.array([((i / 255.0) ** (1.0 / self.gamma_value)) * 255 for i in range(256)], dtype=np.uint8)
|
||
self._current_gamma = self.gamma_value
|
||
|
||
def _generate_contour_image_opencv(self, depth):
|
||
"""改进版 OpenCV 等高线渲染，梯度平滑、局部对比增强"""
|
||
try:
|
||
# 初始化 depth_filtered 缓冲区
|
||
if self._depth_filtered is None or self._depth_filtered.shape != depth.shape:
|
||
self._depth_filtered = np.zeros_like(depth, dtype=np.uint16)
|
||
|
||
np.copyto(self._depth_filtered, depth) # 直接覆盖，不生成新数组
|
||
depth_filtered = self._depth_filtered
|
||
depth_filtered[depth_filtered > self.depth_range_max] = 0
|
||
depth_filtered[depth_filtered < self.depth_range_min] = 0
|
||
height, width = depth_filtered.shape
|
||
|
||
# 背景缓存
|
||
if self.background is None or self.background.shape[:2] != (height, width):
|
||
background_gray = int(0.5 * 255 * 0.3 + 255 * (1 - 0.3))
|
||
self.background = np.ones((height, width, 3), dtype=np.uint8) * background_gray
|
||
grid_spacing = max(height // 20, width // 20, 10)
|
||
for x in range(0, width, grid_spacing):
|
||
cv2.line(self.background, (x, 0), (x, height-1), (255, 255, 255), 1)
|
||
for y in range(0, height, grid_spacing):
|
||
cv2.line(self.background, (0, y), (width-1, y), (255, 255, 255), 1)
|
||
|
||
# 初始化输出缓存和模糊缓存
|
||
self.output_buffer = np.empty_like(self.background)
|
||
self._blur_buffer = np.empty_like(self.background)
|
||
|
||
# 复用输出缓存，避免 copy()
|
||
np.copyto(self.output_buffer, self.background)
|
||
output = self.output_buffer
|
||
valid_mask = depth_filtered > 0
|
||
|
||
if np.any(valid_mask):
|
||
# 连续归一化深度值
|
||
norm_depth = np.zeros_like(depth_filtered, dtype=np.float32)
|
||
norm_depth[valid_mask] = (depth_filtered[valid_mask] - self.depth_range_min) / (self.depth_range_max - self.depth_range_min)
|
||
norm_depth = np.clip(norm_depth, 0, 1) ** 0.8 # Gamma增强
|
||
|
||
# 使用 colormap 映射
|
||
cmap_colors = (self.custom_cmap(norm_depth)[..., :3] * 255).astype(np.uint8)
|
||
output[valid_mask] = cmap_colors[valid_mask]
|
||
|
||
# Sobel 边界检测 + cv2.magnitude 替换 np.hypot
|
||
depth_uint8 = (norm_depth * 255).astype(np.uint8)
|
||
gx = cv2.Sobel(depth_uint8, cv2.CV_32F, 1, 0, ksize=3)
|
||
gy = cv2.Sobel(depth_uint8, cv2.CV_32F, 0, 1, ksize=3)
|
||
grad_mag = cv2.magnitude(gx, gy)
|
||
grad_mag = grad_mag.astype(np.uint8)
|
||
|
||
# 自适应局部对比度增强（向量化）
|
||
edge_mask = grad_mag > 30
|
||
output[edge_mask] = np.clip(output[edge_mask].astype(np.float32) * 1.5, 0, 255).astype(np.uint8)
|
||
|
||
# 高斯平滑，复用 dst 缓冲区
|
||
cv2.GaussianBlur(output, (3, 3), 0.3, dst=self._blur_buffer)
|
||
|
||
# 注意：这里不进行裁剪，而是返回完整图像
|
||
# 推迟裁剪到显示阶段，与 testfemtobolt.py 保持一致
|
||
# 原代码在这里进行了裁剪：
|
||
# target_width = height // 2
|
||
# if width > target_width:
|
||
# left = (width - target_width) // 2
|
||
# right = left + target_width
|
||
# output = output[:, left:right]
|
||
return self._blur_buffer
|
||
|
||
except Exception as e:
|
||
self.logger.error(f"优化等高线生成失败: {e}")
|
||
return None
|
||
|
||
|
||
|
||
def _generate_contour_image_plt(self, depth):
|
||
"""使用matplotlib生成等高线图像（性能优化版本）"""
|
||
|
||
try:
|
||
# 设置图形背景色和边距
|
||
self.fig.patch.set_facecolor((50/255, 50/255, 50/255)) # 设置深灰色背景 rgb(50, 50, 50)
|
||
self.fig.tight_layout(pad=0) # 移除所有边距
|
||
|
||
# 清除之前的绘图
|
||
self.ax.clear()
|
||
self.ax.set_facecolor((50/255, 50/255, 50/255)) # 设置坐标区域背景色为黑色
|
||
|
||
# 深度数据过滤（与display_x.py完全一致）
|
||
depth[depth > self.depth_range_max] = 0
|
||
depth[depth < self.depth_range_min] = 0
|
||
|
||
# 背景图（深灰色背景）
|
||
# 创建RGB格式的背景图，确保颜色准确性
|
||
background_gray_value = 50 # RGB(50, 50, 50)
|
||
background = np.full((*depth.shape, 3), background_gray_value, dtype=np.uint8)
|
||
|
||
# 使用 np.ma.masked_equal() 来屏蔽深度图中的零值
|
||
depth = np.ma.masked_equal(depth, 0)
|
||
|
||
# 绘制背景（不使用colormap，直接显示RGB图像）
|
||
self.ax.imshow(background, origin='lower', alpha=1.0)
|
||
|
||
# 优化：简化网格绘制，减少绘制操作，使用深灰色网格以适应黑色背景
|
||
self.ax.grid(True, which='major', axis='both', color='#333333', linestyle='-', linewidth=0.5, zorder=0)
|
||
|
||
# 隐藏坐标轴
|
||
self.ax.set_xticks([])
|
||
self.ax.set_yticks([])
|
||
|
||
# 优化：减少等高线级别从100到30，大幅提升性能
|
||
start_time = time.perf_counter()
|
||
self.ax.contourf(depth, levels=80, cmap=self.custom_cmap, vmin=self.depth_range_min, vmax=self.depth_range_max, origin='upper', zorder=2)
|
||
# 优化：直接从canvas获取图像数据，避免PNG编码/解码
|
||
try:
|
||
|
||
# 绘制到canvas
|
||
self.fig.canvas.draw()
|
||
# 兼容不同版本的matplotlib获取图像数据
|
||
canvas_width, canvas_height = self.fig.canvas.get_width_height()
|
||
# 尝试使用新版本的方法
|
||
try:
|
||
# matplotlib 3.8+ 使用 buffer_rgba()
|
||
buf = np.frombuffer(self.fig.canvas.buffer_rgba(), dtype=np.uint8)
|
||
buf = buf.reshape((canvas_height, canvas_width, 4))
|
||
# 转换RGBA到RGB
|
||
img = cv2.cvtColor(buf, cv2.COLOR_RGBA2BGR)
|
||
except AttributeError:
|
||
try:
|
||
# matplotlib 3.0-3.7 使用 tostring_argb()
|
||
buf = np.frombuffer(self.fig.canvas.tostring_argb(), dtype=np.uint8)
|
||
buf = buf.reshape((canvas_height, canvas_width, 4))
|
||
# 转换ARGB到BGR
|
||
img = cv2.cvtColor(buf, cv2.COLOR_BGRA2BGR)
|
||
except AttributeError:
|
||
# 旧版本matplotlib使用 tostring_rgb()
|
||
buf = np.frombuffer(self.fig.canvas.tostring_rgb(), dtype=np.uint8)
|
||
buf = buf.reshape((canvas_height, canvas_width, 3))
|
||
# 转换RGB到BGR
|
||
img = cv2.cvtColor(buf, cv2.COLOR_RGB2BGR)
|
||
return img
|
||
except Exception as e:
|
||
self.logger.error(f"Canvas图像转换失败: {e}")
|
||
# 降级到PNG方法
|
||
return None
|
||
|
||
except Exception as e:
|
||
self.logger.error(f"生成等高线图像失败: {e}")
|
||
return None
|
||
|
||
def _generate_contour_image_source(self, depth):
|
||
"""直接读取深度相机图片数据，不做处理返回深度相机图片（用于对比分析原始图像的延时情况）"""
|
||
generation_start = time.perf_counter()
|
||
|
||
try:
|
||
# 将深度数据转换为可视化图像
|
||
# 深度数据通常是单通道的，需要转换为3通道BGR格式
|
||
if len(depth.shape) == 2:
|
||
# 深度数据过滤（与其他方法保持一致）
|
||
depth_filtered = depth.copy()
|
||
depth_filtered[depth_filtered > self.depth_range_max] = 0
|
||
depth_filtered[depth_filtered < self.depth_range_min] = 0
|
||
|
||
# 将深度数据归一化到0-255范围
|
||
depth_normalized = cv2.normalize(depth_filtered, None, 0, 255, cv2.NORM_MINMAX, dtype=cv2.CV_8U)
|
||
|
||
# 应用彩色映射创建彩色深度图
|
||
# 使用JET色彩映射：蓝色(近)到红色(远)
|
||
img_colored = cv2.applyColorMap(depth_normalized, cv2.COLORMAP_JET)
|
||
|
||
# 将零值区域设为黑色（无效深度）
|
||
mask = (depth_filtered == 0)
|
||
img_colored[mask] = [0, 0, 0] # 黑色背景
|
||
|
||
img = img_colored
|
||
else:
|
||
# 如果已经是3通道，直接使用
|
||
img = depth.astype(np.uint8)
|
||
|
||
return img
|
||
|
||
except Exception as e:
|
||
self.logger.error(f"生成原始深度图像失败: {e}")
|
||
return None
|
||
|
||
|
||
|
||
def initialize(self) -> bool:
|
||
"""
|
||
初始化FemtoBolt设备
|
||
|
||
Returns:
|
||
bool: 初始化是否成功
|
||
"""
|
||
try:
|
||
self.logger.info("正在初始化FemtoBolt设备...")
|
||
|
||
# 使用构造函数中已加载的配置，避免并发读取配置文件
|
||
self.logger.info(f"使用已加载配置: algorithm_type={self.algorithm_type}, fps={self.fps}, depth_mode={self.depth_mode}")
|
||
|
||
# 初始化SDK
|
||
if not self._initialize_sdk():
|
||
raise Exception("SDK初始化失败")
|
||
|
||
# 配置设备
|
||
if not self._configure_device():
|
||
raise Exception("设备配置失败")
|
||
|
||
# 启动设备
|
||
if not self._start_device():
|
||
raise Exception("设备启动失败")
|
||
|
||
# 使用set_connected方法启动连接监控线程
|
||
self.set_connected(True)
|
||
self.device_info.update({
|
||
'color_resolution': self.color_resolution,
|
||
'depth_mode': self.depth_mode,
|
||
'camera_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}")
|
||
# 使用set_connected方法停止连接监控线程
|
||
self.set_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.error(f"无法导入pykinect_azure库: {e}")
|
||
self.sdk_initialized = False
|
||
return False
|
||
|
||
# 查找并初始化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.error('未找到真实SDK，初始化失败')
|
||
self.sdk_initialized = False
|
||
return False
|
||
|
||
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", "k4a.dll")
|
||
self.logger.info(f"FemtoBolt SDK路径: {dll_path}")
|
||
sdk_paths.append(dll_path)
|
||
return sdk_paths
|
||
|
||
|
||
|
||
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_2X2BINNED
|
||
self.femtobolt_config.color_format = self.pykinect.K4A_IMAGE_FORMAT_COLOR_BGRA32
|
||
self.femtobolt_config.color_resolution = self.pykinect.K4A_COLOR_RESOLUTION_720P
|
||
self.femtobolt_config.camera_fps = self.pykinect.K4A_FRAMES_PER_SECOND_30 # 30FPS
|
||
self.femtobolt_config.synchronized_images_only = False
|
||
|
||
return True
|
||
|
||
except Exception as e:
|
||
self.logger.error(f"FemtoBolt设备配置失败: {e}")
|
||
return False
|
||
|
||
|
||
def _start_device(self) -> bool:
|
||
"""
|
||
启动FemtoBolt设备
|
||
|
||
Returns:
|
||
bool: 启动是否成功
|
||
"""
|
||
try:
|
||
if not self.pykinect:
|
||
return False
|
||
|
||
# 通过探测后再真正启动
|
||
# 在真正调用 start_device 之前，先通过 k4a.dll 查询已安装设备数量，0 则跳过
|
||
try:
|
||
import os, ctypes
|
||
k4a_path = getattr(self, "k4a_dll_path", None)
|
||
if not k4a_path:
|
||
base_dir = os.path.dirname(os.path.abspath(__file__))
|
||
k4a_path = os.path.normpath(os.path.join(base_dir, "..", "dll", "femtobolt", "k4a.dll"))
|
||
k4a = ctypes.CDLL(k4a_path)
|
||
try:
|
||
# 有些环境需要显式声明返回类型
|
||
k4a.k4a_device_get_installed_count.restype = ctypes.c_uint32
|
||
except Exception:
|
||
pass
|
||
device_count = int(k4a.k4a_device_get_installed_count())
|
||
except Exception as e:
|
||
self.logger.warning(f"获取FemtoBolt设备数量失败，跳过启动: {e}")
|
||
return False
|
||
|
||
if device_count <= 0:
|
||
self.logger.warning("未检测到FemtoBolt深度相机，跳过启动")
|
||
return False
|
||
else:
|
||
self.logger.info(f"检测到 FemtoBolt 设备数量: {device_count}")
|
||
|
||
self.device_handle = self.pykinect.start_device(config=self.femtobolt_config)
|
||
if self.device_handle:
|
||
self.logger.info('✓ FemtoBolt深度相机初始化成功!')
|
||
else:
|
||
self.logger.warning('FemtoBolt设备启动返回None，设备可能未连接')
|
||
return False
|
||
|
||
|
||
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
|
||
|
||
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.logger.info("等待FemtoBolt流线程结束...")
|
||
self.streaming_thread.join(timeout=3.0)
|
||
if self.streaming_thread.is_alive():
|
||
self.logger.warning("FemtoBolt流线程未能在超时时间内结束")
|
||
else:
|
||
self.logger.info("FemtoBolt流工作线程结束")
|
||
|
||
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
|
||
last_generation_time = 0.0
|
||
skip_frame_count = 0
|
||
|
||
# 根据算法类型设置自适应采样推流频率
|
||
if self.algorithm_type == 'source':
|
||
# source算法：图像生成时间很短，提高推流频率以减少延时
|
||
adaptive_fps = 25 # 提高到25fps，接近Camera的30fps
|
||
self.logger.info(f"FemtoBolt使用source算法，设置推流频率为{adaptive_fps}fps（优化延时）")
|
||
elif self.algorithm_type == 'plt':
|
||
# plt算法：图像生成时间约0.3-0.4秒，设置较低fps推流以减少延时
|
||
adaptive_fps = 5
|
||
self.logger.info(f"FemtoBolt使用plt算法，设置推流频率为{adaptive_fps}fps（优化延时）")
|
||
elif self.algorithm_type == 'opencv':
|
||
# opencv算法：图像生成时间约0.006秒，设置高fps推流
|
||
adaptive_fps = 20
|
||
self.logger.info(f"FemtoBolt使用opencv算法，设置推流频率为{adaptive_fps}fps")
|
||
else:
|
||
# 默认使用配置文件中的设置
|
||
adaptive_fps = self.send_fps
|
||
self.logger.info(f"FemtoBolt使用默认推流频率{adaptive_fps}fps")
|
||
|
||
# 计算自适应最小发送间隔
|
||
adaptive_min_interval = 1.0 / adaptive_fps if adaptive_fps > 0 else 0.05
|
||
|
||
try:
|
||
while self.is_streaming:
|
||
# 动态调整发送间隔，避免延时累积
|
||
now = time.time()
|
||
|
||
# 对于source算法，简化间隔计算，减少延时
|
||
if self.algorithm_type == 'source':
|
||
# source算法处理时间很短，使用固定间隔
|
||
dynamic_interval = adaptive_min_interval
|
||
elif self.algorithm_type == 'plt' and last_generation_time > 0:
|
||
# 根据实际生成时间动态调整间隔，避免延时累积
|
||
dynamic_interval = max(adaptive_min_interval, last_generation_time * 0.8)
|
||
else:
|
||
dynamic_interval = adaptive_min_interval
|
||
|
||
if now - self._last_send_time < dynamic_interval:
|
||
time.sleep(0.001)
|
||
continue
|
||
|
||
if self.device_handle and self._socketio:
|
||
try:
|
||
capture = self.device_handle.update()
|
||
if capture is not None:
|
||
try:
|
||
ret, depth_image = capture.get_depth_image()
|
||
if ret and depth_image is not None:
|
||
|
||
# 对于source算法，跳过复杂的跳帧逻辑，直接处理
|
||
if self.algorithm_type == 'source':
|
||
# source算法处理时间很短，不需要跳帧
|
||
pass
|
||
elif self.algorithm_type == 'plt':
|
||
# 对于plt算法，如果延时累积过多，跳过部分帧
|
||
time_since_last = now - self._last_send_time
|
||
if time_since_last > adaptive_min_interval * 2:
|
||
skip_frame_count += 1
|
||
if skip_frame_count % 2 == 0: # 每2帧跳过1帧
|
||
# 如果启用帧预测，发送预测帧而不是跳过
|
||
if self._prediction_enabled and self._last_frame is not None:
|
||
self._send_predicted_frame(frame_count, now)
|
||
frame_count += 1
|
||
continue
|
||
|
||
# 测试：记录图像生成开始时间
|
||
generation_start_time = time.time()
|
||
|
||
# 根据配置选择不同的等高线生成方法
|
||
if self.algorithm_type == 'source':
|
||
depth_colored_final = self._generate_contour_image_source(depth_image)
|
||
elif self.algorithm_type == 'plt':
|
||
depth_colored_final = self._generate_contour_image_plt(depth_image)
|
||
elif self.algorithm_type == 'opencv':
|
||
depth_colored_final = self._generate_contour_image_opencv(depth_image)
|
||
|
||
# 测试：计算并打印图像生成时间
|
||
generation_time = time.time() - generation_start_time
|
||
last_generation_time = generation_time
|
||
# print(f"[FemtoBolt] 帧图像生成时间: {generation_time:.4f}秒 ({self.algorithm_type}算法) - 跳帧数: {skip_frame_count}")
|
||
|
||
if depth_colored_final is None:
|
||
# 如果等高线生成失败，跳过这一帧
|
||
continue
|
||
|
||
# 优化裁剪处理（缓存裁剪参数）
|
||
h, w = depth_colored_final.shape[:2]
|
||
# self.logger.info(f"深度图像尺寸: 宽={w}, 高={h}")
|
||
target_width = h // 2
|
||
|
||
# 缓存裁剪参数，避免重复计算
|
||
if self._crop_params is None or self._crop_params[0] != (h, w):
|
||
if w > target_width:
|
||
left = (w - target_width) // 2
|
||
right = left + target_width
|
||
self._crop_params = ((h, w), left, right, target_width)
|
||
else:
|
||
self._crop_params = ((h, w), None, None, w)
|
||
|
||
# 应用裁剪
|
||
if self._crop_params[1] is not None:
|
||
display_image = depth_colored_final[:, self._crop_params[1]:self._crop_params[2]]
|
||
else:
|
||
display_image = depth_colored_final
|
||
|
||
# 优化编码：使用更低的质量和更快的编码
|
||
encode_start = time.time()
|
||
success, buffer = cv2.imencode('.jpg', display_image, self._encode_param)
|
||
encode_time = time.time() - encode_start
|
||
|
||
if success and self._socketio:
|
||
# 优化base64编码：直接使用memoryview避免额外拷贝
|
||
base64_start = time.time()
|
||
jpg_as_text = base64.b64encode(memoryview(buffer)).decode('utf-8')
|
||
base64_time = time.time() - base64_start
|
||
|
||
# 添加编码时间监控
|
||
if encode_time > 0.01 or base64_time > 0.01: # 超过10ms记录
|
||
print(f"[FemtoBolt] 编码时间: JPEG={encode_time:.4f}s, Base64={base64_time:.4f}s")
|
||
|
||
self._socketio.emit('femtobolt_frame', {
|
||
'depth_image': jpg_as_text,
|
||
'frame_count': frame_count,
|
||
'timestamp': now,
|
||
'fps': self.actual_fps,
|
||
'device_id': self.device_id,
|
||
'depth_range': {
|
||
'min': self.depth_range_min,
|
||
'max': self.depth_range_max
|
||
}
|
||
}, namespace='/devices')
|
||
frame_count += 1
|
||
self._last_send_time = now
|
||
|
||
# 更新帧历史用于预测（仅对需要预测的算法）
|
||
if self._prediction_enabled and self.algorithm_type in ['plt', 'opencv']:
|
||
# 优化内存管理：重用缓冲区
|
||
if self._reusable_buffer is None or self._reusable_buffer.shape != display_image.shape:
|
||
self._reusable_buffer = np.empty_like(display_image)
|
||
|
||
# 使用预分配的缓冲区复制图像
|
||
np.copyto(self._reusable_buffer, display_image)
|
||
self._last_frame = self._reusable_buffer
|
||
|
||
# 限制历史帧数量，减少内存占用
|
||
frame_data = {
|
||
'image': display_image.copy(), # 这里仍需要复制，因为display_image可能被修改
|
||
'timestamp': now,
|
||
'depth_range': {
|
||
'min': self.depth_range_min,
|
||
'max': self.depth_range_max
|
||
}
|
||
}
|
||
self._frame_history.append(frame_data)
|
||
|
||
# 主动触发垃圾回收（仅在必要时）
|
||
if frame_count % 100 == 0: # 每100帧触发一次
|
||
gc.collect()
|
||
else:
|
||
time.sleep(0.005)
|
||
except Exception as e:
|
||
# 捕获处理过程中出现异常，记录并继续
|
||
self.logger.error(f"FemtoBolt捕获处理错误: {e}")
|
||
finally:
|
||
# 无论处理成功与否，都应释放capture以回收内存:contentReference[oaicite:3]{index=3}
|
||
try:
|
||
if hasattr(capture, 'release'):
|
||
capture.release()
|
||
except Exception:
|
||
pass
|
||
else:
|
||
time.sleep(0.001)
|
||
|
||
except Exception as e:
|
||
self.logger.error(f'FemtoBolt帧推送失败: {e}')
|
||
time.sleep(0.05)
|
||
|
||
# 降低空转CPU
|
||
time.sleep(0.001)
|
||
|
||
except Exception as e:
|
||
self.logger.error(f"FemtoBolt流处理异常: {e}")
|
||
finally:
|
||
self.is_streaming = False
|
||
self.logger.info("FemtoBolt流工作线程结束")
|
||
|
||
def _send_predicted_frame(self, frame_count: int, timestamp: float):
|
||
"""
|
||
发送预测帧以减少延时感知
|
||
|
||
Args:
|
||
frame_count: 帧计数
|
||
timestamp: 时间戳
|
||
"""
|
||
try:
|
||
if self._last_frame is None:
|
||
return
|
||
|
||
# 简单的帧预测：使用最后一帧
|
||
predicted_frame = self._last_frame
|
||
|
||
# 如果有足够的历史帧，可以进行简单的运动预测
|
||
if len(self._frame_history) >= 2:
|
||
# 这里可以实现更复杂的预测算法
|
||
# 目前使用最新帧作为预测帧
|
||
predicted_frame = self._frame_history[-1]['image']
|
||
|
||
# 编码并发送预测帧
|
||
success, buffer = cv2.imencode('.jpg', predicted_frame, self._encode_param)
|
||
if success and self._socketio:
|
||
jpg_as_text = base64.b64encode(memoryview(buffer)).decode('utf-8')
|
||
self._socketio.emit('femtobolt_frame', {
|
||
'depth_image': jpg_as_text,
|
||
'frame_count': frame_count,
|
||
'timestamp': timestamp,
|
||
'fps': self.actual_fps,
|
||
'device_id': self.device_id,
|
||
'predicted': True, # 标记为预测帧
|
||
'depth_range': {
|
||
'min': self.depth_range_min,
|
||
'max': self.depth_range_max
|
||
}
|
||
}, namespace='/devices')
|
||
self._last_send_time = timestamp
|
||
|
||
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 _cleanup_device(self):
|
||
"""
|
||
清理设备资源
|
||
"""
|
||
try:
|
||
if self.device_handle:
|
||
# 先停止Pipeline以释放设备资源
|
||
if hasattr(self, 'pipeline') and self.pipeline:
|
||
try:
|
||
self.logger.info("正在停止FemtoBolt Pipeline...")
|
||
self.pipeline.stop()
|
||
self.logger.info("FemtoBolt Pipeline已停止")
|
||
# 等待Pipeline完全释放资源
|
||
time.sleep(0.5)
|
||
except Exception as e:
|
||
self.logger.warning(f"停止Pipeline时出现警告: {e}")
|
||
finally:
|
||
self.pipeline = None
|
||
|
||
# 尝试停止设备（如果有stop方法）
|
||
if hasattr(self.device_handle, 'stop'):
|
||
try:
|
||
self.device_handle.stop()
|
||
self.logger.info("FemtoBolt设备已停止")
|
||
# 等待设备完全停止
|
||
time.sleep(0.3)
|
||
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设备连接已关闭")
|
||
# 等待设备连接完全关闭
|
||
time.sleep(0.2)
|
||
except Exception as e:
|
||
self.logger.warning(f"关闭FemtoBolt设备时出现警告: {e}")
|
||
|
||
self.device_handle = None
|
||
|
||
except Exception as e:
|
||
self.logger.error(f"清理FemtoBolt设备失败: {e}")
|
||
finally:
|
||
# 确保所有相关属性都被重置
|
||
self.pipeline = None
|
||
self.device_handle = None
|
||
|
||
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 reload_config(self) -> bool:
|
||
"""
|
||
重新加载设备配置
|
||
|
||
Returns:
|
||
bool: 重新加载是否成功
|
||
"""
|
||
try:
|
||
self.logger.info("正在重新加载FemtoBolt配置...")
|
||
|
||
|
||
# 获取最新配置
|
||
self.config = self.config_manager.get_device_config('femtobolt')
|
||
|
||
# 更新配置属性
|
||
self.algorithm_type = self.config.get('algorithm_type', 'opencv')
|
||
self.color_resolution = self.config.get('color_resolution', '1080P')
|
||
self.depth_mode = self.config.get('depth_mode', 'NFOV_2X2BINNED')
|
||
self.color_format = self.config.get('color_format', 'COLOR_BGRA32')
|
||
self.fps = self.config.get('camera_fps', 20)
|
||
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.contrast_factor = self.config.get('contrast_factor', 1.2)
|
||
self.gamma_value = self.config.get('gamma_value', 0.8)
|
||
self.use_pseudo_color = self.config.get('use_pseudo_color', True)
|
||
|
||
# 更新缓存队列大小
|
||
cache_size = self.config.get('frame_cache_size', 10)
|
||
if cache_size != self.depth_frame_cache.maxlen:
|
||
self.depth_frame_cache = deque(maxlen=cache_size)
|
||
self.color_frame_cache = deque(maxlen=cache_size)
|
||
|
||
# 更新gamma查找表
|
||
self._update_gamma_lut()
|
||
|
||
self.logger.info(f"FemtoBolt配置重新加载成功 - 算法: {self.algorithm_type}, 分辨率: {self.color_resolution}, FPS: {self.fps}")
|
||
return True
|
||
|
||
except Exception as e:
|
||
self.logger.error(f"重新加载FemtoBolt配置失败: {e}")
|
||
return False
|
||
|
||
|
||
def check_hardware_connection(self) -> bool:
|
||
"""
|
||
相机连接检测太复杂，忽略连接检测
|
||
Returns:
|
||
bool: 相机是否连接
|
||
"""
|
||
return self.is_connected
|
||
|
||
|
||
|
||
def cleanup(self):
|
||
"""
|
||
清理资源
|
||
"""
|
||
try:
|
||
# 清理监控线程
|
||
# self._cleanup_monitoring()
|
||
|
||
self.stop_streaming()
|
||
self._cleanup_device()
|
||
|
||
# 清理matplotlib图形对象
|
||
if hasattr(self, 'fig') and self.fig is not None:
|
||
plt.close(self.fig)
|
||
self.fig = None
|
||
self.ax = None
|
||
|
||
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}")
|