点云配准--迭代最近点ICP（Iterative Closest Point）

1 ICP ^[1]

1.1 问题描述

假设有一组配对好的点（即点与点之间的对应关系已知）：

\[P=\{p_1,...,p_n\},P'=\{p_1',...,p_n'\} \]

寻找一个欧氏变换\(R,t\)，使得

\[\forall i,p_i=Rp_i'+t \]

这个问题可以用迭代最近点法（ICP）求解。其求解方式分为两种：
(1)利用线性代数求解（SVD）
(2)利用非线性优化求解

1.2 SVD方法

1.2.1 前置知识：奇异值分解（SVD）

1.2.1.1 一个m×n矩阵的奇异值

令A是m×n矩阵，那么\(A^TA\)是对称矩阵且可以正交对角化，则A的奇异值是\(A^TA\)的特征值的平方根，记为\(\sigma_1,...,\sigma_n\)，且用递减顺序排列

1.2.1.2 奇异值分解 ^[2]

矩阵A的分解涉及一个m×n的“对角”矩阵\(\Sigma\)，其形式为：

\[\Sigma=\begin{bmatrix} D&0 \\ 0&0 \end{bmatrix} \]

其中D是一个r×r矩阵，且\(r<=min\{m,n\}\)

定义：设A是秩为r的m × n矩阵，那么存在m×n矩阵\(\Sigma\)，其中D的对角线元素是A的前r个奇异值，\(\sigma_1>=\sigma_2>=...\sigma_r\)，并且存在一个m×m正交矩阵U和一个nxn正交矩阵V使得\(A=U \Sigma V^T\)
任何分解\(A=U \Sigma V^T\)称为A的一个奇异值分解(或SVD)，其中U和V是正交矩阵，\(\Sigma\)中D具有正的对角线元素.矩阵U和V不是由A惟一确定的（即奇异值分解不惟一），但\(\Sigma\)的对角线元素必须是A的奇异值，.这样的一个分解中U的列称为的左奇异向量，而V的列称为A的右奇异向量.

1.2.1.3 求解步骤及示例

求

\[A=\begin{bmatrix} 4&11&14 \\ 8&7&-2 \\ \end{bmatrix} \]

的一个奇异值分解

解：
第一步：将矩阵\(A^TA\)正交对角化，即求矩阵\(A^TA\)的特征值及其对应的特征向量的单位正交集。
矩阵\(A^TA\)的特征值是\(\lambda_1=360\)，\(\lambda_1=90\)，\(\lambda_3=0\)，对应的单位特征向量是，

\[v_1= \begin{bmatrix} 1/3 \\ 2/3 \\ 2/3 \\ \end{bmatrix} , v_2= \begin{bmatrix} -2/3 \\ -1/3 \\ 2/3 \\ \end{bmatrix} , v_3= \begin{bmatrix} 2/3 \\ -2/3 \\ 1/3 \\ \end{bmatrix} \]

第二步：算出\(V\)和\(\Sigma\)。将\(A^TA\)的特征值按降序排列（在第一步已完成），对应的单位特征向量\(v_1,v_2,v_3\)是A的右奇异值向量，构造

\[V= \begin{bmatrix} v_1,v_2,v_3 \end{bmatrix} = \begin{bmatrix} 1/3 & -2/3 & 2/3 \\ 2/3 & -1/3 & -2/3 \\ 2/3 & 2/3 & 1/3 \\ \end{bmatrix} \]

特征值的平方根就是奇异值：\(\sigma_1=6\sqrt{10},\sigma_2=3\sqrt{10},\sigma_3=0\)，非零奇异值为矩阵D对角线元素。构造矩阵\(\Sigma\)

\[\Sigma = \begin{bmatrix} 6\sqrt{10} & 0 & 0 \\ 0 & 3\sqrt{10} & 0 \end{bmatrix} \]

第三步：构造U。由于\(A=U \Sigma V^T\)，可解出\(U=AV\Sigma^T\)

所以A的奇异值分解是

\[A= \begin{bmatrix} 3/\sqrt{10} & 1/\sqrt{10} \\ \sqrt{10} & -3/\sqrt{10} \end{bmatrix} \begin{bmatrix} 6\sqrt{10} & 0 & 0 \\ 0 & 3\sqrt{10} & 0 \end{bmatrix} \begin{bmatrix} 1/3 & -2/3 & 2/3 \\ 2/3 & -1/3 & -2/3 \\ 2/3 & 2/3 & 1/3 \\ \end{bmatrix} \]

1.2.1.4 编程实现

matlab

C++

1.2.2 求解过程

首先定义第i对点的误差项：

\[e_i=p_i-(Rp_i'+t) \]

然后构建最小二乘问题，求使误差平方和达到最小的R，t（以下是手写的化简过程）:

\[\min_{R,t} \frac{1}{2} \displaystyle\sum_{i=1}^n||(p_i-(Rp_i'+t)||_2^2 \]

1.3 非线性优化方法

参考资料：

---

1. 《视觉SLAM十四讲：从理论到实践》高翔 ↩︎

2. 《线性代数及其应用 原书第5版》 David C.Lay ↩︎