単位二重四元数が表す剛体変換を行列に変換

はじめに

単位二重四元数が表す剛体変換を行列に変換することを考えます。
剛体変換には、「回転後、平行移動」と「平行移動後、回転」の2通りがありました。
それぞれで2通りの方法で行列に変換し、それが一致することを示そうと思います。なので、4通りの導出を以下に記載します。

登場する数

登場する数をまとめておきます。
剛体変換を表す単位二重四元数を $\hat{q}=(a,b)\in\hat{\mathbb H}$ とします。
$a=(w_a,{\bf v}_a)=(w_a,(x_a,y_a,z_a)),b=(w_b,{\bf v}_b)=(w_b,(x_b,y_b,z_b))\in{\mathbb H},{\bf v}_a,{\bf v}_b\in{\mathbb R}^3,w_a,x_a,y_a,z_a,w_b,x_b,y_b,z_b\in{\mathbb R}$ です。
回転を表す単位四元数を $\hat{r}\in{\mathbb H}$ とします。
平行移動を表す四元数を $\hat{t}=(0,{\bf t})\in{\mathbb H}$ とします。 ${\bf t}=(t_x,t_y,t_z)\in{\mathbb R}^3,t_z,t_y,t_z\in{\mathbb R}$ です。

剛体変換を「回転後、平行移動」すると見立てた場合

回転後、平行移動すると見立てた場合、 $r,t$ を使って、 $\hat{q}$ は以下のようにも書けます。

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

このとき、 $r,t$ は $a,b$ を使って、以下のように表せます。

