使用迭代最近点（ICP）算法在 Open3D 中对齐点云

news2024/11/17 17:38:34

一、Open3D 简介及其功能

Open3D 是一个现代库，它提供了用于处理 3D 数据的各种工具。在其功能中，它提供了高效的数据结构和算法来处理点云、网格等，使其成为在计算机视觉、机器人和图形领域工作的研究人员和从业人员的不错选择。Open3D 的特点之一是它实现了迭代最近点（ICP）算法，该算法用于模型对齐任务。

二、Open3D 和 ICP 入门

迭代最近点（ICP）算法是用于对齐 3D 模型的基本技术。它的工作原理是迭代最小化两个点云或一个点云与 3D 模型之间的距离。该算法假设两个点云在不同的方向和/或位置表示相同的对象或场景。ICP 对于机器人和增强现实中的对象识别、定位和映射等任务特别有用。

要在 Open3D 中使用 ICP，您首先需要安装库。您可以使用 pip 执行此操作：

pip install open3d

Open3D 在其示例数据集中包含许多模型。安装后，我们可以导入 Open3D 并加载 Stanford Bunny 模型，这是一个用于测试 3D 算法的标准数据集：

import open3d as o3d 
# Load the Bunny mesh
bunny = o3d.data.BunnyMesh()
mesh = o3d.io.read_triangle_mesh(bunny.path)

接下来，为了使 ICP 算法正常工作，有必要像这样计算顶点法线：

mesh.compute_vertex_normals()

接下来，放下样本，这样我们就没有那么多点可以拟合了：

# Sample points from the mesh
pcd = mesh.sample_points_poisson_disk(number_of_points=1000)

要在 Plotly 中将点云可视化为 3D 散点图，可以将 Open3D 点云转换为 NumPy 数组以进行 3D 绘图：

import plotly.graph_objects as go
import numpy as np

# Convert Open3D point cloud to NumPy array
xyz = np.asarray(pcd.points)

# Create a 3D scatter plot
scatter = go.Scatter3d(x=xyz[:, 0], y=xyz[:, 1], z=xyz[:, 2], mode='markers', marker=dict(size=1))
fig = go.Figure(data=[scatter])
fig.show()

斯坦福兔子点云
在这里插入图片描述

三、旋转模型并查找旋转矩阵

为了演示 ICP，让我们创建一个 Bunny 模型的旋转版本，方法是将原始模型旋转 45 度，然后使用 ICP 找到原始模型和旋转模型之间的旋转矩阵：

# Apply an arbitrary rotation to the original point cloud
R = o3d.geometry.get_rotation_matrix_from_xyz((np.pi / 4, np.pi / 4, np.pi / 4))
rotated_pcd = pcd.rotate(R, center=(0, 0, 0))

查看旋转的兔子，确保一切正常：

# Convert Open3D point cloud to NumPy array
xyz_rot = np.asarray(rotated_pcd.points)

# Create a 3D scatter plot
scatter = go.Scatter3d(x=xyz_rot[:, 0], y=xyz_rot[:, 1], z=xyz[:, 2], mode='markers', marker=dict(size=1.0))
fig = go.Figure(data=[scatter])
fig.show()

在这里插入图片描述
斯坦福兔子旋转 45 度

现在，我们使用 ICP 来查找原始模型和旋转模型之间的转换矩阵。

# Use ICP to find the rotation
threshold = 0.02  # Distance threshold
trans_init = np.identity(4)  # Initial guess (identity matrix)
trans_init[:3, :3] = R  # We set the initial rotation to the known rotation
reg_p2p = o3d.pipelines.registration.registration_icp(
    source=rotated_pcd, target=pcd, max_correspondence_distance=threshold,
    init=trans_init
)

# Extract the rotation matrix from the transformation matrix
estimated_rotation_matrix = reg_p2p.transformation[:3, :3]
rotation_matrix = reg_p2p.transformation[:3, :3]
print("Estimated rotation matrix:")
print(rotation_matrix)

ICP发现的原始模型和旋转模型之间的旋转矩阵
在这里插入图片描述

四、验证旋转

为了验证旋转，我们可以将估计旋转矩阵的逆函数应用于旋转模型，并将其与原始模型进行比较。通过取点之间的均方误差（MSE），我们可以检查旋转的模型是否在指定的公差范围内恢复到其原始对齐方式：

# Extract the rotation matrix from the transformation matrix
estimated_rotation_matrix = reg_p2p.transformation[:3, :3]

# Apply the inverse of the estimated rotation to the rotated point cloud
inverse_rotation_matrix = np.linalg.inv(estimated_rotation_matrix)
rotated_back_pcd = rotated_pcd.rotate(inverse_rotation_matrix, center=(0, 0, 0))

# Compare the original point cloud to the one rotated back to its original state
# We can use the mean squared error (MSE) between corresponding points as a metric
original_points = np.asarray(pcd.points)
rotated_back_points = np.asarray(rotated_back_pcd.points)
mse = np.mean(np.linalg.norm(original_points - rotated_back_points, axis=1) ** 2)

# Check if the MSE is below a certain tolerance
tolerance = 1e-6
if mse < tolerance:
    print(f"Test passed: MSE = {mse}")
else:
    print(f"Test failed: MSE = {mse}")

假设一切顺利，您应该会看到测试通过的结果，表明点云已重新对齐。

在这里插入图片描述
显示 ICP 算法的演示到此结束：

1 在两个未对齐的模型之间查找旋转，以及
2 使用这些结果将旋转的模型重新对齐回原始方向。

五、使用 ICP 的局限性

虽然 ICP 是用于模型对齐的强大工具，但它也有其局限性：

ICP 需要良好的初始猜测才能收敛到正确的解决方案，尤其是对于具有大旋转或平移的点云。
该算法可能在对称或无特征的表面上遇到困难，在这些表面上建立正确的对应关系具有挑战性。
异常值和噪声会显著影响 ICP 的性能，从而导致不正确的对齐方式。
ICP 不处理点云之间的尺度差异，因为它假定点云已经处于相同的尺度。
尽管存在这些局限性，但 ICP 仍然是 3D 数据处理中广泛使用的算法，Open3D 提供了一个用户友好的界面，可将 ICP 应用于各种对齐问题。通过仔细的预处理和参数调整，ICP 可以成为对齐 3D 模型的可靠解决方案。