二重四元数を使ったScLerp

ScLerpとは

英語で「Screw Linear Interpolation」です。
日本語に無理やり訳すと「ねじ線形補間」とか「らせん線形補間」とかになるんですかね。
本記事では、そのまま ScLerp を書くことにします。

ねじパラメータを二重四元数に変換

本記事は、剛体変換をねじパラメータで表すの続きですので、使用する数をまずは書きだします。
剛体変換を表す行列： ${\bf M}\in{\mathbb R}^{4\times4}$
剛体変換の回転を表す行列： ${\bf R}\in{\mathbb R}^{3\times3}$
剛体変換の平行移動を表すベクトル： ${\bf t}\in{\mathbb R}^3$
回転行列 $\bf R$ から求めた原点を通る回転軸を表す単位ベクトル： ${\bf l}\in{\mathbb R}^3,\ \|{\bf l}\|=1$
回転行列 $\bf R$ から求めた回転角： $\theta\in(0,2\pi)\subset{\mathbb R}$
回転軸に沿った平行移動の符号付距離： $d\in{\mathbb R},\ d={\bf t}^\top{\bf l}$
原点から回転軸への垂線の足の位置ベクトル： ${\bf c}\in{\mathbb R}^3$
回転軸のPlucker座標の第二成分： ${\bf m}\in{\mathbb R}^3,\ {\bf m}={\bf c}\times{\bf l}$

剛体変換行列 ${\bf M}$ をねじパラメータ ${\bf l},{\bf m},\theta,d$ にした後の話です。
ねじパラメータを二重四元数に変換します。

まず、あとの式変形のために ${\bf m}$ に関する式を変形しておきます。
${\bf m}$ は以下のように表されるのでした。

$\begin{eqnarray} {\bf m}=\dfrac{1}{2}\left({\bf t}\times{\bf l}+\left({\bf t}-d{\bf l}\right)\cot\frac{\theta}{2}\right)\tag{1} \end{eqnarray}$

式 $(1)$ の両辺に $\sin\dfrac{\theta}{2}\not=0$ を掛けて変形します。

$\begin{eqnarray} &&\sin\dfrac{\theta}{2}{\bf m}=\dfrac{1}{2}\sin\dfrac{\theta}{2}\left({\bf t}\times{\bf l}+\left({\bf t}-d{\bf l}\right)\cot\frac{\theta}{2}\right)\\ &\Leftrightarrow&\sin\dfrac{\theta}{2}{\bf m}+\dfrac{d}{2}\cos\dfrac{\theta}{2}{\bf l}=\dfrac{1}{2}\left(\sin\dfrac{\theta}{2}{\bf t}\times{\bf l}+\cos\frac{\theta}{2}{\bf t}\right)\tag{2} \end{eqnarray}$

やっと準備が整いましたので、やっていきましょう。
回転変換は四元数で $r=\left(\cos\dfrac{\theta}{2},\sin\dfrac{\theta}{2}{\bf l}\right)\in{\mathbb H}$ と表し、二重四元数で $\hat{r}=(r,0)\in\hat{\mathbb H}$ と表します。
平行移動変換は四元数で $t=(0,{\bf t})\in{\mathbb H}$ と表し、二重四元数で $\hat{t}=\left(1,\dfrac{1}{2}t\right)\in\hat{\mathbb H}$ と表します。
回転後に平行移動する二重四元数を $\hat{\sigma}\in\hat{\mathbb H}$ とすると、以下のように表せます。

$\begin{eqnarray} \hat{\sigma}=\hat{t}\hat{r}=\left(1,\dfrac{1}{2}t\right)(r,0)=\left(r,\dfrac{1}{2}tr\right)\tag{3} \end{eqnarray}$

$\hat{\sigma}$ の虚部 $\dfrac{1}{2}tr$ を計算します。

