moving_least_square_smoothing_nodelet.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2015, JSK Lab
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/o2r other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the JSK Lab nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 #define BOOST_PARAMETER_MAX_ARITY 7
00036 #include "jsk_pcl_ros/moving_least_square_smoothing.h"
00037 
00038 #include <geometry_msgs/PoseStamped.h>
00039 #include <geometry_msgs/PointStamped.h>
00040 
00041 #include "jsk_recognition_utils/pcl_conversion_util.h"
00042 
00043 namespace jsk_pcl_ros
00044 {
00045   void MovingLeastSquareSmoothing::smooth(const sensor_msgs::PointCloud2ConstPtr& input)
00046   {
00047     boost::mutex::scoped_lock lock(mutex_);
00048     pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
00049     pcl::PointCloud<pcl::PointXYZRGB>::Ptr result_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
00050     pcl::fromROSMsg(*input, *cloud);
00051 
00052     std::vector<int> indices;
00053     cloud->is_dense = false;
00054     pcl::removeNaNFromPointCloud(*cloud, *cloud, indices);
00055 
00056     pcl::MovingLeastSquares<pcl::PointXYZRGB, pcl::PointXYZRGB> smoother;
00057     smoother.setSearchRadius (search_radius_);
00058     if (gauss_param_set_) smoother.setSqrGaussParam (gauss_param_set_);
00059     smoother.setPolynomialFit (use_polynomial_fit_);
00060     smoother.setPolynomialOrder (polynomial_order_);
00061     smoother.setComputeNormals (calc_normal_);
00062 
00063     typename pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new typename pcl::search::KdTree<pcl::PointXYZRGB> ());
00064     smoother.setSearchMethod (tree);
00065     smoother.setInputCloud (cloud);
00066     smoother.process (*result_cloud);
00067 
00068     sensor_msgs::PointCloud2 pointcloud2;
00069     pcl::toROSMsg(*result_cloud, pointcloud2);
00070     pointcloud2.header.frame_id = input->header.frame_id;
00071     pointcloud2.header.stamp = input->header.stamp;
00072     pub_.publish(pointcloud2);
00073   }
00074 
00075   void MovingLeastSquareSmoothing::subscribe()
00076   {
00077     sub_input_ = pnh_->subscribe("input", 1, &MovingLeastSquareSmoothing::smooth, this);
00078   }
00079 
00080   void MovingLeastSquareSmoothing::unsubscribe()
00081   {
00082     sub_input_.shutdown();
00083   }
00084 
00085   void MovingLeastSquareSmoothing::configCallback(Config &config, uint32_t level)
00086   {
00087     boost::mutex::scoped_lock lock(mutex_);
00088     search_radius_ = config.search_radius;
00089     gauss_param_set_ = config.gauss_param_set;
00090     use_polynomial_fit_ = config.use_polynomial_fit;
00091     polynomial_order_ = config.polynomial_order;
00092     calc_normal_ = config.calc_normal;
00093   }
00094 
00095   void MovingLeastSquareSmoothing::onInit(void)
00096   {
00097     DiagnosticNodelet::onInit();
00098     pnh_->param("gauss_param_set", gauss_param_set_, false);
00099     pnh_->param("search_radius", search_radius_, 0.03);
00100     pnh_->param("use_polynomial_fit", use_polynomial_fit_, false);
00101     pnh_->param("polynomial_order", polynomial_order_, 2);
00102     pnh_->param("calc_normal", calc_normal_, true);
00103     srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> >(*pnh_);
00104     dynamic_reconfigure::Server<Config>::CallbackType f =
00105       boost::bind(&MovingLeastSquareSmoothing::configCallback, this, _1, _2);
00106     srv_->setCallback(f);
00107     pub_ =advertise<sensor_msgs::PointCloud2>(*pnh_, "output", 1);
00108     onInitPostProcess();
00109   }
00110 }
00111 
00112 #include <pluginlib/class_list_macros.h>
00113 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::MovingLeastSquareSmoothing, nodelet::Nodelet);


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Sun Oct 8 2017 02:43:50