PCL点云库之滤波

  • filter是PCL的一个模块,用来对点云进行滤波处理
  • filter模块包含32个类
  • 依赖search、common、sample consensus、k-d tree、octree等

为什么要滤波

  • 有噪声
  • 有离群点
  • 数据量太大,需要降采样
  • 方便后续数据处理

PCL滤波算法及分类

类别滤波器名称类名称功能备注
去噪类统计滤波StatisticalOutlierRemoval滤除噪点
去噪类半径率波RadiusOutlierRemoval滤除离群点
采样类体素网格VoxelGrid下采样立方体
采样类均匀采样UniformSampling下采样
采样类增采样通常平滑时使用
裁剪类直通滤波PassThrough沿坐标轴裁剪
裁剪类立方体CropBox裁剪掉立方体内
裁剪类封闭曲线CropHull
裁剪类Clipper3D
裁剪类BoxClipper3D
裁剪类按索引提取ExtractIndices
平滑类最小二乘MovingLeastSquares平滑在surface模块内
其他投影ProjectInliers向参数化模型
其他双边BilateralFilter
其他快速双边FastBilateralFilter
其他卷积Convolution
其他高斯GaussianKernel
其他中值
其他条件ConditionalRemoval多条件同时处理

PCL滤波器介绍

统计滤波器
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
  • statistical:统计;outlier:离群
  • 作用是去除稀疏离群噪点,比如在估算点云局部特征(计算法向量或曲率)时,噪点可能会导致错误的计算结果,所以要滤除
  • 统计滤波器的主要思想是假设点云中所有的点与其最近的k个邻域的平均距离满足高斯分布,那么根据均值和方差可确定一个距离阈值,当某个点与其最近k个点的平均距离大于这个阈值时判定该点为离群点,并去除
  • 算法流程
开始
遍历点云,计算每个点与其最近k个邻居点之间的平均距离
计算所有平均距离的均值u与标准差s
距离阈值dmax=u+t*s,t为常数,比例系数
遍历点云
剔除与k个邻居点的平均距离大于dmax的点
半径滤波器
pcl::RadiusOutlierRemoval<pcl::PointXYZ> ror;
  • 其思想就是在点云数据中,设定每个点一定半径范围内周围至少有足够多的近邻,不满足就被删除。比如指定一个半径ddd,然后指定该点半径内至少有NNN个近邻,如某一个点半径ddd内的近邻数量少于NNN个,则该点将被删除
体素网格滤波器
pcl::VoxelGrid<pcl::PointXYZ> vg;
  • 体素网格滤波器:在输入点云数据上,创建一个3D体素网格(将体素网格视为空间中的一组微小的空间三维立方体的集合),然后在每个体素(立体方框)中,所有存在的点都将用它们的质心进行近似(即下采样)。这种方法比用体素的中心逼近的方法要慢,但能更准确的表示曲面。
  • 常用于下采样
  • 体素滤波器向下采样时不破坏点云本身几何结构,但会移动点的位置。
均匀采样器
pcl::UniformSampling<pcl::PointXYZ> us;
  • 均匀采样与体素网格原理相同,体素网格是正方体空间内保留一个点(重心)
  • 均匀采样是半径球内保留一个点(重心)
增采样
  • 在最小二乘平滑时做增采样
直通滤波器
pcl::PassThrough<pcl::PointXYZ> pass;
  • 对指定的某一维度实施简单的滤波,过滤掉给定范围外或范围内的数据
  • 分类到裁剪更合适
立方体滤波器
pcl::CropBox<pcl::PointXYZ> cb;
  • 过滤掉在用户给定立方体内的点云数据
  • 分类到裁剪更合适
封闭曲面滤波器
pcl::CropHull<pcl::PointXYZ> ch;
  • 过滤掉在给定三维封闭曲面或二维封闭多边形内部或外部的点云
  • 分类到裁剪更合适
  • 流程:首先输入2D平面点云,这些点是2D封闭曲面的顶点
  • 用凸包和凹包的效果似乎是一样的
Clipper3D
pcl::Clipper3D<pcl::PointXYZ>
BoxClipper3D
pcl::BoxClipper3D<pcl::PointXYZ>;
  • 实现用一个原点为中心,XYZ各个方向尺寸可配置的,经过用户指定的仿射变换的立方体进行裁剪
  • 需要先设置仿射变换矩阵,对立方体进行变换处理
  • 输出仿射变换后落在立方体内的点
  • 待整理,当前资料较少
根据索引提取点云
pcl::ExtractIndices<pcl::PointXYZ> ei;
  • 先运用比如分割等算法,得到想要提取的点云的索引,再用该算子得到点云
  • 该算子可以用于在一个点云中循环提取多个子点云,比如乒乓球抓取案例中,一次采集点云后逐个抓取,不需抓一个就采集一次点云
  • 提取索引外还是索引内的点
