概要

3次元回転の本のchapter4の内容をまとめた記事です。点群データ(a1,...,aN)を回転行列Rだけ回転させた点群データ(a'1,...,a'N)があったとします。このとき、回転前と回転後のデータをもとに、Rを推定する手法をまとめます。今回はデータに誤差が無いときの手法を実装しますが、その場合はデータは2組の対応するベクトルがあれば十分です。データに誤差がある場合の手法についてはこちらをご覧ください。

a\prime_i=Ra,  i=1,..,N

手法

コードはこちらにまとめています。
下画像のように、2点(ori1, ori2)をRだけ回転させた場合(rot1, rot2)のRを求めていきます。

まず、2点の外積ベクトルをそれぞれ求めて、単位ベクトルに正規化します。

ori_1*ori_2 = Norm[ori_1 × ori_2]

rot_1*rot_2 = Norm[rot_1 × rot_2]

下画像のori1*ori2とrot1*rot2が求めたベクトルです。

次に、ori1とori1*ori2の外積ベクトルを求めて、正規化します。rotについても同様です。

ori_1*(ori_1*ori_2) = Norm[ori_1 × (ori_1 × ori_2)]

rot_1*(rot_1*rot_2) = Norm[rot_1 × (rot_1 × rot_2)]

先ほど求めたベクトルを整理すると、以下のような正規直交系が得られます。

r_1 = Norm[ori_1], r_2 = Norm[ori_1 × ori_2], r_3 = Norm[ori_1 × (ori_1 × ori_2)]

r_1\prime = Norm[rot_1], r_2\prime = Norm[rot_1 × rot_2], r_3\prime = Norm[rot_1 × (rot_1 × rot_2)]

これらを列ベクトルとする行列R1, R2は直交行列となります。

R_1=\begin{pmatrix}
r_1 \\
r_2 \\
r_3\\
\end{pmatrix},
R_2=\begin{pmatrix}
r_1\prime \\
r_2\prime \\
r_3\prime \\
\end{pmatrix}

R1, R2はそれぞれ基底{e1, e2, e3}を{r1, r2, r3}, {r1', r2', r3'}に写像します。下の式の.Tは転置です。
よって、

R = R_2R_1^\intercal

となり、Rは{r1, r2, r3}を{r1', r2', r3'}に写像します。これが求める回転行列Rとなります。
それぞれの関係は下画像のようになります。

まとめ

データに誤差が無い場合は、今回説明した手法でRを計算できました。ただ、現実世界ではデータに誤差が乗ってくるので、最小2乗解を解くなどして、推定する必要があります。