【PCL，ROS】クラスタリングのtoleranceを距離に応じて変えたい : LiLaBoC

PCL（Point Cloud Libraly）のユークリッドクラスタリングを少し改良することで、注目点ごとに距離の閾値（ClusterTolerance）を変えられるようにしました。

【PCL，ROS】クラスタリングのtoleranceを距離に応じて変えたい

3D-LiDARを扱う際、点群をクラスリングしたいことがよくあります。

また、3D-LiDARなどの距離センサでは、センサから遠いエリアほど点群の密度が粗（スパース）になる特徴があります。

これらを踏まえると、点群のクラスタリングをするときは、距離の許容値（ClusterTolerance）を、センサからの距離に応じて変化させたくなります。具体的には、センサからの距離が遠いところでは、より大きな許容値を設定します。

既存のクラスだと痒い所に手が届かない

PCL既存のpcl::EuclideanClusterExtractionクラスでは、距離の許容値（ClusterTolerance）が一定で変えることができません。

▶︎【PCL，ROS】ユークリッドクラスタリング（EuclideanClusterExtraction）

具体的にはこんな感じです。

#include <pcl/segmentation/extract_clusters.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> ece;
ece.setClusterTolerance(0.1);//ココ
ece.setMinClusterSize(min_cluster_size);
ece.setMaxClusterSize(cloud->points.size());
ece.setSearchMethod(tree);
ece.setInputCloud(cloud);
ece.extract(cluster_indices);

この例では、距離の許容値（ClusterTolerance）は0.1mで固定です。

解決策

PCLのpcl::EuclideanClusterExtractionの中身を参考に少し改良することで簡単に距離の閾値（ClusterTolerance）を変えることができます。

実装の概要

sub：点群（sensor_msg::pointCloud2）
pub：なし
可視化：pcl::visualization::PCLVisualizer
距離に応じて距離の許容値（ClusterTolerance）を変える
sensor_msg::pointCloud2
↓　変換
pcl::PointCloud<pcl::PointXYZ>::Ptr
↓　クラスタリング
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>
↓
可視化

ソースコード

#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/kdtree/kdtree_flann.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/visualization/cloud_viewer.h>

class EuclideanClusteringFlexibleTolerance{
	private:
		/*node handle*/
		ros::NodeHandle nh;
		ros::NodeHandle nhPrivate;
		/*subscribe*/
		ros::Subscriber sub_pc;
		/*pcl objects*/
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud {new pcl::PointCloud<pcl::PointXYZ>};
		std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clusters;
		pcl::visualization::PCLVisualizer viewer {"Euclidian Clustering"};
		/*parameters*/
		double ratio_depth_tolerance;
		double min_tolerance;
		double max_tolerance;
		int min_cluster_size;
		int max_cluster_size;
	public:
		EuclideanClusteringFlexibleTolerance();
		void CallbackPC(const sensor_msgs::PointCloud2ConstPtr &msg);
		void Clustering(void);
		double ComputeTolerance(const pcl::PointXYZ& point);
		bool CustomCondition(const pcl::PointXYZ& seed_point, const pcl::PointXYZ& candidate_point, float squared_distance);
		void Visualization(void);
};

