通过代码理解迭代最近点 (ICP) 算法

迭代最近点 (ICP) 是一种广泛使用的经典计算机视觉算法，用于二维或三维点云配准。顾名思义，它通过迭代地改进和最小化两个点云之间的空间差异或平方误差之和。这是通过估计最优刚体变换（旋转）来实现的。

迭代最近点 (ICP)算法是一种广泛应用的经典计算机视觉算法，用于二维或三维点云配准。顾名思义，它通过迭代的方式改进并最小化两个点云之间的空间差异或平方误差之和。其实现方式是估计最优的刚体变换（旋转和平移）矩阵，从而更好地将“源”或参考点集与同一坐标系 $[R \ | \ t]$ 中的“目标”点集对齐。ICP 算法在医学成像、三维空间建模、即时定位与地图构建 (SLAM)、自动驾驶汽车和机器人等领域有着广泛的应用。

ICP 最早由 Besl 和 McKay 正式提出，其概念可以追溯到 1992 年。自那时以来，ICP 已经发展了很多，衍生出了许多变体，并进行了显著的改进，主要针对鲁棒的对应匹配、加权误差度量和最近邻搜索的加速结构。

在本文中，我们将概述 ICP、其流程、变体和实际应用案例，并提供一个使用 Open3D 的演示示例。

如果您是刚刚开始接触 3D 计算机视觉的人，我们的系列文章将指导您了解3D 视觉和重建的基础知识。

什么是点云配准？

点云（红色）- SfM 中的三角测量 - 3D 重建 - 多相机视图 — 点云红色的 SfM中的三角测量

点云本质上是三维坐标系中的一组顶点，表示为 P = {p_1, p_2, …, p_N} $p_i = { x_i, y_i, z_i } \in \mathbb{R}^3$ 。为了构建物体（例如雕像）的三维重建，我们首先需要从多个角度拍摄场景。点云的来源可以是激光雷达、RGB 相机或立体相机，每次扫描都会生成一个独立的点云。

难点在于如何将从不同视角或不同时间间隔拍摄的物体多次扫描的特征组合成一个连贯的整体，避免出现间隙、变形或异常值。为了从中创建完整的几何体，我们需要将它们精细地拼接在一起。在同一个坐标系中，找到使源点云与目标点云对齐的精确旋转和平移的过程称为配准。

斯坦福兔子点云配准示例 - 从粗略的变换矩阵开始 - 使用特征点匹配 - ICP 算法可获得更好的收敛效果 — 点云配准示例

它首先对变换进行初始猜测，然后逐步改进，直到满足收敛准则。ICP 的直观思想是，如果我们知道源点云和目标点云之间的对应点，那么在数学上找到最佳变换就非常简单。另一方面，如果我们知道正确的变换，那么找到对应点也很容易，而且它们会完美对齐。但我们两者都不知道，因此 ICP 会在估计对应点和基于初始对应点估计变换之间交替进行。如果两个点云来自同一源，我们就无需考虑缩放问题。

尽管 ICP 的概念很简单，但它的有效性取决于初始化质量、误差度量的选择以及异常值剔除策略等因素。

迭代最近点 (ICP) 流水线

使用斯坦福兔子数据集对迭代最近点算法进行概述，结果显示其能够实现正确的对齐。 — 迭代最近点算法概述

标准ICP算法包含以下步骤：

标准ICP流程：首先，进行粗略变换初始化；其次，寻找匹配对应点；第三，估计A到B的变换矩阵；第四，将变换应用于A；第五，检查是否满足收敛准则。 — 标准ICP管线源

步骤 1：初始化

ICP 不是从零开始；它从对变换的初始估计开始 $R_o，t_o$ ，大致将源云 $P$ 与目标云对齐 $问$ 。

$P_0 = R_0 P + t_0$

这里 $P_0$ 展示的是应用初始变换后的源云。初始猜测的质量至关重要；糟糕的初始化会导致ICP收敛到错误的算法（局部最小值），甚至根本无法收敛。

最初的猜测可能来自：

