105 lines
3.5 KiB
Python
105 lines
3.5 KiB
Python
#!/usr/bin/env python3
|
||
# -*- coding: utf-8 -*-
|
||
|
||
import os
|
||
import time
|
||
import numpy as np
|
||
import matplotlib.pyplot as plt
|
||
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
|
||
colors = ['fuchsia', 'red', 'yellow', 'lime', 'cyan', 'blue',
|
||
'fuchsia', 'red', 'yellow', 'lime', 'cyan', 'blue']
|
||
self.cmap = LinearSegmentedColormap.from_list("custom_cmap", colors)
|
||
|
||
# SDK设备句柄
|
||
self.device_handle = None
|
||
self.pykinect = None
|
||
self.config = None
|
||
|
||
# 初始化matplotlib figure
|
||
plt.ion()
|
||
self.fig, self.ax = plt.subplots(figsize=(7, 7))
|
||
|
||
def _load_sdk(self):
|
||
"""加载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}")
|
||
return False
|
||
|
||
def _configure_device(self):
|
||
"""配置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
|
||
self.config.synchronized_images_only = False
|
||
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)
|
||
|
||
# 屏蔽深度为0的部分
|
||
depth = np.ma.masked_equal(depth, 0)
|
||
|
||
# 背景图（灰色）
|
||
background = np.ones_like(depth) * 0.5
|
||
|
||
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()
|
||
|
||
def run(self):
|
||
if not self._load_sdk():
|
||
print("SDK加载失败，退出")
|
||
return
|
||
|
||
self._configure_device()
|
||
print("FemtoBolt深度相机启动成功，按 Ctrl+C 退出")
|
||
|
||
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)
|
||
|
||
except KeyboardInterrupt:
|
||
print("退出程序")
|
||
finally:
|
||
self.device_handle.stop()
|
||
self.device_handle.close()
|
||
plt.close(self.fig)
|
||
|
||
|
||
if __name__ == "__main__":
|
||
viewer = FemtoBoltViewer(depth_min=900, depth_max=1300)
|
||
viewer.run()
|