EuclideanClusteringFlexibleTolerance::EuclideanClusteringFlexibleTolerance()
	:nhPrivate("~")
{
	sub_pc = nh.subscribe("/velodyne_points", 1, &EuclideanClusteringFlexibleTolerance::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("ratio_depth_tolerance", ratio_depth_tolerance, 0.05);
	std::cout << "ratio_depth_tolerance = " << ratio_depth_tolerance << std::endl;
	nhPrivate.param("min_tolerance", min_tolerance, 0.1);
	std::cout << "min_tolerance = " << min_tolerance << std::endl;
	nhPrivate.param("max_tolerance", max_tolerance, 0.5);
	std::cout << "max_tolerance = " << max_tolerance << std::endl;
	nhPrivate.param("min_cluster_size", min_cluster_size, 100);
	std::cout << "min_cluster_size = " << min_cluster_size << std::endl;
}

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

	/*ROS点群→PCL点群*/
	pcl::fromROSMsg(*msg, *cloud);
	std::cout << "==========" << std::endl;
	std::cout << "cloud->points.size() = " << cloud->points.size() << std::endl;

	max_cluster_size = cloud->points.size();
	clusters.clear();

	Clustering();
	Visualization();
}

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

	/*search config*/
	/*kd-treeクラスを宣言*/
	pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
	/*探索する点群をinput*/
	kdtree.setInputCloud(cloud);
	max_cluster_size = cloud->points.size();
	/*objects*/
	std::vector<pcl::PointIndices> cluster_indices;
	std::vector<bool> processed(cloud->points.size(), false);
	std::vector<int> nn_indices;
	std::vector<float> nn_distances;
	/*clustering*/
	for(size_t i=0;i<cloud->points.size();++i){
		if(processed[i])	continue;	//既に分類されているかチェック
		/*set seed（シード点を設定）*/
		std::vector<int> seed_queue;
		int sq_idx = 0;
		seed_queue.push_back(i);
		processed[i] = true;
		/*clustering*/
		while(sq_idx < seed_queue.size()){	//探索しきるまでループ
			/*search*/
			double tolerance = ComputeTolerance(cloud->points[seed_queue[sq_idx]]);
			int ret = kdtree.radiusSearch(cloud->points[seed_queue[sq_idx]], tolerance, nn_indices, nn_distances);
			if(ret == -1){
				PCL_ERROR("[pcl::extractEuclideanClusters] Received error code -1 from radiusSearch\n");
				exit(0);
			}
			/*check*/
			for(size_t j=0;j<nn_indices.size();++j){
				/*//既に分類されているかチェック*/
				if(nn_indices[j]==-1 || processed[nn_indices[j]])	continue;
				/*カスタム条件でチェック*/
if(CustomCondition(cloud->points[seed_queue[sq_idx]], cloud->points[nn_indices[j]], nn_distances[j])){
					seed_queue.push_back(nn_indices[j]);
					processed[nn_indices[j]] = true;
				}
			}
			sq_idx++;
		}
		/*judge（クラスタのメンバ数が条件を満たしているか）*/
		if(seed_queue.size()>=min_cluster_size && seed_queue.size()<=max_cluster_size){
			pcl::PointIndices tmp_indices;
			tmp_indices.indices = seed_queue;
			cluster_indices.push_back(tmp_indices);
		}
	}
	std::cout << "cluster_indices.size() = " << cluster_indices.size() << std::endl;
	/*extraction（クラスタごとに点群を分割）*/
	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;
}

double EuclideanClusteringFlexibleTolerance::ComputeTolerance(const pcl::PointXYZ& point)
{
	/*センサからの距離（depth）*/
	double depth = sqrt(
		point.x * point.x
		+ point.y * point.y
		+ point.z * point.z
	);

	double tolerance = ratio_depth_tolerance*depth;	//距離に比例
	if(tolerance < min_tolerance)	tolerance = min_tolerance;
	if(tolerance > max_tolerance)	tolerance = max_tolerance;

	return tolerance;
}

bool EuclideanClusteringFlexibleTolerance::CustomCondition(const pcl::PointXYZ& seed_point, const pcl::PointXYZ& candidate_point, float squared_distance){
	return true;
}

void EuclideanClusteringFlexibleTolerance::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_flexible_tolerance");
	
	EuclideanClusteringFlexibleTolerance euclidean_clustering_flexible_tolerance;

	ros::spin();
}

実装の解説

関数　void Clustering(void)
実際にクラスタリングを行っている関数で、今回のメインパート。
関数　double ComputeTolerance(const pcl::PointXYZ& point)
引数の注目点に応じて距離の閾値（ClusterTolerance）を計算する。この記事では、注目点とセンサーの距離に比例するように実装した（最大値、最小値の制限あり）。
関数　bool CustomCondition(const pcl::PointXYZ& seed_point, const pcl::PointXYZ& candidate_point, float squared_distance)
任意でカスタム条件を追加できるように実装した。x, y, z以外の情報（インテンシティや法線など）をもとに判定を行いたい場合に便利。この記事では、特に設定していない（return true）。
関数　void Visualization(void)
いい感じに色分けするために複雑に書かれているが、今回の本題ではないので、興味がなければ流し読みでOK。
パラメータ　cluster_tolerance
点と点の距離がこの閾値（cluster_tolerance）より小さい場合、その点と点は同じクラスタに分類される。逆に、この閾値より遠い点は別のクラスタに分類される。
パラメータ　ratio_depth_tolerance
センサーから注目点までの距離と閾値（ClusterTolerance）の比例定数
パラメータ　min_tolerance
閾値（ClusterTolerance）の下限
パラメータ　max_tolerance
閾値（ClusterTolerance）の上限
パラメータ　min_cluster_size
メンバー（点）の数がこの閾値（min_cluster_size）より多いクラスタのみ出力される。逆に、この閾値より点が少ないクラスタは無視される。

距離に応じて変える系の記事

▶︎【PCL, ROS】NormalEstimationで距離に応じてRadiusSearchを変えたい

さいごに

参考になれば幸いです。

以上です。

Ad.

タグ：: #PCL; #ROS

LiLaBoC

【PCL，ROS】クラスタリングのtoleranceを距離に応じて変えたい