回転行列と回転角 -回転角に戻す-

# はじめに
:::note info
この記事はOpenCVアドベントカレンダー2023 2日目の記事です。その他の記事は[目次](https://qiita.com/advent-calendar/2023/opencv)から参照可能です
:::

[昨日](https://qiita.com/tomoaki_teshima/items/0fc58637a8b223879d9f)に続き回転行列の話をしようと思う

# TL;DR

:::note info
- あんな、回転行列って6種類ぐらいあんねん
- 回転角を決めれば回転行列が一意に決まると言ったな、あれは嘘だ。
:::

# 昨日の振り返り

[昨日の記事](https://qiita.com/tomoaki_teshima/items/0fc58637a8b223879d9f)では主に2つの点を紹介した
 - 回転行列とは $\mathbf{R}\left(\theta_p, \theta_y, \theta_r\right)$で表され、3行3列の行列であり、 **3つの回転角により一意に決まる行列** と紹介した。
 - 3次元の回転行列が2つ出てくる場合2次元の回転行列と違い、各回転行列から回転角を抽出して足し合わせるのは、一般的に成り立たず計算結果が期待したものにならない。

昨日の記事には不正確な記述が含まれていると記述したが、1点目の太字で強調した部分が意図的に隠した誤りである(それ以外のtypoや意図していない間違いが無いことを祈っている）なお、回転角はオイラー角とも呼ばれ、英語の記事だとeuler angle と表記されることが一般的であるが、本記事では昨日の記事との整合性のため、「回転角」で統一する。

3次元の回転行列は、昨日も紹介したように、各軸について回転させる3つの回転行列の積で構成されている

```math
\mathbf{R}\left(\theta_p,\theta_y,\theta_r\right) = 
\mathbf{R}\left(0,0,\theta_r\right)
\mathbf{R}\left(0,\theta_y,0\right)
\mathbf{R}\left(\theta_p,0,0\right)
```
```math
=
\left( \begin{matrix}
\cos\theta_r && -\sin\theta_r && 0 \\
\sin\theta_r && \cos\theta_r  && 0 \\
0 && 0 && 1
\end{matrix}\right)
\left( \begin{matrix}
\cos\theta_y  && 0 && \sin\theta_y\\
0 && 1 && 0 \\
-\sin\theta_y && 0 && \cos\theta_y
\end{matrix}\right)\left( \begin{matrix}
1 && 0 && 0 \\
0 && \cos\theta_p && -\sin\theta_p \\
0 && \sin\theta_p && \cos\theta_p
\end{matrix}\right)
```

この回転行列の場合、左から順番に**Z軸周りの回転、Y軸周りの回転、X軸周りの回転**を表す。各回転行列の、対応する軸に関しては、掛け算の前と後で、対応する軸の座標が変わっていないことが確認できると思う。

さて、ここで「どの順番に軸の回転を適用するか」で結果が変わってくる。ここで明確に回転行列を区別するために、前述の回転行列を$\mathbf{R}_{ZYX}$とあらわす。これは**左から順番にZ軸、Y軸、X軸に関する回転**が記述されてることを表す。この順序のことを以降「**回転軸の順序**」と記述する。

なお、非常に分かり辛いのだが、実際の回転は、**右側から掛け算を行う以上、X軸→Y軸→Z軸の純に回転が行われる**ことに注意が必要である。つまり回転軸の順序の逆順に回転が行われる。

```math
\left( \begin{matrix} X^{'} \\ Y^{'} \\ Z^{'} \end{matrix} \right)
=
\left( \begin{matrix}
\cos\theta_r && -\sin\theta_r && 0 \\
\sin\theta_r && \cos\theta_r  && 0 \\
0 && 0 && 1
\end{matrix}\right)
\left( \begin{matrix}
\cos\theta_y  && 0 && \sin\theta_y\\
0 && 1 && 0 \\
-\sin\theta_y && 0 && \cos\theta_y
\end{matrix}\right)\left( \begin{matrix}
1 && 0 && 0 \\
0 && \cos\theta_p && -\sin\theta_p \\
0 && \sin\theta_p && \cos\theta_p
\end{matrix}\right)
\left( \begin{matrix} X \\ Y \\ Z \end{matrix} \right)
```

さて、 $\mathbf{R}_{ZYX}$ がある以上、以下の回転軸の順序6通り全ての回転行列が存在することになる。

- $\mathbf{R}_{XYZ}\left(\theta_p,\theta_y,\theta_r\right)=\mathbf{R}\left(\theta_p,0,0\right)\mathbf{R}\left(0,\theta_y,0\right)\mathbf{R}\left(0,0,\theta_r\right)$
- $\mathbf{R}_{XZY}\left(\theta_p,\theta_y,\theta_r\right)=\mathbf{R}\left(\theta_p,0,0\right)\mathbf{R}\left(0,0,\theta_r\right)\mathbf{R}\left(0,\theta_y,0\right)$
- $\mathbf{R}_{YXZ}\left(\theta_p,\theta_y,\theta_r\right)=\mathbf{R}\left(0,\theta_y,0\right)\mathbf{R}\left(\theta_p,0,0\right)\mathbf{R}\left(0,0,\theta_r\right)$
- $\mathbf{R}_{YZX}\left(\theta_p,\theta_y,\theta_r\right)=\mathbf{R}\left(0,\theta_y,0\right)\mathbf{R}\left(0,0,\theta_r\right)\mathbf{R}\left(\theta_p,0,0\right)$
- $\mathbf{R}_{ZXY}\left(\theta_p,\theta_y,\theta_r\right)=\mathbf{R}\left(0,0,\theta_r\right)\mathbf{R}\left(\theta_p,0,0\right)\mathbf{R}\left(0,\theta_y,0\right)$
- $\mathbf{R}_{ZYX}\left(\theta_p,\theta_y,\theta_r\right)=\mathbf{R}\left(0,0,\theta_r\right)\mathbf{R}\left(0,\theta_y,0\right)\mathbf{R}\left(\theta_p,0,0\right)$

これが冒頭に書いた「**回転行列が6種類ある**」という趣旨である

![あんな、回転行列って6種類ぐらいあんねん](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/12162/a69216f4-c23b-1b56-1772-60d986a3b006.png)


# 回転行列から回転角への戻し方

回転行列を回転角に戻す計算方法についても言及しておく。前述の $\mathbf{R}_{ZYX}$ を展開して行列の乗算をすると、以下の行列が得られる

```math
\mathbf{R}_{ZYX}\left(\theta_p,\theta_y,\theta_r\right) = 
\mathbf{R}\left(0,0,\theta_r\right)
\mathbf{R}\left(0,\theta_y,0\right)
\mathbf{R}\left(\theta_p,0,0\right)
```
```math
=\left( \begin{matrix}
r_{00} && r_{01} && r_{02}\\
r_{10} && r_{11} && r_{12}\\
r_{20} && r_{21} && r_{22}
\end{matrix}\right)
```
```math
=
\left( \begin{matrix}
\cos\theta_y\cos\theta_r && \cos\theta_r\sin\theta_p\sin\theta_y - \cos\theta_p\sin\theta_r && \cos\theta_p\cos\theta_r\sin\theta_y + \sin\theta_p\sin\theta_r\\
\cos\theta_y\sin\theta_r  && \cos\theta_p\cos\theta_r + \sin\theta_p\sin\theta_y\sin\theta_r && -\cos\theta_r\sin\theta_p + \cos\theta_p\sin\theta_y\sin\theta_r \\
-\sin\theta_y && \cos\theta_y\sin\theta_p && \cos\theta_p\cos\theta_y
\end{matrix}\right)
```

ここで、
- $r_{20}$が$-\sin\theta_y$と、単一の回転角で決まっていることがわかる。
- $r_{00}$と$r_{10}$には$\cos\theta_y$で割り算することで$\cos\theta_r$と$\sin\theta_r$が得られる
- $r_{21}$と$r_{22}$には$\cos\theta_y$で割り算することで$\cos\theta_p$と$\sin\theta_p$が得られる

これにより
- $\theta_y = \sin^{-1}\left(-r_{20}\right)$
- $\theta_r = atan2\left(r_{00},r_{10}\right)$
- $\theta_p = atan2\left(r_{21},r_{22}\right)$
- ただし $atan2$関数はCやPythonなどで提供される関数で、$\cos\theta$と$\sin\theta$の2パラメータから$\theta$を計算する関数である

これ以上は割愛するが、6パターンの回転行列どれを使ったとしても、
- 単一の$\sin\theta$が9個の要素に1箇所あらわれる。このとき、2番目の回転角の$\sin$が現れる。前述の例で言えば$r_{20}$。
- 2番目の回転角の$\cos$と、残りの2つの回転角の$\cos$、$\sin$合計4つの積が現れる。前述の例で言えば$r_{00},r_{10},r_{21},r_{22}$の4つ

以上の2点は必ず満たすので、適切な要素をピックアップし、$\sin^{-1}$関数と$atan2$ 関数を使うことで回転角を復元できる。

## 制約
ここまでは、昨日の記事でも言及したOpenCVに同梱されてるチュートリアルコードにも実装されてる範囲である。

では完璧に回転行列から回転角に戻せるかと言うと、これまた戻せない特殊な状況が2種類存在する。その制約について言及しておこう

### 回転角は2つの可能性をもつ

ここまで厳密に計算しても、回転角については「**裏返った状態**」とでもいうべき回転角が存在し、そのどちらも同じ回転行列を構築するため、区別がつかない。$x^2=2$の解が$x=\pm\sqrt{2}$の2パターン存在するのと同じイメージで、どちらが正しいのでなく、両方正しいのである。$\left(\theta_p,\theta_y,\theta_r\right)$ の3つ回転角があった場合、 $\left(\theta_p-\pi,\frac{\pi}{2}-\theta_y,\theta_r-\pi\right)$ の回転角でも同じ回転行列が計算される。

別の表現として、前述の回転角の計算で $\sin^{-1}$関数を使うが、どんな言語の実装でも$\sin^{-1}(x)$もしくは$asin$関数は、値域が $-{\pi}/{2}<x<{\pi}/{2}$ の間となっている。よって、$r_{20}$に使われている回転角がこの範囲外だった場合、期待した結果には戻らない。

この点については具体的な例をもって後述したい。

### 0が発生する特異な場合
前述の計算では$atan2$ 関数にわたすパラメータ$r_{00},r_{10},r_{21},r_{22}$の4つは、いずれも$\cos\theta_y$を含む項である。これは$\theta_y$が$\pi / 2$ か $-\pi/2$の場合、$\cos\theta_y$が0になり、4つのパラメータすべてが0になってしまう特異な場合が発生する。

例えば前述の回転行列で$\theta_y=\pi/2$の場合、回転行列は以下の形に書き換えることができる

```math
\mathbf{R}_{ZYX}\left(\theta_p,\pi/2,\theta_r\right) = 
\mathbf{R}\left(0,0,\theta_r\right)
\mathbf{R}\left(0,\pi/2,0\right)
\mathbf{R}\left(\theta_p,0,0\right)
```
```math
=
\left( \begin{matrix}
0 && \cos\theta_r\sin\theta_p - \cos\theta_p\sin\theta_r && \cos\theta_p\cos\theta_r + \sin\theta_p\sin\theta_r\\
0  && \cos\theta_p\cos\theta_r + \sin\theta_p\sin\theta_r && -\cos\theta_r\sin\theta_p + \cos\theta_p\sin\theta_r \\
-1 && 0 && 0
\end{matrix}\right)
``` 

このとき、残りの4つのパラメータ$r_{01},r_{02},r_{11},r_{12}$は加法定理により以下のように書き換えることができる

- $r_{01} = -r_{12} = \cos\theta_r\sin\theta_p-\cos\theta_p\sin\theta_r=\sin(\theta_p-\theta_r)$
- $r_{02} = r_{11} = \cos\theta_r\cos\theta_p+\sin\theta_p\sin\theta_r=\cos(\theta_p-\theta_r)$

このとき、$atan2$ 関数により、復元できるのは$(\theta_p-\theta_r)$だけであり、$\theta_r$および$\theta_p$を独立して計算するだけの情報量が失われる。

同様に前述の回転行列で$\theta_y=-\pi/2$の場合、回転行列は以下の形に書き換えることができる

```math
\mathbf{R}_{ZYX}\left(\theta_p,=\pi/2,\theta_r\right) = 
\mathbf{R}\left(0,0,\theta_r\right)
\mathbf{R}\left(0,-\pi/2,0\right)
\mathbf{R}\left(\theta_p,0,0\right)
```
```math
=
\left( \begin{matrix}
0 && -\left(\cos\theta_r\sin\theta_p + \cos\theta_p\sin\theta_r\right) &&  \sin\theta_p\sin\theta_r -\cos\theta_p\cos\theta_r\\
0  && \cos\theta_p\cos\theta_r - \sin\theta_p\sin\theta_r && -\left(\cos\theta_r\sin\theta_p + \cos\theta_p\sin\theta_r\right) \\
1 && 0 && 0
\end{matrix}\right)
``` 

このとき、残りの4つのパラメータ$r_{01},r_{02},r_{11},r_{12}$は加法定理により以下のように書き換えることができる

- $r_{01} = r_{12} = -\left(\cos\theta_r\sin\theta_p + \cos\theta_p\sin\theta_r\right)= -\sin(\theta_p+\theta_r)$
- $r_{02} = -r_{11} = \cos\theta_p\cos\theta_r - \sin\theta_p\sin\theta_r =\cos(\theta_p+\theta_r)$

このとき、$atan2$ 関数により、復元できるのは$(\theta_p+\theta_r)$だけであり、$\theta_r$および$\theta_p$を独立して計算するだけの情報量が失われる。

このような得意な状況がどんなものであるかは、例を踏まえて後述したい。

## 回転角への戻し方

前述したように、回転行列は回転軸の順序によって6種類存在するわけだが、回転行列自体にはそんな情報が含まれていない。前述の計算式は $\mathbf{R}_{ZYX}$ における計算方法であったが、どの回転軸の順序を採用したとしても回転角を計算することはできる。そしてその計算方法は、意図した順序でないと、全く別の計算方法となる。

下図に示す通り、回転行列から回転角を計算する方法は回転軸の順序により6種類ある。結果単一の回転角からスタートしても、6種類の回転行列が発生し、また回転角に戻す方法も回転軸の順序により6種類あるため36種類の回転角が計算しうる。もちろん重複する回転角も存在する。

![mat2euler_36patterns.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/12162/51026521-b70a-2d3e-7d18-6c64c8593162.png)


# 回転行列を試すサンプルプログラム

ここで回転行列で遊べる[サンプルプログラム](https://github.com/tomoaki0705/rotationExperiment)を紹介する

https://github.com/tomoaki0705/rotationExperiment

![Screenshot 2023-11-29 23.18.14.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/12162/43add78e-8666-a4b6-f1c8-ff1d5cd3ba52.png)

個人的に好きな小説「すべてがFになる」からFの文字を取りました。森博嗣先生の作品で度々言及される「大文字でも小文字でも、点対称にも線対称にもならないアルファベット」の一つでもあります。

このプログラムはウィンドウ上部に出てきたトラックバーを動かすことで、ヨー、ピッチ、ロール方向に自由に**F**の字を回転できる。

- ピッチ角だけ変更した場合
![pitch_only.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/12162/a465fce0-24f5-0fda-ca8e-44ecae44c8a1.png)

- ヨー角だけ変更した場合
![yaw_only.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/12162/bef88d99-54c7-a526-0aa4-cc7f4fc72fd1.png)

- ロール角だけ変更した場合
![roll_only.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/12162/359ab82f-1e13-b59f-ec96-3e5782d893ff.png)


本来なら、$[-180,180]$の範囲で動かせるのが直感的なのだが、OpenCVのトラックバーは最小値が0で固定なので、$[0,360]$で動かすことになる。

## ビルド方法
- なるべく簡単にしたつもりだが、それでも
  - OpenCV
  - 最近のC++対応コンパイラ
  - CMake
  - が必要になる。
- Python でチョロっと試すってのもやりたかったが、時間がなくてそこまで手が回らなかった。
- なお、インタラクションで **Shift + 2**、および**Shift + 6**が必要となり、筆者は英字配列のキーボードを使っているので、

```
cmake -DLAYOUT_CHOICE=LAYOUT_EN ..
```

- みたいに`LAYOUT_CHOICE`オプションを書き換えてほしい
- デフォルトで`LAYOUT_JP`で日本語レイアウト対応になる

## 実行画面について
- トラックバーで各回転軸を独立に回転させられる。なお、トラックバーの角度は度数表記である
- 実行中にスペースバーを押すと、画面下部にこんな感じに情報が表示される

![image73.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/12162/40ef26cd-ea1e-5438-f264-03175d56973d.png)

- これはロール角ピッチ角ヨー角全て60度に設定した状態を表している。
- 画面下部に3行表示が出ているが、それぞれ **トラックバーからの入力**、**計算して戻された回転角**、**OpenCVのAPIで戻された回転角** の3つである。それぞれについて少し解説する
  - **トラックバーからの入力** : 画面上部のトラックバーからの入力である。角度は左から**ピッチ角(X軸)**、**ヨー角(Y軸)**、**ロール角(Z軸)** の3つである。一番左に出ている**XYZ**は前述の回転行列の種類を表し、**XYZ**は $\textbf{R}_{XYZ}$ に相当する。**XYZ** はキー`1`から`6`で切り替えることができ、それに合わせて回転行列の計算方法も変わる。以下に示すように、全パターンを網羅してある。

```cpp
    Mat rotationMatrix;
    switch (orderCompose)
    {
    case XYZ:
    default:
        rotationMatrix = rotationMatrixPitch * rotationMatrixYaw * rotationMatrixRoll;
        break;
    case XZY:
        rotationMatrix = rotationMatrixPitch * rotationMatrixRoll * rotationMatrixYaw;
        break;
    case YXZ:
        rotationMatrix = rotationMatrixYaw * rotationMatrixPitch * rotationMatrixRoll;
        break;
    case YZX:
        rotationMatrix = rotationMatrixYaw * rotationMatrixRoll * rotationMatrixPitch;
        break;
    case ZXY:
        rotationMatrix = rotationMatrixRoll * rotationMatrixPitch * rotationMatrixYaw;
        break;
    case ZYX:
        rotationMatrix = rotationMatrixRoll * rotationMatrixYaw * rotationMatrixPitch;
        break;
    }
```
  - **計算して戻された回転角** : 前段で計算された回転行列を、指定された回転軸の順序で計算されたとして、回転角を計算する。一番左に出ている **XYZ** が $\textbf{R}_{XYZ}$ **として解釈する**、という意味になり、結果計算された回転角が右側に表示される。先程と同じく左から**ピッチ角(X軸)**、**ヨー角(Y軸)**、**ロール角(Z軸)** の3つで、前述の画面上では回転行列を計算する際の軸の順序と、回転角を計算する際の軸の順序が一緒(どちらも**XYZ**)であるため、全く同じ数字が表示されている。

<details><summary>こちらもエンジニアとしては嫌になるほどのswitch文とif文のネストでできている。</summary>

```cpp
    switch (orderDecompose)
    {
    default:
    case XYZ:
        if (r02 < 1)
        {
            if (r02 > -1)
            {
                _yaw = asin(r02);
                _pitch = atan2(-r12, r22);
                _roll = atan2(-r01, r00);
            }
            else
            {
                _yaw = -CV_PI / 2;
                _pitch = -atan2(r10, r11);
                _roll = 0;
            }
        }
        else // r02 = +1
        {
            _yaw = CV_PI / 2;
            _pitch = atan2(r10, r11);
            _roll = 0;
        }
        break;
    case XZY:
        if (r01 < 1)
        {
            if (r01 > -1)
            {
                _roll = asin(-r01);
                _pitch = atan2(r21, r11);
                _yaw = atan2(r02, r00);
            }
            else
            {
                _roll = -CV_PI / 2;
                _pitch = -atan2(r20, r22);
                _yaw = 0;
            }
        }
        else // r01 = +1
        {
            _roll = -CV_PI / 2;
            _pitch = atan2(-r20, r22);
            _yaw = 0;
        }
        break;
    case YXZ:
        if (r12 < 1)
        {
            if (r12 > -1)
            {
                _pitch = asin(-r12);
                _yaw = atan2(r02, r22);
                _roll = atan2(r10, r11);
            }
            else
            {
                _pitch = CV_PI / 2;
                _yaw = -atan2(-r01, r00);
                _roll = 0;
            }
        }
        else // r12 = +1
        {
            _pitch = -CV_PI / 2;
            _yaw = atan2(-r01, r00);
            _roll = 0;
        }
        break;
    case YZX:
        if (r10 < 1)
        {
            if (r10 > -1)
            {
                _roll = asin(r10);
                _yaw = atan2(-r20, r00);
                _pitch = atan2(-r12, r11);
            }
            else
            {
                _roll = -CV_PI / 2;
                _yaw = -atan2(r21, r22);
                _pitch = 0;
            }
        }
        else // r10 = +1
        {
            _roll = -CV_PI / 2;
            _yaw = atan2(r21, r22);
            _pitch = 0;
        }
        break;
    case ZXY:
        if (r21 < 1)
        {
            if (r21 > -1)
            {
                _pitch = asin(r21);
                _yaw = atan2(-r01, r11);
                _roll = atan2(-r20, r22);
            }
            else
            {
                _pitch = -CV_PI / 2;
                _yaw = -atan2(r02, r00);
                _roll = 0;
            }
        }
        else // r21 = +1
        {
            _pitch = CV_PI / 2;
            _yaw = atan2(r02, r00);
            _roll = 0;
        }
        break;
    case ZYX:
        if (r20 < 1)
        {
            if (r20 > -1)
            {
                _yaw = asin(-r20);
                _roll = atan2(r10, r00);
                _pitch = atan2(r21, r22);
            }
            else
            {
                _yaw = CV_PI / 2;
                _roll = -atan2(-r12, r11);
                _pitch = 0;
            }
        }
        else // r20 = +1
        {
            _yaw = -CV_PI / 2;
            _roll = atan2(-r12, r11);
            _pitch = 0;
        }
        break;
    }
```

</details>

  - **OpenCVのAPIで戻された回転角** : これについては後述するが、`RQDecompose3x3`の返り値を表示している

# 回転角が裏返る状況を確認
回転角が裏返る状況を先程説明したが、実際のその状況を見てみよう。以下に示す図は、単純にヨー角だけ90度($\pi/2$)近辺で動かした状態を表している。回転行列を計算する際の、および回転角を計算する際の回転軸の順序は揃えてるため、3枚中1番左のスクリーンショットでは**計算された回転角が入力と一致している**ことがわかる。(しばらくは最下段のAPIと表示されてる行は無視してほしい)ところがヨー角が90度を超えると、ピッチ角とロール各が-180度($-\pi$)を示し、ヨー角も100度ではなく、80度を示している。これが「**裏返った状態**」である。この回転角も正しい回転角であり、同じ回転行列を計算できる。

![flip_euler.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/12162/b285c2b8-97dd-af97-f986-b455a04b38e9.png)

なお、この例ではヨー角を使ったが、ヨー角が原因ではなく、回転軸の順序の2番目の回転角が$[-90,90]$の範囲でしか計算できないため、そのつじつま合わせとして残りの2つの角がひっくり返った状態になる。 `1`から`6`のキーで軸の順序を変えると、その状況が確認できると思う。

# 2つの角度が独立して計算できなくなる状況

前述の回転角がちゃんと戻せない状況のうち、「**0が発生する特異な場合**」を示したい。下図に示すのは回転軸の順序を**ZYX**にし、**ヨー角を90度**に設定した。ヨー角90度にすると、**F**の形が線になってるため、段階的にヨー角を回転させる様子を下図に示す。下図のなかで1番右がヨー角90度であり、その際の回転角が**ピッチ角80度、ロール角0度**であるのに、計算された回転角は**ピッチ角0度、ロール角-80度**となっている。

![singular_situation_1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/12162/e117ca4b-d637-974e-d2c6-4c4ed5e65369.png)

この状況下ではピッチ角とロール角の回転方向が一致しているため、ロール角で回転されてるのか、ピッチ角で回転されているのか、区別がつかない。

![singular_situation_2.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/12162/b3557498-b527-81b9-c61a-f446a21b3537.png)

このため、ロール角とピッチ角を独立して求めることができず、**ロール角とピッチ角をあわせた角度**(この例では角度差)しか求めることができない。

# 回転行列が6種類あることの確認
繰り返しているように、回転行列は回転角を決めても一意に決まらない。これを確認するために、適当な回転角を決めた上で、`1`キーから`6`キーで回転角の順序を変えてみてほしい。
下に示す例ではヨー角ピッチ角ロール角全て60度にし、軸の計算順序を6通り全てバラバラにした状況を示す。
![changing_compose_order.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/12162/7d52d7e6-a9aa-6fab-8fc8-51d808785884.png)

例に図示する通り、**F**の形がどれ一つとして同じではなく、バラバラになっていることが確認できる。

# 回転角も6種類あることの確認

同様に、同じ回転行列でも回転軸の順序の違い、解釈の違いにより回転角もバラバラに変わってくる。下図に示す例では、いずれも**F**の姿勢が全て同じである。回転軸の順序は**XYZ**固定のまま、回転角を計算する際の回転軸の順序を変えている。
![changing_decompose_order.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/12162/52bd39b7-6481-f99e-31f1-7ca6300a55a5.png)

例に図示する通り、**F**の形がどれも同じであるにも関わらず、計算された回転角が全てバラバラであることが確認できる。

# OpenCV のAPIについて

さて、もうちょっとOpenCVについて触れておこう。「**そもそも回転角を計算するAPIがOpenCVで用意されてるのでは？**」と思う方がいるかもしれない。実はAPIは存在し、`RQDecomp3x3`がそれである。[公式ドキュメント](https://docs.opencv.org/4.8.0/d9/d0c/group__calib3d.html#ga1aaacb6224ec7b99d34866f8f9baac83)

このAPIは回転行列を渡すと、色々分解した上で返り値として回転角を返す。ただし、回転角を計算する際に軸の順序を指定するオプションは存在しない。実際に試してみると、下図に示すようにサンプルプログラムで**ZYX**と呼ばれる順序の回転角と一致していることがわかる
![image600.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/12162/437de880-3459-e78b-d268-410ee8b4522a.png)

また、軸の順序を指定できないことはドキュメントでも触れられている。

```
three Euler angles are only one of the possible solutions.
```



# クォータニオン、四元数について

回転行列の話題において触れずに通れないのがクォータニオン (Quaternion) もしくは四元数である。これはよくよく「**回転行列を一意に決める方法**」などと紹介されている。「ということは、この2日間の記事はクォータニオンで全部忘れられるのでは？」とお思いの読者がいるかもしれない。

クォータニオンはたしかに、回転行列を求める際に、回転軸の順序に関する不定性を取り除ける方法の一つである。今まで回転行列は回転角3つで表していたが、クォータニオンではQuatの名の通り4つのパラメータで表す。

このとき、3つのパラメータで空間中の直線を定義し、残り一つのパラメータで直線周りの回転角を定義する。

OpenCVにおいては、安定の [`cv::Rodrigues`](https://docs.opencv.org/4.8.0/d9/d0c/group__calib3d.html#ga61585db663d9da06b68e70cfbf6a1eac)で変換/逆変換がサポートされている。余談ではあるが、OpenCVのAPIのうち、大文字で始まる非常に数少ないAPIの1つである。これはRodriguesが[人名](https://en.wikipedia.org/wiki/Olinde_Rodrigues)に由来するためである。

という訳で`cv::Roadrigues`を使うことで3次元ベクトルが入手できる。前述の通り、クォータニオンは回転行列を4つのパラメータで表すベクトルであるが、OpenCVにおいては軸を定義する3つのパラメータで表され、4つ目のパラメータは3つのパラメータのノルムで表されているため、メモリ上では3次元ベクトルで保持される。（かしこい！）

と、ここまで丁寧に解説してきたが、クォータニオンはあくまで「回転行列を表す」ベクトルでしかなく、回転角では無い。よって、回転行列を回転角に戻すことはできない点に注意が必要である。

# おわりに
 - 昨日は回転行列を2つ使う場合を解説した
 - 本日の記事では回転行列と回転角の対応関係について説明した
     - 回転角と回転軸の順序を決めると、回転行列は一意に決まる。
     - 回転角だけだと、一般的に6通りの回転行列がありうる
     - 回転行列をもとに戻すには回転軸の順序を決める必要がある。回転軸の順序は6通りあり得るので、1つの回転行列は6通りの回転角を表しうる。
         - さらに回転角は必ず裏返しの状態が存在するので、どちらが期待した回転角かは自分で判断する必要がある
         - さらに回転角は特定の条件下で回転角2つが混ざった状態でしか復元できなくなる。
     - クォータニオンは回転行列と1対1の関係を持つため、回転軸の順序を心配しなくても回転行列を保持/変換できる
         - ただしクォータニオンはクォータニオンであり、回転角ではない。
     - 明日は @dandelion1124 先生の担当でタイトルは「[OpenCV 5クラウドファンディング](https://qiita.com/dandelion1124/items/5a93a3deecefc0e17c45)」です！

# 参考文献
https://www.geometrictools.com/Documentation/EulerAngles.pdf