3次元相似変換パラメータ推定の原理

# 3次元相似変換パラメータ推定の原理

最小二乗法による 3 次元相似変換パラメータ推定の原理を説明します．

## 背景

- 自分の研究で使う．
- 剛体変換パラメータ推定等を調べてもやり方ばかりが出てきてしまい，肝心の理屈の説明が少ない．
- わかりやすい理屈を説明する文献もあるが証明が間違ってたり省略されてたり．．．

## 対象読者

- 点群位置合わせとかに興味ある人．
- 大学教養レベルの線形代数，微分積分を終えている人．
- 特異値分解の知識がある人．

愚直に証明しているので長く感じるかもしれませんがお付き合いください．

## 回転行列に関する最小化

3次元相似変換パラメータ推定に行く前に次の最小化問題を扱う．本記事ではこれが主題と言っても過言ではないかも．


```math
\begin{equation*}
  \hat{\boldsymbol{R}} = \mathop{\rm arg~min}_{\boldsymbol {R} \in \mathrm{SO}(3)}  \| \boldsymbol{M} - \boldsymbol{R} \| _{\mathrm{F}}^2
  \tag{1}
\end{equation*}
```
ただし $\boldsymbol{M, R}$ はそれぞれ 3x3 の実行列， 3 次回転行列である．

フロベニウスノルム $\| \cdot \|_\mathrm{F}$ とは，
```math
\begin{equation*}
  \| \boldsymbol{A} \| _\mathrm{F} = \sqrt{\sum _{i,j} a _{ij}}
  = \sqrt{\mathop{\rm tr}(A^\top A)} 
  % = \sqrt{\mathop{\rm tr}(A A^\top)}
  \tag{2}
\end{equation*}
```

フロベニウスノルムを使って式 (1) を変形すると（以降， $\mathop{\rm arg~min}$ 下の $\boldsymbol{R}$ を省略），
```math
\begin{align*}
  \mathop{\rm arg~min} \| \boldsymbol{M} - \boldsymbol{R} \|_\mathrm{F}^2 
  &= \mathop{\rm arg~min} \mathop{\rm tr} ((\boldsymbol{M} - \boldsymbol{R})^\top (\boldsymbol{M} - \boldsymbol{R})) 
  \\
  & = \mathop{\rm arg~min} \mathop{\rm tr} (\boldsymbol{M}^\top \boldsymbol{M} - \boldsymbol{M}^\top \boldsymbol{R} - \boldsymbol{R}^\top \boldsymbol{M} + \boldsymbol{R}^\top \boldsymbol{R}) 
  \\
  &= \mathop{\rm arg~max} \mathop{\rm tr} (\boldsymbol{M}^\top \boldsymbol{R} + \boldsymbol{R}^\top \boldsymbol{M} )
  \\
  % \text{（} \mathop{\rm tr}(\boldsymbol{A}+\boldsymbol{B})=\mathop{\rm tr}(\boldsymbol{A})+\mathop{\rm tr}(\boldsymbol{B}) ~\text{より）}~~~
  &= \mathop{\rm arg~max} ( \mathop{\rm tr} (\boldsymbol{M}^\top \boldsymbol{R}) + \mathop{\rm tr} (\boldsymbol{R}^\top \boldsymbol{M}) )
  \\
  % \text{（} \mathop{\rm tr}(\boldsymbol{A})=\mathop{\rm tr}(\boldsymbol{A}^\top) ~\text{より）}~~~
  &= \mathop{\rm arg~max} \mathop{\rm tr} (\boldsymbol{M}^\top \boldsymbol{R})
  \tag{3}
  % \label{MinimizeRTransformed1}
\end{align*}
```

### 特異値分解による変形

ここからさらに式変形を進めるために特異値分解を導入する．

$\boldsymbol{A}$を階数 $r$ の $m$ 行 $n$ 列の行列とするとき，次のような $\boldsymbol{A}$ の分解が存在する（特異値分解）．

```math
\begin{equation*}
  \boldsymbol{A} = \boldsymbol{U}\boldsymbol{\Sigma}\boldsymbol{V}^\top.
\end{equation*}
```

ただし，$\boldsymbol{U}$ は $m$ 行 $m$ 列の直交行列，$\boldsymbol{V}$ は $n$ 行 $n$ 列の直交行列，

