제7부 센서·인지 파이프라인

7.4 포인트클라우드 — PointCloud2·PCL

7.4.1 이 장에서 배우는 것 / 선수 지식

sensor_msgs/PointCloud2는 ROS에서 3D 점 집합을 표현하는 표준이며, 가장 용량이 크고 가장 유연한 메시지 타입이다. 유연함의 대가로 구조가 복잡하다 — 필드가 동적으로 정의되고, 바이트 레이아웃을 직접 다뤄야 하며, 패딩과 정렬이 개입한다.

그리고 크기가 문제다. 64채널 라이다가 초당 200만 점을 만들면, 점당 32바이트로 초당 64 MB다. 이미지(7.2)와 같은 종류의 대역·CPU 문제가 여기서도 발생한다.

배우는 것:

• 메시지 구조 — 필드 기술과 바이트 레이아웃.
• 반복자 — 안전하게 점에 접근하는 법.
• 정렬·비정렬 구름 — 차이와 활용.
• PCL 연동 — 변환 비용과 회피.
• 필터링 — 다운샘플링·영역 제한·이상점 제거.
• 크기 계산 — 대역·메모리 예산.
• 변환 비용 — tf 적용이 왜 비용이 큰가.

선수 지식: 7.1(센서 규약), 1.3.4(CDR·정렬), 5.1(tf 변환), 7.2(대용량 데이터 경험), 7.8(파이프라인 설계 — 함께 읽으면 좋다).

정직: Jazzy 기준 버전과 커스텀 포인트 타입 등록을 실제로 컴파일·실행해 확인했다(7.4.13). pcl_conversions 2.6.4 · pcl_ros 2.6.4 · PCL 1.14.0이며 둘 다 apt로 설치된다. 다른 배포판에서는 PCL 버전이 다를 수 있다(목차 3).

7.4.2 메시지 구조

header:
  stamp: {...}                 # 스캔 시각(7.5.3에서 정밀 논의)
  frame_id: "lidar_link"
height: 1                      # 비정렬이면 1
width: 57600                   # 점의 개수(비정렬) 또는 열 수(정렬)
fields:                        # ★ 각 점의 필드 기술
  - {name: "x",         offset: 0,  datatype: 7, count: 1}    # FLOAT32
  - {name: "y",         offset: 4,  datatype: 7, count: 1}
  - {name: "z",         offset: 8,  datatype: 7, count: 1}
  - {name: "intensity", offset: 12, datatype: 7, count: 1}
  - {name: "ring",      offset: 16, datatype: 4, count: 1}    # UINT16
  - {name: "time",      offset: 20, datatype: 7, count: 1}
is_bigendian: false
point_step: 32                 # ★ 점 하나의 바이트 수 (패딩 포함)
row_step: 1843200              # 한 행의 바이트 수 = width * point_step
data: [...]                    # height * row_step 바이트
is_dense: true                 # 무효 점(NaN/Inf)이 없으면 true

datatype 값:

point_step이 필드 크기 합보다 클 수 있다. 위 예에서 필드 합은 4+4+4+4+2+4 = 22바이트인데 point_step이 32다 — 패딩이 10바이트 들어 있다.

함정: point_step을 필드 크기 합으로 계산하지 말라. 정렬을 위한 패딩이 있고, 벤더가 여분 필드를 두기도 한다. 반드시 point_step을 읽어 쓰라 — 7.2.2에서 step 대신 width를 쓰면 안 되는 것과 같은 성질의 함정이다.

필드는 고정이 아니다. 센서·처리 단계마다 다르다.

함정: 필드 이름·존재를 가정하지 말라. intensity가 있을 것이라 믿고 접근하면 다른 센서에서 크래시한다. 필드 존재를 먼저 확인하는 코드를 쓰라.

bool has_field(const sensor_msgs::msg::PointCloud2 & cloud, const std::string & name) {
  return std::any_of(cloud.fields.begin(), cloud.fields.end(),
                     [&](const auto & f) { return f.name == name; });
}

7.4.3 정렬·비정렬 구름

정렬 구름의 이점:

정렬 구름에는 무효 점이 포함된다 — 측정 실패 지점을 NaN으로 채워 격자를 유지한다. 그래서 is_dense = false가 된다.

함정: is_dense를 무시하고 계산하면 NaN이 전파된다. 평균·중심·법선 계산에서 NaN 하나가 결과 전체를 NaN으로 만든다. is_dense == false면 반드시 유효성을 검사하라.

for (...) {
  if (!std::isfinite(*iter_x) || !std::isfinite(*iter_y) || !std::isfinite(*iter_z)) {
    continue;      // 무효 점 건너뜀
  }
  // ...
}

함정: is_dense를 잘못 설정한 드라이버가 있다. true인데 NaN이 있는 경우가 실제로 발생한다. 신뢰하되 검증하라 — 특히 새 센서를 장착할 때 확인한다.

7.4.4 반복자로 접근하기

바이트 오프셋을 직접 계산하지 말고 반복자를 쓰라.

#include "sensor_msgs/point_cloud2_iterator.hpp"

// 읽기
sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
sensor_msgs::PointCloud2ConstIterator<float> iter_i(cloud, "intensity");

for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++iter_i) {
  if (!std::isfinite(*iter_x)) continue;
  process(*iter_x, *iter_y, *iter_z, *iter_i);
}

쓰기 — 모디파이어로 필드를 정의한 뒤:

sensor_msgs::msg::PointCloud2 out;
out.header = in.header;

sensor_msgs::PointCloud2Modifier modifier(out);
modifier.setPointCloud2Fields(4,
    "x", 1, sensor_msgs::msg::PointField::FLOAT32,
    "y", 1, sensor_msgs::msg::PointField::FLOAT32,
    "z", 1, sensor_msgs::msg::PointField::FLOAT32,
    "intensity", 1, sensor_msgs::msg::PointField::FLOAT32);
modifier.resize(n_points);

sensor_msgs::PointCloud2Iterator<float> ox(out, "x");
// ... 채우기 ...

편의 함수:

// XYZ 또는 XYZRGB 표준 레이아웃을 한 번에
modifier.setPointCloud2FieldsByString(1, "xyz");
modifier.setPointCloud2FieldsByString(2, "xyz", "rgb");

견해: 반복자가 안전하지만 빠르지는 않다. 각 증가마다 오프셋 계산이 들어가므로, 점 수백만 개를 순회하는 고빈도 경로에서는 비용이 된다. 그런 경우 point_step을 이용한 직접 포인터 접근이 빠르지만, 레이아웃 가정이 깨질 위험을 감수해야 한다. 먼저 반복자로 작성하고 측정 후 필요할 때만 최적화하라.

Python:

from sensor_msgs_py import point_cloud2

# 구조화 numpy 배열로 (2.8.7)
points = point_cloud2.read_points_numpy(msg, field_names=('x', 'y', 'z'),
                                        skip_nans=True)
# → numpy 연산으로 처리 (GIL 해제)
centroid = points.mean(axis=0)

함정: Python에서 점을 하나씩 순회하면 사실상 불가능하다(2.8.7). 100만 점 반복은 수 초가 걸린다. 반드시 numpy 배열로 받아 벡터 연산을 쓴다.

7.4.5 PCL 연동

PCL(Point Cloud Library)은 포인트클라우드 처리의 표준 라이브러리다.

#include "pcl_conversions/pcl_conversions.h"
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>

// ROS → PCL
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*ros_cloud, *pcl_cloud);

// 처리
pcl::VoxelGrid<pcl::PointXYZ> voxel;
voxel.setInputCloud(pcl_cloud);
voxel.setLeafSize(0.05f, 0.05f, 0.05f);
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered(new pcl::PointCloud<pcl::PointXYZ>);
voxel.filter(*filtered);

// PCL → ROS
sensor_msgs::msg::PointCloud2 out;
pcl::toROSMsg(*filtered, out);
out.header = ros_cloud->header;      // ★ 헤더 전파(7.2.5)

변환 비용:

함정: 변환에도 비용이 든다. 100만 점이면 수십 MB를 복사한다. 파이프라인에서 ROS↔PCL을 여러 번 왕복하면 그것만으로 CPU가 소진된다. 가능하면 PCL 영역 안에서 여러 필터를 연속 적용하고, 마지막에 한 번만 변환하라.

// ❌ 왕복이 많다
auto c1 = filter_voxel(ros_cloud);      // 내부에서 from/to
auto c2 = filter_passthrough(c1);       // 또 from/to
auto c3 = filter_outlier(c2);           // 또

// ✅ 한 번 변환, PCL 안에서 연속 처리
pcl::fromROSMsg(*ros_cloud, *pcl_cloud);
voxel.filter(*tmp1);
passthrough.setInputCloud(tmp1); passthrough.filter(*tmp2);
outlier.setInputCloud(tmp2); outlier.filter(*result);
pcl::toROSMsg(*result, out);

PCL 포인트 타입:

함정: ROS 메시지의 필드와 PCL 타입이 맞아야 한다. intensity가 있는 구름을 PointXYZ로 변환하면 그 필드가 버려진다 — 오류는 안 나지만 정보가 사라진다.

커스텀 필드(라이다의 ring·time)를 다루려면 PCL 커스텀 포인트 타입을 정의해야 한다. 정확한 매크로 형태와, 그것이 메모리를 얼마나 쓰는지는 7.4.13에서 실제로 컴파일해 확인했다. 번거롭기 때문에 반복자로 직접 다루는 편이 나은 경우도 많다.

