前置き

OpenCVやROSと組み合わせてUnityやUE4といったゲームエンジンを使うとき困るのが座標変換です。右手系か左手系か、xは右か前か、プラットフォームごとにバラバラです。

この変換方法についてのメモです。

座標系の変換行列

といっても実は難しいことはありません。座標系の変換行列を作って掛けるだけです。

まず、y座標系からx座標系への変換行列(＝x座標系におけるy座標系)を

{}_x^{}R_y \in\mathbb{R}^{3 \times 3}

と定義します。このとき、$ {}_y^{}R_x = {}_x^{}R_y^{-1} $ と $ {}_x^{}R_z = {}_x^{}R_y \cdot {}_y^{}R_z $ が成り立ちます。

例えばOpenCVの座標系(x-right, y-down, z-forward)からunityの座標系(x-right, y-up, z-forward)へ変換する行列は下記のようになります。

{}_{Unity}^{}R_{OpenCV} =
\begin{bmatrix}1 & 0 & 0\\
0 & -1 & 0\\
0 & 0 & 1\end{bmatrix}

同様にROSやUE4との変換行列も定義してみましょう。

{}_{Unity}^{}R_{ROS} =
\begin{bmatrix}0 & -1 & 0\\
0 & 0 & 1\\
1 & 0 & 0\end{bmatrix}
\\
{}_{Unity}^{}R_{UE4} =
\begin{bmatrix}0 & 1 & 0\\
0 & 0 & 1\\
1 & 0 & 0\end{bmatrix}

unityへの変換のみを作りましたが、これらを元に相互変換する行列も機械的に作れます。OpenCVからROSなら下記のように求めます。

\begin{eqnarray}
{}_{ROS}^{}R_{OpenCV} 
&=& {}_{ROS}^{}R_{Unity} \cdot {}_{Unity}^{}R_{OpenCV} \\
&=& {}_{Unity}^{}R_{ROS}^{-1} \cdot {}_{Unity}^{}R_{OpenCV} \\
&=& \begin{bmatrix}0 & -1 & 0\\
0 & 0 & 1\\
1 & 0 & 0\end{bmatrix} ^{-1} \cdot
\begin{bmatrix}1 & 0 & 0\\
0 & -1 & 0\\
0 & 0 & 1\end{bmatrix} \\
&=& \begin{bmatrix}0 & 0 & 1\\
-1 & 0 & 0\\
0 & -1 & 0\end{bmatrix}

\\
\end{eqnarray}

変換方法

y座標系の位置 ${}_y^{}P \in\mathbb{R}^3$ をx座標系 ${}_x^{}P$ に変換：

{}_x^{}P = {}_x^{}R_y \cdot {}_y^{}P

y座標系の3x3回転行列 ${}_y^{}R\in SO(3)$ をx座標系 ${}_x^{}R$ に変換：

{}_x^{}R = {}_x^{}R_y \cdot {}_y^{}R \cdot {}_x^{}R_y^{-1}

y座標系の4x4行列 ${}_y^{}T \in SE(3)$ をx座標系 ${}_x^{}T$ に変換：

{}_x^{}T = \begin{bmatrix} {}_x^{}R_y & 0 \\ 0 & 1 \end{bmatrix}
\cdot
{}_y^{}T
\cdot
\begin{bmatrix} {}_x^{}R_y^{-1} & 0 \\ 0 & 1 \end{bmatrix}

y座標系のquaternion $ {}_y^{}Q = [x,y,z,w]^T$をx座標系$ {}_x^{}Q$に変換：

{}_x^{}Q = \begin{bmatrix} {}_x^{}R_y & 0 \\ 0 & 1 \end{bmatrix}
\cdot
{}_y^{}Q

maximaによるquaternionの変換式の導出

位置や回転が上の変換で良いのは分かるのですが、quaternionが本当にこれでよいのか直感的に分からなかったのでmaximaで検算してみました。おまけみたいですが、これが本題！

数式処理システムmaxima
https://wxmaxima-developers.github.io/wxmaxima/

quaternionから3x3行列を求めて座標変換をしてからquaternionに戻して、式がどう変形するかを見ています。上記で示した結果が得られると思います。(回転行列に変換する場合quaternionは符号反転しても等しくなることに留意)

/* quaternionから回転行列SO(3) を求める */
skew: matrix(
  [0,-z,y],
  [z,0,-x],
  [-y,x,0]
);
R:ident(3) + 2*skew.skew+2*w*skew;

/* 各プラットフォームの座標系からunityへの変換 */
uRo: matrix([1,0,0], [0,-1, 0], [0,0,1]);  /* opencv to unity */
uRr: matrix([0,-1,0], [0,0, 1], [1,0,0]); /* ros to unity */
uRe: matrix([0,1,0], [0,0, 1], [1,0,0]); /* ue4 to unity */

/* 適用する座標変換 */
dR: uRo$

/* 回転行列に座標変換を適用しquaternionを求める
   wは座標変換では不変のため計算を省く */
mtoq(m):=ratsimp(matrix([4*w^2,m[3,2]-m[2,3],m[1,3]-m[3,1],m[2,1]-m[1,2]])/4/w)$
mtoq(dR.R.invert(dR));