【点云处理教程】03使用 Python 实现地面检测

一、说明

        这是我的“点云处理”教程的第3篇文章。“点云处理”教程对初学者友好，我们将在其中简单地介绍从数据准备到数据分割和分类的点云处理管道。

        在上一教程中，我们在不使用 Open3D 库的情况下从深度数据计算点云。在本教程中，我们将首先描述系统坐标。然后，我们将以地面检测为例，仔细分析点云。我们还将介绍有组织的点云，这是一种有趣的3D表示。

作者的相机视野

这是我的“点云处理”教程的第3篇文章。“点云处理”教程对初学者友好，我们将在其中简单地介绍从数据准备到数据分割和分类的点云处理管道。

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

二、计算机视觉坐标系

        在开始之前，了解计算机视觉中的传统坐标系非常重要。它们在Open3D [1]和Microsoft Kinect传感器[2]中紧随其后。在计算机视觉中，图像以独立的 2D 坐标系表示，其中 X 轴从左到右指向，Y 轴从上到下指向。对于相机，3D 坐标系原点位于相机的焦点，其中 X 轴指向右，Y 轴指向下方，Z 轴指向前方。

        计算机视觉坐标系

        我们首先导入所需的库：

import numpy as np
import open3d as o3d

        为了更好地理解，让我们从 PLY 文件导入点云，使用 Open3D 创建默认 3D 坐标系并显示它们：

# Read point cloud:
pcd = o3d.io.read_point_cloud("data/depth_2_pcd.ply")
# Create a 3D coordinate system:
origin = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.5)
# geometries to draw:
geometries = [pcd, origin]
# Visualize:
o3d.visualization.draw_geometries(geometries)

        显示的具有坐标系原点的点云。蓝色箭头是 Z 轴，红色箭头是 X 轴，绿色箭头是 Y 轴。

        知道蓝色、红色和绿色箭头分别表示 Z 轴、X 轴和 Y 轴，您可以看到点云与 Open3D 坐标系表示在同一坐标系中。现在，让我们得到每个轴具有最小值和最大值的点： 

# Get max and min points of each axis x, y and z:
x_max = max(pcd.points, key=lambda x: x[0])
y_max = max(pcd.points, key=lambda x: x[1])
z_max = max(pcd.points, key=lambda x: x[2])
x_min = min(pcd.points, key=lambda x: x[0])
y_min = min(pcd.points, key=lambda x: x[1])
z_min = min(pcd.points, key=lambda x: x[2])

        我们可以打印它们，但为了获得更好的可视化效果，我们在每个点位置创建一个球体几何形状。默认情况下，Open3D 在原点位置创建 3D 几何体：

要将球体移动到给定位置，需要进行平移转换。在下面的示例中，球体由向量 [1，1，1] 平移：

让我们回到我们的示例，为每个球体分配一种颜色。对于每个位置，我们创建一个球体并将其转换为该位置。然后，我们分配正确的颜色，最后我们将其添加到最后显示。

# Colors:
RED = [1., 0., 0.]
GREEN = [0., 1., 0.]
BLUE = [0., 0., 1.]
YELLOW = [1., 1., 0.]
MAGENTA = [1., 0., 1.]
CYAN = [0., 1., 1.]positions = [x_max, y_max, z_max, x_min, y_min, z_min]
colors = [RED, GREEN, BLUE, MAGENTA, YELLOW, CYAN]
for i in range(len(positions)):# Create a sphere mesh:sphere = o3d.geometry.TriangleMesh.create_sphere(radius=0.05)# move to the point position:sphere.translate(np.asarray(positions[i]))# add color:sphere.paint_uniform_color(np.asarray(colors[i]))# compute normals for vertices or faces:sphere.compute_vertex_normals()# add to geometry list to display later:geometries.append(sphere)# Display:
o3d.visualization.draw_geometries(geometries)

        嗯，我们可以看到对应的黄色球体在墙上，对应的绿色球体在地面上。实际上，Y 轴表示点的高度：在现实世界中，最高的球体是黄色的，最低的球体是绿色的球体。但是，由于 Y 轴指向下方，因此黄色球体具有最小值，绿色球体具有最大值。y_min y_max

        另一个有趣的球体是原点上的青色球体。正如我们在上一教程中提到的，深度值为 0 的像素是噪声点，因此位于原点的点是从这些噪声像素计算的点（当和 ）。z=0x=0y=0

三、 地面检测

        现在我们展示了一些要点，如何检测地面？在前面的示例中，绿色球体位于地面上。确切地说，它与沿 Y 轴的最高点相对应的中心是一个接地点。假设为了检测地面，我们将所有必须的点的颜色更改为绿色。y_max

        如果显示点云，您会注意到并非所有地面点都为绿色。事实上，只有一个与前一个绿色球体的中心相对应的点是绿色的。这是由于深度相机的准确性和噪点水平。

        为了克服此限制，我们需要添加一个阈值，以便将具有 y 坐标的点都视为地面点。为此，在得到之后，我们检查每个点的 y 坐标是否在该区间内，然后将其颜色设置为绿色。最后，我们更新点云的颜色属性并显示结果。[y_max-threshold, y_max]y_max

