【C++】ロボット×PCLで学ぶ点群処理の基本と応用

## はじめに

ロボット開発や3Dセンサーを使ったアプリづくりに関わっていると、仕事でも趣味でも、いつか必ず出会うのが**点群処理**です。LiDARやDepthカメラなどから得られる3Dデータをどう扱うかは、物体検出、マッピング、経路計画といった多くの場面で重要になってきます。

でも実際のところ、「PCL（Point Cloud Library）は使っているけど、何が起きているのかよくわからない」「フィルタの種類が多すぎて、どれを使えばいいのか迷う」「動くには動くけど、パラメータの意味は理解していない」――そんな感覚を持ったことはありませんか？

この記事では、そんな“今さら聞けない”点群処理の基本を、C++とPCLを使って丁寧に整理しながら紹介していきます。

仕事でも趣味でも、PCLをしっかり使いこなしたい方に向けた、基礎から応用までの実践ガイドになるよう頑張りました。初心者にもイメージしていただけるようになっています！！

:::note info
LiDAR（Light Detection and Ranging）とは、レーザーを使って対象物までの距離を高精度に測定するセンサーです。Depthカメラは、RGB画像と同時に各ピクセルの深度（距離）情報を取得できるカメラです。
:::

:::note warn
本記事のAPIはPCL 1.12以降で検証しています。1.11以前はヘッダ名や名前空間が一部異なる場合があります。
:::

## TL;DR - 初級者向けクイックガイド

まずはこれだけ押さえれば点群処理の基本ができます。

| 機能名 | 用途 |
|--------|------|
| **VoxelGrid** | 点群のダウンサンプリング（データ量削減） |
| **PassThrough** | 範囲指定で点群を切り出し |
| **StatisticalOutlierRemoval** | ノイズ除去 |
| **EuclideanClusterExtraction** | 物体ごとに点群を分離 |
| **RANSAC平面検出** | 床面や壁面の検出 |

これらを組み合わせれば「ノイズ除去 → 範囲指定 → 床面除去 → 物体検出」の基本的な処理パイプラインが構築できます！！

## 点群データとは何か？

**点群（Point Cloud）** とは、3次元空間上の点の集合体です。

各点は通常、X、Y、Z座標を持ち、場合によっては色情報（RGB）や反射強度、法線ベクトルなどの追加情報も含みます。

```cpp:C++
// PCLでの基本的な点の定義
pcl::PointXYZ point;
point.x = 1.0;
point.y = 2.0;
point.z = 3.0;

// 色情報付きの点
pcl::PointXYZRGB color_point;
color_point.x = 1.0;
color_point.y = 2.0;
color_point.z = 3.0;
color_point.r = 255;
color_point.g = 0;
color_point.b = 0;
```

![001.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2253716/495fd6ac-3172-4be0-8ecf-7899d7960051.png)

![001-1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2253716/f42524c9-b702-40ed-87ad-5343cc830683.png)


## なぜロボティクスで点群処理が必要なのか？

ロボットが環境を理解し、適切に行動するためには、周囲の3D構造を把握する必要があります。点群処理により以下が可能になります。

| 用途 | 説明 |
|------|------|
| **環境マッピング** | 周囲の壁や障害物の位置を把握 |
| **物体検出・認識** | 特定の物体を見つけて識別 |
| **経路計画** | 障害物を避けた最適な経路を生成 |
| **把持計画** | ロボットアームが物体を掴むための最適な位置を計算 |

## PCL（Point Cloud Library）の基本

PCLは点群処理に特化したC++ライブラリで、フィルタリング、セグメンテーション、レジストレーション、特徴抽出など、点群処理に必要な機能が包括的に提供されています。

### 基本的な使い方

```cpp:C++
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>

// 点群の読み込み
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::io::loadPCDFile<pcl::PointXYZ>("input.pcd", *cloud);

// 点群の保存
pcl::io::savePCDFileASCII("output.pcd", *cloud);
```

## 基本操作編

### 1. フィルタリング（ノイズ除去・範囲指定）

点群データには必ずノイズが含まれます。適切なフィルタリングは、後続の処理精度を大きく左右する重要なステップです。

#### PassThroughフィルタ（範囲指定フィルタ）

特定の軸方向で範囲を指定して点群を切り出します。