$\begin{eqnarray} \dfrac{1}{2}tr&=&\dfrac{1}{2}(0,{\bf t})\left(\cos\dfrac{\theta}{2},\sin\dfrac{\theta}{2}{\bf l}\right)\\ &=&\dfrac{1}{2}\Bigg(-\underbrace{({\bf t}^\top{\bf l})}_{=d}\sin\dfrac{\theta}{2},\ \cos\dfrac{\theta}{2}+\sin\dfrac{\theta}{2}{\bf t}\times{\bf l}\Bigg)\\ &=&\Bigg(-\dfrac{d}{2}\sin\dfrac{\theta}{2},\ \underbrace{\dfrac{1}{2}\big(\sin\dfrac{\theta}{2}{\bf t}\times{\bf l}+\cos\dfrac{\theta}{2}\big)}_{=(2)}\Bigg)\\ &=&\left(-\dfrac{d}{2}\sin\dfrac{\theta}{2},\ \sin\dfrac{\theta}{2}{\bf m}+\dfrac{d}{2}\cos\dfrac{\theta}{2}{\bf l}\right)\tag{4} \end{eqnarray}$

式 $(4)$ より、 $\hat{\sigma}$ は以下のように書けます。

$\begin{eqnarray} \hat{\sigma}&=&\left(r,\dfrac{1}{2}tr\right)\\ &=&\left(\left(\cos\dfrac{\theta}{2},\sin\dfrac{\theta}{2}{\bf l}\right),\ \left(-\dfrac{d}{2}\sin\dfrac{\theta}{2},\ \sin\dfrac{\theta}{2}{\bf m}+\dfrac{d}{2}\cos\dfrac{\theta}{2}{\bf l}\right)\right)\tag{5} \end{eqnarray}$

これまで四元数を(スカラー部、ベクトル部)、二重数を(実部、虚部)という風に丸括弧を使った記法で書いてきましたが
式 $(5)$ をスカラー部とベクトル部に分けて表示したいので、+を使った記法で二重四元数を表示したいと思います。

$\begin{eqnarray} \hat{\sigma}&=&\cos\dfrac{\theta}{2}+\sin\dfrac{\theta}{2}{\bf l}+\varepsilon \left(-\dfrac{d}{2}\sin\dfrac{\theta}{2}+\sin\dfrac{\theta}{2}{\bf m}+\dfrac{d}{2}\cos\dfrac{\theta}{2}{\bf l}\right)\\ &=&\left(\cos\dfrac{\theta}{2}-\varepsilon\dfrac{d}{2}\sin\dfrac{\theta}{2}\right)+\left(\sin\dfrac{\theta}{2}{\bf l}+\varepsilon\left(\sin\dfrac{\theta}{2}{\bf m}+\dfrac{d}{2}\cos\dfrac{\theta}{2}{\bf l}\right)\right)\\ &=&\cos\dfrac{\theta+\varepsilon d}{2}+\sin\dfrac{\theta+\varepsilon d}{2}({\bf l}+\varepsilon{\bf m})\tag{6} \end{eqnarray}$

式 $(6)$ の変形で以下の、 $\cos,\sin$ のテイラー展開を利用しました。

$\begin{eqnarray} &&\cos\dfrac{\theta+\varepsilon d}{2}=\cos\dfrac{\theta}{2}-\varepsilon\dfrac{d}{2}\sin\dfrac{\theta}{2}\tag{7}\\ &&\sin\dfrac{\theta+\varepsilon d}{2}=\sin\dfrac{\theta}{2}+\varepsilon\dfrac{d}{2}\cos\dfrac{\theta}{2}\tag{8} \end{eqnarray}$

式 $(6)$ の導出でここでやるべきことはすでに終わっているんですが、後で使うために、 $\hat\sigma$ の累乗を $\tau>0$ を使って、定義しておきます。

$\begin{eqnarray} \hat{\sigma}^\tau&=&\cos\dfrac{\tau(\theta+\varepsilon d)}{2}+\sin\dfrac{\tau(\theta+\varepsilon d)}{2}({\bf l}+\varepsilon{\bf m})\tag{9} \end{eqnarray}$

