点の移動・回転

座標系

3次元空間を表す(直交)座標系は二種類あるらしい。右手系と左手系。えーと、親指の付け根に原点をとって、親指、人差し指、中指にそれぞれ$\boldsymbol{x}$、$\boldsymbol{y}$、$\boldsymbol{z}$軸を割り当てると$\boldsymbol{x} \rightarrow \boldsymbol{y} \rightarrow \boldsymbol{z} \rightarrow \boldsymbol{x} \rightarrow \cdots$が右回りになるのが右手系、左回りになるのが左手系。 図から分かるように、この二つの座標系はどんなに回転させても一致しないので無駄な努力はしない事。右手系と左手系の変換は適当な軸一つまたは三軸全ての符号を反転させればよい。ただし、二つの座標系を一緒に扱うと混乱するだけなので、どちらか一方に統一した方が賢い。まぜるな危険。 ここでは特に断らない限り右手系を用いる事にする。

3次元の回転は「どれだけ回したか」の他に「どの軸周りに回したか」というのも必要。なので、回転軸と回転角を適当に定める必要がある。 ここでは図に示すように、右手系は右ネジの向き、左手系は左ネジの向きに軸と回転方向をとる。こうすると2次元の回転方向と(z軸周りの)回転方向が同じになるため、何かと考えやすいはず。

2次元の点の移動と回転

いわゆる座標変換の話。ここで注意するのは、どの座標系からどの座標系への変換なのか、計算して出てきた座標はどの座標系で表されているかという事。ここでは、固定されている座標系から見て原点が$(u, v)$にあって$\theta$回転している座標系で表現されている点$\boldsymbol{x}^\prime$を、もとの固定されている座標系で表すとどうなるか、というのをやる。言い換えると、「ある点$\boldsymbol{x}$が原点周りに$\theta$回転して、平行移動$(u, v)$しました。この点$\boldsymbol{x}^\prime$はどこでしょう」という問題。

まあ、これは図を見たまんまで。$\boldsymbol{x}^\prime = \left( x^\prime_1, x^\prime_2 \right)$とすると、$x^\prime_1$成分はもとの座標系で$x^\prime_1 ( \cos \theta, \sin \theta )$、$x^\prime_2$は$x^\prime_2 ( -\sin \theta, \cos \theta )$になって$(u, v)$平行移動するので、次式になる。
$\boldsymbol{x} = \begin{bmatrix} \cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{bmatrix} \boldsymbol{x}^\prime + \begin{bmatrix} u \\ v \end{bmatrix}$

このとき、
$R = \begin{bmatrix} \cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{bmatrix}$
を回転行列といったりする。この行列は正規直交行列($R^\text{T} R = R R^\text{T} = I$)で$\text{det}(R) = 1$になる。

複素数を使った回転の表現

2次元の原点周りの回転は、大きさ1の複素数$c + is$を使っても表現できる。
$\begin{align} \left (x^\prime_1 + ix^\prime_2 \right) (c + is) &= c x^\prime_1 - s x^\prime_2 + i (s x^\prime_1 + c x^\prime_2) \\ &= \begin{bmatrix} c & -s \end{bmatrix} \boldsymbol{x}^\prime + i \begin{bmatrix} s & c \end{bmatrix} \boldsymbol{x}^\prime \end{align}$
ただし$i$は虚数単位とする。

で、何が言いたいかというと、「大きさが1」という条件は使いやすいので、覚えておくと良い事あるかもよってこと。

3次元の座標変換

2次元の場合と同じ。原点周りの回転$R$、平行移動$\boldsymbol{t}$とすると次式になる。
$\boldsymbol{x} = R \boldsymbol{x}^\prime + \boldsymbol{t}$
ただし、$R$は行列式の値が1の3x3正規直交行列である。

ちなみに、$\text{det}(R) = -1$のものは擬回転と呼ぶ事があり、これは回転の他に右手系・左手系を一方から他方に変換する作用も持つ。 つまり、右手系の話しかしていない時にこのような行列が出てきた場合は、何かがおかしいってこと。

逆問題を解く

