10.17 쿼터니언과 회전 행렬의 상호 변환 공식

1. 양방향 변환의 필요성

쿼터니언과 회전 행렬은 각각 회전 표현의 장점을 가지므로, 응용에 따라 두 표현 사이를 자유롭게 변환하는 것이 필요하다.

쿼터니언: 메모리 효율, 합성 효율, 보간 매끄러움
회전 행렬: 점 변환의 효율, 직접적 행렬 연산

본 절에서는 두 표현 사이의 닫힌 형태 변환 공식을 제시한다.

2. 쿼터니언에서 회전 행렬로의 변환

단위 쿼터니언 $\mathbf{q} = (q_w, q_x, q_y, q_z)$ 가 주어졌을 때, 대응하는 회전 행렬은 다음과 같다.

$\mathbf{R}(\mathbf{q}) = \begin{bmatrix} 1 - 2(q_y^2 + q_z^2) & 2(q_x q_y - q_w q_z) & 2(q_x q_z + q_w q_y) \\ 2(q_x q_y + q_w q_z) & 1 - 2(q_x^2 + q_z^2) & 2(q_y q_z - q_w q_x) \\ 2(q_x q_z - q_w q_y) & 2(q_y q_z + q_w q_x) & 1 - 2(q_x^2 + q_y^2) \end{bmatrix}$

이 행렬은 단위 노름 조건 $q_w^2 + q_x^2 + q_y^2 + q_z^2 = 1$ 하에서 정확히 회전 행렬이다.

3. 변환 공식의 유도

이 공식은 회전 공식 $\mathbf{v}' = \mathbf{q}\mathbf{v}\mathbf{q}^*$ 의 명시적 전개로부터 얻을 수 있다. 회전 공식은 벡터 $\mathbf{v}$ 에 대해 선형이므로

$\mathbf{v}' = \mathbf{R}(\mathbf{q})\mathbf{v}$

의 형태로 표현된다. $\mathbf{R}(\mathbf{q})$ 의 각 원소는 표준 기저 벡터 $\hat{\mathbf{e}}_x, \hat{\mathbf{e}}_y, \hat{\mathbf{e}}_z$ 를 회전 공식에 대입하여 직접 계산할 수 있다.

예를 들어, $\mathbf{R}(\mathbf{q})$ 의 첫 번째 열은 $\mathbf{q}\hat{\mathbf{e}}_x\mathbf{q}^*$ 의 결과이다.

4. 변환의 다른 형태

위의 공식은 단위 쿼터니언 조건 $q_w^2 + q_x^2 + q_y^2 + q_z^2 = 1$ 을 사용하여 일부 항을 단순화한 형태이다. 사용하지 않은 형태도 존재한다.

4.1 동차 형태

$\mathbf{R}(\mathbf{q}) = \begin{bmatrix} q_w^2 + q_x^2 - q_y^2 - q_z^2 & 2(q_x q_y - q_w q_z) & 2(q_x q_z + q_w q_y) \\ 2(q_x q_y + q_w q_z) & q_w^2 - q_x^2 + q_y^2 - q_z^2 & 2(q_y q_z - q_w q_x) \\ 2(q_x q_z - q_w q_y) & 2(q_y q_z + q_w q_x) & q_w^2 - q_x^2 - q_y^2 + q_z^2 \end{bmatrix}$

이 형태는 $q_w^2 + q_x^2 + q_y^2 + q_z^2 = 1$ 을 가정하지 않으며, 일반 쿼터니언에 대해서도 유효하다. 단위 쿼터니언의 경우 위의 단순화된 형태와 같다.

5. 계산 비용

쿼터니언에서 회전 행렬로의 변환은 다음의 연산을 필요로 한다.

약 18 곱셈 (대각선 원소 9, 비대각선 원소 18)
약 12 덧셈

이는 한 번의 변환에 대해 효율적이다. 점 변환을 여러 번 수행하기 전에 한 번 변환하면 효율적이다.

6. 회전 행렬에서 쿼터니언으로의 변환

역방향 변환은 더 복잡하다. 회전 행렬 $\mathbf{R} = [r_{ij}]$ 로부터 단위 쿼터니언을 추출하는 방법은 여러 가지가 있다.

6.1 방법 1: 대각합으로부터의 추출

회전 행렬의 대각합과 쿼터니언의 관계를 이용한다.

$\mathrm{tr}(\mathbf{R}) = r_{11} + r_{22} + r_{33}$

쿼터니언과 회전 행렬의 관계로부터