式 $(9)$ の右辺を見てみると、 $\theta$ と $d$ のみが $\tau$ 倍されていることがわかります。

ここでやることは、
剛体変換を表す2つの単位二重四元数 $\hat{\sigma}_1,\hat{\sigma}_2\in\hat{\mathbb H}$ とパラメータ $t\in[0,1]$ を使って、ねじのように補間した単位二重四元数 $\hat{\sigma}(t)\in\hat{\mathbb H}$ を求めることです。
$\hat{\sigma}_1$ から $\hat{\sigma}_2$ への変換は $\hat{\sigma}_1^{-1}\hat{\sigma}_2$ と書けます。
よって、求める $\hat{\sigma}(t)$ は以下のようになります。

$\begin{eqnarray} \hat{\sigma}(t)=\hat{\sigma}_1(\hat{\sigma}_1^{-1}\hat{\sigma}_2)^t\tag{10} \end{eqnarray}$

式 $(10)$ は $t=0$ の時 $\hat{\sigma}(t=0)=\hat{\sigma}_1$ になっており、 $t=1$ の時 $\hat{\sigma}(t=1)=\hat{\sigma}_2$ になっていることが分かるかと思います。

$\hat{\sigma}_1^{-1}$ は剛体変換なので、 $\hat{\sigma}_1^{-1}\hat{\sigma}_2$ は剛体変換を表します。
$\hat{\sigma}_1^{-1}\hat{\sigma}_2$ が表す剛体変換から求めたねじパラメータを ${\bf l}',{\bf m}',\theta',d'$ として、式 $(10)$ を書き直します。

$\begin{eqnarray} \hat{\sigma}(t) &=&\hat{\sigma}_1\left(\cos\dfrac{t(\theta'+\varepsilon d')}{2}+\sin\dfrac{t(\theta'+\varepsilon d')}{2}({\bf l}'+\varepsilon{\bf m}')\right)\\ &=&\hat{\sigma}_1\left(\cos\dfrac{t\theta'}{2}+\sin\dfrac{t\theta'}{2}{\bf l}'+\varepsilon \left(-\dfrac{td'}{2}\sin\dfrac{t\theta'}{2}+\sin\dfrac{t\theta'}{2}{\bf m}'+\dfrac{td'}{2}\cos\dfrac{t\theta'}{2}{\bf l}'\right)\right)\\ &=&\hat{\sigma}_1\left(\left(\cos\dfrac{t\theta'}{2},\sin\dfrac{t\theta'}{2}{\bf l}'\right),\left(-\dfrac{td'}{2}\sin\dfrac{t\theta'}{2},\sin\dfrac{t\theta'}{2}{\bf m}'+\dfrac{td'}{2}\cos\dfrac{t\theta'}{2}{\bf l}'\right)\right)\tag{11} \end{eqnarray}$

式 $(11)$ より、 $\hat{\sigma}(t)$ が求まりました。

最後に

四元数を(スカラー部、ベクトル部)、二重数を(実部、虚部)という風に書いていましたが、
本記事の内容ではこの書き方では限界があると感じて、途中から+を使って記法で書きました。
本記事より、はっきり言ってDual Quaternion読んだ方が良いです。
$\hat{\sigma}(t)=\hat{\sigma}_1(\hat{\sigma}_1^{-1}\hat{\sigma}_2)^t$ としたけど、 $\hat{\sigma}(t)=(\hat{\sigma}_2\hat{\sigma}_1^{-1})^t\hat{\sigma}_1$ でも同じ結果になると思う。
これ計算して同じになること確認しようと思ったけど、計算量多そうとか色々な理由で諦めました。誰か証明してください。

参考リンク

二重四元数
 二重四元数による剛体変換
 剛体変換をねじパラメータで表す

参考文献

Dual Quaternion
Hand-Eye Calibration Using Dual Quaternions