00001 #include <ros/ros.h> 00002 #include <pcl_ros/point_cloud.h> 00003 #include <pcl/point_types.h> 00004 00005 #include <iostream> 00006 #include "pcl/io/pcd_io.h" 00007 #include "pcl/filters/statistical_outlier_removal.h" 00008 00009 typedef pcl::PointXYZRGB PointType; 00010 typedef pcl::PointCloud<PointType> PointCloud; 00011 00012 ros::Publisher pub; 00013 00014 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>); 00015 pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor; 00016 00017 void callback(const PointCloud::ConstPtr& cloud) 00018 { 00019 sor.setInputCloud (cloud); 00020 sor.setMeanK (50); 00021 sor.setStddevMulThresh (1.0); 00022 sor.filter (*cloud_filtered); 00023 00024 pub.publish (cloud_filtered); 00025 } 00026 00027 int main(int argc, char** argv) 00028 { 00029 ros::init (argc, argv, "outlier_default"); 00030 00031 ros::NodeHandle nh = ros::NodeHandle(ros::this_node::getName()); 00032 std::string in, out; 00033 nh.param<std::string>("in", in, "outlier_filer_in"); 00034 nh.param<std::string>("out", out, "outlier_filer_out"); 00035 00036 ros::NodeHandle nx; 00037 ros::Subscriber sub = nx.subscribe<PointCloud>(in, 1000, callback); 00038 pub = nx.advertise<PointCloud> (out, 1000); 00039 00040 ros::spin(); 00041 00042 return (0); 00043 } 00044 00045 00046