pyrealsense2获取保存点云

一、第一种实现代码

Python

import sys
import cv2
import pyrealsense2 as rs
import numpy as np
import keyboard
import open3d as o3d
import osif __name__ == "__main__":output_folder = 'output_data/'os.makedirs(output_folder, exist_ok=True)pipeline = rs.pipeline()config = rs.config()config.enable_stream(rs.stream.depth, 848, 480, rs.format.z16, 30)config.enable_stream(rs.stream.color, 848, 480, rs.format.bgr8, 30)# Start streamingpipeline.start(config)# 深度图像向彩色对齐align_to_color = rs.align(rs.stream.color)i = 0try:while True:pc = rs.pointcloud()# Wait for a coherent pair of frames: depth and colorframes = 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!")cv2.imshow("color_image", np.asanyarray(color_frame.get_data()))cv2.waitKey(1)if keyboard.is_pressed("a"):pc.map_to(color_frame)points = pc.calculate(depth_frame)vtx = np.asanyarray(points.get_vertices())npy_vtx = np.array(vtx).view(np.float32).reshape(-1, 3)tex_coords = np.asanyarray(points.get_texture_coordinates())tex_coords = np.array(tex_coords).view(np.float32).reshape(-1, 2)color_image = np.asanyarray(color_frame.get_data())u_pixel_indices = (tex_coords[:, 0] * (color_image.shape[1] - 1)).astype(int)v_pixel_indices = (tex_coords[:, 1] * (color_image.shape[0] - 1)).astype(int)u_pixel_indices = np.clip(u_pixel_indices, 0, color_image.shape[1] - 1)v_pixel_indices = np.clip(v_pixel_indices, 0, color_image.shape[0] - 1)colors = color_image[v_pixel_indices, u_pixel_indices, :]colors = colors[:, [2, 1, 0]] / 255.0print("points", type(points), points)print("npy_vtx", npy_vtx.shape, npy_vtx)pcd = o3d.geometry.PointCloud()pcd.points = o3d.utility.Vector3dVector(npy_vtx)pcd.colors = o3d.utility.Vector3dVector(colors)# 显示点云o3d.io.write_point_cloud(output_folder + str(i) + ".ply",pcd)i += 1if keyboard.is_pressed("q"):sys.exit()finally:# Stop streamingpipeline.stop()

代码解析：

这段代码是使用Python和Intel RealSense SDK (pyrealsense2) 来捕捉深度相机的数据，并将深度数据与彩色数据结合，形成彩色的点云，然后将点云保存为PLY文件。同时，这里也使用了OpenCV来显示彩色图像和Open3D来处理和保存点云。

代码的执行流程如下：

首先，设置输出文件夹，如果不存在则创建。
初始化RealSense流水线(pipeline)和配置(config)，分别启用深度和彩色流。
启动流水线开始捕获数据。
创建一个rs.align对象，以便将深度帧对齐到彩色帧。
进入一个无限循环中，不断地从流水线获取帧（即图像数据）。
使用align_to_color.process(frames)处理帧，使深度帧与彩色帧对齐。
提取深度帧和彩色帧，如果没有获取到这些帧，打印错误信息。
使用OpenCV显示彩色图像。
检查用户是否按下了“A”键。如果按下了，执行以下操作：
- 使用pc.map_to(color_frame)映射颜色到点云。
- 使用pc.calculate(depth_frame)计算点云。
- 将点云的顶点(vtx)转换为NumPy数组，并重塑为三维数据。
- 获取纹理坐标，重塑并转换为像素索引。
- 使用像素索引从彩色图像中检索每个点的颜色。
- 颜色数组的通道顺序可能需要从BGR转换为RGB，并归一化到[0, 1]范围。
- 创建一个Open3D点云对象，并设置其顶点和颜色。
- 将点云保存为PLY文件，文件名包含一个递增的编号。
如果程序被中断（可能是通过Ctrl+C或其他方式），则会执行finally块，停止流水线。

此代码还包含了一些关键函数和方法：

pipeline.wait_for_frames()：从流水线等待并获取一组一致的帧。
pipeline.stop()：停止流水线。
cv2.imshow("color_image", image)：用于显示图片的OpenCV函数。
cv2.waitKey(1)：用于OpenCV图像显示的键盘事件监听。
o3d.geometry.PointCloud()：创建一个空的Open3D点云对象。
o3d.io.write_point_cloud()：用于保存点云到文件的Open3D函数。
keyboard.is_pressed("a")和keyboard.is_pressed("q")：检查是否按下了特定的按键。

结果：

本人用的是一个放在白纸上的小方块，用cloudcompare软件查看点云

二、第一种实现代码

Python