```cpp:C++
#include <pcl/filters/passthrough.h>

pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud(cloud);
pass.setFilterFieldName("z");  // Z軸方向でフィルタ
pass.setFilterLimits(0.0, 1.0);  // 0.0m～1.0mの範囲
pass.filter(*filtered_cloud);
```

**使用例**: ロボットから一定距離内の物体のみを処理対象にしたい場合

![002.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2253716/076a9f7b-5c62-4b91-ae0a-8432c8e48219.png)

![002-1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2253716/29a81efb-0ef6-437f-8c9b-767a02b1b649.png)

#### VoxelGridフィルタ（ダウンサンプリング）

3D空間を格子状に分割し、各格子内の点を代表点で置き換えることで点数を削減します。

```cpp:C++
#include <pcl/filters/voxel_grid.h>

pcl::VoxelGrid<pcl::PointXYZ> voxel;
voxel.setInputCloud(cloud);
voxel.setLeafSize(0.01f, 0.01f, 0.01f);  // 1cm×1cm×1cmの格子
voxel.filter(*downsampled_cloud);
```

:::note alert
**重要**
リーフサイズが小さすぎると効果が薄く、大きすぎると重要な特徴が失われます。対象物体のサイズに応じて調整が必要です。
:::

**パラメータの目安**
- LiDARデータ : 0.05m（5cm）程度から開始
- RGB-Dカメラ : 0.01m（1cm）程度から開始
- 精密計測 : 0.005m（5mm）以下

![003.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2253716/eefb7331-2aaf-42ab-bbc7-842cd8f24a26.png)

![003-1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2253716/65765d40-5088-45b6-a721-3ee92905b013.png)

#### StatisticalOutlierRemovalフィルタ（外れ値除去）

各点の近傍点との距離統計に基づいて外れ値を除去します。

```cpp:C++
#include <pcl/filters/statistical_outlier_removal.h>

pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
sor.setInputCloud(cloud);
sor.setMeanK(50);  // 近傍50点の平均距離を計算
sor.setStddevMulThresh(1.0);  // 標準偏差の1倍を閾値とする
sor.filter(*filtered_cloud);
```

![004.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2253716/0a6ae41e-5fd6-400d-aefa-24e473cf43a5.png)

![004-1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2253716/d2fa898e-9b6d-4432-8904-38f565f7ca84.png)

### 2. 座標変換

点群を異なる座標系間で変換する処理です。センサー座標系からロボット座標系、世界座標系への変換でよく使用されます。

```cpp:C++
#include <pcl/common/transforms.h>
#include <Eigen/Dense>
#include <cmath>  // M_PIが必要

// 変換行列の定義（例：Z軸周りに45度回転）
Eigen::Affine3f transform = Eigen::Affine3f::Identity();
transform.rotate(Eigen::AngleAxisf(M_PI/4, Eigen::Vector3f::UnitZ()));
transform.translation() << 1.0, 2.0, 3.0;  // 平行移動成分

// 変換実行
pcl::transformPointCloud(*cloud, *transformed_cloud, transform);
```

![005.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2253716/d0d04fd7-899c-4562-9a36-dccd2ccfe87f.png)

![005-1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2253716/e8bba192-97c4-40ea-87dc-8daa89b01e1e.png)

## 中級操作編

### 1. セグメンテーション（領域分割）

点群を意味のある領域に分割する処理です。

#### 平面検出（RANSAC）

RANSACアルゴリズムを使用して平面を検出します。床面や壁面の検出によく使用されます。

```cpp:C++
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/ModelCoefficients.h>

pcl::SACSegmentation<pcl::PointXYZ> seg;
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);

seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setDistanceThreshold(0.01);  // 1cmの距離閾値

seg.setInputCloud(cloud);
seg.segment(*inliers, *coefficients);

// 平面の方程式: ax + by + cz + d = 0
std::cout << "平面の係数: " 
          << coefficients->values[0] << "x + "
          << coefficients->values[1] << "y + "
          << coefficients->values[2] << "z + "
          << coefficients->values[3] << " = 0" << std::endl;
```

![006.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2253716/4f2781a0-190e-48b4-9a83-017704ed6050.png)

![006-1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2253716/960bfbb7-e326-4028-af5e-be8f9d4a17e2.png)