```math
\begin{align*}
  \boldsymbol{\Sigma} = \left\{\begin{matrix}
    (\boldsymbol{\Delta} ~ \boldsymbol{O}) 
    & \mathrm{if}~ (m < n)
    \\
    \boldsymbol{\Delta} & 
    \mathrm{if}~ (m = n)
    \\
    \begin{pmatrix} \boldsymbol{\Delta} \\ \boldsymbol{O} \end{pmatrix} & 
    \mathrm{if}~ (m > n)
  \end{matrix}\right. 
  ,~~~ \boldsymbol{\Delta} = \mathop{\rm diag}(\sigma_1, \sigma_2, ... \sigma_q) 
  \\
  \sigma_1 \ge \sigma_2 \ge ... \ge \sigma_r > 0
  ,~~~ \sigma_{r+1} = \sigma_{r+2} = ... = \sigma_{q} = 0
  ,~~~ q = \min(m,n)
\end{align*}
```
である．

$\boldsymbol{M} = \boldsymbol{U}\boldsymbol{\Sigma}\boldsymbol{V}^\top$ と分解して式 (3) を更に変形する．
```math
\begin{align*}
  \mathop{\rm arg~max} \mathop{\rm tr} (\boldsymbol{M}^\top \boldsymbol{R}) 
  &= \mathop{\rm arg~max} \mathop{\rm tr} ((\boldsymbol{U}\boldsymbol{\Sigma}\boldsymbol{V}^\top)^\top \boldsymbol{R})
  \\
  &= \mathop{\rm arg~max} \mathop{\rm tr} (\boldsymbol{V}\boldsymbol{\Sigma}\boldsymbol{U}^\top \boldsymbol{R})
  \\
  % \text{（} \mathop{\rm tr}(\boldsymbol{AB})=\mathop{\rm tr}(\boldsymbol{BA}) ~\text{より）}~~~
  &= \mathop{\rm arg~max} \mathop{\rm tr}(\boldsymbol{\Sigma}\boldsymbol{U}^\top \boldsymbol{R} \boldsymbol{V})
  \\
  % \text{（} \boldsymbol{Z} = \boldsymbol{U}^\top \boldsymbol{R} \boldsymbol{V} ~\text{とすると）}~~~
  &= \mathop{\rm arg~max} \mathop{\rm tr} (\boldsymbol{\Sigma} \boldsymbol{Z}) 
  % \label{SigmaZ}
  \tag{4}
\end{align*}
```
ただし， $\boldsymbol{Z} = \boldsymbol{U}^\top \boldsymbol{R} \boldsymbol{V}$ とする．
$\boldsymbol{Z}$ は直交行列のため， $\det(\boldsymbol{Z})=1,-1$ のいずれかになる．

以降では，$\det \boldsymbol{Z}$ の値で場合分けして，式 (4) の最大化を考える．

### 補題 1

$\det \boldsymbol{Z} = 1$ の時， $\boldsymbol{Z} = \boldsymbol{I}$ （単位行列）で $\mathop{\rm tr} (\boldsymbol{\Sigma Z})$ は最大となる．
また， $\mathop{\rm rank} \boldsymbol{Z} \ge 2 \Leftrightarrow \hat{\boldsymbol{Z}}$は一意．

（証明）

式 (4) の中身を書き下すと，
```math
\mathop{\rm tr} (\boldsymbol{\Sigma Z}) = \sum_{k=1}^3 \sigma_k z_{kk}
```

$\det \boldsymbol{Z} = 1$ と直交行列の性質より， $\boldsymbol{Z} = \boldsymbol{I}$ で $\mathop{\rm tr} (\boldsymbol{\Sigma Z})$ は最大となる．

$\mathop{\rm rank} \boldsymbol{\Sigma} = 3$ の場合，明らかに $\hat{\boldsymbol{Z}}$ は一意．  
$\mathop{\rm rank} \boldsymbol{\Sigma} = 2$ の場合， $z_{11} = z_{22} = 1$ で最大となる． $\det \boldsymbol{Z} = 1$ より $z_{33}=1$ ．よって $\hat{\boldsymbol{Z}}$ は一意．  
$\mathop{\rm rank} \boldsymbol{\Sigma} = 1$ の場合， $z_{11} = 1$ で最大となる． $\hat{\boldsymbol{Z}}$ の右下 2x2 は任意の回転行列で良いため， $\hat{\boldsymbol{Z}}$ は一意ではない．  
$\mathop{\rm rank} \boldsymbol{\Sigma} = 0$ の場合， $\hat{\boldsymbol{Z}}$ は任意の回転行列で良いため， $\hat{\boldsymbol{Z}}$ は一意ではない．   

