PCL(Point Cloud Libraly)の条件付きユークリッドクラスタリング(ConditionalEuclideanClustering)を使った法線群のクラスタリングの実装例を公開します。

【PCL,ROS】法線群のクラスタリング(ConditionalEuclideanClustering)

3D-LiDARを扱う際、点群をクラスリングしたいことがよくあります。以前、最も単純なユークリッドクラスタリングの実装を公開しました。この実装では単純にユークリッド距離(x, y, z情報)のみを考慮していました。

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

一方で、点群の法線方向まで考慮したクラスタリングを行えば、平面ごとに点群をクラスタリングしたりできます。

このような法線群のクラスタリングは、PCLの条件付きユークリッドクラスタリング(ConditionalEuclideanClustering)を利用すれば、簡単に実装することができます。今回の記事では、その実装例や解説を公開します。

▼法線推定の実装も公開しています
【ROS, PCL】3D-LiDARの点群から法線群を生成(Normal Estimation)

条件付きユークリッドクラスタリングとは?

条件付きユークリッドクラスタリングでは、点S(seed point)に対して点C(candidate point)が同じクラスタか判断するときに、ユークリッド距離で判定した後に、独自に設定した条件で判定を行います。例えば、法線ベクトルの角度や受光強度(インテンシティー)の差などの条件を設定することで、より高級なクラスタリングを実現できます。

実装の概要

  • sub:点群(sensor_msg::pointCloud2)
  • pub:なし
  • 可視化:pcl::visualization::PCLVisualizer
  • クラスタリング追加条件:法線ベクトルの角度が閾値以下
  • 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/segmentation/conditional_euclidean_clustering.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/visualization/cloud_viewer.h>

class ConditionalEuclideanClustering{
	private:
		/*node handle*/
		ros::NodeHandle nh;
		ros::NodeHandle nhPrivate;
		/*subscribe*/
		ros::Subscriber sub_pc;
		/*pcl objects*/
		pcl::visualization::PCLVisualizer viewer {"Conditional Euclidian Clustering"};
		pcl::PointCloud<pcl::PointNormal>::Ptr cloud {new pcl::PointCloud<pcl::PointNormal>};
		std::vector<pcl::PointCloud<pcl::PointNormal>::Ptr> clusters;
		/*parameters*/
		double cluster_tolerance;
		int min_cluster_size;
	public:
		ConditionalEuclideanClustering();
		void CallbackPC(const sensor_msgs::PointCloud2ConstPtr &msg);
		void Clustering(void);
		static bool CustomCondition(const pcl::PointNormal& seedPoint, const pcl::PointNormal& candidatePoint, float squaredDistance);
		void Visualization(void);
};

