書籍転載：KINECT for Windows SDKプログラミング Kinect for Windows v2センサー対応版（10）

クォータニオン ― Kinectで学ぶ数学
― Chapter 8　―　8.5 ―

Device & OSS Kinect 数学

2015年12月22日

全5回にわたる、Kinectで必要となる数学の解説は、今回で完結。最終回では、Kinect SDKでも関節の回転角度として採用されているクォータニオンについて説明する。

高田智広

　Kinectで必要となる数学を説明します。前回はロール・ピッチ・ヨーについての数学を解説しました。本稿はその続きとして、クォータニオンについて説明します。

書籍転載について

　本コーナーは、秀和システム発行の書籍『KINECT for Windows SDKプログラミング Kinect for Windows v2センサー対応版』の中から、特にBuild Insiderの読者に有用だと考えられる項目を編集部が選び、同社の許可を得て転載したものです。

　『KINECT for Windows SDKプログラミング Kinect for Windows v2センサー対応版』の詳細や購入は秀和システムのサイトや目次ページをご覧ください。プログラムのダウンロードも、秀和システムのサイトから行えます。

ご注意

本記事は、書籍の内容を改変することなく、そのまま転載したものです。このため用字用語の統一ルールなどはBuild Insiderのそれとは一致しません。あらかじめご了承ください。

■

8.5　クォータニオン

　いよいよクォータニオン（四元数）と呼ばれる「数」を定義しますが、その前に複素数について復習しておきます。

8.5.1　複素数と複素平面

　複素数とは、大ざっぱに言えば i²= -1 となるような想像上の数 i を実数に付け加え、自然に四則演算ができるようにしたものです。すべての複素数は実数 a,b を使って

と書くことができます。これを仮に

と書くことにすると、複素数は足し算と実数倍ができますから、実数 a₁,a₂,b₁,b₂,k について

(a_1,b_1)+(a_2,b_2)=(a_1+a_2,b_1+b_2)
k(a_1,b_1)=(ka_1,kb_1)

が成り立つことになり、これはまさに2次元ベクトルです。さらに複素数はかけ算もできるので

(a_1+b_1 i)(a_2+b_2 i)=(a_2+b_2 i)(a_1+b_1 i)=a_1 a_2-b_1 b_2+(a_1 b_2+b_1 a_2)i

から

(a_1,b_1)×(a_2,b_2)=(a_2,b_2)×(a_1,b_1)=(a_1 a_2-b_1 b_2,a_1 b_2+b_1 a_2)

ということになります。つまり、複素数とは2次元ベクトルに特殊なかけ算を定義したものだと思うことができます。これは4次元ベクトルにかけ算を定義して2×2行列を作ったのと非常に似ています。実は、

（ a,b は実数）という特別な形の行列のかけ算は

(■(a_1&-b_1@b_1&a_1))(■(a_2&-b_2@b_2&a_2))=(■(a_2&-b_2@b_2&a_2))(■(a_1&-b_1@b_1&a_1))=(■(a_1 a_2-b_1 b_2&-(b_1 a_2+a_1 b_2)@b_1 a_2+a_1 b_2&a_1 a_2-b_1 b_2))

ですから複素数のかけ算の定義とぴったり一致し、この形の行列を複素数だと思うことができます。

　さて、複素数は2次元ベクトルでもありましたから、当然複素数にも幾何学的な性質を持っています。例えば、複素数は下図のように平面上の有向線分として表すことができます。

しかも、その複素数に大きさ 1 の複素数をかけることがベクトルの回転にきれいに対応するのです。実際、上に挙げた特別な形の行列のかけ算

(■(a_1&-b_1@b_1&a_1))(■(a_2&-b_2@b_2&a_2))

において a₁=cosθ, b₁=sinθ とおけば

(■(cosθ&-sinθ@sinθ&cosθ))(■(a_2&-b_2@b_2&a_2))=(■(a_2 cosθ-b_2 sinθ&-a_2 sinθ-b_2 cosθ@a_2 sinθ+b_2 cosθ&a_2 cosθ- b_2 sinθ))

