［Swift］クォータニオンからオイラー角を計算したい。

Question

iphoneで端末を始点から終点まで動かした際の角度をオイラー角を取得したいと考えています。
当初は直接オイラー角を取得していましたが、ジンバルロックのため、特定の動きで正確な値が得られませんでした。

そこで、クォータニオンからオイラー角を計算したいと考えています。
PythonやC++のコードを参考に、以下のコードを作成してみたのですが、直接取得したオイラー角と、クォータニオンを元に計算したオイラー角の数値が一致しませんでした。
（参考にしたコード：Pythonでクォータニオンをz-y-x系オイラー角に変換するコード書いてみた、Quaternion to Euler angles conversion）

ここ数週間、自分なりに色々と試行錯誤してみたのですが、門外漢なこともあり、全く見通しがつかず困り果てています。
swiftでのクォータニオンからオイラー角への変換コードについて、ご助言願えれば幸いです。

    func startMotionManager(){

// MotionManagerスタート
        motionManager.deviceMotionUpdateInterval = 0.1
        
        motionManager.startDeviceMotionUpdates(
            to: OperationQueue.current!,
            withHandler: {
                
                deviceManager, error in
        
//オイラー角を取得
                let attitude: CMAttitude = deviceManager!.attitude
                self.roll =  attitude.roll
                self.pitch =  attitude.pitch
                self.yaw =  attitude.yaw

//クォータニオンを取得
                let quaternion: CMQuaternion = attitude.quaternion
                self.qw =  quaternion.w
                self.qx =  quaternion.x
                self.qy =  quaternion.y
                self.qz =  quaternion.z
        })

// クォータニオンからオイラー角への変換
//         roll : x軸回転
        let sinr_cosp = 2 * ((qw * qx) + (qy * qz))
        let cosr_cosp = 1 - 2 * ((qx * qx) + (qy * qy))
        roll2 = atan2(sinr_cosp, cosr_cosp)

//         pitch : y軸回転
        let sinp = 2 * ((qw * qy) - (qz * qx))
        if fabs(sinp) >= 1 {    
            pitch2 = copysign(Double.pi / 2, sinp)
        }else{
            pitch2 = asin(sinp)
        }

//         yaw : z軸回転
        let siny_cosp = 2 * ((qw * qz) + (qx * qy))
        let cosy_cosp = 1 - 2 * ((qy * qy) + (qz * qz))
        yaw2 = atan2(siny_cosp, cosy_cosp)
        
    }

得られた数値：
roll: -0.10607059671615462
pitch: 0.025374394903295194
yaw: 0.0013470739423799597

qw: 0.012704718066535153
qx: -0.052997630007723055
qy: 0.0
qz: 0.9985137777995401

roll2: 0.025934772364812272
pitch2: -0.10552734591014738
yaw2: -0.0028793097335274853

######[追記]修正したコードを以下に記載します。

// クォータニオン取得までは上述の通り
// クォータニオンからオイラー角への変換
//pitch
        let sinr_cosp = 2 * (qw * qx + qy * qz)
        let cosr_cosp = 1 - 2 * (qx * qx + qy * qy)
        pitch2 = atan2(sinr_cosp, cosr_cosp)

//roll
        let sinp = 2 * (qw * qy - qz * qx)
        if fabs(sinp) >= 1{
            roll2 = copysign(Double.pi / 2, sinp)
            }else{
                roll2 = asin(sinp)
            }

//yaw
        let siny_cosp = 2 * (qw * qz + qx * qy)
        let cosy_cosp = 1 - 2 * (qy * qy + qz * qz)
        yaw2 = atan2(siny_cosp, cosy_cosp)
        
//ラジアンから度(degree)への変換
        self.currentXAngle = Int(pitch2 * 180 / Double.pi)
        self.currentYAngle = Int(roll2 * 180 / Double.pi)
        self.currentZAngle = Int(yaw2 * 180 / Double.pi)

