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

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

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

$$SO(2)=\left\{\left(\begin{array}{cc}\cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array}\right)\right|\theta \in \mathbb{R}\}\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$それぞれに対応する回転角であり、 $\bar{\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\iff x=y$も明らかである(劣弧の長さが0であることと、円上の2点が一致することは同値)。
3. 対称性 $d(x,y)=d(y,x)$も明らかである(「長さ」なので)。
4. 三角不等式 $d(x,y)+d(y,z)\ge d(x,z)$を示す。

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

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

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

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

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

を証明すれば良い。

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

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

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

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

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

$$|\overline{\theta -\psi }|=|\overline{\stackrel{―}{\theta }-\stackrel{―}{\psi }}|=|\bar{\theta }-\bar{\psi }|=\bar{\theta }-\bar{\psi }=|\bar{\theta }|+|\bar{\psi }|$$

となり成立。

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

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

となり成立。

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

距離関数の別の見方

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

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

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

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

$$d(r_0,r_1)=|r_0{r}_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)\to \mathbb{R}$$ を $$|r|=|\bar{\theta }|\in [0,\pi ]$$ によって定める。
ただし、$\theta$は$r$を axis-angle representation で表現したときの回転角である。

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

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

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

証明

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

$$(r_0{r}_1^{-1})\cdot (r_1{r}_0^{-1})=1$$

より、

$$r_1{r}_0^{-1}=(r_0{r}_1^{-1}{)}^{-1}$$

したがって、 $r_0{r}_1^{-1}$のaxis-angle representationによる、軸を$\overrightarrow{e}$、角を$\theta$とすると、 逆回転$r_1{r}_0^{-1}$のaxis-angle representationは、軸$\overrightarrow{e}$、角$-\theta$である。

そこで、

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

よって示された。

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

$$|r_0{r}_1^{-1}|+|r_1{r}_2^{-1}|\ge |r_0{r}_2^{-1}|$$

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

$$|r_0|+|r_1|\ge |r_0{r}_1|$$

となる。これを示す。

$r_2=r_0{r}_1$とし、 $r_n(n=0,1,2)$の axis-angle representationを軸$\overrightarrow{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}\overrightarrow{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}\overrightarrow{e_0}\cdot \overrightarrow{e_1}$$

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

$$\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 \varphi$$

を得る。

ここまで、axis-angle representationのパラメータのとり方に制限を与えていなかったが、 ${\theta }_n$は、$[0,\pi ]$の範囲で取れるので、そのように取ることにする。
つまり、${\bar{\theta }}_n$で${\theta }_n$を置き換えて、${\theta }_n<0$だった場合は、 $\overrightarrow{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 \varphi$$

したがって、

$$\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_0{r}_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^{\ast }))=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((\overrightarrow{e_0},{\theta }_0),(\overrightarrow{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}\overrightarrow{e_0}\cdot \overrightarrow{e_1})$$

により計算できる。

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

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

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

与えられた3次元回転を、単位クォータニオン $$q=a+bi+cj+dk$$ で表し、単位ベクトルを $$\overrightarrow{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^{\prime }(\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^{\prime }(\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);

となる。

• 数学
• 幾何