[Math] Rotation Vector (= Axis-Angle, Rodrigues Angle)

3차원 공간에서의 rotation을 표현하는 방법.

Euler angle과 함께 가장 많이 사용되는 방법 중 하나임.

하지만 3개의 축에 대한 3개의 rotation angle로 표현하는 Euler angle과 달리,

Rotation Vector는 하나의 vector만으로 표현한다.

컴퓨터 비전과 3D 그래픽스에서 자주 사용되며, 카메라 캘리브레이션, 객체 추적, 포즈 추정 등에 중요함.

---

정의

3차원 공간에서의 rotation은 회전축과 회전각이 필요한데

Rotation Vector는

• 회전축 : Rotation Vector의 direction으로 표현, $\mathbf{e}$.
• 회전각 : Rotation Vector의 length로 표현 (L-2 norm), $\theta$

일반적으로 회전중심은 origin이 됨.

---

Axis-Angle 표현의 수학적 정의

Rotation Vector(회전 벡터) $\mathbf{v} = \begin{bmatrix} x & y & z \end{bmatrix}^T$

• 회전 축 $\mathbf{e} = \begin{bmatrix} e_x & e_y & e_z \end{bmatrix}^T$와
• 회전 각도 $\theta$로 변환될 수 있음.

회전 벡터 $\mathbf{v}$의 크기는 회전 각도 $\theta$에 해당하고, 회전 축 $\mathbf{e}$는 $\mathbf{v}$의 방향을 나타냄.

$$
\theta = |\mathbf{v}| = \sqrt{x^2 + y^2 + z^2}
$$

$$
\mathbf{e} = \frac{\mathbf{v}}{|\mathbf{v}|} = \frac{1}{\theta} \begin{bmatrix} x & y & z \end{bmatrix}^T
$$

---

from Rotation Vector to Rotation Matrix

Rodrigues 공식에 따르면, 회전 행렬 $\mathbf{R}$는 다음과 같이 계산됨:

$$
\mathbf{R} = \mathbf{I} + \sin(\theta) \mathbf{K} + (1 - \cos(\theta)) \mathbf{K}^2
$$

여기서 $\mathbf{I}$는 단위 행렬이고, $\mathbf{K}$는 회전 축 $\mathbf{e}$에 대한 반대칭 행렬임:

$$
\mathbf{K} = \begin{bmatrix}
0 & -e_z & e_y \\
e_z & 0 & -e_x \\
-e_y & e_x & 0
\end{bmatrix}
$$

따라서, 최종 Rotation Matrix $\mathbf{R}$는 다음과 같음:

$$
\mathbf{R} = \begin{bmatrix}
1 + (1 - \cos(\theta))(e_x^2 - 1) & -e_z \sin(\theta) + (1 - \cos(\theta))e_x e_y & e_y \sin(\theta) + (1 - \cos(\theta))e_x e_z \\
e_z \sin(\theta) + (1 - \cos(\theta))e_x e_y & 1 + (1 - \cos(\theta))(e_y^2 - 1) & -e_x \sin(\theta) + (1 - \cos(\theta))e_y e_z \\
-e_y \sin(\theta) + (1 - \cos(\theta))e_x e_z & e_x \sin(\theta) + (1 - \cos(\theta))e_y e_z & 1 + (1 - \cos(\theta))(e_z^2 - 1)
\end{bmatrix}
$$

---

OpenCV를 사용한 변환

OpenCV의 cv2.Rodrigues 함수를 사용하여 Rotation Vector $\mathbf{v}$와 Rotation Matrix $\mathbf{R}$ 간의 변환을 쉽게 수행할 수 있음.

Example 0: from $\mathbf{v}$ to $\mathbf{R}$

import cv2
import numpy as np

# 회전 벡터 (x, y, z 축에 대한 회전 각도)
rvec = np.array([0.1, 0.2, 0.3])

# 회전 벡터를 회전 행렬로 변환하고 Jacobian 계산
rmat, jacobian = cv2.Rodrigues(rvec)

print("회전 벡터:")
print(rvec)
print("회전 행렬:")
print(rmat)
print("Jacobian 행렬:")
print(jacobian)

---

Example 1: from $\mathbf{R}$ to $\mathbf{v}$

import cv2
import numpy as np

