10.18 회전 행렬에서 쿼터니언 추출 (셰퍼드 방법)

1. 셰퍼드 방법의 개요

셰퍼드(S. W. Shepperd)가 1978년에 제안한 알고리즘은 회전 행렬에서 단위 쿼터니언을 추출하는 가장 표준적이고 수치적으로 안정한 방법이다. 이 알고리즘은 회전 행렬의 대각합과 대각선 원소를 분석하여 가장 큰 쿼터니언 성분을 먼저 결정하고, 나머지 성분을 비대각 원소로부터 계산한다.

이 방법은 단순한 대각합 기반 방법이 회전 각이 $\pi$ 에 가까울 때 발생하는 수치적 불안정성을 피한다.

2. 단순 방법의 한계

2.1 단순 대각합 방법

가장 간단한 방법은 대각합으로부터 $q_w$ 를 직접 계산하는 것이다.

$\mathrm{tr}(\mathbf{R}) = 4q_w^2 - 1 \quad \Rightarrow \quad q_w = \frac{\sqrt{\mathrm{tr}(\mathbf{R}) + 1}}{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}$

2.2 한계

이 방법은 $q_w$ 가 0에 가까울 때 분모가 0에 가까워 수치 오차가 발생한다. $q_w = 0$ 인 경우는 회전 각이 $\pi$ 인 회전(180도 회전)이며, 회전 행렬의 대각합이 $-1$ 이다.

180도 회전 근처에서 단순 대각합 방법은 실패한다.

3. 셰퍼드 방법의 핵심 아이디어

셰퍼드 방법의 핵심 아이디어는 가장 큰 쿼터니언 성분을 먼저 결정하는 것이다. 단위 쿼터니언의 네 성분 중 적어도 하나는 절댓값이 $1/2$ 이상이어야 한다(노름이 1이므로). 가장 큰 성분을 사용하여 분모가 충분히 크도록 보장한다.

4. 알고리즘

4.1 단계 1: 네 후보 양의 계산

회전 행렬의 대각 원소와 대각합으로부터 네 후보 양을 계산한다.

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

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

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

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

이 양들은 각각 $4q_w^2$ , $4q_x^2$ , $4q_y^2$ , $4q_z^2$ 에 대응된다.

4.2 단계 2: 가장 큰 양 선택

네 양 중 가장 큰 것을 선택한다.

$T_{\max} = \max(T_w, T_x, T_y, T_z)$

이는 가장 큰 절댓값을 가진 쿼터니언 성분에 대응된다. $T_{\max}$ 가 최소 1 이상이므로(단위 쿼터니언 조건), 분모로 사용해도 안정적이다.

4.3 단계 3: 가장 큰 성분 계산

선택된 양에 따라 가장 큰 쿼터니언 성분을 계산한다.

4.3.1 Case 1: $T_{\max} = T_w$

$q_w = \frac{\sqrt{T_w}}{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}$

4.3.2 Case 2: $T_{\max} = T_x$

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

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

$q_y = \frac{r_{12} + r_{21}}{4q_x}$

$q_z = \frac{r_{13} + r_{31}}{4q_x}$

4.3.3 Case 3: $T_{\max} = T_y$

$q_y = \frac{\sqrt{T_y}}{2}$

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

$q_x = \frac{r_{12} + r_{21}}{4q_y}$

$q_z = \frac{r_{23} + r_{32}}{4q_y}$

4.3.4 Case 4: $T_{\max} = T_z$

$q_z = \frac{\sqrt{T_z}}{2}$

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

$q_x = \frac{r_{13} + r_{31}}{4q_z}$

$q_y = \frac{r_{23} + r_{32}}{4q_z}$

각 경우에서 가장 큰 성분을 먼저 계산하고, 나머지 성분을 비대각 원소로부터 추출한다.

5. 알고리즘의 의사 코드