近くはなりましたが、一致しない値も多いです。
得られた値：
測定1
rad:
roll 1,2= 0.04704940478649479 0.047029632217478634
pitch 1,2= 0.028981734561906995 0.029013823832668956
yaw 1,2= -0.7736049074468174 -0.7722405186353041
degree:
roll 1,2= 1 1
pitch 1,2= 2 2
yaw 1,2= -44 -44
測定2
rad:
roll 1,2= 1.0113515659438153 0.5706119049349081
pitch 1,2= 0.8798014278180685 1.1572053442575996
yaw 1,2= -0.12041428202827054 0.7680234373632876
degree:
roll 1,2= 50 66
pitch 1,2= 57 32
yaw 1,2= -6 44
測定3
rad:
roll 1,2= -2.735484920517521 -0.08937369139539292
pitch 1,2= 1.3428872701500676 1.780730424715864
yaw 1,2= 2.8914778638136567 0.14657073038362464
degree:
roll 1,2= 76 102
pitch 1,2= -156 -5
yaw 1,2= 165 8
測定4
rad:
roll 1,2= -0.3030519733024809 -0.08215438603880901
pitch 1,2= 1.2922329531814778 1.3043213207876034
yaw 1,2= 1.5531307326353834 1.26109598325311
degree:
roll 1,2= 74 74
pitch 1,2= -17 -4
yaw 1,2= 88 72

Comments

[TsukubaDepot]

言語としては Swift ですが、話題としては他の分野にも関係しますので、ご質問のタグを見直されてはいかがでしょうか（ちなみに、先日のご質問ではジンバルロック系の問題だとは思っていたのですが、それほど知識がないのでコメントは控えていました）。

クォータニオンで過去の質問を検索すると、3DCG, C++, Python などがタグで出てきますし、実際C++ や Python についてはコードを参考にされているということなので、全くの的外れではないとおもいます。

むしろ、3DCGとかのタグではこの辺りの計算について深い造詣を持った方が多くいらっしゃるようなので、タグを追加して目に触れるよにした方が良いかもしれません。

[Masa_teratail]

ご助言ありがとうございました。
ご指摘いただきました通り、お詳しい方からお返事がいただけるように関連しそうなタグを追加させていただきました。

[fana]

参考にした場所で "z-y-x系" とか何とか書いてあるように，オイラー角の表現は複数あるわけですが，
ここでの {roll,pitch,yaw} と {roll2,pitch2,yaw2} が全く同じ式で使用される値だという確証はあるのでしょうか？

[TsukubaDepot]

質問者さん:
今回の解決方法は、おそらく数式としては一般化された話だとおもいますので、解決の糸口となるような回答がつくと良いですね。

[Masa_teratail]

fanaさま、
ご返信ありがとうございました。
ご指摘の通り、オイラー角の表現には複数があるかと存じます。
私の理解では、roll = x,pitch = y,yaw = z と認識しており、xyz系のオイラー角が得られていると考えています。

TsukubaDepotさま、
ご助言いただき、ありがとうございました。

[fana]

> 考えています

という部分に，どれだけの確証があるのか（確認が取れているのか）というところが示されないと，
間違っているのが　話なのか実装なのか　という切り分けができないので，そこは明確にされた方が良いのではないかな，と．

例えば，
(A)元々得られている｛roll,ptich,yaw｝を用いて，「考えている表現の数式で」求めた回転行列
(B)得られている四元数から求めた回転行列
の両者が一致していることを実際に確認した
→だから，元々のオイラー角の表現というのはこういう形である．
→ならば，四元数からオイラー角を得るには，この数式を使えばよいハズで…

だとか．

[stdio]

まず、最終目的が何なのかが分かりません。それが分かれば別の案を提出できる可能性があると思います。

[Masa_teratail]

fanaさま、ありがとうございます。
以下のサイトを使い、取得した値をいろいろと比較していたところ、クォータニオンをオイラー角に変換するコードでrollとpirchが入れ替わっていることに気づきました。
「3D Rotation Converter」
　https://www.andre-gaschler.com/rotationconverter/

