PCL之点云的分割
参考博客:https://www.yuque.com/huangzhongqing/pcl/kg7wvi#peMqz
https://blog.csdn.net/lizhengze1117/article/details/89034332
背景
点云分割是根据空间,几何和纹理等特征对点云进行划分,使得同一划分内的点云拥有相似的特征,点云的有效分割往往是许多应用的前提,例如逆向工作,CAD领域对零件的不同扫描表面进行分割,然后才能更好的进行空洞修复曲面重建,特征描述和提取,进而进行基于3D内容的检索,组合重用等。
平面分割
- 工作原理:采用RSNSAC算法,Ransac为了找到点云的平面,不停的改变平面模型(ax+by+cz+d=0)的参数:a, b, c和d。经过多次调参后,找出哪一组的参数能使得这个模型一定程度上拟合最多的点。这个程度就是由distance threshold这个参数来设置。
于是当distance threshold调大,离平面更远的点也被算进平面来。distance threshold可以等同于平面厚度阈值。
- 文档代码:
#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>);
// 填充点云
cloud->width = 15;
cloud->height = 1;
cloud->points.resize(cloud->width * cloud->height);
// 生成数据,采用随机数填充点云的x,y坐标,都处于z为1的平面上
for (size_t i = 0; i < cloud->points.size(); ++i)
{
cloud->points[i].x = 1024 * rand() / (RAND_MAX + 1.0f);
cloud->points[i].y = 1024 * rand() / (RAND_MAX + 1.0f);
cloud->points[i].z = 1.0;
}
// 设置几个局外点,即重新设置几个点的z值,使其偏离z为1的平面
cloud->points[0].z = 2.0;
cloud->points[3].z = -2.0;
cloud->points[6].z = 4.0;
std::cerr << "Point cloud data: " << cloud->points.size() << " points" << std::endl; //打印
for (size_t i = 0; i < cloud->points.size(); ++i)
std::cerr << " " << cloud->points[i].x << " "
<< cloud->points[i].y << " "
<< cloud->points[i].z << std::endl;
//创建分割时所需要的模型系数对象,coefficients及存储内点的点索引集合对象inliers
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
// 创建分割对象
pcl::SACSegmentation<pcl::PointXYZ> seg;
// 可选择配置,设置模型系数需要优化
seg.setOptimizeCoefficients(true);
// 必要的配置,设置分割的模型类型,所用的随机参数估计方法,距离阀值,输入点云
seg.setModelType(pcl::SACMODEL_PLANE); //设置模型类型
seg.setMethodType(pcl::SAC_RANSAC); //设置随机采样一致性方法类型
seg.setDistanceThreshold(0.01); //设定距离阀值,距离阀值决定了点被认为是局内点是必须满足的条件
//表示点到估计模型的距离最大值,
seg.setInputCloud(cloud);
//引发分割实现,存储分割结果到点几何inliers及存储平面模型的系数coefficients
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);
}
圆柱体分割
- 工作原理:采用随机采样一致性估计从带有噪声的点云中提取一个圆柱体模型。
- 文档代码:
#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>
#include <pcl/visualization/pcl_visualizer.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("../table_scene_mug_stereo_textured.pcd", *cloud);
std::cerr << "PointCloud has: " << cloud->points.size() << " data points." << std::endl;
// 直通滤波,将Z轴不在(0,1.5)范围的点过滤掉,将剩余的点存储到cloud_filtered对象中
pass.setInputCloud(cloud);
pass.setFilterFieldName("z");
pass.setFilterLimits(0, 1.5);
pass.filter(*cloud_filtered);
std::cerr << "PointCloud after filtering has: " << cloud_filtered->points.size() << " data points." << std::endl;
// 过滤后的点云进行法线估计,为后续进行基于法线的分割准备数据
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);
seg.setModelType(pcl::SACMODEL_NORMAL_PLANE);
seg.setNormalDistanceWeight(0.1);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setMaxIterations(100);
seg.setDistanceThreshold(0.03);
seg.setInputCloud(cloud_filtered);
seg.setInputNormals(cloud_normals);
//获取平面模型的系数和处在平面的内点
seg.segment(*inliers_plane, *coefficients_plane);
std::cerr << "Plane coefficients: " << *coefficients_plane << std::endl;
// 从点云中抽取分割的处在平面上的点集
extract.setInputCloud(cloud_filtered);
extract.setIndices(inliers_plane);
extract.setNegative(false);
// 存储分割得到的平面上的点到点云文件
pcl::PointCloud<PointT>::Ptr cloud_plane(new pcl::PointCloud<PointT>());
extract.filter(*cloud_plane);
std::cerr << "PointCloud representing the planar component: " << cloud_plane->points.size() << " data points." << std::endl;
writer.write("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); //参数估计方法
seg.setNormalDistanceWeight(0.1); //设置表面法线权重系数
seg.setMaxIterations(10000); //设置迭代的最大次数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->points.size() << " data points." << std::endl;
writer.write("table_scene_mug_stereo_textured_cylinder.pcd", *cloud_cylinder, false); // 分割得到的平面
}
// 可视化
pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("three 三窗口 "));
int v1(0); //设置左右窗口
int v2(1);
int v3(2);
viewer->createViewPort(0.0, 0.0, 0.5,1.0, v1); //(Xmin,Ymin,Xmax,Ymax)设置窗口坐标
viewer->createViewPort(0.5, 0.0, 1.0, 0.5, v2);
viewer->createViewPort(0.5, 0.5, 1.0, 1.0, v3);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_out_red(cloud, 255, 0, 0); // 显示红色点云
viewer->addPointCloud(cloud, cloud_out_red, "cloud_out1", v1);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_out_green(cloud, 0, 255, 0); // 显示绿色点云
viewer->addPointCloud(cloud_plane, cloud_out_green, "cloud_out2", v2); // 平面
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_out_blue(cloud, 0, 0, 255); // 显示蓝色点云
viewer->addPointCloud(cloud_cylinder, cloud_out_blue, "cloud_out3", v3); // 圆柱
// 1. 阻塞式
viewer->spin();
// 2. 非阻塞式
// while (!viewer->wasStopped())
// {
// viewer->spinOnce();
// }
return (0);
}
欧几里得簇提取
- 工作原理:检查两点之间的距离。如果小于阈值,则两者被认为属于同一簇。它的工作原理就像一个洪水填充算法:在点云中的一个点被“标记”则表示为选择在一个的集群中。然后,它像病毒一样扩散到其他足够近的点,从这些点到更多点,直到没有新的添加为止。这样,就是一个初始化的新的群集,并且该过程将以剩余的无标记点再次进行。
- 文档代码:
#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 ("../table_scene_lms400.pcd", *cloud); // 点云文件
std::cout << "PointCloud before filtering has: " << cloud->points.size () << " data points." << std::endl; //*
// Create the filtering object: downsample the dataset using a leaf size of 1cm
pcl::VoxelGrid<pcl::PointXYZ> vg; // VoxelGrid类在输入点云数据上创建3D体素网格(将体素网格视为一组空间中的微小3D框
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
vg.setInputCloud (cloud); //输入
vg.setLeafSize (0.01f, 0.01f, 0.01f); // setLeafSize (float lx, float ly, float lz)
vg.filter (*cloud_filtered); //输出
std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl; //*滤波后
//创建平面模型分割的对象并设置参数
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->points.size ();
while (cloud_filtered->points.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;
}
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->points.size () << " data points." << std::endl;
// // 移去平面局内点,提取剩余点云
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);
ec.extract (cluster_indices); //从点云中提取聚类,并将点云索引保存在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->points.push_back (cloud_filtered->points[*pit]); //*
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.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);
}
区域增长细分
- 工作原理:从曲率小的面播种,从种子的位置出发,开始往四周搜索点,然后比对点于点之间的曲率和法线方向,如果差距小于阈值就视为同一个cluster。如果一个cluster无法再蔓延,在剩下的点云里再找曲率小的面播种,然后继续重复直到遍历完毕。
- 文档代码:
基于颜色的区域增长分割
- 工作原理:实现的方法类似于区域增长细分。主要有两点不同,第一是它使用的颜色而不是发现,第二是它使用合并算法进行过分割和欠分割控制。
在分割之后,尝试合并具有相近颜色的簇,将平均颜色之间具有小差异的两个相邻聚类合并在一起,然后进行第二次合并步骤,在此步骤中,每个簇通过其包含的点数进行验证,如果数字小于阈值,则当前集群与最近的相邻集群合并。
- 文档代码:
基于最小割的分割算法
参考博客:https://blog.csdn.net/zhan_zhan1/article/details/104754016
- 两种边两种权重,最后使用最小割算法进行处理。
条件欧几里得分割
- 工作原理:分割的工作方式与标准的欧几里得分割方法基本一样,条件分割除了进行距离检查,点还要满足一个特殊的、可以自定义的要求的限制。它归结为如下:对于每一对点(第一个点,作为种子点,第二个点,作为候选点,是一个临近点的选择与计算比较等操作)将会有自定义函数。此函数具有一定的特性:这两个点具有副本,以便我们可以执行我们自己的选择计算的平方距离
函数并返回布尔值。如果值为true,则可以将候选添加到群集。如果假,它不会被添加,即使它通过距离检查。
- 文档代码:
#include <pcl/io/pcd_io.h>
#include <pcl/segmentation/conditional_euclidean_clustering.h>
#include <iostream>
// 如果这个函数返回的是真,这这个候选点将会被加入聚类中
bool
customCondition(const pcl::PointXYZ& seedPoint, const pcl::PointXYZ& candidatePoint, float squaredDistance)
{
// Do whatever you want here.做你想做的条件的筛选
if (candidatePoint.y < seedPoint.y) //如果候选点的Y的值小于种子点的Y值(就是之前被选择为聚类的点),则不满足条件,返回假
return false;
return true;
}
int main(int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ>(argv[1], *cloud) != 0)
{
return -1;
}
pcl::ConditionalEuclideanClustering<pcl::PointXYZ> clustering;
clustering.setClusterTolerance(0.02);
clustering.setMinClusterSize(100);
clustering.setMaxClusterSize(25000);
clustering.setInputCloud(cloud);
//设置每次检测一对点云时的函数
clustering.setConditionFunction(&customCondition);
std::vector<pcl::PointIndices> clusters;
clustering.segment(clusters);
int currentClusterNum = 1;
for (std::vector<pcl::PointIndices>::const_iterator i = clusters.begin(); i != clusters.end(); ++i)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cluster(new pcl::PointCloud<pcl::PointXYZ>);
for (std::vector<int>::const_iterator point = i->indices.begin(); point != i->indices.end(); point++)
cluster->points.push_back(cloud->points[*point]);
cluster->width = cluster->points.size();
cluster->height = 1;
cluster->is_dense = true;
if (cluster->points.size() <= 0)
break;
std::cout << "Cluster " << currentClusterNum << " has " << cluster->points.size() << " points." << std::endl;
std::string fileName = "cluster" + boost::to_string(currentClusterNum) + ".pcd";
pcl::io::savePCDFileASCII(fileName, *cluster);
currentClusterNum++;
}
}
基于法线差异来分割点云
如何求解法线:https://cloud.tencent.com/developer/article/1756228
参考博客:https://blog.csdn.net/zhan_zhan1/article/details/104755582?utm_medium=distribute.pc_relevant.none-task-blog-baidujs-3
- DoN算法:DoN特征源于观察到基于所给半径估计的表面法向量可以反映曲面的内在几何特征,因此这种分割算法是基于法线估计的,需要计算点云中某一点的法线估计。而通常在计算法线估计的时候都会用到邻域信息,很明显邻域大小的选取会影响法线估计的结果。而在DoN算法中,邻域选择的大小就被称为support radius(支持半径)。对点云中某一点选取不同的支持半径,即可以得到不同的法线估计,而法线之间的差异,就是是所说的法线差异。
可以使用单位法向量的差来定义DoN特征:
- 算法流程:
(1)对于输入点云数据中的每一点,利用较大的支撑半径rl计算法向量;
(2)对于输入点云数据中的每一点,利用较大的支撑半径rs计算法向量;
(3)对于输入点云数据中的每一点,单位化每一点的法向量差异;
(4)过滤所得的向量域(DoN特征向量),分割出目标尺寸对应的点云;
- 实例代码:
#include <string>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/search/organized.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/filters/conditional_removal.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/segmentation/impl/extract_clusters.hpp>
#include <pcl/features/don.h>
// for visualization
#include <pcl/visualization/pcl_visualizer.h>
#include <vtkAutoInit.h>
VTK_MODULE_INIT(vtkRenderingOpenGL);
using namespace pcl;
using namespace std;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr getColoredCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr input_, std::vector <pcl::PointIndices> clusters_, float r, float g, float b)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored_cloud;
if (!clusters_.empty())
{
colored_cloud = (new pcl::PointCloud<pcl::PointXYZRGB>)->makeShared();
srand(static_cast<unsigned int> (time(0)));
std::vector<unsigned char> colors;
for (size_t i_segment = 0; i_segment < clusters_.size(); i_segment++)
{
colors.push_back(static_cast<unsigned char> (rand() % 256));
colors.push_back(static_cast<unsigned char> (rand() % 256));
colors.push_back(static_cast<unsigned char> (rand() % 256));
}
colored_cloud->width = input_->width;
colored_cloud->height = input_->height;
colored_cloud->is_dense = input_->is_dense;
for (size_t i_point = 0; i_point < input_->points.size(); i_point++)
{
pcl::PointXYZRGB point;
point.x = *(input_->points[i_point].data);
point.y = *(input_->points[i_point].data + 1);
point.z = *(input_->points[i_point].data + 2);
point.r = r;
point.g = g;
point.b = b;
colored_cloud->points.push_back(point);
}
std::vector< pcl::PointIndices >::iterator i_segment;
int next_color = 0;
for (i_segment = clusters_.begin(); i_segment != clusters_.end(); i_segment++)
{
std::vector<int>::iterator i_point;
for (i_point = i_segment->indices.begin(); i_point != i_segment->indices.end(); i_point++)
{
int index;
index = *i_point;
colored_cloud->points[index].r = colors[3 * next_color];
colored_cloud->points[index].g = colors[3 * next_color + 1];
colored_cloud->points[index].b = colors[3 * next_color + 2];
}
next_color++;
}
}
return (colored_cloud);
}
int main(int argc, char *argv[])
{
int VISUAL = 1, SAVE = 0;//0 indicate shows nothing, 1 indicate shows very step output 2 only shows the final results
///The smallest scale to use in the DoN filter.
double scale1 = 5, mean_radius;
///The largest scale to use in the DoN filter.
double scale2 = 10;
///The minimum DoN magnitude to threshold by
double threshold = 0.3;
///segment scene into clusters with given distance tolerance using euclidean clustering
double segradius = 10;
pcl::PointCloud<PointXYZRGB>::Ptr cloud(new pcl::PointCloud<PointXYZRGB>);
pcl::io::loadPCDFile("region_growing_tutorial_.pcd", *cloud);
// Create a search tree, use KDTreee for non-organized data.
pcl::search::Search<PointXYZRGB>::Ptr tree;
if (cloud->isOrganized())
{
tree.reset(new pcl::search::OrganizedNeighbor<PointXYZRGB>());
}
else
{
tree.reset(new pcl::search::KdTree<PointXYZRGB>(false));
}
// Set the input pointcloud for the search tree
tree->setInputCloud(cloud);
//caculate the mean radius of cloud and mutilply with corresponding input
{
int size_cloud = cloud->size();
int step = size_cloud / 10;
double total_distance = 0;
int i, j = 1;
for (i = 0; i < size_cloud; i += step, j++)
{
std::vector<int> pointIdxNKNSearch(2);
std::vector<float> pointNKNSquaredDistance(2);
tree->nearestKSearch(cloud->points[i], 2, pointIdxNKNSearch, pointNKNSquaredDistance);
total_distance += pointNKNSquaredDistance[1] + pointNKNSquaredDistance[0];
}
mean_radius = sqrt((total_distance / j));
cout << "mean radius of cloud is: " << mean_radius << endl;
scale1 *= mean_radius;//5*mean_radius
scale2 *= mean_radius;//10*mean_radius
segradius *= mean_radius;
}
if (scale1 >= scale2)
{
cerr << "Error: Large scale must be > small scale!" << endl;
exit(EXIT_FAILURE);
}
// Compute normals using both small and large scales at each point
pcl::NormalEstimationOMP<PointXYZRGB, PointNormal> ne;
ne.setInputCloud(cloud);
ne.setSearchMethod(tree);
/**
* NOTE: setting viewpoint is very important, so that we can ensure
* normals are all pointed in the same direction!
*/
ne.setViewPoint(std::numeric_limits<float>::max(), std::numeric_limits<float>::max(), std::numeric_limits<float>::max());
// calculate normals with the small scale
cout << "Calculating normals for scale1..." << scale1 << endl;
pcl::PointCloud<PointNormal>::Ptr normals_small_scale(new pcl::PointCloud<PointNormal>);
ne.setNumberOfThreads(4);
ne.setRadiusSearch(scale1);
ne.compute(*normals_small_scale);
// calculate normals with the large scale
cout << "Calculating normals for scale2..." << scale2 << endl;
pcl::PointCloud<PointNormal>::Ptr normals_large_scale(new pcl::PointCloud<PointNormal>);
ne.setNumberOfThreads(4);
ne.setRadiusSearch(scale2);
ne.compute(*normals_large_scale);
//visualize the normals
if (VISUAL = 1)
{
cout << "click q key to quit the visualizer and continue!!" << endl;
boost::shared_ptr<pcl::visualization::PCLVisualizer> MView(new pcl::visualization::PCLVisualizer("Showing normals with different scale"));
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> green(cloud, 0, 255, 0);
int v1(0), v2(0);
MView->createViewPort(0.0, 0.0, 0.5, 1.0, v1);
MView->createViewPort(0.5, 0.0, 1.0, 1.0, v2);
MView->setBackgroundColor(1, 1, 1);
MView->addPointCloud(cloud, green, "small_scale", v1);
MView->addPointCloud(cloud, green, "large_scale", v2);
MView->addPointCloudNormals<pcl::PointXYZRGB, pcl::PointNormal>(cloud, normals_small_scale, 100, mean_radius * 10, "small_scale_normal");
MView->addPointCloudNormals<pcl::PointXYZRGB, pcl::PointNormal>(cloud, normals_large_scale, 100, mean_radius * 10, "large_scale_normal");
MView->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "small_scale", v1);
MView->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.5, "small_scale", v1);
MView->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "large_scale", v1);
MView->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.5, "large_scale", v1);
MView->spin();
}
// Create output cloud for DoN results
PointCloud<PointNormal>::Ptr doncloud(new pcl::PointCloud<PointNormal>);
copyPointCloud<PointXYZRGB, PointNormal>(*cloud, *doncloud);
cout << "Calculating DoN... " << endl;
// Create DoN operator
pcl::DifferenceOfNormalsEstimation<PointXYZRGB, PointNormal, PointNormal> don;
don.setInputCloud(cloud);
don.setNormalScaleLarge(normals_large_scale);
don.setNormalScaleSmall(normals_small_scale);
if (!don.initCompute())
{
std::cerr << "Error: Could not intialize DoN feature operator" << std::endl;
exit(EXIT_FAILURE);
}
// Compute DoN
don.computeFeature(*doncloud);//对输入点集,计算每一个点的DON特征向量,并输出
//输出一些不同的曲率
{
cout << "You may have some sense about the input threshold(curvature) next time for your data" << endl;
int size_cloud = doncloud->size();
cout << size_cloud << endl;
int step = size_cloud / 10;
for (int i = 0; i < size_cloud; i += step)cout << " " << doncloud->points[i].curvature << " " << endl;
}
//show the differences of curvature with both large and small scale
if (VISUAL = 1)
{
cout << "click q key to quit the visualizer and continue!!" << endl;
boost::shared_ptr<pcl::visualization::PCLVisualizer> MView(new pcl::visualization::PCLVisualizer("Showing the difference of curvature of two scale"));
pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointNormal> handler_k(doncloud, "curvature");//PointCloudColorHandlerGenericField方法是将点云按深度值(“x”、“y”、"z"均可)的差异着以不同的颜色。
MView->setBackgroundColor(1, 1, 1);
MView->addPointCloud(doncloud, handler_k);
MView->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3);
MView->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.5);
MView->spin();
}
// Save DoN features
pcl::PCDWriter writer;
if (SAVE == 1) writer.write<pcl::PointNormal>("don.pcd", *doncloud, false);
// 按大小滤波
cout << "Filtering out DoN mag <= " << threshold << "..." << endl;
// 创建条件滤波函数
pcl::ConditionOr<PointNormal>::Ptr range_cond(
new pcl::ConditionOr<PointNormal>()
);
range_cond->addComparison(pcl::FieldComparison<PointNormal>::ConstPtr(
new pcl::FieldComparison<PointNormal>("curvature", pcl::ComparisonOps::GT, threshold))
);//添加比较条件
// Build the filter
pcl::ConditionalRemoval<PointNormal> condrem;
condrem.setCondition(range_cond);
condrem.setInputCloud(doncloud);
pcl::PointCloud<PointNormal>::Ptr doncloud_filtered(new pcl::PointCloud<PointNormal>);
// Apply filter
condrem.filter(*doncloud_filtered);
doncloud = doncloud_filtered;
// Save filtered output
std::cout << "Filtered Pointcloud: " << doncloud->points.size() << " data points." << std::endl;
if (SAVE == 1)writer.write<pcl::PointNormal>("don_filtered.pcd", *doncloud, false);
//show the results of keeping relative small curvature points
if (VISUAL == 1)
{
cout << "click q key to quit the visualizer and continue!!" << endl;
boost::shared_ptr<pcl::visualization::PCLVisualizer> MView(new pcl::visualization::PCLVisualizer("Showing the results of keeping relative small curvature points"));
pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointNormal> handler_k(doncloud, "curvature");
MView->setBackgroundColor(1, 1, 1);
MView->addPointCloud(doncloud, handler_k);
MView->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3);
MView->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.5);
MView->spin();
}
// Filter by magnitude
cout << "Clustering using EuclideanClusterExtraction with tolerance <= " << segradius << "..." << endl;
pcl::search::KdTree<PointNormal>::Ptr segtree(new pcl::search::KdTree<PointNormal>);
segtree->setInputCloud(doncloud);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<PointNormal> ec;
ec.setClusterTolerance(segradius);
ec.setMinClusterSize(50);
ec.setMaxClusterSize(100000);
ec.setSearchMethod(segtree);
ec.setInputCloud(doncloud);
ec.extract(cluster_indices);
if (VISUAL == 1)
{//visualize the clustering results
pcl::PointCloud <pcl::PointXYZ>::Ptr tmp_xyz(new pcl::PointCloud<pcl::PointXYZ>);
copyPointCloud<pcl::PointNormal, pcl::PointXYZ>(*doncloud, *tmp_xyz);
pcl::PointCloud <pcl::PointXYZRGB>::Ptr colored_cloud = getColoredCloud(tmp_xyz, cluster_indices, 0, 255, 0);
cout << "click q key to quit the visualizer and continue!!" << endl;
boost::shared_ptr<pcl::visualization::PCLVisualizer> MView(new pcl::visualization::PCLVisualizer("visualize the clustering results"));
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgbps(colored_cloud);
MView->setBackgroundColor(1, 1, 1);
MView->addPointCloud(colored_cloud, rgbps);
MView->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3);
MView->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.5);
MView->spin();
}
if (SAVE == 1)
{
int j = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it, j++)
{
pcl::PointCloud<PointNormal>::Ptr cloud_cluster_don(new pcl::PointCloud<PointNormal>);
for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit)
{
cloud_cluster_don->points.push_back(doncloud->points[*pit]);
}
cloud_cluster_don->width = int(cloud_cluster_don->points.size());
cloud_cluster_don->height = 1;
cloud_cluster_don->is_dense = true;
//Save cluster
cout << "PointCloud representing the Cluster: " << cloud_cluster_don->points.size() << " data points." << std::endl;
stringstream ss;
ss << "don_cluster_" << j << ".pcd";
writer.write<pcl::PointNormal>(ss.str(), *cloud_cluster_don, false);
}
}
}
点云聚集成超级体素-理论底漆
超体素:https://blog.csdn.net/studyeboy/article/details/93981017
CIELAB颜色空间:https://blog.csdn.net/gdymind/article/details/82357139
参考博客:https://cloud.tencent.com/developer/article/1555179
点云地面点滤波算法
参考博客:https://blog.csdn.net/wxc_1998/article/details/107010826