一. 奇异值分解在3D视觉中的应用
ICP问题的应用
ICP (Iterative Closest Point)迭代最近点在点云配准、手眼标定当中都有应用。
比如在点云配准中，两帧点云有n对对应的点，如何寻找欧式变换将这两组点重合，这就是ICP在点云配准中的应用。
再比如3D相机和机械臂的手眼标定，获得多个点在相机坐标系中的坐标Pc以及在机械臂坐标系中的坐标PB，然后将Pb和PC配准重合，就得到了相机坐标系和机械臂坐标系的欧式变换。
矩阵的奇异值分解 (SVD)
令矩阵\(A\in \mathbb{R}^{m \times n}\)，存在两个正交矩阵（复数域则是酉矩阵）\(U\in \mathbb{R}^{m \times m}\) 和 \(V\in \mathbb{R}^{n \times n}\)，使得
式中，\(\Sigma =\begin{bmatrix}\Sigma_1 & 0\\0 & 0\end{bmatrix},\Sigma_{1} = \text{diag}(\sigma_1, \dots, \sigma_r)\)为r阶方阵，\(\sigma_i\) 为A的奇异值，且按大小降序排列，\(r=\text{rank}(A)\)。
SVD求解迭代最近点 (ICP)
如下图1所示，有两个三维点的集合A和B，集合A中的点\(p_i\)和集合B中的点\(p_i'\)一一对应。
ICP问题就是寻找欧式变换 R, t，使得\(p_i = Rp_i'+t\)。即寻找一个欧式变换使得点集B经过此变换与点集A重合，如图2所示。
如果这里点集A是一帧点云，B是下一帧点云，那么ICP就是如何对这两帧点云进行配准。
如果这里点集A是相机坐标系中的点，点集B是在机械臂基坐标系下的点，那么这里ICP就是求解手眼标定问题。
这里的推导过程参考了 K. S. ARUN, T. S. HUANG, AND S. D. BLOSTEIN. Least-Squares Fitting of Two 3-D Point Sets，省略了一些证明的部分，感兴趣的读者可以看看这篇文章。
构建最小二乘问题，我们的目标就是求解R, t使得下式最小二乘误差最小
我们使用每个点集的去质心坐标带入上面的最小二乘误差中，令A的质心为\(\bar p = {1 \over n} \sum_{i=1}^np_i\)，A中每个点的去质心坐标表示为\(q_i = p_i -\bar p\)，类似的，定义B的质心为\(\bar p' = {1 \over n} \sum_{i=1}^np_i'\)，B中每个点的去质心坐标为\(q_i'=p_i' - \bar p'\)。
将去质心坐标带入误差\(J=\sum_i^n||p_i -(Rp_i'+t) ||^2\)，（这里我省略了二范数的下标），得到
我们观察上式中，前面的部分\((q_i -Rq_i')\) 是点集A和旋转后的点集B的去质心坐标之差，表示了姿态的误差，后面的项 \(\bar p - (R \bar p' +t)\) 是点集A的质心和点集B质心的误差。而最小二乘法得到的 R, t 是保证两个点集的质心重合的，因此 \(\bar p - (R \bar p' +t) = 0\)，误差变为(省略\(\sum\)上下标)
上式中，\(q_i^TRq_i'\) 和 \(q_i'^TR^Tq_i\) 两项都是标量，可以合并；第四项中的\(R^TR=I\)，于是上式简化为
上面的误差中，只有 \(- 2q_i^TRq_i'\) 中含有优化变量，于是最小化上面的误差函数等效于最大化 \(F= \sum q_i^TRq_i'\)
\(q_i^TRq_i'\) 其实是个标量，我们利用矩阵的迹的性质 \(x^TAx = tr(Axx^T)\)，得到
令\(H=\sum q_i' q_i^T\)，我们展开看\(\sum q_i' q_i^T = \sum(p_i'-\bar p')(p_i - \bar p)\) 。
如果我们以随机向量的观点来看的话，随机向量a用a(i)表示（可以理解为点集A中的点），随机向量b用p'(i)表示，a和b的协方差（统计中一般除以n-1）就是
\[Cov(a,b)=E\{[a(i)-\bar a][b(i) - \bar b]^T \} = {1 \over n}\sum_{i=1}^n[a(i)-\bar a ][b(i)- \bar b]^T \]对比H矩阵展开的结果，我们发现H矩阵其实就是A和B点集中点的协方差乘以点的个数n。
对H进行奇异值分解，\(H=\sum q_i' q_i^T = U \Lambda V^T\)，则可以求得旋转矩阵\(R=VU^T\)。
再将求得的R带入\(\bar p - (R \bar p' +t) = 0\)中求得平移向量 t。
案例讲解
下面我们用Python来实现一个SVD求解点对配准的案例
import numpy as np
from scipy import linalg
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
from scipy.spatial.transform import Rotation
# Generate pointset A and B
A = np.random.rand(10,3)
# Random rotation and translation
R = Rotation.random().as_matrix()
print('Rotation matrix:\n ', R)
t = np.random.rand(3)
print('Translation vector:\n ', t)
B = []
for i in range(len(A)):
p = R @ A[i] + t
print(f'pa:{A[i]} -> pb:{p}')
B.append(p)
B = np.array(B)
fig = plt.figure()
ax1 = Axes3D(fig)
ax1.scatter(A[:,0], A[:,1], A[:,2], c='r', label='A')
ax1.scatter(B[:,0], B[:,1], B[:,2], c='b', marker='x', label='B')
for i in range(len(A)):
ax1.plot([A[i,0], B[i,0]], [A[i,1], B[i,1]], [A[i,2], B[i,2]], c='k', linewidth=0.5)
ax1.legend()
plt.show()
centroid_A = A.mean(axis=0)
centroid_B = B.mean(axis=0)
decenter_A = A - centroid_A
decenter_B = B - centroid_B
sum = np.zeros((3,3))
for i in range(len(A)):
sum += np.outer(decenter_A[i], decenter_B[i])
U, S, Vt = linalg.svd(sum) # sum = U @ S @ Vt
R_hat = Vt.T @ U.T
t_hat = centroid_B - R_hat @ centroid_A
print('Estimated rotation matrix:\n', R_hat)
print('Estimated tranlation vector:\n', t_hat)
图3为随机生成的点集A中的十个点，以及经过随机变换后的点集B中的十个点，连线表明了点之间的对应关系。
图4是SVD计算的R, t，可以看到和随机生成的R, t是一致的。
奇异值分解正交矩阵UV的意义
我们使用去质心坐标q带入A, B点对的对应关系中
可以发现在去质心坐标下有 \(q_i = R q_i'\)，即A中的点 \(q_i\) 和B中对应的点 \(q_i'\)仅相差一个旋转矩阵，我们将求得的旋转矩阵展开
U和V分别都是正交矩阵\(UU^T=I\)，我们将他们写成列向量形式，\(U=[U_x, U_y, U_z], V=[V_x, V_y, V_z]\)，带入上式
也就是说，在以U的三个列向量为基底的表示下的A中的点 \(q_i\) 与 以V的三个列向量为基底的表示下的B中的对应点 \(q_i'\) 的是相等的。
\(U^Tq_i\)和\(V^Tq_i'\)表示的是同一个向量，这个向量在基U下坐标为\(q_i\)，在基V下坐标为\(q_i'\)。
求得的旋转矩阵\(R=VU^T\)其实就是进行了基的变换。
下图中粗一点的坐标轴是U的三个列向量构成的正交基（点集A的坐标系），细一点的坐标轴是V的三个列向量构成的正交基（点集B的坐标系）。
我们再将坐标系绘制在每个点集的质心处，得到下图。我们求解ICP得到的R, t就是这两个坐标系之间的姿态差异和位移。
看到这个坐标轴，读者可能会发现和主成分分析(Principal Component Analysis，PCA)有些类似，后续会用另一篇文章进行介绍。
参考文献
张贤达. 《矩阵分析与应用》第二版
高翔.《视觉SLAM十四讲》
K. S. ARUN, T. S. HUANG, AND S. D. BLOSTEIN. Least-Squares Fitting of Two 3-D Point Sets
浙公网安备 33010602011771号