仕事で回転行列でドはまりし,数日悩みました....
結局いろいろと教えてもらって解決したのですが,「沖縄三次元復元プロジェクト」にも大いに関係するのでちょうどいい機会と思い,座標変換をきちんと勉強して整理することにしました.
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軸の順番に回転させています.
座標系の変換をするときに,回転行列を連鎖させて変換をすると思います.例えば,座標系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)の求め方
R_Fixed = RotZ * RotY * RotX
B. オイラー角表現での角度が分かっている.
この場合,「座標系AからBの変換は,X->Y1->Z2の順番に,オイラー角表現で30度,45度,90度回せば変換できるよ!」といわれれば回転行列が一意に求まります.
(このケースでは,上記の固定角表現と順番も角度も同じですが,当然ながら違う変換を表します.)
オイラー角表現で与えられた場合の回転行列(A->B)の求め方
R_Euler = RotX * RotY * RotZ (固定角の場合と比べて,XとZの順番が入れ替わっていることに注目!)
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)