# 회전 행렬 (3x3)
rmat = np.array([
    [0.975, -0.197, 0.096],
    [0.198, 0.980, -0.002],
    [-0.093, 0.019, 0.995]
])

# 회전 행렬을 회전 벡터로 변환하고 Jacobian 계산
rvec, jacobian = cv2.Rodrigues(rmat)

print("회전 행렬:")
print(rmat)
print("회전 벡터:")
print(rvec)
print("Jacobian 행렬:")
print(jacobian)

---

Euler Angle과의 관계

Rotation Matrix $\mathbf{R}$ 이 주어졌을 때, Euler 각도

• x-axis: $\phi$,
• y-axis: $\theta$,
• z-axis: $\psi$

는 다음과 같이 계산할 수 있음:

$$\begin{aligned} \phi &= \arctan2(R_{32} / \cos(\theta), R_{33} / \cos(\theta)) \\ \theta &= \arcsin(-R_{31}) \\ \psi &= \arctan2(R_{21} / \cos(\theta), R_{11} / \cos(\theta)) \end{aligned}$$

• X축, Y축, Z축 순서로 회전하는 경우를 기준으로 계산함.

https://cookierobotics.com/082/

ZYX Euler Angles

---

Example 2: from $\mathbf{R}$ to Euler Angle

우선 Rotation Vector $\mathbf{v} = [0.1, 0.2, 0.3]$을 사용하여 Rotation Matrix을 계산하고,
이를 Euler 각도로 변환해보겠음.

import cv2
import numpy as np

# 회전 벡터 (x, y, z 축에 대한 회전 각도)
v = np.array([0.1, 0.2, 0.3])

# 회전 벡터를 회전 행렬로 변환
R, _ = cv2.Rodrigues(v)

# 회전 행렬을 Euler 각도로 변환
def rotation_matrix_to_euler_angles(R):
    sy = np.sqrt(R[0, 0] ** 2 + R[1, 0] ** 2)
    singular = sy < 1e-6
    if not singular:
        x = np.arctan2(R[2, 1], R[2, 2])  # \phi = \arctan2(R_{3,2}, R_{3,3})
        y = np.arcsin(-R[2, 0])           # \theta = \arcsin(-R_{3,1})
        z = np.arctan2(R[1, 0], R[0, 0])  # \psi = \arctan2(R_{2,1}, R_{1,1})
    else:
        x = np.arctan2(-R[1, 2], R[1, 1])
        y = np.arcsin(-R[2, 0])
        z = 0
    return np.array([x, y, z])

euler_angles = rotation_matrix_to_euler_angles(R)

print("회전 벡터:")
print(v)
print("회전 행렬:")
print(R)
print("Euler 각도 (라디안):")
print(euler_angles)
print("Euler 각도 (도):")
print(np.degrees(euler_angles))

반대로 Eular angle로부터 Rotation Matrix 계산은 다음과 같음.

 

Euler 각도 $(\phi, \theta, \psi)$로부터 회전 행렬 $R_{XYZ순}$을 구하는 수식은 다음과 같음.

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

Euler 각도 순서가 XYZ(즉, roll-pitch-yaw)인 경우, 회전 행렬은 먼저 X축(roll), 다음 Y축(pitch), 마지막으로 Z축(yaw)을 기준으로 회전함.

이제 전체 회전 행렬 $R$는 다음과 같이 계산됩니다:
$$
R = R_z \cdot R_y \cdot R_x = \begin{bmatrix}
\cos(\psi) & -\sin(\psi) & 0 \\
\sin(\psi) & \cos(\psi) & 0 \\
0 & 0 & 1
\end{bmatrix}
\cdot
\begin{bmatrix}
\cos(\theta) & 0 & \sin(\theta) \\
0 & 1 & 0 \\
-\sin(\theta) & 0 & \cos(\theta)
\end{bmatrix}
\cdot
\begin{bmatrix}
1 & 0 & 0 \\
0 & \cos(\phi) & -\sin(\phi) \\
0 & \sin(\phi) & \cos(\phi)
\end{bmatrix}
$$

