140 lines
5.8 KiB
Python
140 lines
5.8 KiB
Python
import os
|
|
import numpy as np
|
|
import matplotlib.pyplot as plt
|
|
from matplotlib.colors import LinearSegmentedColormap, ListedColormap
|
|
|
|
# 设置matplotlib支持中文显示
|
|
plt.rcParams['font.sans-serif'] = ['SimHei', 'DejaVu Sans'] # 用来正常显示中文标签
|
|
plt.rcParams['axes.unicode_minus'] = False # 用来正常显示负号
|
|
|
|
|
|
class FemtoBoltDynamicViewer:
|
|
def __init__(self, depth_min=900, depth_max=1300):
|
|
self.depth_min = depth_min
|
|
self.depth_max = depth_max
|
|
|
|
# 使用display_x.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']
|
|
self.mcmap = LinearSegmentedColormap.from_list("custom_cmap", colors)
|
|
|
|
# SDK 设备句柄和配置
|
|
self.device_handle = None
|
|
self.pykinect = None
|
|
self.config = None
|
|
|
|
def _load_sdk(self):
|
|
"""加载并初始化 FemtoBolt SDK"""
|
|
import pykinect_azure as pykinect
|
|
base_dir = os.path.dirname(os.path.abspath(__file__))
|
|
dll_path = os.path.join(base_dir, "..", "dll", "femtobolt", "k4a.dll")
|
|
self.pykinect = pykinect
|
|
self.pykinect.initialize_libraries(track_body=False, module_k4a_path=dll_path)
|
|
|
|
def _configure_device(self):
|
|
"""配置 FemtoBolt 深度相机"""
|
|
self.config = self.pykinect.default_configuration
|
|
self.config.depth_mode = self.pykinect.K4A_DEPTH_MODE_NFOV_2X2BINNED
|
|
self.config.color_format = self.pykinect.K4A_IMAGE_FORMAT_COLOR_BGRA32
|
|
self.config.color_resolution = self.pykinect.K4A_COLOR_RESOLUTION_720P
|
|
self.config.synchronized_images_only = False
|
|
self.config.color_resolution = 0
|
|
self.device_handle = self.pykinect.start_device(config=self.config)
|
|
|
|
|
|
|
|
def run(self):
|
|
"""运行实时深度数据可视化 - 融合display_x.py原始算法"""
|
|
self._load_sdk()
|
|
self._configure_device()
|
|
|
|
plt.ion() # 打开交互模式
|
|
plt.figure(figsize=(7, 7)) # 使用display_x.py的图形设置
|
|
print("FemtoBolt 深度相机启动成功，关闭窗口或 Ctrl+C 退出")
|
|
print(f"深度范围: {self.depth_min} - {self.depth_max} mm")
|
|
|
|
try:
|
|
frame_count = 0
|
|
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
|
|
|
|
# 使用display_x.py的原始算法处理深度数据
|
|
depth = depth_image.copy()
|
|
|
|
# 深度数据过滤 (根据输入参数动态设置)
|
|
depth[depth > self.depth_max] = 0
|
|
depth[depth < self.depth_min] = 0
|
|
|
|
# 裁剪感兴趣区域 (与display_x.py完全一致)
|
|
# depth = depth[50:200, 50:210]
|
|
|
|
# 背景图 (与display_x.py完全一致)
|
|
background = np.ones_like(depth) * 0.5 # 设定灰色背景
|
|
|
|
# 使用 np.ma.masked_equal() 来屏蔽深度图中的零值 (与display_x.py完全一致)
|
|
depth = np.ma.masked_equal(depth, 0)
|
|
|
|
# 绘制背景 (与display_x.py完全一致)
|
|
plt.imshow(background, origin='lower', cmap='gray', alpha=0.3)
|
|
|
|
# 绘制白色栅格线，并将其置于底层 (与display_x.py完全一致)
|
|
plt.grid(True, which='both', axis='both', color='white', linestyle='-', linewidth=1, zorder=0)
|
|
|
|
# 绘制等高线图并设置原点在左下角 (根据输入参数动态设置)
|
|
# 通过设置 zorder 来控制它们的层级。例如，设置 zorder=2 或更大的值来确保它们位于栅格线之上。
|
|
plt.contourf(depth, levels=50, cmap=self.mcmap, vmin=self.depth_min, vmax=self.depth_max, origin='upper', zorder=2)
|
|
|
|
# 更新显示 (与display_x.py完全一致)
|
|
plt.pause(0.1) # 暂停0.1秒
|
|
plt.draw() # 重绘图像
|
|
plt.clf() # 清除当前图像
|
|
|
|
frame_count += 1
|
|
if frame_count % 30 == 0: # 每30帧打印一次信息
|
|
print(f"已处理 {frame_count} 帧")
|
|
|
|
except KeyboardInterrupt:
|
|
print("\n检测到退出信号，结束程序")
|
|
except Exception as e:
|
|
print(f"运行时错误: {e}")
|
|
finally:
|
|
# 清理资源
|
|
if self.device_handle:
|
|
try:
|
|
if hasattr(self.device_handle, 'stop'):
|
|
self.device_handle.stop()
|
|
if hasattr(self.device_handle, 'close'):
|
|
self.device_handle.close()
|
|
except Exception as e:
|
|
print(f"设备关闭时出现错误: {e}")
|
|
plt.ioff() # 关闭交互模式
|
|
plt.close('all')
|
|
print("程序已安全退出")
|
|
|
|
def save_current_frame(self, filename="depth_frame.png"):
|
|
"""保存当前帧到文件"""
|
|
try:
|
|
plt.savefig(filename, dpi=150, bbox_inches='tight')
|
|
print(f"当前帧已保存到: {filename}")
|
|
except Exception as e:
|
|
print(f"保存帧失败: {e}")
|
|
|
|
|
|
if __name__ == "__main__":
|
|
# 创建查看器实例
|
|
viewer = FemtoBoltDynamicViewer(depth_min=700, depth_max=1000)
|
|
|
|
print("=" * 50)
|
|
print("FemtoBolt 深度相机动态可视化测试")
|
|
print("基于 display_x.py 算法的实时成像")
|
|
print("=" * 50)
|
|
|
|
# 运行可视化
|
|
viewer.run() |