概要
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乗解を解くなどして、推定する必要があります。