【第19话：定位建图】SLAM点云配准之3D-3D ICP（Iterative Closest Point）方法详解

SLAM点云配准：ICP方法详解

在SLAM（同时定位与地图构建）中，点云配准是核心任务，用于对齐不同时刻采集的点云数据，实现精确的位姿估计和地图更新。ICP（Iterative Closest Point）方法是一种经典的点云配准算法，它通过迭代优化来计算两个点云之间的最优刚体变换（旋转和平移）。下面我将详细解释ICP方法，确保内容清晰可靠，并逐步展开。

1. ICP方法概述

ICP算法的核心思想是通过迭代最小化点云之间的距离误差，来求解最优变换参数。给定源点云P={pi}P = \{ \mathbf{p}_i \}P={pi​}和目标点云Q={qj}Q = \{ \mathbf{q}_j \}Q={qj​}，ICP的目标是找到一个刚体变换（旋转矩阵$\mathbf{R}$和平移向量$\mathbf{t}$），使变换后的源点云与目标点云对齐：
$$\underset{\mathbf{R},\mathbf{t}}{\min }\sum _i\Vert {\mathbf{q}}_i-(\mathbf{R}{\mathbf{p}}_i+\mathbf{t}){\Vert }^2$$
其中$\mathbf{R}$是旋转矩阵（满足${\mathbf{R}}^{\top }\mathbf{R}=\mathbf{I}$），$\mathbf{t}$是平移向量。ICP通过以下步骤迭代求解。

2. ICP算法步骤

ICP是一个迭代过程，包含四个主要步骤：

• 步骤1：找到最近点对应关系
  对于源点云$P$中的每个点${\mathbf{p}}_i$，在目标点云$Q$中搜索其最近邻点${\mathbf{q}}_i$：
  $${\mathbf{q}}_i=\arg \underset{\mathbf{q}\in Q}{\min }\Vert \mathbf{q}-{\mathbf{p}}_i\Vert$$
  这通常使用KD树等数据结构加速搜索。

• 步骤2：计算最优变换
  基于对应点对$({\mathbf{p}}_i,{\mathbf{q}}_i)$，计算最小化距离误差的$\mathbf{R}$和$\mathbf{t}$。目标函数为：
  $$E(\mathbf{R},\mathbf{t})=\sum _i\Vert {\mathbf{q}}_i-(\mathbf{R}{\mathbf{p}}_i+\mathbf{t}){\Vert }^2$$
  通过以下子步骤求解：

  • 计算点云质心：
    $$\bar{\mathbf{p}}=\frac{1}{N}\sum _i{\mathbf{p}}_i,\bar{\mathbf{q}}=\frac{1}{N}\sum _i{\mathbf{q}}_i$$
  • 中心化点云：
    $${\mathbf{p}}_i^{\prime }={\mathbf{p}}_i-\bar{\mathbf{p}},{\mathbf{q}}_i^{\prime }={\mathbf{q}}_i-\bar{\mathbf{q}}$$
  • 构建协方差矩阵：
    $$\mathbf{H}=\sum _i{\mathbf{q}}_i^{\prime }{\mathbf{p}}_i^{\prime \top }$$
  • 使用SVD分解$\mathbf{H}=\mathbf{U\Sigma }{\mathbf{V}}^{\top }$，最优旋转和平移为：
    $$\mathbf{R}=\mathbf{V}{\mathbf{U}}^{\top },\mathbf{t}=\bar{\mathbf{q}}-\mathbf{R}\bar{\mathbf{p}}$$
    注意：需确保$\det (\mathbf{R})=1$，否则调整符号。

• 步骤3：应用变换
  使用计算出的$\mathbf{R}$和$\mathbf{t}$更新源点云：
  Pnew={Rpi+t∣pi∈P} P_{\text{new}} = \{ \mathbf{R} \mathbf{p}_i + \mathbf{t} \mid \mathbf{p}_i \in P \} Pnew​={Rpi​+t∣pi​∈P}

