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

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

---

相关博文介绍
- 矩阵乘法可交换与可同时对角化的关系 —— 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$) 找出最小误差平方意义下的相似变换参数 (包括旋转 $\mathbf{R}$、平移 $\mathbf{t}$、缩放 $c$).

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

---

I. 引理 —— 旋转参数的最小方差估计

在完整解决相似变换的参数估计前, 原论文先提出了如下引理用于解决一个较小的问题 —— 旋转变换的参数估计.

$\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)}$$
其中 $\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-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-3)}$$
当 $\mathrm{rank}(\mathbf{A}{\mathbf{B}}^{\mathrm{T}})\ge m-1$ 时, 达到式 (I-2) 目标函数最小化所需的最优的旋转矩阵 $\mathbf{R}$ 被唯一确定为
$$\mathbf{R}=\mathbf{US}{\mathbf{V}}^{\mathrm{T}}\text{\hspace{1em}(I-4)}$$
其中当 $\det (\mathbf{A}{\mathbf{B}}^{\mathrm{T}})=0$ 时 (也就是说, 当 $\mathrm{rank}(\mathbf{A}{\mathbf{B}}^{\mathrm{T}})=m-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-5)}$$

本篇博客内容就是详细推导一下以上引理.

---

II. Lagrange 乘子法求最优参数估计

我们已由 “旋转矩阵约束下的朗格朗日乘子 —— Umeyama 算法推导的数学准备 (II)” 一文中构建了 “旋转参数的最小方差估计” 的拉格朗日函数

$$L=\Vert \mathbf{A}-\mathbf{RB}{\Vert }_F^2+\mathrm{tr}\left(\mathbf{\lambda }\left({\mathbf{R}}^{\mathrm{T}}\mathbf{R}-\mathbf{I}\right)\right)+g\left[\det (\mathbf{R})-1\right]\text{\hspace{1em}(II-1)}$$
其中 $\mathbf{R}$ 是 $m\times m$ 的旋转矩阵. $\mathbf{\lambda }$ 是 $m\times m$ 的对称矩阵, 作为拉格朗日乘子矩阵. $g$ 是标量拉格朗日乘子.

需要说明:

我们虽然知道这里的 $\mathbf{R}$ 是旋转矩阵, 但在做具体运算时只能当做普通矩阵, 而旋转矩阵的约束或特性已经通过拉格朗日函数中的两个拉格朗日乘子项体现了.

根据拉格朗日乘子法标准步骤进行计算.

---

1. 极值驻点

A. 拉格朗日函数第一项对 $\mathbf{R}$ 的偏导数

根据矩阵的 Frobenius-范数的定义以及 [Identity 1] Matrix Trace 可知
$$\begin{array}{rl}\Vert \mathbf{A}-\mathbf{RB}{\Vert }_F^2& =\mathrm{tr}\left[(\mathbf{A}-\mathbf{RB}{)}^{\mathrm{T}}(\mathbf{A}-\mathbf{RB})\right]\\ & =\mathrm{tr}\left[(\mathbf{A}-\mathbf{RB})(\mathbf{A}-\mathbf{RB}{)}^{\mathrm{T}}\right]\\ & =\mathrm{tr}(\mathbf{A}{\mathbf{A}}^{\mathrm{T}}-\mathbf{RB}{\mathbf{A}}^{\mathrm{T}}-\mathbf{A}{\mathbf{B}}^{\mathrm{T}}{\mathbf{R}}^{\mathrm{T}}+\mathbf{RB}{\mathbf{B}}^{\mathrm{T}}{\mathbf{R}}^{\mathrm{T}})\end{array}\text{\hspace{1em}(II-1-A-1)}$$
根据 Partial Derivative of a Matrix Trace 对式 (II-1-1) 求偏导数
$$\frac{\partial \Vert \mathbf{A}-\mathbf{RB}{\Vert }_F^2}{\partial \mathbf{R}}=-2\mathbf{A}{\mathbf{B}}^{\mathrm{T}}+2\mathbf{RB}{\mathbf{B}}^{\mathrm{T}}\text{\hspace{1em}(II-1-A-2)}$$

---

B. 拉格朗日函数第二项对 $\mathbf{R}$ 的偏导数

