农业采摘--RGBD数据转point cloud

一、RGBD图像转点云数据的步骤

将RGBD图像转点云数据常包含五个步骤：
1. 图像采集： 使用RGBD相机同时捕获颜色（RGB）和深度（Depth）信息。颜色记录了场景的彩色视觉信息，而深度图像记录了场景中每个像素点到相机的距离。

2. 获取相机内参： 相机内参是一组参数，描述了图像像素坐标到相机坐标系坐标中点的映射关系。它通常包含焦距（fx, fy）和光学中心（cx, cy）等参数，定义了一个3x3的矩阵。这些参数用于将2D图像坐标转换为3D相机坐标。

3. 深度图处理： 深度图中的像素值通常以16位或32位整数格式存储，表示每个像素点到相机的距离。这个距离需要转换成实际的物理单位（cm或m）。

4. 像素坐标到相机坐标的转换公式： 对于深度图中的每个非零像素（表示有效的深度信息），使用一下步骤转换为相机坐标系中的点：

X = (u - cx) * Z / fx #(u,v)表示图像坐标；(X,Y,Z)表示相机坐标
Y = (v - cy) * Z / fy
Z = Z  # 深度值已经以米为单位

5. 点云创建： 将转换后的三维坐标(X,Y,Z)与对应的RGB颜色值相结合，形成点云数据。每个点包括位置信息和颜色信息。

使用Open3D中的PointCloud类来存储所有点的位置和颜色信息。

二、 RGBD转Point Cloud的实例

"""
RGBD转Point Cloud数据
"""
import cv2
import open3d as o3d
import numpy as np

# 相机内参（需要根据你的相机进行设置）
camera_intrinsics = np.array([[518.8579, 0, 325.1451],
                              [0, 519.2371, 249.7019],
                              [0, 0, 1]])

# 读取RGB图像
rgb_image_path = "RGB图像存储路径"
rgb_image = cv2.imread(rgb_image_path)
rgb_image = cv2.cvtColor(rgb_image, cv2.COLOR_BGR2RGB)

# 读取深度图像
depth_image_path = "Depth图像存储路径"
depth_image = cv2.imread(depth_image_path, cv2.IMREAD_UNCHANGED)

# 转换深度图像为meters
depth_image = depth_image.astype(np.float32) / 1000.0  # 假设深度图像是以毫米为单位的

# 创建点云
height, width = depth_image.shape
points = np.zeros((height, width, 3), dtype=np.float32)
colors = np.zeros((height, width, 3), dtype=np.float32)

for v in range(height):
    for u in range(width):
        Z = depth_image[v, u]
        if Z == 0:
            continue  # 忽略深度为0的点
        X = (u - camera_intrinsics[0, 2]) * Z / camera_intrinsics[0, 0]
        Y = (v - camera_intrinsics[1, 2]) * Z / camera_intrinsics[1, 1]
        points[v, u] = [X, Y, Z]
        colors[v, u] = rgb_image[v, u] / 255.0  # 归一化颜色值

# 将点云数据转换为Open3D格式
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(points.reshape(-1, 3))
pcd.colors = o3d.utility.Vector3dVector(colors.reshape(-1, 3))

# 保存点云为PLY文件
output_path = "创建的点云文件存储路径" #"\output_point_cloud.pcd"
o3d.io.write_point_cloud(output_path, pcd)

print(f'点云已保存到 {output_path}')

"""
打开所创建的点云数据
"""
import open3d as o3d
import numpy as np

print("->正在加载点云... ")
pcd_path = "所创建的点云的存储路径"
pcd = o3d.io.read_point_cloud(pcd_path)
print(pcd)

print("->正在可视化点云")
o3d.visualization.draw_geometries([pcd])

测试用例：
RGB图：

Depth图像

测试结果：