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()
|