（証明終）

### 補題 2

$\det \boldsymbol{Z} = -1$ の時， $\boldsymbol{Z} = \mathop{\rm diag}(1,1,-1)$ （対角行列）で $\mathop{\rm tr} (\boldsymbol{\Sigma Z})$ は最大となる．
また， $(\mathop{\rm rank} \boldsymbol{Z} \ge 2 ~ \land \sigma_2 \ne \sigma_3 )\Leftrightarrow \hat{\boldsymbol{Z}}$は一意．

（証明）

$\mathop{\rm rank} \boldsymbol{\Sigma} = 3$の場合から考える．

$\boldsymbol{Z}$ の固有値を $\lambda_k ~(k=1,2,3)$ とし，$|\lambda_1| \ge |\lambda_2| \ge |\lambda_3|$ とする．

$|\lambda_k| = 1 ~ (k=1,2,3)$（直交行列の固有値の性質），複素固有値 $\lambda$ が $\boldsymbol{Z}$ の固有値ならば $\bar{\lambda}$ （$\bar{\lambda}$ は複素共役）も $\boldsymbol{Z}$ の固有値であること， $\det \boldsymbol{Z} = \prod_{k=1}^3 \lambda_k = -1$ より， $\boldsymbol{Z}$ の固有値は少なくとも 1 つが -1 であり（$\lambda_1 = -1$ とする），残りの $\lambda_2, \lambda_3$ は $\exp(i\theta), \exp(-i\theta)$である．

よって $\mathop{\rm tr} \boldsymbol{Z} = \sum_{k=1}^n \lambda_k = 2 \cos \theta - 1$ であり， $z_{33} = -1$ の時 $\mathop{\rm tr} (\boldsymbol{\Sigma Z})$ は最大となる（実際，そのような $z_{33}$ の $\boldsymbol{Z}$ は左上 2x2 を回転行列にすることで得られる）．  $\cos \theta$ が大きければ $z_{11}, z_{22}$ を大きくできるため， $\theta = 0$ の時， $z_{11} = z_{22} = 1$ で $\mathop{\rm tr} \boldsymbol{\Sigma Z}$ は最大となる．  
$\sigma_2 > \sigma_3$ の場合， $\hat{\boldsymbol{Z}}$ は明らかに一意．  
$\sigma_2 = \sigma_3$ の場合， $\hat{\boldsymbol{Z}}$ の右下 2x2 は行列式が -1 の任意の直交行列で良いため， $\hat{\boldsymbol{Z}}$ は一意ではない．

$\mathop{\rm rank} \boldsymbol{\Sigma} = 2$ の場合， $z_{11} = z_{22} = 1$ で $\mathop{\rm tr} (\boldsymbol{\Sigma Z})$ は最大となり， $\det \boldsymbol{Z} = -1$ より $z_{33} = -1$ ．  よって $\hat{\boldsymbol{Z}}$ は一意．  
$\mathop{\rm rank} \boldsymbol{\Sigma} = 1$ の場合， $z_{11} = 1$ で $\mathop{\rm tr} (\boldsymbol{\Sigma Z})$ は最大となる．$\boldsymbol{Z}$ の右下 2x2 は行列式が -1 の任意の直交行列で良いため， $\hat{\boldsymbol{Z}}$ は一意ではない．  
$\mathop{\rm rank} \boldsymbol{\Sigma} = 0$ の場合， $\hat{\boldsymbol{Z}}$ は任意の回転行列で良いため， $\hat{\boldsymbol{Z}}$ は一意ではない． 

（証明終）

補題 1,2 より，次の補題が成り立つ．

### 補題 3

<!-- 式 (1) の  -->
<!-- $\Rightarrow$ $\mathop{\rm arg~max} \mathop{\rm tr} (\boldsymbol{\Sigma Z})$ の解は -->
$\hat{\boldsymbol{R}}$ が一意  
$\Rightarrow$ $\hat{\boldsymbol Z}$ が $\mathop{\rm diag}(1,1,\det(\boldsymbol{UV}))$で一意  
$\Leftrightarrow ~ (\det \boldsymbol{Z} = 1 \land \mathop{\rm rank} \boldsymbol{\Sigma} \ge 2) \lor (\det \boldsymbol{Z} = -1 \land (\mathop{\rm rank} \boldsymbol{\Sigma} \ge 2 \land \sigma_2 \ne \sigma3))$

（証明）

