Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 #include <pcl_ros/publisher.h>
00037 #include "jsk_pcl_ros/bilateral_filter.h"
00038
00039
00040 #include <pcl/filters/bilateral.h>
00041 #include <pcl/filters/fast_bilateral.h>
00042 #include <pcl/filters/fast_bilateral_omp.h>
00043
00044 namespace jsk_pcl_ros
00045 {
00046 void BilateralFilter::onInit()
00047 {
00048 ConnectionBasedNodelet::onInit();
00049 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
00050 typename dynamic_reconfigure::Server<Config>::CallbackType f =
00051 boost::bind (&BilateralFilter::configCallback, this, _1, _2);
00052 srv_->setCallback (f);
00053
00054 pub_ = advertise<sensor_msgs::PointCloud2>(*pnh_, "output", 1);
00055 }
00056
00057 void BilateralFilter::subscribe()
00058 {
00059 sub_ = pnh_->subscribe("input", 1, &BilateralFilter::filter, this);
00060 }
00061
00062 void BilateralFilter::unsubscribe()
00063 {
00064 sub_.shutdown();
00065 }
00066
00067 void BilateralFilter::filter(const sensor_msgs::PointCloud2::ConstPtr& msg)
00068 {
00069 boost::mutex::scoped_lock lock(mutex_);
00070 pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>);
00071 pcl::PointCloud<PointT>::Ptr
00072 output (new pcl::PointCloud<PointT>);
00073 pcl::fromROSMsg(*msg, *cloud);
00074 pcl::FastBilateralFilter<PointT> bilateral;
00075
00076 bilateral.setInputCloud(cloud);
00077 bilateral.setSigmaS(sigma_s_);
00078 bilateral.setSigmaR(sigma_r_);
00079 bilateral.filter(*output);
00080 sensor_msgs::PointCloud2 ros_output;
00081 pcl::toROSMsg(*output, ros_output);
00082 ros_output.header = msg->header;
00083
00084 pub_.publish(ros_output);
00085 }
00086
00087 void BilateralFilter::configCallback(Config &config, uint32_t level)
00088 {
00089 boost::mutex::scoped_lock lock(mutex_);
00090 sigma_s_ = config.sigma_s;
00091 sigma_r_ = config.sigma_r;
00092 }
00093
00094 }
00095
00096 #include <pluginlib/class_list_macros.h>
00097 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::BilateralFilter,
00098 nodelet::Nodelet);