import sys
import cv2
import pyrealsense2 as rs
import numpy as np
import keyboard
import open3d as o3d
import osif __name__ == "__main__":output_folder = 'output_data/'os.makedirs(output_folder, exist_ok=True)pipeline = rs.pipeline()config = rs.config()config.enable_stream(rs.stream.depth, 848, 480, rs.format.z16, 30)config.enable_stream(rs.stream.color, 848, 480, rs.format.bgr8, 30)i = 0pipeline.start(config)align_to_color = rs.align(rs.stream.color)try:while True:# Wait for a coherent pair of frames: depth and colorframes = pipeline.wait_for_frames()frames = align_to_color.process(frames)depth_frame = frames.get_depth_frame()color_frame = frames.get_color_frame()depth_intrinsics = depth_frame.profile.as_video_stream_profile().intrinsicsprofile = frames.get_profile()if not depth_frame or not color_frame:continue# Convert images to numpy arraysdepth_image = np.asanyarray(depth_frame.get_data())color_image = np.asanyarray(color_frame.get_data())cv2.imshow("color_image", color_image)cv2.waitKey(1)color_image = cv2.cvtColor(color_image, cv2.COLOR_BGR2RGB)depth_image = cv2.cvtColor(depth_image, cv2.COLOR_BGR2RGB)if keyboard.is_pressed("a"):print("type of depth_image:", type(depth_image))print("shape of depth_image:", depth_image.shape)o3d_color = o3d.geometry.Image(color_image)o3d_depth = o3d.geometry.Image(depth_image)rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(o3d_color, o3d_depth,depth_scale=1000.0,depth_trunc=3.0,convert_rgb_to_intensity=False)intrinsics = profile.as_video_stream_profile().get_intrinsics()# 转换为open3d中的相机参数pinhole_camera_intrinsic = o3d.camera.PinholeCameraIntrinsic(intrinsics.width, intrinsics.height,intrinsics.fx, intrinsics.fy,intrinsics.ppx, intrinsics.ppy)o3d_result = o3d.geometry.PointCloud.create_from_rgbd_image(rgbd_image,pinhole_camera_intrinsic)o3d.io.write_point_cloud(output_folder + str(i) + ".ply",o3d_result)i += 1if keyboard.is_pressed("q"):sys.exit()finally:pipeline.stop()

代码解析：

这段代码使用Intel RealSense SDK (pyrealsense2) 与 Open3D 库结合来捕获深度和彩色图像数据，并生成彩色点云，最后将点云保存为PLY文件格式。同时，该代码也使用了OpenCV来显示彩色图像，并使用keyboard库来监听键盘事件。下面是对代码的详细解析：

output_folder = 'output_data/'：定义输出文件夹的路径。
os.makedirs(output_folder, exist_ok=True)：创建输出文件夹，如果该文件夹已经存在，则不会引发错误。
创建一个RealSense流水线(pipeline)和配置(config)对象，配置深度流和彩色流。
使用配置启动流水线(pipeline.start(config))。
定义一个rs.align对象align_to_color，以便将深度数据对齐到彩色数据。
进入一个无限循环，等待获取一对深度和彩色帧。
使用align_to_color.process(frames)将获取的帧对齐，以确保深度和彩色图像匹配。
转换深度帧和彩色帧到NumPy数组，并使用OpenCV显示彩色图像。
将彩色图像从BGR格式转换为RGB格式，因为Open3D使用的是RGB格式。
如果用户按下了“A”键，则执行以下步骤：
- 打印深度图像的类型和形状。
- 将NumPy中的图像数组转换为Open3D图像对象。
- 使用Open3D创建RGBD图像对象。
- 从帧的配置文件中获取相机内参。
- 将相机内参转换成Open3D格式。
- 使用RGBD图像和相机内参生成Open3D点云。
- 将点云保存为PLY格式的文件。
如果用户按下了“Q”键，则退出程序。
如果程序被中断，无论是正常退出还是异常终止，finally块确保流水线被停止，释放相关资源。

代码中的depth_scale和depth_trunc参数在创建RGBD图像时使用，分别用于定义深度图像的单位转换(毫米到米)和深度截断阈值(超过此值的深度将被忽略)。convert_rgb_to_intensity参数设置为False，意味着在生成RGBD图像时不需要将彩色图像转换为灰度图像的强度值。

结果：

三、提取感兴趣区域点云的第一种实现代码

Python

