VM LAB: 点群のクラスタリング

　Kinectから得られる点群データは基本的に背景や、空間内に存在する物体情報が一つのファイルの中に含まれている。物体認識などを点群データを用いて行う場合は、ラスタースキャンなどによって3次元空間中をマッチングしていくことも勿論可能であるが、2次元での探索以上に時間がかかってしまう。そこで入力点群にクラスタリングを行い1つのデータを複数のデータに分割してそれぞれを比較するとにより高速に行う方法がある。

処理の流れ

入力点群PのKd-treeを作る
空のクラスタを作り、検査が必要な点Qを設定する
Pに属する全ての点ｐiに対して以下の処理を行う。

ｐiををQに加える
Qに属する全ての点ｐiに対して以下の処理を行う

設定半径内に存在するpiの近隣の点ｐnを捜査する
発見された全ての点pnがすでに処理を行われていなければQに加える

Qに保存された全ての点への処理が終了した場合QをCに登録し、Qの点群を空にする

全てのPに属する点pへの処理が完了したら終了とする。Cに登録された各QがPから得られたクラスタである。

コード

#include "stdafx.h"

#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <direct.h>

class Size
{
public:
   double length;///x方向
   double width;///y方向
   double height;///z方向
};

Size max_min(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)//点群の各XYZ方向の最大値と最小値を求める
{
   double min_x, min_y, min_z, max_x, max_y, max_z;
   Size size;
   min_x = 10000;
   min_y = 10000;
   min_z = 100000;
   max_x = -10000;
   max_y = -10000;
   max_z = -10000;

   for(int i = 0; i < cloud->points.size(); i++)
   {
       if(max_x < cloud->points[i].x)
           max_x = cloud->points[i].x;
       if(max_y < cloud->points[i].y)
           max_y = cloud->points[i].y;
       if(max_z < cloud->points[i].z)
           max_z = cloud->points[i].z;

       if(min_x > cloud->points[i].x)
           min_x = cloud->points[i].x;
       if(min_y > cloud->points[i].y)
           min_y = cloud->points[i].y;
       if(min_z > cloud->points[i].z)
           min_z = cloud->points[i].z;
   }

   size.length = abs(max_x) + abs(min_x);
   size.width = abs(max_y) + abs(min_y);
   size.height = abs(max_z) + abs(min_y);
   return size;
}

void Clustering(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
   Size size;
   size = max_min(cloud);
   double min = size.height;
   if(min > size.width)
       min = size.width;
   if(min > size.length)
       min = size.length;
   if(min > size.height)
       min = size.height;

   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 (min / 10);
   ec.setMinClusterSize (10);//最小のクラスターの値を設定
   ec.setMaxClusterSize (25000);//最大のクラスターの値を設定
   ec.setSearchMethod (tree);//検索に使用する手法を指定
   ec.setInputCloud (cloud);//点群を入力
   ec.extract (cluster_indices);//クラスター情報を出力

   std::string name;
   name = "cluster";

   int j = 0;
   for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)//クラスターを1塊ごと出力
   {
       pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
       for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
           cloud_cluster->points.push_back (cloud->points[*pit]); //*

       cloud_cluster->width = cloud_cluster->points.size ();
       cloud_cluster->height = 1;
       cloud_cluster->is_dense = true;

       std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
       std::stringstream ss;
       ss << name << "_p_" << j << ".pcd";//クラスター毎に名前を変化
       pcl::io::savePCDFileBinary(ss.str(), *cloud_cluster);
       j++;
   }
}

int _tmain(int argc, _TCHAR* argv[])
{
   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
   pcl::io::loadPCDFile(argv[1], *cloud);

   Clustering(cloud);

   return (0);

}

実行結果