659.73 tf2_eigen을 이용한 Eigen 라이브러리 연동

1. 개요

Eigen은 C++ 기반의 고성능 선형 대수 라이브러리로, 행렬 연산, 벡터 연산, 기하학적 변환, 분해(decomposition) 등 수치 선형 대수의 광범위한 기능을 제공한다. 로봇 공학 분야에서 Eigen은 사실상의 표준 수치 연산 라이브러리로 활용되며, PCL(Point Cloud Library), MoveIt 2, OpenCV, Ceres Solver 등 주요 로봇 소프트웨어 라이브러리가 Eigen에 의존하고 있다. tf2_eigen 패키지는 TF2의 변환 타입과 Eigen의 기하학적 타입 간의 양방향 변환을 제공하여, 두 프레임워크의 원활한 통합을 가능하게 한다.

2. tf2_eigen의 핵심 기능

2.1 타입 변환 인터페이스

tf2_eigen은 다음의 핵심 기능을 제공한다.

1. Eigen ↔ ROS 메시지 변환: Eigen 타입과 geometry_msgs 타입 간의 양방향 변환
2. doTransform() 특수화: Eigen 타입에 대한 TF2 변환 파이프라인 통합
3. fromMsg()/toMsg(): Eigen 타입에 대한 TF2 표준 변환 함수 제공

2.2 지원 타입 매핑

3. 의존성 설정

3.1 CMakeLists.txt

find_package(tf2_eigen REQUIRED)
find_package(Eigen3 REQUIRED)

ament_target_dependencies(${PROJECT_NAME}
  tf2
  tf2_ros
  tf2_eigen
)

3.2 package.xml

<depend>tf2_eigen</depend>
<build_depend>eigen</build_depend>

3.3 헤더 포함

#include <tf2_eigen/tf2_eigen.hpp>
#include <Eigen/Geometry>

4. Eigen ↔ ROS 메시지 변환

4.1 Point 변환

#include <tf2_eigen/tf2_eigen.hpp>

// Eigen::Vector3d → geometry_msgs::msg::Point
Eigen::Vector3d eigen_point(1.0, 2.0, 3.0);
geometry_msgs::msg::Point ros_point = tf2::toMsg(eigen_point);

// geometry_msgs::msg::Point → Eigen::Vector3d
Eigen::Vector3d eigen_point_out;
tf2::fromMsg(ros_point, eigen_point_out);

4.2 Quaternion 변환

// Eigen::Quaterniond → geometry_msgs::msg::Quaternion
Eigen::Quaterniond eigen_quat(1.0, 0.0, 0.0, 0.0);  // (w, x, y, z)
geometry_msgs::msg::Quaternion ros_quat = tf2::toMsg(eigen_quat);

// geometry_msgs::msg::Quaternion → Eigen::Quaterniond
Eigen::Quaterniond eigen_quat_out;
tf2::fromMsg(ros_quat, eigen_quat_out);

주의: Eigen의 Quaterniond 생성자는 (w, x, y, z) 순서를 사용하지만, geometry_msgs::msg::Quaternion은 (x, y, z, w) 필드 순서를 갖는다. tf2_eigen의 변환 함수는 이 순서 차이를 자동으로 처리한다.

4.3 Pose/Transform 변환

// Eigen::Isometry3d → geometry_msgs::msg::Pose
Eigen::Isometry3d eigen_pose = Eigen::Isometry3d::Identity();
eigen_pose.translate(Eigen::Vector3d(1.0, 2.0, 0.0));
eigen_pose.rotate(Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitZ()));

geometry_msgs::msg::Pose ros_pose = tf2::toMsg(eigen_pose);

// geometry_msgs::msg::Pose → Eigen::Isometry3d
Eigen::Isometry3d eigen_pose_out;
tf2::fromMsg(ros_pose, eigen_pose_out);

4.4 Transform 변환

// Eigen::Isometry3d → geometry_msgs::msg::Transform
geometry_msgs::msg::Transform ros_transform = 
  tf2::eigenToTransform(eigen_pose).transform;

// geometry_msgs::msg::TransformStamped → Eigen::Isometry3d
geometry_msgs::msg::TransformStamped transform_stamped;
// ... (lookupTransform 등으로 획득)
Eigen::Isometry3d T = tf2::transformToEigen(transform_stamped);

5. TF2 변환 파이프라인과의 통합

5.1 Eigen 타입에 대한 doTransform()

tf2_eigen은 Eigen 타입에 대한 doTransform() 특수화를 제공하여, Eigen 데이터를 TF2 파이프라인에서 직접 변환할 수 있게 한다.

// Eigen::Vector3d (PointStamped 래퍼) 변환
Eigen::Vector3d point_in(1.0, 2.0, 3.0);
geometry_msgs::msg::PointStamped point_msg;
point_msg.header.frame_id = "sensor_link";
point_msg.header.stamp = node->now();
point_msg.point = tf2::toMsg(point_in);

auto point_out_msg = tf_buffer->transform(point_msg, "base_link");
Eigen::Vector3d point_out;
tf2::fromMsg(point_out_msg.point, point_out);