7.4.6 필터링

대부분의 파이프라인에서 첫 단계는 데이터를 줄이는 것이다.

VoxelGrid의 효과 추정:

$$N_{\text{after}} \approx \min\left(N_{\text{before}}, \frac{V_{\text{occupied}}}{l^3}\right)$$

여기서 $l$은 리프 크기. 리프 5 cm면 1 m³ 공간에 최대 8000점이다.

견해: VoxelGrid를 파이프라인 앞쪽에 두는 것이 가장 효과적인 최적화다. 이후 모든 단계의 비용이 비례해 줄어든다. 리프 크기는 응용의 정밀도 요구에서 정한다 — 항법 장애물 회피에 5 cm면 대개 충분하고, 정밀 파지에는 더 작아야 한다.

함정: 필터 순서가 성능을 좌우한다. 이상점 제거를 먼저 하면 100만 점에 대해 비용이 큰 이웃 탐색을 하고, VoxelGrid를 먼저 하면 1만 점에 대해서만 한다. 비용이 낮은 필터를 앞에, 비용이 큰 필터를 뒤에.

7.4.7 크기와 대역 예산

1.3.9의 식을 포인트클라우드에 적용한다.

$$S = \text{height} \times \text{width} \times \text{point\_step} + \text{헤더}$$

정직: 위 수치는 일반적 구성 기준의 계산 예시이며 센서·설정에 따라 다르다(확인 필요). 자신의 센서에서 ros2 topic bw로 실측해야 한다(1.3.9).

RGB-D 정렬 구름이 특히 용량이 크다 — 이미지 해상도만큼의 점을 만들기 때문이다. 깊이 이미지로 전송하고 필요한 곳에서 구름으로 변환하는 편이 훨씬 부하가 작다(깊이 16UC1은 점당 2바이트, 구름은 32바이트).

견해: "깊이 이미지로 옮기고 소비 지점에서 구름 생성"이 RGB-D 파이프라인의 기본 원칙이다. 16배 차이가 나며, CameraInfo(7.3)만 있으면 언제든 변환할 수 있다.

7.4.8 tf 변환 비용

포인트클라우드를 다른 프레임으로 옮기는 것은 비용이 크다.

#include "tf2_sensor_msgs/tf2_sensor_msgs.hpp"

sensor_msgs::msg::PointCloud2 out;
tf_buffer_->transform(in, out, "base_link", tf2::durationFromSec(0.1));

비용: 모든 점에 4×4 행렬 곱. 100만 점이면 100만 번의 변환이다.

견해: "필터 후 변환"이 기본 순서다. 1만 점으로 줄인 뒤 변환하면 비용이 100분의 1이다. 반대로 하면 버릴 점까지 변환하는 셈이다.

함정: tf 조회 실패 시 예외가 난다(5.1.3). 고빈도 포인트클라우드에서 이 예외 처리를 빠뜨리면 노드가 종료되고, 포착하되 로그만 남기면 조용히 데이터를 버린다. 5.2.8의 견고한 패턴(실패 카운터 + 진단)을 적용하라.

7.4.9 파이프라인 설계

7.8에서 일반화하지만, 포인트클라우드 특유의 원칙이 있다.

원칙:

견해: 라이다·RGB-D 파이프라인은 컴포지션의 대표 수혜 사례다(7.2.6의 이미지와 같다). 드라이버부터 필터·처리까지 한 컨테이너에 넣고 intra-process를 켜면, 수 MB 구름이 포인터로 오간다. ros2 topic echo나 RViz2로 원본 구름을 열면 그 순간 직렬화가 다시 발생하므로(2.7.6), 디버깅 시 유의한다.

7.4.10 진단·검증

# 크기·주기
ros2 topic bw /points
ros2 topic hz /points

# 구조 확인 (배열 생략 — 0.7.6)
ros2 topic echo /points --no-arr --once

# RViz2에서 시각 확인
# - PointCloud2 디스플레이, Fixed Frame 확인
# - Color Transformer로 intensity·z 확인

--no-arr가 필수다 — 100만 점을 터미널에 출력하면 시스템이 멈춘다.

확인 항목:

견해: RViz2에서 "앞에 벽" 검증이 여기서도 가장 빠르다(7.1.9). 좌표 규약·프레임·변환이 한 번에 확인된다.

7.4.13 커스텀 포인트 타입 — 실측

7.4.5가 "커스텀 필드는 매크로로 등록한다"고만 하고 넘어갔다. 실제 형태와 그 대가를 Jazzy에서 컴파일·실행해 확인했다(실행 확인, PCL 1.14.0 · pcl_conversions 2.6.4).

7.4.13.1 등록 매크로의 형태

