深度相机采集RGB+D转点云pcd

1、采集数据

import pyrealsense2 as rs
import numpy as np
import keyboard
import cv2
import open3d as o3d
#进行一些初始化，配置一些相机参数
pipeline = rs.pipeline()
config = rs.config()
#这里需要知道RGB彩色图像和D深度图像的分辨率，否则报错。（另外用opencv的话，是BGR通道）
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
#使用上述初始化配置开始采集数据流
pipeline.start(config)
# 创建对齐对象（深度对齐颜色）
align_to = rs.stream.color
align = rs.align(align_to)
#声明点云对象
pc = rs.pointcloud()
#创建pcd类型的数据对象
cloud_3d = o3d.geometry.PointCloud()

#下面其实就类似opencv使用循环一帧一帧读取视频，并显示出来
try:
	#创建1个计数器，用于后面保存数据的文件名；以及类别名
	counter = 0
    obj_class = 'Hello_Wolrd'
    while True:
    	#获取深度图和彩色图
        frames = 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())
        
		#显示出深度图和RGB
        cv2.imshow('depth', depth_image * 20)
        cv2.imshow('color', color_image)
        cv2.waitKey(1)
        #使用keyboard监听键盘事件
        if keyboard.is_pressed('F3'):
        	#根据当前帧深度图的可迭代对象点计算出每个点的x，y，z，并转numpy。
    		 points = pc.calculate(depth_frame)
    		 vtx = np.asanyarray(points.get_vertices())
     		save_pcd(vtx, color_image, counter, obj_class)
     		
			"""下面是保存RGB图像"""
            save_path = 'D:/data/yolo_rgb/' + str(obj_class) + str(counter) + '_rgb' + '.png'
            cv2.imwrite(save_path, color_image)
            print(f'{str(obj_class) + str(counter)}_rgb_png已保存')

            """下面是保存D深度图像"""
            save_path2 = 'D:/data/yolo_d/' + str(counter) + '_d' + '.png'
            cv2.imwrite(save_path2, depth_image * 20)
            print(f'{str(obj_class) + str(counter)}_d_png已保存')
			
			#计数器+1
            counter += 1  

        if keyboard.is_pressed('Esc'):
            break
finally:
        # Stop streaming
        pipeline.stop()

2、保存pcd数据

def save_pcd(pt, color, counter, obj_class):
    # pt (307200,)  就是D深度图的像素数
	#遍历传进来的每个点的位置，存放进列表，并转nunmpy。
    non_color_point = [[pt[i][0], pt[i][1], pt[i][2]] for i in range(len(pt))]
    non_color_point = np.array(non_color_point, dtype=np.float32)
    # 生成open3d点云
	#将点云转换成open3d中的数据形式并用pcd来保存，以方便用open3d处理
	#o3d.utility.Vector3dVector()这个就是将所有点的数据保存成pcd数据格式，比如下面保存每个点的颜色信息，就需要将RGB图像维度变为-->(307200, 3) 。
	# cloud_3d.points就是点的位置信息x，y，z，cloud_3d.colors就是RGB颜色信息
    cloud_3d.points = o3d.utility.Vector3dVector(non_color_point)
	#将RGB图像的颜色信息保存到点云数据里去
    color_point = np.asarray(color.reshape(-1, 3))
    color_point = color_point/255.0
    color_point[:,[0,2]] = color_point[:,[2,0]]
    cloud_3d.colors = o3d.utility.Vector3dVector(color_point)
    
    point_save_path = 'D:/data/point_data/' + str(obj_class) + str(counter) + '.pcd'
    o3d.io.write_point_cloud(point_save_path, cloud_3d)
    print(f'{str(obj_class) + str(counter)}pcd已保存')