式 (4) より， $\hat{\boldsymbol{Z}}$ の一意性は $\hat{\boldsymbol{R}}$ の一意性の必要条件となる．  
$\det \boldsymbol{R} = 1$より， $\det \boldsymbol{Z}$ の符号を決めるのは $\det(\boldsymbol{UV})$ のため，補題 1,2 の結果をまとめると $\boldsymbol{Z} = \mathop{\rm diag}(1,1,\det(\boldsymbol{UV}))$ となる．  
（ $\det \boldsymbol{A} \det \boldsymbol{B} = \det (\boldsymbol{AB}),~ \det(\boldsymbol{A}) = \det(\boldsymbol{A}^\top)$ を利用）

（証明終）

これで $\hat{\boldsymbol{Z}}$ が求まれば，次のように $\hat{\boldsymbol{R}}$ を次のように求められる．
```math
\begin{align*}
  \hat{\boldsymbol{R}} = \boldsymbol{U} 
  \begin{pmatrix}
    1 & & \\
    & 1 & \\
    & & \det(\boldsymbol{UV})
  \end{pmatrix}
  \boldsymbol{V}^\top
  \tag{5}
\end{align*}
```

次に気になるのは， $\hat{\boldsymbol{R}}$ が一意となる十分条件である．
補題 3 後半の条件より， $\hat{\boldsymbol{R}}$ が一意となる必要条件を$\boldsymbol{M}$ について書くと次のようになる．  

<!-- 式 (1) の -->
$\hat{\boldsymbol R}$ が一意   
$\Rightarrow$ $(\det \boldsymbol{M} > 0) \lor (\mathop{\rm rank} \boldsymbol{M} = 2) \lor (\det \boldsymbol{M} < 0 \land \boldsymbol{M} ~\text{の最小特異値が単根}~ )$

次の補題より，この条件は十分条件にもなっている．

### 補題 4 

<!-- 式 (1) の  -->
$\hat{\boldsymbol{R}}$ が $\boldsymbol{U} \mathop{\rm diag} (1,1,\det(\boldsymbol{UV})) \boldsymbol{V}^\top$ で一意  

$\Leftarrow$ $(\det \boldsymbol{M} > 0) \lor (\mathop{\rm rank} \boldsymbol{M} = 2) \lor (\det \boldsymbol{M} < 0 \land \boldsymbol{M} ~\text{の最小特異値が単根}~ )$

（証明）

( i ) $\det \boldsymbol{M} > 0$ の場合．  
( i - a ) $\sigma_1 > \sigma_2 > \sigma_3$の場合．
```math
\hat{\boldsymbol{R}}
= \boldsymbol {U} \mathop{\rm diag}(1,1,\det(\boldsymbol{UV})) \boldsymbol{V}^\top
= \sum_{k=1}^3 \boldsymbol{u}_k \boldsymbol{v}_k^\top
```
ただし $\boldsymbol{u}_k, \boldsymbol{v}_k$ はそれぞれ $\boldsymbol{U}, \boldsymbol{V}$ の列ベクトルとする．

$\sigma_k > 0$ なら $\boldsymbol{u}_k$ の向きが反転すると $\boldsymbol{v}_k$ の向きも反転するため， $\boldsymbol{u}_k \boldsymbol{v}_k^\top$ は変化しない（以降の証明ではこの説明を省略する）．よって $\hat{\boldsymbol{R}}$ は一意．

( i - b ) $\sigma_1 = \sigma_2 > \sigma_3$ の場合（ $\sigma_1 > \sigma_2 = \sigma3$ の場合も同様）．  
$\sigma_1 = \sigma_2 > 0$ なら，次の $\boldsymbol{U}', \boldsymbol{V}'$ もまた $\boldsymbol{M}$ の特異値分解の直交行列となる．
```math
\begin{align*}
  \boldsymbol{U}' &= (
    a\boldsymbol{u}_1 + b\boldsymbol{u}_2 ~~~ 
    s(b\boldsymbol{u}_1 - a\boldsymbol{u}_2) ~~~ 
    \boldsymbol{u}_3) \\
  \boldsymbol{V}' &= (
    a\boldsymbol{v}_1 + b\boldsymbol{v}_2 ~~~ 
    s(b\boldsymbol{v}_1 - a\boldsymbol{v}_2) ~~~ 
    \boldsymbol{v}_3) \\
\end{align*}
```
ただし， $a^2 + b^2 = 1,~ s=1,-1$ である．

