"Umeyama alignment"と呼ばれる、2つのtrajectoryの距離が最小となるようなアライメント手法についての論文をまとめる。

元論文

https://pdfs.semanticscholar.org/d107/231cce2676dbeea87e00bb0c587c280b9c53.pdf?_ga=2.170295078.783089896.1609935849-1595059948.1609935849

命題

問題設定

2つの3次元トラジェクトリ $\vec{x}_{i}$ 、 $\vec{y}_{i}$ （ $i=1, 2, \dots n$ ）があるとする。このとき

$\begin{equation} e^{2}(c, R, \vec{t}) \equiv \displaystyle{ \frac{1}{n} \sum_{i=1}^{n} |\vec{y}_{i} - (cR\vec{x}_{i} + \vec{t}) | ^{2}} \end{equation}$

を最小にするような $c, R, \vec{t}$ を求めたい。

変数の定義

$\begin{eqnarray} \vec{\mu}_{x} &=& \frac{1}{n} \sum_{i=1}^{n} \vec{x}_{i} \\ \vec{\mu}_{y} &=& \frac{1}{n} \sum_{i=1}^{n} \vec{y}_{i} \\ \sigma_{x}^{2} &=& \frac{1}{n} \sum_{i=1}^{n} (\vec{x}_{i}-\vec{\mu}_{x})^{2} \\ \sigma_{y}^{2} &=& \frac{1}{n} \sum_{i=1}^{n} (\vec{y}_{i}-\vec{\mu}_{y})^{2} \\ \Sigma_{xy} &=& \frac{1}{n} \sum_{i=1}^{n} (\vec{x}_{i}-\vec{\mu}_{x}) (\vec{y}_{i}-\vec{\mu}_{y})^\mathsf{T} \\ \end{eqnarray}$

とし、 $\Sigma_{xy}$ が直行行列 $U, V$ 、対角行列 $D$ を使って特異値分解できるとする。すなわち

$\begin{equation} \sigma_{xy} = UDV^\mathsf{T} \end{equation}$

また、行列 $S$ は
1: $\mathrm{det}(\Sigma_{xy}) \gt 0$ のとき

$\begin{equation} S = \left( \begin{array}{cccccc} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ \dots & & & & & \\ 0 & 0 & 0 & 0 & 0 & 1 \end{array}\right) \end{equation}$

2: $\mathrm{det}(\Sigma_{xy}) \lt 0$ のとき

$\begin{equation} S = \left( \begin{array}{cccccc} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ \dots & & & & & \\ 0 & 0 & 0 & 0 & 0 & -1 \end{array}\right) \end{equation}$

と定義する。

主張

$e^{2}$ を最小にする $c, R, \vec{t}$ は

$\begin{eqnarray} R &=& USV^\mathsf{T} \\ \vec{t} &=& \vec{\mu}_{y} - cR\vec{\mu}_{x} \\ c &=& \frac{1}{\sigma_{x}^{2}}Tr(DS) \end{eqnarray}$

である。

証明

Lemma

行列 $A$ に対するFrobenius normを $||A||$ と表現する。 $m\times n$ 行列の $A, B$ に対して

$\begin{equation} min_{R} ||A-RB||^{2} = ||A||^{2} + ||B||^{2} - 2 \mathrm{Tr}(DS) \end{equation}$

となる。ただし $AB^\mathsf{T}$ は

$\begin{equation} AB^\mathsf{T} = UDV^\mathsf{T} \end{equation}$

と特異値分解できるとし、 $S$ は $\mathrm{det}(AB^\mathsf{T})$ の符号によって上と同じように決まる行列である。また、行列 $AB^\mathsf{T}$ のrankが $m-1$ 以上である場合は、 $||A-RB||^{2}$ を最小にする $R$ が一意に決まり

$\begin{equation} R = USV^\mathsf{T} \end{equation}$

である。

Lemmaの証明

前半

ラグランジアン $F$ を考える。最小化したいのは $||A-RB||^{2}$ であり、 $R$ に関する制約条件は $RR^\mathsf{T}=I$ および $\mathrm{det}(R)=1$ なので、

