sac_segmentation.h
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, Willow Garage, Inc.
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/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. 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  * $Id: sac_segmentation.h 35564 2011-01-27 07:32:12Z rusu $
00035  *
00036  */
00037 
00038 #ifndef PCL_ROS_SAC_SEGMENTATION_H_
00039 #define PCL_ROS_SAC_SEGMENTATION_H_
00040 
00041 #include "pcl_ros/pcl_nodelet.h"
00042 #include <message_filters/pass_through.h>
00043 
00044 // PCL includes
00045 #include <pcl/segmentation/sac_segmentation.h>
00046 
00047 // Dynamic reconfigure
00048 #include <dynamic_reconfigure/server.h>
00049 #include "pcl_ros/SACSegmentationConfig.h"
00050 #include "pcl_ros/SACSegmentationFromNormalsConfig.h"
00051 
00052 namespace pcl_ros
00053 {
00054   namespace sync_policies = message_filters::sync_policies;
00055 
00057 
00061   class SACSegmentation : public PCLNodelet
00062   {
00063     typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
00064     typedef PointCloud::Ptr PointCloudPtr;
00065     typedef PointCloud::ConstPtr PointCloudConstPtr;
00066 
00067     public:
00069       SACSegmentation () : min_inliers_(0) {}
00070 
00074       inline void setInputTFframe (std::string tf_frame) { tf_input_frame_ = tf_frame; }
00075       
00077       inline std::string getInputTFframe () { return (tf_input_frame_); }
00078 
00082       inline void setOutputTFframe (std::string tf_frame) { tf_output_frame_ = tf_frame; }
00083       
00085       inline std::string getOutputTFframe () { return (tf_output_frame_); }
00086 
00087     protected:
00088       // The minimum number of inliers a model must have in order to be considered valid.
00089       int min_inliers_;
00090 
00091       // ROS nodelet attributes
00093       ros::Publisher pub_indices_;
00094 
00096       ros::Publisher pub_model_;
00097 
00099       ros::Subscriber sub_input_;
00100 
00102       boost::shared_ptr<dynamic_reconfigure::Server<SACSegmentationConfig> > srv_;
00103 
00105       std::string tf_input_frame_;
00106       
00108       std::string tf_input_orig_frame_;
00109       
00111       std::string tf_output_frame_;
00112 
00115       message_filters::PassThrough<PointIndices> nf_pi_;
00116 
00118       virtual void onInit ();
00119 
00124       void config_callback (SACSegmentationConfig &config, uint32_t level);
00125 
00130       void input_indices_callback (const PointCloudConstPtr &cloud, const PointIndicesConstPtr &indices);
00131 
00135       PointIndices indices_;
00136 
00140       inline void
00141       indices_callback (const PointIndicesConstPtr &indices)
00142       {
00143         indices_ = *indices;
00144       }
00145 
00149       inline void
00150       input_callback (const PointCloudConstPtr &input)
00151       {
00152         indices_.header = input->header;
00153         PointIndicesConstPtr indices;
00154         indices.reset (new PointIndices (indices_));
00155         nf_pi_.add (indices);
00156       }
00157 
00158     private:
00160       boost::mutex mutex_;
00161 
00163       pcl::SACSegmentation<pcl::PointXYZ> impl_;
00164 
00166       boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointIndices> > >       sync_input_indices_e_;
00167       boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointIndices> > > sync_input_indices_a_;
00168 
00169     public:
00170       EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00171   };
00172 
00174 
00177   class SACSegmentationFromNormals: public SACSegmentation
00178   {
00179     typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
00180     typedef PointCloud::Ptr PointCloudPtr;
00181     typedef PointCloud::ConstPtr PointCloudConstPtr;
00182 
00183     typedef pcl::PointCloud<pcl::Normal> PointCloudN;
00184     typedef PointCloudN::Ptr PointCloudNPtr;
00185     typedef PointCloudN::ConstPtr PointCloudNConstPtr;
00186 
00187     public:
00191       inline void setInputTFframe (std::string tf_frame) { tf_input_frame_ = tf_frame; }
00192       
00194       inline std::string getInputTFframe () { return (tf_input_frame_); }
00195 
00199       inline void setOutputTFframe (std::string tf_frame) { tf_output_frame_ = tf_frame; }
00200       
00202       inline std::string getOutputTFframe () { return (tf_output_frame_); }
00203 
00204     protected:
00205       // ROS nodelet attributes
00207       message_filters::Subscriber<PointCloudN> sub_normals_filter_;
00208 
00210       ros::Subscriber sub_axis_;
00211 
00213       boost::shared_ptr<dynamic_reconfigure::Server<SACSegmentationFromNormalsConfig> > srv_;
00214     
00219       inline void
00220       input_callback (const PointCloudConstPtr &cloud)
00221       {
00222         PointIndices indices;
00223         indices.header.stamp = cloud->header.stamp;
00224         nf_.add (boost::make_shared<PointIndices> (indices));
00225       }
00226 
00229       message_filters::PassThrough<PointIndices> nf_;
00230 
00232       std::string tf_input_frame_;
00234       std::string tf_input_orig_frame_;
00236       std::string tf_output_frame_;
00237 
00239       virtual void onInit ();
00240 
00244       void axis_callback (const pcl::ModelCoefficientsConstPtr &model);
00245 
00250       void config_callback (SACSegmentationFromNormalsConfig &config, uint32_t level);
00251 
00257       void input_normals_indices_callback (const PointCloudConstPtr &cloud, 
00258                                            const PointCloudNConstPtr &cloud_normals, 
00259                                            const PointIndicesConstPtr &indices);
00260    
00261     private:
00263       boost::mutex mutex_;
00264 
00266       pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> impl_;
00267 
00269       boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointCloudN, PointIndices> > > sync_input_normals_indices_a_;
00270       boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloudN, PointIndices> > > sync_input_normals_indices_e_;
00271 
00272     public:
00273       EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00274   };
00275 }
00276 
00277 #endif  //#ifndef PCL_ROS_SAC_SEGMENTATION_H_


pcl_ros
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:22:24