【点云处理教程】02从 Python 中的深度图像估计点云

二. 深度图像

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

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

        让我们从导入库开始：

1. import imageio.v3 as iio
2. import numpy as np
3. import matplotlib.pyplot as plt
4. import open3d as o3d

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

1. # Read depth image:
2. depth_image = iio.imread('data/depth.png')
4. # print properties:
5. print(f"Image resolution: {depth_image.shape}")
6. print(f"Data type: {depth_image.dtype}")
7. print(f"Min value: {np.min(depth_image)}")
8. 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 生成的深度图像。

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

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

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

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

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

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

三. 点云

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

3.2 深度相机校准

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

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

1. # Depth camera parameters:
2. FX_DEPTH = 5.8262448167737955e+02
3. FY_DEPTH = 5.8269103270988637e+02
4. CX_DEPTH = 3.1304475870804731e+02
5. CY_DEPTH = 2.3844389626620386e+02

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

3.3 点云计算

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

该公式应用于每个像素：

1. # compute point cloud:
2. pcd = []
3. height, width = depth_image.shape
4. for i in range(height):
5. for j in range(width):
6. z = depth_image[i][j]
7. x = (j - CX_DEPTH) * z / FX_DEPTH
8. y = (i - CY_DEPTH) * z / FY_DEPTH
9. pcd.append([x, y, z])

让我们使用 Open3D 库显示它：

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

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

四. 彩色点云

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

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

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

1. # Read the rgb image:
2. rgb_image = iio.imread('../data/rgb.jpg')
4. # Display depth and grayscale image:
5. fig, axs = plt.subplots(1, 2)
6. axs[0].imshow(depth_image, cmap="gray")
7. axs[0].set_title('Depth image')
8. axs[1].imshow(rgb_image)
9. axs[1].set_title('RGB image')
10. plt.show()

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

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

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

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

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

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

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

1. """
2. Convert the point from depth sensor 3D coordinate system
3. to rgb camera coordinate system:
4. """
5. [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数据集的参数：

1. # RGB camera intrinsic Parameters:
2. FX_RGB = 5.1885790117450188e+02
3. FY_RGB = 5.1946961112127485e+02
4. CX_RGB = 3.2558244941119034e+0
5. CY_RGB = 2.5373616633400465e+02

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

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

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

1. colors = []
2. pcd = []
3. for i in range(height):
4. for j in range(width):
5. """
6. Convert the pixel from depth coordinate system
7. to depth sensor 3D coordinate system
8. """
9. z = depth_image[i][j]
10. x = (j - CX_DEPTH) * z / FX_DEPTH
11. y = (i - CY_DEPTH) * z / FY_DEPTH
13. """
14. Convert the point from depth sensor 3D coordinate system
15. to rgb camera coordinate system:
16. """
17. [x_RGB, y_RGB, z_RGB] = np.linalg.inv(R).dot([x, y, z]) - np.linalg.inv(R).dot(T)
19. """
20. Convert from rgb camera coordinates system
21. to rgb image coordinates system:
22. """
23. j_rgb = int((x_RGB * FX_RGB) / z_RGB + CX_RGB + width / 2)
24. i_rgb = int((y_RGB * FY_RGB) / z_RGB + CY_RGB)
26. # Add point to point cloud:
27. pcd.append([x, y, z])
29. # Add the color of the pixel if it exists:
30. if 0 <= j_rgb < width and 0 <= i_rgb < height:
31. colors.append(rgb_image[i_rgb][j_rgb] / 255)
32. else:
33. colors.append([0., 0., 0.])
35. # Convert to Open3D.PointCLoud:
36. pcd_o3d = o3d.geometry.PointCloud() # create a point cloud object
37. pcd_o3d.points = o3d.utility.Vector3dVector(pcd)
38. pcd_o3d.colors = o3d.utility.Vector3dVector(colors)
39. # Visualize:
40. o3d.visualization.draw_geometries([pcd_o3d])

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

五、代码优化

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

5.1 点云

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

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

1. # get depth resolution:
2. height, width = depth_im.shape
3. length = height * width
4. # compute indices:
5. jj = np.tile(range(width), height)
6. ii = np.repeat(range(height), width)
7. # rechape depth image
8. z = depth_im.reshape(length)
9. # compute pcd:
10. pcd = np.dstack([(ii - CX_DEPTH) * z / FX_DEPTH,
11. (jj - CY_DEPTH) * z / FY_DEPTH,
12. z]).reshape((length, 3))

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

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

4.2 彩色点云

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

1. # compute indices:
2. jj = np.tile(range(width), height)
3. ii = np.repeat(range(height), width)
5. # Compute constants:
6. xx = (jj - CX_DEPTH) / FX_DEPTH
7. yy = (ii - CY_DEPTH) / FY_DEPTH
9. # transform depth image to vector of z:
10. length = height * width
11. z = depth_image.reshape(length)
13. # compute point cloud
14. pcd = np.dstack((xx * z, yy * z, z)).reshape((length, 3))
15. cam_RGB = np.apply_along_axis(np.linalg.inv(R).dot, 1, pcd) - np.linalg.inv(R).dot(T)
16. xx_rgb = ((cam_RGB[:, 0] * FX_RGB) / cam_RGB[:, 2] + CX_RGB + width / 2).astype(int).clip(0, width - 1)
17. yy_rgb = ((cam_RGB[:, 1] * FY_RGB) / cam_RGB[:, 2] + CY_RGB).astype(int).clip(0, height - 1)
18. colors = rgb_image[yy_rgb, xx_rgb]/255

六. 结论

        在本教程中，我们学习了如何从 RGB-D 数据计算点云。在下一个教程中，我们将以一个简单的地面检测为例，仔细分析点云。

谢谢，我希望你喜欢阅读这篇文章。您可以在我的 GitHub 存储库中找到示例。

引用

[1] Zhang， S.， & Huang， P. S. （2006）.结构光系统校准的新方法。光学工程， 45（8）， 083601.

[2] 周旭， （2012）.Microsoft Kinect 校准的研究。费尔法克斯乔治梅森大学计算机科学系。