$\begin{equation} F \equiv ||A-RB||^{2} + \mathrm{Tr}(L(RR^\mathsf{T}-I)) + g(\mathrm{det}(R)-I)とする。 \end{equation}$

とする。ここで $L$ の性質について考えると、一般に $\mathrm{Tr}(A) = \mathrm{Tr}(A^\mathsf{T})$ であることを利用すれば

$\begin{eqnarray} \mathrm{Tr}\left[ L(RR^\mathsf{T}-I) \right] &=& \mathrm{Tr}\left[ (RR^\mathsf{T}-I)^\mathsf{T}L^\mathsf{T} \right] \\ &=& \mathrm{Tr}\left[ (RR^\mathsf{T}-I)L^\mathsf{T} \right] \\ &=& \mathrm{Tr}\left[ L^\mathsf{T}(RR^\mathsf{T}-I) \right] \ \ \ \ \ (一般に\mathrm{Tr}(AB)=\mathrm{Tr}(BA)であることを利用) \end{eqnarray}$

となるので、行列 $L$ は対称行列として扱ってよいことがわかる。
この $F$ を $R$ で微分することを考えるが、公式まとめの定理より

$\begin{equation} \frac{\partial}{\partial R}||A-RB||^{2} = -2AB^\mathsf{T} + 2RBB^\mathsf{T} \end{equation}$

および

$\begin{equation} \frac{\partial}{\partial R}\mathrm{det}(R) = R \end{equation}$

なので、

$\begin{equation} \frac{\partial F}{\partial R} = -2AB^\mathsf{T} + 2RBB^\mathsf{T} + 2RL + gR = 0 \end{equation}$

となるべきである。これを整理していくと

$\begin{eqnarray} R(BB^\mathsf{T}+L+\frac{1}{2}gI) &=& AB^\mathsf{T} \ \ \ \ \ (1) \\ (BB^\mathsf{T}+L+\frac{1}{2}gI) R^\mathsf{T} &=& BA^\mathsf{T} \ \ \ \ \ (転置をとり、Lが対称行列であることを利用) \\ (BB^\mathsf{T}+L+\frac{1}{2}gI) R^\mathsf{T} R(BB^\mathsf{T}+L+\frac{1}{2}gI) &=& BA^\mathsf{T}AB^\mathsf{T} \\ (BB^\mathsf{T}+L+\frac{1}{2}gI)^{2} &=& (VDU^\mathsf{T})(UDV^\mathsf{T}) \\ &=& VD^{2}V^\mathsf{T} \end{eqnarray}$

ここで、 $S'^{2}=I$ となる対角行列 $S'$ （すなわち対角成分が1か-1のいずれかである）を導入すると

