659.70 포인트 클라우드 (sensor_msgs::PointCloud2) 좌표 변환

1. 개요

포인트 클라우드(point cloud)는 3차원 공간에서 다수의 점으로 구성된 데이터 구조로, LiDAR, 깊이 카메라(depth camera), 스테레오 카메라 등 다양한 3차원 센서에서 생성된다. 로봇 시스템에서 포인트 클라우드는 환경 인식, 장애물 탐지, SLAM, 객체 인식, 지도 생성 등에 광범위하게 활용된다. 센서에서 취득한 포인트 클라우드를 로봇 기체 좌표계, 오도메트리 좌표계, 또는 세계 좌표계 등 다른 좌표 프레임으로 변환하는 것은 센서 데이터 처리의 필수적인 전처리 단계이다.

2. sensor_msgs::msg::PointCloud2 메시지 구조

2.1 메시지 정의

sensor_msgs/PointCloud2
  std_msgs/Header header
    builtin_interfaces/Time stamp
    string frame_id
  uint32 height
  uint32 width
  sensor_msgs/PointField[] fields
  bool is_bigendian
  uint32 point_step
  uint32 row_step
  uint8[] data
  bool is_dense

2.2 주요 필드 설명

PointCloud2는 바이너리 직렬화 형식을 사용하여 대용량 포인트 데이터를 효율적으로 저장하고 전송한다.

3. 포인트 클라우드 좌표 변환의 원리

3.1 점별 변환

포인트 클라우드의 좌표 변환은 각 포인트(점)에 대해 동차 변환 행렬을 적용하는 점별(point-wise) 변환이다. 포인트 클라우드 \mathcal{P} = \{\mathbf{p}_1, \mathbf{p}_2, \ldots, \mathbf{p}_N\}의 각 포인트에 대해,

{}^{B}\mathbf{p}_i = R \cdot {}^{A}\mathbf{p}_i + \mathbf{t}, \quad \forall i \in \{1, \ldots, N\}

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

3.2 행렬 연산을 이용한 일괄 변환

N개의 포인트를 3 \times N 행렬 P = [\mathbf{p}_1, \ldots, \mathbf{p}_N]로 배열하면, 전체 포인트 클라우드의 변환을 단일 행렬 연산으로 수행할 수 있다.

P^{B} = R \cdot P^{A} + \mathbf{t} \cdot \mathbf{1}^{\top}

여기서 \mathbf{1} \in \mathbb{R}^{N}은 모든 원소가 1인 벡터이다. 이 행렬 연산은 SIMD 명령어와 캐시 지역성(cache locality)을 활용하여 개별 점 변환에 비해 월등한 계산 효율을 제공한다.

4. tf2를 이용한 PointCloud2 변환

4.1 tf2_sensor_msgs 패키지

sensor_msgs::msg::PointCloud2에 대한 doTransform() 특수화는 tf2_sensor_msgs 패키지에서 제공된다. 이 패키지는 Eigen 라이브러리를 활용하여 대규모 포인트 클라우드의 효율적인 좌표 변환을 수행한다.

4.2 C++ 의존성 설정

find_package(tf2_sensor_msgs REQUIRED)
ament_target_dependencies(${PROJECT_NAME}
  tf2
  tf2_ros
  tf2_sensor_msgs
  sensor_msgs
)

<depend>tf2_sensor_msgs</depend>

4.3 C++에서의 변환

#include <tf2_sensor_msgs/tf2_sensor_msgs.hpp>

sensor_msgs::msg::PointCloud2 cloud_in;
// cloud_in은 센서 토픽으로부터 수신

sensor_msgs::msg::PointCloud2 cloud_out;

try {
  auto transform = tf_buffer->lookupTransform(
    "base_link", cloud_in.header.frame_id,
    cloud_in.header.stamp);
  tf2::doTransform(cloud_in, cloud_out, transform);
} catch (const tf2::TransformException & ex) {
  RCLCPP_ERROR(node->get_logger(), "%s", ex.what());
}

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

try {
  sensor_msgs::msg::PointCloud2 cloud_out;
  tf_buffer->transform(cloud_in, cloud_out, "base_link");
} catch (const tf2::TransformException & ex) {
  RCLCPP_ERROR(node->get_logger(), "%s", ex.what());
}

4.5 Python에서의 변환

import tf2_sensor_msgs

try:
    transform = tf_buffer.lookup_transform(
        'base_link', cloud_in.header.frame_id,
        cloud_in.header.stamp)
    cloud_out = tf2_sensor_msgs.do_transform_cloud(
        cloud_in, transform)
except TransformException as ex:
    node.get_logger().error(f'{ex}')

5. PCL(Point Cloud Library) 기반 변환

5.1 pcl_ros를 이용한 변환

PCL(Point Cloud Library)과 ROS2의 통합 패키지인 pcl_ros는 포인트 클라우드의 좌표 변환 기능을 제공한다.

#include <pcl_ros/transforms.hpp>
#include <pcl_conversions/pcl_conversions.h>

