659.63 tf2::doTransform()을 이용한 데이터 변환

1. 개요

tf2::doTransform()은 TF2 프레임워크에서 임의의 데이터 타입에 대한 좌표 변환을 수행하는 핵심 함수 템플릿이다. 이 함수는 입력 데이터, 출력 데이터, 그리고 변환 정보(geometry_msgs::msg::TransformStamped)를 인자로 받아, 입력 데이터를 소스 좌표 프레임에서 대상 좌표 프레임으로 변환한 결과를 출력 데이터에 기록한다. doTransform()은 TF2의 타입 확장성(type extensibility)을 실현하는 핵심 메커니즘으로, 새로운 데이터 타입에 대한 좌표 변환 지원을 개발자가 직접 추가할 수 있게 설계되어 있다.

2. 함수 시그니처

2.1 C++ 템플릿 선언

tf2::doTransform()은 C++에서 함수 템플릿(function template)으로 선언되어 있으며, 기본 선언은 다음과 같다.

namespace tf2 {

template <typename T>
void doTransform(
  const T & data_in,
  T & data_out,
  const geometry_msgs::msg::TransformStamped & transform);

}  // namespace tf2

매개변수 설명:

2.2 Python 함수 형태

Python에서는 각 메시지 타입별로 개별 함수가 제공된다.

def do_transform_point(point: PointStamped, transform: TransformStamped) -> PointStamped
def do_transform_vector3(vector: Vector3Stamped, transform: TransformStamped) -> Vector3Stamped
def do_transform_pose(pose: PoseStamped, transform: TransformStamped) -> PoseStamped
def do_transform_wrench(wrench: WrenchStamped, transform: TransformStamped) -> WrenchStamped

Python 함수는 변환된 새 객체를 반환하는 함수형(functional) 인터페이스를 채택하고 있어, C++의 참조 기반 출력 방식과 차이가 있다.

3. 템플릿 특수화 메커니즘

3.1 기본 템플릿과 특수화

tf2::doTransform()의 기본 템플릿(primary template)은 의도적으로 구현이 제공되지 않는다. 특정 데이터 타입에 대한 변환 로직은 해당 타입의 확장 패키지에서 명시적 템플릿 특수화(explicit template specialization)를 통해 제공된다.

// tf2_geometry_msgs에서의 PointStamped 특수화 예시
namespace tf2 {

template <>
inline void doTransform(
  const geometry_msgs::msg::PointStamped & data_in,
  geometry_msgs::msg::PointStamped & data_out,
  const geometry_msgs::msg::TransformStamped & transform)
{
  // 변환 로직 구현
  tf2::Transform tf;
  tf2::fromMsg(transform.transform, tf);
  
  tf2::Vector3 point_in(
    data_in.point.x, data_in.point.y, data_in.point.z);
  tf2::Vector3 point_out = tf * point_in;
  
  data_out.header.stamp = transform.header.stamp;
  data_out.header.frame_id = transform.header.frame_id;
  data_out.point.x = point_out.x();
  data_out.point.y = point_out.y();
  data_out.point.z = point_out.z();
}

}  // namespace tf2

3.2 특수화 등록의 필요성

doTransform()의 특수화는 대응하는 헤더 파일의 포함(include)을 통해 컴파일러에 등록된다. 헤더를 포함하지 않으면 기본 템플릿에 대한 링크가 시도되어 링크 오류 또는 런타임 오류가 발생한다. 이는 TF2 확장 패키지를 사용할 때 반드시 해당 헤더를 포함해야 하는 이유이다.

4. doTransform()의 내부 동작

4.1 변환 해석 과정

doTransform() 내부에서는 geometry_msgs::msg::TransformStamped 메시지를 tf2::Transform 객체로 변환한 후, 해당 변환을 입력 데이터에 적용한다. 이 과정은 다음의 단계로 이루어진다.

1. 메시지→내부 표현 변환: tf2::fromMsg()를 사용하여 geometry_msgs::msg::Transform을 tf2::Transform으로 변환한다. tf2::Transform은 내부적으로 tf2::Quaternion(회전)과 tf2::Vector3(병진)을 저장한다.

2. 기하학적 변환 적용: 데이터 타입에 따른 적절한 변환 연산을 수행한다.

• 점(Point): \mathbf{p}_{\text{out}} = R \cdot \mathbf{p}_{\text{in}} + \mathbf{t} (회전 + 병진)
• 벡터(Vector3): \mathbf{v}_{\text{out}} = R \cdot \mathbf{v}_{\text{in}} (회전만)
• 자세(Pose): 위치에 회전+병진, 방향에 회전 합성 적용
• 렌치(Wrench): 힘과 토크에 각각 회전만 적용

3. 헤더 갱신: 출력 데이터의 header.frame_id를 대상 프레임으로, header.stamp를 변환의 타임스탬프로 설정한다.

4.2 tf2::Transform 연산자

tf2::Transform 클래스는 operator*()를 오버로딩하여 변환의 합성과 점의 변환을 동일한 연산자로 처리한다.

// 점의 변환: p_out = T * p_in
tf2::Vector3 p_out = transform * p_in;

// 변환의 합성: T_AC = T_AB * T_BC
tf2::Transform T_AC = T_AB * T_BC;

점의 변환에서 operator*()는 내부적으로 다음을 수행한다.

\mathbf{p}_{\text{out}} = q \otimes \mathbf{p}_{\text{in}} \otimes q^{*} + \mathbf{t}

