点云滤波和缩减采样

当尝试处理点云时，可能会遇到两个问题：

• 过多的噪声
• 太大的密度
  前者导致算法错误地解释数据，并导致不正确或不准确的结果；后者则使算法花费很多时间去完成运算。

滤波

创建一个节点，负责过滤pcl-output主题中产生的点云离群值，并且通过pcl_filtered主题将他们发送回来。

源文件pcl_filter.cpp：

#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/filters/statistical_outlier_removal.h>

class cloudHandler
{
public:
    cloudHandler()
    {
        pcl_sub = nh.subscribe("pcl_output", 10, &cloudHandler::cloudCB, this);
        pcl_pub = nh.advertise<sensor_msgs::PointCloud2>("pcl_filtered", 1);
    }

    void cloudCB(const sensor_msgs::PointCloud2& input)
    {
        pcl::PointCloud<pcl::PointXYZ> cloud;
        pcl::PointCloud<pcl::PointXYZ> cloud_filtered;
        sensor_msgs::PointCloud2 output;

        pcl::fromROSMsg(input, cloud);

        pcl::StatisticalOutlierRemoval<pcl::PointXYZ> statFilter;
        statFilter.setInputCloud(cloud.makeShared());
        statFilter.setMeanK(10);
        statFilter.setStddevMulThresh(0.2);
        statFilter.filter(cloud_filtered);

        pcl::toROSMsg(cloud_filtered, output);
        pcl_pub.publish(output);
    }

protected:
    ros::NodeHandle nh;
    ros::Subscriber pcl_sub;
    ros::Publisher pcl_pub;
};

main(int argc, char** argv)
{
    ros::init(argc, argv, "pcl_filter");

    cloudHandler handler;

    ros::spin();

    return 0;
}

为了滤波，需要用到 PCL 提供的统计离群值剔除算法。这个算法执行点云的分析并且能够剔除不满足指定统计特征的点。

本例中的统计特征是处于平均值附近的一个范围内，并剔除那些偏离平均值太多的点。可以通过setMeanK函数设置用来计算平均值的相邻点的数目，可以通过setStddevMulThresh函数设置标准偏差阈值的乘值。

缩减采样

减少点云或其他数据的密度成为缩减采样（downsampling）。缩减采样的目的是提高算法的执行效率。缩减采样算法需要保持点云的基本特性和结构。

示例

用体素栅格滤波器（Voxel Grid Filter）来缩减点云的采样，输入点云为上个示例中滤波后的。

源文件：pcl_downsampling.cpp：

#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/filters/voxel_grid.h>

class cloudHandler
{
public:
    cloudHandler()
    {
        pcl_sub = nh.subscribe("pcl_filtered", 10, &cloudHandler::cloudCB, this);
        pcl_pub = nh.advertise<sensor_msgs::PointCloud2>("pcl_downsampled", 1);
    }

    void cloudCB(const sensor_msgs::PointCloud2 &input)
    {
        pcl::PointCloud<pcl::PointXYZ> cloud;
        pcl::PointCloud<pcl::PointXYZ> cloud_downsampled;
        sensor_msgs::PointCloud2 output;

        pcl::fromROSMsg(input, cloud);

        pcl::VoxelGrid<pcl::PointXYZ> voxelSampler;
        voxelSampler.setInputCloud(cloud.makeShared());
        voxelSampler.setLeafSize(0.01f, 0.01f, 0.01f);
        voxelSampler.filter(cloud_downsampled);

        pcl::toROSMsg(cloud_downsampled, output);
        pcl_pub.publish(output);

    }
protected:
    ros::NodeHandle nh;
    ros::Subscriber pcl_sub;
    ros::Publisher pcl_pub;
};

main(int argc, char **argv)
{
    ros::init(argc, argv, "pcl_downsampling");

    cloudHandler handler;

    ros::spin();

    return 0;
}

体素栅格滤波器算法将点云分解成体素（Voxel），或者是更加精确的3D网格，并且用子云的中心点代替每个体素中包含的所有点。每个体素的大小可以通过setLeafSize设定，并且这将确定点云密度。

运行示例

1.启动roscore

roscore

2.运行pcl_read

rosrun chapter6_tutorials pcl_read

3.运行pcl_filter

rosrun chapter6_tutorials pcl_filter

4.运行downsampling

rosrun chapter6_tutorials pcl_downsampling

5.运行rviz

rviz

原始点云

滤波后的点云

缩减采样后的点云

有个问题是：
滤波后的点云和缩减采样后的点云区别不大，跟书上不一样。。。