PCL——从点云到网格（二）点云预处理

前一篇得到的点云还不能拿来直接用，因为一个是太多了，另一个是还存在一大堆的噪声。所以这一篇我会记录一下我使用到的一些滤波操作。
主要的的参考资料都在这里了：
https://mp.weixin.qq.com/s/9pxff6LwcecDHsx4kI34sw
https://mp.weixin.qq.com/s/GFDWOudJ08In6jFyrZ7hhg

但是我利用得到的点云做ICP得到的融合的矩阵效果不理想，有可能是我相机移动的幅度较大，也可能是我计算ICP的点太少（我是先利用降采样，再做ICP的，效果不太理想，得到的T矩阵应用之后效果也不是很好，但是如果直接做ICP实在是太慢了）

点云滤波操作

点云中密度不规则，存在离群点，数据太多，噪声等因素导致点云需要被滤波。没有滤波的点云不能用来做Mesh。
PCL1.8.1 的滤波函数都在这：
http://docs.pointclouds.org/1.8.1/group__filters.html

我对得到的点云做的预处理：

#include <pcl/filters/voxel_grid.h>           //用于体素网格化的滤波类头文件 
#include <pcl/filters/filter.h>             //滤波相关头文件
#include <pcl/filters/passthrough.h>
#include <pcl/filters/project_inliers.h>          //滤波相关类头文件
#include <pcl/filters/statistical_outlier_removal.h> //统计方法去除离群点
#include <pcl/filters/radius_outlier_removal.h> //统计方法去除离群点
#include <pcl/filters/approximate_voxel_grid.h>  //ApproximateVoxelGrid 

//对点云的预处理，就是点云下采样，去离群点滤波，
void PrePorcessingOfPointCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud_in, pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud_out) 
{
	pcl::StopWatch time;
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_temp(new pcl::PointCloud<pcl::PointXYZRGB>());

	DownSample(cloud_in, cloud_temp);

	OutlierFilter(cloud_temp, cloud_out);
	
	std::cout << "point cloud pre processing time(s): " << time.getTimeSeconds() << std::endl;
}

接着来逐个看看每个函数。
首先进行下采样：
我用到的下采样函数比较简单，pcl::VoxelGrid

//点云下采样
void DownSample(pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud_in, pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud_out)
{
	//down sample
	std::cout << "begin downSample cloud_in size: " << cloud_in->size() << std::endl;
	pcl::VoxelGrid<pcl::PointXYZRGB> downSampled;  //创建滤波对象
	downSampled.setInputCloud(cloud_in);            //设置需要过滤的点云给滤波对象
	downSampled.setLeafSize(0.05f, 0.05f, 0.05f);  //设置滤波时创建的体素体积为1cm的立方体（1为米，0.01就是1cm）
	downSampled.filter(*cloud_out);  //执行滤波处理，存储输出
		
	std::cout << "success downSample, size: " << cloud_out->size() << std::endl;
	
}

可以看到调用封装好的函数还是很方便的，最简单的做法：设置输入点云，设置提体素大小，滤波即可。
pcl::VoxelGrid 使用了“体素”的概念，对整个点云按照 LeafSize 设置体素大小，也就是将整个点云空间分割成许许多多以 LeafSize 为基本单位的小立方体，每个小立方体中最中间的点是这些点的中心（而不是立方体中心）（PCL文档中原文：The VoxelGrid class creates a 3D voxel grid (think about a voxel grid as a set of tiny 3D boxes in space) over the input point cloud data. Then, in each voxel (i.e., 3D box), all the points present will be approximated (i.e., downsampled) with their centroid. This approach is a bit slower than approximating them with the center of the voxel, but it represents the underlying surface more accurately.）。
还有一个类 ApproximateVoxelGrid 我不是很清楚他的做法，貌似是利用体素中点来代替体素中的点的。
应该还是有一些参数设置应该可以让输出效果变好点吧，还是没有研究。

