PCL点云库 学习五(直通、体素、离群、投影滤波器)

来自：点云库从入门到精通

1. 直通滤波器：对点云某一维度剔除/提取指定范围的数据

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/filters/passthrough.h>
#include <ctime>

#include <boost/thread/thread.hpp>
#include <pcl/common/common_headers.h>
#include <pcl/common/common_headers.h>
#include <pcl/features/normal_3d.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>
#include <pcl/features/normal_based_signature.h>
using namespace std;
using namespace pcl;

void CreatPoints(PointCloud<PointXYZ>::Ptr cloudPtr ){
    srand(time(0));
    cloudPtr->width=100;
    cloudPtr->height=1;
    cloudPtr->points.resize(cloudPtr->width* cloudPtr->height);
    for(int i=0;i<cloudPtr->size();i++){
        cloudPtr->points[i].x=rand()/(RAND_MAX+1.0f)-0.5;
        cloudPtr->points[i].y=rand()/(RAND_MAX+1.0f)-0.5;
        cloudPtr->points[i].z=rand()/(RAND_MAX+1.0f)-0.5;
    }
}
boost::shared_ptr<pcl::visualization::PCLVisualizer> Simple_viwer(const pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud,const pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud2){
    //创建句柄
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("simple viewer"));
    viewer->initCameraParameters ();
    int v1(0),v2(1);
    viewer->createViewPort(0.0, 0.0, 0.5, 1.0, v1);
    viewer->createViewPort(0.5, 0.0, 1, 1.0, v2);
    //设置背景颜色
    viewer->setBackgroundColor(0,0,0,v1);
    viewer->setBackgroundColor(0,0,0,v2);
    //添加点云
    viewer->addPointCloud<pcl::PointXYZ>(cloud,"all data",v1);
    viewer->addPointCloud<pcl::PointXYZ>(cloud2,"filter data",v2);
    //设置点云属性
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,1,"all data",v1);
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,1,"filter data",v2);
    //显示坐标系
    viewer->addCoordinateSystem(1.0,v1);
    viewer->addCoordinateSystem(1.0,v2);
    //初始化相机位置
    return(viewer);
}
int main(int argc,char *argv){
    PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>());
    PointCloud<PointXYZ>::Ptr cloudout(new PointCloud<PointXYZ>());
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
    PassThrough<PointXYZ> pass;
    CreatPoints(cloud);
    pass.setInputCloud(cloud);               // 设置点云输入
    pass.setFilterFieldName("z");           //指定z轴
    pass.setFilterLimits(0.0,0.2);            //指定范围
    pass.setFilterLimitsNegative (false);    //剔除还是提取     true剔除  false提取
    pass.filter(*cloudout);                 ///执行滤波
    viewer=Simple_viwer(cloud,cloudout);
    while (!viewer->wasStopped ())
    {
        viewer->spinOnce (100);
        boost::this_thread::sleep (boost::posix_time::microseconds (100000));
    }
    return 0;
}

2. 体素滤波：用质心代替指定删格块

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/io/pcd_io.h>
#include <pcl/common/conversions.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>
#include <pcl/features/normal_based_signature.h>
#include <boost/thread/thread.hpp>
using namespace pcl;
using namespace std;
boost::shared_ptr<pcl::visualization::PCLVisualizer> Simple_viwer(const pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud,const pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud2){
    //创建句柄
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("simple viewer"));
    viewer->initCameraParameters ();
    int v1(0),v2(1);
    viewer->createViewPort(0.0, 0.0, 0.5, 1.0, v1);
    viewer->createViewPort(0.5, 0.0, 1, 1.0, v2);
    //设置背景颜色
    viewer->setBackgroundColor(0,0,0,v1);
    viewer->setBackgroundColor(0,0,0,v2);
    //添加点云
    viewer->addPointCloud<pcl::PointXYZ>(cloud,"all data",v1);
    viewer->addPointCloud<pcl::PointXYZ>(cloud2,"filter data",v2);
    //设置点云属性
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,1,"all data",v1);
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,1,"filter data",v2);
    //显示坐标系
    viewer->addCoordinateSystem(1.0,v1);
    viewer->addCoordinateSystem(1.0,v2);
    //初始化相机位置
    return(viewer);
}
int main(int argc, const char** argv) {
    PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>());    
    PointCloud<PointXYZ>::Ptr cloudout(new PointCloud<PointXYZ>());    
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
    pcl::PCDReader read;
    read.read("/home/n1/notes/pcl/voxelgrid/output1.pcd",*cloud);    
    std::cout<<"初始大小："<<cloud->size()<< std::endl;
    pcl::VoxelGrid<PointXYZ> Vox;
    Vox.setInputCloud(cloud);
    Vox.setLeafSize(0.1,0.1,0.1);
    Vox.filter(*cloudout);
    std::cout<<"滤波后"<<cloudout->size()<< std::endl;
    pcl::PCDWriter write;write.writeBinary("/home/n1/notes/pcl/voxelgrid/out.pcd",*cloudout);
    viewer=Simple_viwer(cloud,cloudout);
    while (!viewer->wasStopped ())
    {
        viewer->spinOnce (100);
        boost::this_thread::sleep (boost::posix_time::microseconds (100000));
    }
    //保存，初始位姿，初始姿态,是否2进制保存
    return 0;
}

3. 离群点滤波：移除与局部方差过大的点

    PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>());
    PointCloud<PointXYZ>::Ptr cloudout(new PointCloud<PointXYZ>());
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("simple viewer"));
    PCDReader rd;
    rd.read<PointXYZ>("/home/n1/notes/pcl/outlier_remove/test.pcd",*cloud);

    StatisticalOutlierRemoval<PointXYZ> SOR;//  初始化对像
    SOR.setInputCloud(cloud);
    SOR.setMeanK(50);               ///最临近距离求取均值点的个数
    SOR.setStddevMulThresh(1);       //设置阈值 阈值=均值+设置系数*标准差 小于阈值为内点，大于阈值为离群点
    SOR.filter(*cloudout);
    //可视化
    viewer=Simple_viwer(cloud,cloudout);
    while (!viewer->wasStopped ())
    {
        viewer->spinOnce (100);
        boost::this_thread::sleep (boost::posix_time::microseconds (100000));
    }

4. 投影：将点投影到线、平面、求等

/**
 * 投影到一个平面，平面方程为ax+by+cz+d=0; 
 */
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/visualization/pcl_visualizer.h>
using namespace pcl;
using namespace std;
void create_points(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud){
    srand((unsigned int)time(0));
    cloud->width=20;
    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);
    }
}
int main(int argc, const char** argv) {
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudout (new pcl::PointCloud<pcl::PointXYZ>);
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new 				
    pcl::visualization::PCLVisualizer("simple viewer"));
     create_points(cloud);
    //创建平面系数 a=b=d=0, c=1, X，Y构成的平面 
    pcl::ModelCoefficients::Ptr coeff(new pcl::ModelCoefficients());
    coeff->values.resize(4);
    coeff->values[0]=0;
    coeff->values[1]=0;
    coeff->values[2]=1;
    coeff->values[3]=1;

    //滤波器
    pcl::ProjectInliers<pcl::PointXYZ> PROJ;
    PROJ.setModelType(pcl::SACMODEL_PLANE);     //设置模式
    PROJ.setInputCloud(cloud);                  //输入点云
    PROJ.setModelCoefficients(coeff);           //对应模型的系数
    PROJ.filter(*cloudout);
    viewer=Simple_viwer(cloud,cloudout);
    //可视化