传感器先验信息（移动扫描仪上的 GPS/IMU 数据）
用户手动放置，
匹配云朵之间的独特特征。
全局配准算法的输出（提供粗略的对齐结果）。我们初始化计数器 $k = 0$ 。

第二步：寻找对应关系

$p_{i,k}$ 对于当前迭代变换后的源点云中的每个点，找到目标点云中 $P_k$ 距离它最近的点。“最近”通常由欧氏距离定义。此步骤建立了一组相关的对应点对。然而，为点云中的每个点找到匹配点对的计算量非常大。为了克服这一瓶颈，最常用的方法是在静态目标点云上构建kd 树。kd 树递归地划分三维空间，从而实现高效的最近邻搜索。这通过从源点云中找到查询点的最近邻来缩小搜索空间，从而显著加快了处理速度。 $q_{i,k}$ $问$ ${(p_{i,k}, q_{i,k})}$

$问$ $P_k$

步骤 3：估算转换

${(p_{i,k}, x_i)}$ 给定上一步找到的匹配点对集合，该算法计算 $R_{k+1}, t_{k+1}$ 使这些点对最佳对齐的刚体变换（）。通过最小化变换后的源点与其对应目标点之间的平方欧氏距离之和。

$(R_{k+1}, t_{k+1}) = \operatorname*{arg\,min}{R,t} \sum_i ||(R p{i,k} + t) - q_i||^2$

该优化问题具有闭式解。寻找最优旋转 $拉$ 和平移的常用且稳健的方法 $t$ 是使用奇异值分解 (SVD)。首先计算对应源点 ( $\bar{q}$ ) 的质心（平均位置）。然后计算质心点： $p'_{i,k} = p_{i,k} - \bar{p}k$ 和 $q'_i = q_i - \bar{q}$ 。

交叉方差矩阵是将这些中心化后的对应对的外积相加而得到的。

$H = \sum_i p'_{i,k} (q'_i)^T$

对该矩阵应用奇异值分解 (SVD) $H$ 可得到三个矩阵，

$H = USV^T$ 。

这里，

$U$ 并且 $V^T$ 是表示旋转/反射的正交矩阵
$S$ 是一个对角矩阵，对角线上只有一个值。

$R_{k+1}$ 然后计算最优旋转矩阵 $R_{k+1} = VU^T$ 。通常需要进行特殊检查：如果的行列式 $R_{k+1}$ 为 -1（表示反射而非正确的旋转），则 $V$ 通常需要先将最后一列的符号取反，然后再重新计算，以确保它是一个有效的旋转矩阵。一旦找到 $R_{k+1}$ 最优旋转，就可以使用质心轻松计算最优平移。 $R_{k+1}$ $t_{k+1}$

$t_{k+1} = \bar{q} - R_{k+1} \bar{p}_k$

步骤 4：应用转换

然后应用新计算的变换来更新源点云 $R_{k+1}, t_{k+1}$ 。通常，变换是相对于原始源点云累积的， $P$ 而不是迭代地变换点本身，以避免潜在的漂移。

$P_{k+1} = R_{k+1} P_k + t_{k+1}$

步骤 5：检查收敛性

该算法会检查是否满足终止条件。常见的收敛准则包括：

迭代之间均方误差（在步骤 3 中计算）的相对变化低于预定义的阈值。
两次迭代之间变换参数（R 和 t）的相对变化低于阈值。
已达到最大迭代次数。

如果满足收敛准则，算法终止并输出最终变换（ $R_{k+1}, t_{k+1}$ ）。否则，递增 $k$ （即 $k → k+1$ ），并返回步骤 2。找到两个云之间最佳的匹配，并找到对应关系。基于此，尝试优化平移和旋转。

Computerphile 对迭代最近点算法的解释 — **迭代最近点 (ICP) Computerphile**

icp-explained-computerphile-itertaive-closest-point – LearnOpenCV — **迭代最近点 (ICP) Computerphile**

ICP绩效的实际考量

虽然流程看起来很简单，但有几个因素会严重影响 ICP 在现实世界中的有效性：

