Bootstrap

【点云处理】使用pcl内置的Voxel Grid Filter对原始点云进行降采样

        该节点会订阅"/livox/lidar"话题的点云数据,使用PCL内置的Voxel Grid Filter对点云数据进行降采样,过滤后的结果发布到"filtered_points"上。

 

#include <iostream>
#include <thread>
#include <sstream>

#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/common/common.h>
#include <pcl/common/transforms.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/people/person_cluster.h>
#include <pcl/filters/voxel_grid.h>

#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <functional>


int main(int argc,char** argv)
{
    ros::init(argc,argv,"n_lidar_voxel_grid_filter");
    ros::NodeHandle node;
    ros::Subscriber pointcloud_sub;
    ros::Publisher pointcloud_pub = node.advertise<sensor_msgs::PointCloud2>("filtered_points",1);

    std::function<void(pcl::PointCloud<pcl::PointXYZ>::Ptr pc)> filter_process = [&](pcl::PointCloud<pcl::PointXYZ>::Ptr pc)
    {
        {
            pcl::VoxelGrid<pcl::PointXYZ> vg;
            pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_pc_ptr(new pcl::PointCloud<pcl::PointXYZ>);

            vg.setInputCloud(pc);
            vg.setLeafSize(0.2f, 0.2f, 0.2f); //0.2m
            //vg.setLeafSize(0.5f, 0.5f, 0.5f); //0.5m
            vg.filter(*filtered_pc_ptr);

            sensor_msgs::PointCloud2 adjust_msg;
            pcl::toROSMsg(*filtered_pc_ptr, adjust_msg);
            adjust_msg.header.frame_id = "livox_frame";
            pointcloud_pub.publish(adjust_msg);
        }
    };

    {
        const boost::function<void (const boost::shared_ptr<sensor_msgs::PointCloud2 const>&)> callback= [&](sensor_msgs::PointCloud2::ConstPtr pointloud_ptr){
            pcl::PointCloud<pcl::PointXYZ>::Ptr pc(new pcl::PointCloud<pcl::PointXYZ>());
            pcl::fromROSMsg(*pointloud_ptr,*pc);
            filter_process(pc);
        };
        pointcloud_sub = node.subscribe("/livox/lidar",1,callback);
    }
    ros::spin();
    return 0;
}

        Voxel Grid Filter对原始点云进行降采样,只需要定义pcl::VocelGrid,并且指定输入点云和leaf。上面的代码中Voxel Grid Filter将输入的点云空间使用0.2m*0.2m*0.2m的立方体进行分隔,用小立方体的质心来表示其内部的点云,保留这些点作为降采样的输出。

运行起来:

....

origin width:24000,height:1
filter width:10095,height:1
origin width:24000,height:1
filter width:10233,height:1
origin width:24000,height:1
filter width:10189,height:1
....

原始点云:

降采样后的点云:

;