1.PCL滤波算法及相关概念

1）需要进行滤波的情况及对应的处理方式：
点云数据密度不规则需要平滑处理——按具体给定的规则限制过滤
因遮挡造成离群点
大量数据——降采样
噪声数据
2）双边滤波：通过取临近点的加权平均来修正当前采样点的位置，从而达到滤波的效果。同时剔除与当前采样点差异较大的相邻采样点，达到保持原有特征的目的。

2.直通滤波器对点云处理

指定某一维度实行简单滤波，即去掉用户指定范围内部（或者外部）的点。

#include <iostream>
#include <ctime>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>
int
 main (int argc, char** argv)
{ 
	  srand(time(0));
	  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
	  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
	  //填入点云数据
	  cloud->width  = 5;
	  cloud->height = 1;
	  cloud->points.resize (cloud->width * cloud->height);
	  for (size_t i = 0; i < cloud->points.size (); ++i)
	  {
		cloud->points[i].x = rand () / (RAND_MAX + 1.0f)-0.5;
		cloud->points[i].y = rand () / (RAND_MAX + 1.0f)-0.5;
		cloud->points[i].z = rand () / (RAND_MAX + 1.0f)-0.5;
	  }
	  std::cerr << "Cloud before filtering: " << 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;
	  // 创建滤波器对象
	  pcl::PassThrough<pcl::PointXYZ> pass;
	  pass.setInputCloud (cloud);
	  pass.setFilterFieldName ("z");//滤波字段为z，按照z的阈值进行过滤
	  pass.setFilterLimits (0.0, 1.0);
	  pass.setFilterLimitsNegative (true);//true为过滤掉该范围内的点，false为保留
	  pass.filter (*cloud_filtered);

	  std::cerr << "Cloud after filtering: " << std::endl;
	  for (size_t i = 0; i < cloud_filtered->points.size (); ++i)
		std::cerr << "    " << cloud_filtered->points[i].x << " " 
							<< cloud_filtered->points[i].y << " " 
							<< cloud_filtered->points[i].z << std::endl;
	  return (0);
}

3.VoxelGrid滤波器对点云进行下采样

VoxelGrid利用体素中所有点的重心来近似标识体素中的所有点。

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
int
main (int argc, char** argv)
{
  sensor_msgs::PointCloud2::Ptr cloud (new sensor_msgs::PointCloud2 ());
  sensor_msgs::PointCloud2::Ptr cloud_filtered (new sensor_msgs::PointCloud2 ());
  // 填入点云数据
  pcl::PCDReader reader;
  // 把路径改为自己存放文件的路径
  reader.read ("C:\Users\1987wangsanguo\Desktop\PCD_Viewer\PCD_Viewer\2f_only_voxel.pcd", *cloud); // 记住要事先下载这个数据集！
  std::cerr << "PointCloud before filtering: " << cloud->width * cloud->height 
       << " data points (" << pcl::getFieldsList (*cloud) << ").";
  // 创建滤波器对象
  pcl::VoxelGrid<sensor_msgs::PointCloud2> sor;
  sor.setInputCloud (cloud);
  sor.setLeafSize (0.01f, 0.01f, 0.01f);//体素大小为1cm的立方体，体素越小越精确，但内存占用越多
  sor.filter (*cloud_filtered);//结果存储到cloud_filter
  std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height 
       << " data points (" << pcl::getFieldsList (*cloud_filtered) << ").";
  pcl::PCDWriter writer;
  writer.write ("2f.pcd", *cloud_filtered, 
         Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), false);//过滤结果写出
  return (0);
}

4.移除离群点

1)基于统计的方法StatisticalOutlierRemoval
稀疏离群点移除方法基于在输入数据中对点到临近点的距离分布的计算。对每个点, 我们计算它到它的所有临近点的平均距离。假设得到的结果是一个高斯分布，其形状由均值和标准差决定，平均距离在标准范围（由全局距离平均值和方差定义）之外的点，可被定义为离群点并可从数据集中去除掉。