import sys
import cv2
import pyrealsense2 as rs
import numpy as np
import keyboard
import open3d as o3d
import osif __name__ == "__main__":output_folder = 'output_data/'os.makedirs(output_folder, exist_ok=True)width = 848height = 480pipeline = rs.pipeline()config = rs.config()config.enable_stream(rs.stream.depth, width, height, rs.format.z16, 30)config.enable_stream(rs.stream.color, width, height, rs.format.bgr8, 30)pipeline.start(config)align_to_color = rs.align(rs.stream.color)i = 0try:while True:pc = rs.pointcloud()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:continuecv2.imshow("color_image", np.asanyarray(color_frame.get_data()))cv2.waitKey(1)if keyboard.is_pressed("a"):pc.map_to(color_frame)points = pc.calculate(depth_frame)# 获取点云的顶点vtx = np.asarray(points.get_vertices())# 获取点云的纹理坐标tex_coords = np.asarray(points.get_texture_coordinates())# 获取彩色图像的数据，用于提取颜色color_image = np.asanyarray(color_frame.get_data())# (定义ROI边界 (x_min, y_min, x_max, y_max))x_min, y_min, x_max, y_max = 100, 100, 540, 380# 过滤ROI内的点和纹理坐标roi_vtx = []roi_tex_coords = []for y in range(y_min, y_max):for x in range(x_min, x_max):index = y * width + xif vtx[index][2]:  # 如果深度值非零roi_vtx.append(vtx[index])roi_tex_coords.append(tex_coords[index])roi_vtx = np.array(roi_vtx).view(np.float32).reshape(-1, 3)roi_tex_coords = np.array(roi_tex_coords).view(np.float32).reshape(-1, 2)# 转换纹理坐标为像素坐标u_pixel_indices = (roi_tex_coords[:, 0] * (color_image.shape[1] - 1)).astype(int)v_pixel_indices = (roi_tex_coords[:, 1] * (color_image.shape[0] - 1)).astype(int)# 获取点云颜色roi_colors = color_image[v_pixel_indices, u_pixel_indices, :]# 创建Open3D点云pcd = o3d.geometry.PointCloud()pcd.points = o3d.utility.Vector3dVector(roi_vtx)pcd.colors = o3d.utility.Vector3dVector(roi_colors[:, [2, 1, 0]] / 255.0)  # 归一化颜色值# 保存点云o3d.io.write_point_cloud(output_folder + str(i) + ".ply", pcd)i += 1if keyboard.is_pressed("q"):sys.exit()finally:pipeline.stop()

代码解析：

x_min, y_min, x_max, y_max = 100, 100, 540, 380
# 过滤ROI内的点和纹理坐标
roi_vtx = []
roi_tex_coords = []
for y in range(y_min, y_max):
for x in range(x_min, x_max):
index = y * width + x
if vtx[index][2]:  # 如果深度值非零
roi_vtx.append(vtx[index])
roi_tex_coords.append(tex_coords[index])

x_min, y_min, x_max, y_max = 100, 100, 540, 380：这一行定义了ROI的边界，在此示例中，ROI是矩形框，其左上角坐标为 (100, 100)，右下角坐标为 (540, 380)。
width = 848 和 height = 480：这两行定义了深度图像的宽度和高度。在此示例中，深度图像的分辨率是 848x480。
roi_vtx = [] 和 roi_tex_coords = []：这两行初始化两个空列表，用于存储ROI内的顶点（vtx）和纹理坐标（tex_coords）。
接下来的嵌套循环遍历定义的ROI中的每个像素坐标 (x, y)：
- for y in range(y_min, y_max): 遍历ROI垂直方向上的每一行。
- for x in range(x_min, x_max): 遍历ROI水平方向上的每一列。
index = y * width + x：这一行计算当前 (x, y) 坐标在深度图像一维数组中的索引。深度图像的数据以一维数组形式存储，计算索引时需要将二维坐标 (x, y) 转换为一维索引。转换方法是：y 行乘以每行的像素数(width)，然后加上当前行的列坐标 x。
if vtx[index][2]:：这一行检查当前点的深度值是否非零（索引 [2] 表示 Z 轴，即深度）。如果深度值为零，表示该点没有有效的深度信息，因此不包含在ROI内的点云中。
roi_vtx.append(vtx[index]) 和 roi_tex_coords.append(tex_coords[index])：如果当前点具有有效的深度信息，则将点的顶点和纹理坐标追加到 roi_vtx 和 roi_tex_coords 列表中。

结果：

四、提取感兴趣区域点云的第二种实现代码

Python

