10.79 Eigen 라이브러리의 쿼터니언 클래스

10.79 Eigen 라이브러리의 쿼터니언 클래스

1. Eigen 라이브러리의 개요

Eigen은 C++로 작성된 헤더 전용(header-only) 선형 대수 라이브러리이며, 행렬, 벡터, 변환, 기하학적 연산을 위한 풍부한 기능을 제공한다. 가브리엘 게네보(Gaël Guennebaud)와 베누아 자콥(Benoît Jacob)이 주도하여 개발하였으며, 현재 가장 널리 사용되는 C++ 선형 대수 라이브러리 중 하나이다. Eigen의 기하학 모듈에는 쿼터니언을 다루는 Eigen::Quaternion 클래스가 포함되어 있으며, 로봇공학과 컴퓨터 그래픽스를 비롯한 다양한 응용에서 표준적으로 사용된다.

2. Eigen Quaternion 클래스의 정의

2.1 클래스 선언

Eigen의 쿼터니언 클래스는 템플릿으로 정의되어 있다.

namespace Eigen {
    template <typename Scalar>
    class Quaternion;
}

여기서 Scalar는 부동 소수점 형식이며, 일반적으로 float 또는 double을 사용한다.

2.2 형식 별칭

자주 사용되는 형식에 대한 별칭이 정의되어 있다.

typedef Quaternion<float> Quaternionf;
typedef Quaternion<double> Quaterniond;

2.3 헤더 포함

쿼터니언 클래스를 사용하려면 다음의 헤더를 포함한다.

#include <Eigen/Geometry>

이 헤더는 쿼터니언 외에도 다른 기하학적 클래스(회전, 변환 등)를 포함한다.

3. 데이터 표현

3.1 메모리 레이아웃

Eigen의 쿼터니언은 4개의 스칼라 값으로 표현된다. 메모리 상의 순서는 [x, y, z, w]이지만, 생성자와 접근자는 일반적으로 [w, x, y, z] 순서를 사용한다. 이는 사용 시 혼동을 일으킬 수 있는 점이다.

3.2 메모리 정렬

성능 최적화를 위해 쿼터니언 데이터가 16바이트 경계에 정렬된다. 이로 인해 SIMD 명령어를 효율적으로 활용할 수 있다.

4. 생성과 초기화

4.1 기본 생성자

기본 생성자는 쿼터니언을 초기화하지 않는다.

Eigen::Quaternionf q;  // uninitialized

4.2 성분으로 초기화

네 개의 성분으로 직접 초기화할 수 있다. 생성자 인자는 [w, x, y, z] 순서이다.

Eigen::Quaternionf q(1.0f, 0.0f, 0.0f, 0.0f);  // identity

4.3 항등 쿼터니언

Eigen::Quaternionf q = Eigen::Quaternionf::Identity();

4.4 축-각도로부터 생성

축-각도 표현으로부터 쿼터니언을 생성할 수 있다.

Eigen::Vector3f axis(1.0f, 0.0f, 0.0f);
float angle = M_PI / 4;
Eigen::Quaternionf q(Eigen::AngleAxisf(angle, axis));

4.5 회전 행렬로부터 생성

회전 행렬로부터 쿼터니언을 생성할 수 있다.

Eigen::Matrix3f R = ...;
Eigen::Quaternionf q(R);

이 변환은 셰퍼드 방법을 사용하여 수치적으로 안정적이다.

4.6 오일러 각으로부터 생성

오일러 각을 직접 받는 생성자는 없지만, AngleAxis와의 곱으로 변환할 수 있다.

Eigen::Quaternionf q = Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ())
                     * Eigen::AngleAxisf(pitch, Eigen::Vector3f::UnitY())
                     * Eigen::AngleAxisf(roll, Eigen::Vector3f::UnitX());

5. 성분 접근

5.1 개별 성분 접근

각 성분에 접근하기 위한 메서드가 제공된다.

float w = q.w();
float x = q.x();
float y = q.y();
float z = q.z();

5.2 스칼라와 벡터 부분

스칼라 부분과 벡터 부분으로 나누어 접근할 수도 있다.

float scalar = q.w();
Eigen::Vector3f vec = q.vec();  // [x, y, z]