function shepperd_extract(R):
    Tw = 1 + R[0,0] + R[1,1] + R[2,2]
    Tx = 1 + R[0,0] - R[1,1] - R[2,2]
    Ty = 1 - R[0,0] + R[1,1] - R[2,2]
    Tz = 1 - R[0,0] - R[1,1] + R[2,2]
    
    Tmax = max(Tw, Tx, Ty, Tz)
    
    if Tmax == Tw:
        S = 2 * sqrt(Tw)
        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 Tmax == Tx:
        S = 2 * sqrt(Tx)
        qx = S / 4
        qw = (R[2,1] - R[1,2]) / S
        qy = (R[0,1] + R[1,0]) / S
        qz = (R[0,2] + R[2,0]) / S
    elif Tmax == Ty:
        S = 2 * sqrt(Ty)
        qy = S / 4
        qw = (R[0,2] - R[2,0]) / S
        qx = (R[0,1] + R[1,0]) / S
        qz = (R[1,2] + R[2,1]) / S
    else:  # Tmax == Tz
        S = 2 * sqrt(Tz)
        qz = S / 4
        qw = (R[1,0] - R[0,1]) / S
        qx = (R[0,2] + R[2,0]) / S
        qy = (R[1,2] + R[2,1]) / S
    
    return Quaternion(qw, qx, qy, qz)

6. 알고리즘의 수치적 안정성

6.1 분모의 크기

가장 큰 $T_i$ 를 선택하므로 $S = 2\sqrt{T_{\max}}$ 가 충분히 크다. 단위 쿼터니언의 경우 적어도 한 성분이 $\pm 1/2$ 이상이므로, $T_{\max} \geq 1$ 이고 $S \geq 2$ 이다. 따라서 분모가 안정적이다.

6.2 모든 경우의 처리

네 가지 경우 모두 안정적이며, 각 경우에서 가장 큰 성분을 분모로 사용하므로 수치 오차가 작다. 어떤 회전 각에서도 실패하지 않는다.

7. 부호의 결정

셰퍼드 방법은 가장 큰 성분의 부호를 양수로 고정한다. 이는 부호 이중성( $\mathbf{q}$ 와 $-\mathbf{q}$ )을 일관되게 처리하는 한 방법이다.

다른 방법은 $q_w$ 를 항상 양수로 고정하는 것이지만, 이는 $q_w$ 가 0에 가까울 때 부호가 불안정해질 수 있다. 셰퍼드 방법이 더 안정적이다.

8. 셰퍼드 방법의 효율성

8.1 계산 비용

네 양의 계산: 약 6 덧셈
최대값 결정: 3 비교
가장 큰 성분 계산: 1 제곱근, 1 나누기
나머지 성분 계산: 9 덧셈, 9 나누기

총 약 15 덧셈, 10 나누기, 1 제곱근이다.

8.2 단순 방법과의 비교

단순 대각합 방법보다 약간 더 비용이 들지만, 수치 안정성의 이점이 훨씬 크다. 안정적인 결과를 위해 셰퍼드 방법이 권장된다.

9. 셰퍼드 방법의 한계

9.1 분기 비용

알고리즘이 네 가지 경우로 분기되므로, 분기 예측이 어려울 수 있다. CPU 파이프라인의 효율을 약간 저하시킬 수 있다.

9.2 대안 알고리즘

분기 없는 알고리즘도 존재하지만, 일반적으로 셰퍼드 방법이 가장 견고하고 표준적이다.

10. 검증

셰퍼드 방법으로 추출한 쿼터니언을 다음과 같이 검증한다.

10.1 단위 노름 검사

$\lVert\mathbf{q}\rVert^2 = q_w^2 + q_x^2 + q_y^2 + q_z^2 \approx 1$

부동 소수점 오차 임계값 내에서 만족해야 한다.

10.2 회전 행렬로의 재변환

추출된 쿼터니언을 다시 회전 행렬로 변환하면 원래 행렬과 일치해야 한다.

$\mathbf{R}(\mathrm{shepperd\_extract}(\mathbf{R})) \approx \mathbf{R}$

11. 알고리즘의 변형

11.1 마칼리(Markley)의 변형

마칼리(F. L. Markley)는 셰퍼드 알고리즘의 변형을 제안하여 일부 경우에서 정확도를 향상시켰다. 이 변형은 SVD 기반 방법과 유사한 결과를 산출한다.

11.2 베일리(Bailey)의 알고리즘