#### ユークリッドクラスタリング

近い距離にある点同士をクラスタとしてまとめる処理です。物体の分離に有効です。

```cpp:C++
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/kdtree/kdtree.h>

pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud(cloud);

std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance(0.02);  // 2cmの距離閾値
ec.setMinClusterSize(100);     // 最小クラスタサイズ
ec.setMaxClusterSize(25000);   // 最大クラスタサイズ
ec.setSearchMethod(tree);
ec.setInputCloud(cloud);
ec.extract(cluster_indices);

std::cout << "検出されたクラスタ数: " << cluster_indices.size() << std::endl;
```

![007.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2253716/70aac258-89e5-4c0f-ab49-85514b011a9d.png)

![007-1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2253716/6382aac6-d759-4f89-9e0c-e314c828568d.png)

### 2. 特徴量抽出

点群から幾何学的特徴を抽出します。

#### 法線推定

各点での表面法線ベクトルを計算します。表面の向きや曲率の情報が得られます。

```cpp:C++
#include <pcl/features/normal_3d.h>

pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());

ne.setInputCloud(cloud);
ne.setSearchMethod(tree);
ne.setRadiusSearch(0.03);  // 3cm半径内の近傍点を使用
ne.compute(*normals);
```
![008.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2253716/eae067f8-e5bb-40ec-b561-6cad2edaf2c2.png)

![008-1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2253716/65d601dd-a674-4d99-8db9-76c30f9179c3.png)

### 3. レジストレーション（位置合わせ）

複数の点群を統合するための位置合わせ処理です。

#### ICP（Iterative Closest Point）

2つの点群間の最適な変換を反復的に求めます。

```cpp:C++
#include <pcl/registration/icp.h>

pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setInputSource(source_cloud);
icp.setInputTarget(target_cloud);
icp.setMaxCorrespondenceDistance(0.05);  // 対応点の最大距離
icp.setMaximumIterations(100);           // 最大反復回数
icp.setTransformationEpsilon(1e-6);      // 収束判定の閾値

pcl::PointCloud<pcl::PointXYZ>::Ptr aligned_cloud(new pcl::PointCloud<pcl::PointXYZ>);
icp.align(*aligned_cloud);

if (icp.hasConverged()) {
    std::cout << "収束スコア: " << icp.getFitnessScore() << std::endl;
    std::cout << "変換行列:\n" << icp.getFinalTransformation() << std::endl;
}
```

![009.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2253716/b00d6a25-5cc2-479e-bf8f-8e1fae62130e.png)

![009-1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2253716/2fb82071-aa9d-4a7c-b602-b8000765ec82.png)

## 実践応用編

### ROSとの連携

ロボティクスではROSとの連携が不可欠です。

```cpp:C++
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>

class PointCloudProcessor {
private:
    ros::NodeHandle nh;
    ros::Subscriber sub;
    ros::Publisher pub;

public:
    PointCloudProcessor() {
        sub = nh.subscribe("/camera/depth/points", 1, &PointCloudProcessor::pointCloudCallback, this);
        pub = nh.advertise<sensor_msgs::PointCloud2>("/processed_points", 1);
    }

    void pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg) {
        // ROSメッセージをPCL形式に変換
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::fromROSMsg(*msg, *cloud);
        
        // 点群処理
        pcl::PointCloud<pcl::PointXYZ>::Ptr processed_cloud(new pcl::PointCloud<pcl::PointXYZ>);
        // ... (フィルタリングなどの処理)
        
        // PCL形式をROSメッセージに変換して配信
        sensor_msgs::PointCloud2 output_msg;
        pcl::toROSMsg(*processed_cloud, output_msg);
        output_msg.header.frame_id = "base_link";
        output_msg.header.stamp = ros::Time::now();
        
        pub.publish(output_msg);
    }
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "pointcloud_processor");
    PointCloudProcessor processor;
    ros::spin();
    return 0;
}
```

ROS2を使用する場合は、tf2_rosを使用してください。

```cpp:C++
#include <tf2_ros/transform_listener.h>  // ROS2用
#include <tf2_ros/buffer.h>

tf2_ros::Buffer tf_buffer;
tf2_ros::TransformListener tf_listener(tf_buffer);

// 座標変換の取得
geometry_msgs::TransformStamped transform_stamped = 
    tf_buffer.lookupTransform("base_link", "camera_link", tf2::TimePointZero);
```

