オイラー角,固定角,回転行列...

仕事で回転行列でドはまりし,数日悩みました....
結局いろいろと教えてもらって解決したのですが,「沖縄三次元復元プロジェクト」にも大いに関係するのでちょうどいい機会と思い,座標変換をきちんと勉強して整理することにしました.

1.回転の表現方法

回転の表現方法には,固定角,オイラー角,クオータニオン等,いろいろとありますが,ここではひとまず固定角とオイラー角について考えます.

固定角

座標Aを回転変換させて,座標Bに変換することを考えた場合に,座標Aの各軸(固定)について回転することを前提に角度表現する方法です.
つまり,固定角表現で「X軸周りに30度,Y軸周りに45度,Z軸周りに90度」といった場合は,座標Aの各軸周りに回転させることになります.

オイラー

座標Aを回転変換させて,座標Bに変換することを考えた場合に,都度都度回転させた座標系の軸に関しての回転で角度表現する方法です.
つまり,オイラー角表現で「X軸周りに30度,Y軸周りに45度,Z軸周りに90度」といった場合は,まず座標AのX軸周りに30度回転,そのあと回転させた座標系A´のY軸に関して45度回転,そのあと回転させた座標系A´´のZ軸に関して90度回転という形になります.

簡単に二つの回転の表現方法を図示してみました.ちなみに,どちらの回転方法でも「どの順番でどの軸周りに回転させるか」で結果が変わってきます.下記の図では,固定角の場合にはZ軸,Y軸,X軸の順番に回転,オイラー角の場合はX軸,Y1軸,Z2軸の順番に回転させています.

f:id:rkoichi2001:20171119004406p:plain

座標系の変換をするときに,回転行列を連鎖させて変換をすると思います.例えば,座標系Aから座標系Bに変換し,座標系Bから座標系Cに変換したいとすると,
R_A2C = R_B2C * R_A2B
となります.自分の場合は座標間の相対角度があらかじめ分かっていて,そこからこの R_A2B を作る必要があったのですが,ここで固定角とオイラー角がごちゃごちゃになってしまってはまってしまいました.
固定角で考えても,オイラー角で考えても正しく変換すれば回転行列は一意に求まります.正しく変換すればですが...

2.固定角・オイラー角の関係

で,ここではハマった状況を考えます.
座標系AからBへの回転行列が求めたいのだけど,

A. 固定角表現での角度が分かっている.

この場合,「座標系AからBの変換は,X->Y->Zの順番に,固定角表現で30度,45度,90度回せば変換できるよ!」といわれれば回転行列が一意に求まります.

固定角表現が与えられた場合の回転行列(A->B)の求め方
RotX = \begin{pmatrix} 1&0&0 \\ 0&cos(\theta1)&-sin(\theta1) \\ 0&sin(\theta1)&cos(\theta1) \end{pmatrix}
RotY = \begin{pmatrix} cos(\theta2)&0&sin(\theta2) \\ 0&1&0 \\ -sin(\theta2)&0&cos(\theta2) \end{pmatrix}
RotZ = \begin{pmatrix} cos(\theta3)&-sin(\theta3)&0 \\ sin(\theta3)&cos(\theta3)&0  \\ 0&0&1 \end{pmatrix}
R_Fixed = RotZ * RotY * RotX

B. オイラー角表現での角度が分かっている.

この場合,「座標系AからBの変換は,X->Y1->Z2の順番に,オイラー角表現で30度,45度,90度回せば変換できるよ!」といわれれば回転行列が一意に求まります.
(このケースでは,上記の固定角表現と順番も角度も同じですが,当然ながら違う変換を表します.)

オイラー角表現で与えられた場合の回転行列(A->B)の求め方
RotX = \begin{pmatrix} 1&0&0 \\ 0&cos(\theta1)&-sin(\theta1) \\ 0&sin(\theta1)&cos(\theta1) \end{pmatrix}
RotY = \begin{pmatrix} cos(\theta2)&0&sin(\theta2) \\ 0&1&0 \\ -sin(\theta2)&0&cos(\theta2) \end{pmatrix}
RotZ = \begin{pmatrix} cos(\theta3)&-sin(\theta3)&0 \\ sin(\theta3)&cos(\theta3)&0  \\ 0&0&1 \end{pmatrix}

R_Euler = RotX * RotY * RotZ (固定角の場合と比べて,XとZの順番が入れ替わっていることに注目!)

C. 固定角とオイラー角の関係.