なので、

(■(cosθ&-sinθ@sinθ&cosθ))(■(a_2@b_2))=(■(a_2 cosθ-b_2 sinθ@a_2 sinθ+b_2 cosθ))

のみに注目すればこれはベクトル (a₂,b₂) に回転行列をかけていることと全く同じです。

8.5.2　クォータニオンの定義

　前節で複素数を使うとかけ算で2次元ベクトルの回転が行えることを述べました。3次元ベクトルでも何かの「数」を用いて同じようなことができないか、ということで生み出さされたのがクォータニオン（四元数）です。1843年にクォータニオンがアイルランドの数学者ハミルトンにより発見された（それよりも前の1840年にフランスのロドリグによって本質的に同様のものが発表されているが注目されなかった）のは行列が現れるよりも前のことですから、当時は行列を知っている我々が想像するよりもはるかに大きな意味を持っていました。複素数と同じようにクォータニオンを大ざっぱに定義すれば、実数に i²=j²=k²=-1, ij=k, jk=i, ki=j であるような3種類の想像上の数“i,j,k”を付け加えて自然に四則演算ができるようにしたものです。

定義1

　4つの実数 w,x,y,z の組

であって、次のような3つの演算、和・スカラー倍・積が定義されているものをクォータニオン（四元数）という。

(■(w_1@x_1@y_1@z_1))+(■(w_2@x_2@y_2@z_2))=(■(w_1+w_2@x_1+x_2@y_1+y_2@z_1+z_2))
k(■(w_1@x_1@y_1@z_1))=(■(kw_1@kx_1@ky_1@kz_1))
(■(w_1@x_1@y_1@z_1))×(■(w_2@x_2@y_2@z_2))=(■(w_1 w_2-x_1 x_2-y_1 y_2-z_1 z_2@w_1 x_2+x_1 w_2+y_1 z_2-z_1 y_2@w_1 y_2-x_1 z_2+y_1 w_2+z_1 x_2@w_1 z_2+x_1 y_2-y_1 x_2+z_1 w_2))

なお、積記号の“×”はしばしば省略される。また、クォータニオン (w;x,y,z) に対し

という非負の実数をクォータニオン (w;x,y,z) の大きさという。

　ここでは紙面の都合で縦ベクトル風に書きましたが、多くの場合横ベクトル風に (w;x,y,z) と書かれます。もちろん (w;x,y,z) が「素朴な表記」の w+xi+yj+zk に対応します。一つ目の区切りをカンマではなくセミコロンにしたのは w が実成分であることを分かりやすくするためです。特に、

(0;1,0,0)×(0;0,1,0)=(0;0,0,1)
(0;0,1,0)×(0;1,0,0)=(0;0,0,-1)

ですから、(0;1,0,0)×(0;0,1,0)≠(0;0,1,0)×(0;1,0,0) です。したがって、行列のかけ算と同じくクォータニオンのかけ算は順序を交換することができません。

　後で用いますので、共役クォータニオンもここで定義しておきます。

定義2

　クォータニオン Q=(w;x,y,z) について

を共役クォータニオンと言い、Q̅ と書く。また、

を Q の大きさといい |Q| で表す。

　共役複素数が虚部の正負を逆にしたものでしたから、それに対応する定義となっています。また、

QQ̅=Q̅Q=(w^2+x^2+y^2+z^2;0,0,0)=(|Q|^2 ;0,0,0)

となることにも注意してください。これはちょうど複素数 z について

が成り立つことに対応します。

8.5.3　クォータニオンと3次元空間

　クォータニオンは4次元ベクトルですから、そのままでは3次元空間内のベクトルには対応づけられません。そこで、

という特別な形のクォータニオンのみを3次元空間内のベクトル (x,y,z) に対応づけます。見やすくするために、任意のクォータニオン (w;x,y,z) に対して次のような写像を定義します。