$\mathrm{tr}(\mathbf{R}) = 4q_w^2 - 1$

따라서

$q_w = \frac{\sqrt{\mathrm{tr}(\mathbf{R}) + 1}}{2}$

벡터 부분은 비대각 원소에서 추출된다.

$q_x = \frac{r_{32} - r_{23}}{4q_w}$

$q_y = \frac{r_{13} - r_{31}}{4q_w}$

$q_z = \frac{r_{21} - r_{12}}{4q_w}$

6.2 방법 1의 한계

$q_w$ 가 0에 가까우면(즉, $\mathrm{tr}(\mathbf{R})$ 이 $-1$ 에 가까우면) 분모가 0에 가까워 수치 오차가 발생한다. 이는 회전 각이 $\pi$ 에 가까울 때이다.

6.3 방법 2: Shepperd의 알고리즘

Shepperd(1978)가 제안한 알고리즘은 위의 한계를 피한다. $\mathbf{R}$ 의 대각 원소 중 가장 큰 것을 선택하여 대응하는 쿼터니언 성분을 먼저 계산한다.

6.3.1 단계 1: 가장 큰 항 결정

다음 네 양 중 가장 큰 것을 결정한다.

$T_0 = 1 + r_{11} + r_{22} + r_{33}$

$T_1 = 1 + r_{11} - r_{22} - r_{33}$

$T_2 = 1 - r_{11} + r_{22} - r_{33}$

$T_3 = 1 - r_{11} - r_{22} + r_{33}$

6.3.2 단계 2: 가장 큰 성분 계산

$T_0$ 가 가장 크면 $q_w$ 를 먼저 계산:

$q_w = \frac{\sqrt{T_0}}{2}$

$q_x = \frac{r_{32} - r_{23}}{4q_w}, \quad q_y = \frac{r_{13} - r_{31}}{4q_w}, \quad q_z = \frac{r_{21} - r_{12}}{4q_w}$

$T_1$ 이 가장 크면 $q_x$ 를 먼저 계산:

$q_x = \frac{\sqrt{T_1}}{2}$

$q_w = \frac{r_{32} - r_{23}}{4q_x}, \quad q_y = \frac{r_{12} + r_{21}}{4q_x}, \quad q_z = \frac{r_{13} + r_{31}}{4q_x}$

다른 경우( $T_2$ , $T_3$ )에도 비슷한 공식이 적용된다.

6.4 방법 2의 장점

가장 큰 $T_i$ 를 선택하므로 분모가 충분히 크고, 수치적 안정성이 보장된다. 모든 회전 각 범위에서 잘 작동한다.

7. 변환 알고리즘

견고한 회전 행렬에서 쿼터니언으로의 변환 알고리즘을 의사 코드로 표현하면 다음과 같다.

function matrix_to_quaternion(R):
    trace = R[0,0] + R[1,1] + R[2,2]
    
    if trace > 0:
        S = 2 * sqrt(trace + 1)
        qw = S / 4
        qx = (R[2,1] - R[1,2]) / S
        qy = (R[0,2] - R[2,0]) / S
        qz = (R[1,0] - R[0,1]) / S
    elif R[0,0] > R[1,1] and R[0,0] > R[2,2]:
        S = 2 * sqrt(1 + R[0,0] - R[1,1] - R[2,2])
        qw = (R[2,1] - R[1,2]) / S
        qx = S / 4
        qy = (R[0,1] + R[1,0]) / S
        qz = (R[0,2] + R[2,0]) / S
    elif R[1,1] > R[2,2]:
        S = 2 * sqrt(1 + R[1,1] - R[0,0] - R[2,2])
        qw = (R[0,2] - R[2,0]) / S
        qx = (R[0,1] + R[1,0]) / S
        qy = S / 4
        qz = (R[1,2] + R[2,1]) / S
    else:
        S = 2 * sqrt(1 + R[2,2] - R[0,0] - R[1,1])
        qw = (R[1,0] - R[0,1]) / S
        qx = (R[0,2] + R[2,0]) / S
        qy = (R[1,2] + R[2,1]) / S
        qz = S / 4
    
    return Quaternion(qw, qx, qy, qz)

이 알고리즘은 모든 회전 각에서 안정적이다.

8. 변환의 검증

변환의 정확성을 다음과 같이 검증할 수 있다.

8.1 왕복 변환

쿼터니언을 회전 행렬로 변환한 후 다시 쿼터니언으로 변환했을 때, 원래 쿼터니언(또는 그 부호 반전)을 얻어야 한다.