StatisticalOutlierRemoval算法迭代整个点云两次过程，第一次迭代时候它首先计算每个点pi与最近k个邻域的平均距离di。这里k的值可以通过setMeanK()进行参数设置。其次，计算出所有这些点的平均距离（d1,d2,…,di）的均值与标准差，以来确定距离阈值。距离阈值的计算将等于：stddevMult∗stddev。这里面的标准差系数stddevMult 可以使用函数setStdevMulthresh()进行设置。第二次迭代的过程中，如果点的平均邻域距离高于此阈值，则被判断为离群点。否则，为内联点。

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/statistical_outlier_removal.h>
int
main (int argc, char** argv)
{
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
  // 填入点云数据
  pcl::PCDReader reader;
  // 把路径改为自己存放文件的路径
  reader.read<pcl::PointXYZ> ("table_scene_lms400.pcd", *cloud);
  std::cerr << "Cloud before filtering: " << std::endl;
  std::cerr << *cloud << std::endl;
  // 创建滤波器对象
  pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
  sor.setInputCloud (cloud);
  sor.setMeanK (50);//设置在进行统计时考虑的查询点临近点数
  sor.setStddevMulThresh (1.0);//标准差倍数，意味着如果一个点的距离超出平均距离一个标准差以上，则被标记为离群点
  sor.filter (*cloud_filtered);
  std::cerr << "Cloud after filtering: " << std::endl;
  std::cerr << *cloud_filtered << std::endl;
  pcl::PCDWriter writer;
  writer.write<pcl::PointXYZ> ("table_scene_lms400_inliers.pcd", *cloud_filtered, false);
  sor.setNegative (true);//过滤掉离群点
  sor.filter (*cloud_filtered);
  writer.write<pcl::PointXYZ> ("table_scene_lms400_outliers.pcd", *cloud_filtered, false);
  return (0);
}

2）RadiusOutlierRemoval:基于半径内相邻点的个数
在指定的每个点的一定范围内至少要有足够数量的k个相邻点，否则将被视为离群点。
3）ConditionalRemoval
用于删除点云中不符合用户指定的一个或者多个条件的数据点。
这种方法可以由用户自己设定逻辑关系，筛选出符合条件的点云，支持and，or，大于，小于等逻辑关系。参考博客

#include <iostream>
#include <pcl/point_types.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/conditional_removal.h>
int
 main (int argc, char** argv)
{
  if (argc != 2)
  {
    std::cerr << "please specify command line arg '-r' or '-c'" << std::endl;
    exit(0);
  }
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
  // 填入点云数据
  cloud->width  = 5;
  cloud->height = 1;
  cloud->points.resize (cloud->width * cloud->height);
  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 = 1024 * rand () / (RAND_MAX + 1.0f);
  }
  /*RadiusOutlierRemoval*/
  if (strcmp(argv[1], "-r") == 0){
    pcl::RadiusOutlierRemoval<pcl::PointXYZ> outrem;
    // 创建滤波器
    outrem.setInputCloud(cloud);
    outrem.setRadiusSearch(0.8);//搜索半径
    outrem.setMinNeighborsInRadius (2);//最少相邻点数
    // 应用滤波器
    outrem.filter (*cloud_filtered);
  }
  /*ConditionalRemoval*/
  else if (strcmp(argv[1], "-c") == 0){
    // 创建环境
    pcl::ConditionAnd<pcl::PointXYZ>::Ptr range_cond (new
      pcl::ConditionAnd<pcl::PointXYZ> ());// pcl::ConditionAnd条件“与”
    range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new
      pcl::FieldComparison<pcl::PointXYZ> ("z", pcl::ComparisonOps::GT, 0.0)));//GT:大于
    range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new
      pcl::FieldComparison<pcl::PointXYZ> ("z", pcl::ComparisonOps::LT, 0.8)));//LT:小于，滤除z处于（0，0.8）之间的点
    // 创建滤波器
    pcl::ConditionalRemoval<pcl::PointXYZ> condrem (range_cond);
    condrem.setInputCloud (cloud);
    condrem.setKeepOrganized(true);
    // 应用滤波器
    condrem.filter (*cloud_filtered);
  }
  else{
    std::cerr << "please specify command line arg '-r' or '-c'" << std::endl;
    exit(0);
  }
  std::cerr << "Cloud before filtering: " << 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;
  // 显示滤波后的点云
  std::cerr << "Cloud after filtering: " << std::endl;
  for (size_t i = 0; i < cloud_filtered->points.size (); ++i)
    std::cerr << "    " << cloud_filtered->points[i].x << " "
                        << cloud_filtered->points[i].y << " "
                        << cloud_filtered->points[i].z << std::endl;
  return (0);
}

5.参数化模型投影点云

将点云投影到平面或球等模型上，在PCL中存储了常见模型的系数的数据结构。例如：平面模型 ax+by+cz+d=0
PCL支持的模型类型
具体的每种模型的系数结构：参考博客

