はじめに
- 本記事はOpenCV Advent Calendar 2023 1日目の記事です。
- その他の記事は目次をご覧ください。
- 今日は回転行列の話をしようと思う。
TL;DR
- 回転角は足しちゃだめ
回転行列のおさらい
いきなり回転行列と言っても面食らう人もいると思うので、まずは回転行列のおさらいから
2次元の回転行列
回転行列とは、馴染み深いのは以下の形のものだと思う。
\left( \begin{matrix}
\cos\theta & -\sin\theta \\
\sin\theta & \cos\theta
\end{matrix} \right)
これは一般的に高校で習う2次元における回転行列で、2 $\times$ 2の行列で表される。
実際はある平面上の点 $a$ を $a^{\prime}$ に、原点を中心として $\theta$ だけ回転させることから、回転行列と呼ばれる。
2次元の回転行列で4つのパラメータを持つが、自由度は1、つまり $\theta$ 1つで回転行列が一意に決まることを意味する。
また、回転行列の積は、その特性から回転角の和で表すこともできる
\left( \begin{matrix}
\cos\theta_1 & -\sin\theta_1 \\
\sin\theta_1 & \cos\theta_1
\end{matrix} \right)
\left( \begin{matrix}
\cos\theta_2 & -\sin\theta_2 \\
\sin\theta_2 & \cos\theta_2
\end{matrix} \right)=
\left( \begin{matrix}
\cos(\theta_1+\theta_2) & -\sin(\theta_1+\theta_2) \\
\sin(\theta_1+\theta_2) & \cos(\theta_1+\theta_2)
\end{matrix} \right)
前述の図の通り、回転行列の積はある点を2回回転移動することと等価で、結果、回転行列の積は、回転角の和で表すことができる。完全に余談ではあるが、この回転行列の積を展開することで加法定理を導出できる。
三次元の回転行列
この2次元の回転行列を3次元に拡張してみよう。
三次元の回転行列は、 3$\times$3 の行列で表され、9つのパラメータを持つが、自由度は3である。
つまり実際のところ、 $\theta_p,\theta_y,\theta_r$ の3つのパラメータを決定すれば一意に決まる関数とみなすこともできる。
前述の2次元の回転行列は $\theta$ 1つで一意に決まるように、三次元の回転行列は3つのパラメータにより一意に決まる。
\mathbf{R}\left(\theta_p,\theta_y,\theta_r\right)=\left( \begin{matrix}
r_{00} && r_{01} && r_{02}\\
r_{10} && r_{11} && r_{12}\\
r_{20} && r_{21} && r_{22}
\end{matrix}\right)
ここで表した $\theta_p,\theta_y,\theta_r$ はピッチ角、ヨー角、ロール角とも呼び、それぞれ $X$軸周り、 $Y$軸周り、 $Z$ 軸周りの回転角を表す。
また、回転行列 $\mathbf{R}$ は直交行列であることが知られている。
\mathbf{R}^{\intercal}=\mathbf{R}^{-1}
つまり自身の逆行列は自身の転置行列に等しい。
\mathbf{R}\mathbf{R}^{-1}=\mathbf{R}^{\intercal}\mathbf{R}=\mathbf{I}
回転行列の行列式は必ず1となる
|\mathbf{R}|=1
結局のところ、回転行列は $\sin$ や $\cos$ の集まりであることを考えるとなんとなくその特性がイメージできると思う。
回転角は足しちゃだめ
さてここで回転行列の具体例として、飛行機を考える。
このように機体の進捗方向が $Z$軸、機体右向き水平方向が $X$軸、垂直方向上向きが $Y$軸とする、左手系の座標系を考える。この機体の機種の先頭に、ザクの目のような、自由に球面上を移動できるカメラがついている状況を考えてみよう。
カメラはピッチ、ヨー方向に自由に移動できる状況を考える。任意のピッチ角、ヨー角を与えることでカメラを任意の方向に向けることができる、機械制御のカメラを考える。
今、先頭についてるカメラで、地上にあるマーカを読み取ることで機体の進行方向を求めることを考える。現実の飛行機はそんなことしなくても高度な計器類がついているため、こんな非現実的なことは不要だが、今回の問題を説明するために特殊な状況を前提としたい。
回転行列から回転角へ
ここで回転行列から回転角を計算する方法だが、OpenCVのチュートリアルコード から抜粋してみよう
cv::Mat rot2euler(const cv::Mat & rotationMatrix)
{
double m00 = rotationMatrix.at<double>(0,0);
double m02 = rotationMatrix.at<double>(0,2);
double m10 = rotationMatrix.at<double>(1,0);
double m11 = rotationMatrix.at<double>(1,1);
double m12 = rotationMatrix.at<double>(1,2);
double m20 = rotationMatrix.at<double>(2,0);
double m22 = rotationMatrix.at<double>(2,2);
double bank, attitude, heading;
{
bank = atan2(-m12,m11);
attitude = asin(m10);
heading = atan2(-m20,m00);
}
このサンプルコードではピッチ角を attitude、ヨー角を heading、ロール角をbankと呼んでいるので、この計算に基づいて回転行列からヨー角を求める。
カメラから撮影されたマーカの向きにより、カメラとマーカの間の回転行列が求められ、ヨー角 $\theta_1$が求められる。
またカメラの向きは既知なので、カメラのヨー角 $\theta_2$ が求められる。
冒頭に書いた通り、この回転角の足し算は誤りです。
次節で解説しますが一体何故誤っているのでしょうか？
解説
何故誤っているのか考えられたでしょうか。
実は、３次元の回転行列は各軸周りの回転行列をかけあわせた形になっています。
\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)
=
\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)
先程の問題だと回転行列が2つ登場するわけですが、カメラとマーカ間の回転角を $\theta_{1p},\theta_{1y},\theta_{1r}$、カメラと機体の相対角度を $\theta_{2p},\theta_{2y}$で表すと、求めたいヨー角は以下の式の通り、行列の掛け算に紛れ込んでいます。
\mathbf{R}\left(0,0,\theta_{1r}\right)
\mathbf{R}\left(0,\theta_{1y},0\right)
\mathbf{R}\left(\theta_{1p},0,0\right)
\mathbf{R}\left(0,0,\theta_{2r}\right)
\mathbf{R}\left(0,\theta_{2y},0\right)
\mathbf{R}\left(\theta_{2p},0,0\right)
前述の通りカメラは球面上を自由に動くので、本来はロール成分を無視できるのですがここでは敢えて記載しています。
この回転行列たちのうち、次の図に示すように、直接隣り合ってない回転行列たちのヨー角を足し合わせています。
機体が完全に水平で、カメラもピッチ角が0であるならば、前述の回転行列は純粋にヨー角成分だけになり、角度を足し合わせても問題なくなります。
しかし現実世界では一般的にはロール成分ピッチ成分が存在するわけで、ヨー角の足し合わせには問題が発生します。
ではどうすれば良かったのかと言えば、回転行列をかけ合わせることがキモです
\mathbf{R}\left(0,0,\theta_{1r}\right)
\mathbf{R}\left(0,\theta_{1y},0\right)
\mathbf{R}\left(\theta_{1p},0,0\right)
\mathbf{R}\left(0,0,\theta_{2r}\right)
\mathbf{R}\left(0,\theta_{2y},0\right)
\mathbf{R}\left(\theta_{2p},0,0\right)
これらを掛け合わせた回転行列から、前述の数式よりヨー角を求めることにより、正しいヨー角を求めることができます。