＊質問とは直接関係ありませんが、変換を繰り返していた際にiOSのオイラー角がZXY系であることも分かりました。
（この方も検証していました。→ https://stackoverflow.com/questions/40128906/euler-angles-to-rotation-matrix-manual-transformation-for-ios-devices）

再度コードを書き直して試したところ、取得したオイラー角とクォータニオンから変換したオイラー角が一致するようになりました。まだ自分でもどの部分が誤っていたのかしっかりと確認できていない状態ですが、ひとまずは第一の問題を解決できたようです。どうもありがとうございました。

stdioさま、
ありがとうございます。
最終目的は、iPhoneで角度を計測することです。
最初はオイラー角そのまま使用していたのですが、ジンバルロックにより特定条件下で正確に測定できないことが分かりました。
この問題を回避する上で、クォータニオンを用いれば始点と終点の差分角度が算出できるのではないかと考えており、今回質問させていただいております。
ただ、この方法も本当に正しいのかは分からず、肝心の差分の算出方法もわかっていない状態です・・。

TsukubaDepotさま、
クォータニオンからオイラー角の変換は解決することができました。ありがとうございました。

[TsukubaDepot]

根本的に解決したのか、それとも偶然なのかは私にはよく判断できませんが、もし「自己解決」ということであれば一度回答本文に解決方法をまとめ、自己回答されてはいかがでしょうか。
もし何か間違いがあれば今度は回答本文で指摘が付きますし、前述の通り言語の分野に関係なく幅広い知識を持った方もいらっしゃるので、また違った解決策がでてくるかもしれません。

なにせこの追記コメント欄は目立たないため他の方が気づかないという問題もありますので、ご検討いただければと思います。

[Masa_teratail]

すみません、測定を何度か繰り返したところ、一致する場合と大きくズレる場合があることが分かり、データを質問欄に追記させていただきました。
そのため、クォータニオンからオイラー角への変換も解決しておらず、不完全な状態のようです・・。

[fana]

（ほんとにもう非常にめんどくさい作業になるでしょうけども）私なら，
どこかのサイトに値を入力してみる　とか　どこかのログでこう言ってるから　とかを土台にするのではなくて，
そろそろ自分で数式を立てて考えます．

オイラー角だとかRPYというのは（私がちゃんと知らないだけだとは思いますが…）
何やら情報元によって説明やら表現が異なっていて，
「オイラー角とRPYは違う話」と言われることもあれば，「RPYはオイラー角に含まれる」みたく言われることもあったりする気がするし，
３回の回転の軸が，「回されるものの側に固定されている」と言われることもあれば，「ワールド座標の軸的な固定の３軸だ」と言われることもある（これは結局，回転適用順序の問題なのだが）．
回転順序だけでなく，回転角もどっちに回すのがプラスでどっちがマイナスなのかも違うかもしれない．
…等々，無意味にめんどくさいです．

どこかを参考にしようにも，参考元の記述なりに関して，こういった事柄がどうなっているのか？を把握しなきゃならない．なのに大抵は明言されてなかったりもする．面倒すぎる．何かを１つでも誤解していると結果が狂うし．

[fana]

> まず、最終目的が何なのかが分かりません

クォータニオンからオイラー角を計算　することが，ジンバルロック起因で生じている何らかの問題への対抗手段になるのか？という点は，私もわからないところです．

[Masa_teratail]

fanaさま、
ご返信ありがとうございました。
今後、ご提案いただいた自分で計算する方法も含めて、検討したいと思います。

また、ジンバルロックの解消法についても、始点と終点のクォータニオンの差分を求め、それをRPYに変換すればよいのではないかと考えたのですが、その辺りの実現可能性についても十分に検討できてない状態です。
ちょっと見通しが立たない状態にはまり込んでいますので、今一度、アプリの基本設計から見直したいと思います。
的確なご助言を賜り、どうもありがとうございました。