同样利用 [Identity 1] Matrix Trace 和 Partial Derivative of a Matrix Trace 计算

$$\begin{array}{rl}\frac{\partial \mathrm{tr}\left(\mathbf{\lambda }\left({\mathbf{R}}^{\mathrm{T}}\mathbf{R}-\mathbf{I}\right)\right)}{\partial \mathbf{R}}& =\frac{\partial \mathrm{tr}\left(\mathbf{\lambda }{\mathbf{R}}^{\mathrm{T}}\mathbf{R}\right)}{\partial \mathbf{R}}\\ & =\frac{\partial \mathrm{tr}\left(\mathbf{R}\mathbf{\lambda }{\mathbf{R}}^{\mathrm{T}}\right)}{\partial \mathbf{R}}\\ & =2\mathbf{R}\mathbf{\lambda }\end{array}\text{\hspace{1em}(II-1-B-1)}$$

C. 拉格朗日函数第三项对 $\mathbf{R}$ 的偏导数

利用 [Identity 13-a] Derivative of a Matrix’s Determinant 计算
$$\frac{\partial g\left[\det (\mathbf{R})-1\right]}{\partial \mathbf{R}}=g{\left(\det (\mathbf{R}){\mathbf{R}}^{-1}\right)}^{\mathrm{T}}\text{\hspace{1em}(II-1-C-1)}$$

---

D. 拉格朗日函数对 $\mathbf{\lambda }$ 的偏导数

利用 [Identity 4] Partial Derivative of a Matrix Trace of the First Order (1) 计算
$$\frac{\partial L}{\partial \mathbf{\lambda }}={\mathbf{R}}^{\mathrm{T}}\mathbf{R}-\mathbf{I}=0\text{\hspace{1em}(II-1-D-1)}$$
求驻点可以得到
$${\mathbf{R}}^{-1}={\mathbf{R}}^{\mathrm{T}}\text{\hspace{1em}(II-1-D-2)}$$

---

E. 拉格朗日函数对 $g$ 的偏导数

$$\frac{\partial L}{\partial g}=\det (\mathbf{R})-1=0\text{\hspace{1em}(II-1-E-1)}$$

求驻点可以得到
$$\det (\mathbf{R})=1\text{\hspace{1em}(II-1-E-2)}$$

---

F. 拉格朗日函数对 $\mathbf{R}$ 的偏导数

结合式 (II-1-A-2)、式 (II-1-B-1)和式 (II-1-C-1) 得到
$$\frac{\partial L}{\partial \mathbf{R}}=-2\mathbf{A}{\mathbf{B}}^{\mathrm{T}}+2\mathbf{RB}{\mathbf{B}}^{\mathrm{T}}+2\mathbf{R}\mathbf{\lambda }+g{\left(\det (\mathbf{R}){\mathbf{R}}^{-1}\right)}^{\mathrm{T}}=0\text{\hspace{1em}(II-1-F-1)}$$
求驻点后, 获得的式 (II-1-D-2) 和式 (II-1-E-2) 体现出 $\mathbf{R}$ 作为旋转矩阵的特性, 至此以后才可以放心地使用这些旋转性质.

那么继续简化式 (II-1-F-1) 为
$$\frac{\partial L}{\partial \mathbf{R}}=-2\mathbf{A}{\mathbf{B}}^{\mathrm{T}}+2\mathbf{RB}{\mathbf{B}}^{\mathrm{T}}+2\mathbf{R}\mathbf{\lambda }+g\mathbf{R}=0\text{\hspace{1em}(II-1-F-2)}$$

---

2. 目标函数的最小值

A. 共同的特征向量