今までは、移動量や回転角が与えられた場合に点がどう動くかを見てきた。ここでは反対に移動前後の点の組$\left \{ (x_i, x^\prime_i) \right \}_i$が与えられた時に、平行移動量および回転角(・回転軸)を求めることを考える。

要するに、次式を満たす$R, \boldsymbol{t}$を求める。
$\boldsymbol{x}_i = R \boldsymbol{x}^\prime_i + \boldsymbol{t}$
実際には誤差なんかがあって等式は成り立たなくなるだろうから、できるだけ等しくなるような値を求める事にする。

重心に関する運動

とりあえず、$\boldsymbol{x}_i$の重心$\bar{\boldsymbol{x}}$を計算してみる。
$\begin{align} \bar{\boldsymbol{x}} &= \frac{1}{n} \sum_i^n \boldsymbol{x}_i \\ &= \frac{1}{n} \sum_i^n R \boldsymbol{x}^\prime_i + \boldsymbol{t} \\ &= R \bar{\boldsymbol{x}}^\prime + \boldsymbol{t} \end{align}$

重心を原点にとって$\left( \boldsymbol{y}_i, \boldsymbol{y}^\prime_i \right)$とすると、
$\begin{align} \boldsymbol{y}_i &= \boldsymbol{x}_i - \bar{\boldsymbol{x}} \\ &= \left( R \boldsymbol{x}^\prime_i + \boldsymbol{t} \right) - \left( R \bar{\boldsymbol{x}}^\prime + \boldsymbol{t} \right) \\ &= R \left( \boldsymbol{x}^\prime_i - \bar{\boldsymbol{x}}^\prime \right) \\ &= R \boldsymbol{y}^\prime_i \end{align}$
となるから、これから回転$R$が求められそうだ。

回転が分かれば平行移動成分は次式で計算できる。
$\boldsymbol{t} = \bar{\boldsymbol{x}} - R \bar{\boldsymbol{x}}^\prime$
特に$\bar{\boldsymbol{x}}^\prime = \boldsymbol{0}$なら回転と無関係に平行移動成分が分かる。

というわけで、ここからは回転を求める事に精を出す。

2次元の回転

誤差の定義は色々考えられるが、ここではベクトルの内積は同じベクトル同士を掛けた時が最大というのを使って次式を用いる。
$e = \sum_i \boldsymbol{y}_i^\text{T} R \boldsymbol{y}^\prime_i \longrightarrow \max$
これは長いベクトルの方が誤差に表れやすいため、そういうベクトルを重視する人向けの誤差である。

$R = \begin{bmatrix} c & -s \\ s & c \end{bmatrix}$
とおいて$e$を未知数$c, s$に付いて書き直すと以下の式を得る。
$e = \sum_i \begin{bmatrix} y_{1i} y^\prime_{1i} + y_{2i} y^\prime_{2i} & y_{2i} y^\prime_{1i} - y_{1i} y^\prime_{2i} \end{bmatrix} \begin{bmatrix} c \\ s \end{bmatrix}$

この式が$c^2 + s^2 = 1$の下で最大になるのは当然次の時。
$\begin{bmatrix} c \\ s \end{bmatrix} = \frac{\boldsymbol{u}}{|\boldsymbol{u}|}$
$\boldsymbol{u} = \begin{bmatrix} \sum_i \boldsymbol{y}_i \cdot \boldsymbol{y}^\prime_i \\ -\sum_i \boldsymbol{y}_i \times \boldsymbol{y}^\prime_i \end{bmatrix}$

3次元の回転

まあ、2次元の時と同じ。大きさが1の複素数$c + is$の代わりに、大きさが1のクォータニオン$\boldsymbol{q}$を使うだけ。

クォータニオンの成分を
$\boldsymbol{q} = \left( s; \boldsymbol{v} \right)$
とすると、回転は次式で表される。
$\boldsymbol{y}_i = 2 \boldsymbol{v} \left( \boldsymbol{v} \cdot \boldsymbol{y}^\prime_i \right) + \left ( 2 s^2 - 1 \right) \boldsymbol{y}^\prime_i + 2 s \left( \boldsymbol{v} \times \boldsymbol{y}^\prime_i \right)$