初始化和局部最小值：如前所述，ICP 是一种局部优化算法。它在初始猜测的最近邻域内寻找最佳对齐方式。如果初始猜测较差，ICP 很容易陷入局部最小值——一个看起来局部最优的对齐方式可能全局正确。确保一个合理的初始位置至关重要，这通常可以通过粗略的初始对齐或全局配准技术来实现。因此，初始姿态的选择方式应确保源点和目标点在开始时有足够的重叠。

异常值敏感性：标准最小二乘误差度量对异常值（由噪声、不重叠区域或动态对象引起的错误对应关系）非常敏感。少数错误匹配就可能显著扭曲变换估计。缓解此问题的策略包括：
- 距离阈值法：拒绝距离超过一定值的配对。
- 百分比拒收：剔除错误率最大的固定比例的配对。
- 稳健误差指标：使用对异常值不太敏感的指标，例如绝对差值之和（L1 范数）或更高级的稳健函数（例如 Huber 损失）。
- 随机抽样一致性（RANSAC）：迭代地选择最小的配对子集来估计变换，并找到由最多内点支持的变换。

计算成本：即使使用kd树，处理大型点云也十分耗时。以下技术可以缓解这一问题：
- 点采样：仅使用源点云中的点子集（例如，随机采样、均匀采样或选择高曲率区域中的点）。
- 多分辨率方法：首先对点云的降采样版本执行 ICP 以进行粗略配准，然后逐步提高分辨率以进行精细配准。

常见ICP变异

基本的ICP算法（通常称为点对点ICP算法）启发了许多旨在提高收敛速度和精度的变体：

Point2point-point2plane-symmetric-icp-itertaive-closest-point-algorithm – LearnOpenCV

点对点 ICP：这是经典的或标准的 ICP 算法，它通过在目标云中找到源云中每个点的最近点来建立对应关系。目标是最小化这些对应点之间欧氏距离平方和。虽然算法简单直接，但在某些情况下收敛速度可能较慢，并且它完全依赖于点之间的邻近性。

$E(T) = \sum_{(p,q) \in \mathcal{K}} |p - Tq|^2$

点到平面 ICP 算法：这种常用的变体算法并非最小化对应点之间的直接距离，而是最小化每个源点到其对应目标点处切平面的距离。这需要估计目标点云（有时也包括源点云）的表面法线。误差度量也会随之改变，以反映这种点到平面的距离。这种方法通常能更有效地利用底层表面几何形状，从而实现更快的收敛速度和更高的精度，尤其适用于具有平坦或结构化表面的场景。

$E(T) = \sum_{(p,q) \in \mathcal{K}} ((p - Tq) \cdot n_P)^2,$

迭代最近点斯坦福兔子收敛迭代点到点到点到平面 — 点对点和点对平面ICP 源的比较

平面到平面（广义ICP-G-ICP）：该方法通过对源云和目标云中的不确定性和局部平面性（使用协方差矩阵）进行建模，进一步扩展了上述思想。它有效地最小化了局部平面区域之间的距离，与点到点或点到平面方法相比，通常对噪声和点密度变化具有更强的鲁棒性。

Velodyne 数据上广义 ICP、点对平面和标准 ICP 图表的比较 — 广义ICP源、点对平面ICP源和标准ICP源的比较

对称ICP：在标准ICP算法中，通常只寻找源点到目标点的对应关系。对称ICP算法在优化过程中同时考虑源点到目标点和目标点到源点两个方向的对应关系。这有时可以提高稳定性，并防止对齐结果向某一方向漂移。

下载代码 为了方便您学习本教程，请点击下方按钮下载代码。完全免费！

ICP代码详解

首先，我们将通过 Open3D 示例可视化 ICP。然后，我们将用 Python 实现一个非常简单的 ICP 版本，以理解其底层机制。

使用 Open3D

!pip install open3d

导入依赖项

import numpy as np
import matplotlib.pyplot as plt
import open3d as o3d
import copy

导入数据集

我们将从 Open3D ICP 演示数据集中加载两个示例点云（源点云和目标点云），每个点云都代表从不同视角捕获的房间的 3D 扫描。

# Read source and target PCD
demo_pcds = o3d.data.DemoICPPointClouds()
source = o3d.io.read_point_cloud(demo_pcds.paths[0]) # PointCloud with 191397 points.
#source.dimension( )  -> 3
target = o3d.io.read_point_cloud(demo_pcds.paths[1]) # PointCloud with 137833 points.
#target.dimension( )  -> 3