$\mathrm{matrix\_to\_quaternion}(\mathrm{quaternion\_to\_matrix}(\mathbf{q})) = \pm\mathbf{q}$

8.2 회전 행렬의 검증

변환된 행렬이 회전 행렬임을 확인한다.

$\mathbf{R}^T\mathbf{R} = \mathbf{I}, \quad \det(\mathbf{R}) = 1$

수치 오차의 임계값 내에서 만족해야 한다.

9. 부호의 일관성

회전 행렬에서 쿼터니언으로의 변환은 부호 이중성을 결정한다. 두 가지 일관성 보장 방법이 있다.

9.1 방법 1: 항상 양의 스칼라

$q_w \geq 0$ 이 되도록 한다. 이는 표준 형태이며, 같은 회전이 항상 같은 쿼터니언으로 매핑된다.

9.2 방법 2: 이전 쿼터니언과의 연속성

이전 시각의 쿼터니언과 가까운 쿼터니언을 선택한다. 이는 자세 추정 시퀀스에서 부호 점프를 방지한다.

10. 변환의 응용

10.1 자세 추정

자세 추정 알고리즘의 결과가 쿼터니언이지만 시각화나 다른 처리를 위해 회전 행렬로 변환한다.

10.2 점 변환

쿼터니언으로 표현된 자세를 점군이나 메쉬 변환에 사용할 때, 회전 행렬로 변환한 후 적용한다. 점 변환에서는 회전 행렬이 더 효율적이다.

10.3 라이브러리 호환성

다른 라이브러리가 다른 표현을 사용할 때, 변환 함수가 필수이다. ROS는 쿼터니언을, OpenCV는 회전 행렬을 주로 사용하므로 변환이 자주 필요하다.

10.4 캘리브레이션

캘리브레이션 알고리즘이 회전 행렬을 출력하지만 결과를 쿼터니언으로 저장하거나 추가 처리할 수 있다.

10.5 시각화

3D 시각화 도구가 회전 행렬 또는 쿼터니언을 입력으로 받는다. 내부 표현과 다른 경우 변환이 필요하다.

11. 변환 관련 라이브러리

다양한 라이브러리가 두 표현 사이의 변환을 제공한다.

11.1 Eigen

C++의 Eigen 라이브러리는 Eigen::Quaternion과 Eigen::Matrix3d 사이의 변환을 자동으로 처리한다.

Eigen::Quaterniond q;
Eigen::Matrix3d R = q.toRotationMatrix();
Eigen::Quaterniond q2(R);

11.2 Sophus

Sophus 라이브러리는 $SO(3)$ 과 $SE(3)$ 의 매니폴드 연산을 지원하며, 쿼터니언과 회전 행렬 변환이 포함된다.

11.3 ROS의 tf2

ROS의 tf2 패키지는 쿼터니언과 회전 행렬 사이의 변환 함수를 제공한다.

11.4 Python의 SciPy

SciPy의 spatial.transform.Rotation 클래스는 쿼터니언, 회전 행렬, 오일러 각 등 다양한 회전 표현 사이의 변환을 지원한다.

12. 변환의 수치적 안정성

12.1 단위 노름의 유지

쿼터니언 입력이 단위 노름이 아니면 결과 회전 행렬이 정확한 회전 행렬이 아닐 수 있다. 변환 전에 정규화하는 것이 안전하다.

12.2 회전 행렬의 직교성

입력 회전 행렬이 정확히 직교적이지 않으면(부동 소수점 오차로) 변환된 쿼터니언이 정확히 단위 노름이 아닐 수 있다. 변환 후 정규화한다.

12.3 작은 회전과 큰 회전

작은 회전에서는 모든 변환 방법이 안정적이다. 큰 회전( $\phi \approx \pi$ )에서는 Shepperd의 알고리즘이 권장된다.

13. 참고 문헌

Shepperd, S. W. (1978). “Quaternion from Rotation Matrix.” Journal of Guidance, Control, and Dynamics, 1(3), 223–224.
Diebel, J. (2006). “Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors.” Stanford University Technical Report.
Shoemake, K. (1985). “Animating Rotation with Quaternion Curves.” SIGGRAPH Computer Graphics, 19(3), 245–254.
Kuipers, J. B. (1999). Quaternions and Rotation Sequences. Princeton University Press.
Sola, J. (2017). “Quaternion Kinematics for the Error-State Kalman Filter.” arXiv:1711.02508.

version: 1.0