で、内積が最大になるようにする、と。
$e = \sum_i \left( 2 \left( \boldsymbol{v} \cdot \boldsymbol{y}_i \right) \left( \boldsymbol{v} \cdot \boldsymbol{y}^\prime_i \right) + \left ( 2 s^2 - 1 \right) \left( \boldsymbol{y}_i \cdot \boldsymbol{y}^\prime_i \right) + 2 s \, \boldsymbol{y}_i {\cdot} \left( \boldsymbol{v} \times \boldsymbol{y}^\prime_i \right) \right) \longrightarrow \max$

$1 = s^2 + \boldsymbol{v}^\text{T} \boldsymbol{v}$に注意して適当に整理する。
$\begin{align} e &= \begin{bmatrix} s & v_1 & v_2 & v_3 \end{bmatrix} \sum_i \left( \begin{bmatrix} 0 & \boldsymbol{0}^\text{T} \\ \boldsymbol{0} & \boldsymbol{y}_i {\boldsymbol{y}^\prime_i}^\text{T} + \left( \boldsymbol{y}_i {\boldsymbol{y}^\prime_i}^\text{T} \right)^\text{T} \end{bmatrix} + \left( \boldsymbol{y}_i \cdot \boldsymbol{y}^\prime \right) \begin{bmatrix} 1 & \boldsymbol{0}^\text{T} \\ \boldsymbol{0} & -I \end{bmatrix} + \begin{bmatrix} 0 & \left( -\boldsymbol{y}_i \times \boldsymbol{y}^\prime_i \right)^\text{T} \\ -\boldsymbol{y}_i \times \boldsymbol{y}^\prime_i & O \end{bmatrix} \right) \begin{bmatrix} s \\ v_1 \\ v_2 \\ v_3 \end{bmatrix} \\ &= \boldsymbol{q}^\text{T} \left( \sum_i \begin{bmatrix} y_{1i} y^\prime_{1i} + y_{2i} y^\prime_{2i} + y_{3i} y^\prime_{3i} & y_{3i} y^\prime_{2i} - y_{2i} y^\prime_{3i} & y_{1i} y^\prime_{3i} - y_{3i} y^\prime_{1i} & y_{2i} y^\prime_{1i} - y_{1i} y^\prime_{2i} \\ y_{3i} y^\prime_{2i} - y_{2i} y^\prime_{3i} & y_{1i} y^\prime_{1i} - y_{2i} y^\prime_{2i} - y_{3i} y^\prime_{3i} & y_{1i} y^\prime_{2i} + y_{2i} y^\prime_{1i} & y_{1i} y^\prime_{3i} + y_{3i} y^\prime_{1i} \\ y_{1i} y^\prime_{3i} - y_{3i} y^\prime_{1i} & y_{1i} y^\prime_{2i} + y_{2i} y^\prime_{1i} & -y_{1i} y^\prime_{1i} + y_{2i} y^\prime_{2i} - y_{3i} y^\prime_{3i} & y_{2i} y^\prime_{3i} + y_{3i} y^\prime_{2i} \\ y_{2i} y^\prime_{1i} - y_{1i} y^\prime_{2i} & y_{1i} y^\prime_{3i} + y_{3i} y^\prime_{1i} & y_{3i} y^\prime_{2i} + y_{2i} y^\prime_{3i} & -y_{1i} y^\prime_{1i} - y_{2i} y^\prime_{2i} + y_{3i} y^\prime_{3i} \end{bmatrix} \right) \boldsymbol{q} \\ &= \boldsymbol{q}^\text{T} M \boldsymbol{q} \end{align}$

ということで、$M$の最大固有値に対応する(単位)固有ベクトルを求めればよい。$|\boldsymbol{q}| = 1$っていうのがこの辺で役に立つね。ちなみに、固有ベクトルには符号の不定性があって$\pm \boldsymbol{q}$になるけど、どっちを使っても同じ回転になる。なので、好きな方を採用すべし。

この計算はHornの方法と多分同じ。