5.2 TransformStamped에서 Eigen 변환 행렬 추출

TF2의 lookupTransform()으로 조회한 변환을 Eigen 형식으로 변환하여 고성능 연산에 활용할 수 있다.

auto transform_stamped = tf_buffer->lookupTransform(
  "map", "base_link", node->now());

// Eigen::Isometry3d로 변환
Eigen::Isometry3d T = tf2::transformToEigen(transform_stamped);

// Eigen::Matrix4d로도 사용 가능
Eigen::Matrix4d T_matrix = T.matrix();

// 회전 행렬 추출
Eigen::Matrix3d R = T.rotation();

// 병진 벡터 추출
Eigen::Vector3d t = T.translation();

// 쿼터니언 추출
Eigen::Quaterniond q(T.rotation());

6. Eigen 기반 고성능 변환

6.1 포인트 배열의 일괄 변환

// 변환 행렬 조회
auto transform_stamped = tf_buffer->lookupTransform(
  "map", "sensor_link", time);
Eigen::Isometry3d T = tf2::transformToEigen(transform_stamped);

// 3×N 행렬로 포인트 배열 구성
Eigen::Matrix3Xd points(3, N);
for (size_t i = 0; i < N; ++i) {
  points.col(i) = Eigen::Vector3d(data[i].x, data[i].y, data[i].z);
}

// 일괄 변환: 행렬 곱으로 모든 포인트 동시 변환
Eigen::Matrix3Xd transformed = T.rotation() * points;
transformed.colwise() += T.translation();

이 방식은 Eigen의 SSE/AVX SIMD 최적화를 활용하여 개별 포인트 변환에 비해 수배에서 수십 배의 성능 향상을 달성할 수 있다.

6.2 역기구학 연산에서의 활용

// 말단 장치의 목표 자세 (map 프레임)
Eigen::Isometry3d goal_in_map;
tf2::fromMsg(goal_pose.pose, goal_in_map);

// base_link 프레임으로 변환
auto T_base_map = tf2::transformToEigen(
  tf_buffer->lookupTransform("base_link", "map", time));
Eigen::Isometry3d goal_in_base = T_base_map * goal_in_map;

// 역기구학 해석에 사용
// ik_solver.solve(goal_in_base, joint_positions);

6.3 공분산 행렬의 변환

// 회전 행렬 추출
Eigen::Matrix3d R = T.rotation();

// 6×6 공분산 행렬의 회전 변환
Eigen::Matrix<double, 6, 6> J = 
  Eigen::Matrix<double, 6, 6>::Zero();
J.block<3, 3>(0, 0) = R;
J.block<3, 3>(3, 3) = R;

Eigen::Matrix<double, 6, 6> cov_out = J * cov_in * J.transpose();

7. Eigen 타입 선택 가이드

7.1 Isometry3d vs Affine3d vs Matrix4d

로봇 공학에서의 좌표 변환은 대부분 강체 변환이므로 Eigen::Isometry3d의 사용이 권장된다. Isometry3d는 역변환을 O(n) 복잡도의 전치 연산으로 수행할 수 있어 Matrix4d의 일반 역행렬(O(n^3))보다 효율적이다.

7.2 double vs float 정밀도

TF2는 내부적으로 double 정밀도를 사용하므로, Eigen::Isometry3d(double)의 사용이 정밀도 손실 없이 TF2와 연동된다. Eigen::Isometry3f(float)를 사용하면 타입 변환(casting) 과정에서 정밀도 손실이 발생할 수 있으나, 대규모 포인트 클라우드 처리에서는 메모리 절약과 SIMD 성능 향상을 위해 float를 선택하기도 한다.

8. 일반적 오류

8.1 쿼터니언 순서 혼동

Eigen의 Quaterniond 생성자는 (w, x, y, z) 순서를, ROS 메시지는 필드 순서 (x, y, z, w)를 사용한다. 수동 변환 시 이 순서를 혼동하면 완전히 잘못된 회전이 적용된다. tf2_eigen의 fromMsg()/toMsg() 함수를 사용하면 이 순서 차이가 자동으로 처리된다.

8.2 Isometry3d의 초기화

Eigen::Isometry3d를 선언만 하고 초기화하지 않으면 불확정 값을 가진다. 반드시 Eigen::Isometry3d::Identity()로 초기화한 후 회전과 병진을 설정해야 한다.

// 올바른 초기화
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
T.translate(Eigen::Vector3d(1.0, 0.0, 0.0));
T.rotate(Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0));

9. 참고 문헌

• G. Guennebaud, B. Jacob, et al., “Eigen v3,” https://eigen.tuxfamily.org
• Open Robotics, “tf2_eigen,” ROS 2 Documentation, https://docs.ros.org/en/rolling/p/tf2_eigen/
• T. Foote, “tf: The transform library,” in Proc. IEEE Conf. Technologies for Practical Robot Applications (TePRA), 2013, pp. 1–6.

버전: 2026-03-26