setNegative(true);   //true为提取索引外的点,false为提取缩影内的点
滑动最小二乘
pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointXYZ> mls;
  • 起到平滑滤波的功能,放在surface下面,但并不能输出任何表面,不能生成mesh或者Triangulations,只是将点云进行了MLS的映射,使点云更加平滑了。
  • 输入点云最好是经过离群点滤除的,否则将会牺牲表面拟合的精度。
  • 该算法可以增采样
    • 即当点云密度不足时,增加点云密度
    • 该方法的准确性不一定100%正确
参数化模型投影点云滤波器
pcl::ModelCoefficients::Ptr mc;  //建立模型
pcl::ProjectInliers<pcl::PointXYZ> pi;
  • 创建一个参数化的模型(平面,球体,锥体等),然后将点云投影到上面,比如说投影到平面上,那么就滤掉了一个维度
  • 都有哪些模型
双边滤波器
pcl::BilateralFilter<pcl::PointXYZI> bf;
  • 该类的实现不是利用XYZ字段,而是利用强度数据进行双边滤波算法的实现,所以在使用该类时点云的类型必须有强度字段,否则无法进行双边滤波处理
  • 双边滤波算法是通过采取临近采样点和加权平均来修正当前采样点的位置,从而达到滤波的效果,同时也会有选择剔除与当前采样点差异太大的相邻采样点,从而保持原特征的目的
  • 双边滤波的运算时间太长,不适合快速处理点云
快速双边滤波器
pcl::FastBilateralFilter<pcl::PointXYZRGB> fbf;
  • 不支持PointXYZI点云格式
  • 此算法只适用有序点云
卷积滤波器
pcl::filters::Convolution<pcl::PointXYZ,pcl::PointXYZ>;
高斯滤波器
pcl::filters::GaussianKernel<PointInT, PointOutT>;
  • 是基于高斯核的卷积滤波实现,高斯滤波器相当于一个具有平滑性能的低通滤波器
中值滤波
pcl::MedianFilter<pcl::PointXYZ> mf;
  • 待整理,当前资料较少
  • 针对有序点云
条件滤波
pcl::ConditionalRemoval<pcl::PointXYZ> cr;
  • 该滤波器用于一次删除不符合用户指定的一个或多个条件的点云
  • 如下所示的代码,运行结果是保留符合条件的点云
  • 有两种条件类型,也是以算子的形式
    • conditionAnd:所有条件都要满足
    • ConditionOr:满足一个条件即可
pcl::ConditionAnd<pcl::PointXYZ>::Ptr condition_and(new pcl::ConditionAnd<pcl::PointXYZ>);    //“与”条件类型

pcl::ConditionOr<pcl::PointXYZ>::Ptr condition_or(new pcl::ConditionOr<pcl::PointXYZ>);     //“或”条件类型
  • 程序流程:
创建滤波器
设置条件类型:或/与
给条件类型设置条件
滤波
  • 比较运算符
运算符含义
GTgreater than 大于
GEgreater than or equal 大于等于
LTless than 小于
LEless than or equal 小于等于
EQequal 等于
  • 处理后点云的有序性
pcl::ConditionalRemoval<pcl::PointXYZ> cr;      //条件滤波器(算子)
cr.setKeepOrganized(true);    //保持点云有序
- 以上API可以设置处理后点云的有序性
- ture为有序,被删除的点云会以NAN或0代替
- fals为无需,被删除的点直接被删除,不保留原始点云的结构

滤波器例程


#include <iostream>

//PCL
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
//去噪类
#include <pcl/filters/statistical_outlier_removal.h>        //统计滤波
#include <pcl/filters/radius_outlier_removal.h>             //半径滤波
//采样类
#include <pcl/filters/voxel_grid.h>                         //体素网格
#include <pcl/filters/uniform_sampling.h>                   //均匀采样
//裁剪类
#include <pcl/filters/passthrough.h>                        //直通滤波
#include <pcl/filters/crop_box.h>                           //立方体滤波
#include <pcl/surface/convex_hull.h>                        //凸包,为封闭曲线准备
#include <pcl/filters/crop_hull.h>                          //封闭曲线
#include <pcl/filters/clipper3D.h> 
#include <pcl/filters/box_clipper3D.h> 
#include <pcl/common/centroid.h>                            //计算重心要用到
#include <pcl/filters/extract_indices.h>                    //根据索引提取点云
//平滑类
#include <pcl/surface/mls.h>                                //滑动最小二乘
//其他类
#include <pcl/ModelCoefficients.h>                          //模型系数头文件,用于建立模型
#include <pcl/filters/project_inliers.h>                    //参数化模型投影滤波
#include <pcl/filters/bilateral.h>                          //双边滤波
#include <pcl/filters/fast_bilateral.h>                     //快速双边
#include <pcl/filters/convolution.h>                        //卷积滤波
#include <pcl/filters/convolution_3d.h>                     //高斯滤波
#include <pcl/filters/median_filter.h>                      //中值滤波
#include <pcl/filters/conditional_removal.h>                //条件滤波


