OpenGL与PCL库实现点云体系滤波优化及三角面片化

**

点云优化过滤方法效果对比

**

体素滤波
基本保持点云形状及特征点，可以准确地保持宏观的几何外形。优化效率较好，时间根据参数线性增长，本点云建议参数为0.2

直通滤波
以坐标系的值限制来进行滤波，该方法的使用比较灵活但完全取决于用户的限定字段和对应条件 ，适合过滤不需要的高度范围内的点，时间根据参数线性递增。

统计滤波
可以用来剔除离群点，可能会改变点云的部分特征点，优化效率一般。时间根据参数线性递增，本点云建议参数为50,0.3

半径滤波
用户指定每个的点一定范围内周围至少要有足够多的近邻，否则视为离群点删除。会改变点云的特征点，优化效率较好，本点云建议参数0.1,2

双边滤波
计算时间较长，参数需多次调整，优化效率20%
移动最小二乘法 优化效果较好，但会改变部分离散特征点
时间与参数成指数增长，建议参数0.1

点云优化过滤方法
一．滤波器

1. 体素滤波器 pcl::VoxelGrid

VoxelGrid滤波原理 : 使用体素化网格的方法实现下采样，并保持点云的形状特征；voxelGrid类通过在点云数据中创建三维体素栅格，然后用每个体素的重心来近似表达体素中的其它点，采样点对应曲面的表示更为准确

#include <pcl/filters/voxel_grid.h>

  pcl::VoxelGrid<pcl::PCLPointXYZ> sor;      // 设置滤波器对象
  sor.setInputCloud (cloud);                 // 设置输入点云
  sor.setLeafSize (0.1f,.0.1f, 0.1f);        // 设置三维体素网格
  sor.filter (*cloudFiltered);               // 执行过滤，过滤结果保存在 cloudFiltered

优化对比

setLeafSize(x,x,x) 算法运行时间及结果
X = 0.1
基本形状及特征点不改变，密度变小 62s，29万个点

X = 0.2
基本形状及特征点不改变，密度变小 55s，17万个点

X = 0.3
基本形状及特征点不改变，密度变小 52s，9.8万个点

X = 0.4
基本形状及特征点不改变，密度变小 49s，7.3万个点

X = 0.5
基本形状及特征点不改变，密度变小 47s，5.3万个点

2. 直通滤波器 pcl::PassThrough

#include <pcl/filters/passthrough.h>

  pcl::PassThrough<pcl::PointXYZ> pass; // 设置滤波器对象
  pass.setInputCloud (cloud);           // 设置输入点云
  pass.setFilterFieldName ("z");        // 设置过滤时所需要点云类型的字段
  pass.setFilterLimits (0.0, 1.0);      // 设置过滤字段的范围
  pass.setFilterLimitsNegative (false); // 设置保留范围内的还是过滤掉范围内的:默认false，即保留范围内的；若设为true，则保留范围外的；
  //pass.setNegative (true);            // 作用与 setFilterLimitsNegative 一样
  pass.filter (*cloudFiltered);         // 执行过滤，过滤结果保存在 cloudFiltered

优化对比
setFilterLimits (x,y); 算法运行时间及结果
X= 0.0,Y=1.0 61s,43.1万个点
X= 0.0,Y=5.0
过滤范围内形状及特征点改变 56s，28.5万个点

X= 0.0,Y=5.0
过滤范围内形状及特征点改变 53s，18.4万个点

3. 双边滤波器 pcl::BilateralFilter

双边滤波算法是通过取临近采样点和加权平均来修正当前采样点的位置，从而达到滤波效果，同时也会有选择剔除与当前采样点“差异”太大的相邻采样点，从而保持原特征的目的。但是计算时间较长，且两个参数需要多次试验调整。

#include <pcl/filters/bilateral.h>
#include <pcl/kdtree/kdtree_flann.h>

    // 创建KDtree
	pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointXYZ>);//用一个子类作为形参传入
	pcl::BilateralFilter<PointT> bf;     // 设置滤波器对象
	bf.setInputCloud(cloud);             // 设置输入点云
	bf.setSearchMethod(tree);            // 设置搜索模式为树状
	bf.setHalfSize(sigma_s);             // 设置滤波半径
	bf.setStdDev(sigma_r);               // 设置标准差
	bf.filter(cloudFiltered);            // 执行滤波，结果保存在cloudFiltered