$\boldsymbol{U'}, \boldsymbol{V'}$ を使って $\boldsymbol{R}$ を計算すると，
```math
\begin{align*}
  \hat{\boldsymbol{R} }
  &= \boldsymbol{U}' \mathop{\rm diag} (1,1,\det(\boldsymbol{U}'\boldsymbol{V}')) \boldsymbol{V}'^\top 
  \\
  &= (a \boldsymbol{u}_1 + b \boldsymbol{u}_2) 
    (a \boldsymbol{v}_1 + b \boldsymbol{v}_2)^\top
  + s^2(b \boldsymbol{u}_1 - a \boldsymbol{u}_2) 
    (b \boldsymbol{v}_1 - a \boldsymbol{v}_2)^\top
  + \boldsymbol{u}_3 \boldsymbol{v}_3^\top
  \\
  &= \sum_{k=1}^{3} \boldsymbol{u}_k \boldsymbol{v}_k^\top
\end{align*}
```
よって $\hat{\boldsymbol{R}}$ は一意．

( i - c ) $\sigma_1 = \sigma_2 = \sigma_3 = \sigma$ の場合．  
$\boldsymbol{M}$ は $\boldsymbol{M} = \sigma \boldsymbol{UV}^\top$ と表すことができ，これは回転行列の定数倍である．式 (3) の $\mathop{\rm tr} (\boldsymbol{M}^\top \boldsymbol{R})$ が最大となるのは明らかに ${\boldsymbol{R}} = \boldsymbol{M}$ の時であり，よって $\hat{\boldsymbol{R}}$ は一意．

( ii ) $\mathop{\rm rank} \boldsymbol{M} = 2$ の場合．  
$\boldsymbol{M}$ のある特異値分解 $\boldsymbol{M} = \boldsymbol{U \Sigma V}^\top$ において， $\det (\boldsymbol{U V}) = 1$ とする．  
同様に $\boldsymbol{M} = \boldsymbol{U}' \boldsymbol{\Sigma} \boldsymbol{V}'^\top$ において， $\det (\boldsymbol{U}' \boldsymbol{V}') = 1$ とする．

$\sigma_1 = \sigma_2$ の場合は ( i - b) と同様のため省略し， $\sigma_1 \ne \sigma_2$ の場合のみを考える．

$\sigma_3 = 0$ のため，$\boldsymbol{u}_3, \boldsymbol{v}_3$ のみ，一方の向きが反転しても他方の向きは反転しなくて良い．よって，$\det (\boldsymbol{U} \boldsymbol{V}), \det (\boldsymbol{U}' \boldsymbol{V}')$ の符号の違いは $\boldsymbol{u}_3, \boldsymbol{v}_3$ の向きの違いによるものである．これを利用すると，

<!-- ( ii - a ) $\boldsymbol{U}, \boldsymbol{V}$ の場合   -->
```math
\begin{align*}
  \hat{\boldsymbol{R}}
%   \boldsymbol{U V}^\top = \sum_{k=1}^{3} \boldsymbol{u}_k \boldsymbol{v}_k^\top
  &= \boldsymbol{U}' \mathop{\rm diag} (1,1,\det(\boldsymbol{U}'\boldsymbol{V}')) \boldsymbol{V}' 
  \\
  &= \boldsymbol{u}_1 \boldsymbol{v}_1^\top + \boldsymbol{u}_2 \boldsymbol{v}_2^\top - (- \boldsymbol{u}_3 \boldsymbol{v}_3^\top) 
  \\
  &= \sum_{k=1}^{3} \boldsymbol{u}_k \boldsymbol{v}_k^\top
  \\
  &= \boldsymbol{U} \mathop{\rm diag} (1,1,\det(\boldsymbol{U} \boldsymbol{V})) \boldsymbol{V}^\top
\end{align*}
```
よって $\hat{\boldsymbol{R}}$ は一意．

( iii ) $\det \boldsymbol{M} < 0$ かつ $\boldsymbol{M}$ の最小特異値が単根の場合．  
$\sigma_1 = \sigma_2$ の場合は ( i - b) と同様のため省略し， $\sigma_1 \ne \sigma_2$ の場合のみを考えると

```math
\begin{align*}
  \hat{\boldsymbol{R}}
  = \boldsymbol{U} \mathop{\rm diag} (1,1,\det(\boldsymbol{U}\boldsymbol{V})) \boldsymbol{V}^\top
  = \boldsymbol{u}_1 \boldsymbol{v}_1^\top
  + \boldsymbol{u}_2 \boldsymbol{v}_2^\top
  - \boldsymbol{u}_k \boldsymbol{v}_3^\top
\end{align*}
```
よって $\hat{\boldsymbol{R}}$ は一意．