// sensor_msgs → pcl 변환
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_pcl(
  new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(cloud_in, *cloud_pcl);

// 변환 행렬 구성
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
tf2::Transform tf2_transform;
tf2::fromMsg(transform_msg.transform, tf2_transform);

// Eigen 변환 행렬로 변환
Eigen::Matrix4f transform_matrix = Eigen::Matrix4f::Identity();
// ... (tf2_transform에서 Eigen 행렬로 변환)

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_transformed(
  new pcl::PointCloud<pcl::PointXYZ>);
pcl::transformPointCloud(
  *cloud_pcl, *cloud_transformed, transform_matrix);

// pcl → sensor_msgs 변환
sensor_msgs::msg::PointCloud2 cloud_out;
pcl::toROSMsg(*cloud_transformed, cloud_out);
cloud_out.header.frame_id = "base_link";

5.2 PCL 직접 변환의 장점

PCL의 transformPointCloud() 함수는 내부적으로 Eigen의 SIMD 최적화를 활용하며, 변환과 동시에 필터링(voxel grid downsampling, statistical outlier removal 등)을 적용할 수 있어 전체 처리 파이프라인의 효율성이 향상된다.

6. 성능 최적화

6.1 대규모 포인트 클라우드의 성능 고려사항

현대의 3D LiDAR 센서(예: Velodyne VLP-16, Ouster OS1-64)는 초당 수십만에서 수백만 개의 포인트를 생성한다. 이러한 대규모 데이터의 실시간 좌표 변환은 상당한 연산 부하를 요구한다.

6.2 변환 조회 최적화

포인트 클라우드 변환에서 lookupTransform()은 전체 클라우드에 대해 한 번만 호출하면 되므로, 변환 조회의 부하는 미미하다. 핵심 병목은 개별 포인트에 대한 변환 적용 단계이다.

6.3 메모리 접근 패턴

PointCloud2의 바이너리 데이터는 연속 메모리 블록에 저장되므로, 순차적 접근 시 CPU 캐시의 공간적 지역성(spatial locality)을 활용할 수 있다. 반면, 포인트의 필드가 비연속적으로 배치된 경우(예: x, y, z 사이에 intensity 등 다른 필드가 삽입된 경우) 캐시 미스가 증가할 수 있다.

6.4 병렬 처리

OpenMP나 Intel TBB를 이용하여 포인트 클라우드의 변환을 병렬 처리할 수 있다.

#include <omp.h>

auto R_matrix = T.getBasis();
auto t_vector = T.getOrigin();

#pragma omp parallel for
for (size_t i = 0; i < point_count; ++i) {
  float * p = reinterpret_cast<float *>(
    &cloud_out.data[i * cloud_in.point_step]);
  tf2::Vector3 p_in(p[0], p[1], p[2]);
  tf2::Vector3 p_out = R_matrix * p_in + t_vector;
  p[0] = static_cast<float>(p_out.x());
  p[1] = static_cast<float>(p_out.y());
  p[2] = static_cast<float>(p_out.z());
}

7. 운동 보정 (Motion Compensation)

7.1 스캐닝 왜곡 문제

회전형 LiDAR는 기계적으로 회전하면서 데이터를 수집하므로, 하나의 스캔(scan) 내에서 각 포인트의 취득 시점이 다르다. 로봇이 이동 중인 경우, 스캔 시작 시점과 종료 시점의 로봇 자세가 달라지므로 포인트 클라우드에 왜곡(distortion)이 발생한다.

7.2 TF2를 이용한 운동 보정

운동 보정(motion compensation 또는 deskewing)은 각 포인트의 취득 시점에서의 변환을 TF2로 조회하여, 모든 포인트를 동일한 시점의 좌표 프레임으로 변환하는 과정이다.

for (size_t i = 0; i < point_count; ++i) {
  // 각 포인트의 상대적 시간 오프셋 계산
  double time_offset = compute_point_time_offset(cloud, i);
  rclcpp::Time point_time = cloud_time + 
    rclcpp::Duration::from_seconds(time_offset);
  
  // 해당 시점의 변환 조회
  auto transform = tf_buffer->lookupTransform(
    "odom", cloud.header.frame_id, point_time);
  
  // 포인트 변환
  // ...
}

이 방식은 포인트별로 lookupTransform()을 호출하므로 계산 비용이 높다. 실제로는 시간 구간을 분할하여 보간하는 방식이 사용된다.

8. 주의사항

8.1 float 정밀도

PointCloud2의 좌표 데이터는 일반적으로 float32(단정밀도)로 저장되지만, TF2의 변환 행렬은 float64(배정밀도)로 계산된다. 변환 후 다시 float32로 저장할 때 정밀도 손실이 발생할 수 있으며, 원점에서 멀리 떨어진 포인트(예: GPS 좌표)에서 이 문제가 두드러진다.

8.2 헤더 갱신

변환 후 포인트 클라우드의 header.frame_id를 대상 프레임으로 갱신해야 한다. tf2::doTransform()은 이를 자동으로 처리하지만, PCL을 통한 수동 변환에서는 개발자가 직접 갱신해야 한다.

8.3 비좌표 필드의 보존

포인트 클라우드에는 좌표(x, y, z) 외에도 intensity, RGB, normal, ring 등의 추가 필드가 포함될 수 있다. 좌표 변환 시 이러한 비좌표 필드는 변경되지 않고 보존되어야 한다. 단, 법선 벡터(normal)는 좌표 변환의 회전 성분에 따라 함께 회전되어야 한다.

9. 참고 문헌

• Open Robotics, “sensor_msgs/PointCloud2,” ROS 2 Documentation, https://docs.ros.org/en/rolling/p/sensor_msgs/
• Open Robotics, “tf2_sensor_msgs,” ROS 2 Documentation, https://docs.ros.org/en/rolling/p/tf2_sensor_msgs/
• R. B. Rusu and S. Cousins, “3D is here: Point Cloud Library (PCL),” in Proc. IEEE Int. Conf. Robotics and Automation (ICRA), 2011, pp. 1–4.

버전: 2026-03-26