SACMODEL_PLANE,
SACMODEL_LINE,
SACMODEL_CIRCLE2D,
SACMODEL_CIRCLE3D,
SACMODEL_SPHERE,
SACMODEL_CYLINDER,
SACMODEL_CONE,
SACMODEL_TORUS,
SACMODEL_PARALLEL_LINE,
SACMODEL_PERPENDICULAR_PLANE,
SACMODEL_PARALLEL_LINES,
SACMODEL_NORMAL_PLANE,
SACMODEL_NORMAL_SPHERE,
SACMODEL_REGISTRATION,
SACMODEL_REGISTRATION_2D,
SACMODEL_PARALLEL_PLANE,
SACMODEL_NORMAL_PARALLEL_PLANE,
SACMODEL_STICK

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/filters/project_inliers.h>
int
 main (int argc, char** argv)
{
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected (new pcl::PointCloud<pcl::PointXYZ>);
  // 填入点云数据
  cloud->width  = 5;
  cloud->height = 1;
  cloud->points.resize (cloud->width * cloud->height);
  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 = 1024 * rand () / (RAND_MAX + 1.0f);
  }
  std::cerr << "Cloud before projection: " << 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;
  // 创建一个系数为X=Y=0,Z=1的平面 ax+by+cz=0----平面为z=0
  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
  coefficients->values.resize (4);
  coefficients->values[0] = coefficients->values[1] = 0;
  coefficients->values[2] = 1.0;
  coefficients->values[3] = 0;
  // 创建滤波器对象
  pcl::ProjectInliers<pcl::PointXYZ> proj;
  proj.setModelType (pcl::SACMODEL_PLANE);
  proj.setInputCloud (cloud);
  proj.setModelCoefficients (coefficients);//投影模型对应的系数
  proj.filter (*cloud_projected);

  std::cerr << "Cloud after projection: " << std::endl;
  for (size_t i = 0; i < cloud_projected->points.size (); ++i)
    std::cerr << "    " << cloud_projected->points[i].x << " " 
                        << cloud_projected->points[i].y << " " 
                        << cloud_projected->points[i].z << std::endl;

  return (0);
}

6.从点云中提取一个子集

基于某一种分割算法提取点云中的一个子集

#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>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/extract_indices.h>
int
main (int argc, char** argv)
{
  sensor_msgs::PointCloud2::Ptr cloud_blob (new sensor_msgs::PointCloud2), cloud_filtered_blob (new sensor_msgs::PointCloud2);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>), cloud_p (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
  // 填入点云数据
  pcl::PCDReader reader;
  reader.read ("table_scene_lms400.pcd", *cloud_blob);
  std::cerr << "PointCloud before filtering: " << cloud_blob->width * cloud_blob->height << " data points." << std::endl;
  // 创建滤波器对象:使用叶大小为1cm的下采样
  pcl::VoxelGrid<sensor_msgs::PointCloud2> sor;
  sor.setInputCloud (cloud_blob);
  sor.setLeafSize (0.01f, 0.01f, 0.01f);
  sor.filter (*cloud_filtered_blob);
  // 转化为模板点云
  pcl::fromROSMsg (*cloud_filtered_blob, *cloud_filtered);
  std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height << " data points." << std::endl;
  // 将下采样后的数据存入磁盘
  pcl::PCDWriter writer;
  writer.write<pcl::PointXYZ> ("table_scene_lms400_downsampled.pcd", *cloud_filtered, false);
  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.setMaxIterations (1000);//最大迭代次数
  seg.setDistanceThreshold (0.01);//距离阈值
  // 创建滤波器对象
  pcl::ExtractIndices<pcl::PointXYZ> extract;
  int i = 0, nr_points = (int) cloud_filtered->points.size ();
  // 当还有30%原始点云数据时
  while (cloud_filtered->points.size () > 0.3 * nr_points)//为了能多分割多个模型（多个平面或者其他几何模型），采用循环的方式进行分割
  {
    // 从余下的点云中分割最大平面组成部分
    seg.setInputCloud (cloud_filtered);
    seg.segment (*inliers, *coefficients);
    if (inliers->indices.size () == 0)
    {
      std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
      break;
    }
    // 分离内层
    extract.setInputCloud (cloud_filtered);
    extract.setIndices (inliers);//按照索引分离内层点，即平面点
    extract.setNegative (false);
    extract.filter (*cloud_p);
    std::cerr << "PointCloud representing the planar component: " << cloud_p->width * cloud_p->height << " data points." << std::endl;
    std::stringstream ss;
    ss << "table_scene_lms400_plane_" << i << ".pcd";
    writer.write<pcl::PointXYZ> (ss.str (), *cloud_p, false);
    extract.setNegative (true);
    extract.filter (*cloud_f);
    cloud_filtered.swap (cloud_f);
    i++;
  }
  return (0);
}

