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_msgs/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
00053 #include <pcl_conversions/pcl_conversions.h>
00054
00055 namespace pcl_ros
00056 {
00057 namespace sync_policies = message_filters::sync_policies;
00058
00062
00066 class Feature : public PCLNodelet
00067 {
00068 public:
00069 typedef pcl::KdTree<pcl::PointXYZ> KdTree;
00070 typedef pcl::KdTree<pcl::PointXYZ>::Ptr KdTreePtr;
00071
00072 typedef pcl::PointCloud<pcl::PointXYZ> PointCloudIn;
00073 typedef PointCloudIn::Ptr PointCloudInPtr;
00074 typedef PointCloudIn::ConstPtr PointCloudInConstPtr;
00075
00076 typedef boost::shared_ptr <std::vector<int> > IndicesPtr;
00077 typedef boost::shared_ptr <const std::vector<int> > IndicesConstPtr;
00078
00080 Feature () : tree_(), k_(0), search_radius_(0),
00081 use_surface_(false), spatial_locator_type_(-1)
00082 {};
00083
00084 protected:
00086
00087
00089
00090
00092
00093
00095 KdTreePtr tree_;
00096
00098 int k_;
00099
00101 double search_radius_;
00102
00103
00105 message_filters::Subscriber<PointCloudIn> sub_surface_filter_;
00106
00108 ros::Subscriber sub_input_;
00109
00111 bool use_surface_;
00112
00118 int spatial_locator_type_;
00119
00121 boost::shared_ptr <dynamic_reconfigure::Server<FeatureConfig> > srv_;
00122
00124 virtual bool childInit (ros::NodeHandle &nh) = 0;
00125
00127 virtual void emptyPublish (const PointCloudInConstPtr &cloud) = 0;
00128
00130 virtual void computePublish (const PointCloudInConstPtr &cloud,
00131 const PointCloudInConstPtr &surface,
00132 const IndicesPtr &indices) = 0;
00133
00138 void config_callback (FeatureConfig &config, uint32_t level);
00139
00142 message_filters::PassThrough<PointIndices> nf_pi_;
00143 message_filters::PassThrough<PointCloudIn> nf_pc_;
00144
00149 inline void
00150 input_callback (const PointCloudInConstPtr &input)
00151 {
00152 PointIndices indices;
00153 indices.header.stamp = pcl_conversions::fromPCL(input->header).stamp;
00154 PointCloudIn cloud;
00155 cloud.header.stamp = input->header.stamp;
00156 nf_pc_.add (cloud.makeShared ());
00157 nf_pi_.add (boost::make_shared<PointIndices> (indices));
00158 }
00159
00160 private:
00162 boost::shared_ptr <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn, PointCloudIn, PointIndices> > > sync_input_surface_indices_a_;
00163 boost::shared_ptr <message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, PointCloudIn, PointIndices> > > sync_input_surface_indices_e_;
00164
00166 virtual void onInit ();
00167
00173 void input_surface_indices_callback (const PointCloudInConstPtr &cloud,
00174 const PointCloudInConstPtr &cloud_surface,
00175 const PointIndicesConstPtr &indices);
00176
00177 public:
00178 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00179 };
00180
00184 class FeatureFromNormals : public Feature
00185 {
00186 public:
00187 typedef sensor_msgs::PointCloud2 PointCloud2;
00188
00189 typedef pcl::PointCloud<pcl::Normal> PointCloudN;
00190 typedef PointCloudN::Ptr PointCloudNPtr;
00191 typedef PointCloudN::ConstPtr PointCloudNConstPtr;
00192
00193 FeatureFromNormals () : normals_() {};
00194
00195 protected:
00197 PointCloudNConstPtr normals_;
00198
00200 virtual bool childInit (ros::NodeHandle &nh) = 0;
00201
00203 virtual void emptyPublish (const PointCloudInConstPtr &cloud) = 0;
00204
00206 virtual void computePublish (const PointCloudInConstPtr &cloud,
00207 const PointCloudNConstPtr &normals,
00208 const PointCloudInConstPtr &surface,
00209 const IndicesPtr &indices) = 0;
00210
00211 private:
00213 message_filters::Subscriber<PointCloudN> sub_normals_filter_;
00214
00216 boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn, PointCloudN, PointCloudIn, PointIndices> > > sync_input_normals_surface_indices_a_;
00217 boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, PointCloudN, PointCloudIn, PointIndices> > > sync_input_normals_surface_indices_e_;
00218
00220 void computePublish (const PointCloudInConstPtr &,
00221 const PointCloudInConstPtr &,
00222 const IndicesPtr &) {}
00223
00225 virtual void onInit ();
00226
00233 void input_normals_surface_indices_callback (const PointCloudInConstPtr &cloud,
00234 const PointCloudNConstPtr &cloud_normals,
00235 const PointCloudInConstPtr &cloud_surface,
00236 const PointIndicesConstPtr &indices);
00237
00238 public:
00239 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00240 };
00241 }
00242
00243 #endif //#ifndef PCL_ROS_FEATURE_H_