import sys
import cv2
import pyrealsense2 as rs
import numpy as np
import keyboard
import open3d as o3d
import osif __name__ == "__main__":output_folder = 'output_data3/'os.makedirs(output_folder, exist_ok=True)pipeline = rs.pipeline()config = rs.config()# 定义感兴趣区域(ROI)的边界x_min, y_min, x_max, y_max = 100, 100, 540, 380config.enable_stream(rs.stream.depth, 848, 480, rs.format.z16, 30)config.enable_stream(rs.stream.color, 848, 480, rs.format.bgr8, 30)i = 0pipeline.start(config)align_to_color = rs.align(rs.stream.color)try:while True:# Wait for a coherent pair of frames: depth and colorframes = 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:continue# Convert images to numpy arraysdepth_image = np.asanyarray(depth_frame.get_data())color_image = np.asanyarray(color_frame.get_data())# Defining ROI on color imagecolor_image_roi = color_image[y_min:y_max, x_min:x_max]# Defining ROI on depth imagedepth_image_roi = depth_image[y_min:y_max, x_min:x_max]cv2.imshow("color_image", color_image)cv2.imshow("color_image_roi", color_image_roi)  # 显示ROI的彩色图像cv2.waitKey(1)color_image = cv2.cvtColor(color_image_roi, cv2.COLOR_BGR2RGB)depth_image = cv2.cvtColor(depth_image_roi, cv2.COLOR_BGR2RGB)if keyboard.is_pressed("a"):o3d_color = o3d.geometry.Image(color_image)o3d_depth = o3d.geometry.Image(depth_image)rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(o3d_color,o3d_depth,depth_scale=1000.0,depth_trunc=3.0,convert_rgb_to_intensity=False)profile = frames.get_profile()intrinsics = profile.as_video_stream_profile().get_intrinsics()# 转换为open3d中的相机参数pinhole_camera_intrinsic = o3d.camera.PinholeCameraIntrinsic(intrinsics.width, intrinsics.height,intrinsics.fx, intrinsics.fy,intrinsics.ppx, intrinsics.ppy)o3d_result = o3d.geometry.PointCloud.create_from_rgbd_image(rgbd_image,pinhole_camera_intrinsic)# Transform the point cloud to the original coordinate systemo3d_result.transform([[1, 0, 0, -x_min], [0, 1, 0, -y_min], [0, 0, 1, 0], [0, 0, 0, 1]])o3d.io.write_point_cloud(output_folder + str(i) + ".ply", o3d_result)i += 1if keyboard.is_pressed("q"):sys.exit()finally:pipeline.stop()

代码解析：

color_image_roi = color_image[y_min:y_max, x_min:x_max]
depth_image_roi = depth_image[y_min:y_max, x_min:x_max]
color_image = cv2.cvtColor(color_image_roi, cv2.COLOR_BGR2RGB)
depth_image = cv2.cvtColor(depth_image_roi, cv2.COLOR_BGR2RGB)
o3d_result.transform([[1, 0, 0, -x_min], [0, 1, 0, -y_min], [0, 0, 1, 0], [0, 0, 0, 1]])

color_image_roi = color_image[y_min:y_max, x_min:x_max]: 这行代码定义了彩色图像的一个区域兴趣（ROI），通过在原始彩色图像数组上使用数组切片功能。color_image 是一个NumPy数组，而 [y_min:y_max, x_min:x_max] 是一个切片操作，它从图像中提取从 y_min 到 y_max 行和从 x_min 到 x_max 列的像素。结果是一个新的图像数组，只包含原始彩色图像中指定矩形区域的像素。
depth_image_roi = depth_image[y_min:y_max, x_min:x_max]: 这行代码与上面的类似，它定义了深度图像的一个ROI。这意味着只有深度图像中的指定区域将被用于后续处理。在捕获彩色图像的同时，同样的区域也从深度图像中取出，以确保彩色和深度图像的对应关系。
color_image = cv2.cvtColor(color_image_roi, cv2.COLOR_BGR2RGB): 这行代码使用OpenCV库的 cvtColor 函数将ROI中的彩色图像从BGR颜色空间转换到RGB颜色空间。这通常是必要的，因为OpenCV默认使用BGR颜色顺序，而Open3D和其他许多图像处理库则预期图像为RGB颜色顺序。
depth_image = cv2.cvtColor(depth_image_roi, cv2.COLOR_BGR2RGB): 这行代码似乎不正确，因为深度图像通常是单通道的（灰度图），它只包含深度信息而没有颜色。因此，深度图像不需要从BGR转换到RGB，而应该保留原始格式。这可能是一个编码错误，应该将该行代码删除或替换为正确处理单通道深度图像的代码。
o3d_result.transform([[1, 0, 0, -x_min], [0, 1, 0, -y_min], [0, 0, 1, 0], [0, 0, 0, 1]]): 这行代码应用了一个4x4的仿射变换矩阵，以调整点云的坐标。在这个变换中，仅在x和y方向上应用了平移（-x_min 和 -y_min），目的是将点云对齐回其在原始图像中的相对位置。这是因为创建点云时仅使用了原始图像的一个小区域（ROI），而不是整个图像。