659.64 포인트 (geometry_msgs::Point) 변환

1. 개요

3차원 공간에서의 점(point)은 위치(position)를 나타내는 가장 기본적인 기하학적 원소이다. 로봇 시스템에서 특정 좌표 프레임에서 표현된 점을 다른 좌표 프레임으로 변환하는 것은 센서 데이터 처리, 목표 위치 설정, 장애물 탐지 등 다양한 응용에서 필수적인 연산이다. TF2 프레임워크에서는 geometry_msgs::msg::Point 및 geometry_msgs::msg::PointStamped 메시지 타입에 대한 좌표 변환을 tf2_geometry_msgs 패키지를 통해 지원한다.

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

geometry_msgs::msg::Point는 3차원 공간에서의 위치를 나타내는 메시지로, 다음과 같은 구조를 갖는다.

geometry_msgs/Point
  float64 x
  float64 y
  float64 z

세 개의 float64 필드는 각각 좌표 프레임의 $x$ , $y$ , $z$ 축 방향의 좌표 성분을 미터(m) 단위로 나타낸다.

2.1 PointStamped 메시지 구조

시간 및 프레임 정보가 포함된 PointStamped 메시지는 다음과 같다.

geometry_msgs/PointStamped
  std_msgs/Header header
    builtin_interfaces/Time stamp
    string frame_id
  geometry_msgs/Point point
    float64 x
    float64 y
    float64 z

header.frame_id는 해당 점이 표현된 좌표 프레임을 식별하며, header.stamp는 해당 시점의 타임스탬프를 나타낸다. TF2의 변환 파이프라인에서 PointStamped는 자동 프레임 추적이 가능하므로 Point보다 선호된다.

3. 점 변환의 수학적 원리

3.1 동차 좌표 변환

프레임 $A$ 에서 표현된 점 ${}^{A}\mathbf{p} = (p_x, p_y, p_z)^{\top}$ 를 프레임 $B$ 에서 표현된 좌표 ${}^{B}\mathbf{p}$ 로 변환하려면, 프레임 $B$ 에서 프레임 $A$ 로의 변환 행렬 ${}^{B}T_{A}$ 를 적용해야 한다.

$\begin{bmatrix} {}^{B}\mathbf{p} \\ 1 \end{bmatrix} = {}^{B}T_{A} \begin{bmatrix} {}^{A}\mathbf{p} \\ 1 \end{bmatrix}$

이를 회전 행렬 $R$ 과 병진 벡터 $\mathbf{t}$ 로 분해하면 다음과 같다.

${}^{B}\mathbf{p} = R \cdot {}^{A}\mathbf{p} + \mathbf{t}$

여기서 $R \in SO(3)$ 는 $3 \times 3$ 회전 행렬이고, $\mathbf{t} \in \mathbb{R}^{3}$ 는 병진 벡터이다.

3.2 점과 벡터의 변환 차이

점(point)의 변환에는 회전과 병진이 모두 적용된다. 이는 점이 공간상의 절대적 위치를 나타내기 때문이다. 반면, 자유 벡터(free vector)의 변환에는 회전만 적용되고 병진은 무시된다. 이 차이는 동차 좌표(homogeneous coordinates) 표현에서 점의 마지막 성분이 1이고, 벡터의 마지막 성분이 0인 것으로 구분된다.

$\text{Point: } \begin{bmatrix} \mathbf{p} \\ 1 \end{bmatrix}, \quad \text{Vector: } \begin{bmatrix} \mathbf{v} \\ 0 \end{bmatrix}$

4. C++에서의 Point 변환

4.1 doTransform()을 이용한 변환

#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>

// PointStamped 변환
geometry_msgs::msg::PointStamped point_in;
point_in.header.frame_id = "sensor_link";
point_in.header.stamp = node->now();
point_in.point.x = 1.0;
point_in.point.y = 2.0;
point_in.point.z = 0.5;

geometry_msgs::msg::PointStamped point_out;

try {
  auto transform = tf_buffer->lookupTransform(
    "base_link", "sensor_link", point_in.header.stamp);
  tf2::doTransform(point_in, point_out, transform);
  
  RCLCPP_INFO(node->get_logger(),
    "변환된 점: (%.2f, %.2f, %.2f) in %s",
    point_out.point.x, point_out.point.y, point_out.point.z,
    point_out.header.frame_id.c_str());
} catch (const tf2::TransformException & ex) {
  RCLCPP_ERROR(node->get_logger(), "%s", ex.what());
}

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

geometry_msgs::msg::PointStamped point_in;
point_in.header.frame_id = "sensor_link";
point_in.header.stamp = node->now();
point_in.point.x = 1.0;
point_in.point.y = 2.0;
point_in.point.z = 0.5;

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

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

tf2::Vector3와 tf2::Transform을 직접 사용하여 보다 저수준의 변환을 수행할 수 있다.