int main()
{
    std::cout << "Hello PineLiu!\n";

    //*****************************************Load PointCloud********************************

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

    if (pcl::io::loadPCDFile <pcl::PointXYZ>("D:/My Learning/C++/PrectiseofC++/49.PCL_Filtering/Appendix/Poisson_cloud.pcd", *cloud) == -1)
    {
        PCL_ERROR("Couldn't read file.\n"); //文件不存在时,返回错误,终止程序。
        return (-1);
    }
    else
    {
        std::cout << "Load PointCloud Down!\n";
    }
    boost::shared_ptr<pcl::visualization::PCLVisualizer> Viewer(new pcl::visualization::PCLVisualizer("visualleft"));
    //VIEW1->initCameraParameters();
    Viewer->setBackgroundColor(0, 0, 0);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> left_color(cloud, 250, 0, 0);
    Viewer->addPointCloud<pcl::PointXYZ>(cloud, left_color, "left pointcloud");
    Viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "left pointcloud");
    Viewer->addCoordinateSystem(1.0);

    //***************************************************************************
    //统计滤波
//***************************************************************************
    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;

    sor.setInputCloud(cloud); //设置滤波对象的需要的点云
    sor.setMeanK(50);  //设置滤波时考虑查询点邻近点数
    sor.setStddevMulThresh(1.0);  //设置是否为离群点的阈值
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_SOR_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    sor.filter(*cloud_SOR_filtered);  //执行滤波处理,存储输出的对象

    pcl::io::savePCDFile("D:/My Learning/C++/PrectiseofC++/49.PCL_Filtering/Appendix/StatisticalOutlierRemoval.pcd", *cloud_SOR_filtered);

    //显示
    boost::shared_ptr<pcl::visualization::PCLVisualizer> sor_viewer(new pcl::visualization::PCLVisualizer("sor_filter"));
    sor_viewer->setBackgroundColor(0, 0, 0);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> sor_color(cloud_SOR_filtered, 250, 0, 0);
    sor_viewer->addPointCloud<pcl::PointXYZ>(cloud_SOR_filtered, sor_color, "sorfilterpointcloud");
    sor_viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "sorfilterpointcloud");
    sor_viewer->addCoordinateSystem(1.0);

    //***************************************************************************
    //半径滤波
    //***************************************************************************
    pcl::RadiusOutlierRemoval<pcl::PointXYZ> ror;

    ror.setInputCloud(cloud);                    //输入点云
    ror.setRadiusSearch(2);                      //设置半径
    ror.setMinNeighborsInRadius(3);              //设置最小近邻点数量   
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ROR_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    ror.filter(*cloud_ROR_filtered);

    pcl::io::savePCDFile("D:/My Learning/C++/PrectiseofC++/49.PCL_Filtering/Appendix/RadiusOutlierRemoval.pcd", *cloud_ROR_filtered);

    //显示
    boost::shared_ptr<pcl::visualization::PCLVisualizer> ror_viewer(new pcl::visualization::PCLVisualizer("ror_filter"));
    ror_viewer->setBackgroundColor(0, 0, 0);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> ror_color(cloud_ROR_filtered, 250, 0, 0);
    ror_viewer->addPointCloud<pcl::PointXYZ>(cloud_ROR_filtered, ror_color, "rorfilterpointcloud");
    ror_viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "rorfilterpointcloud");
    ror_viewer->addCoordinateSystem(1.0);

    //*************************************************************************
    //体素网格
    //*************************************************************************
    pcl::VoxelGrid<pcl::PointXYZ> vg;

    vg.setInputCloud(cloud);			//设置滤波对象的需要的点云
    vg.setLeafSize(4.0f, 4.0f, 4.0f);   //设置滤波创建的体素大小为4mm
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_VG_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    vg.filter(*cloud_VG_filtered);				//执行滤波处理,存储输出的对象

    pcl::io::savePCDFile("D:/My Learning/C++/PrectiseofC++/49.PCL_Filtering/Appendix/VoxelGrid.pcd", *cloud_VG_filtered);

    //显示
    boost::shared_ptr<pcl::visualization::PCLVisualizer> vg_viewer(new pcl::visualization::PCLVisualizer("vg_filter"));
    vg_viewer->setBackgroundColor(0, 0, 0);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> vg_color(cloud_VG_filtered, 0, 254, 0);
    vg_viewer->addPointCloud<pcl::PointXYZ>(cloud_VG_filtered, vg_color, "vgfilterpointcloud");
    vg_viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "vgfilterpointcloud");
    vg_viewer->addCoordinateSystem(1.0);


