该节点会订阅"/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
....
原始点云:
降采样后的点云: