38 #ifndef PCL_ROS_FEATURE_H_ 39 #define PCL_ROS_FEATURE_H_ 42 #include <pcl/features/feature.h> 43 #include <pcl_msgs/PointIndices.h> 49 #include <dynamic_reconfigure/server.h> 50 #include "pcl_ros/FeatureConfig.h" 69 typedef pcl::KdTree<pcl::PointXYZ>
KdTree;
127 virtual void emptyPublish (
const PointCloudInConstPtr &cloud) = 0;
131 const PointCloudInConstPtr &surface,
132 const IndicesPtr &indices) = 0;
155 cloud.header.stamp = input->header.stamp;
156 nf_pc_.
add (cloud.makeShared ());
157 nf_pi_.
add (boost::make_shared<PointIndices> (indices));
178 const PointCloudInConstPtr &cloud_surface,
182 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
211 const PointCloudNConstPtr &normals,
242 const PointCloudNConstPtr &cloud_normals,
247 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
251 #endif //#ifndef PCL_ROS_FEATURE_H_ boost::shared_ptr< const std::vector< int > > IndicesConstPtr
virtual void emptyPublish(const PointCloudInConstPtr &cloud)=0
Publish an empty point cloud of the feature output type.
virtual void onInit()
Nodelet initialization routine.
virtual void subscribe()
NodeletLazy connection routine.
PointCloudIn::Ptr PointCloudInPtr
pcl::PointCloud< pcl::Normal > PointCloudN
bool use_surface_
Set to true if the nodelet needs to listen for incoming point clouds representing the search surface...
boost::shared_ptr< message_filters::Synchronizer< sync_policies::ApproximateTime< PointCloudIn, PointCloudIn, PointIndices > > > sync_input_surface_indices_a_
Synchronized input, surface, and point indices.
boost::shared_ptr< message_filters::Synchronizer< sync_policies::ExactTime< PointCloudIn, PointCloudIn, PointIndices > > > sync_input_surface_indices_e_
void config_callback(FeatureConfig &config, uint32_t level)
Dynamic reconfigure callback.
message_filters::PassThrough< PointCloudIn > nf_pc_
PointCloudNConstPtr normals_
A pointer to the input dataset that contains the point normals of the XYZ dataset.
int k_
The number of K nearest neighbors to use for each point.
PointCloudN::ConstPtr PointCloudNConstPtr
void fromPCL(const pcl::uint64_t &pcl_stamp, ros::Time &stamp)
boost::shared_ptr< std::vector< int > > IndicesPtr
double search_radius_
The nearest neighbors search radius for each point.
pcl::KdTree< pcl::PointXYZ >::Ptr KdTreePtr
boost::shared_ptr< message_filters::Synchronizer< sync_policies::ExactTime< PointCloudIn, PointCloudN, PointCloudIn, PointIndices > > > sync_input_normals_surface_indices_e_
Feature()
Empty constructor.
message_filters::Subscriber< PointCloudN > sub_normals_filter_
The normals PointCloud subscriber filter.
void computePublish(const PointCloudInConstPtr &, const PointCloudInConstPtr &, const IndicesPtr &)
Internal method.
PCLNodelet represents the base PCL Nodelet class. All PCL nodelets should inherit from this class...
pcl_msgs::PointIndices PointIndices
boost::shared_ptr< dynamic_reconfigure::Server< FeatureConfig > > srv_
Pointer to a dynamic reconfigure service.
pcl::PointCloud< pcl::PointXYZ > PointCloudIn
void input_surface_indices_callback(const PointCloudInConstPtr &cloud, const PointCloudInConstPtr &cloud_surface, const PointIndicesConstPtr &indices)
Input point cloud callback. Used when use_indices and use_surface are set.
message_filters::PassThrough< PointIndices > nf_pi_
Null passthrough filter, used for pushing empty elements in the synchronizer.
message_filters::Subscriber< PointCloudIn > sub_surface_filter_
The surface PointCloud subscriber filter.
sensor_msgs::PointCloud2 PointCloud2
ros::Subscriber sub_input_
The input PointCloud subscriber.
int spatial_locator_type_
Parameter for the spatial locator tree. By convention, the values represent: 0: ANN (Approximate Near...
void add(const MConstPtr &msg)
virtual void unsubscribe()
PointCloudIn::ConstPtr PointCloudInConstPtr
pcl::KdTree< pcl::PointXYZ > KdTree
KdTreePtr tree_
The input point cloud dataset.
Feature represents the base feature class. Some generic 3D operations that are applicable to all feat...
boost::shared_ptr< message_filters::Synchronizer< sync_policies::ApproximateTime< PointCloudIn, PointCloudN, PointCloudIn, PointIndices > > > sync_input_normals_surface_indices_a_
Synchronized input, normals, surface, and point indices.
virtual void computePublish(const PointCloudInConstPtr &cloud, const PointCloudInConstPtr &surface, const IndicesPtr &indices)=0
Compute the feature and publish it. Internal method.
PointCloudN::Ptr PointCloudNPtr
PointIndices::ConstPtr PointIndicesConstPtr
virtual bool childInit(ros::NodeHandle &nh)=0
Child initialization routine. Internal method.
void input_callback(const PointCloudInConstPtr &input)
Input point cloud callback. Because we want to use the same synchronizer object, we push back empty e...