接下来是去离群点：
去除一些离群噪声。（资料中说：因为离群点会使局部点云特征(如表面法线或曲率变化)的估计复杂化，从而导致错误的值，从而可能导致点云配准失败。离群点还会累积，误差会越来越大）
pcl::StatisticalOutlierRemoval 和 pcl::RadiusOutlierRemoval
利用统计学 和 利用几何来去除离群点。
pcl::StatisticalOutlierRemoval 利用统计学，对每个点的邻域进行统计分析，剔除不符合一定标准的邻域点。（具体来说，对于每个点，我们计算它到所有相邻点的平均距离。假设得到的分布是高斯分布，我们可以计算出一个均值 μ 和一个标准差 σ，那么这个邻域点集中所有点与其邻域距离大于μ + std_mul * σ 区间之外的点都可以被视为离群点，并可从点云数据中去除。std_mul 是标准差倍数的一个阈值，可以自己指定。）

pcl::RadiusOutlierRemoval 设置半径后，每个点周围再该半径的球内至少要有足够多的邻居，否则他就是个外点。（米为单位）

//去除离群点
void OutlierFilter(pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud_in, pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud_out)
{
	std::cout << "begin outlierFilter cloud_in size: " << cloud_in->size() << std::endl;
	
	统计，有点慢
	//pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor;   //创建滤波器对象
	//sor.setInputCloud(cloud_in);                           //设置待滤波的点云
	//sor.setMeanK(50);                               //设置在进行统计时考虑的临近点个数
	//sor.setStddevMulThresh(1.0);                      //设置判断是否为离群点的阀值，用来倍乘标准差
	//sor.filter(*cloud_out);                    //滤波结果存储到cloud_filtered
	//
	
	pcl::RadiusOutlierRemoval<pcl::PointXYZRGB> pcFilter;  //创建滤波器对象
	pcFilter.setInputCloud(cloud_in);             //设置待滤波的点云
	pcFilter.setRadiusSearch(0.03);               // 设置搜索半径
	pcFilter.setMinNeighborsInRadius(3);      // 设置一个内点最少的邻居数目
	pcFilter.filter(*cloud_out);        //滤波结果存储到cloud_filtered

	std::cout << "success OutlierFilter, size: " << cloud_out ->size() <<std::endl;

}

统计这个比较慢，我最后还是用了根据几何距离来找离群点的，可能没有统计上那么准，但是快。

我的预处理到这就结束了

不够由于还有一些工作，想融合两帧的点云。但是由于变换矩阵算出来实在是有问题，因此效果并不好。不知道是哪里出了问题。先把方法记下来，之后再改改。如果有dalao能够帮我指出我的问题我真的非常感谢~

由于2帧点云不相同，因此考虑利用 TrimmedICP 计算点云之间的变换矩阵。这个函数返回变换矩阵，并且利用下面这个函数做变换（参数为示意参数 T为变换矩阵）

pcl::transformPointCloud(Ptr cloud_from,Ptr cloud_from，Eigen::Matrix4f T)

变换矩阵求解（没有读过这个论文，具体的参数并不是很了解）：

Eigen::Matrix4f useTrimmedICPWithTwoPointClouds(pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud_1, pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud_2)
{
	pcl::StopWatch time;
	pcl::PointCloud<pcl::PointXYZ> src,dst;
	pcl::copyPointCloud(*cloud_1, src);
	pcl::copyPointCloud(*cloud_2, dst);

	std::cout << "begin trimmed-ICP...." << std::endl;
	auto t_icp = pcl::recognition::TrimmedICP<pcl::PointXYZ, float>::TrimmedICP();
	t_icp.init(dst.makeShared());
	Eigen::Matrix4f T = Eigen::Matrix4f::Identity();
	t_icp.setNewToOldEnergyRatio(0.9f); //default 0.99
	t_icp.align(src, (int)(src.size()*0.7f), T);
	return T;
	std::cout << "trimmed-icp time(s): " << time.getTimeSeconds() << std::endl;
}

