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

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

---

I. 背景

Umeyama 算法^[1]要解决的问题

给定两个 $m$ 维空间内的点集 {${\mathbf{x}}_i$} 和 {${\mathbf{y}}_i$} (其中 $i=1,2,\dots ,n$) 找出最小误差平方意义下的相似变换参数 (包括旋转 $\mathbf{R}$、平移 $\mathbf{t}$、缩放 $c$).

原论文在一开始提出了一个引理. 在这个引理中,

$\mathbf{A}$ 和 $\mathbf{B}$ 都是 $m\times n$ 的矩阵. 而 $\mathbf{R}$ 是 $m\times m$ 的旋转矩阵. 要求使得 $\Vert \mathbf{A}-\mathbf{RB}{\Vert }^2$ 取最小值时的旋转矩阵 $\mathbf{R}$.

翻译一下就是
$$\begin{gathered} \underset{\mathbf{R}}{\min }\Vert \mathbf{A}-\mathbf{RB}{\Vert }^2 \\ \begin{array}{lc}s.t.& {\mathbf{R}}^{\mathrm{T}}\mathbf{R}=\mathbf{I}\\ & \det (\mathbf{R})=1\end{array}\text{\hspace{1em}(I-1)} \end{gathered}$$
在本篇博客中, 我们就对这种旋转矩阵约束的优化问题构建拉格朗日函数 (Lagrangian Function) 和拉格朗日乘子 (Lagrange multipliers).

---

II. 推导

1. 拉格朗日乘子法

拉格朗日乘子法求极值^[2]

求函数 $f(x,y)$ 在条件 $g(x,y)＝0$ 下的极值, 先构造拉格朗日函数 (Lagrangian Function)
$$L(x,y,\lambda )=f(x,y)+\lambda g(x,y)$$
其中 $\lambda$ 称为拉格朗日乘子 (Lagrange Multiplier). 则函数 $L(x,y,\lambda )$ 的取极值的必要条件是
$$\frac{\partial L}{\partial x}=0,\frac{\partial L}{\partial y}=0,\frac{\partial L}{\partial \lambda }=0$$
如果在两个约束条件 $g_1(x,y)＝0$ 和 $g_2(x,y)＝0$ 下, 为求函数 $f(x,y)$ 极值而构造的朗格朗日函数为
$$L(x,y,{\lambda }_1,{\lambda }_2)=f(x,y)+{\lambda }_1{g}_1(x,y)+{\lambda }_2{g}_2(x,y)$$
取得极值的必要条件则为
$$\frac{\partial L}{\partial x}=0,\frac{\partial L}{\partial y}=0,\frac{\partial L}{\partial {\lambda }_1}=0,\frac{\partial L}{\partial {\lambda }_2}=0$$
更多约束条件依次类推.

---

2. 旋转矩阵约束

我们先以三维旋转矩阵约束为例展开. 如式 (I-1) 中旋转矩阵约束可以拆分成两部分约束方程.

---

A. 正交约束

正交约束
$${\mathbf{R}}^{\mathrm{T}}\mathbf{R}-\mathbf{I}=0\text{\hspace{1em}(II-2-A-1)}$$
三维情况下
$$\mathbf{R}\triangleq \left[\begin{array}{ccc}{R}_{11}& R_{12}& R_{13}\\ R_{21}& R_{22}& R_{23}\\ R_{31}& R_{32}& R_{33}\end{array}\right]\text{\hspace{1em}(II-2-A-2)}$$
则正交约束方程
$${\left[\begin{array}{ccc}{R}_{11}& R_{12}& R_{13}\\ R_{21}& R_{22}& R_{23}\\ R_{31}& R_{32}& R_{33}\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{ccc}{R}_{11}& R_{12}& R_{13}\\ R_{21}& R_{22}& R_{23}\\ R_{31}& R_{32}& R_{33}\end{array}\right]-\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]=0\text{\hspace{1em}(II-2-A-3)}$$
计算得到
$$\left[\begin{array}{ccc}{\sum }_{i=1}^3{R}_{i1}{R}_{i1}-1& {\sum }_{i=1}^3{R}_{i1}{R}_{i2}& {\sum }_{i=1}^3{R}_{i1}{R}_{i3}\\ {\sum }_{i=1}^3{R}_{i2}{R}_{i1}& {\sum }_{i=1}^3{R}_{i2}{R}_{i2}-1& {\sum }_{i=1}^3{R}_{i2}{R}_{i3}\\ {\sum }_{i=1}^3{R}_{i3}{R}_{i1}& {\sum }_{i=1}^3{R}_{i3}{R}_{i2}& {\sum }_{i=1}^3{R}_{i3}{R}_{i3}-1\end{array}\right]=0\text{\hspace{1em}(II-2-A-4)}$$
如果不考虑上式的对称性 (重复约束), 三维空间中的正交约束对应 9 个标量约束方程;

