3次元回転の誤差の定量化・3次元回転の特定の軸に沿った最良近似

2次元回転の誤差の定量化

2次元回転から考える。
2次元ユークリッド空間$\mathbb{R}^2$上の回転は線型写像であり、その集合は特殊直交群$SO(2)$をなす。
$SO(2)$は具体的に書き表すことができて、

$$ SO(2) = \left\{ \begin{pmatrix} \cos{\theta} & -\sin{\theta} \\ \sin{\theta} &\cos{\theta} \end{pmatrix} \middle | \theta \in \mathbb{R} \right\} \simeq \mathbb{R} / 2 \pi \mathbb{Z} \simeq S^1 $$

であることはよく知られている。

$SO(2)$の2元$r_0, r_1$が与えられたとき、これらが「どれくらい異なるか」を定量化したい場面がある。
たとえば、回転を測るセンサーがあり、そのセンサーの性能を評価したい、というような場面である。

よく行われる定量化は、次のようなものだろう。

$$ d(r_0, r_1) = |\overline{\theta_0 - \theta_1}| $$

ただし、ここで$\theta_0, \theta_1$は、$r_0, r_1$それぞれに対応する回転角であり、 $\overline\theta$は$\theta$を$2\pi$で割った余り$\in (-\pi, \pi]$である。

これはつまり、$SO(2)$と単位円$S^1$の同型を使って、$r_0, r_1$を単位円上にプロットし、 その劣弧の長さを誤差としているのである。

$d$が$SO(2)$の距離であることの証明

ここで与えた$d$によって$SO(2)\simeq S^1$が距離空間になることは、確認しておこう。

一般的に、「どのくらい異なるか」ということを扱う場合には、距離空間という道具立てをする。

証明

1. 正定値性 $d(x, y) \ge 0$は定義より明らかである(「長さ」なので)。
2. 非退化性 $d(x, y) = 0 \Leftrightarrow x = y$も明らかである(劣弧の長さが0であることと、円上の2点が一致することは同値)。
3. 対称性 $d(x, y) = d(y, x)$も明らかである(「長さ」なので)。
4. 三角不等式 $d(x, y) + d(y, z) \ge d(x, z)$を示す。

$$ |\ \overline{\theta - \phi}\ | + |\ \overline{\phi - \psi}\ | \ge |\ \overline{\theta - \psi}\ | $$

を示せば良いが、ここで、

$$ |\ \overline{\phi - \psi}\ | = |\ \overline{\psi - \phi}\ | $$

であることに注意して、$\theta - \phi$を改めて$\theta$、$\psi - \phi$を改めて$\psi$とおくと、結局

$$ |\ \overline{\theta}\ | + |\ \overline{\psi}\ | \ge |\ \overline{\theta - \psi}\ | $$

を証明すれば良い。

$$ \overline{\theta - \psi}\ = \overline{\overline{\theta} - \overline{\psi}} $$

に注意して、$\overline{\theta}, \overline{\psi}$の符号で場合分けを行う。

i.) $0 \le \overline{\psi} \le \overline{\theta} \le \pi$の場合 $$ |\overline{\theta - \psi}| = |\overline{\overline{\theta} - \overline{\psi}}| = |\overline{\theta} - \overline{\psi}| = \overline{\theta} - \overline{\psi} \le \overline{\theta} + \overline{\psi} = |\overline{\theta}| + |\overline{\psi}| $$ となり成立。

ii.) $-\pi < \overline{\psi} \le 0 \le \overline{\theta} \le \pi$の場合

ii-a.) $0 \le \overline{\theta} - \overline{\psi} \le \pi$の場合

$$ |\overline{\theta - \psi}| = |\overline{\overline{\theta} - \overline{\psi}}| = |\overline{\theta} - \overline{\psi}| = \overline{\theta} - \overline{\psi} = |\overline{\theta}| + |\overline{\psi}| $$

となり成立。

ii-b.) $\pi \le \overline{\theta} - \overline{\psi} < 2\pi$の場合

$$ |\overline{\theta - \psi}| = |\overline{\overline{\theta} - \overline{\psi}}| = |(\overline{\theta} - \overline{\psi}) - 2\pi| = 2\pi - (\overline{\theta} - \overline{\psi}) \le \pi \le \overline{\theta} - \overline{\psi} = |\overline{\theta}| + |\overline{\psi}| $$

となり成立。

他の場合も、上記3つの場合に帰着できる。Q.E.D.

距離関数の別の見方

$d: SO(2) \times SO(2) \rightarrow \mathbb{R}$が、$SO(2)$上の距離関数を与えていることを見たが、これを別の見方でも捉えておこう。

つまり、$SO(2)$に絶対値

$$ |\cdot|: SO(2) \rightarrow \mathbb{R} $$

を、$|r| = |\overline\theta| \in [0, \pi]$によって定めると、