定义对称矩阵
$${\mathbf{L}}^{{}^{\prime }}\triangleq \mathbf{B}{\mathbf{B}}^{\mathrm{T}}+\mathbf{\lambda }+\frac{g}{2}\mathbf{I}\text{\hspace{1em}(II-2-A-1)}$$
根据矩阵乘法结合律
$${\mathbf{L}}^{{}^{\prime }}{\mathbf{L}}^{{}^{\prime }}{\mathbf{L}}^{{}^{\prime }}={\mathbf{L}}^{{}^{\prime }}{{\mathbf{L}}^{{}^{\prime }}}^2={{\mathbf{L}}^{{}^{\prime }}}^2{\mathbf{L}}^{{}^{\prime }}\text{\hspace{1em}(II-2-A-2)}$$
就是说 ${\mathbf{L}}^{{}^{\prime }}$ 和 ${{\mathbf{L}}^{{}^{\prime }}}^2$ 是乘法可交换的. 那么由 矩阵乘法可交换与可同时对角化的关系 —— Umeyama 算法推导的数学准备 (I) 可知, 存在正交矩阵 $\bar{\mathbf{V}}$ 由同时是 ${\mathbf{L}}^{{}^{\prime }}$ 和 ${{\mathbf{L}}^{{}^{\prime }}}^2$ 的特征向量构成, 称其为 ${\mathbf{L}}^{{}^{\prime }}$ 和 ${{\mathbf{L}}^{{}^{\prime }}}^2$ 共同的特征向量矩阵.

因为 $\mathbf{\lambda }$ 是对称的, 所以 ${\mathbf{L}}^{{}^{\prime }}$ 也是对称的. 式 (II-1-F-2) 简写为
$$\begin{gathered} \mathbf{R}{\mathbf{L}}^{{}^{\prime }}=\mathbf{A}{\mathbf{B}}^{\mathrm{T}} \\ \text{\hspace{1em}(II-2-A-3)} \end{gathered}$$
或者两边转置
$${\mathbf{L}}^{{}^{\prime }}{\mathbf{R}}^{\mathrm{T}}=\mathbf{B}{\mathbf{A}}^{\mathrm{T}}\text{\hspace{1em}(II-2-A-4)}$$
上面两式左右两边相乘,
$${\mathbf{L}}^{{}^{\prime }}{\mathbf{L}}^{{}^{\prime }}={\mathbf{L}}^{{}^{\prime }}{\mathbf{R}}^{\mathrm{T}}\mathbf{R}{\mathbf{L}}^{{}^{\prime }}=\mathbf{B}{\mathbf{A}}^{\mathrm{T}}\mathbf{A}{\mathbf{B}}^{\mathrm{T}}\text{\hspace{1em}(II-2-A-5)}$$
并利用奇异值分解式 (I-1) 得到
$$\begin{array}{rl}{{\mathbf{L}}^{{}^{\prime }}}^2& =\mathbf{B}{\mathbf{A}}^{\mathrm{T}}\mathbf{A}{\mathbf{B}}^{\mathrm{T}}\\ & =(\mathbf{UD}{\mathbf{V}}^{\mathrm{T}}{)}^{\mathrm{T}}\mathbf{UD}{\mathbf{V}}^{\mathrm{T}}\\ & =\mathbf{V}{\mathbf{D}}^2{\mathbf{V}}^{\mathrm{T}}\end{array}\text{\hspace{1em}(II-2-A-6)}$$
可以看出 $\mathbf{V}$ 也是 ${{\mathbf{L}}^{{}^{\prime }}}^2$ 特征向量矩阵.

根据 奇异值分解之常用结论 可知, 奇异值分解中正交矩阵 $\mathbf{V}$ 的选取不唯一, 此处只要满足 “$\mathbf{V}$ 是 $\mathbf{B}{\mathbf{A}}^{\mathrm{T}}\mathbf{A}{\mathbf{B}}^{\mathrm{T}}$ (即 ${{\mathbf{L}}^{{}^{\prime }}}^2$) 的特征向量矩阵” 这一条件就能满足奇异值分解的要求. 故此处奇异值分解中的正交矩阵 $\mathbf{V}$ 选为 ${\mathbf{L}}^{{}^{\prime }}$ 和 ${{\mathbf{L}}^{{}^{\prime }}}^2$ 共同的特征向量矩阵 $\bar{\mathbf{V}}$, 两者一致
$$\bar{\mathbf{V}}\triangleq \mathbf{V}\text{\hspace{1em}(II-2-A-7)}$$

---

B. 特征值的关系