それぞれ実部、虚部という気持ちです。すなわち Q=(0;x,y,z) であるとき Im Q=(x,y,z) です。書くまでもないことかもしれませんが、

(0;x_1,y_1,z_1)+(0;x_1,y_2,z_2)=(0;x_1+x_2,y_1+y_2,z_1+z_2)
k(0;x_1,y_1,z_1)=(0;kx_1,ky_1,kz_1)

が成り立ちますので、ベクトルの足し算やスカラー倍はこの形のクォータニオンの中で問題なく行うことができます。さらに、積について

(■(0@x_1@y_1@z_1))×(■(0@x_2@y_2@z_2))=(■(-x_1 x_2-y_1 y_2 -z_1 z_2@y_1 z_2 -z_1 y_2@-x_1 z_2+z_1 x_2@x_1 y_2 -y_1 x_2))

が成り立ちますから、クォータニオンとしての積を計算することで3次元ベクトルの内積及び外積が一度に計算できます。つまり、ReQ₁=ReQ₂=0であるようなクォータニオン Q₁,Q₂ について

Re(Q_1 Q_2)=-Im Q_1·Im Q_2
Im(Q_1 Q_2)=Im Q_1×Im Q_2

となります。

8.5.4　クォータニオンによる回転

　大きさ 1 のクォータニオン R=(d;a,b,c) およびクォータニオン Q=(0;x,y,z) を考えます。このとき、

RQR̅=(■(0@(d^2+a^2-b^2-c^2)x+(2ab-2cd)y+(2ac+2bd)z@(2ab+2cd)x+(d^2-a^2+b^2-c^2)y+(2bc-2ad)z@(2ac-2bd)x+(2bc+2ad)y+(d^2-a^2-b^2+c^2)z))

が成り立ちます。これを3次元ベクトル

から

Im(RQR̅)=(■((d^2+a^2-b^2-c^2)x+(2ab-2cd)y+(2ac+2bd)z@(2ab+2cd)x+(d^2-a^2+b^2-c^2)y+(2bc-2ad)z@(2ac-2bd)x+(2bc+2ad)y+(d^2-a^2-b^2+c^2)z))

への写像とみると、これは線型写像であって、それを表す行列は

(■(d^2+a^2-b^2-c^2&2ab-2cd&2ac+2bd@2ab+2cd&d^2-a^2+b^2-c^2&2bc-2ad@2ac-2bd&2bc+2ad&d^2-a^2-b^2+c^2))

です。これは d²+a²+b²+c²=1 のとき回転行列になっています。実際、複号（プラスマイナス）はすべて正かすべて負にとるとして