# Define a threshold:
THRESHOLD = 0.075# Get the max value along the y-axis:
y_max = max(pcd.points, key=lambda x: x[1])[1]# Get the original points color to be updated:
pcd_colors = np.asarray(pcd.colors)# Number of points:
n_points = pcd_colors.shape[0]# update color:
for i in range(n_points):# if the current point is aground point:if pcd.points[i][1] >= y_max - THRESHOLD:pcd_colors[i] = GREEN  # color it greenpcd.colors = o3d.utility.Vector3dVector(pcd_colors)# Display:
o3d.visualization.draw_geometries([pcd, origin])

        在此示例中，我们仅将表示地面的点着色为绿色。在实际应用中，提取地面以定义可步行区域，如机器人或视障系统，或将物体放在其上，如室内设计系统。它也可以被移除，因此剩余的点可以像在场景理解和对象检测系统中一样进行分割或分类。

四、有组织的点云

        在我们的第一个教程中，我们将点云定义为一组 3D 点。集合是一种无序结构，因此由集合表示的点云称为无组织点云。与 RGB 矩阵类似，有组织的点云是一个 2D 矩阵，具有 3 个通道，表示点的 x、y 和 z 坐标。矩阵结构提供了相邻点之间的关系，从而降低了某些算法（如最近邻）的时间复杂性。

        例如，假设我们正在撰写一篇研究论文，我们希望将地面检测算法的结果显示为图表。不幸的是，无法选择动画人物。因此，我们可以截取点云的屏幕截图，也可以在深度图像上显示结果，如下图所示。在我看来，第二种选择是最好的。在这种情况下，需要一个有组织的点云来保留深度像素的位置。

左：3D 可视化的屏幕截图。右：深度图像的结果。

        让我们从上一个深度图像创建一个有组织的点云。我们首先，像上一篇文章中那样导入相机参数。我们还导入深度图像并将其转换为 3 通道灰度图像，以便我们可以将地面像素设置为绿色：

import imageio.v3 as iio
import numpy as np
import matplotlib.pyplot as plt# Camera parameters:
FX_DEPTH = 5.8262448167737955e+02
FY_DEPTH = 5.8269103270988637e+02
CX_DEPTH = 3.1304475870804731e+02
CY_DEPTH = 2.3844389626620386e+02# Read depth image:
depth_image = iio.imread('../data/depth_2.png')
# Compute the grayscale image:
depth_grayscale = np.array(256 * depth_image / 0x0fff, dtype=np.uint8)
# Convert a grayscale image to a 3-channel image:
depth_grayscale = np.stack((depth_grayscale,) * 3, axis=-1)

        为了计算有组织的点云，我们按照与上一个教程中相同的方式进行。我们没有展平深度图像，而是重塑并具有与深度图像相同的形状，如下所示：jjii

# get depth image resolution:
height, width = depth_image.shape
# compute indices and reshape it to have the same shape as the depth image:
jj = np.tile(range(width), height).reshape((height, width))
ii = np.repeat(range(height), width).reshape((height, width))
# Compute constants:
xx = (jj - CX_DEPTH) / FX_DEPTH
yy = (ii - CY_DEPTH) / FY_DEPTH
# compute organised point cloud:
organized_pcd = np.dstack((xx * depth_image, yy * depth_image, depth_image))

        如果打印创建的点云的形状，您可以看到它是具有3个通道的矩阵。如果您发现此代码难以理解，请返回上一个教程，如果仍然不清楚，请随时给我留下您的问题，我将很乐意为您提供帮助。(480, 640, 3)

        同样，我们像上面一样检测地面，但不是更新点的颜色并显示点云，而是更新灰度图像的像素并显示它：

# Ground_detection:
THRESHOLD = 0.075 * 1000  # Define a threshold
y_max = max(organized_pcd.reshape((height * width, 3)), key=lambda x: x[1])[1]  # Get the max value along the y-axis# Set the ground pixels to green:
for i in range(height):for j in range(width):if organized_pcd[i][j][1] >= y_max - THRESHOLD:depth_grayscale[i][j] = [0, 255, 0]  # Update the depth image# Display depth_grayscale:
plt.imshow(depth_grayscale)
plt.show()

五、结论

        在本教程中，为了习惯点云，我们引入了默认坐标系，并实现了一个简单的地面检测算法。事实上，地面探测在某些应用中（如导航）是一项重要任务，文献中已经提出了几种算法。实现的算法很简单;它将最低点视为地面。然而，它的局限性在于深度相机必须与地面平行，而大多数实际应用并非如此。