라이다의 ring까지 담는 타입을 이렇게 정의한다. 아래 코드는 그대로 컴파일되고 실행된다(실행 확인).

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>

struct EIGEN_ALIGN16 PointXYZIR {
  PCL_ADD_POINT4D;              // x, y, z (+ 정렬용 w)
  float intensity;
  std::uint16_t ring;
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZIR,
  (float, x, x)(float, y, y)(float, z, z)
  (float, intensity, intensity)(std::uint16_t, ring, ring))

읽는 법: 매크로의 두 번째 인자는 괄호를 이어 붙인 나열이고, 각 항목은 (C++ 타입, 멤버 이름, 메시지 필드 이름)이다. 멤버 이름과 필드 이름을 다르게 줄 수 있다는 뜻인데, 다르게 주면 혼란만 커지므로 같게 쓰는 것이 관행이다.

PCL_ADD_POINT4D와 EIGEN_ALIGN16·EIGEN_MAKE_ALIGNED_OPERATOR_NEW는 선택이 아니다 — PCL이 SSE 연산을 위해 16바이트 정렬을 가정하므로, 빼면 정렬 위반으로 런타임에 종료된다.

7.4.13.2 이 타입 하나가 32바이트다

위 타입으로 구름을 만들어 toROSMsg로 변환한 결과다(실행 확인):

필드 5개: x y z intensity ring
point_step=32  is_dense=1

쓸모 있는 값은 float×4(x·y·z·intensity) + uint16(ring) = 18바이트인데 point_step은 32바이트다. 나머지 14바이트는 전부 정렬 때문이다:

44%가 버려진다(14/32). 7.4.8의 대역 계산에 이 값을 그대로 넣어야 한다 — "18바이트 × 점 개수"로 계산하면 실제의 56%밖에 반영되지 않는다.

함정: z 뒤에 숨은 w 4바이트를 잊기 쉽다. PCL_ADD_POINT4D라는 이름이 말해 주듯 x·y·z가 아니라 네 개가 들어간다. 이 때문에 PointXYZ조차 12바이트가 아니라 16바이트다. 점 백만 개면 4MB가 아무 정보 없이 나간다.

견해: ring·time만 필요하다면 커스텀 타입을 만들지 말고 sensor_msgs::PointCloud2Iterator로 직접 읽어라. 매크로·정렬·빌드 설정을 감수할 값어치는, PCL의 알고리즘(필터·정합·분할)을 실제로 쓸 때 생긴다. 필드를 몇 개 꺼내 보는 것이 목적이면 반복자가 훨씬 비용이 낮다(7.4.4).

함정: 커스텀 타입은 그것을 아는 노드끼리만 통한다. 매크로 등록은 컴파일 시점의 일이므로, 같은 PointCloud2 메시지를 받는 다른 노드가 그 타입을 모르면 fromROSMsg로 못 읽는다. 메시지 자체는 필드 이름으로 자기 서술적이니, 경계를 넘는 곳에서는 반복자로 읽는 편이 안전하다.

7.4.11 함정 카탈로그

7.4.12 체크포인트

• PointCloud2의 핵심 필드 여섯 가지와 각각의 의미를 말할 수 있는가. point_step을 직접 계산하면 안 되는 이유는?
• 필드가 고정이 아니라는 사실의 실무적 함의는? 안전한 접근 방법은?
• 정렬·비정렬 구름의 차이와, 정렬 구름의 이점 세 가지를 말할 수 있는가.
• is_dense의 의미와, 무시했을 때의 결과는? 신뢰해도 되는가.
• 반복자를 쓰는 이유와, 그것이 빠르지 않다는 사실의 함의는?
• ROS↔PCL 변환 비용이 파이프라인 설계에 미치는 영향을 설명할 수 있는가.
• VoxelGrid를 앞쪽에 두는 것이 왜 가장 효과적인 최적화인가. 필터 순서 원칙은?
• 64채널 라이다 10 Hz의 대역을 계산할 수 있는가. RGB-D를 구름으로 보내면 왜 16배가 되는가.
• tf 변환 비용과, "필터 후 변환"이 기본인 이유를 말할 수 있는가.
• 포인트클라우드 파이프라인의 다섯 가지 설계 원칙을 들 수 있는가.
• ros2 topic echo로 구름을 볼 때 반드시 필요한 옵션은? 좌표 검증의 가장 빠른 방법은?
• 커스텀 포인트 타입 등록 매크로의 형태는? point_step이 유효 데이터보다 큰 이유와 그 비율은(7.4.13).

다음 장 7.5은 2D·3D 라이다의 고유 주제 — LaserScan의 시간 규약과 운동 왜곡을 다룬다. 스캔 한 바퀴 동안 로봇이 움직이면 무슨 일이 생기는가.