PCL(Point Cloud Libraly)を使ったユークリッドクラスタリングの実装例を公開しました。

【PCL,ROS】ユークリッドクラスタリング(EuclideanClusterExtraction)

3D-LiDARを扱う際、点群をクラスリングしたいことがよくあります。典型的な点群のクラスタリングの1つに、ユークリッドクラスタリングがあります。ユークリッドクラスタリングは、名前の通り、点と点のユークリッド距離(単純な距離)のみを考慮したクラスタリングです。

PCLを用いれば、簡単にユークリッドクラスタリングを実装することができます。この記事では、ROS対応の3D-LiDARで観測した点群にユークリッドクラスタ分析を適用するソースコードを公開します。

実装の概要

  • Subscribe(入力):点群(sensor_msg::pointCloud2)
  • Publish(出力):なし
  • 可視化クラス:pcl::visualization::PCLVisualizer
  • 処理の流れ
    1. 点群をサブスクライブ
      sensor_msg::pointCloud2
    2. 点群情報の型を変換(ROS型→PCL型)
      pcl::PointCloud<pcl::PointXYZ>::Ptr
    3. クラスタリング
      std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>
    4. クラスタごとに色付けして可視化

ソースコード

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/visualization/cloud_viewer.h>

class EuclideanClustering{
	private:
		/*node handle*/
		ros::NodeHandle nh;
		ros::NodeHandle nhPrivate;
		/*subscribe*/
		ros::Subscriber sub_pc;
		/*pcl objects*/
		pcl::visualization::PCLVisualizer viewer {"Euclidian Clustering"};
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud {new pcl::PointCloud<pcl::PointXYZ>};
		std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clusters;
		/*parameters*/
		double cluster_tolerance;
		int min_cluster_size;
	public:
		EuclideanClustering();
		void CallbackPC(const sensor_msgs::PointCloud2ConstPtr &msg);
		void Clustering(void);
		void Visualization(void);
};

EuclideanClustering::EuclideanClustering()
	:nhPrivate("~")
{
	sub_pc = nh.subscribe("/velodyne_points", 1, &EuclideanClustering::CallbackPC, this);
	viewer.setBackgroundColor(1, 1, 1);
	viewer.addCoordinateSystem(1.0, "axis");
	viewer.setCameraPosition(0.0, 0.0, 35.0, 0.0, 0.0, 0.0);

	nhPrivate.param("cluster_tolerance", cluster_tolerance, 0.1);
	nhPrivate.param("min_cluster_size", min_cluster_size, 100);
	std::cout << "cluster_tolerance = " << cluster_tolerance << std::endl;
	std::cout << "min_cluster_size = " << min_cluster_size << std::endl;
}

void EuclideanClustering::CallbackPC(const sensor_msgs::PointCloud2ConstPtr &msg)
{
	/* std::cout << "CALLBACK PC" << std::endl; */

	pcl::fromROSMsg(*msg, *cloud);
	std::cout << "==========" << std::endl;
	std::cout << "cloud->points.size() = " << cloud->points.size() << std::endl;

	clusters.clear();
	Clustering();
	Visualization();
}

void EuclideanClustering::Clustering(void)
{
	double time_start = ros::Time::now().toSec();

	/*clustering*/
	/*kd-treeクラスを宣言*/
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
	/*探索する点群をinput*/
	tree->setInputCloud(cloud);
	/*クラスタリング後のインデックスが格納されるベクトル*/
	std::vector<pcl::PointIndices> cluster_indices;
	/*今回の主役*/
	pcl::EuclideanClusterExtraction<pcl::PointXYZ> ece;
	/*距離の閾値を設定*/
	ece.setClusterTolerance(cluster_tolerance);
	/*各クラスタのメンバの最小数を設定*/
	ece.setMinClusterSize(min_cluster_size);
	/*各クラスタのメンバの最大数を設定*/
	ece.setMaxClusterSize(cloud->points.size());
	/*探索方法を設定*/
	ece.setSearchMethod(tree);
	/*クラスリング対象の点群をinput*/
	ece.setInputCloud(cloud);
	/*クラスリング実行*/
	ece.extract(cluster_indices);

	std::cout << "cluster_indices.size() = " << cluster_indices.size() << std::endl;

	/*dividing(クラスタごとに点群を分割)*/
	pcl::ExtractIndices<pcl::PointXYZ> ei;
	ei.setInputCloud(cloud);
	ei.setNegative(false);
	for(size_t i=0;i<cluster_indices.size();i++){
		/*extract*/
		pcl::PointCloud<pcl::PointXYZ>::Ptr tmp_clustered_points (new pcl::PointCloud<pcl::PointXYZ>);
		pcl::PointIndices::Ptr tmp_clustered_indices (new pcl::PointIndices);
		*tmp_clustered_indices = cluster_indices[i];
		ei.setIndices(tmp_clustered_indices);
		ei.filter(*tmp_clustered_points);
		/*input*/
		clusters.push_back(tmp_clustered_points);
	}

	std::cout << "clustering time [s] = " << ros::Time::now().toSec() - time_start << std::endl;
}

void EuclideanClustering::Visualization(void)
{
	/*前ステップの可視化をリセット*/
	viewer.removeAllPointClouds();

	/*cloud*/
	viewer.addPointCloud(cloud, "cloud");
	viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0, 0.0, 0.0, "cloud");
	viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "cloud");
	/*clusters*/
	double rgb[3] = {};
	const int channel = 3;	//RGB
	const double step = ceil(pow(clusters.size()+2, 1.0/(double)channel));	//exept (000),(111)
	const double max = 1.0;
	/*クラスタをいい感じに色分け*/
	for(size_t i=0;i<clusters.size();i++){
		std::string name = "cluster_" + std::to_string(i);
		rgb[0] += 1/step;
		for(int j=0;j<channel-1;j++){
			if(rgb[j]>max){
				rgb[j] -= max + 1/step;
				rgb[j+1] += 1/step;
			}
		}
		viewer.addPointCloud(clusters[i], name);
		viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, rgb[0], rgb[1], rgb[2], name);
		viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, name);
	}
	/*表示の更新*/
	viewer.spinOnce();
}

int main(int argc, char** argv)
{
	ros::init(argc, argv, "euclidean_clustering");
	
	EuclideanClustering euclidean_clustering;

	ros::spin();
}

実装の解説

  • パラメータ cluster_tolerance
    点と点の距離がこの閾値(cluster_tolerance)より小さい場合、その点と点は同じクラスタに分類される。逆に、この閾値より遠い点は別のクラスタに分類される。
  • パラメータ min_cluster_size
    メンバー(点)の数がこの閾値(min_cluster_size)より多いクラスタのみ出力される。逆に、この閾値より点が少ないクラスタは無視される。
  • 関数 void CallbackPC(const sensor_msgs::PointCloud2ConstPtr &msg)
    点群をサブスクライブするたびにこの関数が呼ばれます。
  • 関数 void Clustering(void)
    実際にクラスタリングを行っている関数で、今回のメインパート。
  • 関数 void Visualization(void)
    いい感じに色分けするために複雑に書かれているが、今回の本題ではないので、興味がなければ流し読みでOK。

クラスタリングの関連記事

クラスタリングの関連記事のリンクを載せておきます。

【PCL,ROS】法線群のクラスタリング(ConditionalEuclideanClustering)
【PCL,ROS】クラスタリングのtoleranceを距離に応じて変えたい

さいごに

主にClustering(void)をゆっくり読んでいただいて、使い方の例を把握していただけたらと思います。参考になれば幸いです。


以上です。

Ad.