求旋转矩阵平均
1 背景
在工作中，可能会遇到对多个旋转矩阵求平均的问题。这时，如何求平均，就变成了本文需要解决的问题。
2 理论
现在假设，有\(n\)个旋转矩阵\(R_1,R_2,…,R_n\)，已知它们每个矩阵对应的权重为\(w_1,w_2,…,w_n\)，现在想计算一个旋转矩阵\(R\)来表示它们的平均。
现在问题变成了，需要找到一个\(R\)，使得它尽量靠近每一个\(R_i\)。一种简单粗暴的思想是求解如下问题
(1)
$$
\mathbf{min}:e=\sum_{i=1}^nw_i||R-R_i||^2
$$
由于矩阵的欧式距离不方便公式推导，因此这里构建如下的最小二乘问题来达到求解问题的目的
(2)
$$
\mathbf{min}:e=\sum_{i=1}^nw_i(\mathbf{tr}(R_iR^T)-3)^2
$$
注意，公式(2)与(1)不完全等价，但是公式(2)依然有使得结果为平均的思想在里面。
为了便于后续公式推导，这里先设置如下中间变量
(3)
$$
\begin{cases}
R^T&=\begin{bmatrix}\mathbf{r}_1 &\mathbf{r}_2 & \mathbf{r}_3\end{bmatrix}
\\
\mathbf{p}_1 &=\begin{bmatrix}1&0&0\end{bmatrix}^T
\\
\mathbf{p}_2 &=\begin{bmatrix}0&1&0\end{bmatrix}^T
\\
\mathbf{p}_3 &=\begin{bmatrix}0&0&1\end{bmatrix}^T
\\
\mathbf{x}_i^T &=\mathbf{p}_1^TR_i
\\
\mathbf{y}_i^T &=\mathbf{p}_2^TR_i
\\
\mathbf{z}_i^T &=\mathbf{p}_3^TR_i
\\
\mathbf{a}_1 &=-6\sum_{i=1}^nw_i\mathbf{x}_i
\\
\mathbf{a}_2 &=-6\sum_{i=1}^nw_i\mathbf{y}_i
\\
\mathbf{a}_3 &=-6\sum_{i=1}^nw_i\mathbf{z}_i
\\
\alpha&=\sum_{i=1}^nw_i(\mathbf{x}_i^T\mathbf{x}_i+\mathbf{y}_i^T\mathbf{y}_i+\mathbf{z}_i^T\mathbf{z}_i+9)
\\
B&=\sum_{i=1}^nw_iR_i^T
\\
A&=\begin{bmatrix}\mathbf{a}_1 &\mathbf{a}_2 & \mathbf{a}_3\end{bmatrix}=-6\sum_{i=1}^nw_iR_i^T=-6B
\end{cases}
$$
然后对\(B\)矩阵进行如下奇异值分解
(4)
$$
B=USV^T
$$
这里设置中间矩阵\(H\)
(5)
$$
H=V^TRU
$$
由于\(V、R、U\)均为正交矩阵，因此\(H\)也为正交矩阵。
接下来，对于(2)可以做如下展开
(6)
$$
\begin{aligned}
\mathbf{min}:e&=\sum_{i=1}^nw_i(\mathbf{tr}(R_iR^T)-3)^2
\\
&=\sum_{i=1}^nw_i(\mathbf{p}_1^TR_i\mathbf{r}_1+\mathbf{p}_2^TR_i\mathbf{r}_2+\mathbf{p}_3^TR_i\mathbf{r}_3-3)^2
\\
&=\sum_{i=1}^nw_i(\mathbf{x}_i^T\mathbf{r}_1+\mathbf{y}_i^T\mathbf{r}_2+\mathbf{z}_i^T\mathbf{r}_3-3)^2
\\
&=\sum_{i=1}^nw_i(\mathbf{x}_i^T\mathbf{r}_1\mathbf{r}_1^T\mathbf{x}_i+\mathbf{y}_i^T\mathbf{r}_2\mathbf{r}_2^T\mathbf{y}_i+\mathbf{z}_i^T\mathbf{r}_3\mathbf{r}_3^T\mathbf{z}_i+9
\\
&\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ +2\mathbf{x}_i^T\mathbf{r}_1\mathbf{r}_2^T\mathbf{y}_i+2\mathbf{x}_i^T\mathbf{r}_1\mathbf{r}_3^T\mathbf{z}_i+2\mathbf{y}_i^T\mathbf{r}_2\mathbf{r}_3^T\mathbf{z}_i-6\mathbf{x}_i^T\mathbf{r}_1-6\mathbf{y}_i^T\mathbf{r}_2-6\mathbf{z}_i^T\mathbf{r}_3)
\\
&=\sum_{i=1}^nw_i(\mathbf{x}_i^T\mathbf{x}_i+\mathbf{y}_i^T\mathbf{y}_i+\mathbf{z}_i^T\mathbf{z}_i+9-6\mathbf{x}_i^T\mathbf{r}_1-6\mathbf{y}_i^T\mathbf{r}_2-6\mathbf{z}_i^T\mathbf{r}_3)
\\
&=\alpha+\mathbf{a}_1^T\mathbf{r}_1+\mathbf{a}_2^T\mathbf{r}_2+\mathbf{a}_3^T\mathbf{r}_3
\\
&=\alpha+\mathbf{tr}(A^TR^T)
\\
&=\alpha+\mathbf{tr}(RA)
\\
&=\alpha-6\mathbf{tr}(RB)
\\
&=\alpha-6\mathbf{tr}(RUSV^T)
\\
&=\alpha-6\mathbf{tr}(V^TRUS)
\\
&=\alpha-6\mathbf{tr}(HS)
\\
&\Updownarrow
\\
\mathbf{max}:&e=\mathbf{tr}({HS)}
\end{aligned}
$$
这里设\(H,S\)对角线元素分别为\(h_1,h_2,h_3,s_1,s_2,s_3\)。因此(6)等价于求如下问题
(7)
$$
\mathbf{max}:e=h_1s_1+h_2s_2+h_3s_3
$$
由于\(H\)是正交矩阵,因此\(h_1,h_2,h_3\)的取值范围在[-1,1]。很明显，要使得(7)成立，\(h_1,h_2,h_3\)取-1或者1时才满足，因此这时的\(H\)矩阵必为对角矩阵，即
(8)
$$
H=\begin{bmatrix}
h_1 & 0 & 0
\\
0 & h_2&0
\\
0&0&h_3
\end{bmatrix}
$$
因此，结合(7)(8)可以得到
(9)
$$
h_i=\begin{cases}
1 & ,s_i \geq 0
\\
-1&,s_i<0
\end{cases}
$$
最终，可以得到
(10)
$$
R=VHU^T
$$
因此，最终求解步骤如下:
①根据公式(2)计算\(B\);
②对\(B\)进行奇异值分解得到\(U、S、V\);
③根据(8)(9)算式计算\(H\);
④根据(10)计算出\(R\)。