【Python/NumPy】回転行列で座標変換

Python言語とNumPyを用いて、回転行列を生成し、座標変換の計算をする方法をソースコード付きで解説します。

回転行列

回転行列とは、姿勢変換などに用いられる重要な行列です。
この行列を使うと、2次元平面上や3次元空間上の点(x、y)をθ度回転させた後の座標を簡単に求めることができます。
【参考記事】x-y-z軸周りの回転行列 (式・導出・証明)

今回は、上記事のx-y-z軸周りの回転行列をPythonとNumPyで計算してみました。

ソースコード

サンプルプログラムのソースコードです。

# -*- coding: utf-8 -*-
import numpy as np

def rotate_x(deg):
    # degreeをradianに変換
    r = np.radians(deg)
    C = np.cos(r)
    S = np.sin(r)
    # x軸周りの回転行列
    R_x = np.matrix((
        (1, 0, 0),
        (0, C, -S),
        (0, S, C)
    ))

    return R_x


def main():
    # 元の行列
    a = np.array((1,1,0))
    # 回転行列の生成
    Rx = rotate_x(90)
    # 回転後のベクトルを計算
    b = np.dot(Rx,a)
    # 整数型に変換
    b = np.array(b,dtype=np.uint8)
    print("a=" + str(a))
    print("R=" + str(Rx))
    print("b=" + str(b[0]))


if __name__ == '__main__':
    main()

実行結果

サンプルプログラムの実行結果は下記の通りです。
点aがx-y-z空間上でx軸に対して90度回転した後の点bを回転行列Rで求めています。
b = R・a

a=[1 1 0]

R=[[  1.00000000e+00   0.00000000e+00   0.00000000e+00]
 [  0.00000000e+00   6.12323400e-17  -1.00000000e+00]
 [  0.00000000e+00   1.00000000e+00   6.12323400e-17]]

b=[1 0 1]
関連ページ
1 Pythonでロボットシミュレーション
2 ロボット工学入門 基礎編
3 Python入門 サンプル集
4 NumPy入門 サンプル集

コメント