深度图渲染效果提交修改
This commit is contained in:
zhaozilong12
parent
a83df57987
commit
640d14c57a
@ -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)
|
||||
|
||||
@ -1,37 +1,35 @@
|
||||
#!/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)
|
||||
@ -41,7 +39,7 @@ class FemtoBoltViewer:
|
||||
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:
|
||||
if self.device_handle:
|
||||
self.device_handle.stop()
|
||||
self.device_handle.close()
|
||||
plt.close(self.fig)
|
||||
cv2.destroyAllWindows()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
viewer = FemtoBoltViewer(depth_min=900, depth_max=1300)
|
||||
viewer = FemtoBoltViewer(depth_min=900, depth_max=1100)
|
||||
viewer.run()
|
||||
|
||||
Loading…
Reference in New Issue
Block a user