Answer 1

クォータニオンからオイラー角を計算することについて

「質問への追記・修正、ベストアンサー選択の依頼」のところにも書きましたが，
ご自身で数式を立てて導出するのが結局は最も手っ取り早いのではないかと思います．

すなわち，

1. まず，自身で用いるオイラー角の表現方法を決定し，
2. その表現方法に見合う計算式を導出する

どこかで見つけた結果の2.を用いようとする場合，
まず，その話に対する1.を把握する必要がありますし，且つ，その表現で使っていかねばならないという枷も生じます．

それよりも，自身で1.を決めて得た2.の方が，楽であろう，と．

自分で求めた角度が直接取得した角度と合わないことについて

このことを問題にする必要はないように思えます．

なぜならば，自分で求めた角度の値を（以降の処理で）上記の1.に基づいて用いればよいからです．

目的が「直接取得した角度を再現すること」なのであれば，合わないことは問題でしょうけども，今は合わせることが目的ではないと見えます．
（完全一致する値を得る手段を用意する意味は無い．だったら最初から直接取得した値をそのまま用いればよいわけで）

Answer 2

get pitch, yaw, roll from a CMRotationMatrix

roll = atan2(2yw - 2xz, 1 - 2yy - 2zz)

pitch = atan2(2xw - 2yz, 1 - 2xx - 2zz)
yaw = asin(2xy + 2zw)

未検証です。

---

肝心の差分の算出方法

姿勢 q_0 から 座標系回転 q_d を経て 姿勢 q_1になったとすると
q_1 = q_0 * q_d
左から q_0^(-1)を掛けて

q_0^(-1) * q_1 = q_d

回転を表すクォータニオンに関しては
q=(w,x,y,z)に対してq^(-1) = (w,-x,-y,-z)

Comments

[Masa_teratail]

ozwkさま、
ありがとうございます。

いただいた数式に当てはめ、変換してみました。
roll2 = atan2(2*qy*qw - 2*qx*qz, 1 - 2*qy*qy - 2*qz*qz)
pitch2 = atan2(2*qx*qw - 2*qy*qz, 1 - 2*qx*qx - 2*qz*qz)
yaw2 = asin(2*qx*qy + 2*qz*qw)

以下、3回の計測値です（1は直接取得した値、2はクォータニオン→オイラー角の値）。

rad:
roll 1,2= -0.11675182532904153 -0.08873572820517733
pitch 1,2= 0.7087068492345608 0.7063837052604676
yaw 1,2= 0.011820378320660014 -0.0641146823165717
degree:
roll 1,2= 40 40
pitch 1,2= -6 -5
yaw 1,2= 0 -3

rad:
roll 1,2= -1.4652921947697979 -0.8453017084942999
pitch 1,2= 0.8404609267125356 0.9962086692894003
yaw 1,2= 0.7635529217901318 -0.48061867615003256
degree:
roll 1,2= 48 57
pitch 1,2= -83 -48
yaw 1,2= 43 -27

rad:
roll 1,2= -1.3226650594302076 -1.3359935377402834
pitch 1,2= -0.020832342197108788 0.25125249542756
yaw 1,2= 0.26371758679634977 0.08361114311833065
degree:
roll 1,2= -1 14
pitch 1,2= -75 -76
yaw 1,2= 15 4

このランダム？に生じるズレがよくわからないです・・。
これはだんだんと自分の手に負えない領域に踏み込んでいる気がしてきました・・。

[ozwk]

本題とはズレますがradとdegがあってないように見えるんですけれども

例えばこれ

pitch 1,2= -0.020832342197108788 0.25125249542756
pitch 1,2= -75 -76

-0.0208 rad は -75度じゃないと思うんですが。

多分pitchとrollが入れ替わっているのかな?

[Masa_teratail]

すみません、ご指摘の通り、degのroll, pitchを逆に記載していました。