相似变换 (旋转、平移、缩放) 参数的最小方差估计 —— Umeyama 算法详细推导 (II)

Title: 相似变换 (旋转、平移、缩放) 参数的最小方差估计 —— Umeyama 算法详细推导 (II)

---

相关博文介绍

- 矩阵乘法可交换与可同时对角化的关系 —— Umeyama 算法推导的数学准备 (I)

- 旋转矩阵约束下的朗格朗日乘子 —— Umeyama 算法推导的数学准备 (II)

- 矩阵乘操作、三角化、开方特征值 —— Umeyama 算法推导的数学准备 (III)

- 奇异值分解之常用结论

- 旋转参数的最小方差估计 —— Umeyama 算法详细推导 (I)

- 相似变换参数 (旋转、平移、缩放) 的最小方差估计 —— Umeyama 算法详细推导 (II)

- Umeyama 算法之源码阅读与测试

---

前言

SLAM 轨迹的对齐和评估时, 多用 Umeyama 算法实现.

该算法要解决的问题为:

给定两个 $m$ 维空间内的点集 {${\mathbf{x}}_i$} 和 {${\mathbf{y}}_i$} (其中 $i=1,2,\dots ,n$) 找出最小误差平方意义下的相似变换参数 (Similarity Transformation).

该算法初见于 S. Umeyama 的一篇论文 “Least-squares estimation of transformation parameters between two point patterns”^[1].

相似变换

相似变换 Similarity Transformation = 旋转 Rotation $\mathbf{R}$ + 平移 Translation $\mathbf{t}$ + 放缩 Scale $c$

上一篇博文中已经详细推导了 “[引理] 旋转参数的最小方差估计”, 该引理中只考虑了旋转变换, 获得了最优旋转参数估计 $\mathbf{R}$.

---

I. Umeyama 算法

1. 引理

[引理] 旋转参数的最小方差估计

$\mathbf{A}$ 和 $\mathbf{B}$ 都是 $m\times n$ 的矩阵.

$\mathbf{R}$ 是 $m\times m$ 的旋转矩阵.

$\mathbf{UD}{\mathbf{V}}^{\mathrm{T}}$ 是 $\mathbf{A}{\mathbf{B}}^{\mathrm{T}}$ 的奇异值分解, 即
$$\mathbf{A}{\mathbf{B}}^{\mathrm{T}}=\mathbf{UD}{\mathbf{V}}^{\mathrm{T}}\text{\hspace{1em}(I-1-1)}$$
其中 $\mathbf{U}{\mathbf{U}}^{\mathrm{T}}=\mathbf{V}{\mathbf{V}}^{\mathrm{T}}=\mathbf{I}$, $\mathbf{D}=\mathrm{diag}(d_i)$, $d_1\ge d_2\ge \cdots \ge d_m\ge 0$.

