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

はじめに

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

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

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

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

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

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

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

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

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

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

点群データとは何か？

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

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

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;

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

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

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

PCL（Point Cloud Library）の基本

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

基本的な使い方

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フィルタ（範囲指定フィルタ）

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

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);

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

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

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

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);

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

パラメータの目安

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

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

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

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);

2. 座標変換

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

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);

中級操作編

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

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

平面検出（RANSAC）

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

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;

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

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

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;

2. 特徴量抽出

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

法線推定

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

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);

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

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

ICP（Iterative Closest Point）

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

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;
}

実践応用編

ROSとの連携

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

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を使用してください。

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. メモリ管理 : 不要な点群データの適切な解放

C++

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

物体認識への応用例

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

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）を使用して座標系を明確に管理
• 座標変換の方向を常に意識する

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公式ドキュメント
• ROS Point Cloud Library (PCL) Tutorials
• Open3D Documentation - Pythonでの点群処理
• pclpy - PCLのPythonバインディング

おまけ：PCL 1.14の新機能

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

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

詳細はPCL Release Notesをご確認ください。