$$ d(r_0, r_1) = |r_0r_1^{-1}| $$

である。

すなわち、$r_0, r_1$の距離とは、$r_0, r_1$の「差分」回転の「大きさ」である。

3次元回転の誤差の定量化

さて、ここまでは2次元平面上での回転についての議論だった。

次に、3次元空間$\mathbb{R}^3$上の回転群$SO(3)$について考えよう。

$SO(2)$のときと同様に、 $SO(3)$の2元$r_0, r_1$が与えられたとき、これらが「どれくらい異なるか」を定量化することが目標である。
そして、その定量化は、$SO(2)$のときの自然な拡張になっていると、良い。

$SO(2)$のときの最後の方で議論した、絶対値的な捉え方を元に考えると、次のような定義が思い浮かぶ。

定義

まず、 $$ |\cdot|: SO(3) \rightarrow \mathbb{R} $$ を $$ |r| = |\overline\theta| \in [0, \pi] $$ によって定める。
ただし、$\theta$は$r$を axis-angle representation で表現したときの回転角である。

次に、 $$ d: SO(3) \times SO(3) \rightarrow \mathbb{R} $$ を $$ d(r_0, r_1) = |r_0r_1^{-1}| $$ によって定める。

$d$が$SO(3)$の距離であることの証明

興味があるのは、この定義で得られた$d$が距離関数となっているかどうかである。
局所的には距離関数となっているのは直観的だが、大域的にもそうかどうかは、自明ではないので、証明を与えよう。

証明

1. 正定値性 $d(x, y) \ge 0$は定義より明らかである(値域が正になるように定義した)。
2. 非退化性 $d(x, y) = 0 \Leftrightarrow x = y$も明らかである(回転角0ということは、軸が何であれ、無回転を表す)。
3. 対称性 $d(x, y) = d(y, x)$は次のように示す。

$$ (r_0r_1^{-1}) \cdot (r_1r_0^{-1}) = 1 $$

より、

$$ r_1r_0^{-1} = (r_0r_1^{-1})^{-1} $$

したがって、 $r_0r_1^{-1}$のaxis-angle representationによる、軸を$\vec{e}$、角を$\theta$とすると、 逆回転$r_1r_0^{-1}$のaxis-angle representationは、軸$\vec{e}$、角$-\theta$である。

そこで、

$$ d(r_0, r_1) = |\overline\theta| = |\overline{-\theta}| = d(r_1, r_0) $$

よって示された。

4. 三角不等式 $d(x, y) + d(y, z) \ge d(x, z)$を示す。

$$ |r_0r_1^{-1}| + |r_1r_2^{-1}| \ge |r_0r_2^{-1}| $$

を示せばよいが、$r_0r_1^{-1}$を改めて$r_0$とし、$r_1r_2^{-1}$を改めて$r_1$とすると、

$$ |r_0| + |r_1| \ge |r_0r_1| $$

となる。これを示す。

$r_2 = r_0 r_1$とし、 $r_n (n = 0, 1, 2)$の axis-angle representationを軸$\vec{e_n}$、角$\theta_n$、 ベルソル表現 = 単位クォータニオン表現を $q_n = a_n + b_n i + c_n j + d_n k$とする。

axis-angle representationとversor representationの間には、

$$ a_n = \cos \frac{\theta_n}{2} $$ $$ \left( \begin{array}{c} b_n \\ c_n \\ d_n \end{array} \right) = \sin \frac{\theta_n}{2} \vec{e_n} $$

なる関係があることが知られている。

これらの関係を用いると、

$$ \cos{\frac{\theta_2}{2}} = a_2 = a_0 a_1 - (b_0 b_1 + c_0 c_1 + d_0 d_1) = a_0 a_1 - \left( \begin{array}{c} b_0 \\ c_0 \\ d_0 \end{array} \right) \cdot \left( \begin{array}{c} b_1 \\ c_1 \\ d_1 \end{array} \right) = \cos{\frac{\theta_0}{2}} \cos{\frac{\theta_1}{2}} - \sin{\frac{\theta_0}{2}} \sin{\frac{\theta_1}{2}} \vec{e_0} \cdot \vec{e_1} $$

なる等式を得る。
$\vec{e_n}$は単位ベクトルだから、その内積は、なす角の余弦に等しい。
そこで、軸$\vec{e_0}$と$\vec{e_1}$のなす角を$\phi$とすると、結局、

$$ \cos{\frac{\theta_2}{2}} = \cos{\frac{\theta_0}{2}} \cos{\frac{\theta_1}{2}} - \sin{\frac{\theta_0}{2}} \sin{\frac{\theta_1}{2}} \cos{\phi} $$

を得る。