$\begin{eqnarray} (BB^\mathsf{T}+L+\frac{1}{2}gI)^{2} &=& VD^{2}S'^{2}V^\mathsf{T} \\ &=& VDS'^{2}V^\mathsf{T}VDS'V^\mathsf{T} \ \ \ \ \ (DとS'はどちらも対角行列なので可換) \\ &=& (VDS'V^\mathsf{T})^{2} \\ BB^\mathsf{T}+L+\frac{1}{2}gI &=& VDS'V^\mathsf{T} \end{eqnarray}$

となる。ここで、 $S'$ の行列式について見てみると

$\begin{eqnarray} \mathrm{det}(AB^\mathsf{T}) &=& \mathrm{det}(BB^\mathsf{T}+L+\frac{1}{2}gI) \\ &=& \mathrm{det}(VDS'V^\mathsf{T}) \\ &=& \mathrm{det}(V) \mathrm{det}(D) \mathrm{det}(S') \mathrm{det}(V) \\ &=& \mathrm{det}(D) \mathrm{det}(S') \end{eqnarray}$

となり $\mathrm{det}(D) \gt 0$ であるから、 $\mathrm{det}(AB^\mathsf{T})$ と $\mathrm{det}(S')$ の符号は一致しないといけない。

$\begin{eqnarray} ||A-RB||^{2} &=& \mathrm{Tr} \left[ (A-RB)^\mathsf{T} (A-RB) \right] \\ &=& \mathrm{Tr} \left[ A^\mathsf{T}A + (RB)^\mathsf{T}(RB) - A^\mathsf{T}(RB) - B^\mathsf{T}R^\mathsf{T}A \right] \\ &=& ||A||^{2} + ||RB||^{2} - 2 \mathrm{Tr}(AB^\mathsf{T}R^\mathsf{T}) \\ &=& ||A||^{2} + ||RB||^{2} - 2 \mathrm{Tr}(R^\mathsf{T}AB^\mathsf{T}) \\ &=& ||A||^{2} + ||RB||^{2} - 2 \mathrm{Tr}(BB^\mathsf{T}+L+\frac{1}{2}gI) \ \ \ \ \ (1)より\\ &=& ||A||^{2} + ||RB||^{2} - 2 \mathrm{Tr}(VDS'V^\mathsf{T}) \ \ \ \ \ (2)より\\ &=& ||A||^{2} + ||RB||^{2} - 2 \mathrm{Tr}(V^\mathsf{T}VDS') \\ &=& ||A||^{2} + ||RB||^{2} - 2 \mathrm{Tr}(DS') \\ \end{eqnarray}$

となる。ここでこの式の値を最小にするには、 $S'$ の対角成分はなるべく多く1であった方がよく、-1がある場合は一番右下の成分が-1となるべきであり、かつ $S'$ の行列式は $AB^\mathsf{T}$ の行列式と一致しないといけないことから、 $S'$ はLemmaにある $S$ の形をとるのが最前あることがわかり、題意の前半は示された。

後半

最適なRは $\mathrm{rank}(AB^\mathsf{T})=m$ と $\mathrm{rank}(AB^\mathsf{T})=m-1$ のケースで場合分けを行う。

ケース1: $AB^\mathsf{T}$ のrankが $m$ （フルランク）である場合
$D$ には逆行列が存在するので

$\begin{eqnarray} R &=& AB^\mathsf{T}(BB^\mathsf{T} + L + \frac{1}{2}gI)^{-1} \\ &=& (UDV^\mathsf{T}) (VDSV^\mathsf{T})^{-1} \\ &=& UDV^\mathsf{T} VS^{-1}D^{-1}V^\mathsf{T} \\ &=& UDV^\mathsf{T} VD^{-1}SV^\mathsf{T} \ \ \ \ \ (S^{-1}=Sであり、SとD^{-1}はともに対角行列なので可換) \\ &=& USV^\mathsf{T} \end{eqnarray}$

と求まる。

ケース2: $D$ のrankが $m-1$ の場合

$\begin{eqnarray} R(BB^\mathsf{T}+L+\frac{1}{2}gI) &=& AB^\mathsf{T} \\ R(VDSV^\mathsf{T}) &=& UDV^\mathsf{T} \\ RVDS &=& UD \ \ \ \ \ (両辺に右からVをかけた) \\ RVD &=& UD \ \ \ \ \ (3)(DS=D) \\ U^\mathsf{T}RVD &=& D \end{eqnarray}$

ここで(3)では、Dの第 $m$ 成分は0であり、Sの第 $m$ 成分が1であろうと-1であろうと $DS=D$ が成り立つことを利用した。
$D=\mathrm{diag}(d_{i}) (i=1, 2, \dots, m)$ において $d_{m}=0$ となり、 $D$ の $(m-1)\times(m-1)$ の部分だけみればDの逆行列が存在し $U^\mathsf{T}RV$ が単位行列とならないといけない。残った第 $m$ 成分であるが、

$\begin{eqnarray} \mathrm{det}(U^\mathsf{T}RV) &=& \mathrm{det}(U) \mathrm{det}(R) \mathrm{det}(V) \\ &=& \mathrm{det}(U) \mathrm{det}(V) &=& 1 or -1 \end{eqnarray}$

もし $\mathrm{det}(U) \mathrm{det}(V) = 1$ なら、 $U^\mathsf{T}RV$ は単位行列になるべきだし、 $\mathrm{det}(U) \mathrm{det}(V) = -1$ なら、 $U^\mathsf{T}RV$ は単位行列のうち最後の第 $m$ 成分のみ-1になるべきなので、整理すると

$\begin{eqnarray} U^\mathsf{T}RV &=& S \\ R = USV^\mathsf{T} \end{eqnarray}$

ただし、 $m\times m$ 行列 $S$ は

$\begin{eqnarray} S &=& \left( \begin{array}{cccc} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ \dots & & & \\ 0 & 0 & 0 & 1 \end{array} \right) \ \ \ \ \ (\mathrm{det}(U)\mathrm{det}(V)=1のとき) \\ S &=& \left( \begin{array}{cccc} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ \dots & & & \\ 0 & 0 & 0 & -1 \end{array} \right) \ \ \ \ \ (\mathrm{det}(U)\mathrm{det}(V)=-1のとき) \\ \end{eqnarray}$

と表される。

Lemmaを利用した証明

$\begin{eqnarray} K &\equiv& I - \frac{1}{n}\left( \begin{array}{cccc} 1 & 1 & 1 & 1 \\ 1 & 1 & 1 & 1 \\ \dots & & & \\ 1 & 1 & 1 & 1 \end{array} \right) \\ &=& I - \frac{1}{n} \left( \begin{array}{cccc} 1 \\ 1 \\ \dots \\ 1 \end{array} \right) (1 1 \dots 1) \\ &=& I - \frac{1}{n} \vec{h} \vec{h}^\mathsf{T} \end{eqnarray}$

となる $n \times n$ 行列 $K$ を定義する。この $K$ の性質として

$\begin{eqnarray} K^{2} &=& I - \frac{2}{n} \vec{h} \vec{h}^\mathsf{T} + \frac{1}{n^{2}} \left( \begin{array}{cccc} n & n & n & n \\ n & n & n & n \\ \dots \\ n & n & n & n \end{array} \right) \\ &=& I - \frac{1}{n} \vec{h} \vec{h}^\mathsf{T} \\ &=& K \end{eqnarray}$

を満たす。この $K$ を使うと

$\begin{eqnarray} \sigma_{x}^{2} &\equiv& \frac{1}{n} \sum_{i} |\vec{x}_{i} - \vec{\mu}_{x}|^{2} \\ &=& \frac{1}{n} || X - \frac{1}{n}X \vec{h}\vec{h}^\mathsf{T} ||^{2} \\ &=& \frac{1}{n} || XK ||^{2} \\ \sigma_{y}^{2} &=& \frac{1}{n} ||YK||^{2} \\ \Sigma_{xy} &\equiv& \frac{1}{n} \sum_{i} (\vec{y}_{i}-\vec{\mu}_{y})(\vec{x}_{i}-\vec{\mu}_{x})^\mathsf{T} \\ &=& \frac{1}{n} || (Y-\frac{1}{n}Y\vec{h}\vec{h}^\mathsf{T}) (X-\frac{1}{n}X\vec{h}\vec{h}^\mathsf{T}) || \\ &=& \frac{1}{n} || YK(XK)^\mathsf{T} || \\ &=& \frac{1}{n} || YKKX^\mathsf{T} || \\ &=& \frac{1}{n} || YKX^\mathsf{T} || \end{eqnarray}$

また、最小化関数は

$\begin{eqnarray} e^{2}(c, R, \vec{t}) &\equiv& \displaystyle{ \frac{1}{n} \sum_{i=1}^{n} |\vec{y}_{i} - (cR\vec{x}_{i} + \vec{t}) | ^{2}} \\ &=& || (\vec{y_{1}, \vec{y}_{2}, \dots \vec{y}_{n}}) - \left[ cR(\vec{x}_{1}, \vec{x}_{2}, \dots, \vec{x}_{n}+\vec{t}) \right] ||^{2} \\ &=& || Y - (cRX+t\vec{h}) || ^{2} \end{eqnarray}$

となるが、

$\begin{eqnarray} XK &=& X - \frac{1}{n}X\vec{h}\vec{h}^\mathsf{T} \\ YK &=& Y - \frac{1}{n}Y\vec{h}\vec{h}^\mathsf{T} \\ \end{eqnarray}$

の関係性を利用すると

$\begin{eqnarray} e^{2} &=& \frac{1}{n} || YK + \frac{1}{n}Y\vec{h}\vec{h}^\mathsf{T} - cRXK - \frac{c}{n}RX\vec{h}\vec{h}^\mathsf{T} - \vec{t}\vec{h}^\mathsf{T} ||^{2} \\ &=& \frac{1}{n} || YK-cRXK + (\frac{1}{n}Y\vec{h} - \frac{c}{n}RX\vec{h} - \vec{t}) \vec{h}^\mathsf{T} ||^{2} \\ &\equiv& \frac{1}{n} || YK-cRXK - \vec{t}^{'} \vec{h}^\mathsf{T} ||^{2} \ \ \ \ \ (\vec{t}^{'}の導入)\\ &=& \frac{1}{n} \left[ ||YK-cRXK || ^{2} + || \vec{t}^{'}\vec{h}^\mathsf{T} ||^{2} -2\mathrm{Tr}((TK-cRXK)^\mathsf{T} \vec{t}^{'}\vec{h}^\mathsf{T}) \right] \\ &=& \frac{1}{n} \left[ ||YK-cRXK || ^{2} + \mathrm{Tr}(\vec{h}\vec{t}'^\mathsf{T} \vec{t}^{'}\vec{h}^\mathsf{T}) -2\mathrm{Tr}(K(Y^\mathsf{T}-cX^\mathsf{T}K)\vec{t}^{'}\vec{h}^\mathsf{T}) \right] \\ &=& \frac{1}{n} \left[ ||YK-cRXK || ^{2} + \mathrm{Tr}(\vec{h}^\mathsf{T}\vec{h}\vec{t}'^\mathsf{T} \vec{t}^{'}) -2\mathrm{Tr}(\vec{h}^\mathsf{T}K(Y^\mathsf{T}-cX^\mathsf{T}K)\vec{t}^{'}) \right] \\ &=& \frac{1}{n} \left[ ||YK-cRXK || ^{2} + n\mathrm{Tr}(\vec{t}'^\mathsf{T} \vec{t}^{'}) -2\mathrm{Tr}((\vec{h}^\mathsf{T}-\vec{h}^\mathsf{T})(Y^\mathsf{T}-cX^\mathsf{T}K)\vec{t}^{'}) \right] \\ &=& \frac{1}{n} \left[ ||YK-cRXK || ^{2} + n|| \vec{t}^{'} || ^{2} -2\mathrm{Tr}(O) \right] \\ &=& \frac{1}{n} ||YK-cRXK || ^{2} + || \vec{t}^{'} || ^{2} \end{eqnarray}$

まず、 $\vec{t}$ については第二項のみに依存するので、 $\vec{t}^{'} = \vec{0}$ とすれば $e^{2}$ が最小になる。すなわち

$\begin{eqnarray} \vec{t}^{'} &=& \frac{1}{n}Y\vec{h} - \frac{c}{n}RX\vec{h} - \vec{t} = \vec{0} \\ \vec{t} &=& \frac{1}{n}Y\vec{h} - \frac{c}{n}RX\vec{h} \\ &=& \vec{\mu}_{y} - cR\vec{\mu}_{x} \end{eqnarray}$

と決まる。次に、 $\Sigma_{xy}$ の特異値分解を $\Sigma_{xy} = UDV^\mathsf{T}$ とすると、

$\begin{eqnarray} (YK)(cXK)^\mathsf{T} &=& cYKK^\mathsf{T}X^\mathsf{T} \\ &=& cYKX^\mathsf{T} \\ &=& cn\Sigma_{xy} \\ &=& U(ncD)V^\mathsf{T} \end{eqnarray}$

と特異値分解できるので、Lemmaを使えば

$\begin{eqnarray} e^{2} &\ge& \frac{1}{n} \left[ ||YK||^{2} + ||cXK||^{2} - 2\mathrm{Tr}(ncDS) \right] \\ &=& \frac{1}{n} \left[ n\sigma_{y}^{2} + c^{2}n\sigma_{x}^{2} - 2nc\mathrm{Tr}(DS) \right] \\ &=& \sigma_{y}^{2} + c^{2}\sigma_{x}^{2} - 2c\mathrm{Tr}(DS) \\ &=& n\sigma_{x}^{2}\left( c-\frac{\mathrm{Tr}(DS)}{\sigma_{x}} \right)^{2} - \mathrm{Tr}(DS)^{2} + c^{2}\sigma_{x}^{2} \end{eqnarray}$