#include <tf2/LinearMath/Transform.h>
#include <tf2/LinearMath/Vector3.h>

tf2::Transform T;
tf2::fromMsg(transform_msg.transform, T);

tf2::Vector3 point_in(1.0, 2.0, 0.5);
tf2::Vector3 point_out = T * point_in;
// 또는: tf2::Vector3 point_out = T(point_in);

이 방식은 geometry_msgs 메시지를 거치지 않으므로 메시지 직렬화/역직렬화 오버헤드가 없으며, 대량의 점을 일괄 변환할 때 유리하다.

5. Python에서의 Point 변환

5.1 do_transform_point()를 이용한 변환

from geometry_msgs.msg import PointStamped
from tf2_geometry_msgs import do_transform_point

# 입력 점 생성
point_in = PointStamped()
point_in.header.frame_id = 'sensor_link'
point_in.header.stamp = node.get_clock().now().to_msg()
point_in.point.x = 1.0
point_in.point.y = 2.0
point_in.point.z = 0.5

try:
    transform = tf_buffer.lookup_transform(
        'base_link', 'sensor_link',
        point_in.header.stamp)
    
    point_out = do_transform_point(point_in, transform)
    
    node.get_logger().info(
        f'변환된 점: ({point_out.point.x:.2f}, '
        f'{point_out.point.y:.2f}, {point_out.point.z:.2f}) '
        f'in {point_out.header.frame_id}')
except TransformException as ex:
    node.get_logger().error(f'{ex}')

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

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

6. 다중 점의 배치 변환

6.1 효율적 배치 처리

다수의 점을 동일한 프레임으로 변환해야 하는 경우, lookupTransform()을 한 번만 호출하고 doTransform()을 반복 적용하는 것이 효율적이다.

// 변환을 한 번만 조회
auto transform = tf_buffer->lookupTransform(
  "map", "sensor_link", time);

// 다수의 점을 일괄 변환
for (const auto & pt_in : sensor_points) {
  geometry_msgs::msg::PointStamped pt_out;
  tf2::doTransform(pt_in, pt_out, transform);
  map_points.push_back(pt_out);
}

6.2 Eigen을 이용한 고성능 배치 처리

대규모 점군(point cloud)의 좌표 변환에는 Eigen 라이브러리를 활용한 행렬 연산이 효율적이다.

#include <Eigen/Geometry>

// TransformStamped → Eigen::Isometry3d
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
T.translate(Eigen::Vector3d(
  transform.transform.translation.x,
  transform.transform.translation.y,
  transform.transform.translation.z));
T.rotate(Eigen::Quaterniond(
  transform.transform.rotation.w,
  transform.transform.rotation.x,
  transform.transform.rotation.y,
  transform.transform.rotation.z));

// Eigen::MatrixXd를 이용한 배치 변환
// points: 3×N 행렬
Eigen::MatrixXd transformed = T.rotation() * points;
transformed.colwise() += T.translation();

이 방식은 SIMD(Single Instruction, Multiple Data) 최적화의 이점을 활용할 수 있으며, 개별 doTransform() 호출에 비해 수천 개 이상의 점을 처리할 때 현저한 성능 향상을 제공한다.

7. 변환 결과의 해석

7.1 헤더 갱신

doTransform() 또는 Buffer::transform()을 통해 변환된 PointStamped의 header 필드는 다음과 같이 갱신된다.

header.frame_id: 대상 프레임(target frame)으로 변경
header.stamp: 변환의 타임스탬프로 변경

7.2 좌표계 확인

변환 결과의 좌표 성분은 대상 프레임의 축 방향에 따라 해석해야 한다. 예를 들어, REP 103에 따르면 로봇 기체 프레임(base_link)은 전방이 $x$ , 좌측이 $y$ , 상방이 $z$ 이며, 카메라 광학 프레임(camera_optical)은 우측이 $x$ , 하방이 $y$ , 전방이 $z$ 이다. 동일한 점이라도 표현되는 좌표 프레임에 따라 좌표 성분이 달라진다.

8. 일반적 오류

8.1 프레임 불일치 오류

입력 점의 header.frame_id와 lookupTransform()의 소스 프레임이 일치하지 않으면, 변환은 정상적으로 수행되지만 결과가 물리적으로 의미 없는 값이 된다. 이 오류는 컴파일 시나 런타임에 검출되지 않으므로 주의가 필요하다.

8.2 타임스탬프 불일치

입력 점의 타임스탬프와 변환 가용 시간 범위가 불일치하면 ExtrapolationException이 발생한다. 최신 가용 변환을 사용하려면 타임스탬프를 rclcpp::Time(0) (C++) 또는 Time() (Python)으로 설정한다.

9. 참고 문헌

Open Robotics, “geometry_msgs/Point,” 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/
T. Foote, “tf: The transform library,” in Proc. IEEE Conf. Technologies for Practical Robot Applications (TePRA), 2013, pp. 1–6.

버전: 2026-03-26