이를 풀어서 쓰면:
$$
R = \begin{bmatrix}
\cos(\psi)\cos(\theta) & \cos(\psi)\sin(\theta)\sin(\phi) - \sin(\psi)\cos(\phi) & \cos(\psi)\sin(\theta)\cos(\phi) + \sin(\psi)\sin(\phi) \\
\sin(\psi)\cos(\theta) & \sin(\psi)\sin(\theta)\sin(\phi) + \cos(\psi)\cos(\phi) & \sin(\psi)\sin(\theta)\cos(\phi) - \cos(\psi)\sin(\phi) \\
-\sin(\theta) & \cos(\theta)\sin(\phi) & \cos(\theta)\cos(\phi)
\end{bmatrix}
$$

다음은 Euler 각도로부터 회전 행렬을 구하는 예제입니다:

import numpy as np

# Euler 각도 (라디안)
euler_angles = np.array([0.34689985, 0.09455148, 0.19739826])

phi, theta, psi = euler_angles

# 각 축에 대한 회전 행렬 계산
R_x = np.array([
    [1, 0, 0],
    [0, np.cos(phi), -np.sin(phi)],
    [0, np.sin(phi), np.cos(phi)]
])

R_y = np.array([
    [np.cos(theta), 0, np.sin(theta)],
    [0, 1, 0],
    [-np.sin(theta), 0, np.cos(theta)]
])

R_z = np.array([
    [np.cos(psi), -np.sin(psi), 0],
    [np.sin(psi), np.cos(psi), 0],
    [0, 0, 1]
])

# 최종 회전 행렬 계산
R = np.dot(R_z, np.dot(R_y, R_x))

print("Euler 각도 (라디안):")
print(euler_angles)
print("Euler 각도 (도):")
print(np.degrees(euler_angles))
print("회전 행렬:")
print(R)

---

Example 3 : Scikit 사용하기.

scikit.spatial.transform 모듈에서 Rotation 클래스가 3차원 공간에서의 rotation을 추상화한 클래스이며,

여기서 from_rotvec 가 바로 rotation vector를 이용한 rotation 객체를 얻어낸다.

from scipy.spatial.transform import Rotation
from scipy.linalg import norm

# rotation vector를 통한 rotation 객체
r = Rotation.from_rotvec( np.pi/2. * np.array([0,0,1]))

# r의 rotation vector
rv = r.as_rotvec()
print(rv)
print(norm(rv,ord=2)

위의 예제는 z축으로 90도 회전(ccw)을 하는 rotation을 의미하는 r 을 얻고,

해당 r을 표현하는 rotation vector rv를 출력하고, rv의 length를 출력하는 예제임.

결과는 다음과 같음.

[0.         0.         1.57079633]
1.5707963267948963

이를 이용하여 3차원의 좌표로 구성된 x-y plane의 타원형을 z축으로 ccw 90도 rotation시키는 예제는 다음과 같음.

import numpy as np
import matplotlib.pyplot as plt
from scipy.spatial.transform import Rotation

m = 60 # num of samples
X = np.zeros((m,3)) # initialize 3D dataset

np.random.seed(42)
angles = (np.random.rand(m) ** 3 + 0.5) * 2 *np.pi # uneven distribution
X[:,0], X[:,1] = np.cos(angles), np.sin(angles) * 0.5 # oval(계란형, 타원형)
# X += 0.28 * np.random.randn(m, 3)  # add more noise

fig, axes = plt.subplots(figsize=(4,4))
axes.set_title('before')
axes.scatter(X[:,0],X[:,1])
axes.set_xlim([-1,1])
axes.set_ylim([-1,1])

r = Rotation.from_rotvec( np.pi/2. * np.array([0,0,1]))
rv = r.as_rotvec()
print(rv)
print(norm(rv,ord=2))

X_rot = r.apply(X)
fig, axes = plt.subplots(figsize=(4,4))
axes.set_title('after')
axes.scatter(X_rot[:,0],X_rot[:,1])
axes.set_xlim([-1,1])
axes.set_ylim([-1,1])

결과는 다음과 같음.

---

References

https://gist.github.com/dsaint31x/904de1e5b49dd8433894beedb7c10a30

py_scipy_rotation_rotvec.ipynb

 

https://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation#Rotation_vector

Axis–angle representation - Wikipedia

 

https://gofo-coding.tistory.com/entry/Orientation-Rotation#title-3

Orientation, Rotation