注册前可视化输入

o3d.visualization.draw_plotly([source],
                                  zoom=0.455,
                                  front=[-0.4999, -0.1659, -0.8499],
                                  lookat=[2.1813, 2.0619, 2.0999],
                                  up=[0.1204, -0.9852,Q 0.1215])
 
o3d.visualization.draw_plotly([target],
                                  zoom=0.455,
                                  front=[-0.4999, -0.1659, -0.8499],
                                  lookat=[2.1813, 2.0619, 2.0999],
                                  up=[0.1204, -0.9852, 0.1215])

初始注册信息来自全球注册系统

以下函数可视化了经过粗略初始对齐变换后的目标点云和源点云。目标点云和源点云分别用青色和黄色表示。两个点云重叠越多、越紧密，对齐结果就越好。

def draw_registration_result(source, target, transformation):
      source_temp = copy.deepcopy(source)
      target_temp = copy.deepcopy(target)
      source_temp.paint_uniform_color([1, 0.0706, 0])
      target_temp.paint_uniform_color([0, 0.651, 0.929])
      source_temp.transform(transformation)
      o3d.visualization.draw_plotly([source_temp, target_temp])
 
 
# *** Initial Transformation ***
trans_init = np.asarray([[0.862, 0.011, -0.507, 0.5],
                         [-0.139, 0.967, -0.215, 0.7],
                         [0.487, 0.255, 0.835, -1.4], [0.0, 0.0, 0.0, 1.0]])
 
draw_registration_result(source, target, trans_init)

该函数 evaluate_registration 计算两个主要指标：

fitness该指标衡量重叠区域（内点对应点数/源点数）。数值越高越好。
inlier_rmse它衡量所有内点对应关系的均方根误差 (RMSE)。数值越低越好。

默认情况下， registration_icp 程序会运行直至收敛或达到最大迭代次数（默认为 30 次）。可以更改此设置以允许更多计算时间并进一步提高结果。

threshold = 0.02
print("Initial Alignment")
evaluation = o3d.pipelines.registration.evaluate_registration(
                       source, target, threshold, trans_init)
print(evaluation)
 
"""Initial alignment
RegistrationResult with fitness=2.740900e-02, inlier_rmse=1.259521e-02, and correspondence_set size of 5246
Access transformation to get result."""

通过使用ICP进行注册

点对点
点到平面

该类 TransformationEstimationPointToPoint 提供用于计算点对点ICP目标函数的残差和雅可比矩阵的函数。该函数 `registration_icp` 以残差和雅可比矩阵作为参数，并运行点对点ICP算法以获得结果。

点对点

# ******* Point-to-Point *********
threshold = 0.02
print("Apply point-to-point ICP")
reg_p2p = o2d.pipelines.registration.registration_icp(
                   source, target, threshold, trans_init,
                   o3d.pipelines.registration.TransformationEstimationPointToPoint())
print(reg_p2p)
print("Transformation is:")
print(reg_p2p.transformation)
draw_registration_result(source, target, reg_p2p.transformation)
 
"""
Apply point-to-point ICP
RegistrationResult with fitness=3.132755e-02, inlier_rmse=1.168464e-02, and correspondence_set size of 5996
Access transformation to get result.
Transformation is:
[[ 0.86481662  0.0378514  -0.50084398  0.42597758]
 [-0.15157681  0.97072361 -0.18799414  0.66535821]
 [ 0.47817432  0.23769297  0.84517011 -1.35537445]
 [ 0.          0.          0.          1.        ]]
"""

得分 fitness 从 0.174723 增加到 0.372450。 inlier_rmse 从 0.011771 减少到 0.007760。

点到平面

点到面 ICP 算法使用点法线。在本教程中，我们将从文件中加载法线。如果未提供法线，则可以使用顶点法线估计来计算。`registration_icp` 函数会使用不同的参数调用 TransformationEstimationPointToPlane。该类内部实现了用于计算点到面 ICP 目标函数的残差矩阵和雅可比矩阵的函数。