7.CropHull任意多边形内部点云提取

利用二维多边形截取点云。

#include <pcl/visualization/cloud_viewer.h>
#include <iostream>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <vector>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/crop_hull.h>
#include <pcl/surface/concave_hull.h>

int main(int argc, char** argv)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PCDReader reader;
	reader.read(argv[1],*cloud);
	//输入2D平面点，作为封闭多边形的顶点
	pcl::PointCloud<pcl::PointXYZ>::Ptr boundingbox_ptr (new pcl::PointCloud<pcl::PointXYZ>);
	boundingbox_ptr->push_back(pcl::PointXYZ(0.1, 0.1, 0));
	boundingbox_ptr->push_back(pcl::PointXYZ(0.1, -0.1,0 ));
	boundingbox_ptr->push_back(pcl::PointXYZ(-0.1, 0.1,0 ));
	boundingbox_ptr->push_back(pcl::PointXYZ(-0.1, -0.1,0 ));
	boundingbox_ptr->push_back(pcl::PointXYZ(0.15, 0.1,0 ));

	pcl::ConvexHull<pcl::PointXYZ> hull;//创建凸包对象
	hull.setInputCloud(boundingbox_ptr);//构建2D凸包
	hull.setDimension(2);//设置凸包维度
	std::vector<pcl::Vertices> polygons;//该向量用于保存凸包的顶点
	pcl::PointCloud<pcl::PointXYZ>::Ptr surface_hull (new pcl::PointCloud<pcl::PointXYZ>);//该点云用于描述凸包形状
	hull.reconstruct(*surface_hull, polygons);//构建凸包

	pcl::PointCloud<pcl::PointXYZ>::Ptr objects (new pcl::PointCloud<pcl::PointXYZ>);
	pcl::CropHull<pcl::PointXYZ> bb_filter;
	bb_filter.setDim(2);//注意与凸包维度一致
	bb_filter.setInputCloud(cloud);
	bb_filter.setHullIndices(polygons);//输入凸包顶点向量
	bb_filter.setHullCloud(surface_hull);//输入凸包表面形状
	bb_filter.filter(*objects);
	std::cout << objects->size() << std::endl;

	//visualize
	boost::shared_ptr<pcl::visualization::PCLVisualizer> for_visualizer_v (new pcl::visualization::PCLVisualizer ("crophull display"));
	for_visualizer_v->setBackgroundColor(255,255,255);

	int v1(0);
	for_visualizer_v->createViewPort (0.0, 0.0, 0.33, 1, v1);
	for_visualizer_v->setBackgroundColor (255, 255, 255, v1);
	for_visualizer_v->addPointCloud (cloud,"cloud",v1);
	for_visualizer_v->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR,255,0,0,"cloud");
	for_visualizer_v->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,3,"cloud");
	for_visualizer_v->addPolygon<pcl::PointXYZ>(surface_hull,0,.069*255,0.2*255,"backview_hull_polyline1",v1);

	int v2(0);
	for_visualizer_v->createViewPort (0.33, 0.0, 0.66, 1, v2);	
	for_visualizer_v->setBackgroundColor (255, 255, 255, v2);
	for_visualizer_v->addPointCloud (surface_hull,"surface_hull",v2);
	for_visualizer_v->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR,255,0,0,"surface_hull");
	for_visualizer_v->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,8,"surface_hull");
	for_visualizer_v->addPolygon<pcl::PointXYZ>(surface_hull,0,.069*255,0.2*255,"backview_hull_polyline",v2);

	int v3(0);
	for_visualizer_v->createViewPort (0.66, 0.0, 1, 1, v3);
	for_visualizer_v->setBackgroundColor (255, 255, 255, v3);
	for_visualizer_v->addPointCloud (objects,"objects",v3);
	for_visualizer_v->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR,255,0,0,"objects");
	for_visualizer_v->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,3,"objects");

	while (!for_visualizer_v->wasStopped())
	{

		for_visualizer_v->spinOnce(1000);
	}
	system("pause");
}