realsense d435i 获取点云数据(xyz)
openni不支持,只能用realsense的sdk弄
if __name__ == "__main__":
# Configure 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
pipeline.start(config)
#深度图像向彩色对齐
align_to_color=rs.align(rs.stream.color)
try:
pc = rs.pointcloud()
# Wait for a coherent pair of frames: depth and color
frames = pipeline.wait_for_frames()
frames = align_to_color.process(frames)
depth_frame = frames.get_depth_frame()
color_frame = frames.get_color_frame()
if not depth_frame or not color_frame:
print("wrong!")
# Convert images to numpy arrays
pc.map_to(color_frame)
points = pc.calculate(depth_frame)
print("points",type(points),points)
vtx = np.asanyarray(points.get_vertices())
print(vtx.shape)
npy_vtx = np.zeros((len(vtx), 3), float)
for i in range(len(vtx)):
npy_vtx[i][0] = np.float(vtx[i][0])
npy_vtx[i][1] = np.float(vtx[i][1])
npy_vtx[i][2] = -np.float(vtx[i][2])
print("npy_vtx",npy_vtx.shape,npy_vtx)
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(npy_vtx)
#显示点云
o3d.visualization.draw_geometries([pcd])
finally:
# Stop streaming
pipeline.stop()