[수학 to 파이썬] 3D Coordinate Transformation (3차원 좌표 변환)

3차원 좌표가 $x$축을 기준으로 $\theta_1$만큼, $y$축을 기준으로 $\theta_2$만큼, 그리고 $z$축을 기준으로 $\theta_3$만큼 돌아간다고 했을 때, 변환된 좌표의 값을 어떻게 구할까? (이 글에서는 translation과 scaling이 없다고 가정한다.)

1. 3차원 좌표 변환 공식

3차원 좌표를 각각 $x$축, $y$축, 그리고 $z$축으로 변환하는 행렬을 $R_x(\theta_1)$, $R_y(\theta_2)$, 그리고 $R_z(\theta_3)$ 이라고 했을 때, 3차원 좌표를 움직이는 행렬 $R$은

$$R=R_z(\theta_3)R_y(\theta_2)R_x(\theta_1)$$  

로 정의할 수 있다. 이때 각각의  $R_x(\theta)$, $R_y(\theta)$, 그리고 $R_z(\theta)$은

$$R_x(\theta) = \begin{bmatrix} 1 & 0 & 0 & 0\\ 0 & \cos\theta & -\sin\theta & 0\\ 0 & \sin\theta & \cos\theta & 0\\ 0 & 0 & 0 & 1 \end{bmatrix}$$  

$$R_y(\theta) = \begin{bmatrix} \cos\theta & 0 & \sin\theta & 0\\ 0 & 1 & 0 & 0\\ -\sin\theta & 0 & \cos\theta & 0\\ 0 & 0 & 0 & 1 \end{bmatrix}$$  

$$R_z(\theta) = \begin{bmatrix} \cos\theta & -\sin\theta & 0 & 0\\ \sin\theta & \cos\theta & 0 & 0\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix}$$  

이기 때문에, 이를 이용하여 행렬 $R$을 구하면

$$R=R_z(\theta_3)R_y(\theta_2)R_x(\theta_1)=\begin{bmatrix} 1 & 0 & 0 & 0\\ 0 & \cos\theta_3 & -\sin\theta_3 & 0\\ 0 & \sin\theta_3 & \cos\theta_3 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix}\begin{bmatrix} \cos\theta_2 & 0 & \sin\theta_2 & 0\\ 0 & 1 & 0 & 0\\ -\sin\theta_2 & 0 & \cos\theta_2 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix}\begin{bmatrix} \cos\theta_1 & -\sin\theta_1 & 0 & 0\\ \sin\theta_1 & \cos\theta_1 & 0 & 0\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix}$$

이 된다. 그렇다면 우리가 좌표 $(x, y, z)$를 변환시켜준다고 하면, 변환된 좌표 $(x', y', z')$은

$$\begin{bmatrix} x'\\ y'\\ z'\\ 1\end{bmatrix}=\begin{bmatrix} 1 & 0 & 0 & 0\\ 0 & \cos\theta_3 & -\sin\theta_3 & 0\\ 0 & \sin\theta_3 & \cos\theta_3 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix}\begin{bmatrix} \cos\theta_2 & 0 & \sin\theta_2 & 0\\ 0 & 1 & 0 & 0\\ -\sin\theta_2 & 0 & \cos\theta_2 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix}\begin{bmatrix} \cos\theta_1 & -\sin\theta_1 & 0 & 0\\ \sin\theta_1 & \cos\theta_1 & 0 & 0\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix}\begin{bmatrix} x\\ y\\ z\\ 1\end{bmatrix}$$

로 표현할 수 있다. 이제 이 식을 코드로 적어보자.

2. 코드

import numpy as np


def rotation_function(theta: np.array, coordinate: np.array) -> np.array:
    """
    3차원 좌표의 coordinate transformation을 해주는 함수
    :param theta: x축, y축, z축 변환 각도, 모든 값은 radian 값이어야 한다
    :param coordinate: 3차원 (x, y, z) 좌표, array의 shape은 (1, 3) 이어야 한다
    :return: coordinate transformation이 완료된 3차원 좌표
    """
    rotation_x = np.array([
        [1, 0, 0, 0],
        [0, np.cos(theta[0]), -np.sin(theta[0]), 0],
        [0, np.sin(theta[0]), np.cos(theta[0]), 0],
        [0, 0, 0, 1]
    ])
    rotation_y = np.array([
        [np.cos(theta[1]), 0, np.sin(theta[1]), 0],
        [0, 1, 0, 0],
        [-np.sin(theta[1]), 0, np.cos(theta[1]), 0],
        [0, 0, 0, 1]
    ])
    rotation_z = np.array([
        [np.cos(theta[2]), -np.sin(theta[2]), 0, 0],
        [np.sin(theta[2]), np.cos(theta[2]), 0, 0],
        [0, 0, 1, 0],
        [0, 0, 0, 1]
    ])

    rotation_matrix = rotation_z @ rotation_y @ rotation_x
    transposed_coordinate = coordinate.T
    padded_coordinate = np.concatenate((transposed_coordinate, np.ones((1, transposed_coordinate.shape[1]))), axis=0)
    return (rotation_matrix @ padded_coordinate)[0:3, :].T

3. 예시

theta = np.array([30, 60, -45]) * np.pi/180
coordinate = np.array([[10, -20, 10]])
rotated_coordinate = rotation_function(theta, coordinate)
print(rotated_coordinate)

[[-13.06787221 -18.49809303  -9.33012702]]