1、体素滤波下采样
简介:在建图时,从多个视野扫描的点云会存在重复情况,近似点会占用大量空间,并占用较大时间复杂度,对空间中数据进行下采样,优化时间复杂度。
作用:处理大数据点云时,例如点云批准,花费时间巨大,通过体素滤波选取体素中心点代替目标点云,减少匹配时间,实现更快速的曲面重建。
#include <pcl/filters/voxel_grid.h>
//点云下采样
pcl::PointCloud<pcl::PointXYZ>::Ptr downSample(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double leaf) {
pcl::VoxelGrid<pcl::PointXYZ> sor;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_new(new pcl::PointCloud<pcl::PointXYZ>);
sor.setInputCloud(cloud);
sor.setLeafSize(leaf, leaf, leaf); //大小为2*2*2
sor.filter(*cloud_new);
return cloud_new;
}
2、统计滤波
统计滤波器主要用于去除明显离群点。 离群点特征在空间中分布稀疏。定义某处点云小于某个密度,既点云无效。计算每个点到其最近的k个点平均距离。则点云中所有点的距离应构成高斯分布。根据给定均值与方差,可剔除方差之外的点。对于图中那些离点云规模较聚集的区域,简称为离群点,统计滤波对这些离群点的处理比较好。
#include <pcl/filters/statistical_outlier_removal.h>//统计滤波器支持头文件
// 去除离群点
pcl::PointCloud<pcl::PointXYZ>::Ptr remove_leave_Point(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,int K=50,double eps=2.0) {
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;//创建对象
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_new(new pcl::PointCloud<pcl::PointXYZ>);
sor.setInputCloud(cloud); //输入点云
sor.setMeanK(K); //临近点
sor.setStddevMulThresh(eps); //离群点阈值
sor.filter(*cloud_new);
return cloud_new;
}
首先,遍历点云,计算每个点与其最近的k个邻居点之间的平均距离;其次,计算所有平均距离的均值μ与标准差σ,则距离阈值dmax可表示为dmax=μ+α×σ,α是一个常数,可称为比例系数,它取决于邻居点的数目;最后,再次遍历点云,剔除与k个邻居点的平均距离大于dmax的点。
看着统计滤波的时间比较长,想要用kdtree实现,没想到它的底层就是kdtree实现,这里有我写的kdtree版本的统计滤波,先放这里,以后再做性能优化,通过手写kdtree实现统计滤波和上面算法时间复杂度差不多大。
//基于kdtree 去除离去点
pcl::PointCloud<pcl::PointXYZ>::Ptr remove_leave_Point_KdTree(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, int K = 50, double eps = 2.0) {
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
kdtree.setInputCloud(cloud); //设置搜索空间
vector<float> vis(cloud->size());
double average = 0;
for (int i = 0; i < cloud->size(); i++) {
//int K = 2;
std::vector<int> ptIdxByKNN(K); //存储查询点近邻索引
std::vector<float> ptKNN(K); //存储近邻点对应距离平方
kdtree.nearestKSearch(cloud->points[i], K, ptIdxByKNN, ptKNN); //执行K近邻搜索
vis[i] = 0;
for (int j = 0; j < K; j++) {
vis[i] += ptKNN[j];
}
vis[i] = vis[i] / K * 1.0;
average += vis[i];
}
average = average / (1.0*vis.size());
double dmax = 0;
double rou = 0;
for (int i = 0; i < vis.size(); i++) {
rou = (vis[i] - average)*(vis[i] - average);
}
rou = sqrt(rou*1.0 / vis.size());
dmax = average + eps * rou;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_new(new pcl::PointCloud<pcl::PointXYZ>);
for (int i = 0; i < cloud->size(); i++) {
if (vis[i]<dmax) {
cloud_new->push_back(cloud->points[i]);
}
}
cout <<" 新点云"<< cloud_new->size() << endl;
return cloud_new;
}