5.3 계수 접근

전체 계수에 벡터로 접근할 수 있다. 이 경우 메모리 순서인 [x, y, z, w]로 반환된다.

Eigen::Vector4f coeffs = q.coeffs();  // [x, y, z, w]

6. 기본 연산

6.1 쿼터니언 곱

두 쿼터니언의 곱은 * 연산자로 표현된다.

Eigen::Quaternionf q1 = ...;
Eigen::Quaternionf q2 = ...;
Eigen::Quaternionf q3 = q1 * q2;

6.2 켤레

Eigen::Quaternionf q_conj = q.conjugate();

6.3 역원

Eigen::Quaternionf q_inv = q.inverse();

단위 쿼터니언의 경우 역원은 켤레와 같지만, Eigen은 일반 쿼터니언의 역원도 정확히 계산한다.

6.4 노름과 정규화

float norm = q.norm();
float squared_norm = q.squaredNorm();
q.normalize();  // in-place
Eigen::Quaternionf q_norm = q.normalized();  // returns new

6.5 내적

float dot = q1.dot(q2);

7. 회전 연산

7.1 벡터 회전

쿼터니언을 사용하여 벡터를 회전시키는 방법은 두 가지가 있다.

Eigen::Vector3f v(1.0f, 0.0f, 0.0f);
Eigen::Vector3f v_rot = q * v;

또는 명시적으로

Eigen::Vector3f v_rot = q._transformVector(v);

내부적으로 효율적인 알고리즘이 사용된다.

7.2 회전 행렬로 변환

쿼터니언을 회전 행렬로 변환할 수 있다.

Eigen::Matrix3f R = q.toRotationMatrix();

이는 자세 표현 변환에 자주 사용된다.

7.3 회전 각

두 쿼터니언 사이의 각도를 계산할 수 있다.

float angle = q1.angularDistance(q2);

8. 보간

8.1 SLERP

Eigen은 SLERP을 지원한다.

Eigen::Quaternionf q_interp = q1.slerp(t, q2);

여기서 t는 0에서 1 사이의 매개변수이다. 부호 처리도 자동으로 수행된다.

9. 자세 표현 변환

9.1 AngleAxis로 변환

Eigen::AngleAxisf aa(q);
float angle = aa.angle();
Eigen::Vector3f axis = aa.axis();

9.2 오일러 각으로 변환

오일러 각으로의 직접 변환은 회전 행렬을 거친다.

Eigen::Vector3f euler = q.toRotationMatrix().eulerAngles(2, 1, 0);  // ZYX

회전 순서를 명시적으로 지정해야 한다.

9.3 동차 변환과의 결합

쿼터니언과 위치를 결합하여 동차 변환을 형성할 수 있다.

Eigen::Affine3f transform = Eigen::Affine3f::Identity();
transform.translation() = Eigen::Vector3f(1.0f, 2.0f, 3.0f);
transform.linear() = q.toRotationMatrix();

10. Map 클래스

Eigen은 외부 메모리에서 쿼터니언을 사용할 수 있는 Map 클래스를 제공한다.

float data[4] = {0.0f, 0.0f, 0.0f, 1.0f};  // [x, y, z, w]
Eigen::Map<Eigen::Quaternionf> q_map(data);

이는 ROS 메시지나 외부 라이브러리와의 데이터 공유에 유용하다.

11. 사용 예시

11.1 자세 누적

연속된 회전을 누적하는 예시이다.

Eigen::Quaternionf q_total = Eigen::Quaternionf::Identity();
for (auto& delta_q : rotations) {
    q_total = q_total * delta_q;
    q_total.normalize();
}

정기적인 정규화로 수치 오차의 누적을 방지한다.

11.2 자세 보간

두 자세 사이의 부드러운 전이이다.

Eigen::Quaternionf start = ...;
Eigen::Quaternionf end = ...;
for (float t = 0.0f; t <= 1.0f; t += 0.01f) {
    Eigen::Quaternionf q_t = start.slerp(t, end);
    // use q_t
}

11.3 자세 오차 계산

두 자세 사이의 오차 쿼터니언을 계산한다.

