$search
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: feature.h 35422 2011-01-24 20:04:44Z rusu $ 00035 * 00036 */ 00037 00038 #ifndef PCL_ROS_FEATURE_H_ 00039 #define PCL_ROS_FEATURE_H_ 00040 00041 // PCL includes 00042 #include <pcl/features/feature.h> 00043 #include <pcl/PointIndices.h> 00044 00045 #include "pcl_ros/pcl_nodelet.h" 00046 #include <message_filters/pass_through.h> 00047 00048 // Dynamic reconfigure 00049 #include <dynamic_reconfigure/server.h> 00050 #include "pcl_ros/FeatureConfig.h" 00051 00052 namespace pcl_ros 00053 { 00054 namespace sync_policies = message_filters::sync_policies; 00055 00059 00063 class Feature : public PCLNodelet 00064 { 00065 public: 00066 typedef pcl::KdTree<pcl::PointXYZ> KdTree; 00067 typedef pcl::KdTree<pcl::PointXYZ>::Ptr KdTreePtr; 00068 00069 typedef pcl::PointCloud<pcl::PointXYZ> PointCloudIn; 00070 typedef PointCloudIn::Ptr PointCloudInPtr; 00071 typedef PointCloudIn::ConstPtr PointCloudInConstPtr; 00072 00073 typedef boost::shared_ptr <std::vector<int> > IndicesPtr; 00074 typedef boost::shared_ptr <const std::vector<int> > IndicesConstPtr; 00075 00077 Feature () : /*input_(), indices_(), surface_(), */tree_(), k_(0), search_radius_(0), 00078 use_surface_(false), spatial_locator_type_(-1) 00079 {}; 00080 00081 protected: 00083 //PointCloudInConstPtr input_; 00084 00086 //IndicesConstPtr indices_; 00087 00089 //PointCloudInConstPtr surface_; 00090 00092 KdTreePtr tree_; 00093 00095 int k_; 00096 00098 double search_radius_; 00099 00100 // ROS nodelet attributes 00102 message_filters::Subscriber<PointCloudIn> sub_surface_filter_; 00103 00105 ros::Subscriber sub_input_; 00106 00108 bool use_surface_; 00109 00115 int spatial_locator_type_; 00116 00118 boost::shared_ptr <dynamic_reconfigure::Server<FeatureConfig> > srv_; 00119 00121 virtual bool childInit (ros::NodeHandle &nh) = 0; 00122 00124 virtual void emptyPublish (const PointCloudInConstPtr &cloud) = 0; 00125 00127 virtual void computePublish (const PointCloudInConstPtr &cloud, 00128 const PointCloudInConstPtr &surface, 00129 const IndicesPtr &indices) = 0; 00130 00135 void config_callback (FeatureConfig &config, uint32_t level); 00136 00139 message_filters::PassThrough<PointIndices> nf_pi_; 00140 message_filters::PassThrough<PointCloudIn> nf_pc_; 00141 00146 inline void 00147 input_callback (const PointCloudInConstPtr &input) 00148 { 00149 PointIndices indices; 00150 indices.header.stamp = input->header.stamp; 00151 PointCloudIn cloud; 00152 cloud.header.stamp = input->header.stamp; 00153 nf_pc_.add (cloud.makeShared ()); 00154 nf_pi_.add (boost::make_shared<PointIndices> (indices)); 00155 } 00156 00157 private: 00159 boost::shared_ptr <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn, PointCloudIn, PointIndices> > > sync_input_surface_indices_a_; 00160 boost::shared_ptr <message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, PointCloudIn, PointIndices> > > sync_input_surface_indices_e_; 00161 00163 virtual void onInit (); 00164 00170 void input_surface_indices_callback (const PointCloudInConstPtr &cloud, 00171 const PointCloudInConstPtr &cloud_surface, 00172 const PointIndicesConstPtr &indices); 00173 00174 public: 00175 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00176 }; 00177 00181 class FeatureFromNormals : public Feature 00182 { 00183 public: 00184 typedef sensor_msgs::PointCloud2 PointCloud2; 00185 00186 typedef pcl::PointCloud<pcl::Normal> PointCloudN; 00187 typedef PointCloudN::Ptr PointCloudNPtr; 00188 typedef PointCloudN::ConstPtr PointCloudNConstPtr; 00189 00190 FeatureFromNormals () : normals_() {}; 00191 00192 protected: 00194 PointCloudNConstPtr normals_; 00195 00197 virtual bool childInit (ros::NodeHandle &nh) = 0; 00198 00200 virtual void emptyPublish (const PointCloudInConstPtr &cloud) = 0; 00201 00203 virtual void computePublish (const PointCloudInConstPtr &cloud, 00204 const PointCloudNConstPtr &normals, 00205 const PointCloudInConstPtr &surface, 00206 const IndicesPtr &indices) = 0; 00207 00208 private: 00210 message_filters::Subscriber<PointCloudN> sub_normals_filter_; 00211 00213 boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn, PointCloudN, PointCloudIn, PointIndices> > > sync_input_normals_surface_indices_a_; 00214 boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, PointCloudN, PointCloudIn, PointIndices> > > sync_input_normals_surface_indices_e_; 00215 00217 void computePublish (const PointCloudInConstPtr &, 00218 const PointCloudInConstPtr &, 00219 const IndicesPtr &) {} // This should never be called 00220 00222 virtual void onInit (); 00223 00230 void input_normals_surface_indices_callback (const PointCloudInConstPtr &cloud, 00231 const PointCloudNConstPtr &cloud_normals, 00232 const PointCloudInConstPtr &cloud_surface, 00233 const PointIndicesConstPtr &indices); 00234 00235 public: 00236 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00237 }; 00238 } 00239 00240 #endif //#ifndef PCL_ROS_FEATURE_H_