여기서 q는 회전을 나타내는 단위 쿼터니언이고, \mathbf{t}는 병진 벡터이며, q \otimes \mathbf{p} \otimes q^{*}는 순수 쿼터니언 \mathbf{p} = (0, p_x, p_y, p_z)에 대한 쿼터니언 회전 연산이다.

5. 사용자 정의 타입에 대한 확장

5.1 C++ 커스텀 특수화 작성

개발자는 자체 메시지 타입에 대한 doTransform() 특수화를 작성하여 TF2의 변환 파이프라인을 확장할 수 있다.

#include <tf2/convert.h>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include "my_msgs/msg/custom_stamped.hpp"

namespace tf2 {

template <>
inline void doTransform(
  const my_msgs::msg::CustomStamped & data_in,
  my_msgs::msg::CustomStamped & data_out,
  const geometry_msgs::msg::TransformStamped & transform)
{
  tf2::Transform tf;
  tf2::fromMsg(transform.transform, tf);
  
  // 커스텀 변환 로직 구현
  // ...
  
  data_out.header.stamp = transform.header.stamp;
  data_out.header.frame_id = transform.header.frame_id;
}

}  // namespace tf2

5.2 커스텀 특수화 작성 시 주의사항

1. 네임스페이스: 특수화는 반드시 tf2 네임스페이스 내에 정의해야 한다
2. 헤더 갱신: 출력 메시지의 header를 변환의 헤더로 갱신하는 것이 관례이다
3. Stamped 인터페이스: Buffer::transform() 메서드와의 호환성을 위해 Stamped 메시지에 대한 특수화가 권장된다
4. 인라인 정의: 헤더 파일에 정의되는 특수화는 inline 지정자를 사용하여 다중 정의(ODR violation) 오류를 방지해야 한다

6. Buffer::transform()과의 관계

6.1 호출 흐름

tf2_ros::Buffer::transform() 메서드는 doTransform()의 상위 래퍼(wrapper)로, 다음의 호출 흐름을 내부적으로 수행한다.

Buffer::transform(data_in, target_frame)
  ├── lookupTransform(target_frame, data_in.header.frame_id, data_in.header.stamp)
  │   └── TransformStamped 반환
  └── tf2::doTransform(data_in, data_out, transform)
      └── 타입별 특수화 호출

따라서 doTransform()은 변환 로직의 최하위 계층에 위치하며, 변환 조회와는 독립적으로 동작한다. 이미 TransformStamped를 보유한 경우에는 lookupTransform()을 거치지 않고 직접 doTransform()을 호출하여 불필요한 변환 조회를 회피할 수 있다.

6.2 직접 호출 vs Buffer::transform()

동일한 변환을 다수의 데이터 요소에 적용하는 배치 처리 시나리오에서는 lookupTransform()을 한 번만 호출하고 doTransform()을 반복 호출하는 것이 효율적이다.

// 배치 처리: 변환을 한 번만 조회
auto transform = tf_buffer_->lookupTransform(
  "base_link", "sensor_link", time);

for (const auto & point_in : point_list) {
  geometry_msgs::msg::PointStamped point_out;
  tf2::doTransform(point_in, point_out, transform);
  result_list.push_back(point_out);
}

7. fromMsg()와 toMsg()

7.1 메시지-내부 표현 간 변환

doTransform()의 구현에서는 ROS 메시지 타입과 TF2 내부 타입 간의 변환을 위해 tf2::fromMsg()와 tf2::toMsg() 함수가 사용된다.

// geometry_msgs::msg::Transform → tf2::Transform
tf2::Transform tf;
tf2::fromMsg(transform_msg.transform, tf);

// tf2::Transform → geometry_msgs::msg::Transform
geometry_msgs::msg::Transform msg;
msg = tf2::toMsg(tf);

// geometry_msgs::msg::Quaternion → tf2::Quaternion
tf2::Quaternion q;
tf2::fromMsg(quaternion_msg, q);

// geometry_msgs::msg::Vector3 → tf2::Vector3
tf2::Vector3 v;
tf2::fromMsg(vector_msg, v);

이 변환 함수들도 doTransform()과 마찬가지로 템플릿 특수화 메커니즘을 통해 확장 가능하다.

8. 일반적 오류와 디버깅

8.1 컴파일 오류: 특수화 미등록

error: no matching function for call to 'doTransform'
note: candidate template ignored: could not match 'T' against 'geometry_msgs::msg::PointStamped'

이 오류는 해당 타입에 대한 doTransform() 특수화가 등록되지 않았음을 의미한다. 대응하는 확장 패키지의 헤더(예: tf2_geometry_msgs/tf2_geometry_msgs.hpp)를 포함하여 해결한다.

8.2 런타임 오류: 빈 변환

TransformStamped 메시지가 올바르게 초기화되지 않은 경우(예: 기본 생성자만 호출), 항등 변환(identity transform)이 적용되어 데이터가 변환되지 않는 것처럼 보일 수 있다. lookupTransform()을 통해 유효한 변환을 조회한 후 doTransform()에 전달해야 한다.

8.3 의미론적 오류: 변환 방향 역전

lookupTransform(target_frame, source_frame, time)의 첫 번째 인자가 대상 프레임이고, 두 번째 인자가 소스 프레임이다. 이 순서를 혼동하면 역방향 변환이 적용된다. 입력 데이터의 frame_id가 lookupTransform()의 소스 프레임과 일치하는지 반드시 확인해야 한다.

9. 참고 문헌

• Open Robotics, “tf2 API Documentation,” ROS 2 Documentation, https://docs.ros.org/en/rolling/p/tf2/
• 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