二．去除点云离群点算法

2.1统计滤波 pcl::StatisticalOutlierRemoval

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

#include <pcl/filters/statistical_outlier_removal.h>

  pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor; // 设置滤波器对象
  sor.setInputCloud(cloud);         // 设置输入点云
  sor.setMeanK(50);                // 每个点参考的邻域点个数
  sor.setStddevMulThresh(1.0);        // 标准差倍数设为1，即所有距离查询点的平均距离大于1个标准差的点都将被标记为离群点并删除
  sor.setNegative (false);         // 设置为正(默认值)，保留群内点，过滤掉离群点
  sor.filter(*cloudFiltered);        // 执行过滤，保留的结果保存在 cloudFiltered
  sor.setNegative (true);                       // 设置为负 ，再次执行过滤
  sor.filter (*cloudFiltered);                 // 再次执行过滤，保存的为过滤掉的离群点

优化对比
setMeanK (x)
setStddevMulThresh（y） 算法运行时间及结果
X = 50，Y = 1.0 125s，39.4万个点
X = 50，Y = 3.0
去除离群点、噪声点，点云特征点和形状会有些许改变，但整体看起来更加平滑 130s，42.5万个点

X = 40，Y = 1.0
去除离群点、噪声点，点云特征点和形状改变 110s，39.4万个点

2.2半径滤波 pcl::RadiusOutlierRemoval

RadiusOutlierRemoval滤波原理 : 在点云数据中，用户指定每个的点一定范围内周围至少要有足够多的近邻，否则视为离群点删除。

#include <pcl/filters/radius_outlier_removal.h>   
  pcl::RadiusOutlierRemoval<pcl::PointXYZ> sor; // 设置滤波器对象
  sor.setInputCloud(RawCloud);                  // 设置输入点云
  sor.setRadiusSearch(0.1);                     // 设置在0.1的半径内找临近点
  sor.setMinNeighborsInRadius(2);               // 设置邻近点个数小于2的点均删除
  sor.filter (*cloudFiltered);                  // 执行过滤，保存结果
  sor.setNegative (true);                       // 设置为负 ，再次执行过滤
  sor.filter (*cloudFiltered);                 // 再次执行过滤，保存的为过滤掉的离群点

优化对比
setRadiusSearch（x）
setMinNeighborsInRadius（y） 算法运行时间及结果
X = 0.1，Y = 2
去除离群点、噪声点。平滑点云，但形状和特征点会改变 65s，22.4万个点

X = 0.2，Y = 2 70s，39.2万个点

X = 0.1，Y = 3 64s，9.7万个点

三. 移动最小二乘法

