在本教程中，我们将学习如何在不使用 Open3D 库的情况下从深度图像计算点云。我们还将展示如何优化代码以获得更好的性能。

1. 深度图像

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

除了深度图像外，一些传感器还提供其对应的RGB图像以形成RGB- D图像。这使得计算彩色点云成为可能。本教程将以微软Kinect V1 RGB-D图像为例。

让我们从导入Python库开始:

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表示最远像素的距离。

为了更好的可视化，我们计算它的灰度图像：

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.点云

现在我们已经导入并显示了深度图像，我们如何根据它估计点云呢?首先对深度相机进行标定，估计相机矩阵，然后用它来计算点云。得到的点云也被称为2.5D点云，因为它是从 2D 投影（深度图像）而不是 3D 传感器（如激光传感器）估计的。

2.1 深度相机标定

标定相机意味着通过寻找畸变系数和相机矩阵来估计镜头和传感器参数。一般来说，标定相机有三种方法:使用工厂提供的标准参数，使用标定研究中获得的结果或手动标定Kinect。手动标定包括标定算法，如棋盘格标定法。标定矩阵M是一个3×3矩阵:

其中fx、fy和cx、cy分别为焦距和光心。对于本教程，我们将使用NYU Depth V2数据集获得的结果:

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

2.2 点云计算

这里计算点云意味着将深度像素从深度图像2D坐标系转换到深度相机3D坐标系(x, y和z)。3D坐标使用以下公式计算，其中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])

3.彩色点云

如果我们想从RGB-D图像中计算彩色点云怎么办呢?颜色信息可以提高点云配准等许多任务的性能。彩色点云的定义如下:

其中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()

要查找深度传感器 3D 坐标系中定义的给定点p(x, y,z)的颜色：

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

其中R和T为两个相机之间的外部参数:分别为旋转矩阵和平移向量。

类似地，我们使用NYU Depth 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相机的固有参数，将其映射到彩色图像坐标系

注意，在前面的公式中，焦距和光心是RGB相机的参数。类似地，我们使用NYU Depth 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])

4.代码优化

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

4.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]

5. 结论

在本教程中，我们学习了如何从 RGB-D 数据来计算点云。