ICP-通过一组匹配的3D点估计相机运动

问题描述

假设有两组已匹配的3D点：

• 源点云： { p i ∈ R 3 } i = 1 N \{ \mathbf{p}_i \in \mathbb{R}^3 \}_{i=1}^N {pi​∈R3}i=1N​
• 目标点云： { q i ∈ R 3 } i = 1 N \{ \mathbf{q}_i \in \mathbb{R}^3 \}_{i=1}^N {qi​∈R3}i=1N​

目标是求解刚体变换旋转矩阵$\mathbf{R}\in \text{SO}(3)$ 和平移向量 $\mathbf{t}\in {\mathbb{R}}^3$，使得变换后的源点云与目标点云对齐，即最小化以下目标函数：
$$\underset{\mathbf{R},\mathbf{t}}{\min }\sum _{i=1}^N{\Vert \mathbf{R}{\mathbf{p}}_i+\mathbf{t}-{\mathbf{q}}_i\Vert }^2.$$

SVD方法

分析计算

1. 去中心化与平移向量的求解

首先计算两组点的质心：
$${\mathbf{\mu }}_p=\frac{1}{N}\sum _{i=1}^N{\mathbf{p}}_i,{\mathbf{\mu }}_q=\frac{1}{N}\sum _{i=1}^N{\mathbf{q}}_i.$$
将每个点减去质心，得到去中心化的坐标：
$${\mathbf{x}}_i={\mathbf{p}}_i-{\mathbf{\mu }}_p,{\mathbf{y}}_i={\mathbf{q}}_i-{\mathbf{\mu }}_q.$$
此时，目标函数可简化为：
$$\underset{\mathbf{R},\mathbf{t}}{\min }\sum _{i=1}^N{\Vert \mathbf{R}{\mathbf{x}}_i-{\mathbf{y}}_i+(\mathbf{t}-{\mathbf{\mu }}_q+\mathbf{R}{\mathbf{\mu }}_p)\Vert }^2.$$
令括号内项为零，可得平移向量的最优解：
$$\mathbf{t}={\mathbf{\mu }}_q-\mathbf{R}{\mathbf{\mu }}_p.$$
代入后，优化问题简化为仅关于旋转矩阵$\mathbf{R}$ 的最小化问题：
$$\underset{\mathbf{R}}{\min }\sum _{i=1}^N{\Vert \mathbf{R}{\mathbf{x}}_i-{\mathbf{y}}_i\Vert }^2.$$

2. 目标函数的展开与迹形式转换

展开平方误差：
$$\sum _{i=1}^N{\Vert \mathbf{R}{\mathbf{x}}_i-{\mathbf{y}}_i\Vert }^2=\sum _{i=1}^N{\left(\mathbf{R}{\mathbf{x}}_i-{\mathbf{y}}_i\right)}^{\top }\left(\mathbf{R}{\mathbf{x}}_i-{\mathbf{y}}_i\right).$$
进一步展开并利用迹的线性性质：
$$=\sum _{i=1}^N\left({\mathbf{x}}_i^{\top }{\mathbf{R}}^{\top }\mathbf{R}{\mathbf{x}}_i-2{\mathbf{y}}_i^{\top }\mathbf{R}{\mathbf{x}}_i+{\mathbf{y}}_i^{\top }{\mathbf{y}}_i\right).$$
由于 ${\mathbf{R}}^{\top }\mathbf{R}=\mathbf{I}$，第一项简化为${\mathbf{x}}_i^{\top }{\mathbf{x}}_i$，与 $\mathbf{R}$ 无关。优化问题等价于最大化交叉项：
$$\underset{\mathbf{R}}{\max }\sum _{i=1}^N{\mathbf{y}}_i^{\top }\mathbf{R}{\mathbf{x}}_i=\underset{\mathbf{R}}{\max }\text{tr}\left({\mathbf{R}}^{\top }\sum _{i=1}^N{\mathbf{y}}_i{\mathbf{x}}_i^{\top }\right).$$
定义协方差矩阵：
$$\mathbf{H}=\sum _{i=1}^N{\mathbf{y}}_i{\mathbf{x}}_i^{\top }.$$

3. 通过SVD求解旋转矩阵

