1. 平面分割
下列中,先随机创建了z=1.0的随机点,然后改变其中3个点的z值。最后,使用SACMODEL_PLANE平面模型对它进行拟合。
#include <iostream>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
int main(int argc, char **argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
// Fill in the cloud data,15个点
cloud->width = 15;
cloud->height = 1;
cloud->points.resize(cloud->width * cloud->height);
// Generate the data
for (auto &point : *cloud)//同一个z
{
point.x = 1024 * rand() / (RAND_MAX + 1.0f);
point.y = 1024 * rand() / (RAND_MAX + 1.0f);
point.z = 1.0;
}
// Set a few outliers,设置离群点,设置z就可以
(*cloud)[0].z = 2.0;
(*cloud)[3].z = -2.0;
(*cloud)[6].z = 4.0;
std::cerr << "Point cloud data: " << cloud->size() << " points" << std::endl;
for (const auto &point : *cloud)
std::cerr << " " << point.x << " "
<< point.y << " "
<< point.z << std::endl;
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZ> seg;
// Optional
seg.setOptimizeCoefficients(true);
// Mandatory
seg.setModelType(pcl::SACMODEL_PLANE);//平面模型
seg.setMethodType(pcl::SAC_RANSAC);//设置随机采样一致性方法类型,平面类型
seg.setDistanceThreshold(0.01);表示点到估计模型的距离最大值
seg.setInputCloud(cloud);
seg.segment(*inliers, *coefficients);
if (inliers->indices.size() == 0)
{
PCL_ERROR("Could not estimate a planar model for the given dataset.");
return (-1);
}
//平面模型类型,系数
std::cerr << "Model coefficients: " << coefficients->values[0] << " "
<< coefficients->values[1] << " "
<< coefficients->values[2] << " "
<< coefficients->values[3] << std::endl;
std::cerr << "Model inliers: " << inliers->indices.size() << std::endl;
for (size_t i = 0; i < inliers->indices.size (); ++i)
std::cerr << inliers->indices[i] << " " << cloud->points[inliers->indices[i]].x << " "
<< cloud->points[inliers->indices[i]].y << " "
<< cloud->points[inliers->indices[i]].z << std::endl;
return (0);
}
输出:
Point cloud data: 15 points
0.352222 -0.151883 2
-0.106395 -0.397406 1
-0.473106 0.292602 1
-0.731898 0.667105 -2
0.441304 -0.734766 1
0.854581 -0.0361733 1
-0.4607 -0.277468 4
-0.916762 0.183749 1
0.968809 0.512055 1
-0.998983 -0.463871 1
0.691785 0.716053 1
0.525135 -0.523004 1
0.439387 0.56706 1
0.905417 -0.579787 1
0.898706 -0.504929 1
Model coefficients: 0 0 1 -1
Model inliers: 12
1 -0.106395 -0.397406 1
2 -0.473106 0.292602 1
4 0.441304 -0.734766 1
5 0.854581 -0.0361733 1
7 -0.916762 0.183749 1
8 0.968809 0.512055 1
9 -0.998983 -0.463871 1
10 0.691785 0.716053 1
11 0.525135 -0.523004 1
12 0.439387 0.56706 1
13 0.905417 -0.579787 1
14 0.898706 -0.504929 1
2. 圆柱分割
下例先使用平面分割出平面,使用的是SACMODEL_NORMAL_PLANE,模型约束平面的法向方向,针对复杂的平面,可以更准确分割出平面点云,与SACMODEL_PLANE不同。最后针对剩余的点云进行圆柱分割。
下列中保存了两份分割点云:平面点云和圆柱点云
。
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/passthrough.h>
#include <pcl/features/normal_3d.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
typedef pcl::PointXYZ PointT;
int main(int argc, char **argv)
{
// All the objects needed
pcl::PCDReader reader;//pcd读取
pcl::PassThrough<PointT> pass;//直通滤波器
pcl::NormalEstimation<PointT, pcl::Normal> ne;//法线估计对象
pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg;//分割对象
pcl::PCDWriter writer;//pcd写
pcl::ExtractIndices<PointT> extract;//点提取
pcl::ExtractIndices<pcl::Normal> extract_normals;//点提取
pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>());
// Datasets
pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
pcl::PointCloud<PointT>::Ptr cloud_filtered(new pcl::PointCloud<PointT>);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
pcl::PointCloud<PointT>::Ptr cloud_filtered2(new pcl::PointCloud<PointT>);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2(new pcl::PointCloud<pcl::Normal>);
pcl::ModelCoefficients::Ptr coefficients_plane(new pcl::ModelCoefficients), coefficients_cylinder(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers_plane(new pcl::PointIndices), inliers_cylinder(new pcl::PointIndices);
// Read in the cloud data
reader.read("../../pcd/table_scene_mug_stereo_textured.pcd", *cloud);
std::cerr << "PointCloud has: " << cloud->size() << " data points." << std::endl;
// Build a passthrough filter to remove spurious NaNs
//过滤掉z(0,1.5)的点
pass.setInputCloud(cloud);
pass.setFilterFieldName("z");
pass.setFilterLimits(0, 1.5);
pass.filter(*cloud_filtered);
std::cerr << "PointCloud after filtering has: " << cloud_filtered->size() << " data points." << std::endl;
// Estimate point normals,法线估计,要基于法线分割
ne.setSearchMethod(tree);
ne.setInputCloud(cloud_filtered);
ne.setKSearch(50);
ne.compute(*cloud_normals);
// Create the segmentation object for the planar model and set all the parameters
seg.setOptimizeCoefficients(true);
//模型约束平面的法向方向,针对复杂的平面,可以更准确分割出平面点云,与SACMODEL_PLANE不同
seg.setModelType(pcl::SACMODEL_NORMAL_PLANE);
seg.setNormalDistanceWeight(0.1);
seg.setMethodType(pcl::SAC_RANSAC);//RANSAC方法
seg.setMaxIterations(100);//最大迭代次数
seg.setDistanceThreshold(0.03);//与模型的最大距离
seg.setInputCloud(cloud_filtered);
seg.setInputNormals(cloud_normals);
// Obtain the plane inliers and coefficients
seg.segment(*inliers_plane, *coefficients_plane);
std::cerr << "Plane coefficients: " << *coefficients_plane << std::endl;
// Extract the planar inliers from the input cloud
//提取平面点云
extract.setInputCloud(cloud_filtered);
extract.setIndices(inliers_plane);
extract.setNegative(false);
// Write the planar inliers to disk
//将平面点云写入pcd文件
pcl::PointCloud<PointT>::Ptr cloud_plane(new pcl::PointCloud<PointT>());
extract.filter(*cloud_plane);
std::cerr << "PointCloud representing the planar component: " << cloud_plane->size() << " data points." << std::endl;
writer.write("../../pcd/table_scene_mug_stereo_textured_plane.pcd", *cloud_plane, false);
// Remove the planar inliers, extract the rest,提取剩余部分
extract.setNegative(true);
extract.filter(*cloud_filtered2);
extract_normals.setNegative(true);
extract_normals.setInputCloud(cloud_normals);
extract_normals.setIndices(inliers_plane);
extract_normals.filter(*cloud_normals2);
// Create the segmentation object for cylinder segmentation and set all the parameters
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_CYLINDER);//设置圆柱模型
seg.setMethodType(pcl::SAC_RANSAC);//RANSAC方法
seg.setNormalDistanceWeight(0.1);//设置表面法线权重系数
seg.setMaxIterations(10000);//最大迭代次数
seg.setDistanceThreshold(0.05);//最大阈值
seg.setRadiusLimits(0, 0.1);//设置估计出的圆柱模型的半径的范围
seg.setInputCloud(cloud_filtered2);
seg.setInputNormals(cloud_normals2);
// Obtain the cylinder inliers and coefficients
seg.segment(*inliers_cylinder, *coefficients_cylinder);
std::cerr << "Cylinder coefficients: " << *coefficients_cylinder << std::endl;
// Write the cylinder inliers to disk
extract.setInputCloud(cloud_filtered2);
extract.setIndices(inliers_cylinder);
extract.setNegative(false);
pcl::PointCloud<PointT>::Ptr cloud_cylinder(new pcl::PointCloud<PointT>());
extract.filter(*cloud_cylinder);
if (cloud_cylinder->points.empty())
std::cerr << "Can't find the cylindrical component." << std::endl;
else
{
std::cerr << "PointCloud representing the cylindrical component: " << cloud_cylinder->size() << " data points." << std::endl;
writer.write("../../pcd/table_scene_mug_stereo_textured_cylinder.pcd", *cloud_cylinder, false);
}
return (0);
}
原始点云为:
分割出来的圆柱为:
分割出来的平面为:
3. 欧式聚类分割
- 欧式聚类原理:
- 找到空间中某点 p 0 p_0 p0,有kdTree找到离它
最近的n个点
,判断这n个点到 p 0 p_0 p0的距离。将距离小于阈值r
的点 p 1 , p 2 , p 3 , p 4 . . . . p_1,p_2,p_3,p_4.... p1,p2,p3,p4....放在类Q里。- 对Q中的剩余点
重复以上步骤
,并将满足条件的点放入Q。- 当Q中再也不能有新的点加入时,则完成搜索。
下面的欧式聚类分割代码逻辑为:
- 首先对原始点云做下采样,体素大小为0.01.
- 然后采样点云做平面滤波,使用的是SAC_RANSAC平面滤波,直到总量小于原始的0.3倍之后才不做平面滤波
- 对平面滤波剩余的点云进行欧式距离分割。
#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
int main(int argc, char **argv)
{
// Read in the cloud data
pcl::PCDReader reader;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>), cloud_f(new pcl::PointCloud<pcl::PointXYZ>);
reader.read("../../pcd/table_scene_lms400.pcd", *cloud);
std::cout << "PointCloud before filtering has: " << cloud->size() << " data points." << std::endl; //*
// Create the filtering object: downsample the dataset using a leaf size of 1cm
//下采样
pcl::VoxelGrid<pcl::PointXYZ> vg;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
vg.setInputCloud(cloud);
vg.setLeafSize(0.01f, 0.01f, 0.01f);
vg.filter(*cloud_filtered);
std::cout << "PointCloud after filtering has: " << cloud_filtered->size() << " data points." << std::endl; //*
// Create the segmentation object for the planar model and set all the parameters
//平面分割
pcl::SACSegmentation<pcl::PointXYZ> seg;
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZ>());
pcl::PCDWriter writer;
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_PLANE); //分割模型
seg.setMethodType(pcl::SAC_RANSAC); //参数估计方法
seg.setMaxIterations(100); //最大迭代次数
seg.setDistanceThreshold(0.02); //设置阈值
int i = 0, nr_points = (int)cloud_filtered->size();
//先分割平面,循环分割
//删除平面之后,剩余点云不能大于总数的0.3倍,否则继续分割
while (cloud_filtered->size() > 0.3 * nr_points)
{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud(cloud_filtered);
seg.segment(*inliers, *coefficients);
if (inliers->indices.size() == 0)
{
std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
// Extract the planar inliers from the input cloud
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(cloud_filtered);
extract.setIndices(inliers);
extract.setNegative(false);
// Get the points associated with the planar surface
extract.filter(*cloud_plane);
std::cout << "PointCloud representing the planar component: " << cloud_plane->size() << " data points." << std::endl;
// Remove the planar inliers, extract the rest
//要分割好几个平面,移除平面点
extract.setNegative(true);
extract.filter(*cloud_f);
*cloud_filtered = *cloud_f;
}
// Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud(cloud_filtered);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec; //欧式距离对象
ec.setClusterTolerance(0.02); // 2cm,设置近邻搜索的搜索半径
ec.setMinClusterSize(100); //设置一个聚类需要的最少的点数目为100
ec.setMaxClusterSize(25000); //设置一个聚类需要的最大点数目为25000
ec.setSearchMethod(tree); //设置点云的搜索机制
ec.setInputCloud(cloud_filtered);
//cluster_indices为欧式分割后的聚类索引
ec.extract(cluster_indices);//从点云中提取聚类,并将点云索引保存在cluster_indices中
int j = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
{
//提取每个聚类的点云
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit)
cloud_cluster->push_back((*cloud_filtered)[*pit]); //*
cloud_cluster->width = cloud_cluster->size();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
std::cout << "PointCloud representing the Cluster: " << cloud_cluster->size() << " data points." << std::endl;
std::stringstream ss;
ss << "cloud_cluster_" << j << ".pcd";
writer.write<pcl::PointXYZ>(ss.str(), *cloud_cluster, false); //*
j++;
}
return (0);
}
输出如下,下面对数据进行简单的分析:
- 原始点为:460400个,下采样之后为:41049
- 对下采样数据进行了两次平面分割,分割输出的点云分别为:20536和20536
- 欧式聚类一共分割出5个目标,点云数分别为:4857、1386、321、291和123
PointCloud before filtering has: 460400 data points.
PointCloud after filtering has: 41049 data points.
PointCloud representing the planar component: 20536 data points.
PointCloud representing the planar component: 20536 data points.
PointCloud representing the Cluster: 4857 data points.
PointCloud representing the Cluster: 1386 data points.
PointCloud representing the Cluster: 321 data points.
PointCloud representing the Cluster: 291 data points.
PointCloud representing the Cluster: 123 data points.
原始pcd点云如下:
欧式聚类分割之后的5个目标点云如下:
-
第一个聚类目标点云
-
第二个聚类目标点云
-
第三个聚类目标点云
-
第四个聚类目标点云
-
第五个聚类目标点云