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
00037
00038 #ifndef PCL_ROS_FEATURE_H_
00039 #define PCL_ROS_FEATURE_H_
00040
00041
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
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 () : tree_(), k_(0), search_radius_(0),
00078 use_surface_(false), spatial_locator_type_(-1)
00079 {};
00080
00081 protected:
00083
00084
00086
00087
00089
00090
00092 KdTreePtr tree_;
00093
00095 int k_;
00096
00098 double search_radius_;
00099
00100
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 &) {}
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_