以 $\mathbf{R}$ 为参数的目标函数 $\Vert \mathbf{A}-\mathbf{RB}{\Vert }_F$ 的最小值为
$$\underset{\mathbf{R}}{\min }\Vert \mathbf{A}-\mathbf{RB}{\Vert }_F^2=\Vert \mathbf{A}{\Vert }_F^2+\Vert \mathbf{B}{\Vert }_F^2-2\mathrm{tr}(\mathbf{DS})\text{\hspace{1em}(I-1-2)}$$
其中
$$\mathbf{S}=\{\begin{array}{ll}\mathbf{I},& \text{if}\det (\mathbf{A}{\mathbf{B}}^{\mathrm{T}})\ge 0\\ \mathrm{diag}(1,1,\cdots ,1,-1),& \text{if}\det (\mathbf{A}{\mathbf{B}}^{\mathrm{T}})<0\end{array}\text{\hspace{1em}(I-1-3)}$$
当 $\mathrm{rank}(\mathbf{A}{\mathbf{B}}^{\mathrm{T}})\ge m-1$ 时, 达到式 (I-1-2) 目标函数最小化所需的最优的旋转矩阵 $\mathbf{R}$ 被唯一确定为
$$\mathbf{R}=\mathbf{US}{\mathbf{V}}^{\mathrm{T}}\text{\hspace{1em}(I-1-4)}$$
其中当 $\det (\mathbf{A}{\mathbf{B}}^{\mathrm{T}})=0$ 时 (也就是说, 当 $\mathrm{rank}(\mathbf{A}{\mathbf{B}}^{\mathrm{T}})=m-1$ 时), 式 (I-1-4) 中的 $\mathbf{S}$ 需要选择为
$$\mathbf{S}=\{\begin{array}{ll}\mathbf{I},& \text{if}\det (\mathbf{U})\det (\mathbf{V})=1\\ \mathrm{diag}(1,1,\cdots ,1,-1),& \text{if}\det (\mathbf{U})\det (\mathbf{V})=-1\end{array}\text{\hspace{1em}(I-1-5)}$$

---

2. 定理

[定理] 相似变换参数的最小方差估计

令 X = { x 1 , x 2 , ⋯   , x n } \mathbf{X} = \{\mathbf{x}_1, \mathbf{x}_2,\cdots, \mathbf{x}_n\} X={x1​,x2​,⋯,xn​} 和 Y = { y 1 , y 2 , ⋯   , y n } \mathbf{Y} = \{\mathbf{y}_1, \mathbf{y}_2,\cdots, \mathbf{y}_n\} Y={y1​,y2​,⋯,yn​} 是 $m$-维空间中的两组对应点样本. 这两组对应点样本进行相似变换 (旋转 $\mathbf{R}$、平移 $\mathbf{t}$、缩放 $c$) , 得到的均方误差 (mean-square error, MSE) 为
$$e^2(\mathbf{R},\mathbf{t},c)=\frac{1}{n}\sum _{i=1}^n\Vert {\mathbf{y}}_i-(c\mathbf{R}{\mathbf{x}}_i+\mathbf{t}){\Vert }_2^2\text{\hspace{1em}(I-2-1)}$$
而最小均方误差为
$${\epsilon }^2={\sigma }_y^2-\frac{\mathrm{tr}(\mathbf{DS}{)}^2}{{\sigma }_x^2}\text{\hspace{1em}(I-2-2)}$$
其中
$${\mathbf{\mu }}_x=\frac{1}{n}\sum _{i=1}^n{\mathbf{x}}_i\text{\hspace{1em}(I-2-3)}$$

$${\mathbf{\mu }}_y=\frac{1}{n}\sum _{i=1}^n{\mathbf{y}}_i\text{\hspace{1em}(I-2-4)}$$

$${\sigma }_x^2=\frac{1}{n}\sum _{i=1}^n\Vert {\mathbf{x}}_i-{\mathbf{\mu }}_x{\Vert }_2^2\text{\hspace{1em}(I-2-5)}$$

$${\sigma }_y^2=\frac{1}{n}\sum _{i=1}^n\Vert {\mathbf{y}}_i-{\mathbf{\mu }}_y{\Vert }_2^2\text{\hspace{1em}(I-2-6)}$$

$${\sum }_{xy}=\frac{1}{n}\sum _{i=1}^n({\mathbf{y}}_i-{\mathbf{\mu }}_y)({\mathbf{x}}_i-{\mathbf{\mu }}_x{)}^{\mathrm{T}}\text{\hspace{1em}(I-2-7)}$$

- ${\mathbf{\mu }}_x$ 和 ${\mathbf{\mu }}_y$ 分别是 $\mathbf{X}$ 和 $\mathbf{Y}$ 的均值向量

- ${\sigma }_x^2$ 和 ${\sigma }_x^2$ 分别是 $\mathbf{X}$ 和 $\mathbf{Y}$ 的围绕各自均值向量的方差

- ${\sum }_{xy}$ 是 $\mathbf{X}$ 和 $\mathbf{Y}$ 的协方差矩阵

令 ${\sum }_{xy}$ 的奇异值分解式为 $\mathbf{UD}{\mathbf{V}}^{\mathrm{T}}$, 其中 $\mathbf{D}=\mathrm{diag}(d_i)$, $d_1\ge d_2\ge \cdots \ge d_m\ge 0$, 以及令
$$\mathbf{S}=\{\begin{array}{ll}\mathbf{I},& \text{if}\det ({\sum }_{xy})\ge 0\\ \mathrm{diag}(1,1,\cdots ,1,-1),& \text{if}\det ({\sum }_{xy})<0\end{array}\text{\hspace{1em}(I-2-8)}$$
当 $\mathrm{rank}({\sum }_{xy})>m-1$ 时, 最优相似变换参数被唯一确定
$$\mathbf{R}=\mathbf{US}{\mathbf{V}}^{\mathrm{T}}\text{\hspace{1em}(I-2-9)}$$

$$\mathbf{t}={\mathbf{\mu }}_y-c\mathbf{R}{\mathbf{\mu }}_x\text{\hspace{1em}(I-2-10)}$$

$$c=\frac{1}{{\sigma }_x^2}\mathrm{tr}(\mathbf{DS})\text{\hspace{1em}(I-2-11)}$$

当 $\mathrm{rank}({\sum }_{xy})=m-1$ 时, 式 (I-2-9) 中的 $\mathbf{S}$ 必须选为
$$\mathbf{S}=\{\begin{array}{ll}\mathbf{I},& \text{if}\det (\mathbf{U})\det (\mathbf{V})=1\\ \mathrm{diag}(1,1,\cdots ,1,-1),& \text{if}\det (\mathbf{U})\det (\mathbf{V})=-1\end{array}\text{\hspace{1em}(I-2-12)}$$

---

II. 推导

1. 正则矩阵的定义与性质

表一. 正则矩阵的定义

公式名/编号
T-1-1	$\mathbf{h}\triangleq {\underset{n}{\underbrace{\left[\begin{array}{cccc}1& 1& \cdots & 1\end{array}\right]}}}^{\mathrm{T}}$
正则矩阵 T-1-2	$\mathbf{K}\triangleq \mathbf{I}-\frac{1}{n}\mathbf{h}{\mathbf{h}}^{\mathrm{T}}=\left[\begin{array}{cccc}1-\frac{1}{n}& -\frac{1}{n}& \cdots & -\frac{1}{n}\\ -\frac{1}{n}& 1-\frac{1}{n}& \cdots & -\frac{1}{n}\\ ⋮& ⋮& \ddots & ⋮\\ -\frac{1}{n}& -\frac{1}{n}& \cdots & 1-\frac{1}{n}\end{array}\right]$

表二. 正则矩阵的性质

公式名/编号	性质
T-2-1	${\mathbf{h}}^{\mathrm{T}}\mathbf{h}=n$
复制性 T-2-2	$\mathbf{t}{\mathbf{h}}^{\mathrm{T}}=\mathbf{t}\underset{n}{\underbrace{\left[\begin{array}{cccc}1& 1& \cdots & 1\end{array}\right]}}=\underset{n}{\underbrace{\left[\begin{array}{cccc}\mathbf{t}& \mathbf{t}& \cdots & \mathbf{t}\end{array}\right]}},\mathrm{where}\mathbf{t}\in {\mathbb{R}}^x$
不变性 T-2-3	$\begin{array}{rl}\mathbf{KK}& =\left(\mathbf{I}-\frac{1}{n}\mathbf{h}{\mathbf{h}}^{\mathrm{T}}\right)\left(\mathbf{I}-\frac{1}{n}\mathbf{h}{\mathbf{h}}^{\mathrm{T}}\right)\\ & =\mathbf{I}-\frac{1}{n}\mathbf{h}{\mathbf{h}}^{\mathrm{T}}-\frac{1}{n}\mathbf{h}{\mathbf{h}}^{\mathrm{T}}+\frac{1}{n^2}\mathbf{h}{\mathbf{h}}^{\mathrm{T}}\mathbf{h}{\mathbf{h}}^{\mathrm{T}}\\ & =\mathbf{I}-\frac{1}{n}\mathbf{h}{\mathbf{h}}^{\mathrm{T}}\\ & =\mathbf{K}\end{array}$
对称性 T-2-4	$\mathbf{K}={\mathbf{K}}^{\mathrm{T}}=\mathbf{KK}$
正交性 T-2-5	${\mathbf{h}}^{\mathrm{T}}\mathbf{K}={\mathbf{h}}^{\mathrm{T}}\left(\mathbf{I}-\frac{1}{n}\mathbf{h}{\mathbf{h}}^{\mathrm{T}}\right)=0$

已知矩阵的 Frobenius-范数 的平方是其各个元素平方的和. 将这些元素的平方按元素所在列为一组求和, 再把各列的和相加, 就是所有元素的平方和. 这样就建立了矩阵 Frobenius-范数和矩阵各个列向量 2-范数之间的关系.

表三. 矩阵 Frobenius-范数的列形式

公式名/编号	性质
T-3-1	$|\mathbf{X}{|}_F^2={\sum }_{i=1}^n|{\mathbf{x}}_i{|}_2^2$
T-3-2	$|\mathbf{t}{\mathbf{h}}^{\mathrm{T}}{|}_F^2=\mathrm{tr}\left[\left(\mathbf{t}{\mathbf{h}}^{\mathrm{T}}\right){\left(\mathbf{t}{\mathbf{h}}^{\mathrm{T}}\right)}^{\mathrm{T}}\right]=n\mathrm{tr}(\mathbf{t}{\mathbf{t}}^{\mathrm{T}})=n|\mathbf{t}{|}_2^2$

表四. 正则矩阵的统计应用

公式名/编号	性质
均值 T-4-1	${\mathbf{\mu }}_x=\frac{1}{n}{\sum }_{i=1}^n{\mathbf{x}}_i=\frac{1}{n}\mathbf{Xh}$
均值 T-4-2	${\mathbf{\mu }}_y=\frac{1}{n}{\sum }_{i=1}^n{\mathbf{y}}_i=\frac{1}{n}\mathbf{Yh}$
零均值化 T-4-3	$\begin{array}{rl}\mathbf{XK}& =\left[\begin{array}{cccc}{x}_{11}& x_{12}& \cdots & x_{1n}\\ x_{21}& x_{22}& \cdots & x_{2n}\\ ⋮& ⋮& \ddots & ⋮\\ x_{n1}& x_{n2}& \cdots & x_{nn}\end{array}\right]\left[\begin{array}{cccc}1-\frac{1}{n}& -\frac{1}{n}& \cdots & -\frac{1}{n}\\ -\frac{1}{n}& 1-\frac{1}{n}& \cdots & -\frac{1}{n}\\ ⋮& ⋮& \ddots & ⋮\\ -\frac{1}{n}& -\frac{1}{n}& \cdots & 1-\frac{1}{n}\end{array}\right]\\ & =\left[\begin{array}{cccc}{x}_{11}-\frac{1}{n}{\sum }_i{x}_{1i}& x_{12}-\frac{1}{n}{\sum }_i{x}_{1i}& \cdots & x_{1n}-\frac{1}{n}{\sum }_i{x}_{1i}\\ x_{21}-\frac{1}{n}{\sum }_i{x}_{2i}& x_{22}-\frac{1}{n}{\sum }_i{x}_{2i}& \cdots & x_{2n}-\frac{1}{n}{\sum }_i{x}_{2i}\\ ⋮& ⋮& \ddots & ⋮\\ x_{n1}-\frac{1}{n}{\sum }_i{x}_{ni}& x_{n2}-\frac{1}{n}{\sum }_i{x}_{ni}& \cdots & x_{nn}-\frac{1}{n}{\sum }_i{x}_{ni}\end{array}\right]\\ & =\left[\begin{array}{cccc}{\mathbf{x}}_1-{\mathbf{\mu }}_x& {\mathbf{x}}_2-{\mathbf{\mu }}_x& \cdots & {\mathbf{x}}_n-{\mathbf{\mu }}_x\end{array}\right]\end{array}$
零均值化 T-4-4	$\mathbf{YK}=\left[\begin{array}{cccc}{\mathbf{y}}_1-{\mathbf{\mu }}_y& {\mathbf{y}}_2-{\mathbf{\mu }}_y& \cdots & {\mathbf{y}}_n-{\mathbf{\mu }}_y\end{array}\right]$
方差 T-4-5	$\frac{1}{n}|\mathbf{XK}{|}_F^2=\frac{1}{n}{\sum }_{i=1}^n|{\mathbf{x}}_i-{\mathbf{\mu }}_x{|}_2^2={\sigma }_x^2$
方差 T-4-6	${\sigma }_y^2=\frac{1}{n}|\mathbf{YK}{|}_F^2$
协方差 T-4-7	$\begin{array}{rl}\frac{1}{n}\mathbf{YK}{\mathbf{X}}^{\mathrm{T}}& =\frac{1}{n}\mathbf{YK}{\mathbf{K}}^{\mathrm{T}}{\mathbf{X}}^{\mathrm{T}}=\frac{1}{n}\left(\mathbf{YK}\right){\left(\mathbf{XK}\right)}^{\mathrm{T}}\\ & =\frac{1}{n}\left[\begin{array}{cccc}{\mathbf{y}}_1-{\mathbf{\mu }}_y& {\mathbf{y}}_2-{\mathbf{\mu }}_y& \cdots & {\mathbf{y}}_n-{\mathbf{\mu }}_y\end{array}\right]\left[\begin{array}{c}({\mathbf{x}}_1-{\mathbf{\mu }}_x{)}^{\mathrm{T}}\\ ({\mathbf{x}}_2-{\mathbf{\mu }}_x{)}^{\mathrm{T}}\\ ⋮\\ ({\mathbf{x}}_n-{\mathbf{\mu }}_x{)}^{\mathrm{T}}\end{array}\right]\\ & =\frac{1}{n}\sum _{i=1}^n({\mathbf{y}}_i-{\mathbf{\mu }}_y)({\mathbf{x}}_i-{\mathbf{\mu }}_x{)}^{\mathrm{T}}\\ & ={\sum }_{xy}\end{array}$

表五. 正则矩阵的其他计算结果

公式名/编号	性质
T-5-1	$\mathbf{XK}+\frac{1}{n}\mathbf{Xh}{\mathbf{h}}^{\mathrm{T}}=\mathbf{X}(\mathbf{I}-\frac{1}{n}\mathbf{h}{\mathbf{h}}^{\mathrm{T}})+\frac{1}{n}\mathbf{Xh}{\mathbf{h}}^{\mathrm{T}}=\mathbf{X}$
T-5-2	$\mathbf{YK}+\frac{1}{n}\mathbf{Yh}{\mathbf{h}}^{\mathrm{T}}=\mathbf{Y}$

---

2. 目标函数

由矩阵乘的操作可知
$$\left[\begin{array}{cccc}\mathbf{R}{\mathbf{x}}_1& \mathbf{R}{\mathbf{x}}_2& \cdots & \mathbf{R}{\mathbf{x}}_n\end{array}\right]=\left[\begin{array}{c}\mathbf{RX}\end{array}\right]\text{\hspace{1em}(II-2-1)}$$
再结合式 (T-2-2) 可知
$$\left[\begin{array}{cccc}{\mathbf{y}}_1-(c\mathbf{R}{\mathbf{x}}_1+\mathbf{t})& {\mathbf{y}}_2-(c\mathbf{R}{\mathbf{x}}_2+\mathbf{t})& \cdots & {\mathbf{y}}_n-(c\mathbf{R}{\mathbf{x}}_n+\mathbf{t})\end{array}\right]=\left[\begin{array}{c}\mathbf{Y}-(c\mathbf{RX}+\mathbf{t}{\mathbf{h}}^{\mathrm{T}})\end{array}\right]\text{\hspace{1em}(II-2-2)}$$
根据 Frobenius-范数 (T-3-1) 变换目标函数 (I-2-2)
$$\begin{array}{rl}{e}^2(\mathbf{R},\mathbf{t},c)& =\frac{1}{n}\sum _{i=1}^n{\Vert {\mathbf{y}}_i-(c\mathbf{R}{\mathbf{x}}_i+\mathbf{t})\Vert }_2^2\\ & =\frac{1}{n}{\Vert \mathbf{Y}-c\mathbf{RX}-\mathbf{t}{\mathbf{h}}^{\mathrm{T}}\Vert }_F^2\\ \text{(T-5-1)},\text{(T-5-2)}& =\frac{1}{n}{\Vert \mathbf{YK}+\frac{1}{n}\mathbf{Yh}{\mathbf{h}}^{\mathrm{T}}-c\mathbf{R}\left(\mathbf{XK}+\frac{1}{n}\mathbf{Xh}{\mathbf{h}}^{\mathrm{T}}\right)-\mathbf{t}{\mathbf{h}}^{\mathrm{T}}\Vert }_F^2\\ & =\frac{1}{n}{\Vert \mathbf{YK}-c\mathbf{RXK}+\left(\frac{1}{n}\mathbf{Yh}-\frac{c}{n}\mathbf{RXh}-\mathbf{t}\right){\mathbf{h}}^{\mathrm{T}}\Vert }_F^2\\ & =\frac{1}{n}{\Vert \left(\mathbf{YK}-c\mathbf{RXK}\right)-{\mathbf{t}}^{{}^{\prime }}{\mathbf{h}}^{\mathrm{T}}\Vert }_F^2\\ & =\frac{1}{n}\mathrm{tr}\left\{{\left[\left(\mathbf{YK}-c\mathbf{RXK}\right)-{\mathbf{t}}^{{}^{\prime }}{\mathbf{h}}^{\mathrm{T}}\right]}^{\mathrm{T}}\left[\left(\mathbf{YK}-c\mathbf{RXK}\right)-{\mathbf{t}}^{{}^{\prime }}{\mathbf{h}}^{\mathrm{T}}\right]\right\}\\ \text{(T-3-2)}& =\frac{1}{n}{\Vert \mathbf{YK}-c\mathbf{RXK}\Vert }_F^2+\Vert {\mathbf{t}}^{{}^{\prime }}{\Vert }_F^2+\underline{\mathrm{tr}\left[{\left(\mathbf{YK}-c\mathbf{RXK}\right)}^{\mathrm{T}}{\mathbf{t}}^{{}^{\prime }}{\mathbf{h}}^{\mathrm{T}}\right]}\\ & =\frac{1}{n}{\Vert \mathbf{YK}-c\mathbf{RXK}\Vert }_F^2+\Vert {\mathbf{t}}^{{}^{\prime }}{\Vert }_F^2\end{array}\text{\hspace{1em}(II-2-3)}$$
其中
$${\mathbf{t}}^{{}^{\prime }}\triangleq -\left(\frac{1}{n}\mathbf{Yh}-\frac{c}{n}\mathbf{RXh}-\mathbf{t}\right)\text{\hspace{1em}(II-2-4)}$$
另外, 式 (II-2-2) 倒数第二行下划线项被省略的原因是
$$\begin{array}{rl}\mathrm{tr}\left[{\left(\mathbf{YK}-c\mathbf{RXK}\right)}^{\mathrm{T}}{\mathbf{t}}^{{}^{\prime }}{\mathbf{h}}^{\mathrm{T}}\right]& =\mathrm{tr}\left[{\mathbf{K}}^{\mathrm{T}}{\left(\mathbf{Y}-c\mathbf{RX}\right)}^{\mathrm{T}}{\mathbf{t}}^{{}^{\prime }}{\mathbf{h}}^{\mathrm{T}}\right]\\ & =\mathrm{tr}\left[\underline{{\mathbf{h}}^{\mathrm{T}}{\mathbf{K}}^{\mathrm{T}}}{\left(\mathbf{Y}-c\mathbf{RX}\right)}^{\mathrm{T}}{\mathbf{t}}^{{}^{\prime }}\right]\\ \text{(T-2-5)}& =0\end{array}\text{\hspace{1em}(II-2-5)}$$

---

3. 平移参数的最优估计

要使目标函数式 (II-2-3) 达到最小值, 首先需要
$$\Vert {\mathbf{t}}^{{}^{\prime }}{\Vert }_F^2=0\text{\hspace{1em}(II-3-1)}$$
由式 (II-2-4) 可知, 此时需要 $\mathbf{t}$ 满足
$$\mathbf{t}=\frac{1}{n}\mathbf{Yh}-\frac{c}{n}\mathbf{RXh}={\mathbf{\mu }}_y-c\mathbf{R}{\mathbf{\mu }}_x\text{\hspace{1em}(II-3-2)}$$

这就是 [定理] 中的式 (I-2-10), 即平移参数的最优估计结果.

---

4. 旋转参数的最优估计

满足式 (II-3-1) 或式 (II-3-2) 的目标函数式为
$$\begin{array}{rl}{e}^2(\mathbf{R},\mathbf{t},c)& =\frac{1}{n}{\Vert \mathbf{YK}-c\mathbf{RXK}\Vert }_F^2\\ & =\frac{1}{n}{\Vert \bar{\mathbf{A}}-\mathbf{R}\bar{\mathbf{B}}\Vert }_F^2\end{array}\text{\hspace{1em}(II-4-1)}$$
其中定义
$$\bar{\mathbf{A}}\triangleq \mathbf{YK}\text{\hspace{1em}(II-4-2)}$$

$$\bar{\mathbf{B}}=c\mathbf{XK}\text{\hspace{1em}(II-4-3)}$$

由 [定理] 中的条件已知 “${\sum }_{xy}$ 的奇异值分解式为 $\mathbf{UD}{\mathbf{V}}^{\mathrm{T}}$”, 即
$$\begin{array}{rl}\mathbf{UD}{\mathbf{V}}^{\mathrm{T}}& ={\sum }_{xy}\stackrel{\text{(T-4-7)}}{=}\frac{1}{n}\mathbf{YK}{\mathbf{X}}^{\mathrm{T}}\\ \text{(T-2-4)}& =\frac{1}{n}\mathbf{YK}{\mathbf{K}}^{\mathrm{T}}{\mathbf{X}}^{\mathrm{T}}\\ & =\frac{1}{cn}(\mathbf{YK})(c\mathbf{XK}{)}^{\mathrm{T}}\\ & =\frac{1}{cn}\bar{\mathbf{A}}{\bar{\mathbf{B}}}^{\mathrm{T}}\end{array}\text{\hspace{1em}(II-4-4)}$$
也就是说 $\bar{\mathbf{A}}{\bar{\mathbf{B}}}^{\mathrm{T}}$ 的奇异值分解就是 $\mathbf{U}(cn\mathbf{D}){\mathbf{V}}^{\mathrm{T}}$, 其中 $cn\mathbf{D}$ 是奇异值降序排列组成的对角矩阵.

根据 [引理] 中式 (I-1-4) 可知, 使得式 (II-4-1) 描述的目标函数取最小值的旋转参数的最优估计结果为
$$\mathbf{R}=\mathbf{US}{\mathbf{V}}^{\mathrm{T}}\text{\hspace{1em}(II-4-5)}$$
当然首先要满足存在唯一解的条件
$$\mathrm{rank}({\sum }_{xy})>m-1\text{\hspace{1em}(II-4-6)}$$
否者无确切解 (可参考 旋转参数的最小方差估计 —— Umeyama 算法详细推导 (I) 中的讨论).

矩阵 $\mathbf{S}$ 也完全由 [引理] 替换对应参数获得, 这里不再重复.

根据 [引理] 中式 (I-1-2) 可知, 此时取得的最小值
$$\begin{array}{rl}{e}^2(\mathbf{R},\mathbf{t},c)& =\frac{1}{n}{\Vert \bar{\mathbf{A}}-\mathbf{R}\bar{\mathbf{B}}\Vert }_F^2\\ \text{(I-1-2)}& =\frac{1}{n}\Vert \bar{\mathbf{A}}{\Vert }_F^2+\frac{1}{n}\Vert \bar{\mathbf{B}}{\Vert }_F^2-\frac{2}{n}\mathrm{tr}(cn\mathbf{DS})\\ & =\frac{1}{n}\Vert \mathbf{YK}{\Vert }_F^2+\frac{1}{n}\Vert c\mathbf{XK}{\Vert }_F^2-\frac{2}{n}\mathrm{tr}(cn\mathbf{DS})\\ \text{(T-4-5),(T-4-6)}& ={\sigma }_y^2+c^2{\sigma }_x^2-2c\mathrm{tr}(\mathbf{DS})\end{array}\text{\hspace{1em}(II-4-7)}$$
上式是在对平移参数和旋转参数最优估计后, 得到的最小目标函数值.

---

5. 缩放参数的最优估计

可以看出此时的目标函数式 (II-4-7) 是关于 $c$ 的二次函数, 对其求关于 $c$ 的导数
$$\frac{d{e}^2(\mathbf{R},\mathbf{t},c)}{dc}=2c{\sigma }_x^2-2\mathrm{tr}(\mathbf{DS})\text{\hspace{1em}(II-5-1)}$$
计算极值点, 即为缩放参数的最优估计
$$c=\frac{\mathrm{tr}(\mathbf{DS})}{{\sigma }_x^2}\text{\hspace{1em}(II-5-2)}$$

---

6. 最小化目标函数

将最后一个最优估计参数, 代入式 (II-4-7) 得到最终的最小化目标函数值
$$\begin{array}{rl}{\epsilon }^2& ={\sigma }_y^2+c^2{\sigma }_x^2-2c\mathrm{tr}(\mathbf{DS})\\ & ={\sigma }_y^2+{\left(\frac{\mathrm{tr}(\mathbf{DS})}{{\sigma }_x^2}\right)}^2{\sigma }_x^2-2\left(\frac{\mathrm{tr}(\mathbf{DS})}{{\sigma }_x^2}\right)\mathrm{tr}(\mathbf{DS})\\ & ={\sigma }_y^2-\frac{\mathrm{tr}(\mathbf{DS}{)}^2}{{\sigma }_x^2}\end{array}\text{\hspace{1em}(II-6-1)}$$
这样就完全证明了 [定理] 部分.

---

总结

本篇博文详细推导了 S. Umeyama 论文 “Least-squares estimation of transformation parameters between two point patterns” 中的定理部分^[1], 获得了相似变换参数 (旋转、平移、缩放) 的最小方差估计.

这就为实际应用中的源码实现提供了坚实的数学基础, 相对来说源码实现并不复杂.

后面我们会简单看一下 Umeyama 算法的源码, 包括 Eigen 中的 C++实现和 evo 中的 Python 实现.

(如有问题, 请指出)

---

参考文献

[1] S. Umeyama, “Least-squares estimation of transformation parameters between two point patterns,” in IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 13, no. 4, pp. 376-380, April 1991, doi: 10.1109/34.88573.