概要
デュアルクォータニオンを3次元計算に活用する2つの方法について調べたのでまとめます。
- デュアルクォータニオンを使うと、3次元空間での位置と姿勢両方の補間を、測地線に沿って行うことができる。必要となる計算量も同次変換行列等を使う場合に比べて少ない。
- デュアルクォータニオンを使うと、ハンドアイキャリブレーション問題のうち、座標系の関係式 $AX=XB$ から $X$ を求める問題を高い精度で解くことができる。必要となる計算量も非線形最適化を行う場合に比べて少ない。
デュアルクォータニオンが最も広く活用されているのはスキニングと思われますが、情報も多いのでここでは触れません。スキニングについては参考文献[9]などを参照してください。
リー代数とかちゃんと理解していないので不正確な表現があるかと思います。正確に理解されたい方は参考文献[3]などを参照してください。
座標変換を表すデュアルクォータニオン
デュアルクォータニオンは、2つのクォータニオン$p=p_0+p_1\mathbf{i}+p_2\mathbf{j}+p_3\mathbf{k}$, $q=q_0+q_1\mathbf{i}+q_2\mathbf{j}+q_3\mathbf{k}$, 二重数と呼ばれる2乗するとゼロになる数$\epsilon$を用いて以下のように表す（二重数については Wikipedia がわかりやすいです。複素数とは別のやり方で数を拡張する感じです)。
\sigma = p + \epsilon q
3次元の並進・回転からなる座標変換を考える。
並進成分を純粋クォータニオン$t=0+x\mathbf{i}+y\mathbf{j}+z\mathbf{k}$、回転成分を単位クォータニオン$r$で表すとき、以下のデュアルクォータニオンで座標変換を表すことができる。
\sigma = r + \epsilon \frac{1}{2} t r
実際の座標変換の計算にはクォータニオンと同じく共役なデュアルクォータニオンを用いるが、デュアルクォータニオンの共役には３種類ある。理由は、複素成分と二重数成分それぞれの共役の組み合わせが可能なため。
共役1
\sigma^\bullet = p - \epsilon q
共役2
\sigma^* = p^* + \epsilon q^*
共役3
\sigma^\diamondsuit = p^* - \epsilon q^*
2番目の共役を用いてノルムを定義した時、座標変換を表すデュアルクォータニオンのノルムの大きさは1となる。ノルムが1となるデュアルクォータニオンを単位デュアルクォータニオンと呼ぶ。
\begin{align}
\sigma \sigma^* &= (r + \epsilon \frac{1}{2} t r)(r^* + \epsilon \frac{1}{2} (t r)^*) \\
&= r r^* + \epsilon \frac{1}{2} (r r^* t^* + t r r^*) \\
&= 1
\end{align}
デュアルクォータニオンを用いて三次元空間上の点を座標変換することを考える。点の三次元座標を複素成分にマッピングした純粋クォータニオン$v$と、デュアルクォータニオンの3番目の共役を用いると、座標変換は以下のように計算できる。
\begin{align}
\sigma (1 + \epsilon v) \sigma^\diamondsuit &= (r + \epsilon \frac{1}{2} t r)(1 + \epsilon v)(r^* - \epsilon \frac{1}{2} (t r)^*) \\
&= (r + \epsilon (\frac{1}{2} t r + r v))(r^* - \epsilon \frac{1}{2} (t r)^*) \\
&= r r^* + \epsilon (\frac{1}{2} t r r^* + r v r^* - \frac{1}{2} r r^* t^*) \\
&= 1 + \epsilon (t + r v r^*)
\end{align}
これで単位デュアルクォータニオンは座標変換を表すことが確認できました。
補間
任意の座標変換（同次変換）は、ある回転軸周りの回転とその回転軸に平行な並進移動によって表すことができる(Chaslesの定理)。
単位クォータニオンは回転軸ベクトルと回転角の2つに分解することができるが、同様に単位デュアルクォータニオンはChaslesの定理により回転軸ベクトル$\mathbf{l}$、回転軸のオフセット(ベクトル)$\mathbf{c}$、回転角$\theta$、回転軸に平行に移動する距離$d$の4つに分解できる。
ちなみに、この4つのパラメータはscrew parameterと呼ばれている。
この分解を適用するとデュアルクォータニオンは
\sigma = p + \epsilon q = \cos \frac{\theta + \epsilon d}{2} + \sin \frac{\theta + \epsilon d}{2} (\mathbf{l} + \epsilon \mathbf{c} \times \mathbf{l})
の形になる。
\begin{align}
\theta &= 2 \arccos(p_0) \\
n &= \sqrt{p_1^2 + p_2^2 + p_3^2} \\
d &= -2 q_0 / n \\
\mathbf{l} &= (p_1\mathbf{i}+p_2\mathbf{j}+p_3\mathbf{k}) / n \\
\mathbf{c} \times \mathbf{l} &= (q_1\mathbf{i}+q_2\mathbf{j}+q_3\mathbf{k} - d p_0 \mathbf{l}) / n
\end{align}
のように各値を求めることができる(導出は参考文献[2][3][5][6]あたりを参照)。
slerpと同様に、以下のように三角関数の中身のスカラーの二重数を線形補間することで座標変換を補間することができる。
\sigma(t) = \cos \frac{(\theta + \epsilon d)t}{2} + \sin \frac{(\theta + \epsilon d)t}{2} (\mathbf{l} + \epsilon \mathbf{c} \times \mathbf{l})
参考文献[4][6]によると、これは測地線(geodesic path)に沿った補間になっているそうだが、幾何学的には$\mathbf{c}$を中心とした回転軸ベクトル$\mathbf{l}$周りの回転と、$\mathbf{l}$に平行な移動が同時に起こる軌跡になるため、ネジを締めるような動きになる。実際、この補間はScrew Theoryと関連が深く、ScLerp (Screw Linear Interpolation)と呼ばれている。
http://laht.info/WebGL/test.html にデモがあったので見てイメージを掴むとよいです。
ディスカッション
実際これがなんの役に立つかですが、位置姿勢を単に補間したい時には並進を線形補間して、姿勢を球面線形補間するだけでいい気がします。ScLerpにすると回転と並進の一体感は出るので、キャラクタの振付用途などで使えるんでしょうか。わかりません。スキニングに適しているのは直感的にわかるんですが。
ハンドアイキャリブレーション
ほとんど参考文献[5]の写経ですが、悪しからず。
図のように、先端にカメラが付いている多関節ロボットがある時、ロボットの先端に対してどのような位置姿勢$X$でカメラが取り付けられているかを求める問題を考える。
ロボットの根本は絶対座標系に固定されているとする。
ロボットの根本から先端までの座標変換はロボットの関節角度センサなどの値を用いて順運動学で求めることができるとする。
カメラの位置姿勢は絶対座標系に置かれたマーカーを見せてマーカーとの相対で求めることができるとする。
ロボットの姿勢を変え複数の視点からカメラにマーカーを見せる測定を$n$回行ったとき、
$i$番目の測定のロボットの根本から先端までの座標変換を$B_i$、
マーカーに対するカメラの位置姿勢を$A_i$とすると
ロボットの根本とマーカーは絶対座標で固定されているため、
ロボットの根本からマーカーまでの座標変換$B_i X^{-1} A_i$は、常に一定値となり、
複数回測定を行うことで$X$を推定することができるはずである。
2回の測定結果を用いると
\begin{align}
B_1 X^{-1} A_1 &= B_2 X^{-1} A_2 \\
X B_2^{-1} B_1 &= A_2 A_1^{-1} X
\end{align}
となり、$A=A_2 A_1^{-1}$と$B=B_2^{-1} B_1$を用いて
A X = X B
とシンプルに表すことができる。
単位デュアルクォータニオンは任意の座標変換(同次変換)を表すことができるため、
上式はデュアルクォータニオンを用いて
\begin{align}
a x &= x b \\
a &= x b x^*
\end{align}
と表すことができる。上式のスカラー成分$a_0=\frac{1}{2}(a + a^*)$のみを考えると、
\begin{align}
a_0 &= \frac{1}{2}(a + a^*) = \frac{1}{2}(x b x^* + (x b x^*)^*) \\
&= \frac{1}{2}x (b + b^*) x^* = \frac{1}{2}x x^* (b + b^*) \\
&= b_0
\end{align}
となり、$x$によらず$b_0$と等しくなることがわかる(Screw Congruence Theorem)。
つまり、$a x = x b$の8成分(クォータニオン4 $\times$ 二重数2=8)のうち2成分は無効な成分として除去できたことになる。
逆に複素成分のみを考えると、二重数ではない方の複素成分からは$a=a+\epsilon a^{\prime}$として、$a$の複素成分を$\mathbf{a}$として
\begin{align}
a x - x b &= a_0 \mathbf{x} + x_0 \mathbf{a} + \mathbf{a} \times \mathbf{x} - x_0 \mathbf{b} - b_0 \mathbf{x} - \mathbf{x} \times \mathbf{b} \\
&= (a_0 - b_0) \mathbf{x} + x_0 (\mathbf{a} - \mathbf{b}) + (\mathbf{a} + \mathbf{b}) \times \mathbf{x} \\
&= x_0 (\mathbf{a} - \mathbf{b}) + (\mathbf{a} + \mathbf{b}) \times \mathbf{x} \\
&= \mathbf{0}
\end{align}
二重数である方の複素成分からは
\begin{align}
(a^{\prime} x - x b^{\prime}) + (a x^{\prime} - x^{\prime} b) &= x_0 (\mathbf{a}^{\prime} - \mathbf{b}^{\prime}) + (\mathbf{a}^{\prime} + \mathbf{b}^{\prime}) \times \mathbf{x} + x^{\prime}_0 (\mathbf{a} - \mathbf{b}) + (\mathbf{a} + \mathbf{b}) \times \mathbf{x}^{\prime} \\
&= \mathbf{0}
\end{align}
という関係式が得られる。線形方程式としてまとめると、
\begin{bmatrix}
\mathbf{a} - \mathbf{b} & (\mathbf{a} + \mathbf{b}) \times & \mathbf{0}_{3 \times 1} & \mathbf{0}_{3 \times 3} \\
\mathbf{a}^{\prime} - \mathbf{b}^{\prime} & (\mathbf{a}^{\prime} + \mathbf{b}^{\prime}) \times & \mathbf{a} - \mathbf{b} & (\mathbf{a} + \mathbf{b}) \times
\end{bmatrix}
\begin{bmatrix}
x \\
x^{\prime}
\end{bmatrix} = \mathbf{0}_{6x1}
となる。この方程式の左辺の行列を$S$としてこの後使う。
しかし、上の6つの関係式は4つのベクトルから形成されておりランクは高々4なので、$x$を求めるには3回以上測定を行い、上の関係式を2セット以上用意する必要がある。
複数の$S$を縦に並べた行列を$T$とすると、測定が無限の精度で行われていれば$T$のランクは高々6である。
満たすべき線形方程式は$T x = 0$の形のため、$T$の零空間を求め、零空間の中で単位デュアルクォータニオンの性質$x x^*=1$を満たす$x$を求めれば良い。
よって、次はrank revealing QR分解や特異値分解を使って$T$の零空間の基底ベクトル2つを求める。これらの基底ベクトルは特異値分解の場合7番目、8番目の特異値に対応するため$v_7, v_8$と呼ぶことにする。これらを用いると、
x = \lambda_1 v_7 + \lambda_2 v_8
と$x$を表すことができ、$\lambda_1$、$\lambda_2$を条件$x x^*=1$から求めれば良い。
\begin{align}
v_7 &= u_1 + \epsilon v_1 \\
v_8 &= u_2 + \epsilon v_2
\end{align}
と置くと、実部と二重数部の条件は
\begin{align}
||\lambda_1 u_1 + \lambda_2 u2||^2 = 1 \\
(\lambda_1 u_1 + \lambda_2 u_2) \cdot (\lambda_1 v_1 + \lambda_2 v_2) = 0
\end{align}
となる。$\lambda_1 = s \lambda_2$と置くと
\begin{align}
\lambda_2^2 (s^2 u_1 \cdot u_1 + 2 s u_1 \cdot u_2 + u_2 \cdot u_2) = 1 \\
\lambda_2^2 (s^2 u_1 \cdot v_1 + s (u_1 \cdot v_2 + u_2 \cdot v_1) + u_2 \cdot v_2) = 0
\end{align}
下の式から$s$について解いて、得られた2つの解を上の式の$()$内に代入、大きい方を採用することで、意味のある解$\lambda_1, \lambda_2$を得ることができる。
ディスカッション
この方法の良さは、$AX=XB$から必要な成分だけ取り出すことでノイズに強くなり精度が高まること、線形な処理だけで構成されているため計算が速いことだと思われます。文献[5]のConclusionや被引用文献を見ると、多関節ロボットのハンドアイキャリブレーション以外にも活用の道はあるようで、移動ロボットのIMUとカメラのオフセット算出にも応用されているっぽいです。
また、Conclusionによると3D line setsのレジストレーションへの応用が可能です。参考文献[3]によるとPlücker coordinatesで表した直線$l=(\mathbf{l}, \mathbf{m})$の座標変換はデュアルクォータニオンを用いて下記のように計算できます。
l_a = \sigma l_b \sigma^*
これは$AX=XB$と同じ形なので、ハンドアイキャリブレーションと同様の計算を行えば、直線を2つ以上マーカとして使うことで2視点のレジストレーションが可能となります。
参考文献
一般論・補間
[1] Applications of Dual Quaternions in Three Dimensional Transformation and Interpolation
[2] Dual-Quaternions From Classical Mechanics to Computer Graphics and Beyond
[3] Dual Quaternion
[4] Interpolation of Rigid Motions in 3D
ハンドアイキャリブレーション
[5] Hand-Eye Calibration Using Dual Quaternions
スキニング
日本語
[6] デュアルクォータニオン徹底解説
[7] デュアルクォータニオンの対数/指数/sclerp
[8] DualQuaternionのテスト
英語
[9] Geometric Skinning with Approximate Dual Quaternion Blending