由式 (II-2-A-6) 可知,
$${\mathbf{D}}^2=\left[\begin{array}{cccc}{d}_1^2& & & \\ & d_2^2& & \\ & & \ddots & \\ & & & d_m^2\end{array}\right]\text{\hspace{1em}(II-2-B-1)}$$
是 ${{\mathbf{L}}^{{}^{\prime }}}^2$ 特征值按照降序排列构成的矩阵.

由 矩阵乘操作、三角化、开方特征值 —— Umeyama 算法推导的数学准备 (III) 中 “矩阵开平放的特征值” 可知, ${\mathbf{L}}^{{}^{\prime }}$ 的各个特征值是 ${{\mathbf{L}}^{{}^{\prime }}}^2$ 中对应特征值的开根 $d_i$ 或者负的开根 $-d_i$. 那么 ${\mathbf{L}}^{{}^{\prime }}$ 特征值构成的矩阵可以写成 $\mathbf{DS}$, 其中
$$\mathbf{S}\triangleq \mathrm{diag}(s_i),s_i=1\text{or}-1\text{\hspace{1em}(II-2-B-2)}$$
也就是说
$${\mathbf{L}}^{{}^{\prime }}=\mathbf{VDS}{\mathbf{V}}^{\mathrm{T}}\text{\hspace{1em}(II-2-B-3)}$$
那么其行列式
$$\det ({\mathbf{L}}^{{}^{\prime }})=\det (\mathbf{VDS}{\mathbf{V}}^{\mathrm{T}})=\det (\mathbf{D})\det (\mathbf{S})\text{\hspace{1em}(II-2-B-4)}$$
另外, 由式 (II-2-A-4) 并利用 ${\mathbf{L}}^{{}^{\prime }}$ 的对称性可知
$${\mathbf{L}}^{{}^{\prime }}=\mathbf{B}{\mathbf{A}}^{\mathrm{T}}\mathbf{R}={\mathbf{R}}^{\mathrm{T}}\mathbf{A}{\mathbf{B}}^{\mathrm{T}}\text{\hspace{1em}(II-2-B-5)}$$
计算其行列式
$$\det ({\mathbf{L}}^{{}^{\prime }})=\det ({\mathbf{R}}^{\mathrm{T}}\mathbf{A}{\mathbf{B}}^{\mathrm{T}})=\det (\mathbf{A}{\mathbf{B}}^{\mathrm{T}})\text{\hspace{1em}(II-2-B-6)}$$
结合式 (II-2-B-4) 和式 (II-2-B-6)
$$\det (\mathbf{D})\det (\mathbf{S})=\det (\mathbf{A}{\mathbf{B}}^{\mathrm{T}})\text{\hspace{1em}(II-2-B-7)}$$
因为奇异值的非负性, 可知 $\det (\mathbf{S})$ 需满足
$$\{\begin{array}{ll}\det (\mathbf{S})=1,& \text{if}\det (\mathbf{A}{\mathbf{B}}^{\mathrm{T}})>0\\ \det (\mathbf{S})=-1,& \text{if}\det (\mathbf{A}{\mathbf{B}}^{\mathrm{T}})<0\end{array}\text{\hspace{1em}(II-2-B-8)}$$
我们已经获得了式 (II-2-B-2) 和式 (II-2-B-8) 作为 $\mathbf{S}$ 取值的约束. 这样 ${\mathbf{L}}^{{}^{\prime }}$ 的特征值组成的矩阵 $\mathbf{DS}$ 的取值约束也确定了. 下面就根据目标函数最小化来明确 $\mathbf{S}$ 的具体值.

---

C. 目标函数最小值的确定

利用 [Identity 1] Matrix Trace 对式 (II-1-A-1) 进行计算
$$\begin{array}{rl}\Vert \mathbf{A}-\mathbf{RB}{\Vert }_F^2& =\mathrm{tr}(\mathbf{A}{\mathbf{A}}^{\mathrm{T}}-\mathbf{RB}{\mathbf{A}}^{\mathrm{T}}-\mathbf{A}{\mathbf{B}}^{\mathrm{T}}{\mathbf{R}}^{\mathrm{T}}+\mathbf{RB}{\mathbf{B}}^{\mathrm{T}}{\mathbf{R}}^{\mathrm{T}})\\ & =\mathrm{tr}(\mathbf{A}{\mathbf{A}}^{\mathrm{T}})-\mathrm{tr}(\mathbf{RB}{\mathbf{A}}^{\mathrm{T}})-\mathrm{tr}(\mathbf{A}{\mathbf{B}}^{\mathrm{T}}{\mathbf{R}}^{\mathrm{T}})+\mathrm{tr}(\mathbf{B}{\mathbf{B}}^{\mathrm{T}}{\mathbf{R}}^{\mathrm{T}}\mathbf{R})\\ & =\mathrm{tr}(\mathbf{A}{\mathbf{A}}^{\mathrm{T}})-2\mathrm{tr}(\mathbf{A}{\mathbf{B}}^{\mathrm{T}}{\mathbf{R}}^{\mathrm{T}})+\mathrm{tr}(\mathbf{B}{\mathbf{B}}^{\mathrm{T}})\\ & =\Vert \mathbf{A}{\Vert }_F^2+\Vert \mathbf{B}{\Vert }_F^2-2\mathrm{tr}({\mathbf{R}}^{\mathrm{T}}\mathbf{A}{\mathbf{B}}^{\mathrm{T}})\\ \text{(II-2-B-3),(II-2-B-5)}& =\Vert \mathbf{A}{\Vert }_F^2+\Vert \mathbf{B}{\Vert }_F^2-2\mathrm{tr}(\mathbf{VDS}{\mathbf{V}}^{\mathrm{T}})\\ & =\Vert \mathbf{A}{\Vert }_F^2+\Vert \mathbf{B}{\Vert }_F^2-2\mathrm{tr}(\mathbf{DS})\\ & =\Vert \mathbf{A}{\Vert }_F^2+\Vert \mathbf{B}{\Vert }_F^2-2(d_1{s}_1+d_2{s}_2+\dots +d_m{s}_m)\end{array}\text{\hspace{1em}(II-2-C-1)}$$
那么目标函数取极小值只与 $\mathbf{S}$ 有关, 根据奇异值的非负性以及降序排列特性, 并结合 $\mathbf{S}$ 取值约束式 (II-2-B-2) 和式 (II-2-B-8).

对于 $\mathbf{S}$ 中的元素, 尽量选取更多正元素, 如需取负元素, 对应的奇异值越小越好.

这样就得到了引理中的式 (I-3), 此时能取到最小目标函数值.
$$\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-3)}$$
那么取得最小目标函数值时的旋转矩阵 $\mathbf{R}$ 如何确定呢？

