借到了相机d435i。开始尝试获取点云。
要求：检测砖垛的第一层，计数并定位，便于之后机器人的操作。这里第一层砖块不是规则排布的。
目的：这篇文章主要记录如何与相机交互，获得点云，还有想办法把不需要的点云删除。
思路：俯拍砖垛。然后在z轴方向设置阈值，已达到删除第一层砖以外的平面的目的。
这里的代码我参考了
********************************************************************************************************
导入必要的库，open3d用于点云的处理和可视化。numpy用于数组的处理。pyrealsense2用于和realsense相机交互。
import open3d as o3d
import numpy as np
import pyrealsense2 as rs
建立管道并配置深度和彩色流
#Configure the pipeline to depth and color streams
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
开始流
#Start streaming
pipe_profile = pipeline.start(config)
建立对齐对象，将深度流向彩色流对齐
#Create Alignment
pc = rs.pointcloud()
align_to_color = rs.align(rs.stream.color)
try:
跳过初始的20帧，以便相机稳定下来
#Skip a few frames
for _ in range(20):
pipeline.wait_for_frames()
获取对齐后的深度和彩色帧
frames = pipeline.wait_for_frames()
frames = align_to_color.process(frames)
depth_frame = frames.get_depth_frame()
color_frame = frames.get_color_frame()
将深度帧和彩色帧转换为numpy数组以便后续处理
# Convert images to numpy arrays
depth_image = np.asanyarray(depth_frame.get_data())
color_image = np.asanyarray(color_frame.get_data())
获取相机的内参，这里用的是pyrealsense2
# Intrinsics
profile = frames.get_profile()
intrinsics = profile.as_video_stream_profile().get_intrinsics()
这里用的是open3d
# 转换为open3d中的相机参数
pinhole_camera_intrinsic = o3d.camera.PinholeCameraIntrinsic(
intrinsics.width, intrinsics.height, intrinsics.fx, intrinsics.fy, intrinsics.ppx, intrinsics.ppy)
从深度图创建点云，得到初始点云
# Create point cloud from depth image
depth = o3d.geometry.Image(depth_image)
pcd = o3d.geometry.PointCloud.create_from_depth_image(depth, pinhole_camera_intrinsic)
# Visualize initial point cloud
o3d.visualization.draw_geometries([pcd],window_name="original",
width=1024, height=768,
left=50, top=50,
mesh_show_back_face=False)
删除第一层以外的平面
从初始点云获得各个点的坐标，找出z的最小值作为第一层的代表
这里因为我使用直立的麻将作为模型，高度大约为3cm，用zmin+h/2作为阈值
保留z值小于阈值的点，即为第一层的点
创建点云对象level_pcd,将转换后的点云对象赋值给level_pcd的points属性
保存第一层的点云level_pcd为pcd文件，命名为level_1.pcd
# Remove second layer
point_stack = np.asarray(pcd.points)
z = point_stack[:, 2]
zmin = np.min(z)
print('min z', zmin)
threshold = zmin + 0.015
level_point = point_stack[z < threshold]
level_pcd = o3d.geometry.PointCloud()
level_pcd.points = o3d.utility.Vector3dVector(level_point)
# Visualize first layer point cloud
o3d.visualization.draw_geometries([level_pcd], window_name="Level 1",
width=1024, height=768,
left=50, top=50,
mesh_show_back_face=False)
o3d.io.write_point_cloud("level_1.pcd", level_pcd)
finally:
pipeline.stop()
print('done')
后面要开始滤波，之后再写吧
#Statistical outlier removal
print("Statistical outlier removal")
cl, ind = level_pcd.remove_statistical_outlier(nb_neighbors=20, std_ratio=2.0)
sor_cloud = level_pcd.select_by_index(ind)
print("before",level_pcd)
print("after",sor_cloud)
# Visualize filtered point cloud
o3d.visualization.draw_geometries([sor_cloud], window_name="Statistical Filtering",
width=1024, height=768,
left=50, top=50,
mesh_show_back_face=False)