align这个函数执行之后会填充T，得到变换矩阵（时间巨长）。

接着计算两个点云的重叠区域，这后面就是自己写的了，也不知道对不对：从CorrespondenceEstimation得到重叠的区域，再利用两个点云生成新点云，重叠区域只生成一次（以第二个点云为主，重复了就用第二个点云的数据）

//对两个点云融合？CorrespondenceEstimation 计算点云之间重叠区域
pcl::PointCloud<pcl::PointXYZRGB>::Ptr FuseTwoPointCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud_1, pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud_2)
{
	pcl::StopWatch time;
	std::cout << "start erase overlap part...." << std::endl;
	// ... read or fill in source and target
	pcl::registration::CorrespondenceEstimation<pcl::PointXYZRGB, pcl::PointXYZRGB> cor_est;
	cor_est.setInputSource(cloud_1);
	cor_est.setInputTarget(cloud_2);
	pcl::Correspondences all_correspondences;
	// Determine all reciprocal correspondences
	cor_est.determineReciprocalCorrespondences(all_correspondences);
	if (all_correspondences.size() == 0)
	{
		std::cout << "no valid points to match and fuse,return cloud_1 ! " << std::endl;
		return cloud_1;
	}
	
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr totalPointCloud(new pcl::PointCloud<pcl::PointXYZRGB>());
	totalPointCloud->resize(cloud_1->size() + cloud_2->size() - all_correspondences.size());
	
	int* mask_1 = new int[cloud_1->size()];  //2的点云是新的点云，所以重叠的部分显示二的，这个就存的是相应的1中的index
	for (int i = 0; i < cloud_1->size(); i++)
	{
		mask_1[i] = 0;  //init mask 1
	}

	for (int i = 0; i < all_correspondences.size(); i++)
	{
		int idxInCloud1 = all_correspondences[i].index_query;
		mask_1[idxInCloud1] = -1;
	}

	int cloud2Cnt = cloud_2->size();
	int idxInTotalCnt = 0;  //在totalCloud中的index
	for (int i = 0; i < cloud2Cnt + cloud_1->size() ; i++)
	{
		if (i < cloud_2->size())
		{
			totalPointCloud->points[idxInTotalCnt] = cloud_2->points[idxInTotalCnt];
			idxInTotalCnt++;
		}
		else
		{
			if (mask_1[i - cloud2Cnt] == -1)  //已经用2给他赋过值了
			{				
				continue;
			}
			else
			{
				totalPointCloud->points[idxInTotalCnt] = cloud_1->points[i - cloud2Cnt];
				idxInTotalCnt++;
			}
		}
	}

	std::cout << "fuse Time(s): " << time.getTimeSeconds() << std::endl;
	std::cout << "finish fuse , total size:" << totalPointCloud->size() << std::endl;
	
	totalPointCloud->is_dense = true;
	return totalPointCloud;
}

is_dense 变量是判断点云中是否有NAN这种不合理的点的。不过有时候这玩意不起作用，有时候它是True，但是还是有NAN点。如果它不起做用，就用这个自定义函数，主要部分 pcl_isfinite(double) ：

//遍历并删除重采样之后的无效点
void EraseInvalidPoints(pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud)
{
	pcl::PointCloud<pcl::PointXYZRGB>::iterator it = cloud->points.begin();  //迭代找点
	int cnt = 0;
	while (it != cloud->points.end())
	{
		float x, y, z, rgb;
		x = it->x;
		y = it->y;
		z = it->z;		
		//cout << "x: " << x << "  y: " << y << "  z: " << z << "  rgb: " << rgb << endl;
		if (!pcl_isfinite(x) || !pcl_isfinite(y) || !pcl_isfinite(z))
		{
			it = cloud->points.erase(it); //删除了迭代点的之后他会返回下一个点
			cnt++;
		}
		else
			++it;
	}
	std::cout << "erase points: " << cnt << std::endl;
}