(cos〖θ/2〗,sin〖θ/2〗)=(d,±√(1-d^2)
(u_1,u_2,u_3)=(±〖a/√(1-d^2)〗,±〖b/√(1-d^2)〗,±〖c/√(1-d^2)〗)

であるような θ および u₁,u₂,u₃ について、上の行列は方向 (u₁,u₂,u₃) を回転軸として角度 θ だけ回転する回転行列と一致することが計算により確かめられます。複号が残るのは、方向 (u₁,u₂,u₃) を軸として θ 回転することと、逆方向である -(u₁,u₂,u₃) を軸として -θ 回転することとは回転として全く同じであるからです。これで大きさ 1 のクォータニオン R を使えば任意の3次元ベクトルの回転が表されることが分かりました。

　逆に、上の関係式から方向 (u₁,u₂,u₃) を回転軸として角度 θ だけ回転させるクォータニオンも計算することができ、

(d;a,b,c)=(cos〖θ/2〗;u_1 sin〖θ/2〗,u_2 sin〖θ/2〗,u_3 sin〖θ/2〗)

となります。

8.5.5　座標変換とクォータニオン

　座標変換したとき、すなわち基底ベクトルを変更したときには線型写像を表す行列が変換されることを8.3.3「線型写像による座標変換」で見ました。同じようにクォータニオンも座標変換すれば値が変わります。基底ベクトル e₁,e₂,e₃ を

(■(〖e^'〗_1&〖e^'〗_2&〖e^'〗_3))=(■(e_1&e_2&e_3))T

と変換したとします。このときクォータニオンがどのように変化するか見てみましょう。前節で見たように標準基底のときのクォータニオンを (d;a,b,c) とすると回転行列は

R=(■(d^2+a^2-b^2-c^2&2ab-2cd&2ac+2bd@2ab+2cd&d^2-a^2+b^2-c^2&2bc-2ad@2ac-2bd&2bc+2ad&d^2-a^2-b^2+c^2))

ですから、基底ベクトルを e′₁,e′₂,e′₃ にしたときの回転行列は

となります。したがって、

R^'=(■(〖d^'〗^2+〖a^'〗^2-〖b^'〗^2-〖c^'〗^2&2〖a^'〗〖b^'〗-2〖c^'〗〖d^'〗&2〖a^'〗〖c^'〗+2〖b^'〗〖d^'〗@2〖a^'〗〖b^'〗+2〖c^'〗〖d^'〗&〖d^'〗^2-〖a^'〗^2+〖b^'〗^2-〖c^'〗^2&2〖b^'〗〖c^'〗-2〖a^'〗〖d^'〗@2〖a^'〗〖c^'〗-2〖b^'〗〖d^'〗&2〖b^'〗〖c^'〗+2〖a^'〗〖d^'〗&〖d^'〗^2-〖a^'〗^2-〖b^'〗^2+〖c^'〗^2))=T^-1 RT

であるような (d′;a′,b′,c′) を求めればよいことになります。

　たとえば、Kinect v2のカメラ座標をUnityのワールド座標に変換するにはx座標の符号を反転させればよいので、

となり、

T^-1 RT=(■(d^2+a^2-b^2-c^2&-(2ab-2cd)&-(2ac+2bd)@-(2ab+2cd)&d^2-a^2+b^2-c^2&2bc-2ad@-(2ac-2bd)&2bc+2ad&d^2-a^2-b^2+c^2))

が得られます。方程式 R′=T^-1RT を解けば (d′;a′,b′,c′)=±(d;a,-b,-c) が分かり、Kinect v2のクォータニオンをUnityのクォータニオンに変換するには y 成分と z 成分を反転させればよいことがわかります。

8.5.6　Kinect v2のクォータニオン

　Kinect SDKは、関節の回転角度としてクォータニオンを採用しています。公式ドキュメントによると、ある関節のJointOrientation.Orientationで大きさ 1 のVector4として得られるクォータニオンは、その関節のその親の関節から見た方向を表します。クォータニオンと関連づける3次元座標系をカメラ座標系、すなわちKinectに正対して右を x 軸正の向き、鉛直上向きを y 軸正の向き、後方を z 軸正の向きとした右手系の直交座標系だとします。このとき、クォータニオンに変換したその関節の位置（JointPosition）を P_c、JointOrientation.Orientationで得られたクォータニオンを Q_c、四元数に変換した親関節の位置を P_p とすると次のような式が成り立ちます。

すなわち、Kinect SDKで得られるクォータニオンは親関節から子関節への方向が鉛直上向きであるような向きを標準状態として回転させるクォータニオンです。さらにKinect v2では方向だけでなく関節のひねり（ひいては腕や足の方向のひねり）も認識できるようになりました。どのようなひねりを標準状態にしているかを表すため、下図で「気をつけ」のポーズを取ったときに各関節から伸びた (1,0,0) というベクトルをその関節のクォータニオンで回転させた方向を筆者の独自調査で3種類の矢印で描きましたので参考にしてください。

■

　今回で、転載は完了です。

※以下では、本稿の前後を合わせて5回分（第6回～第10回）のみ表示しています。
　連載の全タイトルを参照するには、［この記事の連載 INDEX］を参照してください。

10. 【現在、表示中】≫ クォータニオン ― Kinectで学ぶ数学

2015年12月22日(火曜日)

書籍転載：KINECT for Windows SDKプログラミング Kinect for Windows v2センサー対応版（10）