如果考虑上式的对称性, 三维空间中的正交约束对应 6 个标量约束方程.

针对对称性约束的两种处理方法的等价性是后面讨论的重点.

---

B. 行列式约束

行列式约束本身就是 1 个标量约束方程
$$\det (\mathbf{R})-1=0\text{\hspace{1em}(II-2-B-1)}$$

无需再做进一步讨论.

---

C. 拉格朗日函数构造 (3 维)

不考虑对称情况下, 按照前面定义的 “拉格朗日乘子法求极值” 构造 3 维旋转矩阵约束情况下的拉格朗日函数
$$\begin{array}{rl}L=\Vert \mathbf{A}-\mathbf{RB}{\Vert }^2& +{\lambda }_{11}\left[\sum _{i=1}^3{R}_{i1}{R}_{i1}-1\right]+{\lambda }_{12}\left[\sum _{i=1}^3{R}_{i1}{R}_{i2}\right]+{\lambda }_{13}\left[\sum _{i=1}^3{R}_{i1}{R}_{i3}\right]\\ & +{\lambda }_{21}\left[\sum _{i=1}^3{R}_{i2}{R}_{i1}\right]+{\lambda }_{22}\left[\sum _{i=1}^3{R}_{i2}{R}_{i2}-1\right]+{\lambda }_{23}\left[\sum _{i=1}^3{R}_{i2}{R}_{i3}\right]\\ & +{\lambda }_{31}\left[\sum _{i=1}^3{R}_{i3}{R}_{i1}\right]+{\lambda }_{32}\left[\sum _{i=1}^3{R}_{i3}{R}_{i2}\right]+{\lambda }_{33}\left[\sum _{i=1}^3{R}_{i3}{R}_{i3}-1\right]\\ & +g\left[\det (\mathbf{R})-1\right]\end{array}\text{\hspace{1em}(II-2-C-1)}$$
其中 $({\lambda }_{11},{\lambda }_{12},{\lambda }_{13},{\lambda }_{21},{\lambda }_{22},{\lambda }_{23},{\lambda }_{31},{\lambda }_{32},{\lambda }_{33},g)$ 都是拉格朗日乘子, 并记
$$\mathbf{\lambda }\triangleq \left[\begin{array}{ccc}{\lambda }_{11}& {\lambda }_{12}& {\lambda }_{13}\\ {\lambda }_{21}& {\lambda }_{22}& {\lambda }_{23}\\ {\lambda }_{31}& {\lambda }_{32}& {\lambda }_{33}\end{array}\right]\text{\hspace{1em}(II-2-C-2)}$$

考虑对称情况下, 即利用对称性进行整理
$$\begin{array}{rl}L=\Vert \mathbf{A}-\mathbf{RB}{\Vert }^2& +{\lambda }_{11}\left[\sum _{i=1}^3{R}_{i1}{R}_{i1}-1\right]+{\lambda }_{12}^{{}^{\prime }}\left[\sum _{i=1}^3{R}_{i1}{R}_{i2}\right]+{\lambda }_{13}^{{}^{\prime }}\left[\sum _{i=1}^3{R}_{i1}{R}_{i3}\right]\\ & +{\lambda }_{22}\left[\sum _{i=1}^3{R}_{i2}{R}_{i2}-1\right]+{\lambda }_{23}^{{}^{\prime }}\left[\sum _{i=1}^3{R}_{i2}{R}_{i3}\right]\\ & +{\lambda }_{33}\left[\sum _{i=1}^3{R}_{i3}{R}_{i3}-1\right]\end{array}\text{\hspace{1em}(II-2-C-3)}$$
其中