베일리(D. C. Bailey)의 알고리즘은 분기를 줄여 효율성을 개선한 변형이다.

이러한 변형은 표준 셰퍼드 방법과 유사한 결과를 산출하며, 응용에 따라 선택될 수 있다.

12. 셰퍼드 방법의 활용

12.1 자세 추정 시스템

회전 행렬 기반 자세 추정 시스템에서 결과를 쿼터니언으로 변환할 때 사용된다.

12.2 컴퓨터 비전

PnP, SVD 기반 자세 추정 등의 결과(회전 행렬)를 쿼터니언으로 변환한다.

12.3 로봇 캘리브레이션

캘리브레이션 알고리즘이 회전 행렬을 출력하지만 후속 처리에 쿼터니언이 필요한 경우 사용된다.

12.4 라이브러리 변환

회전 행렬을 사용하는 라이브러리(OpenCV 등)와 쿼터니언을 사용하는 라이브러리(ROS 등) 사이의 변환에 사용된다.

13. 셰퍼드 방법과 다른 알고리즘

13.1 SVD 기반 방법

회전 행렬로부터 쿼터니언을 추출하는 다른 방법은 SVD를 사용하는 것이다. 그러나 SVD는 더 비싼 연산이며, 셰퍼드 방법이 일반적으로 더 효율적이다.

13.2 대수적 방법

회전 행렬을 회전 축과 회전 각으로 분해한 후 쿼터니언을 구성하는 방법도 있다. 이는 셰퍼드 방법과 비슷한 효율성을 가지지만 다른 수치 특성을 가진다.

13.3 Bar-Itzhack의 방법

Bar-Itzhack(2000)이 제안한 방법은 회전 행렬로부터 쿼터니언을 추출하는 또 다른 견고한 방법이다. 4개의 쿼터니언 성분을 포함하는 4x4 대칭 행렬의 가장 큰 고유값에 해당하는 고유 벡터로 쿼터니언을 추출한다. 이 방법은 회전 행렬이 정확한 회전 행렬이 아닐 때(예: 부동 소수점 오차로 정규 직교성이 손실된 경우)에도 가장 가까운 회전 행렬에 대응하는 쿼터니언을 산출한다.

14. 부정확한 회전 행렬의 처리

실제 응용에서 입력 회전 행렬이 부동 소수점 오차로 인해 정확한 회전 행렬이 아닐 수 있다. 셰퍼드 방법은 이를 어느 정도 처리할 수 있지만, 큰 오차가 있으면 부정확한 결과를 산출할 수 있다.

이러한 경우 다음의 방법이 사용된다.

사전 정규화: 입력 행렬을 SVD 기반 투영으로 정규화한 후 셰퍼드 방법 적용
Bar-Itzhack 방법: 가장 가까운 회전 행렬에 대응하는 쿼터니언을 직접 추정

15. 라이브러리 구현

대부분의 회전 라이브러리가 셰퍼드 방법 또는 그 변형을 구현하고 있다.

Eigen: Eigen::Quaternion::Quaternion(const MatrixBase<Derived>&)이 셰퍼드 방법 사용
ROS의 tf2: tf2::toMsg와 tf2::fromMsg에서 변환 지원
OpenCV: Rodrigues 함수와 결합하여 변환 가능
NumPy/SciPy: scipy.spatial.transform.Rotation이 변환 지원

이러한 라이브러리는 검증되어 있으므로 직접 구현보다 활용이 권장된다.

16. 참고 문헌

Shepperd, S. W. (1978). “Quaternion from Rotation Matrix.” Journal of Guidance, Control, and Dynamics, 1(3), 223–224.
Bar-Itzhack, I. Y. (2000). “New Method for Extracting the Quaternion from a Rotation Matrix.” Journal of Guidance, Control, and Dynamics, 23(6), 1085–1087.
Markley, F. L. (2008). “Unit Quaternion from Rotation Matrix.” Journal of Guidance, Control, and Dynamics, 31(2), 440–442.
Diebel, J. (2006). “Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors.” Stanford University Technical Report.
Sola, J. (2017). “Quaternion Kinematics for the Error-State Kalman Filter.” arXiv:1711.02508.

version: 1.0