一、第一种实现代码
Python
import sys
import cv2
import pyrealsense2 as rs
import numpy as np
import keyboard
import open3d as o3d
import os
if __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 streaming
pipeline.start(config)
# 深度图像向彩色对齐
align_to_color = rs.align(rs.stream.color)
i = 0
try:
while True:
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!")
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.0
print("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 += 1
if keyboard.is_pressed("q"):
sys.exit()
finally:
# Stop streaming
pipeline.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 os
if __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 = 0
pipeline.start(config)
align_to_color = rs.align(rs.stream.color)
try:
while True:
# 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()
depth_intrinsics = depth_frame.profile.as_video_stream_profile().intrinsics
profile = frames.get_profile()
if not depth_frame or not color_frame:
continue
# Convert images to numpy arrays
depth_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 += 1
if 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 os
if __name__ == "__main__":
output_folder = 'output_data/'
os.makedirs(output_folder, exist_ok=True)
width = 848
height = 480
pipeline = 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 = 0
try:
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:
continue
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.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 + x
if 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 += 1
if 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 os
if __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, 380
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 = 0
pipeline.start(config)
align_to_color = rs.align(rs.stream.color)
try:
while True:
# 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:
continue
# Convert images to numpy arrays
depth_image = np.asanyarray(depth_frame.get_data())
color_image = np.asanyarray(color_frame.get_data())
# Defining ROI on color image
color_image_roi = color_image[y_min:y_max, x_min:x_max]
# Defining ROI on depth image
depth_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 system
o3d_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 += 1
if 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),而不是整个图像。
结果: