一、说明

这是“点云处理”教程的第二篇文章。“点云处理”教程对初学者友好，我们将在其中简单地介绍从数据准备到数据分割和分类的点云处理管道。在本教程中，我们将学习如何在不使用 Open3D 库的情况下从深度图像计算点云。我们还将展示如何优化代码以获得更好的性能。

第1条：点云处理简介
文章2：在Python中从深度图像估计点云
文章3：了解点云：使用Python实现地面检测
文章4：Python中的点云过滤
文章 5 ： Python 中的点云分割

二. 深度图像

深度图像（也称为深度图）是每个像素提供其相对于传感器坐标系的距离值的图像。深度图像可以通过结构光或飞行时间传感器捕获。为了计算深度数据，结构光传感器（如 Microsoft Kinect V1）会比较投影光和接收光之间的失真。至于像Kinect V2 Microsoft这样的飞行时间传感器，它们投射光线，然后计算投影和接收这些光线之间的时间。

除了深度图像外，一些传感器还提供相应的RGB图像以形成RGB-D图像。后者使得计算彩色点云成为可能。本教程将使用Microsoft Kinect V1 RGB-D 图像作为示例。

让我们从导入库开始：

import imageio.v3 as iio
import numpy as np
import matplotlib.pyplot as plt
import open3d as o3d

现在，我们可以导入深度图像并打印其分辨率和类型：

# Read depth image:
depth_image = iio.imread('data/depth.png')

# print properties:
print(f"Image resolution: {depth_image.shape}")
print(f"Data type: {depth_image.dtype}")
print(f"Min value: {np.min(depth_image)}")
print(f"Max value: {np.max(depth_image)}")

Image resolution: (480, 640)
Data type: int32
Min value: 0
Max value: 2980

深度图像是大小为 640×480 的矩阵，其中每个像素是一个 32（或 16）位整数，表示以毫米为单位的距离，因此深度图像在打开时显示为黑色（见下图）。最小值 0 表示噪点（没有距离），而最大值 2980 表示最远像素的距离。

由 Microsoft Kinect V1 生成的深度图像。

为了获得更好的可视化效果，我们计算其灰度图像：

depth_instensity = np.array(256 * depth_image / 0x0fff, dtype=np.uint8)
iio.imwrite('output/grayscale.png', depth_instensity)

计算灰度图像意味着将深度值缩放到。现在图像更清晰了：[0, 255]

计算出的灰度图像。黑色像素表示噪点。

请注意，Matplotlib 在可视化深度图像时也会做同样的事情：

# Display depth and grayscale image:
fig, axs = plt.subplots(1, 2)
axs[0].imshow(depth_image, cmap="gray")
axs[0].set_title('Depth image')
axs[1].imshow(depth_grayscale, cmap="gray")
axs[1].set_title('Depth grayscale image')
plt.show()

三. 点云

现在我们已经导入并显示了深度图像，我们如何从中估计点云？第一步是校准深度相机以估计相机矩阵，然后使用它来计算点云。获得的点云也称为2.5D点云，因为它是根据2D投影（深度图像）而不是激光传感器等3D传感器估计的。

3.2 深度相机校准

校准相机意味着通过查找失真系数和相机矩阵（也称为固有参数）来估计镜头和传感器参数。一般来说，校准相机有三种方法：使用工厂提供的标准参数、使用校准研究中获得的结果或手动校准 Kinect。手动校准相机包括应用一种校准算法，例如棋盘算法[1]。该算法在机器人操作系统（ROS）和OpenCV中实现。校准矩阵 M 是一个 3×3 矩阵：

其中fx，fy和cx，cy分别是焦距和光学中心。在本教程中，我们将使用获得的纽约大学深度 V2 数据集的结果：

# Depth camera parameters:
FX_DEPTH = 5.8262448167737955e+02
FY_DEPTH = 5.8269103270988637e+02
CX_DEPTH = 3.1304475870804731e+02
CY_DEPTH = 2.3844389626620386e+02

