WIN11系统D455相机点云聚类识别障碍物
系统环境
python 3.9.23
pip list：
点击查看代码
Package Version
------------------------- ------------
absl-py 2.3.1
ale-py 0.11.2
annotated-types 0.7.0
asttokens 3.0.0
attrs 25.3.0
blinker 1.9.0
certifi 2025.8.3
charset-normalizer 3.4.2
click 8.1.8
cloudpickle 3.1.1
colorama 0.4.6
comm 0.2.3
ConfigArgParse 1.7.1
contourpy 1.3.0
cycler 0.12.1
dash 3.2.0
decorator 5.2.1
dm-env 1.6
dm-tree 0.1.8
et_xmlfile 2.0.0
etils 1.5.2
eval_type_backport 0.2.2
exceptiongroup 1.3.0
executing 2.2.0
Farama-Notifications 0.0.4
fastjsonschema 2.21.2
filelock 3.18.0
filterpy 1.4.5
Flask 3.1.2
fonttools 4.59.0
freetype-py 2.5.1
fsspec 2025.7.0
glfw 2.9.0
grpcio 1.74.0
gymnasium 1.1.1
idna 3.10
ikpy 3.4.2
imageio 2.37.0
importlib_metadata 8.7.0
importlib_resources 6.5.2
ipython 8.18.1
ipywidgets 8.1.7
itsdangerous 2.2.0
jedi 0.19.2
Jinja2 3.1.6
joblib 1.5.1
jsonschema 4.25.1
jsonschema-specifications 2025.4.1
jupyter_core 5.8.1
jupyterlab_widgets 3.0.15
kiwisolver 1.4.7
labmaze 1.0.6
lxml 6.0.0
Markdown 3.8.2
markdown-it-py 3.0.0
MarkupSafe 3.0.2
matplotlib 3.9.4
matplotlib-inline 0.1.7
mdurl 0.1.2
modbus_tk 1.1.5
mpmath 1.3.0
mujoco 3.2.3
narwhals 2.2.0
nbformat 5.10.4
nest-asyncio 1.6.0
networkx 2.2
numpy 1.26.4
open3d 0.19.0
opencv-python 4.9.0.80
openpyxl 3.1.5
packaging 25.0
pandas 2.3.1
parso 0.8.5
patsy 1.0.1
pillow 11.0.0
pip 25.1
platformdirs 4.3.8
plotly 6.3.0
prompt_toolkit 3.0.51
protobuf 6.31.1
psutil 7.0.0
pure_eval 0.2.3
py-cpuinfo 9.0.0
pycollada 0.6
pydantic 2.11.7
pydantic_core 2.33.2
pygame 2.6.1
pyglet 2.1.6
Pygments 2.19.2
PyOpenGL 3.1.0
pyparsing 3.2.3
pyrealsense2 2.56.5.9235
pyrender 0.1.45
pyserial 3.5
PySide2 5.15.2.1
python-dateutil 2.9.0.post0
pytz 2025.2
pywin32 311
PyYAML 6.0.2
referencing 0.36.2
requests 2.32.4
retrying 1.4.2
rich 14.1.0
rpds-py 0.27.0
scikit-learn 1.6.1
scipy 1.13.1
seaborn 0.13.2
setuptools 78.1.1
shiboken2 5.15.2.1
simsimd 6.5.0
six 1.17.0
stable_baselines3 2.7.0
stack-data 0.6.3
statsmodels 0.14.5
stringzilla 3.12.5
sympy 1.14.0
tensorboard 2.20.0
tensorboard-data-server 0.7.2
threadpoolctl 3.6.0
torch 2.7.1+cu118
torchaudio 2.7.1+cu118
torchvision 0.22.1+cu118
tqdm 4.67.1
traitlets 5.14.3
trimesh 4.7.1
typing_extensions 4.14.1
typing-inspection 0.4.1
tzdata 2025.2
ultralytics 8.3.185
ultralytics-thop 2.0.16
urllib3 2.5.0
wcwidth 0.2.13
Werkzeug 3.1.3
wheel 0.45.1
widgetsnbextension 4.0.14
zipp 3.23.0
完整代码
以下代码由千问Qwen3-Max辅助编写
import pyrealsense2 as rs
import numpy as np
import cv2
import open3d as o3d
import threading
import time
import queue
from collections import deque
from sklearn.cluster import DBSCAN
class RealSenseObstacleDetector:
def __init__(self):
self.pipeline = rs.pipeline()
self.config = rs.config()
self.config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
self.config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
# 队列用于线程间通信
self.frame_queue = queue.Queue(maxsize=2)
self.processed_queue = queue.Queue(maxsize=2)
self.running = True
# FPS计算
self.fps_buffer = deque(maxlen=30)
self.last_time = time.time()
# 相机内参
self.intrinsics = None
self.depth_scale = 0.001 # 默认D455深度单位转换为米
# 点云可视化参数
self.point_size = 2.0
self.show_axes = True
# 障碍物检测参数
self.depth_threshold_min = 0.5 # 最小检测距离
self.depth_threshold_max = 2.0 # 最大检测距离
self.dbscan_eps = 0.08 # DBSCAN邻域半径
self.dbscan_min_points = 40 # DBSCAN最小点数
# 降采样参数
self.voxel_size = 0.02 # 体素大小，越大点越少（单位：米）
self.max_points = 50000 # 最大点数限制，超过则随机采样
def setup_camera(self):
"""配置并启动相机，获取内参"""
pipeline_wrapper = rs.pipeline_wrapper(self.pipeline)
pipeline_profile = self.config.resolve(pipeline_wrapper)
device = pipeline_profile.get_device()
# 检查是否找到了设备
if not device:
raise RuntimeError("No RealSense device found")
# 获取深度传感器
depth_sensor = None
for sensor in device.sensors:
if sensor.get_info(rs.camera_info.name).find('Depth') >= 0:
depth_sensor = sensor
break
if depth_sensor:
# 设置深度单位
self.depth_scale = depth_sensor.get_depth_scale()
print(f"Depth scale: {self.depth_scale}m per unit")
# 启动管道
profile = self.pipeline.start(self.config)
# 获取相机内参
depth_profile = rs.video_stream_profile(profile.get_stream(rs.stream.depth))
self.intrinsics = depth_profile.get_intrinsics()
print(f"Camera intrinsics: {self.intrinsics}")
# 等待几帧让自动曝光稳定
for _ in range(30):
self.pipeline.wait_for_frames()
def downsample_point_cloud(self, pcd, max_points=None, voxel_size=None):
"""降采样点云以提高性能"""
if voxel_size:
# 体素网格降采样
pcd = pcd.voxel_down_sample(voxel_size)
if max_points and len(pcd.points) > max_points:
# 随机采样
indices = np.random.choice(len(pcd.points), max_points, replace=False)
points = np.asarray(pcd.points)[indices]
colors = np.asarray(pcd.colors)[indices] if len(pcd.colors) > 0 else np.zeros_like(points)
downsampled_pcd = o3d.geometry.PointCloud()
downsampled_pcd.points = o3d.utility.Vector3dVector(points)
downsampled_pcd.colors = o3d.utility.Vector3dVector(colors)
return downsampled_pcd
return pcd
def camera_thread(self):
"""负责捕获相机数据的线程"""
try:
self.setup_camera()
while self.running:
# 等待同步帧
frames = self.pipeline.wait_for_frames()
depth_frame = frames.get_depth_frame()
color_frame = frames.get_color_frame()
if not depth_frame or not color_frame:
continue
# 转换为numpy数组
depth_image = np.asanyarray(depth_frame.get_data())
color_image = np.asanyarray(color_frame.get_data())
# 尝试清空旧帧
while not self.frame_queue.empty():
try:
self.frame_queue.get_nowait()
except queue.Empty:
break
# 放入队列
self.frame_queue.put((depth_image, color_image, time.time()))
except Exception as e:
print(f"Camera thread error: {e}")
finally:
if hasattr(self, 'pipeline'):
self.pipeline.stop()
def processing_thread(self):
"""负责处理数据并生成点云和障碍物检测的线程"""
while self.intrinsics is None and self.running:
print("Waiting for camera intrinsics...")
time.sleep(0.1)
if not self.running:
return
# 设置相机内参
pinhole_camera_intrinsic = o3d.camera.PinholeCameraIntrinsic(
self.intrinsics.width,
self.intrinsics.height,
self.intrinsics.fx,
self.intrinsics.fy,
self.intrinsics.ppx,
self.intrinsics.ppy
)
print(f"Pinhole camera intrinsic:\n{pinhole_camera_intrinsic}")
while self.running:
try:
depth_image, color_image, timestamp = self.frame_queue.get(timeout=1.0)
# 转换为Open3D图像格式
depth_o3d = o3d.geometry.Image(depth_image)
# 转换BGR到RGB
rgb_image = cv2.cvtColor(color_image, cv2.COLOR_BGR2RGB)
color_o3d = o3d.geometry.Image(rgb_image)
# 创建RGBD图像
rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(
color_o3d,
depth_o3d,
depth_scale=1.0 / self.depth_scale, # 转换深度单位为米
depth_trunc=2.5, # 只处理2.5米内的点
convert_rgb_to_intensity=False # 保留RGB颜色
)
# 从RGBD图像创建点云
pcd = o3d.geometry.PointCloud.create_from_rgbd_image(
rgbd_image,
pinhole_camera_intrinsic
)
# 过滤无效点
pcd = pcd.remove_non_finite_points()
# 检查点云是否为空
if len(pcd.points) == 0:
print("Warning: Generated point cloud is empty")
else:
print(f"Original point cloud has {len(pcd.points)} points")
# 应用降采样
pcd = self.downsample_point_cloud(pcd, self.max_points, self.voxel_size)
print(f"After downsampling: {len(pcd.points)} points")
# 获取点云数据
points = np.asarray(pcd.points)
colors = np.asarray(pcd.colors)
# 过滤距离范围内的点
if len(points) > 0:
z_coords = points[:, 2] # Z轴是深度方向
distance_mask = (z_coords >= self.depth_threshold_min) & (z_coords <= self.depth_threshold_max)
filtered_points = points[distance_mask]
filtered_colors = colors[distance_mask]
else:
filtered_points = np.array([])
filtered_colors = np.array([])
# 检测障碍物
obstacles = []
if len(filtered_points) >= self.dbscan_min_points:
# 使用DBSCAN聚类检测障碍物
clustering = DBSCAN(eps=self.dbscan_eps, min_samples=self.dbscan_min_points)
labels = clustering.fit_predict(filtered_points)
# 获取唯一标签
unique_labels = set(labels)
for label in unique_labels:
if label == -1: # 噪声点
continue
# 获取当前聚类的点
cluster_mask = labels == label
cluster_points = filtered_points[cluster_mask]
# 计算聚类特征
min_bound = cluster_points.min(axis=0)
max_bound = cluster_points.max(axis=0)
center = cluster_points.mean(axis=0)
size = max_bound - min_bound
obstacle = {
'label': label,
'center': center,
'min_bound': min_bound,
'max_bound': max_bound,
'size': size,
'points': cluster_points,
'point_count': len(cluster_points)
}
obstacles.append(obstacle)
# 计算FPS
current_time = time.time()
time_diff = current_time - timestamp
fps = 1.0 / max(time_diff, 0.001)
self.fps_buffer.append(fps)
avg_fps = sum(self.fps_buffer) / len(self.fps_buffer) if self.fps_buffer else 0
# 尝试清空旧数据
while not self.processed_queue.empty():
try:
self.processed_queue.get_nowait()
except queue.Empty:
break
# 放入处理后的数据
self.processed_queue.put((pcd, color_image.copy(), obstacles, avg_fps))
except queue.Empty:
continue
except Exception as e:
print(f"Processing error: {e}")
continue
def visualization_thread(self):
"""负责可视化结果的线程"""
# 创建Open3D可视化窗口
vis = o3d.visualization.Visualizer()
vis.create_window("D455 Real-time 3D Reconstruction & Obstacle Detection", width=1000, height=800)
# 创建点云对象
pcd = o3d.geometry.PointCloud()
vis.add_geometry(pcd)
# 创建障碍物包围盒列表
obstacle_bboxes = []
# 添加坐标系
if self.show_axes:
coord_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.5, origin=[0, 0, 0])
vis.add_geometry(coord_frame)
# 设置可视化选项
opt = vis.get_render_option()
opt.background_color = np.asarray([0.05, 0.05, 0.05]) # 深灰色背景
opt.point_size = self.point_size # 增大点大小，更容易看到
opt.show_coordinate_frame = True
# 初始化视图
view_ctl = vis.get_view_control()
view_ctl.set_front([0, 0, -1]) # 朝向-Z方向
view_ctl.set_up([0, -1, 0]) # Y轴向下
view_ctl.set_lookat([0, 0, 0]) # 看向原点
view_ctl.set_zoom(0.3) # 调整缩放，确保能看到点云
print("Visualization thread started. Press 'q' to quit.")
print("Press 'v' to adjust voxel size, 'm' to adjust max points")
while self.running:
try:
pointcloud, color_image, obstacles, fps = self.processed_queue.get(timeout=1.0)
# 更新点云
pcd.points = pointcloud.points
pcd.colors = pointcloud.colors
# 验证点云数据
if len(pcd.points) == 0:
print("Warning: Received empty point cloud")
# 移除旧的障碍物包围盒
for bbox in obstacle_bboxes:
vis.remove_geometry(bbox, reset_bounding_box=False)
obstacle_bboxes.clear()
# 添加新的障碍物包围盒
for obstacle in obstacles:
min_bound = obstacle['min_bound']
max_bound = obstacle['max_bound']
# 创建包围盒
bbox = o3d.geometry.AxisAlignedBoundingBox(min_bound, max_bound)
bbox.color = np.array([1, 0, 0]) # 红色
# 添加到可视化器
vis.add_geometry(bbox, reset_bounding_box=False)
obstacle_bboxes.append(bbox)
# 在图像上显示FPS和点云数量
text = f"FPS: {int(fps)} | Points: {len(pcd.points)} | Obstacles: {len(obstacles)}"
cv2.putText(color_image, text, (10, 30),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2, cv2.LINE_AA)
# 在图像上标注障碍物
for i, obstacle in enumerate(obstacles):
center = obstacle['center']
# 计算图像坐标 - 修复属性名：使用ppx和ppy
u = int((center[0] * self.intrinsics.fx) / center[2] + self.intrinsics.ppx)
v = int((center[1] * self.intrinsics.fy) / center[2] + self.intrinsics.ppy) # 修正：使用ppy而非py
if 0 <= u < color_image.shape[1] and 0 <= v < color_image.shape[0]:
# 绘制中心点
cv2.circle(color_image, (u, v), 8, (0, 0, 255), -1)
# 添加标签
label = f'Obstacle {i + 1}: {center[2]:.2f}m'
cv2.putText(color_image, label, (u - 50, v - 15),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 1)
except queue.Empty:
continue
except AttributeError as e:
# 如果出现属性错误，记录错误并继续
if "'pyrealsense2.pyrealsense2.intrinsics' object has no attribute 'py'" in str(e):
print(
"AttributeError: pyrealsense2 intrinsics object does not have 'py' attribute, using 'ppy' instead")
continue
except Exception as e:
print(f"Visualization error: {e}")
import traceback
traceback.print_exc()
continue
# 显示RGB图像
cv2.imshow('RGB Stream with Obstacle Detection', color_image)
# 更新可视化
vis.update_geometry(pcd)
# 更新所有障碍物包围盒
for bbox in obstacle_bboxes:
vis.update_geometry(bbox)
# 检查窗口是否被关闭
if not vis.poll_events():
print("Visualization window closed")
self.running = False
break
vis.update_renderer()
# 检查按键
key = cv2.waitKey(1) & 0xFF
if key == ord('q'):
self.running = False
break
elif key == ord('s'):
# 保存当前点云
timestamp = time.strftime("%Y%m%d_%H%M%S")
filename = f"pointcloud_{timestamp}.ply"
o3d.io.write_point_cloud(filename, pcd)
print(f"Saved point cloud to {filename}")
elif key == ord('c'):
# 切换坐标系显示
self.show_axes = not self.show_axes
if self.show_axes:
coord_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.5, origin=[0, 0, 0])
vis.add_geometry(coord_frame)
else:
# 不移除坐标系，保持简单
pass
elif key == ord('+') or key == ord('='):
# 增大点大小
self.point_size = min(10.0, self.point_size + 0.5)
opt.point_size = self.point_size
print(f"Point size: {self.point_size}")
elif key == ord('-'):
# 减小点大小
self.point_size = max(0.5, self.point_size - 0.5)
opt.point_size = self.point_size
print(f"Point size: {self.point_size}")
elif key == ord('v'):
# 调整体素大小
self.voxel_size = min(0.1, self.voxel_size + 0.01)
print(f"Voxel size increased to: {self.voxel_size}m")
elif key == ord('V'):
# 减小体素大小
self.voxel_size = max(0.005, self.voxel_size - 0.01)
print(f"Voxel size decreased to: {self.voxel_size}m")
elif key == ord('m'):
# 增加最大点数限制
self.max_points = min(50000, self.max_points + 1000)
print(f"Max points increased to: {self.max_points}")
elif key == ord('M'):
# 减少最大点数限制
self.max_points = max(1000, self.max_points - 1000)
print(f"Max points decreased to: {self.max_points}")
def run(self):
"""启动所有线程并运行应用"""
print("Starting RealSense D455 point cloud capture with obstacle detection...")
print(f"Downsampling parameters: voxel_size={self.voxel_size}m, max_points={self.max_points}")
# 启动线程
camera_t = threading.Thread(target=self.camera_thread, daemon=True)
processing_t = threading.Thread(target=self.processing_thread, daemon=True)
visualization_t = threading.Thread(target=self.visualization_thread, daemon=True)
camera_t.start()
processing_t.start()
visualization_t.start()
# 等待可视化线程结束
visualization_t.join()
# 停止其他线程
self.running = False
camera_t.join(timeout=1.0)
processing_t.join(timeout=1.0)
print("Application terminated cleanly")
if __name__ == "__main__":
try:
processor = RealSenseObstacleDetector()
processor.run()
except Exception as e:
print(f"Application error: {e}")
import traceback
traceback.print_exc()
finally:
cv2.destroyAllWindows()
浙公网安备 33010602011771号