//*************************************************************************
    //均匀采样
    //*************************************************************************
    pcl::UniformSampling<pcl::PointXYZ> us;

    us.setInputCloud(cloud);			//设置滤波对象的需要的点云
    us.setRadiusSearch(4.0f);           //设置半径,单位mm
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_US_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    us.filter(*cloud_US_filtered);				//执行滤波处理,存储输出的对象

    pcl::io::savePCDFile("D:/My Learning/C++/PrectiseofC++/49.PCL_Filtering/Appendix/UniformSampling.pcd", *cloud_US_filtered);

    //显示
    boost::shared_ptr<pcl::visualization::PCLVisualizer> us_viewer(new pcl::visualization::PCLVisualizer("us_filter"));
    us_viewer->setBackgroundColor(0, 0, 0);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> us_color(cloud_US_filtered, 0, 254, 0);
    us_viewer->addPointCloud<pcl::PointXYZ>(cloud_US_filtered, us_color, "usfilterpointcloud");
    us_viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "usfilterpointcloud");
    us_viewer->addCoordinateSystem(1.0);

    //*************************************************************************
    //增采样
    //*************************************************************************

    //在滑动最小二乘内使用


    //*************************************************************************
    //直通滤波
    //*************************************************************************
    pcl::PassThrough<pcl::PointXYZ> passx;

    passx.setInputCloud(cloud);			//设置滤波对象的需要的点云
    passx.setFilterFieldName("x");  //设置滤波是所需点云方向
    passx.setFilterLimits(-34.0, 47.0);  //设置范围,单位是mm
    passx.setFilterLimitsNegative(1);//设置保留或删除范围内的数据,1:删除上面设定的范围内的数据,0:保留上面设定范围内数据
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_PTx_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    passx.filter(*cloud_PTx_filtered);				//执行滤波处理,存储输出的对象

    pcl::io::savePCDFile("D:/My Learning/C++/PrectiseofC++/49.PCL_Filtering/Appendix/PassThroughx.pcd", *cloud_PTx_filtered);

    //显示x
    boost::shared_ptr<pcl::visualization::PCLVisualizer> ptx_viewer(new pcl::visualization::PCLVisualizer("ptx_filter"));
    ptx_viewer->setBackgroundColor(0, 0, 0);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> ptx_color(cloud_PTx_filtered, 255, 255, 0);
    ptx_viewer->addPointCloud<pcl::PointXYZ>(cloud_PTx_filtered, ptx_color, "ptxfilterpointcloud");
    ptx_viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "ptxfilterpointcloud");
    ptx_viewer->addCoordinateSystem(1.0);

    pcl::PassThrough<pcl::PointXYZ> passz;

    passz.setInputCloud(cloud);			//设置滤波对象的需要的点云
    passz.setFilterFieldName("z");  //设置滤波是所需点云方向
    passz.setFilterLimits(0.0, 470.0);  //设置范围,单位是mm
    passz.setFilterLimitsNegative(0);//设置保留或删除范围内的数据,1:删除上面设定的范围内的数据,0:保留上面设定范围内数据
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_PTz_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    passz.filter(*cloud_PTz_filtered);				//执行滤波处理,存储输出的对象

    pcl::io::savePCDFile("D:/My Learning/C++/PrectiseofC++/49.PCL_Filtering/Appendix/PassThroughz.pcd", *cloud_PTz_filtered);

    //显示x
    boost::shared_ptr<pcl::visualization::PCLVisualizer> ptz_viewer(new pcl::visualization::PCLVisualizer("ptz_filter"));
    ptz_viewer->setBackgroundColor(0, 0, 0);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> ptz_color(cloud_PTz_filtered, 255, 255, 0);
    ptz_viewer->addPointCloud<pcl::PointXYZ>(cloud_PTz_filtered, ptz_color, "ptzfilterpointcloud");
    ptz_viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "ptzfilterpointcloud");
    ptz_viewer->addCoordinateSystem(1.0);

    //*************************************************************************
    //立方体
    //*************************************************************************
    pcl::CropBox<pcl::PointXYZ> cb;

    cb.setInputCloud(cloud);
    cb.setMin(Eigen::Vector4f(-90, -70, 400, 1));
    cb.setMax(Eigen::Vector4f(130, 150, 600, 1));
    //std::cout<<cb.getMin()<<std::endl;
    //std::cout << cb.getMax() << std::endl;
    cb.setNegative(false);  //保留立方体内的数据
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_CB_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    cb.filter(*cloud_CB_filtered);  //执行滤波处理,存储输出的对象
    //std::cout << cloud_CB_filtered->points.size() << std::endl;
    //pcl::io::savePCDFile("D:/My Learning/C++/PrectiseofC++/49.PCL_Filtering/Appendix/CropBox.pcd", *cloud_CB_filtered);

    //cropbox代码未调试通,滤波后点云大小为0,相同参数下cloudcompare可执行


    //*************************************************************************
    //封闭曲线
    //*************************************************************************
    pcl::CropHull<pcl::PointXYZ> ch;

    //定义2D平面点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr Vertex_Point(new pcl::PointCloud<pcl::PointXYZ>);
    Vertex_Point->push_back(pcl::PointXYZ(34, 116, 470));   //所设定的z值平面必须有点才行
    Vertex_Point->push_back(pcl::PointXYZ(-48, 77, 470));
    Vertex_Point->push_back(pcl::PointXYZ(15, 1, 470));
    Vertex_Point->push_back(pcl::PointXYZ(102, 64, 470));
    Vertex_Point->push_back(pcl::PointXYZ(-29, 110, 470));
    Vertex_Point->push_back(pcl::PointXYZ(100, 1, 470));

    //======凸包封闭曲面========
    //创建凸包对象
    pcl::ConvexHull<pcl::PointXYZ> convexhul;  //凸包的构造使用convexhull类
    convexhul.setInputCloud(Vertex_Point);
    convexhul.setDimension(2); //设置凸包的维度
    std::vector<pcl::Vertices> polygons;    //设置vertices类型的向量,用于保存凸包顶点
    pcl::PointCloud<pcl::PointXYZ>::Ptr suhull(new pcl::PointCloud<pcl::PointXYZ>);     //该点云用于描述凸包形状
    convexhul.reconstruct(*suhull, polygons);  //计算2D凸包结果

    ch.setInputCloud(cloud);
    ch.setDim(2);   //设置维度
    ch.setHullIndices(polygons);    //设置封闭多边形的顶点
    ch.setHullCloud(suhull);    //设置封闭多边形的形状
    ch.setNegative(false);      //保留曲线内的
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_CH_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    ch.filter(*cloud_CH_filtered);

    pcl::io::savePCDFile("D:/My Learning/C++/PrectiseofC++/49.PCL_Filtering/Appendix/CropHull_ConvexHull.pcd", *cloud_CH_filtered);

    //显示
    boost::shared_ptr<pcl::visualization::PCLVisualizer> ch_viewer(new pcl::visualization::PCLVisualizer("ch_filter"));
    ch_viewer->setBackgroundColor(0, 0, 0);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> ch_color(cloud_CH_filtered, 255, 255, 0);
    ch_viewer->addPointCloud<pcl::PointXYZ>(cloud_CH_filtered, ch_color, "chfilterpointcloud");
    ch_viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "chfilterpointcloud");
    ch_viewer->addCoordinateSystem(1.0);

    //*************************************************************************
    //Clipper3D
    //*************************************************************************
    pcl::Clipper3D<pcl::PointXYZ>;
    //有待补齐

    //*************************************************************************
    //BoxClipper3D
    //*************************************************************************
    pcl::BoxClipper3D<pcl::PointXYZ>;
    //有待补齐

    //*************************************************************************
    //按照索引提取点云
    //*************************************************************************
     //----先计算索引---
    pcl::PointIndices poin;   //点云索引index数组
    Eigen::Vector4f centroid_in;

    pcl::compute3DCentroid(*cloud, centroid_in); //计算点云重心(质心)
    for (size_t i = 0; i < cloud->points.size(); i++)
    {
        //提取y轴坐标小于重心点的点云索引
        if (cloud->points[i].y < centroid_in[1])
        {
            poin.indices.push_back(i);
        }
    }

    pcl::IndicesPtr index_ptr(new std::vector<int>(poin.indices)); //将自定义的pi数组进行智能指针转换

    pcl::ExtractIndices<pcl::PointXYZ> ei;  //点云提取算子
    ei.setInputCloud(cloud);
    ei.setIndices(index_ptr);   //索引
    ei.setNegative(true);   //true为提取索引外的点,false为提取索引内的点
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_EI_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    ei.filter(*cloud_EI_filtered);

    pcl::io::savePCDFile("D:/My Learning/C++/PrectiseofC++/49.PCL_Filtering/Appendix/ExtractIndices.pcd", *cloud_EI_filtered);

    //显示
    boost::shared_ptr<pcl::visualization::PCLVisualizer> ei_viewer(new pcl::visualization::PCLVisualizer("ei_filter"));
    ei_viewer->setBackgroundColor(0, 0, 0);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> ei_color(cloud_EI_filtered, 255, 255, 0);
    ei_viewer->addPointCloud<pcl::PointXYZ>(cloud_EI_filtered, ei_color, "eifilterpointcloud");
    ei_viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "eifilterpointcloud");
    ei_viewer->addCoordinateSystem(1.0);

    //*************************************************************************
    //滑动最小二乘
    //*************************************************************************
    pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointXYZ> mls;

    mls.setInputCloud(cloud);
    pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree;
    mls.setSearchMethod(kdtree);
    mls.setSearchRadius(3);
    //增采样
    // Upsampling 采样的方法有 DISTINCT_CLOUD, RANDOM_UNIFORM_DENSITY
    mls.setUpsamplingMethod(pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointXYZ>::SAMPLE_LOCAL_PLANE);
    mls.setUpsamplingRadius(0.3);
    mls.setUpsamplingStepSize(0.2);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_MLS_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    mls.process(*cloud_MLS_filtered);

    pcl::io::savePCDFile("D:/My Learning/C++/PrectiseofC++/49.PCL_Filtering/Appendix/MovingLeastSquares.pcd", *cloud_MLS_filtered);

    //显示
    boost::shared_ptr<pcl::visualization::PCLVisualizer> mls_viewer(new pcl::visualization::PCLVisualizer("mls_filter"));
    mls_viewer->setBackgroundColor(0, 0, 0);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> mls_color(cloud_MLS_filtered, 255, 105, 180);
    mls_viewer->addPointCloud<pcl::PointXYZ>(cloud_MLS_filtered, mls_color, "mlsfilterpointcloud");
    mls_viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "mlsfilterpointcloud");
    mls_viewer->addCoordinateSystem(1.0);

    //*************************************************************************
    //参数化模型投影