---

3. 旋转参数的最优估计

A. $\mathrm{rank}(\mathbf{A}{\mathbf{B}}^{\mathrm{T}})=m$ 情况

根据式 (II-2-A-3) 可知, $\mathrm{rank}(\mathbf{A}{\mathbf{B}}^{\mathrm{T}})=m$ 情况下 (即满秩情况下), 对称阵 ${\mathbf{L}}^{{}^{\prime }}$ 非奇异即可逆. 由式 (II-2-B-3) 并结合 $\mathbf{V}$ 的正交性、$\mathbf{S}$ 的对角 $\pm 1$ 元素, 可得
$${{\mathbf{L}}^{{}^{\prime }}}^{-1}=\mathbf{V}{\mathbf{D}}^{-1}\mathbf{S}{\mathbf{V}}^{\mathrm{T}}\text{\hspace{1em}(II-3-A-1)}$$

由式 (II-2-A-3) 可知
$$\begin{array}{rl}\mathbf{R}& =\mathbf{A}{\mathbf{B}}^{\mathrm{T}}{{\mathbf{L}}^{{}^{\prime }}}^{-1}\\ \text{(I-1), (II-3-A-1)}& =\mathbf{UD}{\mathbf{V}}^{\mathrm{T}}\mathbf{V}{\mathbf{D}}^{-1}\mathbf{S}{\mathbf{V}}^{\mathrm{T}}\\ & =\mathbf{US}{\mathbf{V}}^{\mathrm{T}}\end{array}\text{\hspace{1em}(II-3-A-2)}$$
上式中的 $\mathbf{S}$ 按照式 (I-3) 中所列取值.

---

B. $\mathrm{rank}(\mathbf{A}{\mathbf{B}}^{\mathrm{T}})=m-1$ 情况

