From 640d14c57adbe54d04ea33753bff4df23fd78474 Mon Sep 17 00:00:00 2001 From: zhaozilong12 <405241463@qq.com> Date: Tue, 19 Aug 2025 18:15:56 +0800 Subject: [PATCH] =?UTF-8?q?=E6=B7=B1=E5=BA=A6=E5=9B=BE=E6=B8=B2=E6=9F=93?= =?UTF-8?q?=E6=95=88=E6=9E=9C=E6=8F=90=E4=BA=A4=E4=BF=AE=E6=94=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- backend/devices/femtobolt_manager.py | 99 ++++++++++++++++------------ backend/tests/testfemtobolt.py | 94 ++++++++++++++------------ 2 files changed, 111 insertions(+), 82 deletions(-) diff --git a/backend/devices/femtobolt_manager.py b/backend/devices/femtobolt_manager.py index 1b08aed7..48f6b5c1 100644 --- a/backend/devices/femtobolt_manager.py +++ b/backend/devices/femtobolt_manager.py @@ -16,6 +16,7 @@ from typing import Optional, Dict, Any, Tuple import logging from collections import deque import gc +from matplotlib.colors import LinearSegmentedColormap try: from .base_device import BaseDevice @@ -115,6 +116,11 @@ class FemtoBoltManager(BaseDevice): self._grid_bg = None self._grid_size = (480, 640) # 默认尺寸 + # 自定义彩虹色 colormap（参考testfemtobolt.py） + colors = ['fuchsia', 'red', 'yellow', 'lime', 'cyan', 'blue', + 'fuchsia', 'red', 'yellow', 'lime', 'cyan', 'blue'] + self.custom_cmap = LinearSegmentedColormap.from_list("custom_cmap", colors) + self.logger.info("FemtoBolt管理器初始化完成") def _update_gamma_lut(self): @@ -520,32 +526,23 @@ class FemtoBoltManager(BaseDevice): self._grid_bg = bg self._grid_size = (rows, cols) - # 深度范围过滤 + 归一化 - depth_clipped = np.where( - (depth_image >= self.depth_range_min) & (depth_image <= self.depth_range_max), - depth_image, 0 - ) - if np.max(depth_clipped) > 0: - depth_normalized = ((depth_clipped - self.depth_range_min) / (self.depth_range_max - self.depth_range_min) * 255).astype(np.uint8) - else: - depth_normalized = np.zeros((rows, cols), dtype=np.uint8) - - # 对比度与伽马 - depth_normalized = cv2.convertScaleAbs(depth_normalized, alpha=1.5, beta=0) - if self._gamma_lut is None or self._current_gamma != self.gamma_value: - self._update_gamma_lut() - depth_gamma = cv2.LUT(depth_normalized, self._gamma_lut) - - # 伪彩色 - depth_colored = cv2.applyColorMap(depth_gamma, cv2.COLORMAP_JET) - - # 合成到背景（避免copy，使用背景副本） background = self._grid_bg.copy() - mask_valid = (depth_clipped > 0) - for c in range(3): - channel = background[:, :, c] - channel[mask_valid] = depth_colored[:, :, c][mask_valid] - depth_colored_final = background + + # 生成深度掩码，仅保留指定范围内的像素 + mask_valid = (depth_image >= self.depth_range_min) & (depth_image <= self.depth_range_max) + depth_clipped = np.clip(depth_image, self.depth_range_min, self.depth_range_max) + normed = (depth_clipped.astype(np.float32) - self.depth_range_min) / (self.depth_range_max - self.depth_range_min) + + # 反转映射，保证颜色方向与之前一致 + normed = 1.0 - normed + + # 应用自定义 colormap，将深度值映射到 RGB + rgba = self.custom_cmap(normed) + rgb = (rgba[..., :3] * 255).astype(np.uint8) + + # 叠加：在背景上覆盖彩色深度图（掩码处不覆盖，保留灰色背景+网格） + depth_colored_final = background.copy() + depth_colored_final[mask_valid] = rgb[mask_valid] # 裁剪宽度 height, width = depth_colored_final.shape[:2] @@ -605,33 +602,53 @@ class FemtoBoltManager(BaseDevice): def _process_depth_image(self, depth_image) -> np.ndarray: """ - 处理深度图像 + 处理深度图像（采用testfemtobolt.py的渲染方式） """ try: 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) + # 确保二维数据 + if depth_image.ndim == 3 and depth_image.shape[2] == 1: + depth_image = depth_image[:, :, 0] - 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) + h, w = depth_image.shape - enhanced = cv2.convertScaleAbs(normalized, alpha=self.contrast_factor, beta=0) + # 生成灰色背景和白色网格（参考testfemtobolt.py） + background = np.full((h, w, 3), 128, dtype=np.uint8) # 灰色背景 + # 绘制网格线 + for x in range(0, w, 50): # 每50像素一条竖线 + cv2.line(background, (x, 0), (x, h-1), (255, 255, 255), 1) + for y in range(0, h, 50): # 每50像素一条横线 + cv2.line(background, (0, y), (w-1, y), (255, 255, 255), 1) - if self._gamma_lut is None or self._current_gamma != self.gamma_value: - self._update_gamma_lut() - gamma_corrected = cv2.LUT(enhanced, self._gamma_lut) + # 生成深度掩码，仅保留指定范围内的像素 + mask_valid = (depth_image >= self.depth_range_min) & (depth_image <= self.depth_range_max) + depth_clipped = np.clip(depth_image, self.depth_range_min, self.depth_range_max) + normed = (depth_clipped.astype(np.float32) - self.depth_range_min) / (self.depth_range_max - self.depth_range_min) - if self.use_pseudo_color: - colored = cv2.applyColorMap(gamma_corrected, cv2.COLORMAP_JET) - else: - colored = cv2.cvtColor(gamma_corrected, cv2.COLOR_GRAY2BGR) + # 反转映射，保证颜色方向与之前一致 + normed = 1.0 - normed + + # 应用自定义 colormap，将深度值映射到 RGB + rgba = self.custom_cmap(normed) + rgb = (rgba[..., :3] * 255).astype(np.uint8) + + # 叠加：在背景上覆盖彩色深度图（掩码处不覆盖，保留灰色背景+网格） + final_img = background.copy() + final_img[mask_valid] = rgb[mask_valid] + + # 裁剪宽度（保持原有功能） + height, width = final_img.shape[:2] + target_width = height // 2 + if width > target_width: + left = (width - target_width) // 2 + right = left + target_width + final_img = final_img[:, left:right] + + return final_img - return colored except Exception as e: self.logger.error(f"处理深度图像失败: {e}") return np.zeros((480, 640, 3), dtype=np.uint8) diff --git a/backend/tests/testfemtobolt.py b/backend/tests/testfemtobolt.py index 1645f585..823265da 100644 --- a/backend/tests/testfemtobolt.py +++ b/backend/tests/testfemtobolt.py @@ -1,47 +1,45 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- - import os -import time import numpy as np -import matplotlib.pyplot as plt +import cv2 from matplotlib.colors import LinearSegmentedColormap + class FemtoBoltViewer: def __init__(self, depth_min=900, depth_max=1300): self.depth_min = depth_min self.depth_max = depth_max - # 自定义colormap + # 自定义彩虹色 colormap colors = ['fuchsia', 'red', 'yellow', 'lime', 'cyan', 'blue', 'fuchsia', 'red', 'yellow', 'lime', 'cyan', 'blue'] self.cmap = LinearSegmentedColormap.from_list("custom_cmap", colors) - # SDK设备句柄 + # SDK 设备句柄和配置 self.device_handle = None self.pykinect = None self.config = None - # 初始化matplotlib figure - plt.ion() - self.fig, self.ax = plt.subplots(figsize=(7, 7)) + # 缓存背景+网格图像（仅生成一次） + self.background = None + + # OpenCV 窗口 + cv2.namedWindow("Depth CV") def _load_sdk(self): - """加载FemtoBolt SDK""" + """加载并初始化 FemtoBolt SDK""" try: import pykinect_azure as pykinect self.pykinect = pykinect - # 配置DLL路径 base_dir = os.path.dirname(os.path.abspath(__file__)) dll_path = os.path.join(base_dir, "..", "dll", "femtobolt", "bin", "k4a.dll") self.pykinect.initialize_libraries(track_body=False, module_k4a_path=dll_path) return True except Exception as e: - print(f"加载SDK失败: {e}") + print(f"加载 SDK 失败: {e}") return False def _configure_device(self): - """配置FemtoBolt设备""" + """配置 FemtoBolt 深度相机""" self.config = self.pykinect.default_configuration self.config.depth_mode = self.pykinect.K4A_DEPTH_MODE_NFOV_UNBINNED self.config.camera_fps = self.pykinect.K4A_FRAMES_PER_SECOND_15 @@ -49,56 +47,70 @@ class FemtoBoltViewer: self.config.color_resolution = 0 self.device_handle = self.pykinect.start_device(config=self.config) - def _render_depth(self, depth_image: np.ndarray): - """使用matplotlib绘制深度图，带背景和自定义colormap""" - # 过滤深度范围 - depth = np.where((depth_image >= self.depth_min) & (depth_image <= self.depth_max), - depth_image, 0) + def _get_color_image(self, depth_image): + """将原始深度图转换为叠加背景网格后的 RGB 彩色图像""" + h, w = depth_image.shape + # 第一次调用时生成灰色背景和白色网格 + if self.background is None: + self.background = np.full((h, w, 3), 128, dtype=np.uint8) # 灰色 (0.5 -> 128) + # 绘制网格线 + for x in range(w): + cv2.line(self.background, (x, 0), (x, h-1), (255, 255, 255), 1) + for y in range(h): + cv2.line(self.background, (0, y), (w-1, y), (255, 255, 255), 1) - # 屏蔽深度为0的部分 - depth = np.ma.masked_equal(depth, 0) + # 生成深度掩码，仅保留指定范围内的像素 + mask_valid = (depth_image >= self.depth_min) & (depth_image <= self.depth_max) + depth_clipped = np.clip(depth_image, self.depth_min, self.depth_max) + normed = (depth_clipped.astype(np.float32) - self.depth_min) / (self.depth_max - self.depth_min) - # 背景图（灰色） - background = np.ones_like(depth) * 0.5 + # 反转映射，保证颜色方向与之前一致 + normed = 1.0 - normed - self.ax.clear() - # 绘制背景 - self.ax.imshow(background, origin='lower', cmap='gray', alpha=0.3) - # 绘制白色栅格线 - self.ax.grid(True, which='both', axis='both', color='white', linestyle='-', linewidth=1, zorder=0) - # 绘制深度等高线 - self.ax.contourf(depth, levels=200, cmap=self.cmap, vmin=self.depth_min, vmax=self.depth_max, origin='upper', zorder=2) - plt.pause(0.001) - plt.draw() + # 应用自定义 colormap，将深度值映射到 RGB + rgba = self.cmap(normed) + rgb = (rgba[..., :3] * 255).astype(np.uint8) + + # 叠加：在背景上覆盖彩色深度图（掩码处不覆盖，保留灰色背景+网格） + final_img = self.background.copy() + final_img[mask_valid] = rgb[mask_valid] + return final_img def run(self): if not self._load_sdk(): - print("SDK加载失败，退出") + print("SDK 加载失败，程序退出") return self._configure_device() - print("FemtoBolt深度相机启动成功，按 Ctrl+C 退出") + print("FemtoBolt 深度相机启动成功，按 Ctrl+C 或 ESC 退出") try: while True: capture = self.device_handle.update() if capture is None: continue - ret, depth_image = capture.get_depth_image() if not ret or depth_image is None: continue - self._render_depth(depth_image) + # 转换并渲染当前帧 + final_img = self._get_color_image(depth_image) + + # OpenCV 显示 + cv2.imshow("Depth CV", final_img) + # 按 ESC 键退出 + if cv2.waitKey(1) & 0xFF == 27: + break except KeyboardInterrupt: - print("退出程序") + print("检测到退出信号，结束程序") finally: - self.device_handle.stop() - self.device_handle.close() - plt.close(self.fig) + if self.device_handle: + self.device_handle.stop() + self.device_handle.close() + cv2.destroyAllWindows() if __name__ == "__main__": - viewer = FemtoBoltViewer(depth_min=900, depth_max=1300) + viewer = FemtoBoltViewer(depth_min=900, depth_max=1100) viewer.run()