如果您想自己校准相机，可以参考此OpenCV教程。

3.3 点云计算

这里的计算点云是指将深度像素从深度图像2D坐标系转换为深度相机3D坐标系（x，y和z）。3D 坐标使用以下公式 [2] 计算，其中 depth（i， j） 是行 i 和列 j 处的深度值：

该公式应用于每个像素：

# compute point cloud:
pcd = []
height, width = depth_image.shape
for i in range(height):
   for j in range(width):
       z = depth_image[i][j]
       x = (j - CX_DEPTH) * z / FX_DEPTH
       y = (i - CY_DEPTH) * z / FY_DEPTH
       pcd.append([x, y, z])

让我们使用 Open3D 库显示它：

pcd_o3d = o3d.geometry.PointCloud()  # create point cloud object
pcd_o3d.points = o3d.utility.Vector3dVector(pcd)  # set pcd_np as the point cloud points
# Visualize:
o3d.visualization.draw_geometries([pcd_o3d])

从深度图像计算出的点云。

四. 彩色点云

如果我们想从RGB-D图像中计算彩色点云怎么办？颜色信息可以增强许多任务（如点云配准）的性能。在这种情况下，如果输入传感器也提供RGB图像，则最好使用它。彩色点云可以定义如下：

其中 x、y 和 z 是 3D 坐标，r、g 和 b 表示 RGB 系统中的颜色。

我们首先导入上一个深度图像的相应 RGB 图像：

# Read the rgb image:
rgb_image = iio.imread('../data/rgb.jpg')

# Display depth and grayscale image:
fig, axs = plt.subplots(1, 2)
axs[0].imshow(depth_image, cmap="gray")
axs[0].set_title('Depth image')
axs[1].imshow(rgb_image)
axs[1].set_title('RGB image')
plt.show()

深度图像及其对应的 RGB 图像

要查找深度传感器 3D 坐标系中定义的给定点 p（x， y，z）的颜色，请执行以下操作：

1. 我们将其转换为 RGB 相机坐标系 [2]：

其中 R 和 T 是两个相机之间的外在参数：分别是旋转矩阵和平移矢量。

同样，我们使用来自纽约大学深度V2数据集的参数：

# Rotation matrix:
R = -np.array([[9.9997798940829263e-01, 5.0518419386157446e-03, 4.3011152014118693e-03],
                   [-5.0359919480810989e-03, 9.9998051861143999e-01, -3.6879781309514218e-03],
                   [- 4.3196624923060242e-03, 3.6662365748484798e-03, 9.9998394948385538e-01]])
# Translation vector:
T = np.array([2.5031875059141302e-02, -2.9342312935846411e-04, 6.6238747008330102e-04])

RGB 照相机坐标系中的点计算方法如下：

"""
  Convert the point from depth sensor 3D coordinate system
  to rgb camera coordinate system:
"""
[x_RGB, y_RGB, z_RGB] = np.linalg.inv(R).dot([x, y, z]) - np.linalg.inv(R).dot(T)

2. 使用 RGB 相机的固有参数，我们将其映射到彩色图像坐标系 [2]：

这些是获取颜色像素的索引。

请注意，在前面的公式中，焦距和光学中心是RGB相机参数。同样，我们使用来自纽约大学深度V2数据集的参数：

# RGB camera intrinsic Parameters:
FX_RGB = 5.1885790117450188e+02
FY_RGB = 5.1946961112127485e+02
CX_RGB = 3.2558244941119034e+0
CY_RGB = 2.5373616633400465e+02

对应像素的指数计算如下：

"""
  Convert from rgb camera coordinate system
  to rgb image coordinate system:
"""
j_rgb = int((x_RGB * FX_RGB) / z_RGB + CX_RGB + width / 2)
i_rgb = int((y_RGB * FY_RGB) / z_RGB + CY_RGB)

让我们把所有东西放在一起，显示点云：

