659.66 포즈 (geometry_msgs::Pose) 변환

1. 개요

포즈(pose)는 3차원 공간에서 강체(rigid body)의 위치(position)와 자세(orientation)를 동시에 나타내는 기하학적 표현이다. 로봇 공학에서 포즈는 로봇의 현재 상태, 센서의 설치 위치, 목표 위치 및 자세, 그리고 인식된 객체의 공간적 배치를 기술하는 데 핵심적으로 사용된다. TF2에서는 geometry_msgs::msg::Pose 및 관련 메시지 타입에 대한 좌표 변환을 지원하며, 이 변환은 위치에 대한 회전+병진과 자세에 대한 회전 합성의 조합으로 이루어진다.

2. geometry_msgs::msg::Pose 메시지 구조

2.1 Pose

geometry_msgs/Pose
  geometry_msgs/Point position
    float64 x
    float64 y
    float64 z
  geometry_msgs/Quaternion orientation
    float64 x
    float64 y
    float64 z
    float64 w

position 필드는 3차원 공간에서의 위치를 미터(m) 단위로 나타내며, orientation 필드는 자세를 단위 쿼터니언(unit quaternion)으로 표현한다. 포즈는 본질적으로 $SE(3)$ 군의 한 원소를 나타내며, 동차 변환 행렬 $T \in SE(3)$ 와 동치이다.

2.2 PoseStamped

시간 및 프레임 정보가 포함된 형태이다.

geometry_msgs/PoseStamped
  std_msgs/Header header
    builtin_interfaces/Time stamp
    string frame_id
  geometry_msgs/Pose pose

2.3 PoseWithCovariance

공분산(covariance) 정보가 포함된 형태이다.

geometry_msgs/PoseWithCovariance
  geometry_msgs/Pose pose
  float64[36] covariance

covariance 필드는 $6 \times 6$ 공분산 행렬을 행 우선(row-major) 순서로 저장한 배열이며, 처음 3개 차원은 위치의 불확실성 $(x, y, z)$ , 나머지 3개 차원은 자세의 불확실성 $(roll, pitch, yaw)$ 을 나타낸다.

3. 포즈 변환의 수학적 원리

3.1 동차 변환 행렬 표현

포즈 $P = (\mathbf{p}, q)$ 는 위치 벡터 $\mathbf{p} \in \mathbb{R}^{3}$ 와 쿼터니언 $q$ 로 구성되며, 동차 변환 행렬로 다음과 같이 표현된다.

$T_{P} = \begin{bmatrix} R(q) & \mathbf{p} \\ \mathbf{0}^{\top} & 1 \end{bmatrix}$

여기서 $R(q) \in SO(3)$ 는 쿼터니언 $q$ 에 대응하는 회전 행렬이다.

3.2 포즈의 좌표 변환

프레임 $A$ 에서 표현된 포즈 ${}^{A}P$ 를 프레임 $B$ 에서 표현된 포즈 ${}^{B}P$ 로 변환하려면, 프레임 $A$ 에서 프레임 $B$ 로의 변환 ${}^{B}T_{A}$ 를 적용한다.

$T_{{}^{B}P} = {}^{B}T_{A} \cdot T_{{}^{A}P}$

이를 위치와 자세 성분으로 분해하면 다음과 같다.

위치 변환:
${}^{B}\mathbf{p} = R_{BA} \cdot {}^{A}\mathbf{p} + \mathbf{t}_{BA}$

자세 변환:
$q_{B} = q_{BA} \otimes q_{A}$

여기서 $R_{BA}$ 는 프레임 $A$ 에서 $B$ 로의 회전 행렬, $\mathbf{t}_{BA}$ 는 병진 벡터, $q_{BA}$ 는 대응하는 회전 쿼터니언이다.

위치 변환에는 회전과 병진이 모두 적용되며(점의 변환과 동일), 자세 변환에는 회전의 합성(쿼터니언 곱셈)만 적용된다.

3.3 공분산의 변환

PoseWithCovariance의 공분산 행렬 $\Sigma \in \mathbb{R}^{6 \times 6}$ 은 다음과 같이 변환된다.

