PCLで点群のクラスタリング

詳解３次元点群処理に点群の距離を使ったセグメンテーション（クラスタリング）が載っていたのでPCLでやってみました。

www.kspub.co.jp

基本的にはここのサンプルでOK。

pcl.readthedocs.io

このサンプルは、最初に大きな面を取り除いています。テーブルの上に乗っている物をクラスタリングしたいときにテーブルを除くような処理です。今回は、教科書と同じ結果がほしいので後半だけを持ってきています。

やってみるとこのようにクラスタリングされています。うまく行ってますね。 ちなみにCloudCompareで点群の色を変えるには、点群を選択しEdit->Colors->Set Uniqueで変更できます。

以下、ソースです。

#include <iostream>
#include <string>

#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <iomanip>



int main(void) {
    std::string input_file_path = "/home/natu/tmp/fragment.pcd";
    std::string cluster_file_path = "/home/natu/tmp/";

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
    int ret = pcl::io::loadPCDFile<pcl::PointXYZ>(input_file_path, *cloud);
    if(ret == -1) {
        std::cout << "Error, can't read " << input_file_path << std::endl;
        return -1;
    }
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZ>);


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

    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.01);
    ec.setMinClusterSize (100);
    ec.setMaxClusterSize (500000);
    ec.setSearchMethod (tree);
    ec.setInputCloud (cloud);
    ec.extract (cluster_indices);

     int j = 0;
     for (const auto& cluster : cluster_indices) {
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
        for (const auto& idx : cluster.indices) {
              cloud_cluster->push_back((*cloud)[idx]);
        }
        cloud_cluster->width = cloud_cluster->size ();
        cloud_cluster->height = 1;
        cloud_cluster->is_dense = true;

        std::cout << "PointCloud representing the Cluster: " << cloud_cluster->size () << " data points." << std::endl;
        std::stringstream ss;
        ss << std::setw(4) << std::setfill('0') << j;
        writer.write<pcl::PointXYZ> (cluster_file_path + "cloud_cluster_" + ss.str () + ".pcd", *cloud_cluster, false); //*
        j++;
     }

    return 0;
}