ConditionalEuclideanClustering::ConditionalEuclideanClustering()
	:nhPrivate("~")
{
	sub_pc = nh.subscribe("/normals", 1, &ConditionalEuclideanClustering::CallbackPC, this);
	viewer.setBackgroundColor(1, 1, 1);
	viewer.addCoordinateSystem(0.8, "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 ConditionalEuclideanClustering::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;

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

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

	/*clustering*/
	/*クラスタ後のインデックスが格納されるベクトル*/
	std::vector<pcl::PointIndices> cluster_indices;
	/*今回の主役(trueで初期化しないといけないらしい)*/
	pcl::ConditionalEuclideanClustering<pcl::PointNormal> cec(true);
	/*クラスリング対象の点群をinput*/
	cec.setInputCloud(cloud);
	/*カスタム条件の関数を指定*/
	cec.setConditionFunction(&CustomCondition);
	/*距離の閾値を設定*/
	cec.setClusterTolerance(cluster_tolerance);
	/*各クラスタのメンバの最小数を設定*/
	cec.setMinClusterSize(min_cluster_size);
	/*各クラスタのメンバの最大数を設定*/
	cec.setMaxClusterSize(cloud->points.size());
	/*クラスリング実行*/
	cec.segment(cluster_indices);
	/*メンバ数が最小値以下のクラスタと最大値以上のクラスタを取得できる*/
	// cec.getRemovedClusters (small_clusters, large_clusters);

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

	/*dividing(クラスタごとに点群を分割)*/
	pcl::ExtractIndices<pcl::PointNormal> ei;
	ei.setInputCloud(cloud);
	ei.setNegative(false);
	for(size_t i=0;i<cluster_indices.size();i++){
		/*extract*/
		pcl::PointCloud<pcl::PointNormal>::Ptr tmp_clustered_points (new pcl::PointCloud<pcl::PointNormal>);
		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;
}

/*カスタム条件*/
bool ConditionalEuclideanClustering::CustomCondition(const pcl::PointNormal& seedPoint, const pcl::PointNormal& candidatePoint, float squaredDistance)
{
	Eigen::Vector3d N1(
		seedPoint.normal_x,
		seedPoint.normal_y,
		seedPoint.normal_z
	);
	Eigen::Vector3d N2(
		candidatePoint.normal_x,
		candidatePoint.normal_y,
		candidatePoint.normal_z
	);
	double angle = acos(N1.dot(N2)/N1.norm()/N2.norm());	//法線ベクトル間の角度[rad]

	const double threshold_angle = 1.0;	//閾値[deg]
	if(angle/M_PI*180.0 < threshold_angle)	return true;
	else	return false;
}

void ConditionalEuclideanClustering::Visualization(void)
{
	viewer.removeAllPointClouds();

	/*normals*/
	viewer.addPointCloudNormals<pcl::PointNormal>(cloud, 1, 0.5, "cloud");
	viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0, 0.0, 0.0, "cloud");
	viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 0.5, "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;
			}
		}
		/* std::cout << "step = " << step << std::endl; */
		/* std::cout << name << ": (r,g,b) = " << rgb[0] << ", " << rgb[1] << ", " << rgb[2] << std::endl; */
		/*input*/
		viewer.addPointCloudNormals<pcl::PointNormal>(clusters[i], 1, 0.5, name);
		viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, rgb[0], rgb[1], rgb[2], name);
		viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 1, name);
	}

	viewer.spinOnce();
}

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

	ros::spin();
}

実装の解説

  • パラメータ cluster_tolerance
    点と点の距離がこの閾値(cluster_tolerance)より小さい場合、その点と点は同じクラスタに分類される。逆に、この閾値より遠い点は別のクラスタに分類される。
  • パラメータ min_cluster_size
    メンバー(点)の数がこの閾値(min_cluster_size)より多いクラスタのみ出力される。逆に、この閾値より点が少ないクラスタは無視される。
  • 関数 void Clustering(void)
    実際にクラスタリングを行っている関数で、今回のメインパート。
  • 関数 bool CustomCondition(const pcl::PointNormal& seedPoint, const pcl::PointNormal& candidatePoint, float squaredDistance)
    • 独自の判定条件
    • 今回は法線ベクトルのなす角で判定
    • return true:同じクラスタに分類
    • return false:別のクラスタに分類
    • 引数の型は決められている(const pcl::PointNormal&, const pcl::PointNormal&, float)
    • クラスで実装するならstaticにする必要あり(たぶん)
  • 関数 void Visualization(void)
    いい感じに色分けするために複雑に書かれているが、今回の本題ではないので、興味がなければ流し読みでOK。

補足:IndicesClustersPtrとは

ConditionalEuclideanClusteringのPCLのチュートリアル(外部リンク)を読んでいるときにpcl::IndicesClustersPtrという型の正体が分からなかったので補足しておきます。

typedef std::vector<pcl::PointIndices> IndicesClusters;
typedef boost::shared_ptr<std::vector<pcl::PointIndices> > IndicesClustersPtr;
引用:conditional_euclidean_clustering.h

どうやら上記のように定義(typedef)されているようです。

結論:IndicesClustersPtr = boost::shared_ptr<std::vector<pcl::PointIndices> >

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

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

【PCL,ROS】ユークリッドクラスタリング(EuclideanClusterExtraction)
【PCL,ROS】クラスタリングのtoleranceを距離に応じて変えたい

さいごに

参考になれば幸いです。


以上です。

Ad.