• 步骤4：检查收敛
  计算平均误差：
  $$ϵ=\frac{1}{N}\sum _i\Vert {\mathbf{q}}_i-(\mathbf{R}{\mathbf{p}}_i+\mathbf{t})\Vert$$
  如果$ϵ<\tau$（预设阈值）或达到最大迭代次数，则停止；否则，返回步骤1。

整个算法流程可总结为以下伪代码：

def icp(source, target, max_iter=100, tol=1e-6):# 初始化变换R = np.eye(3)  # 单位旋转矩阵t = np.zeros(3)  # 零平移向量error = float('inf')for i in range(max_iter):# 找到最近点对应correspondences = find_nearest_neighbors(source, target)# 计算最优R和tR_new, t_new = compute_optimal_transform(correspondences)# 应用变换source = apply_transform(source, R_new, t_new)# 更新总变换R = R_new @ Rt = R_new @ t + t_new# 计算新误差new_error = compute_error(correspondences, R_new, t_new)# 检查收敛if abs(new_error - error) < tol:breakerror = new_errorreturn R, t

3. ICP方法的数学推导

ICP的核心是优化问题，其数学基础基于最小二乘。以下简化推导：

• 目标函数：
  $$E(\mathbf{R},\mathbf{t})=\sum _i\Vert {\mathbf{q}}_i-\mathbf{R}{\mathbf{p}}_i-\mathbf{t}{\Vert }^2$$

• 平移优化：
  固定$\mathbf{R}$，对$\mathbf{t}$求导：
  $$\frac{\partial E}{\partial \mathbf{t}}=-2\sum _i({\mathbf{q}}_i-\mathbf{R}{\mathbf{p}}_i-\mathbf{t})=0$$
  解得：
  $$\mathbf{t}=\bar{\mathbf{q}}-\mathbf{R}\bar{\mathbf{p}}$$

• 旋转优化：
  代入$\mathbf{t}$后，目标函数简化为：
  $$E(\mathbf{R})=\sum _i\Vert ({\mathbf{q}}_i-\bar{\mathbf{q}})-\mathbf{R}({\mathbf{p}}_i-\bar{\mathbf{p}}){\Vert }^2=\sum _i\Vert {\mathbf{q}}_i^{\prime }-\mathbf{R}{\mathbf{p}}_i^{\prime }{\Vert }^2$$
  最大化${\sum }_i{\mathbf{q}}_i^{\prime \top }\mathbf{R}{\mathbf{p}}_i^{\prime }$，等价于最大化$\text{trace}({\mathbf{R}}^{\top }\mathbf{H})$。通过SVD分解$\mathbf{H}=\mathbf{U\Sigma }{\mathbf{V}}^{\top }$，最优解为$\mathbf{R}=\mathbf{V}{\mathbf{U}}^{\top }$。

4. ICP方法的优缺点

• 优点：

  • 简单高效，计算复杂度为$O(N\log N)$（使用KD树加速）。
  • 适用于刚性变换（旋转和平移），在初始位置较好时收敛快。
  • 广泛集成于SLAM框架（如LOAM、Cartographer），用于局部点云对齐。

• 缺点：

  • 对初始位置敏感：如果初始偏差大，可能陷入局部最优。
  • 假设点云完全重叠：在部分重叠或噪声大时性能下降。
  • 仅处理刚体变换：无法处理非刚性变形。

5. 在SLAM中的应用

在SLAM中，ICP常用于：

• 帧到帧配准：实时对齐连续点云，估计机器人位姿变化。
• 局部地图优化：将新点云与局部子地图对齐，减少累积误差。
• 回环检测：对齐历史点云以检测回环，优化全局地图。
  为提升鲁棒性，常结合其他技术，如点云滤波（去除噪声）或特征匹配（如FPFH）来初始化ICP。

总结

ICP是一种基础且强大的点云配准方法，通过迭代优化最小化点间距离，实现高效配准。尽管有局限性，但其简单性和效率使其成为SLAM中不可或缺的组件。理解ICP的原理和实现，有助于深入掌握SLAM的点云处理流程。