### パフォーマンス最適化のポイント

リアルタイム処理では以下の点に注意が必要です。

1. **適切なダウンサンプリング** : VoxelGridでデータ量を削減
2. **効率的な近傍探索** : KdTreeの使用
3. **並列処理** : OpenMPを活用した処理の並列化
4. **メモリ管理** : 不要な点群データの適切な解放

```cpp:C++
// OpenMPを使用した並列処理の例
#pragma omp parallel for
for (size_t i = 0; i < cloud->points.size(); ++i) {
    // 各点に対する処理
}
```

### 物体認識への応用例

点群処理を使った簡単な物体認識の流れ。

```cpp:C++
// 1. ノイズ除去
applyStatisticalFilter(cloud, filtered_cloud);

// 2. 平面（床面）の除去
removePlane(filtered_cloud, objects_cloud);

// 3. クラスタリングで物体分離
extractClusters(objects_cloud, clusters);

// 4. 各クラスタの特徴量計算
for (auto& cluster : clusters) {
    // バウンディングボックス計算
    pcl::PointXYZ min_pt, max_pt;
    pcl::getMinMax3D(*cluster, min_pt, max_pt);
    
    // 体積計算
    float volume = (max_pt.x - min_pt.x) * 
                   (max_pt.y - min_pt.y) * 
                   (max_pt.z - min_pt.z);
    
    // 物体分類（サイズベース）
    if (volume > 0.01) {  // 10cm³以上
        // 大きな物体として処理
    }
}
```

## よくあるハマりポイント

### 1. 座標系の混乱

**問題**: センサー座標系、ロボット座標系、世界座標系が混同される

**[解決策]**
- tf（Transform）を使用して座標系を明確に管理
- 座標変換の方向を常に意識する

```cpp:C++
// 正しい座標変換の例（ROS1の場合）
tf::TransformListener listener;
tf::StampedTransform transform;
listener.lookupTransform("/base_link", "/camera_link", ros::Time(0), transform);
```

### 2. パラメータ調整の落とし穴

**[問題]**
フィルタのパラメータが適切でない

**[よくある間違い]**
- VoxelGridのリーフサイズが大きすぎて特徴が失われる
- クラスタリングの距離閾値が不適切で過分割・過統合が発生

**[解決策]**
- 対象物体のサイズスケールに応じたパラメータ設定
- 可視化ツール（RViz、PCL Viewer）での結果確認

### 3. メモリ使用量とパフォーマンス

**[問題]**
大きな点群データでメモリ不足やパフォーマンス低下

**[解決策]**
- 処理前のダウンサンプリング
- 必要な領域のみの切り出し（PassThroughフィルタ）
- スマートポインタの適切な使用

## まとめ

点群処理は複雑に見えますが、基本的な操作の組み合わせで様々なタスクを実現できます。

重要なのは・・

- **目的に応じた適切なフィルタの選択**
- **パラメータの意味を理解した調整**
- **座標系の明確な管理**
- **パフォーマンスを意識した実装**

特に、実際のロボットシステムでは、リアルタイム性と精度のバランスが重要になります。まずは基本的な処理フローを理解し、実際のデータで試行錯誤しながら最適なパラメータを見つけていくことが上達の近道です。

PCLは非常に強力なライブラリですが、その分学習コストも高めです。しかし、一度基本を押さえれば、3Dビジョンの様々な課題に対応できる強力なツールとなるでしょう。

## 参考資料

- [PCL公式ドキュメント](http://pointclouds.org/)
- [ROS Point Cloud Library (PCL) Tutorials](http://wiki.ros.org/pcl)
- [Open3D Documentation](http://www.open3d.org/) - Pythonでの点群処理
- [pclpy](https://github.com/davidcaron/pclpy) - PCLのPythonバインディング

## おまけ：PCL 1.14の新機能

PCL 1.14（2025年Q1リリース）では以下の新機能が追加されています。

- **VoxelGrid::setSaveLeafLayout()** - ボクセル構造の保存機能
- **改良されたGPU加速サポート**
- **より効率的なメモリ管理**

詳細は[PCL Release Notes](https://github.com/PointCloudLibrary/pcl/releases)をご確認ください。