今天尝试跑一下欧式聚类,看看在路侧场景下的聚类效果。
【代码】
#include <stdlib.h>
#include <boost/bind.hpp>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <functional>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/console/time.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/visualization/pcl_visualizer.h>
void pc_callback(sensor_msgs::PointCloud2::ConstPtr msg_pc_ptr,
const ros::Publisher& pc_pub,
const float& cluster_tolerance,
const float& min_cluster_size) {
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::fromROSMsg(*msg_pc_ptr,*cloud);
pcl::console::TicToc tt;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filter(new pcl::PointCloud<pcl::PointXYZRGB>);
{
tt.tic(); //before voxel grid
// Create the filtering object: downsample the dataset using a leaf size of 1cm
pcl::VoxelGrid<pcl::PointXYZRGB> vg;
vg.setInputCloud (cloud);
float leaf=0.1f;
vg.setLeafSize(leaf,leaf,leaf);
vg.setDownsampleAllData(true); //work on all points?
vg.filter(*cloud_filter);
std::cerr << "voxel grid done,cost " << tt.toc() << " ms," \
<< cloud->points.size() << " -> " << cloud_filter->points.size() << " points" << std::endl;
}
// Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
tree->setInputCloud(cloud_filter);
tt.tic(); //before voxel grid
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
ec.setClusterTolerance(cluster_tolerance); //设置近邻搜索的搜索半径,e.g. 0.5
ec.setMinClusterSize(min_cluster_size); //设置一个聚类需要的最少点数
ec.setMaxClusterSize(cloud_filter->size()/5.); //设置一个聚类需要的最大点数目
ec.setSearchMethod(tree); //设置点云的搜索机制
ec.setInputCloud(cloud_filter); //设置原始点云
ec.extract(cluster_indices); //从点云中提取聚类
std::cerr << "cluster done,cost " << tt.toc() << " ms," << cluster_indices.size() << " clusters" << std::endl;
for (const auto& c : cluster_indices) {
int r = 255. * std::rand() / RAND_MAX;
int g = 255. * std::rand() / RAND_MAX;
int b = 255. * std::rand() / RAND_MAX;
for (const auto& idx : c.indices) {
cloud_filter->at(idx).r = r;
cloud_filter->at(idx).g = g;
cloud_filter->at(idx).b = b;
}
}
sensor_msgs::PointCloud2 msg_cloud_filter_cluster;
pcl::toROSMsg(*cloud_filter, msg_cloud_filter_cluster);
msg_cloud_filter_cluster.header.frame_id = "livox_frame";
pc_pub.publish(msg_cloud_filter_cluster);
};
int main (int argc, char** argv) {
ros::init(argc, argv, "pcl_euclidean_cluster");
float cluster_tolerance = std::atof(argv[1]);
float min_cluster_size = std::atof(argv[2]);
ros::NodeHandle nd;
ros::Publisher pc_pub = nd.advertise<sensor_msgs::PointCloud2>("euclidean_cluster",1);
ros::Subscriber pc_sub = nd.subscribe<sensor_msgs::PointCloud2>("/livox/lidar",1,
boost::bind(&pc_callback,_1,pc_pub,cluster_tolerance,min_cluster_size));
ros::spin();
return 0;
}
【实验场景1-说明】
雷达:Livox Horizon
安装方式:路侧,非水平安装,向地面倾斜约30度。
【实验场景1-测试】
rosrun n_lidar_learn n_lidar_learn_node 0.5 10
voxel grid done,cost 1 ms,24000 -> 13831 points
cluster done,cost 31 ms,63 clusters
voxel grid done,cost 1 ms,24000 -> 14037 points
cluster done,cost 32 ms,73 clusters
voxel grid done,cost 2 ms,24000 -> 13901 points
cluster done,cost 41 ms,73 clusters
voxel grid done,cost 2 ms,24000 -> 13800 points
cluster done,cost 39 ms,73 clusters
voxel grid done,cost 2 ms,24000 -> 13815 points
cluster done,cost 41 ms,69 clusters
voxel grid done,cost 2 ms,24000 -> 13915 points
cluster done,cost 33 ms,78 clusters
.....
我所用的livox horizon一帧是24000个点,测试的时候我先做了一个体素滤波,过滤调大概40%的点,主要是速度上的考虑。欧式聚类中需要设置的两个重要参数cluster_tolerance和min_cluster_size分别为0.5和10。
这里暂时没有过滤地面点,我就想着一步步来观察效果。我的场景中样本其实还是比较有代表性的,有人,车,挡墙,树木,标识牌等。因为没有分离地面点,人和车基本上就没有单独聚类出来。