（証明終）

### 定理

<!-- 式 (1) の  -->
$\hat{\boldsymbol{R}}$ が $\boldsymbol{U} \mathop{\rm diag} (1,1,\det(\boldsymbol{UV}))\boldsymbol{V}^\top$ で一意   
$\Leftrightarrow$ $(\det \boldsymbol{M} > 0) \lor (\mathop{\rm rank} \boldsymbol{M} = 2) \lor (\det \boldsymbol{M} < 0 \land \boldsymbol{M} ~\text{の最小特異値が単根}~ )$

（証明）

補題 3,4 より成り立つ．

（証明終）

<!-- コンピュータ上での数値計算的に，どのような状況で $\boldsymbol{M}$ が定理の条件を満たしていると判定するかは後で説明する． -->


## 3次元相似変換パラメータ推定

次の最小化問題の解を求めることで， 3 次元相似変換パラメータを推定する．

```math
\begin{align*}
  \hat{\boldsymbol{R}},~ \hat{\boldsymbol{t}},~ \hat{s}~ 
  &= \mathop{\rm arg~min}_{\boldsymbol{R} \in \mathrm{SO}(3),~ \boldsymbol{t}~, s > 0}
     \sum_{k=1}^{n} \| \boldsymbol{y}_k - (s\boldsymbol{R}\boldsymbol{x}_k + \boldsymbol{t}) \|_2^2 
  \\
  &= \mathop{\rm arg~min} L(\boldsymbol{R}, \boldsymbol{t}, s)
  \tag{6}
\end{align*}
```
ただし，$\boldsymbol{x}_k, \boldsymbol{y}_k$ は $\boldsymbol{y_k} = \bar{s}\bar{\boldsymbol{R}}\boldsymbol{x}_k + \bar{\boldsymbol{t}} + \boldsymbol{\epsilon}_k$ 関係であり， $\boldsymbol{\epsilon}_k$ をノイズ， $\bar{p}$ のようなバー付き文字を真のパラメータとする．また，以降 $\mathop{\rm arg~min}$ 下の記述を省略する

一般的に $s > 0$ として良いのかはわからないが， $\hat{s} = 0$ の場合は $\boldsymbol {y}_k$ が同じ 1 点にある時に等しいため，それを使えば $\hat{s} = 0$ か簡単に判定できる．よってここでは $s > 0$ の場合を扱う．

$L(\boldsymbol{R}, \boldsymbol{t}, s)$ を $\boldsymbol{t}$ で偏微分すると， 
```math
\begin{align*}
  \frac{\partial L(\boldsymbol{R}, \boldsymbol{t}, s)}{\partial \boldsymbol{t}}
  = -2 \sum_{k=1}^n (\boldsymbol{y}_k - (s \boldsymbol{R}\boldsymbol{x}_k + \boldsymbol{t}))
\end{align*}

```

上式は $\hat{\boldsymbol{R}}, \hat{\boldsymbol{t}}, \hat{s}$ において 0 となるので，
```math
\begin{align*}
  \hat{\boldsymbol{t}} = \boldsymbol{\mu_y} - \hat{s} \hat{\boldsymbol{R}} \boldsymbol{\mu_x} 
  \tag{7}
\end{align*}
```

ただし，
```math
\boldsymbol{\mu_x} = \sum_{k=1}^n \boldsymbol{x}_k,~~~~~
\boldsymbol{\mu_y} = \sum_{k=1}^n \boldsymbol{y}_k
```
である．

$\hat{\boldsymbol{t}}$ は $\hat{\boldsymbol{R}}, \hat{s}$ より求まるため， $\hat{\boldsymbol{t}}$ をわざわざ推定する必要する必要はない． $L(\boldsymbol{R}, \boldsymbol{t}, s)$ において $\boldsymbol{t} = \boldsymbol{\mu_y} - {s} {\boldsymbol{R}} \boldsymbol{\mu_x}$ として最小化しても同じ結果が得られるため， $L(\boldsymbol{R}, \boldsymbol{t}, s)$ の最小化は次の式の最小化に等しい．
```math
\begin{align*}
  L(\boldsymbol{R,} s)
  = \sum_{k=1}^{n} \| 
    (\boldsymbol{y}_k - \boldsymbol{\mu_y})
    - s \boldsymbol{R}(\boldsymbol{x}_k - \boldsymbol{\mu_x}) 
  \|_2^2
\end{align*}
```