//***************************************************************************
    //定义模型:平面ax+by+cz+d = 0,令a = 0,b = 0,c = 1,d = 0,即xy平面
    pcl::ModelCoefficients::Ptr mc(new pcl::ModelCoefficients());

    mc->values.resize(4);// 参数的系数个数
    mc->values[0] = mc->values[1] = 0;
    mc->values[2] = 1.0;
    mc->values[3] = 0;

    pcl::ProjectInliers<pcl::PointXYZ> pi;

    pi.setModelType(pcl::SACMODEL_PLANE);       //模型为平面
    //pi.setModelType(pcl::SACMODEL_CIRCLE3D);  //模型为3D的圆
    //pi.setModelType(pcl::SACMODEL_CYLINDER);  //模型为圆柱
    pi.setInputCloud(cloud);
    pi.setModelCoefficients(mc);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_PI_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    pi.filter(*cloud_PI_filtered);  //执行滤波处理,存储输出的对象

    pcl::io::savePCDFile("D:/My Learning/C++/PrectiseofC++/49.PCL_Filtering/Appendix/ProjectInliers.pcd", *cloud_PI_filtered);

    //显示
    boost::shared_ptr<pcl::visualization::PCLVisualizer> pi_viewer(new pcl::visualization::PCLVisualizer("pi_filter"));
    pi_viewer->setBackgroundColor(0, 0, 0);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> pi_color(cloud_PI_filtered, 0, 0, 255);
    pi_viewer->addPointCloud<pcl::PointXYZ>(cloud_PI_filtered, pi_color, "pifilterpointcloud");
    pi_viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "pifilterpointcloud");
    pi_viewer->addCoordinateSystem(1.0);

    //*************************************************************************
    //双边滤波
    //*************************************************************************

    /*
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr graycloud(new pcl::PointCloud<pcl::PointXYZRGB>);

    if (pcl::io::loadPCDFile <pcl::PointXYZRGB>("D:/My Learning/C++/PrectiseofC++/49.PCL_Filtering/Appendix/gray.pcd", *graycloud) == -1)
    {
        PCL_ERROR("Couldn't read file.\n"); //文件不存在时,返回错误,终止程序。
        return (-1);
    }
    else
    {
        std::cout << "Load PointCloud Down!\n";
    }

    pcl::PointCloud<pcl::PointXYZI>::Ptr Icloud(new pcl::PointCloud<pcl::PointXYZI>);
    Icloud->width = graycloud->width;
    Icloud->height = graycloud->height;
    Icloud->points.resize(Icloud->width * Icloud->height);

    for (size_t i = 0; i < Icloud->points.size(); ++i)
    {
        Icloud->points[i].x = graycloud->points[i].x;
        Icloud->points[i].y = graycloud->points[i].y;
        Icloud->points[i].z = graycloud->points[i].z;
        Icloud->points[i].intensity = graycloud->points[i].r;
    }
    pcl::io::savePCDFile("D:/pinMy Learning/C++/PrectiseofC++/49.PCL_Filtering/Appendix/IntensityCloud.pcd", *Icloud);

    float sigma_s = 1.0;
    float sigma_r = 1.0;
    pcl::search::KdTree<pcl::PointXYZI>::Ptr kdtree_bf(new pcl::search::KdTree<pcl::PointXYZI>);
    pcl::BilateralFilter<pcl::PointXYZI> bf;
    bf.setInputCloud(Icloud);
    bf.setSearchMethod(kdtree_bf);
    bf.setHalfSize(sigma_s);
    bf.setStdDev(sigma_r);
    pcl::PointCloud<pcl::PointXYZI> cloud_BF_filtered;
    bf.filter(cloud_BF_filtered);

    pcl::io::savePCDFile("D:/My Learning/C++/PrectiseofC++/49.PCL_Filtering/Appendix/BilateralFilter.pcd", cloud_BF_filtered);
    */
    //双边滤波需要的时间太长,不适宜处理大量点云



    //***************************************************************************
    //快速双边滤波
    //***************************************************************************
    pcl::FastBilateralFilter<pcl::PointXYZRGB> fbf;

    //此算法只适用有序点云,暂时不做验证




    //*************************************************************************
    //卷积滤波
    //*************************************************************************
    pcl::filters::Convolution<pcl::PointXYZ, pcl::PointXYZ>;
    //待补充



    //*************************************************************************
    //高斯滤波
    //*************************************************************************
    //待补充
    pcl::filters::GaussianKernel<pcl::PointXYZ, pcl::PointXYZ> kernel;
    kernel.setSigma(0.2); // 设置标准差  
    kernel.setThresholdRelativeToSigma(3.0); // 设置关联作用域(可类比领域半径)  

    // 创建KD树用于邻域搜索  
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
    tree->setInputCloud(cloud);

    // 创建卷积滤波器  
    pcl::filters::Convolution3D<pcl::PointXYZ, pcl::PointXYZ, pcl::filters::GaussianKernel<pcl::PointXYZ, pcl::PointXYZ>> filters;
    filters.setKernel(kernel);
    filters.setInputCloud(cloud);
    filters.setSearchMethod(tree);
    filters.setRadiusSearch(2); // 设置搜索半径  

    // 执行滤波操作  
    filters.convolve(*cloud);



    //*************************************************************************
    //中值滤波
    //*************************************************************************
    int k = 4;

    // 创建一个KdTree用于搜索邻居  
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
    tree->setInputCloud(cloud);


   
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZ>);

    // 输出点云初始化为与输入点云相同的大小  
    cloud_out->resize(cloud->points.size());
    cloud_out->is_dense = cloud->is_dense;
    cloud_out->width = cloud->width;
    cloud_out->height = cloud->height;