当 $\mathrm{rank}(\mathbf{A}{\mathbf{B}}^{\mathrm{T}})=m-1$ 时,

- 由式 (II-2-A-3) 可以知道此时 ${\mathbf{L}}^{{}^{\prime }}$ 也不满秩 (即不可逆).

- 由奇异值分解性质可知, $d_m=0$, 即
$$\mathbf{DS}=\left[\begin{array}{cccc}{d}_1& & & \\ & \ddots & & \\ & & d_{m-1}& \\ & & & 0\end{array}\right]\left[\begin{array}{cccc}1& & & \\ & \ddots & & \\ & & 1& \\ & & & \ast \end{array}\right]=\mathbf{D}\text{\hspace{1em}(II-3-B-1)}$$

式 (II-2-B-3) 和式 (II-3-B-1) 代入式 (II-2-A-3) 得
$$\begin{gathered} \mathbf{RVDS}{\mathbf{V}}^{\mathrm{T}}=\mathbf{A}{\mathbf{B}}^{\mathrm{T}}=\mathbf{UD}{\mathbf{V}}^{\mathrm{T}} \\ \Rightarrow \mathbf{RVD}=\mathbf{UD} \\ \Rightarrow {\mathbf{U}}^{\mathrm{T}}\mathbf{RVD}=\mathbf{D} \\ \text{\hspace{1em}(II-3-B-2)} \end{gathered}$$
那么定义
$$\mathbf{Q}\triangleq {\mathbf{U}}^{\mathrm{T}}\mathbf{RV}\text{\hspace{1em}(II-3-B-3)}$$
则有
$$\begin{gathered} \mathbf{QD}=\mathbf{D} \\ \text{\hspace{1em}(II-3-B-4)} \end{gathered}$$
其中 $\mathbf{Q}$ 写成列向量形式为 $\left[\begin{array}{cccc}{\mathbf{q}}_1& {\mathbf{q}}_2& \dots & {\mathbf{q}}_m\end{array}\right]$. 由 矩阵乘的列操作 可知
$$\begin{array}{rl}\mathbf{QD}& =\left[\begin{array}{cccc}{\mathbf{q}}_1& \dots & {\mathbf{q}}_{m-1}& {\mathbf{q}}_m\end{array}\right]\left[\begin{array}{cccc}{d}_1& & & \\ & \ddots & & \\ & & d_{m-1}& \\ & & & 0\end{array}\right]\\ & =\left[\begin{array}{cccc}{d}_1{\mathbf{q}}_1& \dots & d_{m-1}{\mathbf{q}}_{m-1}& 0\end{array}\right]\end{array}\text{\hspace{1em}(II-3-B-5)}$$

$$\begin{array}{rl}\mathbf{D}& =\left[\begin{array}{cccc}{d}_1& & & \\ & \ddots & & \\ & & d_{m-1}& \\ & & & 0\end{array}\right]\\ & =\left[\begin{array}{cccc}{d}_1{\mathbf{e}}_1& \dots & d_{m-1}{\mathbf{e}}_{m-1}& 0\end{array}\right]\end{array}\text{\hspace{1em}(II-3-B-6)}$$

其中 ${\mathbf{e}}_i=[0,\dots ,\stackrel{i}{1},\dots ,0{]}^{\mathrm{T}}$ 为 ${\mathbb{R}}^m$ 空间中的标准正交基.

根据式 (II-3-B-4) 可知
$$d_i{\mathbf{q}}_i=d_i{\mathbf{e}}_i(\text{for}1\le i\le m-1)\text{\hspace{1em}(II-3-B-7)}$$
故有
$${\mathbf{q}}_i={\mathbf{e}}_i(\text{for}1\le i\le m-1)\text{\hspace{1em}(II-3-B-8)}$$
下面确定$\mathbf{Q}$ 的最后一列 ${\mathbf{q}}_m$.