threshold=0.02
print("Apply point-to-point ICP")
reg_p2p = o3d.pipelines.registration.registration_icp(
    source, target, threshold, trans_init,
    o3d.pipelines.registration.TransformationEstimationPointToPoint())
print(reg_p2p)
print("Transformation is:")
print(reg_p2p.transformation)
draw_registration_result(source, target, reg_p2p.transformation)

点到平面 ICP 在 30 次迭代内达到紧密对齐（ fitness 得分分别为 0.620972 和 inlier_rmse 0.006581）。

下载代码 为了方便您学习本教程，请点击下方按钮下载代码。完全免费！

简单实现

将两个原本相同但经过齐次变换的正弦点云对齐。ICP算法通过最小化两个点云之间的距离，逐步使它们对齐。

导入依赖项

import numpy as np
from sklearn.neighbors import NearestNeighbors
import matplotlib.pyplot as plt
import matplotlib.animation as animation

m函数best_fit_transfor实现了一种最小二乘最佳拟合变换算法，用于映射两个点云（A 和 B）之间的匹配点（对应关系）。它计算将 A 变换到 B 所需的旋转矩阵、平移向量和齐次变换矩阵。

步骤：

检查 A 和 B 是否形状相同
计算点 A 和点 B 的质心，并将这些点平移到它们的质心处。
使用奇异值分解 (SVD) 计算旋转矩阵。如果旋转矩阵的行列式为负，则将 Vt 的最后一行进行反射。
利用变换后的质心和旋转矩阵计算平移量
利用旋转矩阵和平移向量构造齐次变换矩阵。

def best_fit_transform(A, B):
    """
    Calculates the best-fit transform that maps points A onto points B.
    Input:
        A: Nxm numpy array of source points
        B: Nxm numpy array of destination points
    Output:
        T: (m+1)x(m+1) homogeneous transformation matrix
    """
    assert A.shape == B.shape
    m = A.shape[1]
    centroid_A = np.mean(A, axis=0)
    centroid_B = np.mean(B, axis=0)
    AA = A - centroid_A
    BB = B - centroid_B
    H = np.dot(AA.T, BB)
    U, S, Vt = np.linalg.svd(H)
    R = np.dot(Vt.T, U.T)
    if np.linalg.det(R) < 0:
       Vt[m-1,:] *= -1
       R = np.dot(Vt.T, U.T)
    t = centroid_B.reshape(-1,1) - np.dot(R, centroid_A.reshape(-1,1))
    T = np.eye(m+1)
    T[:m, :m] = R
    T[:m, -1] = t.ravel()
    return T

接下来，我们需要找到目标点云和源点云之间的最近邻点。该函数nearest_neighbor接受两个输入 src 和 dst，并使用 kd 树返回 src 中每个点到 dst 中最近邻点的欧氏距离和索引。

def nearest_neighbor(src, dst):
    '''
    Find the nearest (Euclidean) neighbor in dst for each point in src
    Input:
        src: Nxm array of points
        dst: Nxm array of points
    Output:
        distances: Euclidean distances of the nearest neighbor
        indices: dst indices of the nearest neighbor
    '''
    # Ensure shapes are compatible for KNN, although they don't strictly need to be identical N
    assert src.shape[1] == dst.shape[1] 
    neigh = NearestNeighbors(n_neighbors=1)
    neigh.fit(dst)
    distances, indices = neigh.kneighbors(src, return_distance=True)
    return distances.ravel(), indices.ravel()

以下函数实现了ICP算法，迭代地更新更新后的A点和B点之间的变换关系。迭代过程持续进行，直到当前迭代和前一次迭代之间最近点距离的平均误差之差小于指定的容差值，或者达到最大迭代次数为止。输出结果是最终的齐次变换矩阵，T_final该矩阵将A点映射到B点，直至收敛。

为了可视化所有步骤的收敛过程，我们还需要存储中间步骤。

