PCL(Point Cloud Libraly)を使ったユークリッドクラスタリングの実装例を公開しました。
【PCL,ROS】ユークリッドクラスタリング(EuclideanClusterExtraction)
3D-LiDARを扱う際、点群をクラスリングしたいことがよくあります。典型的な点群のクラスタリングの1つに、ユークリッドクラスタリングがあります。ユークリッドクラスタリングは、名前の通り、点と点のユークリッド距離(単純な距離)のみを考慮したクラスタリングです。
PCLを用いれば、簡単にユークリッドクラスタリングを実装することができます。この記事では、ROS対応の3D-LiDARで観測した点群にユークリッドクラスタ分析を適用するソースコードを公開します。
実装の概要
- Subscribe(入力):点群(sensor_msg::pointCloud2)
- Publish(出力):なし
- 可視化クラス:pcl::visualization::PCLVisualizer
- 処理の流れ
- 点群をサブスクライブ
sensor_msg::pointCloud2 - 点群情報の型を変換(ROS型→PCL型)
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/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.
コメント