ここで,固定角とオイラー角の関係ですが,まず固定角でX->Y->Zの順番にx度,y度,z度回転させたときの回転行列を考えてみると,
R_Fixed = Rz(z) * Ry(y) * Rx(x)
となります.こいつと同じ回転行列をオイラー角表現であらわすと,Z->Y->Xの順番に z度,y度,x度回転させると,,,
R_Euler = Rz(z) * Ry(y) * Rx(x)
となります.つまり,固定角とオイラー角の関係は回転させる順番が逆であることだけだということがわかります.

D. 実例

そろそろ書いてる本人も混乱してきたので,簡単な実例を考えます.
下記の例,座標系AからBへの変換行列を考える際に,固定角で考えた場合とオイラー角で考えた場合を書いてますが,計算の順序を上記のようにすると最終結果も一致していることがわかります.

f:id:rkoichi2001:20171119022238p:plain

3.各軸周りの回転角度を求めるスクリプト

ということで,「2つの座標間の固定角が与えられたときの回転行列」,「2つの座標間のオイラー角が与えられたときの回転行列」,「回転行列が与えられたときの固定角」,「回転行列が与えられたときのオイラー角」を求めるスクリプトを作りました.ちょっと完全にテストできているわけではないので,実際に使うときは恐る恐る使ってください.ちなみに,どの順番で回転させるかで結果も変わってきますが,ひとまず xyz と zyx に関して作ってみました.ほかの順番でやりたいときは,”ShowRotMatInSymbol” でシンボルを表示してもらって,同じノリで方程式を解いていけばできると思います.

# -*- coding: utf-8 -*-

from sympy import *
from sympy import Matrix
import numpy as np
import math