// 为每个点找到k个最近的邻居,并计算Z值的中值  
for (size_t i = 0; i < cloud->points.size(); ++i) {
    std::vector<int> pointIdxNKNSearch(k);
    std::vector<float> pointNKNSquaredDistance(k);
    
    // 搜索k个最近的邻居  
    if (tree->nearestKSearch(i, k, pointIdxNKNSearch, pointNKNSquaredDistance) > 0) {
        std::vector<float> z_values(k);
        for (int j = 0; j < k; ++j) {
            z_values[j] = cloud->points[pointIdxNKNSearch[j]].z;
        }

        // 对Z值进行排序并找到中值  
        std::sort(z_values.begin(), z_values.end());
        float median_z = z_values[k / 2];
        if (k % 2 == 0) { // 如果k是偶数,则取中间两个数的平均值作为中值  
            median_z = (z_values[k / 2 - 1] + z_values[k / 2]) / 2.0f;
        }

        // 设置输出点云的Z值为中值,X和Y值保持不变  
        cloud_out->points[i].x = cloud->points[i].x;
        cloud_out->points[i].y = cloud->points[i].y;
        cloud_out->points[i].z = median_z;

        std::cout << "Load  !\n";
    }
    else {
        // 如果没有找到足够的邻居,则复制原始点的值  
        cloud_out->points[i] = cloud->points[i];
        std::cout << "Load  Down!\n";
    }
    }



    //*************************************************************************
    //条件滤波
    //*************************************************************************
    //保留符合条件的点云

    pcl::ConditionalRemoval<pcl::PointXYZ> cr;      //条件滤波器(算子)

    pcl::ConditionAnd<pcl::PointXYZ>::Ptr condition_and(new pcl::ConditionAnd<pcl::PointXYZ>);      //“与”条件类型
    //给与类型添加条件算子,这里是比较,对Z轴进行处理
    condition_and->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(new pcl::FieldComparison<pcl::PointXYZ>("z", pcl::ComparisonOps::LT, 470.0)));   //小于1.0
    condition_and->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(new pcl::FieldComparison<pcl::PointXYZ>("z", pcl::ComparisonOps::GT, 0.0)));   //大于470
    //对X轴进行处理
    condition_and->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(new pcl::FieldComparison<pcl::PointXYZ>("x", pcl::ComparisonOps::GE, -34.0)));    //大于等于
    condition_and->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(new pcl::FieldComparison<pcl::PointXYZ>("x", pcl::ComparisonOps::LE, 47.0)));    //小于等于

    cr.setInputCloud(cloud);
    cr.setCondition(condition_and);
    //cr.setKeepOrganized(true);    //保持点云有序性
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_CR_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    cr.filter(*cloud_CR_filtered);

    pcl::io::savePCDFile("D:/My Learning/C++/PrectiseofC++/49.PCL_Filtering/Appendix/ConditionalRemoval.pcd", *cloud_CR_filtered);

    pcl::ConditionOr<pcl::PointXYZ>::Ptr condition_or(new pcl::ConditionOr<pcl::PointXYZ>);     //“或”条件类型
    //“或”条件类型和“与”条件类型处理手段相同
     
    //显示
    boost::shared_ptr<pcl::visualization::PCLVisualizer> cr_viewer(new pcl::visualization::PCLVisualizer("cr_filter"));
    cr_viewer->setBackgroundColor(0, 0, 0);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cr_color(cloud_CR_filtered, 0, 0, 255);
    cr_viewer->addPointCloud<pcl::PointXYZ>(cloud_CR_filtered, cr_color, "crfilterpointcloud");
    cr_viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "crfilterpointcloud");
    cr_viewer->addCoordinateSystem(1.0);




    //原始点云
    while (!Viewer->wasStopped())
    {
        Viewer->spin();
    }
    //统计滤波
    while (!sor_viewer->wasStopped())
    {
        sor_viewer->spin();
    }
    //半径滤波
    while (!ror_viewer->wasStopped())
    {
        ror_viewer->spin();
    }
    //体素网格滤波
    while (!vg_viewer->wasStopped())
    {
        vg_viewer->spin();
    }
    //均匀采样
    while (!us_viewer->wasStopped())
    {
        us_viewer->spin();
    }
    //直通x
    while (!ptx_viewer->wasStopped())
    {
        ptx_viewer->spin();
    }
    //直通z
    while (!ptz_viewer->wasStopped())
    {
        ptz_viewer->spin();
    }
    //封闭曲线
    while (!ch_viewer->wasStopped())
    {
        ch_viewer->spin();
    }
    //根据索引提取点云
    while (!ch_viewer->wasStopped())
    {
        ch_viewer->spin();
    }
    //滑动最小二乘
    while (!mls_viewer->wasStopped())
    {
        mls_viewer->spin();
    }
    //参数化模型投影
    while (!pi_viewer->wasStopped())
    {
        pi_viewer->spin();
    }
    //条件滤波
    while (!cr_viewer->wasStopped())
    {
        cr_viewer->spin();
    }

    return(0);
}