$L(\boldsymbol{R}, s)$ を $s$ で偏微分すると．
```math
\begin{align*}
  \frac{\partial L (\boldsymbol R, s)}{\partial s}
  = 2 \sum_{k=1}^n (
    - (\boldsymbol{y}_k - \boldsymbol{\mu_y})^\top \boldsymbol{R} (\boldsymbol{x}_k - \boldsymbol{\mu_x})
    + s(\boldsymbol{x}_k - \boldsymbol{\mu_x})^\top(\boldsymbol{x}_k - \boldsymbol{\mu_x})
  )
\end{align*}
```

上式は $\hat{\boldsymbol{R}}, \hat{s}$ において 0 となるので，
```math
\begin{align*}
  \hat{s} &= \frac{
    \sum_{k=1}^{n} 
    (\boldsymbol{y}_k - \boldsymbol{\mu_y})^\top \hat{\boldsymbol{R}} (\boldsymbol{x}_k - \boldsymbol{\mu_x})
  }{
    \sum_{k=1}^{n}
    (\boldsymbol{x}_k - \boldsymbol{\mu_x})^\top(\boldsymbol{x}_k - \boldsymbol{\mu_x})
  }\\
  &= \frac{
    \mathop{\rm tr} (\boldsymbol{Y}^\top \boldsymbol{RX})
  }{
    \mathop{\rm tr} (\boldsymbol{X}^\top \boldsymbol{X})
  }
  \tag{8}
\end{align*}
```
ただし，
```math
\begin{align*}
  \boldsymbol{X} &= (
    \boldsymbol{x}_1 - \boldsymbol{\mu_x} ~~~~~
    \boldsymbol{x}_2 - \boldsymbol{\mu_x} ~~~~~
    ... ~~~~~
    \boldsymbol{x}_n - \boldsymbol{\mu_x}
  ) 
  \\
  \boldsymbol{Y} &= (
    \boldsymbol{y}_1 - \boldsymbol{\mu_x} ~~~~~
    \boldsymbol{y}_2 - \boldsymbol{\mu_x} ~~~~~
    ... ~~~~~
    \boldsymbol{y}_n - \boldsymbol{\mu_x}
  ) 
\end{align*}
```
である．

$\boldsymbol{X}, \boldsymbol{Y}$ を使って $L(\boldsymbol{R}, s)$ を変形すると，
```math
\begin{align*}
  L(\boldsymbol{R}, s)
  &= \| \boldsymbol{Y} - s\boldsymbol{RX} \|_F^2
  \\
  &= \mathop{\rm tr} ((\boldsymbol{Y}-s\boldsymbol{RX})^\top (\boldsymbol{Y}-s\boldsymbol{RX}))
  \\
  &= \mathop{\rm tr} (\boldsymbol{Y}^\top \boldsymbol{Y})
   - 2s \mathop{\rm tr} (\boldsymbol{Y}^\top \boldsymbol{RX})
   + s^2 \mathop{\rm tr} (\boldsymbol{X}\top \boldsymbol{X})
  \\
\end{align*}
```

$\boldsymbol{t}$ を削除した時と同様に， $L(\boldsymbol{R}, s)$ から $s$ を削除すると，
```math
\begin{align*}
  L(\boldsymbol{R})
  &= \mathop{\rm tr} (\boldsymbol{Y}^\top \boldsymbol{Y})
   - 2 \frac{\mathop{\rm tr} (\boldsymbol{Y}^\top \boldsymbol{RX})^2}{\mathop{\rm tr} (\boldsymbol{X}^\top \boldsymbol{X})}
   + \frac{\mathop{\rm tr} (\boldsymbol{Y}^\top \boldsymbol{RX})^2}{\mathop{\rm tr} (\boldsymbol{X}^\top \boldsymbol{X})}
  \\
  &= \mathop{\rm tr} (\boldsymbol{Y}^\top \boldsymbol{Y})
   - \frac{\mathop{\rm tr} (\boldsymbol{Y}^\top \boldsymbol{RX})^2}{\mathop{\rm tr} (\boldsymbol{X}^\top \boldsymbol{X})} 
\end{align*}
```