$$\begin{gathered} {\lambda }_{12}^{{}^{\prime }}\triangleq {\lambda }_{12}+{\lambda }_{21} \\ {\lambda }_{13}^{{}^{\prime }}\triangleq {\lambda }_{12}+{\lambda }_{31} \\ {\lambda }_{23}^{{}^{\prime }}\triangleq {\lambda }_{23}+{\lambda }_{32} \\ \text{\hspace{1em}(II-2-C-4)} \end{gathered}$$

记
$${\mathbf{\lambda }}^{{}^{\prime }}\triangleq \left[\begin{array}{ccc}{\lambda }_{11}& {\lambda }_{12}^{{}^{\prime }}& {\lambda }_{13}^{{}^{\prime }}\\ & {\lambda }_{22}& {\lambda }_{23}^{{}^{\prime }}\\ & & {\lambda }_{33}\end{array}\right]=\left[\begin{array}{ccc}{\lambda }_{11}& {\lambda }_{12}+{\lambda }_{21}& {\lambda }_{12}+{\lambda }_{31}\\ & {\lambda }_{22}& {\lambda }_{23}+{\lambda }_{32}\\ & & {\lambda }_{33}\end{array}\right]\text{\hspace{1em}(II-2-C-5)}$$

根据 “拉格朗日乘子法求极值”, 从式 (II-2-C-1) 得到的极值条件是
$$\{\begin{array}{l}\frac{\partial L}{\partial \mathbf{R}}=0\\ \frac{\partial L}{\partial \mathbf{\lambda }}=\left[\begin{array}{ccc}{\sum }_{i=1}^3{R}_{i1}{R}_{i1}-1& {\sum }_{i=1}^3{R}_{i1}{R}_{i2}& {\sum }_{i=1}^3{R}_{i1}{R}_{i3}\\ {\sum }_{i=1}^3{R}_{i2}{R}_{i1}& {\sum }_{i=1}^3{R}_{i2}{R}_{i2}-1& {\sum }_{i=1}^3{R}_{i2}{R}_{i3}\\ {\sum }_{i=1}^3{R}_{i3}{R}_{i1}& {\sum }_{i=1}^3{R}_{i3}{R}_{i2}& {\sum }_{i=1}^3{R}_{i3}{R}_{i3}-1\end{array}\right]=0\\ \frac{\partial L}{\partial g}=\det (\mathbf{R})-1=0\end{array}\text{\hspace{1em}(II-2-C-6)}$$
根据 “拉格朗日乘子法求极值”, 从式 (II-2-C-3) 得到的极值条件是
$$\{\begin{array}{l}\frac{\partial L}{\partial \mathbf{R}}=0\\ \frac{\partial L}{\partial {\mathbf{\lambda }}^{{}^{\prime }}}=\left[\begin{array}{ccc}{\sum }_{i=1}^3{R}_{i1}{R}_{i1}-1& {\sum }_{i=1}^3{R}_{i1}{R}_{i2}& {\sum }_{i=1}^3{R}_{i1}{R}_{i3}\\ & {\sum }_{i=1}^3{R}_{i2}{R}_{i2}-1& {\sum }_{i=1}^3{R}_{i2}{R}_{i3}\\ & & {\sum }_{i=1}^3{R}_{i3}{R}_{i3}-1\end{array}\right]=0\\ \frac{\partial L}{\partial g}=\det (\mathbf{R})-1=0\end{array}\text{\hspace{1em}(II-2-C-7)}$$
因为对称性, 方程组 (II-2-C-6) 是欠定的, 无法求出 $\mathbf{\lambda }$ 中 9 个元素的所有解, 只能求出 $({\lambda }_{11},{\lambda }_{12}+{\lambda }_{21},{\lambda }_{12}+{\lambda }_{31},{\lambda }_{22},{\lambda }_{23}+{\lambda }_{32},{\lambda }_{33})$ 这 6 个元素. 而这恰恰是可以从式 (II-2-C-7) 中直接求得的.

这样可知, 不管在设定对应于旋转矩阵约束的拉格朗日乘子时是否考虑对称性, 得到的结果都是一样的. 即式 (II-2-C-6) 和式 (II-2-C-7) 等价.

因为只能获得 $({\lambda }_{12}+{\lambda }_{21},{\lambda }_{12}+{\lambda }_{31},{\lambda }_{23}+{\lambda }_{32})$, 至于 ${\lambda }_{12}$ 和 ${\lambda }_{21}$ 之间如何再分配毫不影响结果. 故为了方便设定拉格朗日乘子相关矩阵也为对称矩阵.
$$\mathbf{\lambda }\triangleq \left[\begin{array}{ccc}{\lambda }_{11}& {\lambda }_{12}& {\lambda }_{13}\\ {\lambda }_{12}& {\lambda }_{22}& {\lambda }_{23}\\ {\lambda }_{13}& {\lambda }_{23}& {\lambda }_{33}\end{array}\right]\text{\hspace{1em}(II-2-C-8)}$$

注:

拉格朗日乘子相关矩阵设定为式 (II-2-C-5) 不是更方便吗？

并非如此. 因为我们要把这个方法写成矩阵形式, 对称矩阵可以写得很简洁, 也便于利用对称矩阵的性质.

所以三维旋转矩阵约束情况下的拉格朗日函数写成式 (II-2-C-1) 形式, 拉格朗日乘子写成式 (II-2-C-8) 形式.

---

D. 拉格朗日函数构造 (3 维) 的矩阵形式

矩阵的标准内积定义

The standard inner product between matrices^[3] is
$$<\mathbf{X},\mathbf{Y}>=\mathrm{tr}({\mathbf{X}}^{\mathrm{T}}\mathbf{Y})=\sum _i\sum _j{X}_{ij}{Y}_{ij}$$
where $\mathbf{X},\mathbf{Y}\in {\mathbb{R}}^{m\times n}$

式 (II-2-C-1) 中部分项
$$\begin{array}{rl}& \begin{array}{rl}& {\lambda }_{11}\left[\sum _{i=1}^3{R}_{i1}{R}_{i1}-1\right]+{\lambda }_{12}\left[\sum _{i=1}^3{R}_{i1}{R}_{i2}\right]+{\lambda }_{13}\left[\sum _{i=1}^3{R}_{i1}{R}_{i3}\right]\\ +& {\lambda }_{21}\left[\sum _{i=1}^3{R}_{i2}{R}_{i1}\right]+{\lambda }_{22}\left[\sum _{i=1}^3{R}_{i2}{R}_{i2}-1\right]+{\lambda }_{23}\left[\sum _{i=1}^3{R}_{i2}{R}_{i3}\right]\\ +& {\lambda }_{31}\left[\sum _{i=1}^3{R}_{i3}{R}_{i1}\right]+{\lambda }_{32}\left[\sum _{i=1}^3{R}_{i3}{R}_{i2}\right]+{\lambda }_{33}\left[\sum _{i=1}^3{R}_{i3}{R}_{i3}-1\right]\end{array}\\ =& <\mathbf{\lambda },{\mathbf{R}}^{\mathrm{T}}\mathbf{R}-\mathbf{I}>\\ =& \mathrm{tr}\left[\mathbf{\lambda }\left({\mathbf{R}}^{\mathrm{T}}\mathbf{R}-\mathbf{I}\right)\right]\end{array}\text{\hspace{1em}(II-2-D-1)}$$
其中 $\mathbf{\lambda }$ 是对称矩阵. 这样式 (II-2-C-1) 就可以简写为矩阵形式
$$L=\Vert \mathbf{A}-\mathbf{RB}{\Vert }^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-2-D-2)}$$

---

E. 拉格朗日函数构造 ($m$ 维)

对照三维情况的推导, 我们可以很方便地得到同样形式的 $m$ 维情况的拉格朗日函数构造
$$L=\Vert \mathbf{A}-\mathbf{RB}{\Vert }^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-2-E-1)}$$
其中 $\mathbf{R}$ 是 $m\times m$ 的旋转矩阵. $\mathbf{\lambda }$ 是 $m\times m$ 的对称矩阵, 作为拉格朗日乘子矩阵. $g$ 是标量拉格朗日乘子.

式 (II-2-E-1) 就是本篇博客要求的内容了, 具有旋转矩阵约束的拉格朗日函数及其乘子的构造.

---

2. 注记

从上面的推导我们可以知道:

具有对称性的矩阵约束, 其对应的拉格朗日乘子矩阵也是对称的.

---

参考文献

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

[2] 李大华等, 工科数学分析 (下), 华中科技大学出版社, 第二版, 2004

[3] A.A. Ahmadi, Inner products and norms, ORF 523 Lecture 2 Spring 2016, Princeton University, https://www.princeton.edu/~aaa/Public/Teaching/ORF523/S16/ORF523_S16_Lec2_gh.pdf