对协方差矩阵 $\mathbf{H}$ 进行奇异值分解（SVD）：
$$\mathbf{H}=\mathbf{U\Sigma }{\mathbf{V}}^{\top }.$$
目标函数可写为：
$$\text{tr}({\mathbf{R}}^{\top }\mathbf{H})=\text{tr}({\mathbf{R}}^{\top }\mathbf{U\Sigma }{\mathbf{V}}^{\top }).$$
利用迹的循环置换性质$(\text{tr}(ABC)=\text{tr}(BCA)$，可重写为：
$$\text{tr}({\mathbf{V}}^{\top }{\mathbf{R}}^{\top }\mathbf{U\Sigma }).$$
令 $\mathbf{M}={\mathbf{V}}^{\top }{\mathbf{R}}^{\top }\mathbf{U}$，则目标函数简化为：
$$\text{tr}(\mathbf{M\Sigma }).$$
由于 $\mathbf{M}$ 是正交矩阵$({\mathbf{M}}^{\top }\mathbf{M}=\mathbf{I}$），其对角线元素绝对值不超过1。为了使 $\text{tr}(\mathbf{M\Sigma })$ 最大，需使$\mathbf{M}$的对角线元素尽可能大。当$\mathbf{M}=\mathbf{I}$ 时，迹达到最大值 $\text{tr}(\mathbf{\Sigma })$，即：
$$\mathbf{M}=\mathbf{I}⟹{\mathbf{V}}^{\top }{\mathbf{R}}^{\top }\mathbf{U}=\mathbf{I}.$$
解得旋转矩阵：
$${\mathbf{R}}^{\top }=\mathbf{V}{\mathbf{U}}^{\top }⟹\mathbf{R}=\mathbf{U}{\mathbf{V}}^{\top }.$$

验证行列式：

• 若 $\det (\mathbf{R})=1$，解有效。
• 若 $\det (\mathbf{R})=-1$（存在反射），需修正为：
  $$\mathbf{R}=\mathbf{V}\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& -1\end{array}\right]{\mathbf{U}}^{\top }.$$

4. 迭代优化（可选）

若点对匹配不完美或存在噪声，可多次迭代：

1. 用当前$\mathbf{R},\mathbf{t}$ 变换源点云：${\mathbf{p}}_i^{\prime }=\mathbf{R}{\mathbf{p}}_i+\mathbf{t}$。
2. 重新计算误差并更新变换，直至收敛。

5. 公式总结

1. 质心计算：
   $${\mathbf{\mu }}_p=\frac{1}{N}\sum _{i=1}^N{\mathbf{p}}_i,{\mathbf{\mu }}_q=\frac{1}{N}\sum _{i=1}^N{\mathbf{q}}_i.$$
2. 协方差矩阵：
   $$\mathbf{H}=\sum _{i=1}^N({\mathbf{q}}_i-{\mathbf{\mu }}_q)({\mathbf{p}}_i-{\mathbf{\mu }}_p{)}^{\top }.$$
3. SVD分解与旋转矩阵：
   $$\mathbf{H}=\mathbf{U\Sigma }{\mathbf{V}}^{\top },\mathbf{R}=\mathbf{V}{\mathbf{U}}^{\top }(\text{确保}\det (\mathbf{R})=1).$$
4. 平移向量：
   $$\mathbf{t}={\mathbf{\mu }}_q-\mathbf{R}{\mathbf{\mu }}_p.$$

几何解释

通过SVD分解，协方差矩阵 $\mathbf{H}$ 的奇异向量反映了点云之间的主要对齐方向。最优旋转矩阵 $\mathbf{R}$ 通过最大化点对之间的协方差，使得变换后的源点云与目标点云在最小二乘意义下对齐。平移向量 $\mathbf{t}$ 则通过质心差校正整体偏移。

算法步骤总结

1. 去中心化：计算质心并减去，得到${\mathbf{x}}_i$和${\mathbf{y}}_i$。
2. 构建协方差矩阵：$\mathbf{H}=\sum {\mathbf{y}}_i{\mathbf{x}}_i^{\top }$。
3. SVD分解：$\mathbf{H}=\mathbf{U\Sigma }{\mathbf{V}}^{\top }$。
4. 求解旋转矩阵：$\mathbf{R}=\mathbf{V}{\mathbf{U}}^{\top }$，调整行列式。
5. 计算平移向量：$\mathbf{t}={\mathbf{\mu }}_q-\mathbf{R}{\mathbf{\mu }}_p$。

代码示例（Python）

import numpy as np

def solve_icp(source_points, target_points):
    # 输入：source_points (n×3), target_points (n×3)
    # 输出：R (3×3), t (3×1)
    
    # 计算质心
    mu_p = np.mean(source_points, axis=0)
    mu_q = np.mean(target_points, axis=0)
    
    # 去中心化
    X = source_points - mu_p
    Y = target_points - mu_q
    
    # 协方差矩阵
    H = X.T @ Y
    
    # SVD分解
    U, S, Vt = np.linalg.svd(H)
    
    # 计算旋转矩阵
    R = Vt.T @ U.T
    
    # 处理反射情况
    if np.linalg.det(R) < 0:
        Vt[-1, :] *= -1
        R = Vt.T @ U.T
    
    # 计算平移向量
    t = mu_q - R @ mu_p
    
    return R, t

# 示例数据
source = np.array([[1, 2, 3], [4, 5, 6], [7, 8, 9]])
target = np.array([[2, 3, 4], [5, 6, 7], [8, 9, 10]])

R, t = solve_icp(source, target)
print("Rotation Matrix:\n", R)
print("Translation Vector:\n", t)

关键点

1. 闭式解：通过SVD直接求解最优变换，无需迭代（适用于精确匹配）。
2. 反射问题：需检查$\det (\mathbf{R})$，避免反射变换。
3. 迭代优化：若存在噪声或匹配误差，需多次迭代优化。

通过上述步骤，可高效求解已知匹配3D点对的刚体变换，适用于点云配准、物体位姿估计等场景。