# --- Modified ICP function to store history ---
def iterative_closest_point_visual(A, B, max_iterations=20, tolerance=0.001):
    '''
    The Iterative Closest Point method: finds best-fit transform that maps points A on to points B.
    Stores intermediate results for visualization.
    Input:
        A: Nxm numpy array of source points
        B: Nxm numpy array of destination points
        max_iterations: exit algorithm after max_iterations
        tolerance: convergence criteria
    Output:
        T_final: final homogeneous transformation that maps A on to B
        intermediate_A: List containing A transformed at each iteration (N x m arrays)
        intermediate_errors: List containing the mean error at each iteration
        i: number of iterations to converge
    '''
 
    # Check dimensions
    # Allow N to differ, but dimensions (m) must match
    assert A.shape[1] == B.shape[1] 
 
    # get number of dimensions
    m = A.shape[1]
     
    # --- Store History ---
    intermediate_A = [np.copy(A)] # Store initial state
    intermediate_errors = []
    # --- Store History ---
 
    # make points homogeneous, copy them to maintain the originals
    # Use np.copy() for src to allow modification without affecting original A
    src_h = np.ones((m+1, A.shape[0])) 
    src_h[:m, :] = np.copy(A.T)
     
    # Target points (B) remain fixed, use non-homogeneous for KNN
    dst = np.copy(B) # Non-homogeneous target points for KNN
 
    prev_error = float('inf') # Initialize with infinity
    T_cumulative = np.identity(m+1) # To accumulate transformations correctly
 
    for i in range(max_iterations):
        # Current source points (non-homogeneous)
        current_src = src_h[:m, :].T 
 
        # find the nearest neighbors between the current source and destination points
        distances, indices = nearest_neighbor(current_src, dst)
 
        # compute the transformation between the current source and nearest destination points
        # Use the subset of B (dst) corresponding to the nearest neighbors found
        T_step = best_fit_transform(current_src, dst[indices, :])
 
        # update the current source points *in homogeneous coordinates*
        src_h = np.dot(T_step, src_h)
         
        # --- Store History ---
        intermediate_A.append(src_h[:m, :].T) # Store transformed A for this iteration
        # --- Store History ---
 
        # check error (stop if error is less than specified tolerance)
        mean_error = np.mean(distances)
        intermediate_errors.append(mean_error) # Store error for this iteration
         
        # Use absolute difference check for convergence
        if np.abs(prev_error - mean_error) < tolerance:
            print(f"Converged at iteration {i+1} with error difference {np.abs(prev_error - mean_error)}")
            break
             
        prev_error = mean_error
         
        # Accumulate transformation
        T_cumulative = np.dot(T_step, T_cumulative)
 
 
    # Calculate the *final* transformation from the *original* A to the *final* src position
    # This accounts for the accumulated transform
    T_final = best_fit_transform(A, src_h[:m, :].T)
     
    # If loop finished due to max_iterations without converging based on tolerance
    if i == max_iterations - 1:
         print(f"Reached max iterations ({max_iterations})")
 
    return T_final, intermediate_A, intermediate_errors, i + 1 # Return i+1 for actual count

# Define two sets of points A and B (same as your example)
t = np.linspace(0, 2*np.pi, 10)
A = np.column_stack((t, np.sin(t)))
 
# Define the rotation angle
theta = np.radians(30)
c, s = np.cos(theta), np.sin(theta)
rotation_matrix = np.array(((c, -s), (s, c)))
 
# Define translation vector
translation_vector = np.array([[2, 0]])
 
# Apply the transformation and add randomness to get B
np.random.seed(42) # for reproducible randomness
randomness = 0.3 * np.random.rand(10, 2)
B = np.dot(rotation_matrix, A.T).T + translation_vector + randomness
 
# --- Run ICP and get history ---
max_iter = 20
tolerance = 0.0001 # Lower tolerance for smoother convergence potentially
T_final, history_A, history_error, iters = icp.iterative_closest_point_visual(A, B, max_iterations=max_iter, tolerance=tolerance)
 
print(f'Converged/Stopped after {iters} iterations.')
print(f'Final Mean Error: {history_error[-1]:.4f}')
print('Final Transformation:')
print(np.round(T_final, 3))

# --- Create Animation ---
fig, ax = plt.subplots()
 
