一. 奇异值分解在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}$，使得

$$A_{m\times n}=U_m{\mathrm{\Sigma }}_{m\times n}{V}_n^T$$

式中，$\mathrm{\Sigma }=\left[\begin{array}{cc}{\mathrm{\Sigma }}_1& 0\\ 0& 0\end{array}\right],{\mathrm{\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^{\prime }$一一对应。

ICP问题就是寻找欧式变换 R, t，使得$p_i=R{p}_i^{\prime }+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使得下式最小二乘误差最小

$$\underset{R,t}{min}\frac{1}{2}\sum _i^n||p_i-(R{p}_i^{\prime }+t)|{|}_2^2$$

我们使用每个点集的去质心坐标带入上面的最小二乘误差中，令A的质心为$\overline{p}=\frac{1}{n}\sum _{i=1}^n{p}_i$，A中每个点的去质心坐标表示为$q_i=p_i-\overline{p}$，类似的，定义B的质心为${\overline{p}}^{\prime }=\frac{1}{n}\sum _{i=1}^n{p}_i^{\prime }$，B中每个点的去质心坐标为$q_i^{\prime }=p_i^{\prime }-{\overline{p}}^{\prime }$。

将去质心坐标带入误差$J=\sum _i^n||p_i-(R{p}_i^{\prime }+t)|{|}^2$，（这里我省略了二范数的下标），得到

$$\begin{gathered} J=\sum _i^n||q_i+\overline{p}-[R(q_i^{\prime }+{\overline{p}}^{\prime })+t]|{|}^2 \\ =\sum _i^n||q_i-R{q}_i^{\prime }+\overline{p}-(R{\overline{p}}^{\prime }+t)|{|}^2 \end{gathered}$$

我们观察上式中，前面的部分$(q_i-R{q}_i^{\prime })$ 是点集A和旋转后的点集B的去质心坐标之差，表示了姿态的误差，后面的项 $\overline{p}-(R{\overline{p}}^{\prime }+t)$ 是点集A的质心和点集B质心的误差。而最小二乘法得到的 R, t 是保证两个点集的质心重合的，因此 $\overline{p}-(R{\overline{p}}^{\prime }+t)=0$，误差变为(省略$\sum$上下标)

$$\begin{gathered} J=\sum ||q_i-R{q}_i^{\prime }|{|}^2 \\ =\sum (q_i-R{q}_i^{\prime }{)}^T(q_i-R{q}_i^{\prime }) \\ =\sum (q_i^T{q}_i-q_i^{T}R{q}_i^{\prime }-q_i^{\prime T}{R}^T{q}_i+q_i^{\prime T}{R}^{T}R{q}_i^{\prime }) \end{gathered}$$

上式中，$q_i^{T}R{q}_i^{\prime }$ 和 $q_i^{\prime T}{R}^T{q}_i$ 两项都是标量，可以合并；第四项中的$R^{T}R=I$，于是上式简化为

$$J=\sum (q_i^T{q}_i+q_i^{\prime T}{q}_i^{\prime }-2q_i^{T}R{q}_i^{\prime })$$

上面的误差中，只有 $-2q_i^{T}R{q}_i^{\prime }$ 中含有优化变量，于是最小化上面的误差函数等效于最大化 $F=\sum q_i^{T}R{q}_i^{\prime }$

$q_i^{T}R{q}_i^{\prime }$ 其实是个标量，我们利用矩阵的迹的性质 $x^{T}Ax=tr(Ax{x}^T)$，得到

$$F=\sum q_i^{T}R{q}_i^{\prime }=\text{tr}(R\sum q_i^{\prime }{q}_i^T)$$

令$H=\sum q_i^{\prime }{q}_i^T$，我们展开看$\sum q_i^{\prime }{q}_i^T=\sum (p_i^{\prime }-{\overline{p}}^{\prime })(p_i-\overline{p})$ 。

如果我们以随机向量的观点来看的话，随机向量a用a(i)表示（可以理解为点集A中的点），随机向量b用p'(i)表示，a和b的协方差（统计中一般除以n-1）就是

$$Cov(a,b)=E\{[a(i)-\overline{a}][b(i)-\overline{b}{]}^T\}=\frac{1}{n}\sum _{i=1}^n[a(i)-\overline{a}][b(i)-\overline{b}{]}^T$$

对比H矩阵展开的结果，我们发现H矩阵其实就是A和B点集中点的协方差乘以点的个数n。

对H进行奇异值分解，$H=\sum q_i^{\prime }{q}_i^T=U\mathrm{\Lambda }{V}^T$，则可以求得旋转矩阵$R=V{U}^T$。

再将求得的R带入$\overline{p}-(R{\overline{p}}^{\prime }+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点对的对应关系中

$$\begin{gathered} p_i-(R{p}_i^{\prime }+t) \\ =q_i+\overline{p}-[R(q_i^{\prime }+{\overline{p}}^{\prime })+t] \\ =q-R{q}_i^{\prime }+\overline{p}-(R{\overline{p}}_i^{\prime }+t) \\ =q_i-R{q}_i^{\prime }=0 \end{gathered}$$

可以发现在去质心坐标下有 $q_i=R{q}_i^{\prime }$，即A中的点 $q_i$ 和B中对应的点 $q_i^{\prime }$仅相差一个旋转矩阵，我们将求得的旋转矩阵展开

$$q_i=R{q}_i^{\prime }=V{U}^T{q}_i^{\prime }\Rightarrow V^T{q}_i=U^T{q}_i^{\prime }$$

U和V分别都是正交矩阵$U{U}^T=I$，我们将他们写成列向量形式，$U=[U_x,U_y,U_z],V=[V_x,V_y,V_z]$，带入上式

$$[U_x,U_y,U_z{]}^T{q}_i=[V_x,V_y,V_z{]}^T{q}_i^{\prime }\Rightarrow \left[\begin{array}{c}{U}_x^T{q}_i\\ U_y^T{q}_i\\ U_z^T{q}_i\end{array}\right]=\left[\begin{array}{c}{V}_x^T{q}_i^{\prime }\\ V_y^T{q}_i^{\prime }\\ V_z^T{q}_i^{\prime }\end{array}\right]$$

也就是说，在以U的三个列向量为基底的表示下的A中的点 $q_i$ 与 以V的三个列向量为基底的表示下的B中的对应点 $q_i^{\prime }$ 的是相等的。

$U^T{q}_i$和$V^T{q}_i^{\prime }$表示的是同一个向量，这个向量在基U下坐标为$q_i$，在基V下坐标为$q_i^{\prime }$。

求得的旋转矩阵$R=V{U}^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