$\Sigma_{B} = J \cdot \Sigma_{A} \cdot J^{\top}$

여기서 $J$ 는 변환의 야코비안(Jacobian) 행렬이며, 강체 변환의 경우 블록 대각 형태를 갖는다.

$J = \begin{bmatrix} R & \mathbf{0} \\ \mathbf{0} & R \end{bmatrix}$

이 변환은 불확실성의 방향이 좌표 프레임의 회전에 따라 함께 회전하는 것을 반영한다.

4. C++에서의 Pose 변환

4.1 doTransform()을 이용한 Pose 변환

#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>

// 소스 프레임에서의 포즈
geometry_msgs::msg::PoseStamped pose_in;
pose_in.header.frame_id = "camera_link";
pose_in.header.stamp = node->now();
pose_in.pose.position.x = 0.5;
pose_in.pose.position.y = 0.3;
pose_in.pose.position.z = 1.2;
pose_in.pose.orientation.x = 0.0;
pose_in.pose.orientation.y = 0.0;
pose_in.pose.orientation.z = 0.0;
pose_in.pose.orientation.w = 1.0;

geometry_msgs::msg::PoseStamped pose_out;

try {
  auto transform = tf_buffer->lookupTransform(
    "base_link", "camera_link", pose_in.header.stamp);
  tf2::doTransform(pose_in, pose_out, transform);
  
  RCLCPP_INFO(node->get_logger(),
    "base_link에서의 위치: (%.2f, %.2f, %.2f)",
    pose_out.pose.position.x,
    pose_out.pose.position.y,
    pose_out.pose.position.z);
} catch (const tf2::TransformException & ex) {
  RCLCPP_ERROR(node->get_logger(), "%s", ex.what());
}

4.2 Buffer::transform()을 이용한 간편 변환

try {
  auto pose_out = tf_buffer->transform(pose_in, "base_link");
} catch (const tf2::TransformException & ex) {
  RCLCPP_ERROR(node->get_logger(), "%s", ex.what());
}

4.3 PoseWithCovarianceStamped 변환

공분산이 포함된 포즈의 변환도 동일한 패턴으로 수행된다.

geometry_msgs::msg::PoseWithCovarianceStamped pose_cov_in;
pose_cov_in.header.frame_id = "odom";
pose_cov_in.header.stamp = node->now();
pose_cov_in.pose.pose.position.x = 10.0;
pose_cov_in.pose.pose.position.y = 5.0;
pose_cov_in.pose.pose.position.z = 0.0;
pose_cov_in.pose.pose.orientation.w = 1.0;

// 공분산 설정 (대각 성분만)
pose_cov_in.pose.covariance[0] = 0.1;   // x-x
pose_cov_in.pose.covariance[7] = 0.1;   // y-y
pose_cov_in.pose.covariance[14] = 0.01; // z-z
pose_cov_in.pose.covariance[21] = 0.01; // roll-roll
pose_cov_in.pose.covariance[28] = 0.01; // pitch-pitch
pose_cov_in.pose.covariance[35] = 0.05; // yaw-yaw

try {
  auto pose_cov_out = tf_buffer->transform(
    pose_cov_in, "map");
} catch (const tf2::TransformException & ex) {
  RCLCPP_ERROR(node->get_logger(), "%s", ex.what());
}

4.4 tf2 내부 타입을 이용한 변환

#include <tf2/LinearMath/Transform.h>

// TransformStamped → tf2::Transform
tf2::Transform T_base_camera;
tf2::fromMsg(transform_msg.transform, T_base_camera);

// PoseStamped → tf2::Transform
tf2::Transform pose_camera;
tf2::fromMsg(pose_in.pose, pose_camera);

// 포즈 변환: 변환의 합성
tf2::Transform pose_base = T_base_camera * pose_camera;

// tf2::Transform → Pose 메시지
geometry_msgs::msg::Pose pose_msg;
tf2::toMsg(pose_base, pose_msg);

이 방식은 포즈 변환이 본질적으로 동차 변환 행렬의 합성임을 명시적으로 드러낸다.