Eigen::Quaternionf q_error = q_target * q_current.inverse();
float angle_error = 2.0f * std::acos(std::abs(q_error.w()));

11.4 각속도로부터의 적분

각속도로 자세를 갱신하는 예시이다.

Eigen::Vector3f omega = ...;  // angular velocity
float dt = 0.01f;
Eigen::Quaternionf delta_q;
float angle = omega.norm() * dt;
if (angle > 1e-10) {
    Eigen::Vector3f axis = omega.normalized();
    delta_q = Eigen::AngleAxisf(angle, axis);
} else {
    delta_q = Eigen::Quaternionf::Identity();
}
q = q * delta_q;
q.normalize();

12. 성능과 최적화

12.1 SIMD 활용

Eigen은 SSE, AVX 등의 SIMD 명령어를 자동으로 활용한다. 16바이트 정렬된 쿼터니언 연산이 매우 효율적이다.

12.2 표현식 템플릿

Eigen은 표현식 템플릿(expression template) 기법을 사용하여 임시 객체 생성을 최소화한다. 이는 복잡한 연산을 효율적으로 처리한다.

12.3 컴파일 최적화

-O3 -march=native 등의 컴파일러 옵션을 사용하면 Eigen의 최적화 효과가 극대화된다.

13. ROS와의 통합

13.1 ROS 메시지 변환

ROS의 geometry_msgs/Quaternion과 Eigen 쿼터니언 사이의 변환은 다음과 같다.

#include <tf2_eigen/tf2_eigen.h>
geometry_msgs::Quaternion msg = ...;
Eigen::Quaterniond q = tf2::transformToEigen(msg).rotation();

13.2 tf2_eigen

tf2_eigen 패키지는 ROS와 Eigen 사이의 변환 함수를 제공한다.

14. 일반적인 함정

14.1 부호 모호성

q와 -q가 같은 회전을 표현하지만, 비교 시 다른 객체로 처리된다. 비교 시 부호 일관성을 유지해야 한다.

14.2 정규화 잊기

수치 적분 후 정규화를 잊으면 단위성이 손상된다. 매 갱신 단계에서 정규화하는 습관이 필요하다.

14.3 인자 순서 혼동

생성자는 [w, x, y, z] 순서이지만, coeffs() 메서드는 [x, y, z, w]를 반환한다. 이러한 차이를 명확히 인식해야 한다.

14.4 작은 노름 처리

매우 작은 노름의 쿼터니언을 정규화하면 수치 오류가 발생할 수 있다. 임계값을 설정하여 처리한다.

15. 확장 라이브러리

15.1 Sophus

Sophus는 Eigen 위에 구축된 리 군 라이브러리로, SO(3), SE(3) 등의 회전 군을 다룬다. Eigen 쿼터니언을 내부적으로 사용한다.

15.2 manif

manif도 리 군 라이브러리이며, Eigen 쿼터니언과 호환된다.

15.3 Ceres Solver

Google의 Ceres Solver는 비선형 최적화 라이브러리이며, Eigen 쿼터니언을 사용하여 자세 최적화를 수행한다.

16. 디버깅 팁

16.1 출력

Eigen 쿼터니언을 출력할 때는 명시적으로 성분을 출력한다.

std::cout << "q = [" << q.w() << ", " << q.x() << ", " 
          << q.y() << ", " << q.z() << "]" << std::endl;

16.2 단위성 검증

assert(std::abs(q.norm() - 1.0f) < 1e-6f);

16.3 NaN 검출

if (std::isnan(q.w()) || std::isnan(q.x()) || 
    std::isnan(q.y()) || std::isnan(q.z())) {
    // handle NaN
}

17. 참고 문헌

  • Guennebaud, G., Jacob, B., et al. (2010). Eigen v3. http://eigen.tuxfamily.org
  • Eigen Documentation. (2024). Eigen 3.4 Reference Manual. http://eigen.tuxfamily.org/dox/
  • Strasdat, H. (2012). Sophus: C++ implementation of Lie Groups using Eigen. https://github.com/strasdat/Sophus
  • Solà, J., Deray, J., & Atchuthan, D. (2018). “A Micro Lie Theory for State Estimation in Robotics.” arXiv:1812.01537.

version: 1.0