點雲處理：PCL 基礎

問題定義

雷射雷達提供的是二維平面上的距離量測（第 4.4 節介紹的 LaserScan），但深度相機、三維光達這類感測器提供的是完整三維空間裡的一大群點——每個點帶有自己的 xyz 座標，可能還有顏色、強度等額外資訊。這種資料型態稱為點雲（point cloud），是三維空間感知（例如物體辨識、精確的抓取姿態計算）不可或缺的資料來源。這一節介紹點雲的資料結構，以及用 PCL（Point Cloud Library）做基本的降採樣與濾波處理。

核心概念說明

PointCloud2：欄位可自訂的彈性結構

sensor_msgs/msg/PointCloud2 跟 sensor_msgs/msg/Image 一樣，底層是一維攤平的位元組陣列（data 欄位），但差異在於 PointCloud2 允許自訂每個點包含哪些欄位——fields 陣列描述了每個點裡有哪些資訊（例如 x、y、z、rgb）、每個欄位的資料型別與在每個點內的位元組偏移量。這種彈性讓 PointCloud2 可以同時服務「只需要純幾何位置」跟「需要顏色、法向量等豐富資訊」的不同應用場景，但也代表解析 PointCloud2 需要先讀懂 fields 的定義，不能直接假設固定的資料排列方式。

PCL：專門處理點雲的函式庫

直接手動解析、運算成千上萬個三維點的資料，效率跟開發便利性都不理想。PCL（Point Cloud Library）提供了大量針對點雲設計的資料結構與演算法，類似 OpenCV 之於影像處理的角色。ROS2 生態系裡，pcl_conversions 套件負責在 sensor_msgs/msg/PointCloud2（ROS2 訊息格式）與 PCL 自己的點雲資料結構之間轉換，概念上跟上一節 cv_bridge 扮演的角色是類比的。

體素降採樣：用近似換取效能

原始點雲資料量往往非常龐大（一次深度相機擷取可能有數十萬個點），直接對所有點做運算（例如逐點檢查是否為障礙物）計算成本很高。**體素降採樣（Voxel Grid Downsampling）**是常見的第一步前處理：把三維空間切成規則的小方格（體素，voxel），同一個方格內的所有點合併成一個代表點（通常取重心），大幅減少點的數量，同時保留整體形狀的近似輪廓。這是典型的「用少量精度損失換取大幅效能提升」的權衡，多數下游應用（例如判斷某個區域大致是否有障礙物）對這種程度的近似完全可以接受。

實作範例：訂閱點雲、降採樣、過濾範圍外的點

延續前面章節的模擬環境架構，假設已經橋接好一個深度相機的點雲主題 /camera/points。

檔案位置：~/ros2_ws/src/my_package/my_package/pointcloud_processor.py

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import PointCloud2
import sensor_msgs_py.point_cloud2 as pc2
import numpy as np


class PointCloudProcessor(Node):
    def __init__(self):
        super().__init__("pointcloud_processor")
        self.subscription = self.create_subscription(
            PointCloud2, "/camera/points", self.on_pointcloud, 10
        )
        self.publisher = self.create_publisher(PointCloud2, "/camera/points_filtered", 10)
        self.voxel_size = 0.05
        self.max_range = 3.0

    def on_pointcloud(self, msg: PointCloud2):
        points = np.array(
            list(pc2.read_points(msg, field_names=("x", "y", "z"), skip_nans=True))
        )
        if points.shape[0] == 0:
            return

        distances = np.linalg.norm(points, axis=1)
        points = points[distances < self.max_range]

        if points.shape[0] == 0:
            return
        voxel_indices = np.floor(points / self.voxel_size).astype(int)
        _, unique_idx = np.unique(voxel_indices, axis=0, return_index=True)
        downsampled = points[unique_idx]

        out_msg = pc2.create_cloud_xyz32(msg.header, downsampled.tolist())
        self.publisher.publish(out_msg)

        self.get_logger().info(
            f"原始點數: {points.shape[0]}, 降採樣後: {downsampled.shape[0]}"
        )


def main():
    rclpy.init()
    node = PointCloudProcessor()
    try:
        rclpy.spin(node)
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == "__main__":
    main()

這個範例做了兩層前處理：先用 max_range 過濾掉距離過遠、通常雜訊比例較高的點，再用簡化版的體素降採樣（用整數化的體素索引找出每個體素內的代表點）大幅減少資料量。實務上會直接呼叫 PCL 提供的 VoxelGrid 濾波器（C++ 介面較完整），這裡用 numpy 示範簡化邏輯，方便理解降採樣的實際運作原理。

執行並觀察效果

ros2 run my_package pointcloud_processor

預期輸出

[INFO] [pointcloud_processor]: 原始點數: 76800, 降採樣後: 4213
[INFO] [pointcloud_processor]: 原始點數: 75920, 降採樣後: 4198

從 7 萬多點降到 4 千多點，數量級的減少對後續運算（例如碰撞檢查、物體分割）的效能影響非常明顯。在 RViz2 裡加入兩個 PointCloud2 Display，分別訂閱 /camera/points 跟 /camera/points_filtered，可以直觀比較降採樣前後的視覺差異——整體形狀輪廓保留下來，但點的密度明顯稀疏許多。

常見錯誤與除錯技巧

錯誤一：read_points 讀出來的座標數值明顯不合理（例如出現極大或極小的異常值）

原因：常見於點雲裡混雜了 nan（Not a Number）值，代表該像素位置深度相機沒有量測到有效距離（例如物體表面反光、超出感測範圍），如果沒有正確過濾，這些 nan 值會污染後續的統計運算（例如上面範例的 np.linalg.norm 距離計算，只要有一個維度是 nan，結果就會是 nan）。

排除方式：確認讀取點雲時有正確設定過濾參數，本文範例用的 pc2.read_points(msg, ..., skip_nans=True) 就是明確要求跳過帶有 nan 的點，這是處理真實深度感測器資料時幾乎必要的第一步。

錯誤二：處理節點的 CPU 使用率持續偏高，即使降採樣邏輯本身運算量不大

原因：如果點雲的發布頻率很高（例如某些深度相機可以到 30Hz），且每一幀的原始點數本身就很龐大，即使降採樣演算法本身效率不錯，光是每一幀都要重新讀取、轉換十幾萬個點的資料，累積起來的處理量仍然可觀。

排除方式：評估下游應用是否真的需要處理每一幀資料——很多應用場景（例如靜態環境的粗略避障判斷）不需要 30Hz 的更新頻率，可以在訂閱端加入簡單的跳幀邏輯（例如每三幀只處理一幀），或考慮在感測器端（如果硬體支援）就直接降低發布頻率，而不是每一幀都完整處理後才捨棄。

小結

PointCloud2 用彈性的欄位定義支援不同豐富程度的點雲資料，PCL（或本節示範用 numpy 簡化理解的等效邏輯）提供降採樣、濾波等前處理工具，用適度的精度損失換取大幅的效能提升。點雲資料量通常遠大於一般感測資料，效能考量在設計處理流程時要優先納入。有了影像與點雲兩種感知資料的處理基礎，下一節要把這些資料接上真正的深度學習模型——用 YOLO 做物體偵測，是感知與 AI 整合最常見的入門應用。

延伸閱讀

• 影像處理：image_transport 與 cv_bridge
• ROS2 整合 YOLO 物體偵測