colors = []
pcd = []
for i in range(height):
    for j in range(width):
        """
            Convert the pixel from depth coordinate system
            to depth sensor 3D coordinate system
        """
        z = depth_image[i][j]
        x = (j - CX_DEPTH) * z / FX_DEPTH
        y = (i - CY_DEPTH) * z / FY_DEPTH

        """
            Convert the point from depth sensor 3D coordinate system
            to rgb camera coordinate system:
        """
        [x_RGB, y_RGB, z_RGB] = np.linalg.inv(R).dot([x, y, z]) - np.linalg.inv(R).dot(T)

        """
            Convert from rgb camera coordinates system
            to rgb image coordinates system:
        """
        j_rgb = int((x_RGB * FX_RGB) / z_RGB + CX_RGB + width / 2)
        i_rgb = int((y_RGB * FY_RGB) / z_RGB + CY_RGB)

        # Add point to point cloud:
        pcd.append([x, y, z])

        # Add the color of the pixel if it exists:
        if 0 <= j_rgb < width and 0 <= i_rgb < height:
            colors.append(rgb_image[i_rgb][j_rgb] / 255)
        else:
            colors.append([0., 0., 0.])
            
# Convert to Open3D.PointCLoud:
pcd_o3d = o3d.geometry.PointCloud()  # create a point cloud object
pcd_o3d.points = o3d.utility.Vector3dVector(pcd)
pcd_o3d.colors = o3d.utility.Vector3dVector(colors)
# Visualize:
o3d.visualization.draw_geometries([pcd_o3d])

从RGB-D图像计算出的彩色点云

五、代码优化

在本节中，我们将介绍如何优化代码，使其更高效并适合实时应用程序。

5.1 点云

使用嵌套循环计算点云非常耗时。对于分辨率为 480×640 的深度图像，在具有 8GB RAM 和 i7-4500 CPU 的机器上，计算点云大约需要 2.154 秒。

为了减少计算时间，嵌套循环可以用矢量化操作代替，计算时间可以减少到0.024秒左右：

# get depth resolution:
height, width = depth_im.shape
length = height * width
# compute indices:
jj = np.tile(range(width), height)
ii = np.repeat(range(height), width)
# rechape depth image
z = depth_im.reshape(length)
# compute pcd:
pcd = np.dstack([(ii - CX_DEPTH) * z / FX_DEPTH,
                 (jj - CY_DEPTH) * z / FY_DEPTH,
                 z]).reshape((length, 3))

我们还可以通过在开始时计算一次常数来将计算时间减少到大约 0.015 秒：

# compute indices:
jj = np.tile(range(width), height)
ii = np.repeat(range(height), width)
# Compute constants:
xx = (jj - CX_DEPTH) / FX_DEPTH
yy = (ii - CY_DEPTH) / FY_DEPTH
# transform depth image to vector of z:
length = height * width
z = depth_image.reshape(height * width)
# compute point cloud
pcd = np.dstack((xx * z, yy * z, z)).reshape((length, 3))

4.2 彩色点云

至于彩色点云，在同一台机器上，执行前面的示例大约需要 36.263 秒。通过应用矢量化，运行时间减少到 0.722 秒。

# compute indices:
jj = np.tile(range(width), height)
ii = np.repeat(range(height), width)

# Compute constants:
xx = (jj - CX_DEPTH) / FX_DEPTH
yy = (ii - CY_DEPTH) / FY_DEPTH

# transform depth image to vector of z:
length = height * width
z = depth_image.reshape(length)

# compute point cloud
pcd = np.dstack((xx * z, yy * z, z)).reshape((length, 3))
cam_RGB = np.apply_along_axis(np.linalg.inv(R).dot, 1, pcd) - np.linalg.inv(R).dot(T)
xx_rgb = ((cam_RGB[:, 0] * FX_RGB) / cam_RGB[:, 2] + CX_RGB + width / 2).astype(int).clip(0, width - 1)
yy_rgb = ((cam_RGB[:, 1] * FY_RGB) / cam_RGB[:, 2] + CY_RGB).astype(int).clip(0, height - 1)
colors = rgb_image[yy_rgb, xx_rgb]/255