class RotationMatrixConverter:
    
    def __init__(self):
        self.__thresh = 0.00001
    
    def ShowRotMatInSymbol(self, order):
        
        c1 = Symbol("c1")
        s1 = Symbol("s1")
        c2 = Symbol("c2")
        s2 = Symbol("s2")
        c3 = Symbol("c3")
        s3 = Symbol("s3")
        
        Rx = Matrix([[1, 0, 0], [0, c1, -s1], [0, s1, c1]])
        Ry = Matrix([[c2, 0, s2], [0, 1, 0], [-s2, 0, c2]])
        Rz = Matrix([[c3, -s3, 0], [s3, c3, 0], [0, 0, 1]])
        
        if order is "xyz":
            return (Rz * Ry * Rx)
        elif order is "xzy":
            return (Ry * Rz * Rx)
        elif order is "yxz":
            return (Rz * Rx * Ry)            
        elif order is "yzx":
            return (Rx * Rz * Ry)            
        elif order is "zxy":
            return (Ry * Rx * Rz)           
        elif order is "zyx":
            return (Rx * Ry * Rz)            
    
    def Euler2Rot(self, angleXYZ, order):

        RotX = np.matrix([[1, 0, 0], [0, math.cos(angleXYZ[0]), -math.sin(angleXYZ[0])], [0, math.sin(angleXYZ[0]), math.cos(angleXYZ[0])]])
        RotY = np.matrix([[math.cos(angleXYZ[1]), 0, math.sin(angleXYZ[1])], [0, 1, 0], [-math.sin(angleXYZ[1]), 0, math.cos(angleXYZ[1])]])
        RotZ = np.matrix([[math.cos(angleXYZ[2]), -math.sin(angleXYZ[2]), 0], [math.sin(angleXYZ[2]), math.cos(angleXYZ[2]), 0], [0, 0, 1]])
        
        if order is "xyz":
            return RotX * RotY * RotZ
        elif order is "zyx":
            return RotZ * RotY * RotX
        else:
            print("Specified order is not supported!")
            
    def Fixed2Rot(self, angleXYZ, order):

        RotX = np.matrix([[1, 0, 0], [0, math.cos(angleXYZ[0]), -math.sin(angleXYZ[0])], [0, math.sin(angleXYZ[0]), math.cos(angleXYZ[0])]])
        RotY = np.matrix([[math.cos(angleXYZ[1]), 0, math.sin(angleXYZ[1])], [0, 1, 0], [-math.sin(angleXYZ[1]), 0, math.cos(angleXYZ[1])]])
        RotZ = np.matrix([[math.cos(angleXYZ[2]), -math.sin(angleXYZ[2]), 0], [math.sin(angleXYZ[2]), math.cos(angleXYZ[2]), 0], [0, 0, 1]])
        
        if order is "xyz":
            return RotZ * RotY * RotX
        elif order is "zyx":
            return RotX * RotY * RotZ
        else:
            print("Specified order is not supported!")
    
    def Rot2Fixed(self, R, order):
        
        if order is "xyz":
            fixed = self.__Rot2Fixed_xyz(R)
        elif order is "zyx":
            fixed = self.__Rot2Fixed_zyx(R)
        else:
            print("Specified order is not supported!")
            
        return fixed
    
    def Rot2Euler(self, REuler, order):
    
        if order is "xyz":
            euler = self.__Rot2Fixed_zyx(REuler)
        elif order is "zyx":
            euler = self.__Rot2Fixed_xyz(REuler)
        else:
            print("Specified order is not supported!")
            
        return euler

    def __Rot2Fixed_xyz(self, R):
        
        if (abs(R[2, 0] - 1) > self.__thresh and abs(R[2, 0] + 1) > self.__thresh):
            
            theta2_0 = -math.asin(R[2, 0])
            theta2_1 = math.pi - theta2_0
            theta1_0 = math.atan2(R[2, 1] / math.cos(theta2_0), R[2, 2] / math.cos(theta2_0))
            theta1_1 = math.atan2(R[2, 1] / math.cos(theta2_1), R[2, 2] / math.cos(theta2_1))
            theta3_0 = math.atan2(R[1, 0] / math.cos(theta2_0), R[0, 0] / math.cos(theta2_0))
            theta3_1 = math.atan2(R[1, 0] / math.cos(theta2_1), R[0, 0] / math.cos(theta2_1))    
            FixedResXYZ = np.array([[theta1_0, theta2_0, theta3_0], [theta1_1, theta2_1, theta3_1]])
            
        else:
            theta3 = 0
            if (abs(R[2, 0] + 1) < self.__thresh):
                theta2 = math.pi / 2
                theta1 = theta3 + math.atan2(R[0, 1], R[0, 2])
            else:
                theta2 = -math.pi / 2
                theta1 = -theta3 + math.atan2(-R[0, 1], -R[0, 2])
    
            FixedResXYZ = np.array([theta1, theta2, theta3])
    
        return FixedResXYZ

    def __Rot2Fixed_zyx(self, R):

        if (abs(R[0, 2] - 1) > self.__thresh and abs(R[0, 2] + 1) > self.__thresh):
            
            theta2_0 = math.asin(R[0, 2])
            theta2_1 = math.pi - theta2_0
            theta1_0 = math.atan2(-R[1, 2] / math.cos(theta2_0), R[2, 2] / math.cos(theta2_0))
            theta1_1 = math.atan2(-R[1, 2] / math.cos(theta2_1), R[2, 2] / math.cos(theta2_1))
            theta3_0 = math.atan2(-R[0, 1] / math.cos(theta2_0), R[0, 0] / math.cos(theta2_0))
            theta3_1 = math.atan2(-R[0, 1] / math.cos(theta2_1), R[0, 0] / math.cos(theta2_1))    
            FixedResZYX = np.array([[theta1_0, theta2_0, theta3_0], [theta1_1, theta2_1, theta3_1]])
            
        else:
            theta3 = 0
            if (abs(R[0, 2] - 1) < self.__thresh):
                theta2 = math.pi / 2
                theta1 = -theta3 + math.atan2(R[2, 1], R[1, 1])
            else:
                theta2 = -math.pi / 2
                theta1 = theta3 + math.atan2(R[2, 1], R[1, 1])
    
            FixedResZYX = np.array([theta1, theta2, theta3])
    
        return FixedResZYX


if __name__ == "__main__":
    
    converter = RotationMatrixConverter()
    
    euler = np.array([45, 90, 135]) / 180 * math.pi
    
    # Rotation XYZ
    RotXYZ = converter.Euler2Rot(euler, "xyz")
    eulerRes = converter.Rot2Euler(RotXYZ, "xyz")
    print("Result XYZ")
    print("Rotation Mat : ")
    print(RotXYZ)
    print("Euler Angles : ", eulerRes / math.pi * 180)
    print(converter.Euler2Rot(eulerRes, "xyz"))
    print("")
    
    # Rotation ZYX
    RotZYX = converter.Euler2Rot(euler, "zyx")
    eulerRes = converter.Rot2Euler(RotZYX, "zyx")
    print("Result ZYX")
    print("Rotation Mat : ")
    print(RotZYX)
    print(eulerRes / math.pi * 180)
    print(converter.Euler2Rot(eulerRes, "zyx"))
    print("")

        
    fixed = np.array([180, 0, -90]) / 180 * math.pi
    RotXYZ = converter.Fixed2Rot(fixed, "xyz")
    print("Rotation Mat : ")
    print(RotXYZ)
    eulerRes = converter.Rot2Euler(RotXYZ, "xyz")
    
    print("")
    print("Result Euler XYZ : ")
    print(eulerRes / math.pi * 180)