5. Python에서의 Pose 변환

5.1 do_transform_pose_stamped()를 이용한 변환

from geometry_msgs.msg import PoseStamped
import tf2_geometry_msgs

pose_in = PoseStamped()
pose_in.header.frame_id = 'camera_link'
pose_in.header.stamp = node.get_clock().now().to_msg()
pose_in.pose.position.x = 0.5
pose_in.pose.position.y = 0.3
pose_in.pose.position.z = 1.2
pose_in.pose.orientation.w = 1.0

try:
    transform = tf_buffer.lookup_transform(
        'base_link', 'camera_link',
        pose_in.header.stamp)
    pose_out = tf2_geometry_msgs.do_transform_pose_stamped(
        pose_in, transform)
    
    node.get_logger().info(
        f'base_link에서의 위치: '
        f'({pose_out.pose.position.x:.2f}, '
        f'{pose_out.pose.position.y:.2f}, '
        f'{pose_out.pose.position.z:.2f})')
except TransformException as ex:
    node.get_logger().error(f'{ex}')

5.2 Buffer.transform()을 이용한 간편 변환

try:
    pose_out = tf_buffer.transform(pose_in, 'base_link')
except TransformException as ex:
    node.get_logger().error(f'{ex}')

6. 응용 사례

6.1 객체 인식 결과의 좌표 변환

카메라로 인식한 객체의 포즈를 로봇 기체 좌표계로 변환하는 것은 매니퓰레이션(manipulation)의 핵심 전처리 단계이다.

// 카메라 프레임에서 인식된 객체 포즈
geometry_msgs::msg::PoseStamped detected_object;
detected_object.header.frame_id = "camera_optical_link";
detected_object.pose = detection_result.pose;

// base_link 프레임으로 변환
auto object_in_base = tf_buffer->transform(
    detected_object, "base_link");

6.2 내비게이션 목표 설정

map 프레임에서 정의된 내비게이션 목표를 odom 프레임으로 변환하여 로컬 경로 계획기(local planner)에 전달하는 경우에도 포즈 변환이 사용된다.

6.3 센서 퓨전

서로 다른 좌표 프레임에서 추정된 포즈를 동일한 프레임으로 변환한 후 퓨전(fusion)하는 과정에서, 포즈 변환과 함께 공분산 변환이 필수적이다.

7. 주의사항

7.1 쿼터니언 정규화

포즈의 orientation 필드에 저장된 쿼터니언이 정규화되지 않은 경우(즉, $\|q\| \neq 1$ ), 변환 결과가 올바르지 않을 수 있다. 포즈를 설정하거나 수신할 때 쿼터니언의 정규화 여부를 항상 확인해야 한다.

7.2 항등 자세의 초기화

자세를 항등 회전(identity rotation)으로 초기화할 때는 쿼터니언을 $(x=0, y=0, z=0, w=1)$ 로 설정해야 한다. geometry_msgs::msg::Quaternion의 기본 생성자는 모든 필드를 0으로 초기화하므로, $w=1$ 설정이 누락되면 비정규 쿼터니언 $(0, 0, 0, 0)$ 이 되어 정의되지 않은 동작이 발생한다.

7.3 D vs 3D 포즈

이동 로봇의 내비게이션에서는 흔히 2D 포즈( $(x, y, \theta)$ )를 사용하지만, TF2에서는 모든 변환이 3차원으로 처리된다. 2D 포즈를 3D 포즈로 확장할 때는 $z = 0$ , $roll = 0$ , $pitch = 0$ 으로 설정하고, $yaw$ 각도를 쿼터니언으로 변환하여 orientation에 저장한다.

8. 참고 문헌

Open Robotics, “geometry_msgs/Pose,” ROS 2 Documentation, https://docs.ros.org/en/rolling/p/geometry_msgs/
Open Robotics, “tf2_geometry_msgs,” ROS 2 Documentation, https://docs.ros.org/en/rolling/p/tf2_geometry_msgs/
J. Solà, “Quaternion kinematics for the error-state Kalman filter,” arXiv preprint arXiv:1711.02508, 2017.

버전: 2026-03-26