ここまで、axis-angle representationのパラメータのとり方に制限を与えていなかったが、 $\theta_n$は、$[0, \pi]$の範囲で取れるので、そのように取ることにする。
つまり、$\overline\theta_n$で$\theta_n$を置き換えて、$\theta_n < 0$だった場合は、 $\vec{e_n}, \theta_n$を両方正負反転させることで、$\theta_n \in [0, \pi]$とすることができる。

このとり方をしたとき、定義より

$$ |r_n| = \theta_n $$

である。

この仮定のもとで、$\sin{\frac{\theta_n}{2}} \ge 0$だから、

$$ \cos{\frac{\theta_0}{2}} \cos{\frac{\theta_1}{2}} - \sin{\frac{\theta_0}{2}} \sin{\frac{\theta_1}{2}} \le \cos{\frac{\theta_0}{2}} \cos{\frac{\theta_1}{2}} - \sin{\frac{\theta_0}{2}} \sin{\frac{\theta_1}{2}} \cos{\phi} $$

したがって、

$$ \cos{\frac{\theta_0 + \theta_1}{2}} \le \cos{\frac{\theta_2}{2}} $$

となる。

$[0, \pi]$の範囲で$\cos$は単調減少なので、

$$ \frac{\theta_0 + \theta_1}{2} \ge \frac{\theta_2}{2} $$

すなわち、 $$ \theta_0 + \theta_1 \ge \theta_2 $$

$$ |r_0| + |r_1| \ge |r_0r_1| $$

よって示された。Q.E.D.

$d$の計算方法

以上で、$SO(3)$に、$SO(2)$の自然な拡張として、距離を入れることができた。
これで計算される距離は「角度」なので、幾何にそれほど明るくないエンジニアやテスターにも直観的なメトリクスである。

$SO(3)$の元の各表示について、ここで定義した距離の計算方法を示そう。

versor representation

単位クォータニオンを用いた表現の場合、

$$ d(q_0, q_1) = 2 \arccos{(Re(q_0 \cdot q_1^*))} = 2 \arccos{(a_0 a_1 + b_0 b_1 + c_0 c_1 + d_0 d_1)} $$

と計算できる。
最適化問題などでは、$d$を最小化する代わりに、 $a_0 a_1 + b_0 b_1 + c_0 c_1 + d_0 d_1$を最大化するという問題に読み替えてしまい、 距離の直観性を引き換えに高速化を行う選択肢もあるだろう。

matrix representation

行列を用いた表現の場合、

$$ d(M_0, M_1) = \arccos{\frac{\mathrm{tr}(M_0 M_1^{\mathrm{T}}) - 1}{2}} $$

と計算できる。

axis-angle representation

軸と回転角を用いた表現の場合、既に、証明中でもみたが、

$$ d((\vec{e_0}, \theta_0), (\vec{e_1}, \theta_1)) = 2 \arccos{(\cos{\frac{\theta_0}{2}} \cos{\frac{\theta_1}{2}} - \sin{\frac{\theta_0}{2}} \sin{\frac{\theta_1}{2}} \vec{e_0} \cdot \vec{e_1})} $$

により計算できる。

3次元回転の特定の軸に沿った最良近似

上のversor representationによる距離の計算は簡単で扱いやすいので、応用例を一つ挙げる。

ある3次元回転と単位ベクトルが与えられたときに、与えられた単位ベクトルを軸とする3次元回転で、 与えられた3次元回転を最もよく近似するものを求める。

与えられた3次元回転を、単位クォータニオン $$ q = a + bi + cj + dk $$ で表し、単位ベクトルを $$ \vec{e} = \left( \begin{array}{c} e_x \\ e_y \\ e_z \end{array} \right) $$ とする。

求める回転角$\theta$は、 $$ p = \cos{\frac{\theta}{2}} + \sin{\frac{\theta}{2}}(e_x i + e_y j + e_z k) $$ として、 $d(p, q)$を最小化するものとして求まる。

$$ f(\theta) = d(p, q) = 2 \arccos{(a \cos{\frac{\theta}{2}} + (e_x b + e_y c + e_z d)\sin{\frac{\theta}{2}})} $$ を微分すると、

$$ f’(\theta) = \frac{a \sin{\frac{\theta}{2}} - (e_x b + e_y c + e_z d)\cos{\frac{\theta}{2}}} {\sqrt{1 - (a \cos{\frac{\theta}{2}} + (e_x b + e_y c + e_z d)\sin{\frac{\theta}{2}})^2}} $$

$f’(\theta) = 0$と置けば、

$$ \theta = 2 \alpha $$

ただし、$\alpha$は

$$ \cos{\alpha} = \frac{a}{\sqrt{a^2 + (e_x b + e_y c + e_z d)^2}} $$

$$ \sin{\alpha} = \frac{e_x b + e_y c + e_z d}{\sqrt{a^2 + (e_x b + e_y c + e_z d)^2}} $$

を充たす角である。

C言語的に書けば、

theta = 2 * atan2(ex * b + ey * c + ez * d, a);

となる。