实用例程

提取点云中的平面点云
  • 根据索引提取点云
  • 现利用参数化模型找到平面点云
  • 提取索引
  • 提取点云
// PlaneExtraction.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
//
#include <iostream>

//PCL
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h> 
#include <pcl/kdtree/kdtree.h>
#include <pcl/features/normal_3d.h>

int main()
{
    std::cout << "Hello PineLiu!\n";

	//*****************************************Load PointCloud********************************

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

	if (pcl::io::loadPCDFile <pcl::PointXYZ>("D:/My Learning/C++/PrectiseofC++/50.PlaneExtraction/Appendix/PointCloudPre.pcd", *cloud) == -1)
	{
		PCL_ERROR("Couldn't read file.\n"); //文件不存在时,返回错误,终止程序。
		return (-1);
	}
	else
	{
		std::cout << "Load PointCloud Down!\n";
	}

	
//***************************************************************************
	// 直接提取平面
	//**************************************************************************

	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); //模型
	pcl::PointIndices::Ptr inliers(new pcl::PointIndices); // 索引
	//创建分割对象
	pcl::SACSegmentation<pcl::PointXYZ> seg;

	seg.setModelType(pcl::SACMODEL_PLANE);
	seg.setMethodType(pcl::SAC_RANSAC);
	seg.setMaxIterations(40);
	seg.setDistanceThreshold(6);
	seg.setInputCloud(cloud);
	seg.segment(*inliers, *coefficients);

	//按索引提取点云
	pcl::ExtractIndices<pcl::PointXYZ> extract;
	extract.setInputCloud(cloud);
	extract.setIndices(inliers);
	extract.setNegative(false);

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_extraction(new pcl::PointCloud<pcl::PointXYZ>);
	extract.filter(*cloud_extraction);

	pcl::io::savePCDFile("D:/My Learning/C++/PrectiseofC++/50.PlaneExtraction/Appendix/cloud_extraction.pcd", *cloud_extraction);
	
    //********************************************************************************
    // 借助法向量提取平面
    //********************************************************************************

	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal;	//建立法向量算子
	normal.setInputCloud(cloud);
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
	normal.setSearchMethod(tree);
	pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);	//开辟法向量空间
	normal.setRadiusSearch(2);
	normal.compute(*normals);


	pcl::ModelCoefficients::Ptr coefficients_normal(new pcl::ModelCoefficients); //模型
	pcl::PointIndices::Ptr inliers_normal(new pcl::PointIndices); // 索引
	//创建分割对象
	pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> seg_normal;

	seg_normal.setModelType(pcl::SACMODEL_NORMAL_PLANE);
	seg_normal.setNormalDistanceWeight(0.4);
	seg_normal.setMethodType(pcl::SAC_LMEDS);
	seg_normal.setMaxIterations(40);
	seg_normal.setDistanceThreshold(12);
	seg_normal.setInputCloud(cloud);
	seg_normal.setInputNormals(normals);
	seg_normal.segment(*inliers_normal, *coefficients_normal);

	//按索引提取点云
	pcl::ExtractIndices<pcl::PointXYZ> extract_normal;
	extract_normal.setInputCloud(cloud);
	extract_normal.setIndices(inliers_normal);
	extract_normal.setNegative(false);

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_normalextraction(new pcl::PointCloud<pcl::PointXYZ>);
	extract_normal.filter(*cloud_normalextraction);

	pcl::io::savePCDFile("D:/My Learning/C++/PrectiseofC++/50.PlaneExtraction/Appendix/cloud_normalextraction.pcd", *cloud_normalextraction);

}

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值