$\begin{eqnarray} \left\{ \begin{array}{l} r=a\\ t=2ba^* \end{array} \right.\tag{2} \end{eqnarray}$

また、 $\hat{p}=(1,p)\in\hat{\mathbb H}\ (p\in{\mathbb H})$ を $\hat{q}$ で変換すると、以下のようになります。

$\begin{eqnarray} \hat{q}\hat{p}\overline{\hat{q}^*}=(1,rpr^*+t)\tag{3} \end{eqnarray}$

$(3)$ の導出については、二重四元数による剛体変換の記事を参照してください。

$t$ を $a,b$ の成分を使って表します。

$\begin{eqnarray} t&=&2ba^*\\ &=&2(w_b,{\bf v}_b)(w_a,{\bf v}_a)^*\\ &=&2(w_b,{\bf v}_b)(w_a,-{\bf v}_a)\\ &=&2(w_bw_a-{\bf v}_b^\top(-{\bf v}_a),w_b(-{\bf v}_a)+w_a{\bf v}_b+{\bf v}_b\times(-{\bf v}_a))\\ &=&2(w_aw_b+{\bf v}_a^\top{\bf v}_b,w_a{\bf v}_b-w_b{\bf v}_a+{\bf v}_a\times{\bf v}_b)\\ &=&2(0,w_a{\bf v}_b-w_b{\bf v}_a+{\bf v}_a\times{\bf v}_b)\tag{4} \end{eqnarray}$

$(4)$ に対応する平行移動ベクトルは、以下のようになります。

$\begin{eqnarray} &&2\begin{pmatrix} \left(w_a{\bf v}_b-w_b{\bf v}_a+{\bf v}_a\times{\bf v}_b\right)_x\\ \left(w_a{\bf v}_b-w_b{\bf v}_a+{\bf v}_a\times{\bf v}_b\right)_y\\ \left(w_a{\bf v}_b-w_b{\bf v}_a+{\bf v}_a\times{\bf v}_b\right)_z \end{pmatrix}\\ &=& \begin{pmatrix} 2(w_ax_b-w_bx_a+y_az_b-y_bz_a)\\ 2(w_ay_b-w_by_a+z_ax_b-z_bx_a)\\ 2(w_az_b-w_bz_a+x_ay_b-x_by_a) \end{pmatrix}\tag{5} \end{eqnarray}$

$(5)$ より、平行移動に対する行列は以下のようになります。

$\begin{eqnarray} \begin{pmatrix} 1 & 0 & 0 & 2(w_ax_b-w_bx_a+y_az_b-y_bz_a)\\ 0 & 1 & 0 & 2(w_ay_b-w_by_a+z_ax_b-z_bx_a)\\ 0 & 0 & 1 & 2(w_az_b-w_bz_a+x_ay_b-x_by_a)\tag{6} \end{pmatrix} \end{eqnarray}$

$(6)$ と $r$ に対応する回転行列を掛けたものが求める行列です。( $r$ に対応する回転行列については、四元数による回転の記事を参照してください。)

$\begin{eqnarray} && \begin{pmatrix} 1 & 0 & 0 & 2(w_ax_b-w_bx_a+y_az_b-y_bz_a)\\ 0 & 1 & 0 & 2(w_ay_b-w_by_a+z_ax_b-z_bx_a)\\ 0 & 0 & 1 & 2(w_az_b-w_bz_a+x_ay_b-x_by_a)\\ 0 & 0 & 0 & 1 \end{pmatrix} \begin{pmatrix} 1-2y_a^2-2z_a^2 & 2x_ay_a-2w_az_a & 2x_az_a+2w_ay_a & 0\\ 2x_ay_a+2w_az_a & 1-2x_a^2-2z_a^2 & 2y_az_a-2w_ax_a & 0\\ 2x_az_a-2w_ay_a & 2y_az_a+2w_ax_a & 1-2x_a^2-2y_a^2 & 0\\ 0 & 0 & 0 & 1 \end{pmatrix}\\ &=& \begin{pmatrix} 1-2y_a^2-2z_a^2 & 2x_ay_a-2w_az_a & 2x_az_a+2w_ay_a & 2(w_ax_b-w_bx_a+y_az_b-y_bz_a)\\ 2x_ay_a+2w_az_a & 1-2x_a^2-2z_a^2 & 2y_az_a-2w_ax_a & 2(w_ay_b-w_by_a+z_ax_b-z_bx_a)\\ 2x_az_a-2w_ay_a & 2y_az_a+2w_ax_a & 1-2x_a^2-2y_a^2 & 2(w_az_b-w_bz_a+x_ay_b-x_by_a)\\ 0 & 0 & 0 & 1 \end{pmatrix}\tag{7} \end{eqnarray}$

$(7)$ で行列は求まったのですが、もう1つ別の手法で求めてみます。
$(3)$ を式変形します。

$\begin{eqnarray} \hat{q}\hat{p}\overline{\hat{q}^*}&=&(1,rpr^*+t)\\ &=&(1,rpr^*+rr^*trr^*)\\ &=&(1,r(p+r^*tr)r^*\tag{8} \end{eqnarray}$

$(8)$ は $r^*tr$ だけ先に平行移動して、その後 $r$ で回転するという意味でもあります。
$r^*tr$ を計算します。(下で $rtr^*$ を計算しておりそれと同じ感じで計算できると思うので、ここは計算を省略します。)

$\begin{eqnarray} r^*tr=\left(0,w_a{\bf v}_b-w_b{\bf v}_a-{\bf v}_a\times{\bf v}_b\right)\tag{9} \end{eqnarray}$

あとは $(5)$ から $(7)$ までの計算を行うと、同じ計算結果 $(7)$ が求まります。
このときに、先に平行移動するので平行移動行列は右側で、回転行列が左側であることに注意してください。

剛体変換を「平行移動後、回転」すると見立てた場合

平行移動後、回転すると見立てた場合、 $r,t$ を使って、 $\hat{q}$ は以下のようにも書けます。

$\begin{eqnarray} \hat{q}=\left(r,\dfrac{1}{2}rt\right)\tag{10} \end{eqnarray}$

このとき、 $r,t$ は $a,b$ を使って、以下のように表せます。

$\begin{eqnarray} \left\{ \begin{array}{l} r=a\\ t=2a^*b \end{array} \right.\tag{11} \end{eqnarray}$

また、 $\hat{p}=(1,p)\in\hat{\mathbb H}\ (p\in{\mathbb H})$ を $\hat{q}$ で変換すると、以下のようになります。

$\begin{eqnarray} \hat{q}\hat{p}\overline{\hat{q}^*}=(1,r(p+t)r^*)=(1,rpr^*+rtr^*)\tag{12} \end{eqnarray}$

$(12)$ の導出については、二重四元数による剛体変換の記事を参照してください。

$t$ を $a,b$ の成分を使って表します。

$\begin{eqnarray} t&=&2a^*b\\ &=&2(w_a,{\bf v}_a)^*(w_b,{\bf v}_b)\\ &=&2(w_a,-{\bf v}_a)(w_b,{\bf v}_b)\\ &=&2(w_aw_b-(-{\bf v}_a)^\top{\bf v}_b,w_a{\bf v}_b+w_b(-{\bf v}_a)+(-{\bf v}_a)\times{\bf v}_b)\\ &=&2(w_aw_b+{\bf v}_a^\top{\bf v}_b,w_a{\bf v}_b-w_b{\bf v}_a-{\bf v}_a\times{\bf v}_b)\\ &=&2(0,\underbrace{w_a{\bf v}_b-w_b{\bf v}_a-{\bf v}_a\times{\bf v}_b}_{:={\bf v}_t})\\ &=&2(0,{\bf v}_t)\tag{13} \end{eqnarray}$

${\bf v}_a^\top{\bf v}_t$ を計算します。

$\begin{eqnarray} {\bf v}_a^\top{\bf v}_t&=&{\bf v}_a^\top\left(w_a{\bf v}_b-w_b{\bf v}_a-{\bf v}_a\times{\bf v}_b\right)\\ &=&w_a\underbrace{\left({\bf v}_a^\top{\bf v}_b\right)}_{=-w_aw_b}-w_b\underbrace{\|{\bf v}_a\|^2}_{1-w_a^2}-\underbrace{{\bf v}_a^\top\left({\bf v}_a\times{\bf v}_b\right)}_{=0}\\ &=&w_a^2w_b-w_b+w_a^2w_b\\ &=&-w_b\tag{14} \end{eqnarray}$

${\bf v}_a\times{\bf v}_t$ を計算します。

$\begin{eqnarray} {\bf v}_a\times{\bf v}_t&=&{\bf v}_a\times\left(w_a{\bf v}_b-w_b{\bf v}_a-{\bf v}_a\times{\bf v}_b\right)\\ &=&w_a\left({\bf v}_a\times{\bf v}_b\right)-{\bf v}_a\times\left({\bf v}_a\times{\bf v}_b\right)\\ &=&w_a\left({\bf v}_a\times{\bf v}_b\right)-\left({\bf v}_a^\top{\bf v}_b\right){\bf v}_a+\|{\bf v}_a\|^2{\bf v}_b\\ &=&w_a\left({\bf v}_a\times{\bf v}_b\right)+w_aw_b{\bf v}_a+\|{\bf v}_a\|^2{\bf v}_b\tag{15} \end{eqnarray}$

${\bf v}_a\times\left({\bf v}_a\times{\bf v}_b\right)$ を計算します。

$\begin{eqnarray} {\bf v}_a\times\left({\bf v}_a\times{\bf v}_b\right)&=&\left({\bf v}_a^\top{\bf v}_b\right){\bf v}_a-\|{\bf v}_a\|^2{\bf v}_b\\ &=&-w_aw_b{\bf v}_a-\|{\bf v}_a\|^2{\bf v}_b\tag{16} \end{eqnarray}$

$rtr^*$ を計算します。

$\begin{eqnarray} rtr^*&=&a\left(2(0,{\bf v}_t)\right)a^*\\ &=&2(w_a,{\bf v}_a)(0,{\bf v}_t)(w_a,-{\bf v}_a)\\ &=&2(-{\bf v}_a^\top{\bf v}_t,w_a{\bf v}_t+{\bf v}_a\times{\bf v}_t)(w_a,-{\bf v}_a)\\ &=&2\left(\left(-{\bf v}_a^\top{\bf v}_t\right)w_a-\left(w_a{\bf v}_t+{\bf v}_a\times{\bf v}_t\right)^\top\left(-{\bf v}_a\right),\left(-{\bf v}_a^\top{\bf v}_t\right)\left(-{\bf v}_a\right)+w_a\left(w_a{\bf v}_t+{\bf v}_a\times{\bf v}_t\right)+\left(w_a{\bf v}_t+{\bf v}_a\times{\bf v}_t\right)\times\left(-{\bf v}_a\right)\right)\\ &=&2\left(-w_a{\bf v}_a^\top{\bf v}_t+w_a{\bf v}_t^\top{\bf v}_a,\left({\bf v}_a^\top{\bf v}_t\right){\bf v}_a+w_a^2{\bf v}_t+w_a\left({\bf v}_a\times{\bf v}_t\right)+w_a\left({\bf v}_a\times{\bf v}_t\right)+{\bf v}_a\times\left({\bf v}_a\times{\bf v}_t\right)\right)\\ &=&2\big(0,\underbrace{\left({\bf v}_a^\top{\bf v}_t\right){\bf v}_a}_{=:A}+\underbrace{w_a^2{\bf v}_t}_{=:B}+\underbrace{2w_a\left({\bf v}_a\times{\bf v}_t\right)}_{=:C}+\underbrace{{\bf v}_a\times\left({\bf v}_a\times{\bf v}_t\right)}_{=:D}\big)\tag{17} \end{eqnarray}$

$A$ を計算します。

$\begin{eqnarray} A&:=&(\underbrace{{\bf v}_a^\top{\bf v}_t}_{(12)}){\bf v}_a\\ &=&-w_b{\bf v}_a\\ &=&\underbrace{(w_a^2+\|{\bf v}_a\|^2)}_{=1}(-w_b{\bf v}_a)\tag{18} \end{eqnarray}$

$B$ を計算します。

$\begin{eqnarray} B&:=&w_a^2{\bf v}_t\\ &=&w_a^2\left(w_a{\bf v}_b-w_b{\bf v}_a-{\bf v}_a\times{\bf v}_b\right)\tag{19} \end{eqnarray}$

$C$ を計算します。

$\begin{eqnarray} C&:=&2w_a\left({\bf v}_a\times{\bf v}_t\right)\\ &=&2w_a\left(w_a\left({\bf v}_a\times{\bf v}_b\right)+w_aw_b{\bf v}_a+\|{\bf v}_a\|^2{\bf v}_b\right)\\ &=&w_a^2\left(2\left({\bf v}_a\times{\bf v}_b\right)+2w_b{\bf v}_a\right)+\|{\bf v}_a\|^2\cdot2w_a{\bf v}_b\tag{20} \end{eqnarray}$

$D$ を計算します。

$\begin{eqnarray} D&:=&{\bf v}_a\times\left({\bf v}_a\times{\bf v}_t\right)\\ &=&{\bf v}_a\times\left(w_a\left({\bf v}_a\times{\bf v}_b\right)+w_aw_b{\bf v}_a+\|{\bf v}_a\|^2{\bf v}_b\right)\\ &=&w_a{\bf v}_a\times\left({\bf v}_a\times{\bf v}_b\right)+\|{\bf v}_a\|^2\left({\bf v}_a\times{\bf v}_b\right)\\ &=&-w_a^2w_b{\bf v}_a-\|{\bf v}_a\|^2w_a{\bf v}_b+\|{\bf v}_a\|^2\left({\bf v}_a\times{\bf v}_b\right)\tag{21} \end{eqnarray}$

$(18),(19),(20),(21)$ を $(17)$ に代入します。

$\begin{eqnarray} rtr^*&=&\left(0,(w_a^2+\|{\bf v}_a\|^2)(-w_b{\bf v}_a)+w_a^2\left(w_a{\bf v}_b-w_b{\bf v}_a-{\bf v}_a\times{\bf v}_b\right)+w_a^2\left(2\left({\bf v}_a\times{\bf v}_b\right)+2w_b{\bf v}_a\right)+\|{\bf v}_a\|^2\cdot2w_a{\bf v}_b-w_a^2w_b{\bf v}_a-\|{\bf v}_a\|^2w_a{\bf v}_b+\|{\bf v}_a\|^2\left({\bf v}_a\times{\bf v}_b\right)\right)\\ &=&\big(0,\underbrace{(w_a^2+\|{\bf v}_a\|^2)}_{=1}(w_a{\bf v}_b-w_b{\bf v}_a+{\bf v}_a\times{\bf v}_b)\big)\\ &=&\left(0,w_a{\bf v}_b-w_b{\bf v}_a+{\bf v}_a\times{\bf v}_b\right)\tag{22} \end{eqnarray}$

$(22)$ は $(4)$ と一致します。
よって、後は「回転後、平行移動」すると見立てた場合と同じです。

ここで、式 $(12)$ をもう一度見てみます。

$\begin{eqnarray} \hat{q}\hat{p}\overline{\hat{q}^*}=(1,r(p+t)r^*)\tag{23} \end{eqnarray}$

$(23)$ は $t$ だけ先に平行移動して、その後 $r$ で回転するという意味でもあります。
$t$ はすでに $(13)$ で計算済みです。
あとは $(5)$ から $(7)$ までの計算を行うと、同じ計算結果 $(7)$ が求まります。
このときに、先に平行移動するので平行移動行列は右側で、回転行列が左側であることに注意してください。

SymPyで検算

「回転後、平行移動」すると見立てた場合と「平行移動後、回転」すると見立てた場合で計算結果は同じになりましたが、
念のためSymPyで検算もしておきました。
4通り計算しています。
代入メソッドsubsや因数分解メソッドfactorの使い方が良くないかもしれません。(今回式をキレイにするのに要素ごとに、因数分解して代入とやらなければうまくいきませんでした。)

from sympy.algebras.quaternion import Quaternion
from sympy import *
init_printing()

def get_translate_matrix(t):
    m = simplify(t).to_Matrix()
    m[0] = factor(m[0]).subs(a_dot_b, 0)
    m[1] = factor(m[1]).subs(a_norm2, 1)
    m[1] = factor(m[1]).subs(a_dot_b, 0)
    m[2] = factor(m[2]).subs(a_norm2, 1)
    m[2] = factor(m[2]).subs(a_dot_b, 0)
    m[3] = factor(m[3]).subs(a_norm2, 1)
    m[3] = factor(m[3]).subs(a_dot_b, 0)
    nm = Matrix([[1, 0, 0], [0, 1, 0], [0, 0, 1], [0, 0, 0]])
    nm = nm.col_insert(3, Matrix([m[1], m[2], m[3], 1])) 
    nm = simplify(nm)
    return nm

def refine_matrix(m):
    m0 = m[0].subs(w_a ** 2 + x_a ** 2, 1 - y_a ** 2- z_a ** 2)
    m5 = m[5].subs(w_a ** 2 + y_a ** 2, 1 - x_a ** 2- z_a ** 2)
    m10 = m[10].subs(w_a ** 2 + z_a ** 2, 1 - x_a ** 2- y_a ** 2)
    m3 = factor(m[3]).subs(a_norm2, 1)
    m3 = factor(m3)
    m7 = factor(m[7]).subs(a_norm2, 1)
    m7 = factor(m7)
    m11 = factor(m[11]).subs(a_norm2, 1)
    m11 = factor(m11)
    return Matrix([
        [m0, m[1], m[2], m3],
        [m[4], m5, m[6], m7],
        [m[8], m[9], m10, m11],
        [m[12], m[13], m[14], m[15]],
    ])

var('w_a,x_a,y_a,z_a,w_b,x_b,y_b,z_b')
a_norm2 = w_a ** 2 + x_a ** 2 + y_a ** 2 + z_a ** 2
a_dot_b = w_a * w_b + x_a * x_b + y_a * y_b + z_a * z_b
a = Quaternion(w_a, x_a, y_a, z_a)
b = Quaternion(w_b, x_b, y_b, z_b)
ac = a.conjugate()
r = a
rc = ac

# get rotation matrix
m_r = r.to_rotation_matrix()
m_r = m_r.subs(a_norm2, 1)
m_r = m_r.row_insert(3, Matrix([[0, 0, 0]])) 
m_r = m_r.col_insert(3, Matrix([0, 0, 0, 1]))

# rotate and translate
t = 2 * b * ac
rctr = rc * t * r
m_t = get_translate_matrix(t)
m_rctr = get_translate_matrix(rctr)
display('rotate and translate(m_t * m_r)', refine_matrix(m_t * m_r))
display('rotate and translate(m_r * m_rctr)', refine_matrix(m_r * m_rctr))

# translate and rotate
t = 2 * ac * b
rtrc = r * t * rc
m_t = get_translate_matrix(t)
m_rtrc = get_translate_matrix(rtrc)
display('translate and rotate(m_r * m_t)', refine_matrix(m_r * m_t))
display('translate and rotate(m_rtrc * m_r)', refine_matrix(m_rtrc * m_r))

実行結果

機械学習基礎理論独習

誤りがあればご指摘いただけると幸いです。数式が整うまで少し時間かかります。リンクフリーです。