由定义式 (II-3-B-3) 可知
$$\mathbf{Q}{\mathbf{Q}}^{\mathrm{T}}={\mathbf{U}}^{\mathrm{T}}\mathbf{RV}{\mathbf{V}}^{\mathrm{T}}{\mathbf{R}}^{\mathrm{T}}\mathbf{U}=\mathbf{I}\text{\hspace{1em}(II-3-B-9)}$$
矩阵 $\mathbf{Q}$ 是正交矩阵 (含旋转或镜像). 因为矩阵的前 $m-1$ 列构成了标准正交基, 则最后一列为
$${\mathbf{q}}_m={\mathbf{e}}_m\text{or}{\mathbf{q}}_m=-{\mathbf{e}}_m\text{\hspace{1em}(II-3-B-10)}$$
当 ${\mathbf{q}}_m={\mathbf{e}}_m$ 时, $\det (\mathbf{Q})=1$; 当 ${\mathbf{q}}_m=-{\mathbf{e}}_m$ 时, $\det (\mathbf{Q})=-1$.

对 $\mathbf{Q}$ 的定义式两边求行列式可以得到
$$\begin{array}{rl}\det (\mathbf{Q})& =\det (\mathbf{U})\det (\mathbf{R})\det (\mathbf{V})\\ & =\det (\mathbf{U})\det (\mathbf{V})\end{array}\text{\hspace{1em}(II-3-B-11)}$$
这样就决定了 $\mathbf{Q}$ 的取值
$$\mathbf{Q}=\{\begin{array}{lc}\left[\begin{array}{ccccc}1& & & & \\ & 1& & & \\ & & \ddots & & \\ & & & 1& \\ & & & & 1\end{array}\right]& \text{if}\det (\mathbf{U})\det (\mathbf{V})=1\\ \left[\begin{array}{ccccc}1& & & & \\ & 1& & & \\ & & \ddots & & \\ & & & 1& \\ & & & & -1\end{array}\right]& \text{if}\det (\mathbf{U})\det (\mathbf{V})=-1\end{array}\text{\hspace{1em}(II-3-B-12)}$$
再由 $\mathbf{Q}$ 的定义式 (II-3-B-3) 可知
$$\mathbf{R}=\mathbf{UQ}{\mathbf{V}}^{\mathrm{T}}\text{\hspace{1em}(II-3-B-13)}$$
此即为 $\mathrm{rank}(\mathbf{A}{\mathbf{B}}^{\mathrm{T}})=m-1$ 时的旋转参数估计矩阵. 写成统一形式:

当 $\mathrm{rank}(\mathbf{A}{\mathbf{B}}^{\mathrm{T}})=m-1$ 时
$$\mathbf{R}=\mathbf{US}{\mathbf{V}}^{\mathrm{T}}\text{\hspace{1em}(II-3-B-14)}$$
其中
$$\mathbf{S}=\{\begin{array}{lc}\mathbf{I}& \text{if}\det (\mathbf{U})\det (\mathbf{V})=1\\ \mathrm{diag}(1,1,\dots ,1,-1)& \text{if}\det (\mathbf{U})\det (\mathbf{V})=-1\end{array}\text{\hspace{1em}(II-3-B-15)}$$
即引理中的式 (I-5).

---

C. $\mathrm{rank}(\mathbf{A}{\mathbf{B}}^{\mathrm{T}})<m-1$ 情况

我们可以从 $\mathrm{rank}(\mathbf{A}{\mathbf{B}}^{\mathrm{T}})=m-1$ 情况下的参数估计推导过程可以知道, $\mathbf{Q}$ 的最后一个列 ${\mathbf{q}}_m$ 是通过与前面 $m-1$ 个标准正交基的正交性来确定的.

当 $\mathrm{rank}(\mathbf{A}{\mathbf{B}}^{\mathrm{T}})<m-1$ 时, 将由两列或以上向量待确定, 将有无穷多组正交基 (任意旋转或镜像) 可作为估计值.

故 $\mathrm{rank}(\mathbf{A}{\mathbf{B}}^{\mathrm{T}})<m-1$ 时, 无法确定 $\mathbf{Q}$ (即 $\mathbf{S}$), 即无穷多解.

至此, 旋转参数的最优估计的推导过程完毕.

---

总结

本篇博文详细推导了 S. Umeyama 论文 “Least-squares estimation of transformation parameters between two point patterns” 中的引理部分^[1].

(如有问题, 请指出)

---

参考文献

[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.