# Plot target points (static)
ax.scatter(B[:, 0], B[:, 1], color='blue', label='Target B', marker='x')
 
# Plot initial source points
scatter_A = ax.scatter(history_A[0][:, 0], history_A[0][:, 1], color='red', label='Source A (moving)')
title = ax.set_title(f'Iteration 0, Mean Error: N/A')
ax.legend()
ax.set_xlabel("X")
ax.set_ylabel("Y")
ax.grid(True)
ax.axis('equal') # Important for visualizing rotations correctly
 
# Determine plot limits based on all points across all iterations
all_points = np.vstack([B] + history_A)
min_vals = np.min(all_points, axis=0)
max_vals = np.max(all_points, axis=0)
range_vals = max_vals - min_vals
margin = 0.1 * range_vals # Add 10% margin
ax.set_xlim(min_vals[0] - margin[0], max_vals[0] + margin[0])
ax.set_ylim(min_vals[1] - margin[1], max_vals[1] + margin[1])
 
# Animation update function
def update(frame):
    # Update source points position
    scatter_A.set_offsets(history_A[frame])
    # Update title
    error_str = f"{history_error[frame-1]:.4f}" if frame > 0 else "N/A" # Error calculated *after* step
    title.set_text(f'Iteration {frame}, Mean Error: {error_str}')
    # Return the artists that were modified
    return scatter_A, title,
 
# Create the animation
# Number of frames is number of states stored (initial + iterations)
# Interval is milliseconds between frames (e.g., 500ms = 0.5s)
ani = animation.FuncAnimation(fig, update, frames=len(history_A), 
                              interval=500, blit=True, repeat=False)
 
# Display the final plot (optional, animation already shows it)
plt.figure()
plt.scatter(history_A[-1][:, 0], history_A[-1][:, 1], color='red', label='Final A')
plt.scatter(B[:, 0], B[:, 1], color='blue', label='Target B', marker='x')
plt.legend()
plt.title(f"Final Alignment after {iters} iterations")
plt.xlabel("X")
plt.ylabel("Y")
plt.grid(True)
plt.axis('equal')
plt.show()

点对点 – 正弦曲线

ICP的实际应用案例

SLAM：在机器人和自动驾驶领域，ICP（集成控制点）是扫描匹配的基础。通过对齐连续的传感器扫描（例如，来自激光雷达的扫描），机器人可以估计自身的运动（里程计）并构建其环境的一致性地图。现代最先进的SLAM系统通常使用复杂的ICP变体，例如：KISS-ICP和GenZ-ICP。

“只要方法得当，传统的ICP依然强大。”

医学影像：将不同方式或时间拍摄的医学扫描图像进行比对，以追踪疾病的传播，计划手术或指导干预措施。

制造和质量控制：将制造零件的 3D 扫描与其数字 CAD 模型进行比较，以检测缺陷并确保满足公差（计量）。

增强/虚拟现实（AR/VR）：将虚拟对象与现实世界的点云数据对齐，可在混合现实环境（HoloLens）中实现逼真的集成和交互，确保正确的放置和缩放。

ICP的优势

精准微调：在给定良好起始点的情况下，能非常有效地进行精细化对准。
简洁性：核心迭代理念易于理解和实现。
稳健：增强版本（例如，点到平面）能更好地处理噪声、异常值和无特征区域。

ICP的缺点

需要良好的初始猜测：如果初始对齐效果不佳，则极易失败（陷入局部最小值）。如果两个点云重叠不足，则对齐可能无法收敛。
计算成本高：速度可能较慢，尤其对于大型点云而言。
对异常值/低重叠度敏感：标准版本在噪声、错误配对和有限重叠度方面表现不佳。

结论

我们深入探讨了迭代最近点算法（ICP），并理解了其数学公式如何转化为Python代码。虽然基于深度学习的方法和复杂的配准技术在复杂场景下具有优势，但ICP的核心原理提供了一种简单有效的途径，在某些特定场景下甚至优于最先进的SLAM算法。这凸显了经典的计算机视觉算法在许多应用中仍然具有重要意义，并且是基础架构。

希望您觉得这篇文章有趣。欢迎通过我们的社交媒体账号分享您的反馈意见。