$\mathop{\rm tr} (\boldsymbol{X}^\top \boldsymbol{X}) \ge 0$ だが， $\mathop{\rm tr} (\boldsymbol{X}^\top \boldsymbol{X}) = 0$ の場合，全ての $\boldsymbol{x}_k$ は同じ点となるため，相似変換パラメータを推定できない．よって $\mathop{\rm tr} (\boldsymbol{X}^\top \boldsymbol{X}) > 0$ とする．$\mathop{\rm tr} (\boldsymbol{X}^\top \boldsymbol{X}) > 0$ と 式 (8) より， $\mathop{\rm tr} (\boldsymbol{Y}^\top \boldsymbol{RX}) > 0$ ．
よって， $L(\boldsymbol{R})$ の最小化は次の式の最小化に等しい．
```math
\begin{align*}
  \mathop{\rm arg~min} L(\boldsymbol{R})
  &= \mathop{\rm arg~max} \mathop{\rm tr} (\boldsymbol{Y}^\top \boldsymbol{RX})
  \\
  &= \mathop{\rm arg~max} \mathop{\rm tr} (\boldsymbol{XY}^\top \boldsymbol{R})
\end{align*}
```

これは式 (3) において $\boldsymbol{M} = \boldsymbol{YX}^\top$ としたのと同じ形であるため，特異値分解を用いて同様に $\hat{\boldsymbol{R}}$ を求められる．そして 順に $\hat{s}, \hat{\boldsymbol{t}}$ を求めれば（式 (8), (7)），すべての相似変換パラメータが推定されたことになる．

定理より， $\mathop{\rm rank} \boldsymbol{M} \ge 2$ でなければならないので，$\mathop{\rm rank} \boldsymbol{X} \ge 2$ かつ $\mathop{\rm rank} \boldsymbol{Y} \ge 2$ であることが必要である．幾何学的には，{$\boldsymbol{x}_k$},{$\boldsymbol{y}_k$} それぞれが共線になければ一意な相似変換パラメータを推定できる可能性があるので，最低でも 3 点の対応が必要になる．

## おまけ： M の判定
<!-- ## おまけ： $\boldsymbol{M} = \boldsymbol{YX^\top}$ の判定 -->

（これは適当に考えたものです．一般的にどうしているのかはわかりません．）

<!-- 定理より，どのような条件で $\hat{\boldsymbol{R}}$ が一意に求まるかは分かった． -->
実際にコンピュータで $\boldsymbol{X}, \boldsymbol{Y}$ から $\boldsymbol{M}$ が計算された時，最小特異値がちょうど重根になったり， $n > 3$ の下で $\mathop{\rm rank} \boldsymbol{M} = 2$ になることは少ない．仮に $\boldsymbol{M}$ が定理の条件を満たしていても，その推定結果がノイズに大きく影響されるようなシビアな状況である場合，良い推定とは言いにくい．ではどんな $\boldsymbol{M}$ の時に，定理の条件を満たしている（シビアな状況ではない）といって良いのか．

まず次の条件を準備する．
```math
\begin{align*}

  A_9 &\Leftrightarrow
  \left(
    \frac{\sigma_3}{\sigma_1} < \mathrm{th}_1 
    \Rightarrow \mathop{\rm rank} M \le 2 ~\text{とみなす} 
  \right)
  % \tag{9}
  \\
  A_{10} &\Leftrightarrow 
  \left(
    \frac{\sigma_2}{\sigma_1} < \mathrm{th}_1 
    \Rightarrow \mathop{\rm rank} M \le 1 ~\text{とみなす} 
  \right)
  % \tag{10}
  \\
  A_{11} &\Leftrightarrow
  \left(
    \frac{\sigma_2 - \sigma_3}{\sigma_1} < \mathrm{th}_2 
    \Rightarrow M ~\text{の最小特異値が重根とみなす} 
  \right)
  % \tag{11}
\end{align*}
```
ただし $\mathrm{th_1, th_2}$ は適当な閾値（ 0.001 等）である．そして次の条件を満たす時， $\boldsymbol{M}$ が定理の条件を満たしていると判定する．
```math
\begin{align*}
  \overline{A_{10}} \land \left( 
    A_9 \lor \det \boldsymbol{M} > 0 \lor \overline{A_{11}}
  \right)
\end{align*}
```


## 終わりに

ここまでお付き合い頂きありがとうございました． 3 次元ではなく $n$ 次元の場合で書いたほうが気持ちいいですが，自分にはそこまで必要なかったのと， 3 次元で限定したほうが証明が簡単になるはずだと思ったので 3 次元の場合を扱いました．

この記事が3次元相似変換パラメータ推定の理解に苦しんでいる方々の助けになれば幸いです．