点云形状略微改变，优化效率根据搜索半径而定，下面为40％

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::io::loadPCDFile("boudary.pcd", *cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptrcloud_smoothed(new pcl::PointCloud<pcl::PointXYZ>);
pcl::search::KdTree<pcl::PointXYZ>::PtrtreeSampling(new pcl::search::KdTree<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ> mls_point;
	pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointXYZ> mls;
	mls.setComputeNormals(false);
	mls.setInputCloud(cloud);
	mls.setPolynomialOrder(2);  //MLS拟合的阶数
	mls.setPolynomialFit(false); //设置为true则在平滑过程中采用多项式拟合来提高精度
	mls.setSearchMethod(treeSampling);
	mls.setSearchRadius(0.1);
	mls.process(mls_point);
	// 输出重采样结果
	cloud_smoothed = mls_point.makeShared();
	pcl::io::savePCDFileASCII("NoBoundpoints.pcd", *cloud_smoothed);

setSearchRadius(X) 算法运行时间及结果
X = 0.1
对点云进行平滑处理
形状和特征点改变
点云密度随X成正比变化 105s,22.4万个点

X = 0.2
点云密度随X成正比变化
点云形状及特征点些许改变 190s，39.2万个点

3D效果实现方式

一. 贪婪三角化算法

直接形成三角面片轮廓，时间太久

#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/features/normal_3d.h>
#include <pcl/surface/gp3.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <pcl/io/vtk_io.h>

  // Load input file into a PointCloud<T> with an appropriate type
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
  //sensor_msgs::PointCloud2 cloud_blob;
  pcl::io::loadPCDFile ("table_scene_lms400_downsampled.pcd", *cloud);
  //pcl::fromROSMsg (cloud_blob, *cloud);
  //* the data should be available in cloud

  // Normal estimation*
  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
  pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
  tree->setInputCloud (cloud);
  n.setInputCloud (cloud);
  n.setSearchMethod (tree);
  n.setKSearch (20);
  n.compute (*normals);
  //* normals should not contain the point normals + surface curvatures
 pcl::PointCloud<pcl::PointNormal>::Ptrcloud_with_normals(newpcl::PointCloud<pcl::PointNormal>);
  pcl::concatenateFields (*cloud, *normals, *cloud_with_normals);
  //* cloud_with_normals = cloud + normals

  // Create search tree*
 pcl::search::KdTree<pcl::PointNormal>::Ptrtree2(newpcl::search::KdTree<pcl::PointNormal>);
  tree2->setInputCloud (cloud_with_normals);

  // Initialize objects
  pcl::GreedyProjectionTriangulation<pcl::PointNormal> gp3;   //定义贪婪投影三角化对象
  pcl::PolygonMesh triangles;    //存储最终三角化的网格模型

  // Set the maximum distance between connected points (maximum edge length)
  gp3.setSearchRadius (0.025);  //设置连接点之间最大距离

  // Set typical values for the parameters
  gp3.setMu (2.5);             //设置被样本点搜索其邻近点的最远距离
  gp3.setMaximumNearestNeighbors (100);         //设置样本点可搜索的邻域个数
  gp3.setMaximumSurfaceAngle(M_PI/4);        //某点法线方向偏离样本点法线方向最大角度 45 degrees
  gp3.setMinimumAngle(M_PI/18);     //三角化后三角形内角最小角度 10 degrees
  gp3.setMaximumAngle(2*M_PI/3); //三角化后三角形内角最大角度 120 degrees
  gp3.setNormalConsistency(false);  //输入的法线方向是否一致 false：那就保证它一致 true：那就不管了

  // Get result
  gp3.setInputCloud (cloud_with_normals);
  gp3.setSearchMethod (tree2);
  gp3.reconstruct (triangles);         /重建提取三角化
 // std::cout << triangles;
  // Additional vertex information
  std::vector<int> parts = gp3.getPartIDs();  //获取ID字段
  std::vector<int> states = gp3.getPointStates();     //获取点状态
  return (0);
}

二. 3D效果面实现方式：

1.将点云按Z值大小排序

bool compare(point p1,:point p2)
{
		if (p1.z > p2.z)return true;
		else if (p1.z < p2.z)return false;
		else if (p1.y > p2.y)return true;
		else if (p1.y < p2.y)return false;
		else if (p1.x > p2.x)return true;
		else return false;
}

2.以连续三角的方式成面连接点云的头尾。

Long long lsize = vecPoint.size();
glBegin(GL_TRIANGLE_FAN);
for (register int i = 1; i < vecPoint.size(); ++i)
{
	glVertex3f(vecPoint[i].x, vecPoint[i].y, vecPoint[i].z);//首个点
	glVertex3f(vecPoint[lsize - i].x, vecPoint[lsize - i].y, vecPoint[lsize - i].z);//尾点
}
glend();

1. List item

点云颜色随高度渐变实现方式

以（0，100）区间为例

void setColorPoint(float x, float y, float z)
{
	if (z >= 0.0f && z <= 25.0f) //蓝到青
	{
		glColor3f(0, (1.0f / 25.0f)*z, 1.0f);
	}
	else if (z > 25.0f && z <= 50.0f) //青到绿
	{
		glColor3f(0, 1.0f, (1.0f / 25.0f)*(50.0f - z));
	}
	else if (z > 50.0f && z <= 75.0f) //绿到黄
	{
		glColor3f((1.0f / 25.0f)*(z - 50.0f), 1.0f, 0);
	}
	else//黄到红
	{
		glColor3f(1.0f, (1.0f / 25.0f)*(100.0f - z), 0);
	}
	glVertex3f(x, y, z);
}