copy features/segmentation/surface from fuerte-devel

pcl_ros/CMakeLists.txt

@@ -18,8 +18,15 @@ include_directories(include)
 link_directories(${catkin_LIBRARY_DIRS} ${Boost_LIBRARY_DIRS} ${Eigen_LIBRARY_DIRS} ${PCL_LIBRARY_DIRS})
 
 # generate the dynamic_reconfigure config file
-generate_dynamic_reconfigure_options(cfg/Filter.cfg
+generate_dynamic_reconfigure_options(cfg/Feature.cfg
+                                     cfg/Filter.cfg
+                                     cfg/EuclideanClusterExtraction.cfg
                                      cfg/ExtractIndices.cfg 
+                                     cfg/ExtractPolygonalPrismData.cfg
+                                     cfg/MLS.cfg
+                                     cfg/SACSegmentation.cfg
+                                     cfg/SACSegmentationFromNormals.cfg
+                                     cfg/SegmentDifferences.cfg 
                                      cfg/StatisticalOutlierRemoval.cfg
                                      cfg/VoxelGrid.cfg
 
@@ -53,6 +60,26 @@ target_link_libraries (pcl_ros_io pcl_ros_tf ${PCL_LIBRARIES} ${Boost_LIBRARIES}
 
 class_loader_hide_library_symbols(pcl_ros_io)
 
+# ---[ PCL ROS - Features
+add_library (pcl_ros_features
+              src/pcl_ros/features/feature.cpp
+              # Compilation is much faster if we include all the following CPP files in feature.cpp
+#              src/pcl_ros/features/boundary.cpp
+#              src/pcl_ros/features/fpfh.cpp
+#              src/pcl_ros/features/fpfh_omp.cpp
+#              src/pcl_ros/features/moment_invariants.cpp
+#              src/pcl_ros/features/normal_3d.cpp
+#              src/pcl_ros/features/normal_3d_omp.cpp
+#              src/pcl_ros/features/pfh.cpp
+#             src/pcl_ros/features/principal_curvatures.cpp
+#              src/pcl_ros/features/vfh.cpp
+              )
+#rosbuild_add_compile_flags (pcl_ros_features ${SSE_FLAGS})
+target_link_libraries (pcl_ros_features ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES})
+
+class_loader_hide_library_symbols(pcl_ros_features)
+
+
 # ---[ PCL ROS - Filters
 add_library (pcl_ros_filters
 	      src/pcl_ros/filters/filter.cpp
@@ -68,6 +95,31 @@ target_link_libraries (pcl_ros_filters pcl_ros_tf ${PCL_LIBRARIES} ${Boost_LIBRA
 
 class_loader_hide_library_symbols(pcl_ros_filters)
 
+# ---[ Point Cloud Library - Segmentation
+add_library (pcl_ros_segmentation
+              src/pcl_ros/segmentation/segmentation.cpp
+              src/pcl_ros/segmentation/segment_differences.cpp
+#              src/pcl_ros/segmentation/extract_clusters.cpp
+              src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp
+              src/pcl_ros/segmentation/sac_segmentation.cpp
+              )
+#rosbuild_add_compile_flags (pcl_ros_segmentation ${SSE_FLAGS})
+target_link_libraries (pcl_ros_segmentation pcl_segmentation pcl_ros_tf ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES})
+
+class_loader_hide_library_symbols(pcl_ros_segmentation)
+
+# ---[ Point Cloud Library - Surface
+add_library (pcl_ros_surface
+              src/pcl_ros/surface/surface.cpp
+              # Compilation is much faster if we include all the following CPP files in surface.cpp
+              src/pcl_ros/surface/convex_hull.cpp
+              #src/pcl_ros/surface/moving_least_squares.cpp
+              )
+#rosbuild_add_compile_flags (pcl_ros_surface ${SSE_FLAGS})
+target_link_libraries (pcl_ros_surface ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES})
+
+class_loader_hide_library_symbols(pcl_ros_surface)
+
 ############ TOOLS
 
 add_executable (pcd_to_pointcloud tools/pcd_to_pointcloud.cpp)
@@ -90,7 +142,7 @@ install(DIRECTORY include/${PROJECT_NAME}/
         DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
 )
 
-install(TARGETS pcl_ros_tf pcl_ros_io pcl_ros_filters pcd_to_pointcloud pointcloud_to_pcd bag_to_pcd convert_pcd_to_image convert_pointcloud_to_image
+install(TARGETS pcl_ros_tf pcl_ros_io pcl_ros_features pcl_ros_filters pcl_ros_surface pcl_ros_segmentation pcd_to_pointcloud pointcloud_to_pcd bag_to_pcd convert_pcd_to_image convert_pointcloud_to_image
         RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
         ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
         LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}

pcl_ros/cfg/EuclideanClusterExtraction.cfg

@@ -0,0 +1,19 @@
+#! /usr/bin/env python
+
+# set up parameters that we care about
+PACKAGE = 'pcl_ros'
+
+import roslib;
+roslib.load_manifest (PACKAGE);
+from dynamic_reconfigure.parameter_generator import *;
+gen = ParameterGenerator ()
+
+# enabling/disabling the unit limits
+# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""):
+gen.add ("cluster_tolerance", double_t, 0, "The spatial tolerance as a measure in the L2 Euclidean space", 0.05, 0.0, 2.0)
+gen.add ("cluster_min_size", int_t, 0, "The minimum number of points that a cluster must contain in order to be accepted", 1, 0, 1000)
+gen.add ("cluster_max_size", int_t, 0, "The maximum number of points that a cluster must contain in order to be accepted", 2147483647, 0, 2147483647)
+gen.add ("max_clusters", int_t, 0, "The maximum number of clusters to extract.", 2147483647, 1, 2147483647)
+
+exit (gen.generate (PACKAGE, "pcl_ros", "EuclideanClusterExtraction"))
+

pcl_ros/cfg/ExtractPolygonalPrismData.cfg

@@ -0,0 +1,15 @@
+#! /usr/bin/env python
+
+# set up parameters that we care about
+PACKAGE = 'pcl_ros'
+
+import roslib; roslib.load_manifest (PACKAGE);
+from dynamic_reconfigure.parameter_generator import *;
+import roslib.packages
+
+gen = ParameterGenerator ()
+# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""):
+gen.add ("height_min", double_t, 0, "The minimum allowed distance to the plane model value a point will be considered from", 0.0, -10.0, 10.0)
+gen.add ("height_max", double_t, 0, "The maximum allowed distance to the plane model value a point will be considered from", 0.5, -10.0, 10.0)
+
+exit (gen.generate (PACKAGE, "pcl_ros", "ExtractPolygonalPrismData"))

pcl_ros/cfg/Feature.cfg

@@ -0,0 +1,19 @@
+#! /usr/bin/env python
+
+# set up parameters that we care about
+PACKAGE = 'pcl_ros'
+
+import roslib;
+roslib.load_manifest (PACKAGE);
+from dynamic_reconfigure.parameter_generator import *;
+gen = ParameterGenerator ()
+
+#enum = gen.enum ([ gen.const("ANN", int_t, 0, "ANN"), gen.const("FLANN", int_t, 1, "FLANN"), gen.const("organized", int_t, 2, "Dense/organized data locator") ], "Set the spatial locator")
+# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""):
+gen.add ("k_search", int_t, 0, "Number of k-nearest neighbors to search for", 10, 0, 1000)
+gen.add ("radius_search", double_t,  0, "Sphere radius for nearest neighbor search",  0.0, 0.0, 0.5)
+#gen.add ("spatial_locator", int_t, 0, "Set the spatial locator", 0, 0, 2, enum)
+#gen.add ("spatial_locator", str_t, 0, "Set the spatial locator", "ANN")
+
+exit (gen.generate (PACKAGE, "pcl_ros", "Feature"))
+

pcl_ros/cfg/MLS.cfg

@@ -0,0 +1,22 @@
+#! /usr/bin/env python
+
+# set up parameters that we care about
+PACKAGE = 'pcl_ros'
+
+import roslib;
+roslib.load_manifest (PACKAGE);
+from dynamic_reconfigure.parameter_generator import *;
+gen = ParameterGenerator ()
+
+enum = gen.enum ([ gen.const("ANN", int_t, 0, "ANN"), gen.const("FLANN", int_t, 1, "FLANN"), gen.const("organized", int_t, 2, "Dense/organized data locator") ], "Set the spatial locator")
+# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""):
+gen.add ("spatial_locator", int_t, 0, "Set the spatial locator", 0, 0, 2, enum)
+gen.add ("search_radius", double_t,  0, "Sphere radius for nearest neighbor search",  0.02, 0.0, 0.5)
+#gen.add ("k_search", int_t, 0, "Number of k-nearest neighbors to search for", 10, 0, 1000)
+gen.add ("use_polynomial_fit", bool_t, 0, "Whether to use polynomial fit", True)
+gen.add ("polynomial_order", int_t, 0, "Set the spatial locator", 2, 0, 5)
+gen.add ("gaussian_parameter", double_t,  0, "How 'flat' should the neighbor weighting gaussian be (the smaller, the more local the fit)",  0.02, 0.0, 0.5)
+#gen.add ("spatial_locator", str_t, 0, "Set the spatial locator", "ANN")
+
+exit (gen.generate (PACKAGE, "pcl_ros", "MLS"))
+

pcl_ros/cfg/SACSegmentation.cfg

@@ -0,0 +1,14 @@
+#! /usr/bin/env python
+
+# set up parameters that we care about
+PACKAGE = 'pcl_ros'
+
+import roslib; roslib.load_manifest (PACKAGE);
+from dynamic_reconfigure.parameter_generator import *;
+import SACSegmentation_common as common
+
+gen = ParameterGenerator ()
+common.add_common_parameters (gen)
+
+exit (gen.generate (PACKAGE, "pcl_ros", "SACSegmentation"))
+

pcl_ros/cfg/SACSegmentationFromNormals.cfg

@@ -0,0 +1,16 @@
+#! /usr/bin/env python
+
+# set up parameters that we care about
+PACKAGE = 'pcl_ros'
+
+import roslib; roslib.load_manifest (PACKAGE);
+from dynamic_reconfigure.parameter_generator import *;
+import roslib.packages
+import SACSegmentation_common as common
+
+gen = ParameterGenerator ()
+# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""):
+gen.add ("normal_distance_weight", double_t, 0, "The relative weight (between 0 and 1) to give to the angular distance (0 to pi/2) between point normals and the plane normal.", 0.1, 0, 1.0)
+common.add_common_parameters (gen)
+
+exit (gen.generate (PACKAGE, "pcl_ros", "SACSegmentationFromNormals"))

pcl_ros/cfg/SACSegmentation_common.py

@@ -0,0 +1,21 @@
+#! /usr/bin/env python
+
+# set up parameters that we care about
+PACKAGE = 'pcl_ros'
+
+import roslib; roslib.load_manifest (PACKAGE);
+from dynamic_reconfigure.parameter_generator import *;
+
+def add_common_parameters (gen):
+  # add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = "")
+  gen.add ("max_iterations", int_t, 0, "The maximum number of iterations the algorithm will run for", 50, 0, 100000)
+  gen.add ("probability", double_t, 0, "The desired probability of choosing at least one sample free from outliers", 0.99, 0.5, 0.99)
+  gen.add ("distance_threshold", double_t, 0, "The distance to model threshold", 0.02, 0, 1.0)
+  gen.add ("optimize_coefficients", bool_t, 0, "Model coefficient refinement", True)
+  gen.add ("radius_min", double_t, 0, "The minimum allowed model radius (where applicable)", 0.0, 0, 1.0)
+  gen.add ("radius_max", double_t, 0, "The maximum allowed model radius (where applicable)", 0.05, 0, 1.0)
+  gen.add ("eps_angle", double_t, 0, "The maximum allowed difference between the model normal and the given axis in radians.", 0.17, 0.0, 1.5707)
+  gen.add ("min_inliers", int_t, 0, "The minimum number of inliers a model must have in order to be considered valid.", 0, 0, 100000)
+  gen.add ("input_frame", str_t, 0, "The input TF frame the data should be transformed into, if input.header.frame_id is different.", "")
+  gen.add ("output_frame", str_t, 0, "The output TF frame the data should be transformed into, if input.header.frame_id is different.", "")
+

pcl_ros/cfg/SegmentDifferences.cfg

@@ -0,0 +1,16 @@
+#! /usr/bin/env python
+
+# set up parameters that we care about
+PACKAGE = 'pcl_ros'
+
+import roslib;
+roslib.load_manifest (PACKAGE);
+from dynamic_reconfigure.parameter_generator import *;
+gen = ParameterGenerator ()
+
+# enabling/disabling the unit limits
+# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""):
+gen.add ("distance_threshold", double_t, 0, "The distance tolerance as a measure in the L2 Euclidean space between corresponding points.", 0.0, 0.0, 2.0)
+
+exit (gen.generate (PACKAGE, "pcl_ros", "SegmentDifferences"))
+

pcl_ros/include/pcl_ros/features/boundary.h

@@ -0,0 +1,86 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: boundary.h 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#ifndef PCL_ROS_BOUNDARY_H_
+#define PCL_ROS_BOUNDARY_H_
+
+#include <pcl/features/boundary.h>
+#include "pcl_ros/features/feature.h"
+
+namespace pcl_ros
+{
+  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+  /** \brief @b BoundaryEstimation estimates whether a set of points is lying on surface boundaries using an angle
+    * criterion. The code makes use of the estimated surface normals at each point in the input dataset.
+    *
+    * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
+    * \a NormalEstimationOpenMP and \a NormalEstimationTBB for examples on how to extend this to parallel implementations.
+    * \author Radu Bogdan Rusu
+    */
+  class BoundaryEstimation: public FeatureFromNormals
+  {
+    private:
+      pcl::BoundaryEstimation<pcl::PointXYZ, pcl::Normal, pcl::Boundary> impl_;
+      
+      typedef pcl::PointCloud<pcl::Boundary> PointCloudOut;
+
+      /** \brief Child initialization routine. Internal method. */
+      inline bool 
+      childInit (ros::NodeHandle &nh)
+      {
+        // Create the output publisher
+        pub_output_ = nh.advertise<PointCloudOut> ("output", max_queue_size_);
+        return (true);
+      }
+
+      /** \brief Publish an empty point cloud of the feature output type. */
+      void emptyPublish (const PointCloudInConstPtr &cloud);
+
+      /** \brief Compute the feature and publish it. */
+      void computePublish (const PointCloudInConstPtr &cloud, 
+                           const PointCloudNConstPtr &normals,
+                           const PointCloudInConstPtr &surface,
+                           const IndicesPtr &indices);
+
+    public:
+      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  };
+}
+
+#endif  //#ifndef PCL_ROS_BOUNDARY_H_
+
+

pcl_ros/include/pcl_ros/features/feature.h

@@ -0,0 +1,240 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: feature.h 35422 2011-01-24 20:04:44Z rusu $
+ *
+ */
+
+#ifndef PCL_ROS_FEATURE_H_
+#define PCL_ROS_FEATURE_H_
+
+// PCL includes
+#include <pcl/features/feature.h>
+#include <pcl/PointIndices.h>
+
+#include "pcl_ros/pcl_nodelet.h"
+#include <message_filters/pass_through.h>
+
+// Dynamic reconfigure
+#include <dynamic_reconfigure/server.h>
+#include "pcl_ros/FeatureConfig.h"
+
+namespace pcl_ros
+{
+  namespace sync_policies = message_filters::sync_policies;
+
+  ///////////////////////////////////////////////////////////////////////////////////////////
+  ///////////////////////////////////////////////////////////////////////////////////////////
+  ///////////////////////////////////////////////////////////////////////////////////////////
+  /** \brief @b Feature represents the base feature class. Some generic 3D operations that 
+    * are applicable to all features are defined here as static methods.
+    * \author Radu Bogdan Rusu
+    */
+  class Feature : public PCLNodelet
+  {
+    public:
+      typedef pcl::KdTree<pcl::PointXYZ> KdTree;
+      typedef pcl::KdTree<pcl::PointXYZ>::Ptr KdTreePtr;
+
+      typedef pcl::PointCloud<pcl::PointXYZ> PointCloudIn;
+      typedef PointCloudIn::Ptr PointCloudInPtr;
+      typedef PointCloudIn::ConstPtr PointCloudInConstPtr;
+
+      typedef boost::shared_ptr <std::vector<int> > IndicesPtr;
+      typedef boost::shared_ptr <const std::vector<int> > IndicesConstPtr;
+
+      /** \brief Empty constructor. */
+      Feature () : /*input_(), indices_(), surface_(), */tree_(), k_(0), search_radius_(0),
+                   use_surface_(false), spatial_locator_type_(-1) 
+      {};
+
+    protected:
+      /** \brief The input point cloud dataset. */
+      //PointCloudInConstPtr input_;
+
+      /** \brief A pointer to the vector of point indices to use. */
+      //IndicesConstPtr indices_;
+
+      /** \brief An input point cloud describing the surface that is to be used for nearest neighbors estimation. */
+      //PointCloudInConstPtr surface_;
+
+      /** \brief A pointer to the spatial search object. */
+      KdTreePtr tree_;
+
+      /** \brief The number of K nearest neighbors to use for each point. */
+      int k_;
+
+      /** \brief The nearest neighbors search radius for each point. */
+      double search_radius_;
+
+      // ROS nodelet attributes
+      /** \brief The surface PointCloud subscriber filter. */
+      message_filters::Subscriber<PointCloudIn> sub_surface_filter_;
+      
+      /** \brief The input PointCloud subscriber. */
+      ros::Subscriber sub_input_;
+
+      /** \brief Set to true if the nodelet needs to listen for incoming point clouds representing the search surface. */
+      bool use_surface_;
+
+      /** \brief Parameter for the spatial locator tree. By convention, the values represent:
+        * 0: ANN (Approximate Nearest Neigbor library) kd-tree
+        * 1: FLANN (Fast Library for Approximate Nearest Neighbors) kd-tree
+        * 2: Organized spatial dataset index
+        */
+      int spatial_locator_type_;
+
+      /** \brief Pointer to a dynamic reconfigure service. */
+      boost::shared_ptr <dynamic_reconfigure::Server<FeatureConfig> > srv_;
+
+      /** \brief Child initialization routine. Internal method. */
+      virtual bool childInit (ros::NodeHandle &nh) = 0;
+
+      /** \brief Publish an empty point cloud of the feature output type. */
+      virtual void emptyPublish (const PointCloudInConstPtr &cloud) = 0;
+
+      /** \brief Compute the feature and publish it. Internal method. */
+      virtual void computePublish (const PointCloudInConstPtr &cloud, 
+                                   const PointCloudInConstPtr &surface,
+                                   const IndicesPtr &indices) = 0;
+
+      /** \brief Dynamic reconfigure callback
+        * \param config the config object
+        * \param level the dynamic reconfigure level
+        */
+      void config_callback (FeatureConfig &config, uint32_t level);
+
+      /** \brief Null passthrough filter, used for pushing empty elements in the 
+        * synchronizer */
+      message_filters::PassThrough<PointIndices> nf_pi_;
+      message_filters::PassThrough<PointCloudIn> nf_pc_;
+
+      /** \brief Input point cloud callback.
+        * Because we want to use the same synchronizer object, we push back
+        * empty elements with the same timestamp.
+        */
+      inline void
+      input_callback (const PointCloudInConstPtr &input)
+      {
+        PointIndices indices;
+        indices.header.stamp = input->header.stamp;
+        PointCloudIn cloud;
+        cloud.header.stamp = input->header.stamp;
+        nf_pc_.add (cloud.makeShared ());
+        nf_pi_.add (boost::make_shared<PointIndices> (indices));
+      }
+
+    private:
+      /** \brief Synchronized input, surface, and point indices.*/
+      boost::shared_ptr <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn, PointCloudIn, PointIndices> > > sync_input_surface_indices_a_;
+      boost::shared_ptr <message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, PointCloudIn, PointIndices> > > sync_input_surface_indices_e_;
+
+      /** \brief Nodelet initialization routine. */
+      virtual void onInit ();
+
+      /** \brief Input point cloud callback. Used when \a use_indices and \a use_surface are set.
+        * \param cloud the pointer to the input point cloud
+        * \param cloud_surface the pointer to the surface point cloud
+        * \param indices the pointer to the input point cloud indices
+        */
+      void input_surface_indices_callback (const PointCloudInConstPtr &cloud, 
+                                           const PointCloudInConstPtr &cloud_surface, 
+                                           const PointIndicesConstPtr &indices);
+
+    public:
+      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  };
+
+  //////////////////////////////////////////////////////////////////////////////////////////
+  //////////////////////////////////////////////////////////////////////////////////////////
+  //////////////////////////////////////////////////////////////////////////////////////////
+  class FeatureFromNormals : public Feature
+  {
+    public:
+      typedef sensor_msgs::PointCloud2 PointCloud2;
+
+      typedef pcl::PointCloud<pcl::Normal> PointCloudN;
+      typedef PointCloudN::Ptr PointCloudNPtr;
+      typedef PointCloudN::ConstPtr PointCloudNConstPtr;
+
+      FeatureFromNormals () : normals_() {};
+
+    protected:
+      /** \brief A pointer to the input dataset that contains the point normals of the XYZ dataset. */
+      PointCloudNConstPtr normals_;
+
+      /** \brief Child initialization routine. Internal method. */
+      virtual bool childInit (ros::NodeHandle &nh) = 0;
+
+      /** \brief Publish an empty point cloud of the feature output type. */
+      virtual void emptyPublish (const PointCloudInConstPtr &cloud) = 0;
+
+      /** \brief Compute the feature and publish it. */
+      virtual void computePublish (const PointCloudInConstPtr &cloud, 
+                                   const PointCloudNConstPtr &normals,
+                                   const PointCloudInConstPtr &surface,
+                                   const IndicesPtr &indices) = 0;
+
+    private:
+      /** \brief The normals PointCloud subscriber filter. */
+      message_filters::Subscriber<PointCloudN> sub_normals_filter_;
+
+      /** \brief Synchronized input, normals, surface, and point indices.*/
+      boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn, PointCloudN, PointCloudIn, PointIndices> > > sync_input_normals_surface_indices_a_;
+      boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, PointCloudN, PointCloudIn, PointIndices> > > sync_input_normals_surface_indices_e_;
+
+      /** \brief Internal method. */
+      void computePublish (const PointCloudInConstPtr &, 
+                           const PointCloudInConstPtr &,
+                           const IndicesPtr &) {} // This should never be called
+
+      /** \brief Nodelet initialization routine. */
+      virtual void onInit ();
+
+      /** \brief Input point cloud callback. Used when \a use_indices and \a use_surface are set.
+        * \param cloud the pointer to the input point cloud
+        * \param cloud_normals the pointer to the input point cloud normals
+        * \param cloud_surface the pointer to the surface point cloud
+        * \param indices the pointer to the input point cloud indices
+        */
+      void input_normals_surface_indices_callback (const PointCloudInConstPtr &cloud, 
+                                                   const PointCloudNConstPtr &cloud_normals, 
+                                                   const PointCloudInConstPtr &cloud_surface, 
+                                                   const PointIndicesConstPtr &indices);
+
+    public:
+      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  };
+}
+
+#endif  //#ifndef PCL_ROS_FEATURE_H_

pcl_ros/include/pcl_ros/features/fpfh.h

@@ -0,0 +1,100 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: fpfh.h 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#ifndef PCL_ROS_FPFH_H_
+#define PCL_ROS_FPFH_H_
+
+#include <pcl/features/fpfh.h>
+#include "pcl_ros/features/pfh.h"
+
+namespace pcl_ros
+{
+  /** \brief @b FPFHEstimation estimates the <b>Fast Point Feature Histogram (FPFH)</b> descriptor for a given point cloud
+    * dataset containing points and normals.
+    *
+    * @note If you use this code in any academic work, please cite:
+    *
+    * <ul>
+    * <li> R.B. Rusu, N. Blodow, M. Beetz.
+    *      Fast Point Feature Histograms (FPFH) for 3D Registration.
+    *      In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA),
+    *      Kobe, Japan, May 12-17 2009.
+    * </li>
+    * <li> R.B. Rusu, A. Holzbach, N. Blodow, M. Beetz.
+    *      Fast Geometric Point Labeling using Conditional Random Fields.
+    *      In Proceedings of the 22nd IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS),
+    *      St. Louis, MO, USA, October 11-15 2009.
+    * </li>
+    * </ul>
+    *
+    * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
+    * \a FPFHEstimationOpenMP for examples on parallel implementations of the FPFH (Fast Point Feature Histogram).
+    * \author Radu Bogdan Rusu
+    */
+  class FPFHEstimation : public FeatureFromNormals
+  {
+    private:
+      pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> impl_;
+
+      typedef pcl::PointCloud<pcl::FPFHSignature33> PointCloudOut;
+
+      /** \brief Child initialization routine. Internal method. */
+      inline bool 
+      childInit (ros::NodeHandle &nh)
+      {
+        // Create the output publisher
+        pub_output_ = nh.advertise<PointCloudOut> ("output", max_queue_size_);
+        return (true);
+      }
+
+      /** \brief Publish an empty point cloud of the feature output type. */
+      void emptyPublish (const PointCloudInConstPtr &cloud);
+
+      /** \brief Compute the feature and publish it. */
+      void computePublish (const PointCloudInConstPtr &cloud,
+                           const PointCloudNConstPtr &normals,
+                           const PointCloudInConstPtr &surface,
+                           const IndicesPtr &indices);
+
+    public:
+      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  };
+}
+
+#endif  //#ifndef PCL_FPFH_H_
+
+

pcl_ros/include/pcl_ros/features/fpfh_omp.h

@@ -0,0 +1,96 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: fpfh_omp.h 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#ifndef PCL_ROS_FPFH_OMP_H_
+#define PCL_ROS_FPFH_OMP_H_
+
+#include <pcl/features/fpfh_omp.h>
+#include "pcl_ros/features/fpfh.h"
+
+namespace pcl_ros
+{
+  /** \brief @b FPFHEstimationOMP estimates the Fast Point Feature Histogram (FPFH) descriptor for a given point cloud
+    * dataset containing points and normals, in parallel, using the OpenMP standard.
+    *
+    * @note If you use this code in any academic work, please cite:
+    *
+    * <ul>
+    * <li> R.B. Rusu, N. Blodow, M. Beetz.
+    *      Fast Point Feature Histograms (FPFH) for 3D Registration.
+    *      In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA),
+    *      Kobe, Japan, May 12-17 2009.
+    * </li>
+    * <li> R.B. Rusu, A. Holzbach, N. Blodow, M. Beetz.
+    *      Fast Geometric Point Labeling using Conditional Random Fields.
+    *      In Proceedings of the 22nd IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS),
+    *      St. Louis, MO, USA, October 11-15 2009.
+    * </li>
+    * </ul>
+    * \author Radu Bogdan Rusu
+    */
+  class FPFHEstimationOMP : public FeatureFromNormals
+  {
+    private:
+      pcl::FPFHEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> impl_;
+
+      typedef pcl::PointCloud<pcl::FPFHSignature33> PointCloudOut;
+
+      /** \brief Child initialization routine. Internal method. */
+      inline bool 
+      childInit (ros::NodeHandle &nh)
+      {
+        // Create the output publisher
+        pub_output_ = nh.advertise<PointCloudOut> ("output", max_queue_size_);
+        return (true);
+      }
+
+      /** \brief Publish an empty point cloud of the feature output type. */
+      void emptyPublish (const PointCloudInConstPtr &cloud);
+
+      /** \brief Compute the feature and publish it. */
+      void computePublish (const PointCloudInConstPtr &cloud,
+                           const PointCloudNConstPtr &normals,
+                           const PointCloudInConstPtr &surface,
+                           const IndicesPtr &indices);
+
+    public:
+      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  };
+}
+
+#endif  //#ifndef PCL_ROS_FPFH_OMP_H_
+

pcl_ros/include/pcl_ros/features/moment_invariants.h

@@ -0,0 +1,83 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: moment_invariants.h 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#ifndef PCL_ROS_MOMENT_INVARIANTS_H_
+#define PCL_ROS_MOMENT_INVARIANTS_H_
+
+#include <pcl/features/moment_invariants.h>
+#include "pcl_ros/features/feature.h"
+
+namespace pcl_ros
+{
+  /** \brief MomentInvariantsEstimation estimates the 3 moment invariants (j1, j2, j3) at each 3D point.
+    *
+    * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
+    * \a NormalEstimationOpenMP and \a NormalEstimationTBB for examples on how to extend this to parallel implementations.
+    * \author Radu Bogdan Rusu
+    */
+  class MomentInvariantsEstimation: public Feature
+  {
+    private:
+      pcl::MomentInvariantsEstimation<pcl::PointXYZ, pcl::MomentInvariants> impl_;
+
+      typedef pcl::PointCloud<pcl::MomentInvariants> PointCloudOut;
+
+      /** \brief Child initialization routine. Internal method. */
+      inline bool 
+      childInit (ros::NodeHandle &nh)
+      {
+        // Create the output publisher
+        pub_output_ = nh.advertise<PointCloudOut> ("output", max_queue_size_);
+        return (true);
+      }
+
+      /** \brief Publish an empty point cloud of the feature output type. */
+      void emptyPublish (const PointCloudInConstPtr &cloud);
+
+      /** \brief Compute the feature and publish it. */
+      void computePublish (const PointCloudInConstPtr &cloud,
+                           const PointCloudInConstPtr &surface,
+                           const IndicesPtr &indices);
+
+    public:
+      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  };
+}
+
+#endif  //#ifndef PCL_ROS_MOMENT_INVARIANTS_H_
+
+

pcl_ros/include/pcl_ros/features/normal_3d.h

@@ -0,0 +1,86 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: normal_3d.h 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#ifndef PCL_ROS_NORMAL_3D_H_
+#define PCL_ROS_NORMAL_3D_H_
+
+#include <pcl/features/normal_3d.h>
+#include "pcl_ros/features/feature.h"
+
+namespace pcl_ros
+{
+  /** \brief @b NormalEstimation estimates local surface properties at each 3D point, such as surface normals and
+    * curvatures.
+    *
+    * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
+    * \a NormalEstimationOpenMP and \a NormalEstimationTBB for parallel implementations.
+    * \author Radu Bogdan Rusu
+    */
+  class NormalEstimation: public Feature
+  {
+    private:
+      /** \brief PCL implementation object. */
+      pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> impl_;
+
+      typedef pcl::PointCloud<pcl::Normal> PointCloudOut;
+
+      /** \brief Child initialization routine. Internal method. */
+      inline bool 
+      childInit (ros::NodeHandle &nh)
+      {
+        // Create the output publisher
+        pub_output_ = nh.advertise<PointCloudOut> ("output", max_queue_size_);
+        return (true);
+      }
+
+      /** \brief Publish an empty point cloud of the feature output type.
+        * \param cloud the input point cloud to copy the header from.
+        */ 
+      void emptyPublish (const PointCloudInConstPtr &cloud);
+
+      /** \brief Compute the feature and publish it. */
+      void computePublish (const PointCloudInConstPtr &cloud,
+                           const PointCloudInConstPtr &surface,
+                           const IndicesPtr &indices);
+
+    public:
+      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  };
+}
+
+#endif  //#ifndef PCL_ROS_NORMAL_3D_H_
+

pcl_ros/include/pcl_ros/features/normal_3d_omp.h

@@ -0,0 +1,79 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: normal_3d_omp.h 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#ifndef PCL_ROS_NORMAL_3D_OMP_H_
+#define PCL_ROS_NORMAL_3D_OMP_H_
+
+#include <pcl/features/normal_3d_omp.h>
+#include "pcl_ros/features/normal_3d.h"
+
+namespace pcl_ros
+{
+  /** \brief @b NormalEstimationOMP estimates local surface properties at each 3D point, such as surface normals and
+    * curvatures, in parallel, using the OpenMP standard.
+    * \author Radu Bogdan Rusu
+    */
+  class NormalEstimationOMP: public Feature
+  {
+    private:
+      pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> impl_;
+
+      typedef pcl::PointCloud<pcl::Normal> PointCloudOut;
+
+      /** \brief Child initialization routine. Internal method. */
+      inline bool 
+      childInit (ros::NodeHandle &nh)
+      {
+        // Create the output publisher
+        pub_output_ = nh.advertise<PointCloudOut> ("output", max_queue_size_);
+        return (true);
+      }
+
+      /** \brief Publish an empty point cloud of the feature output type. */
+      void emptyPublish (const PointCloudInConstPtr &cloud);
+
+      /** \brief Compute the feature and publish it. */
+      void computePublish (const PointCloudInConstPtr &cloud,
+                           const PointCloudInConstPtr &surface,
+                           const IndicesPtr &indices);
+
+    public:
+      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  };
+}
+
+#endif  //#ifndef PCL_ROS_NORMAL_3D_OMP_H_

pcl_ros/include/pcl_ros/features/normal_3d_tbb.h

@@ -0,0 +1,86 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: normal_3d_tbb.h 35661 2011-02-01 06:04:14Z rusu $
+ *
+ */
+
+#ifndef PCL_ROS_NORMAL_3D_TBB_H_
+#define PCL_ROS_NORMAL_3D_TBB_H_
+
+//#include "pcl_ros/pcl_ros_config.h"
+//#if defined(HAVE_TBB)
+
+#include <pcl/features/normal_3d_tbb.h>
+#include "pcl_ros/features/normal_3d.h"
+
+namespace pcl_ros
+{
+  /** \brief @b NormalEstimationTBB estimates local surface properties at each 3D point, such as surface normals and
+    * curvatures, in parallel, using Intel's Threading Building Blocks library.
+    * \author Radu Bogdan Rusu
+    */
+  class NormalEstimationTBB: public Feature
+  {
+    private:
+      pcl::NormalEstimationTBB<pcl::PointXYZ, pcl::Normal> impl_;
+
+      typedef pcl::PointCloud<pcl::Normal> PointCloudOut;
+
+      /** \brief Child initialization routine. Internal method. */
+      inline bool 
+      childInit (ros::NodeHandle &nh)
+      {
+        // Create the output publisher
+        pub_output_ = nh.advertise<PointCloud> ("output", max_queue_size_);
+        return (true);
+      }
+
+      /** \brief Publish an empty point cloud of the feature output type. */
+      void emptyPublish (const PointCloudInConstPtr &cloud);
+
+      /** \brief Compute the feature and publish it. */
+      void computePublish (const PointCloudInConstPtr &cloud,
+                           const PointCloudInConstPtr &surface,
+                           const IndicesPtr &indices);
+
+    public:
+      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  };
+}
+
+//#endif  // HAVE_TBB
+
+#endif  //#ifndef PCL_ROS_NORMAL_3D_TBB_H_
+
+

pcl_ros/include/pcl_ros/features/pfh.h

@@ -0,0 +1,100 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: pfh.h 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#ifndef PCL_ROS_PFH_H_
+#define PCL_ROS_PFH_H_
+
+#include <pcl/features/pfh.h>
+#include "pcl_ros/features/feature.h"
+
+namespace pcl_ros
+{
+  /** \brief @b PFHEstimation estimates the Point Feature Histogram (PFH) descriptor for a given point cloud dataset
+    * containing points and normals.
+    *
+    * @note If you use this code in any academic work, please cite:
+    *
+    * <ul>
+    * <li> R.B. Rusu, N. Blodow, Z.C. Marton, M. Beetz.
+    *      Aligning Point Cloud Views using Persistent Feature Histograms.
+    *      In Proceedings of the 21st IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS),
+    *      Nice, France, September 22-26 2008.
+    * </li>
+    * <li> R.B. Rusu, Z.C. Marton, N. Blodow, M. Beetz.
+    *      Learning Informative Point Classes for the Acquisition of Object Model Maps.
+    *      In Proceedings of the 10th International Conference on Control, Automation, Robotics and Vision (ICARCV),
+    *      Hanoi, Vietnam, December 17-20 2008.
+    * </li>
+    * </ul>
+    *
+    * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
+    * \a FPFHEstimationOpenMP for examples on parallel implementations of the FPFH (Fast Point Feature Histogram).
+    * \author Radu Bogdan Rusu
+    */
+  class PFHEstimation : public FeatureFromNormals
+  {
+    private:
+      pcl::PFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::PFHSignature125> impl_;
+
+      typedef pcl::PointCloud<pcl::PFHSignature125> PointCloudOut;
+
+      /** \brief Child initialization routine. Internal method. */
+      inline bool 
+      childInit (ros::NodeHandle &nh)
+      {
+        // Create the output publisher
+        pub_output_ = nh.advertise<PointCloudOut> ("output", max_queue_size_);
+        return (true);
+      }
+
+      /** \brief Publish an empty point cloud of the feature output type. */
+      void emptyPublish (const PointCloudInConstPtr &cloud);
+
+      /** \brief Compute the feature and publish it. */
+      void computePublish (const PointCloudInConstPtr &cloud,
+                           const PointCloudNConstPtr &normals,
+                           const PointCloudInConstPtr &surface,
+                           const IndicesPtr &indices);
+
+    public:
+      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  };
+}
+
+#endif  //#ifndef PCL_ROS_PFH_H_
+
+

pcl_ros/include/pcl_ros/features/principal_curvatures.h

@@ -0,0 +1,83 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: principal_curvatures.h 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#ifndef PCL_ROS_PRINCIPAL_CURVATURES_H_
+#define PCL_ROS_PRINCIPAL_CURVATURES_H_
+
+#include <pcl/features/principal_curvatures.h>
+#include "pcl_ros/features/feature.h"
+
+namespace pcl_ros
+{
+  /** \brief @b PrincipalCurvaturesEstimation estimates the directions (eigenvectors) and magnitudes (eigenvalues) of
+    * principal surface curvatures for a given point cloud dataset containing points and normals.
+    *
+    * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
+    * \a NormalEstimationOpenMP and \a NormalEstimationTBB for examples on how to extend this to parallel implementations.
+    * \author Radu Bogdan Rusu, Jared Glover
+    */
+  class PrincipalCurvaturesEstimation : public FeatureFromNormals
+  {
+    private:
+      pcl::PrincipalCurvaturesEstimation<pcl::PointXYZ, pcl::Normal, pcl::PrincipalCurvatures> impl_;
+
+      typedef pcl::PointCloud<pcl::PrincipalCurvatures> PointCloudOut;
+
+      /** \brief Child initialization routine. Internal method. */
+      inline bool 
+      childInit (ros::NodeHandle &nh)
+      {
+        // Create the output publisher
+        pub_output_ = nh.advertise<PointCloudOut> ("output", max_queue_size_);
+        return (true);
+      }
+
+      /** \brief Publish an empty point cloud of the feature output type. */
+      void emptyPublish (const PointCloudInConstPtr &cloud);
+
+      /** \brief Compute the feature and publish it. */
+      void computePublish (const PointCloudInConstPtr &cloud,
+                           const PointCloudNConstPtr &normals,
+                           const PointCloudInConstPtr &surface,
+                           const IndicesPtr &indices);
+
+    public:
+      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  };
+}
+
+#endif  //#ifndef PCL_ROS_PRINCIPAL_CURVATURES_H_

pcl_ros/include/pcl_ros/features/vfh.h

@@ -0,0 +1,83 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: vfh.h 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#ifndef PCL_ROS_FEATURES_VFH_H_
+#define PCL_ROS_FEATURES_VFH_H_
+
+#include <pcl/features/vfh.h>
+#include "pcl_ros/features/fpfh.h"
+
+namespace pcl_ros
+{
+  /** \brief @b VFHEstimation estimates the <b>Viewpoint Feature Histogram (VFH)</b> descriptor for a given point cloud
+    * dataset containing points and normals.
+    *
+    * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
+    * \a FPFHEstimationOpenMP for examples on parallel implementations of the FPFH (Fast Point Feature Histogram).
+    * \author Radu Bogdan Rusu
+    */
+  class VFHEstimation : public FeatureFromNormals
+  {
+    private:
+      pcl::VFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::VFHSignature308> impl_;
+
+      typedef pcl::PointCloud<pcl::VFHSignature308> PointCloudOut;
+
+      /** \brief Child initialization routine. Internal method. */
+      inline bool 
+      childInit (ros::NodeHandle &nh)
+      {
+        // Create the output publisher
+        pub_output_ = nh.advertise<PointCloudOut> ("output", max_queue_size_);
+        return (true);
+      }
+
+      /** \brief Publish an empty point cloud of the feature output type. */
+      void emptyPublish (const PointCloudInConstPtr &cloud);
+
+      /** \brief Compute the feature and publish it. */
+      void computePublish (const PointCloudInConstPtr &cloud,
+                           const PointCloudNConstPtr &normals,
+                           const PointCloudInConstPtr &surface,
+                           const IndicesPtr &indices);
+
+    public:
+      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  };
+}
+
+#endif  //#ifndef PCL_ROS_FEATURES_VFH_H_

pcl_ros/include/pcl_ros/segmentation/extract_clusters.h

@@ -0,0 +1,105 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2010, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: extract_clusters.h 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#ifndef PCL_ROS_EXTRACT_CLUSTERS_H_
+#define PCL_ROS_EXTRACT_CLUSTERS_H_
+
+#include <pcl/segmentation/extract_clusters.h>
+#include "pcl_ros/pcl_nodelet.h"
+
+// Dynamic reconfigure
+#include <dynamic_reconfigure/server.h>
+#include "pcl_ros/EuclideanClusterExtractionConfig.h"
+
+namespace pcl_ros
+{
+  namespace sync_policies = message_filters::sync_policies;
+
+  ////////////////////////////////////////////////////////////////////////////////////////////
+  ////////////////////////////////////////////////////////////////////////////////////////////
+  /** \brief @b EuclideanClusterExtraction represents a segmentation class for cluster extraction in an Euclidean sense.
+    * \author Radu Bogdan Rusu
+    */
+  class EuclideanClusterExtraction : public PCLNodelet
+  {
+    public:
+      /** \brief Empty constructor. */
+      EuclideanClusterExtraction () : publish_indices_ (false), max_clusters_ (std::numeric_limits<int>::max ()) {};
+                                      
+    protected:
+      // ROS nodelet attributes
+      /** \brief Publish indices or convert to PointCloud clusters. Default: false */
+      bool publish_indices_;
+
+      /** \brief Maximum number of clusters to publish. */
+      int max_clusters_;
+
+      /** \brief Pointer to a dynamic reconfigure service. */
+      boost::shared_ptr<dynamic_reconfigure::Server<EuclideanClusterExtractionConfig> > srv_;
+
+      /** \brief Nodelet initialization routine. */
+      void onInit ();
+
+      /** \brief Dynamic reconfigure callback
+        * \param config the config object
+        * \param level the dynamic reconfigure level
+        */
+      void config_callback (EuclideanClusterExtractionConfig &config, uint32_t level);
+
+      /** \brief Input point cloud callback. 
+        * \param cloud the pointer to the input point cloud
+        * \param indices the pointer to the input point cloud indices
+        */
+      void input_indices_callback (const PointCloudConstPtr &cloud, const PointIndicesConstPtr &indices);
+
+    private:
+      /** \brief The PCL implementation used. */
+      pcl::EuclideanClusterExtraction<pcl::PointXYZ> impl_;
+
+      /** \brief The input PointCloud subscriber. */
+      ros::Subscriber sub_input_;
+
+      /** \brief Synchronized input, and indices.*/
+      boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointIndices> > >       sync_input_indices_e_;
+      boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointIndices> > > sync_input_indices_a_;
+
+    public:
+      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  };
+}
+
+#endif  //#ifndef PCL_ROS_EXTRACT_CLUSTERS_H_

pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.h

@@ -0,0 +1,126 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2010, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: extract_polygonal_prism_data.h 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#ifndef PCL_ROS_EXTRACT_POLYGONAL_PRISM_DATA_H_
+#define PCL_ROS_EXTRACT_POLYGONAL_PRISM_DATA_H_
+
+#include "pcl_ros/pcl_nodelet.h"
+#include <message_filters/sync_policies/exact_time.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/pass_through.h>
+
+// PCL includes
+#include <pcl/segmentation/extract_polygonal_prism_data.h>
+
+// Dynamic reconfigure
+#include <dynamic_reconfigure/server.h>
+#include "pcl_ros/ExtractPolygonalPrismDataConfig.h"
+
+namespace pcl_ros
+{
+  namespace sync_policies = message_filters::sync_policies;
+
+  /** \brief @b ExtractPolygonalPrismData uses a set of point indices that represent a planar model, and together with
+    * a given height, generates a 3D polygonal prism. The polygonal prism is then used to segment all points lying
+    * inside it.
+    *
+    * An example of its usage is to extract the data lying within a set of 3D boundaries (e.g., objects supported by a plane).
+    * \author Radu Bogdan Rusu
+    */
+  class ExtractPolygonalPrismData : public PCLNodelet
+  {
+    typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
+    typedef PointCloud::Ptr PointCloudPtr;
+    typedef PointCloud::ConstPtr PointCloudConstPtr;
+
+    protected:
+       /** \brief The output PointIndices publisher. */
+      ros::Publisher pub_output_;
+
+      /** \brief The message filter subscriber for PointCloud2. */
+      message_filters::Subscriber<PointCloud> sub_hull_filter_;
+
+      /** \brief Synchronized input, planar hull, and indices.*/
+      boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloud, PointIndices> > > sync_input_hull_indices_e_;
+      boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointCloud, PointIndices> > > sync_input_hull_indices_a_;
+
+      /** \brief Pointer to a dynamic reconfigure service. */
+      boost::shared_ptr<dynamic_reconfigure::Server<ExtractPolygonalPrismDataConfig> > srv_;
+
+      /** \brief Null passthrough filter, used for pushing empty elements in the 
+        * synchronizer */
+      message_filters::PassThrough<PointIndices> nf_;
+
+      /** \brief Input point cloud callback.
+        * Because we want to use the same synchronizer object, we push back
+        * empty elements with the same timestamp.
+        */
+      inline void
+      input_callback (const PointCloudConstPtr &input)
+      {
+        PointIndices cloud;
+        cloud.header.stamp = input->header.stamp;
+        nf_.add (boost::make_shared<PointIndices> (cloud));
+      }
+
+       /** \brief Nodelet initialization routine. */
+      void onInit ();
+
+      /** \brief Dynamic reconfigure callback
+        * \param config the config object
+        * \param level the dynamic reconfigure level
+        */
+      void config_callback (ExtractPolygonalPrismDataConfig &config, uint32_t level);
+
+      /** \brief Input point cloud callback. Used when \a use_indices is set.
+        * \param cloud the pointer to the input point cloud
+        * \param hull the pointer to the planar hull point cloud
+        * \param indices the pointer to the input point cloud indices
+        */
+      void input_hull_indices_callback (const PointCloudConstPtr &cloud, 
+                                        const PointCloudConstPtr &hull, 
+                                        const PointIndicesConstPtr &indices);
+
+    private:
+      /** \brief The PCL implementation used. */
+      pcl::ExtractPolygonalPrismData<pcl::PointXYZ> impl_;
+    public:
+      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  };
+}
+
+#endif  //#ifndef PCL_EXTRACT_POLYGONAL_PRISM_DATA_H_

pcl_ros/include/pcl_ros/segmentation/sac_segmentation.h

@@ -0,0 +1,277 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: sac_segmentation.h 35564 2011-01-27 07:32:12Z rusu $
+ *
+ */
+
+#ifndef PCL_ROS_SAC_SEGMENTATION_H_
+#define PCL_ROS_SAC_SEGMENTATION_H_
+
+#include "pcl_ros/pcl_nodelet.h"
+#include <message_filters/pass_through.h>
+
+// PCL includes
+#include <pcl/segmentation/sac_segmentation.h>
+
+// Dynamic reconfigure
+#include <dynamic_reconfigure/server.h>
+#include "pcl_ros/SACSegmentationConfig.h"
+#include "pcl_ros/SACSegmentationFromNormalsConfig.h"
+
+namespace pcl_ros
+{
+  namespace sync_policies = message_filters::sync_policies;
+
+  ////////////////////////////////////////////////////////////////////////////////////////////
+  /** \brief @b SACSegmentation represents the Nodelet segmentation class for Sample Consensus methods and models, in
+    * the sense that it just creates a Nodelet wrapper for generic-purpose SAC-based segmentation.
+    * \author Radu Bogdan Rusu
+    */
+  class SACSegmentation : public PCLNodelet
+  {
+    typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
+    typedef PointCloud::Ptr PointCloudPtr;
+    typedef PointCloud::ConstPtr PointCloudConstPtr;
+
+    public:
+      /** \brief Constructor. */
+      SACSegmentation () : min_inliers_(0) {}
+
+      /** \brief Set the input TF frame the data should be transformed into before processing, if input.header.frame_id is different.
+        * \param tf_frame the TF frame the input PointCloud should be transformed into before processing
+        */
+      inline void setInputTFframe (std::string tf_frame) { tf_input_frame_ = tf_frame; }
+      
+      /** \brief Get the TF frame the input PointCloud should be transformed into before processing. */
+      inline std::string getInputTFframe () { return (tf_input_frame_); }
+
+      /** \brief Set the output TF frame the data should be transformed into after processing.
+        * \param tf_frame the TF frame the PointCloud should be transformed into after processing
+        */
+      inline void setOutputTFframe (std::string tf_frame) { tf_output_frame_ = tf_frame; }
+      
+      /** \brief Get the TF frame the PointCloud should be transformed into after processing. */
+      inline std::string getOutputTFframe () { return (tf_output_frame_); }
+
+    protected:
+      // The minimum number of inliers a model must have in order to be considered valid.
+      int min_inliers_;
+
+      // ROS nodelet attributes
+      /** \brief The output PointIndices publisher. */
+      ros::Publisher pub_indices_;
+
+      /** \brief The output ModelCoefficients publisher. */
+      ros::Publisher pub_model_;
+
+      /** \brief The input PointCloud subscriber. */
+      ros::Subscriber sub_input_;
+
+      /** \brief Pointer to a dynamic reconfigure service. */
+      boost::shared_ptr<dynamic_reconfigure::Server<SACSegmentationConfig> > srv_;
+
+      /** \brief The input TF frame the data should be transformed into, if input.header.frame_id is different. */
+      std::string tf_input_frame_;
+      
+      /** \brief The original data input TF frame. */
+      std::string tf_input_orig_frame_;
+      
+      /** \brief The output TF frame the data should be transformed into, if input.header.frame_id is different. */
+      std::string tf_output_frame_;
+
+      /** \brief Null passthrough filter, used for pushing empty elements in the
+        * synchronizer */
+      message_filters::PassThrough<PointIndices> nf_pi_;
+
+      /** \brief Nodelet initialization routine. */
+      virtual void onInit ();
+
+      /** \brief Dynamic reconfigure callback
+        * \param config the config object
+        * \param level the dynamic reconfigure level
+        */
+      void config_callback (SACSegmentationConfig &config, uint32_t level);
+
+      /** \brief Input point cloud callback. Used when \a use_indices is set.
+        * \param cloud the pointer to the input point cloud
+        * \param indices the pointer to the input point cloud indices
+        */
+      void input_indices_callback (const PointCloudConstPtr &cloud, const PointIndicesConstPtr &indices);
+
+      /** \brief Pointer to a set of indices stored internally.
+       * (used when \a latched_indices_ is set). 
+       */
+      PointIndices indices_;
+
+      /** \brief Indices callback. Used when \a latched_indices_ is set.
+        * \param indices the pointer to the input point cloud indices
+        */
+      inline void
+      indices_callback (const PointIndicesConstPtr &indices)
+      {
+        indices_ = *indices;
+      }
+
+      /** \brief Input callback. Used when \a latched_indices_ is set.
+        * \param input the pointer to the input point cloud
+        */
+      inline void
+      input_callback (const PointCloudConstPtr &input)
+      {
+        indices_.header = input->header;
+        PointIndicesConstPtr indices;
+        indices.reset (new PointIndices (indices_));
+        nf_pi_.add (indices);
+      }
+
+    private:
+      /** \brief Internal mutex. */
+      boost::mutex mutex_;
+
+      /** \brief The PCL implementation used. */
+      pcl::SACSegmentation<pcl::PointXYZ> impl_;
+
+      /** \brief Synchronized input, and indices.*/
+      boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointIndices> > >       sync_input_indices_e_;
+      boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointIndices> > > sync_input_indices_a_;
+
+    public:
+      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  };
+
+  ////////////////////////////////////////////////////////////////////////////////////////////
+  /** \brief @b SACSegmentationFromNormals represents the PCL nodelet segmentation class for Sample Consensus methods and
+    * models that require the use of surface normals for estimation.
+    */
+  class SACSegmentationFromNormals: public SACSegmentation
+  {
+    typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
+    typedef PointCloud::Ptr PointCloudPtr;
+    typedef PointCloud::ConstPtr PointCloudConstPtr;
+
+    typedef pcl::PointCloud<pcl::Normal> PointCloudN;
+    typedef PointCloudN::Ptr PointCloudNPtr;
+    typedef PointCloudN::ConstPtr PointCloudNConstPtr;
+
+    public:
+      /** \brief Set the input TF frame the data should be transformed into before processing, if input.header.frame_id is different.
+        * \param tf_frame the TF frame the input PointCloud should be transformed into before processing
+        */
+      inline void setInputTFframe (std::string tf_frame) { tf_input_frame_ = tf_frame; }
+      
+      /** \brief Get the TF frame the input PointCloud should be transformed into before processing. */
+      inline std::string getInputTFframe () { return (tf_input_frame_); }
+
+      /** \brief Set the output TF frame the data should be transformed into after processing.
+        * \param tf_frame the TF frame the PointCloud should be transformed into after processing
+        */
+      inline void setOutputTFframe (std::string tf_frame) { tf_output_frame_ = tf_frame; }
+      
+      /** \brief Get the TF frame the PointCloud should be transformed into after processing. */
+      inline std::string getOutputTFframe () { return (tf_output_frame_); }
+
+    protected:
+      // ROS nodelet attributes
+      /** \brief The normals PointCloud subscriber filter. */
+      message_filters::Subscriber<PointCloudN> sub_normals_filter_;
+
+      /** \brief The input PointCloud subscriber. */
+      ros::Subscriber sub_axis_;
+
+      /** \brief Pointer to a dynamic reconfigure service. */
+      boost::shared_ptr<dynamic_reconfigure::Server<SACSegmentationFromNormalsConfig> > srv_;
+    
+      /** \brief Input point cloud callback.
+        * Because we want to use the same synchronizer object, we push back
+        * empty elements with the same timestamp.
+        */
+      inline void
+      input_callback (const PointCloudConstPtr &cloud)
+      {
+        PointIndices indices;
+        indices.header.stamp = cloud->header.stamp;
+        nf_.add (boost::make_shared<PointIndices> (indices));
+      }
+
+      /** \brief Null passthrough filter, used for pushing empty elements in the 
+        * synchronizer */
+      message_filters::PassThrough<PointIndices> nf_;
+
+      /** \brief The input TF frame the data should be transformed into, if input.header.frame_id is different. */
+      std::string tf_input_frame_;
+      /** \brief The original data input TF frame. */
+      std::string tf_input_orig_frame_;
+      /** \brief The output TF frame the data should be transformed into, if input.header.frame_id is different. */
+      std::string tf_output_frame_;
+
+      /** \brief Nodelet initialization routine. */
+      virtual void onInit ();
+
+      /** \brief Model callback
+        * \param model the sample consensus model found
+        */
+      void axis_callback (const pcl::ModelCoefficientsConstPtr &model);
+
+      /** \brief Dynamic reconfigure callback
+        * \param config the config object
+        * \param level the dynamic reconfigure level
+        */
+      void config_callback (SACSegmentationFromNormalsConfig &config, uint32_t level);
+
+      /** \brief Input point cloud callback.
+        * \param cloud the pointer to the input point cloud
+        * \param cloud_normals the pointer to the input point cloud normals
+        * \param indices the pointer to the input point cloud indices
+        */
+      void input_normals_indices_callback (const PointCloudConstPtr &cloud, 
+                                           const PointCloudNConstPtr &cloud_normals, 
+                                           const PointIndicesConstPtr &indices);
+   
+    private:
+      /** \brief Internal mutex. */
+      boost::mutex mutex_;
+
+      /** \brief The PCL implementation used. */
+      pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> impl_;
+
+      /** \brief Synchronized input, normals, and indices.*/
+      boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointCloudN, PointIndices> > > sync_input_normals_indices_a_;
+      boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloudN, PointIndices> > > sync_input_normals_indices_e_;
+
+    public:
+      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  };
+}
+
+#endif  //#ifndef PCL_ROS_SAC_SEGMENTATION_H_

pcl_ros/include/pcl_ros/segmentation/segment_differences.h

@@ -0,0 +1,106 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2010, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: segment_differences.h 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#ifndef PCL_ROS_SEGMENT_DIFFERENCES_H_
+#define PCL_ROS_SEGMENT_DIFFERENCES_H_
+
+#include <pcl/segmentation/segment_differences.h>
+#include "pcl_ros/pcl_nodelet.h"
+
+// Dynamic reconfigure
+#include <dynamic_reconfigure/server.h>
+#include "pcl_ros/SegmentDifferencesConfig.h"
+
+
+namespace pcl_ros
+{
+  namespace sync_policies = message_filters::sync_policies;
+
+  ////////////////////////////////////////////////////////////////////////////////////////////
+  ////////////////////////////////////////////////////////////////////////////////////////////
+  ////////////////////////////////////////////////////////////////////////////////////////////
+  /** \brief @b SegmentDifferences obtains the difference between two spatially aligned point clouds and returns the
+    * difference between them for a maximum given distance threshold.
+    * \author Radu Bogdan Rusu
+    */
+  class SegmentDifferences : public PCLNodelet
+  {
+    typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
+    typedef PointCloud::Ptr PointCloudPtr;
+    typedef PointCloud::ConstPtr PointCloudConstPtr;
+
+    public:
+      /** \brief Empty constructor. */
+      SegmentDifferences () {};
+                                      
+    protected:
+      /** \brief The message filter subscriber for PointCloud2. */
+      message_filters::Subscriber<PointCloud> sub_target_filter_;
+
+      /** \brief Synchronized input, and planar hull.*/
+      boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloud> > > sync_input_target_e_;
+      boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointCloud> > > sync_input_target_a_;
+
+      /** \brief Pointer to a dynamic reconfigure service. */
+      boost::shared_ptr<dynamic_reconfigure::Server<SegmentDifferencesConfig> > srv_;
+
+      /** \brief Nodelet initialization routine. */
+      void onInit ();
+
+      /** \brief Dynamic reconfigure callback
+        * \param config the config object
+        * \param level the dynamic reconfigure level
+        */
+      void config_callback (SegmentDifferencesConfig &config, uint32_t level);
+
+      /** \brief Input point cloud callback.
+        * \param cloud the pointer to the input point cloud
+        * \param cloud_target the pointcloud that we want to segment \a cloud from
+        */
+      void input_target_callback (const PointCloudConstPtr &cloud, 
+                                  const PointCloudConstPtr &cloud_target);
+
+    private:
+      /** \brief The PCL implementation used. */
+      pcl::SegmentDifferences<pcl::PointXYZ> impl_;
+
+    public:
+      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  };
+}
+
+#endif  //#ifndef PCL_ROS_SEGMENT_DIFFERENCES_H_

pcl_ros/include/pcl_ros/surface/convex_hull.h

@@ -0,0 +1,92 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2010, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: convex_hull.h 36116 2011-02-22 00:05:23Z rusu $
+ *
+ */
+
+#ifndef PCL_ROS_CONVEX_HULL_2D_H_
+#define PCL_ROS_CONVEX_HULL_2D_H_
+
+#include "pcl_ros/pcl_nodelet.h"
+
+// PCL includes
+#include <pcl/surface/convex_hull.h>
+
+// Dynamic reconfigure
+#include <dynamic_reconfigure/server.h>
+
+namespace pcl_ros
+{
+  namespace sync_policies = message_filters::sync_policies;
+
+  /** \brief @b ConvexHull2D represents a 2D ConvexHull implementation.
+    * \author Radu Bogdan Rusu
+    */
+  class ConvexHull2D : public PCLNodelet
+  {
+    typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
+    typedef PointCloud::Ptr PointCloudPtr;
+    typedef PointCloud::ConstPtr PointCloudConstPtr;
+
+    private:
+      /** \brief Nodelet initialization routine. */
+      virtual void onInit ();
+
+      /** \brief Input point cloud callback.
+        * \param cloud the pointer to the input point cloud
+        * \param indices the pointer to the input point cloud indices
+        */
+      void input_indices_callback (const PointCloudConstPtr &cloud, 
+                                   const PointIndicesConstPtr &indices);
+
+    private:
+      /** \brief The PCL implementation used. */
+      pcl::ConvexHull<pcl::PointXYZ> impl_;
+
+      /** \brief The input PointCloud subscriber. */
+      ros::Subscriber sub_input_;
+
+      /** \brief Publisher for PolygonStamped. */
+      ros::Publisher pub_plane_;
+
+      /** \brief Synchronized input, and indices.*/
+      boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointIndices> > >       sync_input_indices_e_;
+      boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointIndices> > > sync_input_indices_a_;
+
+  public:
+      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  };
+}
+
+#endif  //#ifndef PCL_ROS_CONVEX_HULL_2D_H_

pcl_ros/include/pcl_ros/surface/moving_least_squares.h

@@ -0,0 +1,145 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2010, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: moving_least_squares.h 36097 2011-02-20 14:18:58Z marton $
+ *
+ */
+
+#ifndef PCL_ROS_MOVING_LEAST_SQUARES_H_
+#define PCL_ROS_MOVING_LEAST_SQUARES_H_
+
+#include "pcl_ros/pcl_nodelet.h"
+
+// PCL includes
+#include <pcl/surface/mls.h>
+
+// Dynamic reconfigure
+#include <dynamic_reconfigure/server.h>
+#include "pcl_ros/MLSConfig.h"   
+
+namespace pcl_ros
+{
+  namespace sync_policies = message_filters::sync_policies;
+
+  /** \brief @b MovingLeastSquares represents a nodelet using the MovingLeastSquares implementation.
+    * The type of the output is the same as the input, it only smooths the XYZ coordinates according to the parameters.
+    * Normals are estimated at each point as well and published on a separate topic.
+    * \author Radu Bogdan Rusu, Zoltan-Csaba Marton
+    */
+  class MovingLeastSquares : public PCLNodelet
+  {
+    typedef pcl::PointXYZ PointIn;
+    typedef pcl::Normal NormalOut;
+
+    typedef pcl::PointCloud<PointIn> PointCloudIn;
+    typedef PointCloudIn::Ptr PointCloudInPtr;
+    typedef PointCloudIn::ConstPtr PointCloudInConstPtr;
+    typedef pcl::PointCloud<NormalOut> NormalCloudOut;
+
+    typedef pcl::KdTree<PointIn> KdTree;
+    typedef pcl::KdTree<PointIn>::Ptr KdTreePtr; 
+
+    protected:
+      /** \brief An input point cloud describing the surface that is to be used for nearest neighbors estimation. */
+      PointCloudInConstPtr surface_;
+
+      /** \brief A pointer to the spatial search object. */
+      KdTreePtr tree_;
+
+      /** \brief The nearest neighbors search radius for each point. */
+      double search_radius_;
+
+      /** \brief The number of K nearest neighbors to use for each point. */
+      //int k_;
+
+      /** \brief Whether to use a polynomial fit. */
+      bool use_polynomial_fit_;
+
+      /** \brief The order of the polynomial to be fit. */
+      int polynomial_order_;
+
+      /** \brief How 'flat' should the neighbor weighting gaussian be (the smaller, the more local the fit). */
+      double gaussian_parameter_;
+
+      // ROS nodelet attributes
+      /** \brief The surface PointCloud subscriber filter. */
+      message_filters::Subscriber<PointCloudIn> sub_surface_filter_;
+      
+      /** \brief Parameter for the spatial locator tree. By convention, the values represent:
+        * 0: ANN (Approximate Nearest Neigbor library) kd-tree
+        * 1: FLANN (Fast Library for Approximate Nearest Neighbors) kd-tree
+        * 2: Organized spatial dataset index
+        */
+      int spatial_locator_type_;
+    
+      /** \brief Pointer to a dynamic reconfigure service. */
+      boost::shared_ptr <dynamic_reconfigure::Server<MLSConfig> > srv_;
+
+      /** \brief Dynamic reconfigure callback
+        * \param config the config object  
+        * \param level the dynamic reconfigure level
+        */
+      void config_callback (MLSConfig &config, uint32_t level); 
+    
+    private:
+      /** \brief Nodelet initialization routine. */
+      virtual void onInit ();
+
+      /** \brief Input point cloud callback.
+        * \param cloud the pointer to the input point cloud
+        * \param indices the pointer to the input point cloud indices
+        */
+      void input_indices_callback (const PointCloudInConstPtr &cloud, 
+                                   const PointIndicesConstPtr &indices);
+
+
+    private:
+      /** \brief The PCL implementation used. */
+      pcl::MovingLeastSquares<PointIn, NormalOut> impl_;
+      
+      /** \brief The input PointCloud subscriber. */
+      ros::Subscriber sub_input_;
+
+      /** \brief The output PointCloud (containing the normals) publisher. */
+      ros::Publisher pub_normals_;
+
+      /** \brief Synchronized input, and indices.*/
+      boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, PointIndices> > >       sync_input_indices_e_;
+      boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn, PointIndices> > > sync_input_indices_a_;
+
+    public:
+      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  };
+}
+
+#endif  //#ifndef PCL_ROS_MOVING_LEAST_SQUARES_H_

pcl_ros/src/pcl_ros/features/boundary.cpp

@@ -0,0 +1,77 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: boundary.cpp 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#include <pluginlib/class_list_macros.h>
+#include "pcl_ros/features/boundary.h"
+
+void 
+pcl_ros::BoundaryEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
+{
+  PointCloudOut output;
+  output.header = cloud->header;
+  pub_output_.publish (output.makeShared ());
+}
+
+void 
+pcl_ros::BoundaryEstimation::computePublish (const PointCloudInConstPtr &cloud, 
+                                             const PointCloudNConstPtr &normals,
+                                             const PointCloudInConstPtr &surface,
+                                             const IndicesPtr &indices)
+{
+  // Set the parameters
+  impl_.setKSearch (k_);
+  impl_.setRadiusSearch (search_radius_);
+  // Initialize the spatial locator
+  initTree (spatial_locator_type_, tree_, k_);
+  impl_.setSearchMethod (tree_);
+
+  // Set the inputs
+  impl_.setInputCloud (cloud);
+  impl_.setIndices (indices);
+  impl_.setSearchSurface (surface);
+  impl_.setInputNormals (normals);
+  // Estimate the feature
+  PointCloudOut output;
+  impl_.compute (output);
+
+  // Enforce that the TF frame and the timestamp are copied
+  output.header = cloud->header;
+  pub_output_.publish (output.makeShared ());
+}
+
+typedef pcl_ros::BoundaryEstimation BoundaryEstimation;
+PLUGINLIB_DECLARE_CLASS (pcl, BoundaryEstimation, BoundaryEstimation, nodelet::Nodelet);

pcl_ros/src/pcl_ros/features/feature.cpp

@@ -0,0 +1,443 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: feature.cpp 35422 2011-01-24 20:04:44Z rusu $
+ *
+ */
+
+//#include <pluginlib/class_list_macros.h>
+// Include the implementations here instead of compiling them separately to speed up compile time
+//#include "normal_3d.cpp"
+//#include "boundary.cpp"
+//#include "fpfh.cpp"
+//#include "fpfh_omp.cpp"
+//#include "moment_invariants.cpp"
+//#include "normal_3d_omp.cpp"
+//#include "normal_3d_tbb.cpp"
+//#include "pfh.cpp"
+//#include "principal_curvatures.cpp"
+//#include "vfh.cpp"
+#include <pcl/io/io.h>
+#include "pcl_ros/features/feature.h"
+#include <message_filters/null_types.h>
+
+////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::Feature::onInit ()
+{
+  // Call the super onInit ()
+  PCLNodelet::onInit ();
+
+  // Call the child init
+  childInit (*pnh_);
+
+  // Allow each individual class that inherits from us to declare their own Publisher
+  // This is useful for Publisher<pcl::PointCloud<T> >, as NormalEstimation can publish PointCloud<Normal>, etc
+  //pub_output_ = pnh_->template advertise<PointCloud2> ("output", max_queue_size_);
+
+  // ---[ Mandatory parameters
+  if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("radius_search", search_radius_))
+  {
+    NODELET_ERROR ("[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these parameters before continuing.", getName ().c_str ());
+    return;
+  }
+  if (!pnh_->getParam ("spatial_locator", spatial_locator_type_))
+  {
+    NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ());
+    return;
+  }
+
+  // ---[ Optional parameters
+  pnh_->getParam ("use_surface", use_surface_);
+
+  // Enable the dynamic reconfigure service
+  srv_ = boost::make_shared<dynamic_reconfigure::Server<FeatureConfig> > (*pnh_);
+  dynamic_reconfigure::Server<FeatureConfig>::CallbackType f = boost::bind (&Feature::config_callback, this, _1, _2);
+  srv_->setCallback (f);
+
+  // If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages
+  if (use_indices_ || use_surface_)
+  {
+    // Create the objects here
+    if (approximate_sync_)
+      sync_input_surface_indices_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn, PointCloudIn, PointIndices> > >(max_queue_size_);
+    else
+      sync_input_surface_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, PointCloudIn, PointIndices> > >(max_queue_size_);
+
+    // Subscribe to the input using a filter
+    sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
+    if (use_indices_)
+    {
+      // If indices are enabled, subscribe to the indices
+      sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
+      if (use_surface_)     // Use both indices and surface
+      {
+        // If surface is enabled, subscribe to the surface, connect the input-indices-surface trio and register
+        sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_);
+        if (approximate_sync_)
+          sync_input_surface_indices_a_->connectInput (sub_input_filter_, sub_surface_filter_, sub_indices_filter_);
+        else
+          sync_input_surface_indices_e_->connectInput (sub_input_filter_, sub_surface_filter_, sub_indices_filter_);
+      }
+      else                  // Use only indices
+      {
+        sub_input_filter_.registerCallback (bind (&Feature::input_callback, this, _1));
+        // surface not enabled, connect the input-indices duo and register
+        if (approximate_sync_)
+          sync_input_surface_indices_a_->connectInput (sub_input_filter_, nf_pc_, sub_indices_filter_);
+        else
+          sync_input_surface_indices_e_->connectInput (sub_input_filter_, nf_pc_, sub_indices_filter_);
+      }
+    }
+    else                    // Use only surface
+    {
+      sub_input_filter_.registerCallback (bind (&Feature::input_callback, this, _1));
+      // indices not enabled, connect the input-surface duo and register
+      sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_);
+      if (approximate_sync_)
+        sync_input_surface_indices_a_->connectInput (sub_input_filter_, sub_surface_filter_, nf_pi_);
+      else
+        sync_input_surface_indices_e_->connectInput (sub_input_filter_, sub_surface_filter_, nf_pi_);
+    }
+    // Register callbacks
+    if (approximate_sync_)
+      sync_input_surface_indices_a_->registerCallback (bind (&Feature::input_surface_indices_callback, this, _1, _2, _3));
+    else
+      sync_input_surface_indices_e_->registerCallback (bind (&Feature::input_surface_indices_callback, this, _1, _2, _3));
+  }
+  else
+    // Subscribe in an old fashion to input only (no filters)
+    sub_input_ = pnh_->subscribe<PointCloudIn> ("input", max_queue_size_,  bind (&Feature::input_surface_indices_callback, this, _1, PointCloudInConstPtr (), PointIndicesConstPtr ()));
+
+  NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
+                 " - use_surface    : %s\n"
+                 " - k_search       : %d\n"
+                 " - radius_search  : %f\n"
+                 " - spatial_locator: %d",
+                getName ().c_str (),
+                (use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_);
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::Feature::config_callback (FeatureConfig &config, uint32_t level)
+{
+  if (k_ != config.k_search)
+  {
+    k_ = config.k_search;
+    NODELET_DEBUG ("[config_callback] Setting the number of K nearest neighbors to use for each point: %d.", k_);
+  }
+  if (search_radius_ != config.radius_search)
+  {
+    search_radius_ = config.radius_search;
+    NODELET_DEBUG ("[config_callback] Setting the nearest neighbors search radius for each point: %f.", search_radius_);
+  }
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::Feature::input_surface_indices_callback (const PointCloudInConstPtr &cloud, 
+    const PointCloudInConstPtr &cloud_surface, const PointIndicesConstPtr &indices)
+{
+  // No subscribers, no work
+  if (pub_output_.getNumSubscribers () <= 0)
+    return;
+
+  // If cloud is given, check if it's valid
+  if (!isValid (cloud))
+  {
+    NODELET_ERROR ("[%s::input_surface_indices_callback] Invalid input!", getName ().c_str ());
+    emptyPublish (cloud);
+    return;
+  }
+
+  // If surface is given, check if it's valid
+  if (cloud_surface && !isValid (cloud_surface, "surface"))
+  {
+    NODELET_ERROR ("[%s::input_surface_indices_callback] Invalid input surface!", getName ().c_str ());
+    emptyPublish (cloud);
+    return;
+  }
+    
+  // If indices are given, check if they are valid
+  if (indices && !isValid (indices))
+  {
+    NODELET_ERROR ("[%s::input_surface_indices_callback] Invalid input indices!", getName ().c_str ());
+    emptyPublish (cloud);
+    return;
+  }
+
+  /// DEBUG
+  if (cloud_surface)
+    if (indices)
+      NODELET_DEBUG ("[input_surface_indices_callback]\n"
+                     "                                         - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
+                     "                                         - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
+                     "                                         - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
+                 cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
+                 cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), cloud_surface->header.stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str (),
+                 indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
+    else
+      NODELET_DEBUG ("[input_surface_indices_callback]\n"
+                     "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
+                     "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
+                     cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
+                     cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), cloud_surface->header.stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str ());
+
+  else
+    if (indices)
+      NODELET_DEBUG ("[input_surface_indices_callback]\n"
+                     "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
+                     "                                 - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
+                     cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
+                     indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
+    else
+      NODELET_DEBUG ("[input_surface_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", cloud->width * cloud->height, cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ());
+  ///
+
+
+  if ((int)(cloud->width * cloud->height) < k_)
+  {
+    NODELET_ERROR ("[input_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger than the PointCloud size (%d)!", k_, (int)(cloud->width * cloud->height));
+    emptyPublish (cloud);
+    return;
+  }
+
+  // If indices given...
+  IndicesPtr vindices;
+  if (indices && !indices->header.frame_id.empty ())
+    vindices.reset (new std::vector<int> (indices->indices));
+
+  computePublish (cloud, cloud_surface, vindices);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////////////////
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::FeatureFromNormals::onInit ()
+{
+  // Call the super onInit ()
+  PCLNodelet::onInit ();
+
+  // Call the child init
+  childInit (*pnh_);
+
+  // Allow each individual class that inherits from us to declare their own Publisher
+  // This is useful for Publisher<pcl::PointCloud<T> >, as NormalEstimation can publish PointCloud<Normal>, etc
+  //pub_output_ = pnh_->template advertise<PointCloud2> ("output", max_queue_size_);
+
+  // ---[ Mandatory parameters
+  if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("radius_search", search_radius_))
+  {
+    NODELET_ERROR ("[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these parameters before continuing.", getName ().c_str ());
+    return;
+  }
+  if (!pnh_->getParam ("spatial_locator", spatial_locator_type_))
+  {
+    NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ());
+    return;
+  }
+  // ---[ Optional parameters
+  pnh_->getParam ("use_surface", use_surface_);
+
+  sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
+  sub_normals_filter_.subscribe (*pnh_, "normals", max_queue_size_);
+
+  // Enable the dynamic reconfigure service
+  srv_ = boost::make_shared<dynamic_reconfigure::Server<FeatureConfig> > (*pnh_);
+  dynamic_reconfigure::Server<FeatureConfig>::CallbackType f = boost::bind (&FeatureFromNormals::config_callback, this, _1, _2);
+  srv_->setCallback (f);
+
+  // Create the objects here
+  if (approximate_sync_)
+    sync_input_normals_surface_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn, PointCloudN, PointCloudIn, PointIndices> > > (max_queue_size_);
+  else
+    sync_input_normals_surface_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, PointCloudN, PointCloudIn, PointIndices> > > (max_queue_size_);
+
+  // If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages
+  if (use_indices_ || use_surface_)
+  {
+    if (use_indices_)
+    {
+      // If indices are enabled, subscribe to the indices
+      sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
+      if (use_surface_)     // Use both indices and surface
+      {
+        // If surface is enabled, subscribe to the surface, connect the input-indices-surface trio and register
+        sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_);
+        if (approximate_sync_)
+          sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, sub_indices_filter_);
+        else
+          sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, sub_indices_filter_);
+      }
+      else                  // Use only indices
+      {
+        sub_input_filter_.registerCallback (bind (&FeatureFromNormals::input_callback, this, _1));
+        if (approximate_sync_)
+          // surface not enabled, connect the input-indices duo and register
+          sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, sub_indices_filter_);
+        else
+          // surface not enabled, connect the input-indices duo and register
+          sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, sub_indices_filter_);
+      }
+    }
+    else                    // Use only surface
+    {
+      // indices not enabled, connect the input-surface duo and register
+      sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_);
+
+      sub_input_filter_.registerCallback (bind (&FeatureFromNormals::input_callback, this, _1));
+      if (approximate_sync_)
+        sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, nf_pi_);
+      else
+        sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, nf_pi_);
+    }
+  }
+  else
+  {
+    sub_input_filter_.registerCallback (bind (&FeatureFromNormals::input_callback, this, _1));
+
+    if (approximate_sync_)
+      sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, nf_pi_);
+    else
+      sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, nf_pi_);
+  }
+
+  // Register callbacks
+  if (approximate_sync_)
+    sync_input_normals_surface_indices_a_->registerCallback (bind (&FeatureFromNormals::input_normals_surface_indices_callback, this, _1, _2, _3, _4));
+  else
+    sync_input_normals_surface_indices_e_->registerCallback (bind (&FeatureFromNormals::input_normals_surface_indices_callback, this, _1, _2, _3, _4));
+
+  NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
+                 " - use_surface    : %s\n"
+                 " - k_search       : %d\n"
+                 " - radius_search  : %f\n"
+                 " - spatial_locator: %d",
+                getName ().c_str (),
+                 (use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback (
+   const PointCloudInConstPtr &cloud, const PointCloudNConstPtr &cloud_normals,
+   const PointCloudInConstPtr &cloud_surface, const PointIndicesConstPtr &indices)
+{
+  // No subscribers, no work
+  if (pub_output_.getNumSubscribers () <= 0)
+    return;
+
+  // If cloud+normals is given, check if it's valid
+  if (!isValid (cloud))// || !isValid (cloud_normals, "normals"))
+  {
+    NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input!", getName ().c_str ());
+    emptyPublish (cloud);
+    return;
+  }
+
+  // If surface is given, check if it's valid
+  if (cloud_surface && !isValid (cloud_surface, "surface"))
+  {
+    NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input surface!", getName ().c_str ());
+    emptyPublish (cloud);
+    return;
+  }
+    
+  // If indices are given, check if they are valid
+  if (indices && !isValid (indices))
+  {
+    NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input indices!", getName ().c_str ());
+    emptyPublish (cloud);
+    return;
+  }
+
+  /// DEBUG
+  if (cloud_surface)
+    if (indices)
+      NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n"
+                     "                                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
+                     "                                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
+                     "                                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
+                     "                                                 - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
+                     getName ().c_str (),
+                     cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
+                     cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), cloud_surface->header.stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str (),
+                     cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), cloud_normals->header.stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str (),
+                     indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
+    else
+      NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n"
+                     "                                         - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
+                     "                                         - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
+                     "                                         - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
+                     getName ().c_str (), 
+                     cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
+                     cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), cloud_surface->header.stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str (),
+                     cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), cloud_normals->header.stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str ());
+  else
+    if (indices)
+      NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n"
+                     "                                         - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
+                     "                                         - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
+                     "                                         - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
+                     getName ().c_str (),
+                     cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
+                     cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), cloud_normals->header.stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str (),
+                     indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
+    else
+      NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n"
+                     "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
+                     "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
+                     getName ().c_str (), 
+                     cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
+                     cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), cloud_normals->header.stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str ());
+  ///
+
+  if ((int)(cloud->width * cloud->height) < k_)
+  {
+    NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger than the PointCloud size (%d)!", getName ().c_str (), k_, (int)(cloud->width * cloud->height));
+    emptyPublish (cloud);
+    return;
+  }
+
+  // If indices given...
+  IndicesPtr vindices;
+  if (indices && !indices->header.frame_id.empty ())
+    vindices.reset (new std::vector<int> (indices->indices));
+
+  computePublish (cloud, cloud_normals, cloud_surface, vindices);
+}
+

pcl_ros/src/pcl_ros/features/fpfh.cpp

@@ -0,0 +1,79 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: fpfh.cpp 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#include <pluginlib/class_list_macros.h>
+#include "pcl_ros/features/fpfh.h"
+
+void 
+pcl_ros::FPFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
+{
+  PointCloudOut output;
+  output.header = cloud->header;
+  pub_output_.publish (output.makeShared ());
+}
+
+void 
+pcl_ros::FPFHEstimation::computePublish (const PointCloudInConstPtr &cloud,
+                                         const PointCloudNConstPtr &normals,
+                                         const PointCloudInConstPtr &surface,
+                                         const IndicesPtr &indices)
+{
+  // Set the parameters
+  impl_.setKSearch (k_);
+  impl_.setRadiusSearch (search_radius_);
+  // Initialize the spatial locator
+  initTree (spatial_locator_type_, tree_, k_);
+  impl_.setSearchMethod (tree_);
+
+  // Set the inputs
+  impl_.setInputCloud (cloud);
+  impl_.setIndices (indices);
+  impl_.setSearchSurface (surface);
+  impl_.setInputNormals (normals);
+  // Estimate the feature
+  PointCloudOut output;
+  impl_.compute (output);
+
+  // Publish a Boost shared ptr const data
+  // Enforce that the TF frame and the timestamp are copied
+  output.header = cloud->header;
+  pub_output_.publish (output.makeShared ());
+}
+
+typedef pcl_ros::FPFHEstimation FPFHEstimation;
+PLUGINLIB_DECLARE_CLASS (pcl, FPFHEstimation, FPFHEstimation, nodelet::Nodelet);
+

pcl_ros/src/pcl_ros/features/fpfh_omp.cpp

@@ -0,0 +1,79 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: fpfh_omp.cpp 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#include <pluginlib/class_list_macros.h>
+#include "pcl_ros/features/fpfh_omp.h"
+
+void 
+pcl_ros::FPFHEstimationOMP::emptyPublish (const PointCloudInConstPtr &cloud)
+{
+  PointCloudOut output;
+  output.header = cloud->header;
+  pub_output_.publish (output.makeShared ());
+}
+
+void 
+pcl_ros::FPFHEstimationOMP::computePublish (const PointCloudInConstPtr &cloud,
+                                            const PointCloudNConstPtr &normals,
+                                            const PointCloudInConstPtr &surface,
+                                            const IndicesPtr &indices)
+{
+  // Set the parameters
+  impl_.setKSearch (k_);
+  impl_.setRadiusSearch (search_radius_);
+  // Initialize the spatial locator
+  initTree (spatial_locator_type_, tree_, k_);
+  impl_.setSearchMethod (tree_);
+
+  // Set the inputs
+  impl_.setInputCloud (cloud);
+  impl_.setIndices (indices);
+  impl_.setSearchSurface (surface);
+  impl_.setInputNormals (normals);
+  // Estimate the feature
+  PointCloudOut output;
+  impl_.compute (output);
+
+  // Publish a Boost shared ptr const data
+  // Enforce that the TF frame and the timestamp are copied
+  output.header = cloud->header;
+  pub_output_.publish (output.makeShared ());
+}
+
+typedef pcl_ros::FPFHEstimationOMP FPFHEstimationOMP;
+PLUGINLIB_DECLARE_CLASS (pcl, FPFHEstimationOMP, FPFHEstimationOMP, nodelet::Nodelet);
+

pcl_ros/src/pcl_ros/features/moment_invariants.cpp

@@ -0,0 +1,77 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: moment_invariants.cpp 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#include <pluginlib/class_list_macros.h>
+#include "pcl_ros/features/moment_invariants.h"
+
+void 
+pcl_ros::MomentInvariantsEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
+{
+  PointCloudOut output;
+  output.header = cloud->header;
+  pub_output_.publish (output.makeShared ());
+}
+
+void 
+pcl_ros::MomentInvariantsEstimation::computePublish (const PointCloudInConstPtr &cloud,
+                                                     const PointCloudInConstPtr &surface,
+                                                     const IndicesPtr &indices)
+{
+  // Set the parameters
+  impl_.setKSearch (k_);
+  impl_.setRadiusSearch (search_radius_);
+  // Initialize the spatial locator
+  initTree (spatial_locator_type_, tree_, k_);
+  impl_.setSearchMethod (tree_);
+
+  // Set the inputs
+  impl_.setInputCloud (cloud);
+  impl_.setIndices (indices);
+  impl_.setSearchSurface (surface);
+  // Estimate the feature
+  PointCloudOut output;
+  impl_.compute (output);
+
+  // Publish a Boost shared ptr const data
+  // Enforce that the TF frame and the timestamp are copied
+  output.header = cloud->header;
+  pub_output_.publish (output.makeShared ());
+}
+
+typedef pcl_ros::MomentInvariantsEstimation MomentInvariantsEstimation;
+PLUGINLIB_DECLARE_CLASS (pcl, MomentInvariantsEstimation, MomentInvariantsEstimation, nodelet::Nodelet);
+

pcl_ros/src/pcl_ros/features/normal_3d.cpp

@@ -0,0 +1,77 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: normal_3d.cpp 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#include <pluginlib/class_list_macros.h>
+#include "pcl_ros/features/normal_3d.h"
+
+void 
+pcl_ros::NormalEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
+{
+  PointCloudOut output;
+  output.header = cloud->header;
+  pub_output_.publish (output.makeShared ());
+}
+
+void 
+pcl_ros::NormalEstimation::computePublish (const PointCloudInConstPtr &cloud,
+                                           const PointCloudInConstPtr &surface,
+                                           const IndicesPtr &indices)
+{
+  // Set the parameters
+  impl_.setKSearch (k_);
+  impl_.setRadiusSearch (search_radius_);
+  // Initialize the spatial locator
+  initTree (spatial_locator_type_, tree_, k_);
+  impl_.setSearchMethod (tree_);
+
+  // Set the inputs
+  impl_.setInputCloud (cloud);
+  impl_.setIndices (indices);
+  impl_.setSearchSurface (surface);
+  // Estimate the feature
+  PointCloudOut output;
+  impl_.compute (output);
+
+  // Publish a Boost shared ptr const data
+  // Enforce that the TF frame and the timestamp are copied
+  output.header = cloud->header;
+  pub_output_.publish (output.makeShared ());
+}
+
+typedef pcl_ros::NormalEstimation NormalEstimation;
+PLUGINLIB_DECLARE_CLASS (pcl, NormalEstimation, NormalEstimation, nodelet::Nodelet);
+

pcl_ros/src/pcl_ros/features/normal_3d_omp.cpp

@@ -0,0 +1,77 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: normal_3d_omp.cpp 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#include <pluginlib/class_list_macros.h>
+#include "pcl_ros/features/normal_3d_omp.h"
+
+void 
+pcl_ros::NormalEstimationOMP::emptyPublish (const PointCloudInConstPtr &cloud)
+{
+  PointCloudOut output;
+  output.header = cloud->header;
+  pub_output_.publish (output.makeShared ());
+}
+
+void 
+pcl_ros::NormalEstimationOMP::computePublish (const PointCloudInConstPtr &cloud,
+                                              const PointCloudInConstPtr &surface,
+                                              const IndicesPtr &indices)
+{
+  // Set the parameters
+  impl_.setKSearch (k_);
+  impl_.setRadiusSearch (search_radius_);
+  // Initialize the spatial locator
+  initTree (spatial_locator_type_, tree_, k_);
+  impl_.setSearchMethod (tree_);
+
+  // Set the inputs
+  impl_.setInputCloud (cloud);
+  impl_.setIndices (indices);
+  impl_.setSearchSurface (surface);
+  // Estimate the feature
+  PointCloudOut output;
+  impl_.compute (output);
+
+  // Publish a Boost shared ptr const data
+  // Enforce that the TF frame and the timestamp are copied
+  output.header = cloud->header;
+  pub_output_.publish (output.makeShared ());
+}
+
+typedef pcl_ros::NormalEstimationOMP NormalEstimationOMP;
+PLUGINLIB_DECLARE_CLASS (pcl, NormalEstimationOMP, NormalEstimationOMP, nodelet::Nodelet);
+

pcl_ros/src/pcl_ros/features/normal_3d_tbb.cpp

@@ -0,0 +1,81 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: normal_3d_tbb.cpp 35625 2011-01-31 07:56:13Z gbiggs $
+ *
+ */
+
+#include <pluginlib/class_list_macros.h>
+#include "pcl_ros/features/normal_3d_tbb.h"
+
+#if defined HAVE_TBB
+
+void 
+pcl_ros::NormalEstimationTBB::emptyPublish (const PointCloudInConstPtr &cloud)
+{
+  PointCloud output;
+  output.header = cloud->header;
+  pub_output_.publish (output.makeShared ());
+}
+
+void 
+pcl_ros::NormalEstimationTBB::computePublish (const PointCloudInConstPtr &cloud,
+                                              const PointCloudInConstPtr &surface,
+                                              const IndicesPtr &indices)
+{
+  // Set the parameters
+  impl_.setKSearch (k_);
+  impl_.setRadiusSearch (search_radius_);
+  // Initialize the spatial locator
+  initTree (spatial_locator_type_, tree_, k_);
+  impl_.setSearchMethod (tree_);
+
+  // Set the inputs
+  impl_.setInputCloud (cloud);
+  impl_.setIndices (indices);
+  impl_.setSearchSurface (surface);
+  // Estimate the feature
+  PointCloudOut output;
+  impl_.compute (output);
+
+  // Publish a Boost shared ptr const data
+  // Enforce that the TF frame and the timestamp are copied
+  output.header = cloud->header;
+  pub_output_.publish (output.makeShared ());
+}
+
+typedef pcl_ros::NormalEstimationTBB NormalEstimationTBB;
+PLUGINLIB_DECLARE_CLASS (pcl, NormalEstimationTBB, NormalEstimationTBB, nodelet::Nodelet);
+
+#endif // HAVE_TBB
+

pcl_ros/src/pcl_ros/features/pfh.cpp

@@ -0,0 +1,79 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: pfh.cpp 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#include <pluginlib/class_list_macros.h>
+#include "pcl_ros/features/pfh.h"
+
+void 
+pcl_ros::PFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
+{
+  PointCloudOut output;
+  output.header = cloud->header;
+  pub_output_.publish (output.makeShared ());
+}
+
+void 
+pcl_ros::PFHEstimation::computePublish (const PointCloudInConstPtr &cloud,
+                                        const PointCloudNConstPtr &normals,
+                                        const PointCloudInConstPtr &surface,
+                                        const IndicesPtr &indices)
+{
+  // Set the parameters
+  impl_.setKSearch (k_);
+  impl_.setRadiusSearch (search_radius_);
+  // Initialize the spatial locator
+  initTree (spatial_locator_type_, tree_, k_);
+  impl_.setSearchMethod (tree_);
+
+  // Set the inputs
+  impl_.setInputCloud (cloud);
+  impl_.setIndices (indices);
+  impl_.setSearchSurface (surface);
+  impl_.setInputNormals (normals);
+  // Estimate the feature
+  PointCloudOut output;
+  impl_.compute (output);
+
+  // Publish a Boost shared ptr const data
+  // Enforce that the TF frame and the timestamp are copied
+  output.header = cloud->header;
+  pub_output_.publish (output.makeShared ());
+}
+
+typedef pcl_ros::PFHEstimation PFHEstimation;
+PLUGINLIB_DECLARE_CLASS (pcl, PFHEstimation, PFHEstimation, nodelet::Nodelet);
+

pcl_ros/src/pcl_ros/features/principal_curvatures.cpp

@@ -0,0 +1,79 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: principal_curvatures.cpp 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#include <pluginlib/class_list_macros.h>
+#include "pcl_ros/features/principal_curvatures.h"
+
+void 
+pcl_ros::PrincipalCurvaturesEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
+{
+  PointCloudOut output;
+  output.header = cloud->header;
+  pub_output_.publish (output.makeShared ());
+}
+
+void 
+pcl_ros::PrincipalCurvaturesEstimation::computePublish (const PointCloudInConstPtr &cloud,
+                                                        const PointCloudNConstPtr &normals,
+                                                        const PointCloudInConstPtr &surface,
+                                                        const IndicesPtr &indices)
+{
+  // Set the parameters
+  impl_.setKSearch (k_);
+  impl_.setRadiusSearch (search_radius_);
+  // Initialize the spatial locator
+  initTree (spatial_locator_type_, tree_, k_);
+  impl_.setSearchMethod (tree_);
+
+  // Set the inputs
+  impl_.setInputCloud (cloud);
+  impl_.setIndices (indices);
+  impl_.setSearchSurface (surface);
+  impl_.setInputNormals (normals);
+  // Estimate the feature
+  PointCloudOut output;
+  impl_.compute (output);
+
+  // Publish a Boost shared ptr const data
+  // Enforce that the TF frame and the timestamp are copied
+  output.header = cloud->header;
+  pub_output_.publish (output.makeShared ());
+}
+
+typedef pcl_ros::PrincipalCurvaturesEstimation PrincipalCurvaturesEstimation;
+PLUGINLIB_DECLARE_CLASS (pcl, PrincipalCurvaturesEstimation, PrincipalCurvaturesEstimation, nodelet::Nodelet);
+

pcl_ros/src/pcl_ros/features/vfh.cpp

@@ -0,0 +1,79 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: vfh.cpp 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#include <pluginlib/class_list_macros.h>
+#include "pcl_ros/features/vfh.h"
+
+void 
+pcl_ros::VFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
+{
+  PointCloudOut output;
+  output.header = cloud->header;
+  pub_output_.publish (output.makeShared ());
+}
+
+void 
+pcl_ros::VFHEstimation::computePublish (const PointCloudInConstPtr &cloud,
+                                        const PointCloudNConstPtr &normals,
+                                        const PointCloudInConstPtr &surface,
+                                        const IndicesPtr &indices)
+{
+  // Set the parameters
+  impl_.setKSearch (k_);
+  impl_.setRadiusSearch (search_radius_);
+  // Initialize the spatial locator
+  initTree (spatial_locator_type_, tree_, k_);
+  impl_.setSearchMethod (tree_);
+
+  // Set the inputs
+  impl_.setInputCloud (cloud);
+  impl_.setIndices (indices);
+  impl_.setSearchSurface (surface);
+  impl_.setInputNormals (normals);
+  // Estimate the feature
+  PointCloudOut output;
+  impl_.compute (output);
+
+  // Publish a Boost shared ptr const data
+  // Enforce that the TF frame and the timestamp are copied
+  output.header = cloud->header;
+  pub_output_.publish (output.makeShared ());
+}
+
+typedef pcl_ros::VFHEstimation VFHEstimation;
+PLUGINLIB_DECLARE_CLASS (pcl, VFHEstimation, VFHEstimation, nodelet::Nodelet);
+

pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp

@@ -0,0 +1,220 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2010, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: extract_clusters.hpp 32052 2010-08-27 02:19:30Z rusu $
+ *
+ */
+
+#include <pluginlib/class_list_macros.h>
+#include <pcl/io/io.h>
+#include "pcl_ros/segmentation/extract_clusters.h"
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::EuclideanClusterExtraction::onInit ()
+{
+  // Call the super onInit ()
+  PCLNodelet::onInit ();
+
+  // ---[ Mandatory parameters
+  double cluster_tolerance;
+  if (!pnh_->getParam ("cluster_tolerance", cluster_tolerance))
+  {
+    NODELET_ERROR ("[%s::onInit] Need a 'cluster_tolerance' parameter to be set before continuing!", getName ().c_str ()); 
+    return;
+  }
+  int spatial_locator;
+  if (!pnh_->getParam ("spatial_locator", spatial_locator))
+  {
+    NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ());
+    return;
+  }
+
+  //private_nh.getParam ("use_indices", use_indices_);
+  pnh_->getParam ("publish_indices", publish_indices_);
+
+  if (publish_indices_)
+    pub_output_ = pnh_->advertise<PointIndices> ("output", max_queue_size_);
+  else
+    pub_output_ = pnh_->advertise<PointCloud> ("output", max_queue_size_);
+
+  // Enable the dynamic reconfigure service
+  srv_ = boost::make_shared <dynamic_reconfigure::Server<EuclideanClusterExtractionConfig> > (*pnh_);
+  dynamic_reconfigure::Server<EuclideanClusterExtractionConfig>::CallbackType f =  boost::bind (&EuclideanClusterExtraction::config_callback, this, _1, _2);
+  srv_->setCallback (f);
+
+  // If we're supposed to look for PointIndices (indices)
+  if (use_indices_)
+  {
+    // Subscribe to the input using a filter
+    sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
+    sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
+
+    if (approximate_sync_)
+    {
+      sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<PointCloud, PointIndices> > > (max_queue_size_);
+      sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_);
+      sync_input_indices_a_->registerCallback (bind (&EuclideanClusterExtraction::input_indices_callback, this, _1, _2));
+    }
+    else
+    {
+      sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ExactTime<PointCloud, PointIndices> > > (max_queue_size_);
+      sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_);
+      sync_input_indices_e_->registerCallback (bind (&EuclideanClusterExtraction::input_indices_callback, this, _1, _2));
+    }
+  }
+  else
+    // Subscribe in an old fashion to input only (no filters)
+    sub_input_ = pnh_->subscribe<PointCloud> ("input", max_queue_size_, bind (&EuclideanClusterExtraction::input_indices_callback, this, _1, PointIndicesConstPtr ()));
+
+  NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
+                 " - max_queue_size    : %d\n"
+                 " - use_indices       : %s\n"
+                 " - cluster_tolerance : %f\n"
+                 " - spatial_locator   : %d",
+                 getName ().c_str (),
+                 max_queue_size_,
+                 (use_indices_) ? "true" : "false", cluster_tolerance, spatial_locator);
+
+  // Set given parameters here
+  impl_.setSpatialLocator (spatial_locator);
+  impl_.setClusterTolerance (cluster_tolerance);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::EuclideanClusterExtraction::config_callback (EuclideanClusterExtractionConfig &config, uint32_t level)
+{
+  if (impl_.getClusterTolerance () != config.cluster_tolerance)
+  {
+    impl_.setClusterTolerance (config.cluster_tolerance);
+    NODELET_DEBUG ("[%s::config_callback] Setting new clustering tolerance to: %f.", getName ().c_str (), config.cluster_tolerance);
+  }
+  if (impl_.getMinClusterSize () != config.cluster_min_size)
+  {
+    impl_.setMinClusterSize (config.cluster_min_size);
+    NODELET_DEBUG ("[%s::config_callback] Setting the minimum cluster size to: %d.", getName ().c_str (), config.cluster_min_size);
+  }
+  if (impl_.getMaxClusterSize () != config.cluster_max_size)
+  {
+    impl_.setMaxClusterSize (config.cluster_max_size);
+    NODELET_DEBUG ("[%s::config_callback] Setting the maximum cluster size to: %d.", getName ().c_str (), config.cluster_max_size);
+  }
+  if (max_clusters_ != config.max_clusters)
+  {
+    max_clusters_ = config.max_clusters;
+    NODELET_DEBUG ("[%s::config_callback] Setting the maximum number of clusters to extract to: %d.", getName ().c_str (), config.max_clusters);
+  }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::EuclideanClusterExtraction::input_indices_callback (
+      const PointCloudConstPtr &cloud, const PointIndicesConstPtr &indices)
+{
+  // No subscribers, no work
+  if (pub_output_.getNumSubscribers () <= 0)
+    return;
+
+  // If cloud is given, check if it's valid
+  if (!isValid (cloud))
+  {
+    NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
+    return;
+  }
+  // If indices are given, check if they are valid
+  if (indices && !isValid (indices))
+  {
+    NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
+    return;
+  }
+
+  /// DEBUG
+  if (indices)
+    NODELET_DEBUG ("[%s::input_indices_callback]\n"
+                   "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
+                   "                                 - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
+                   getName ().c_str (),
+                   cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
+                   indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
+  else
+    NODELET_DEBUG ("[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ());
+  ///
+
+  IndicesPtr indices_ptr;
+  if (indices)
+    indices_ptr.reset (new std::vector<int> (indices->indices));
+
+  impl_.setInputCloud (cloud);
+  impl_.setIndices (indices_ptr);
+
+  std::vector<PointIndices> clusters;
+  impl_.extract (clusters);
+
+  if (publish_indices_)
+  {
+    for (size_t i = 0; i < clusters.size (); ++i)
+    {
+      if ((int)i >= max_clusters_)
+        break;
+      // TODO: HACK!!! We need to change the PointCloud2 message to add for an incremental sequence ID number.
+      clusters[i].header.stamp += ros::Duration (i * 0.001);
+      pub_output_.publish (boost::make_shared<const PointIndices> (clusters[i]));
+    }
+
+    NODELET_DEBUG ("[segmentAndPublish] Published %zu clusters (PointIndices) on topic %s", clusters.size (), pnh_->resolveName ("output").c_str ());
+  }
+  else
+  {
+    for (size_t i = 0; i < clusters.size (); ++i)
+    {
+      if ((int)i >= max_clusters_)
+        break;
+      PointCloud output;
+      copyPointCloud (*cloud, clusters[i].indices, output);
+
+      //PointCloud output_blob;     // Convert from the templated output to the PointCloud blob
+      //pcl::toROSMsg (output, output_blob);
+      // TODO: HACK!!! We need to change the PointCloud2 message to add for an incremental sequence ID number.
+      output.header.stamp += ros::Duration (i * 0.001);
+      // Publish a Boost shared ptr const data
+      pub_output_.publish (output.makeShared ());
+      NODELET_DEBUG ("[segmentAndPublish] Published cluster %zu (with %zu values and stamp %f) on topic %s",
+                     i, clusters[i].indices.size (), output.header.stamp.toSec (), pnh_->resolveName ("output").c_str ());
+    }
+  }
+}
+
+typedef pcl_ros::EuclideanClusterExtraction EuclideanClusterExtraction;
+PLUGINLIB_DECLARE_CLASS (pcl, EuclideanClusterExtraction, EuclideanClusterExtraction, nodelet::Nodelet);
+

pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp

@@ -0,0 +1,193 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2010, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: extract_polygonal_prism_data.hpp 32996 2010-09-30 23:42:11Z rusu $
+ *
+ */
+
+#include <pluginlib/class_list_macros.h>
+#include "pcl_ros/transforms.h"
+#include "pcl_ros/segmentation/extract_polygonal_prism_data.h"
+#include <pcl/io/io.h>
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::ExtractPolygonalPrismData::onInit ()
+{
+  // Call the super onInit ()
+  PCLNodelet::onInit ();
+
+  sub_hull_filter_.subscribe (*pnh_, "planar_hull", max_queue_size_);
+  sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
+
+  // Enable the dynamic reconfigure service
+  srv_ = boost::make_shared <dynamic_reconfigure::Server<ExtractPolygonalPrismDataConfig> > (*pnh_);
+  dynamic_reconfigure::Server<ExtractPolygonalPrismDataConfig>::CallbackType f = boost::bind (&ExtractPolygonalPrismData::config_callback, this, _1, _2);
+  srv_->setCallback (f);
+
+  // Create the objects here
+  if (approximate_sync_)
+    sync_input_hull_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointCloud, PointIndices> > > (max_queue_size_);
+  else
+    sync_input_hull_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloud, PointIndices> > > (max_queue_size_);
+
+  if (use_indices_)
+  {
+    sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
+    if (approximate_sync_)
+      sync_input_hull_indices_a_->connectInput (sub_input_filter_, sub_hull_filter_, sub_indices_filter_);
+    else
+      sync_input_hull_indices_e_->connectInput (sub_input_filter_, sub_hull_filter_, sub_indices_filter_);
+  }
+  else
+  {
+    sub_input_filter_.registerCallback (bind (&ExtractPolygonalPrismData::input_callback, this, _1));
+
+    if (approximate_sync_)
+      sync_input_hull_indices_a_->connectInput (sub_input_filter_, sub_hull_filter_, nf_);
+    else
+      sync_input_hull_indices_e_->connectInput (sub_input_filter_, sub_hull_filter_, nf_);
+  }
+  // Register callbacks
+  if (approximate_sync_)
+    sync_input_hull_indices_a_->registerCallback (bind (&ExtractPolygonalPrismData::input_hull_indices_callback, this, _1, _2, _3));
+  else
+    sync_input_hull_indices_e_->registerCallback (bind (&ExtractPolygonalPrismData::input_hull_indices_callback, this, _1, _2, _3));
+
+  // Advertise the output topics
+  pub_output_ = pnh_->advertise<PointIndices> ("output", max_queue_size_);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::ExtractPolygonalPrismData::config_callback (ExtractPolygonalPrismDataConfig &config, uint32_t level)
+{
+  double height_min, height_max;
+  impl_.getHeightLimits (height_min, height_max);
+  if (height_min != config.height_min)
+  {
+    height_min = config.height_min;
+    NODELET_DEBUG ("[%s::config_callback] Setting new minimum height to the planar model to: %f.", getName ().c_str (), height_min);
+    impl_.setHeightLimits (height_min, height_max);
+  }
+  if (height_max != config.height_max)
+  {
+    height_max = config.height_max;
+    NODELET_DEBUG ("[%s::config_callback] Setting new maximum height to the planar model to: %f.", getName ().c_str (), height_max);
+    impl_.setHeightLimits (height_min, height_max);
+  }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback (
+    const PointCloudConstPtr &cloud, 
+    const PointCloudConstPtr &hull, 
+    const PointIndicesConstPtr &indices)
+{
+  // No subscribers, no work
+  if (pub_output_.getNumSubscribers () <= 0)
+    return;
+
+  // Copy the header (stamp + frame_id)
+  pcl::PointIndices inliers;
+  inliers.header = cloud->header;
+
+  // If cloud is given, check if it's valid
+  if (!isValid (cloud) || !isValid (hull, "planar_hull"))
+  {
+    NODELET_ERROR ("[%s::input_hull_indices_callback] Invalid input!", getName ().c_str ());
+    pub_output_.publish (boost::make_shared<const pcl::PointIndices> (inliers));
+    return;
+  }
+  // If indices are given, check if they are valid
+  if (indices && !isValid (indices))
+  {
+    NODELET_ERROR ("[%s::input_hull_indices_callback] Invalid indices!", getName ().c_str ());
+    pub_output_.publish (boost::make_shared<const pcl::PointIndices> (inliers));
+    return;
+  }
+
+  /// DEBUG
+  if (indices)
+    NODELET_DEBUG ("[%s::input_indices_hull_callback]\n"
+                   "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
+                   "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
+                   "                                 - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
+                   getName ().c_str (), 
+                   cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
+                   hull->width * hull->height, pcl::getFieldsList (*hull).c_str (), hull->header.stamp.toSec (), hull->header.frame_id.c_str (), pnh_->resolveName ("planar_hull").c_str (),
+                   indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
+  else
+    NODELET_DEBUG ("[%s::input_indices_hull_callback]\n"
+                   "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
+                   "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
+                   getName ().c_str (),
+                   cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
+                   hull->width * hull->height, pcl::getFieldsList (*hull).c_str (), hull->header.stamp.toSec (), hull->header.frame_id.c_str (), pnh_->resolveName ("planar_hull").c_str ());
+  ///
+
+  if (cloud->header.frame_id != hull->header.frame_id)
+  {
+    NODELET_DEBUG ("[%s::input_hull_callback] Planar hull has a different TF frame (%s) than the input point cloud (%s)! Using TF to transform.", getName ().c_str (), hull->header.frame_id.c_str (), cloud->header.frame_id.c_str ());
+    PointCloud planar_hull;
+    if (!pcl_ros::transformPointCloud (cloud->header.frame_id, *hull, planar_hull, tf_listener_))
+    {
+      // Publish empty before return
+      pub_output_.publish (boost::make_shared<const pcl::PointIndices> (inliers));
+      return;
+    }
+    impl_.setInputPlanarHull (planar_hull.makeShared ());
+  }
+  else
+    impl_.setInputPlanarHull (hull);
+
+  IndicesPtr indices_ptr;
+  if (indices && !indices->header.frame_id.empty ())
+    indices_ptr.reset (new std::vector<int> (indices->indices));
+
+  impl_.setInputCloud (cloud);
+  impl_.setIndices (indices_ptr);
+
+  // Final check if the data is empty (remember that indices are set to the size of the data -- if indices* = NULL)
+  if (!cloud->points.empty ())
+    impl_.segment (inliers);
+  // Enforce that the TF frame and the timestamp are copied
+  inliers.header = cloud->header;
+  pub_output_.publish (boost::make_shared<const PointIndices> (inliers));
+  NODELET_DEBUG ("[%s::input_hull_callback] Publishing %zu indices.", getName ().c_str (), inliers.indices.size ());
+}
+
+typedef pcl_ros::ExtractPolygonalPrismData ExtractPolygonalPrismData;
+PLUGINLIB_DECLARE_CLASS (pcl, ExtractPolygonalPrismData, ExtractPolygonalPrismData, nodelet::Nodelet);
+

pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp

@@ -0,0 +1,645 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: sac_segmentation.hpp 33195 2010-10-10 14:12:19Z marton $
+ *
+ */
+
+#include <pluginlib/class_list_macros.h>
+#include "pcl_ros/segmentation/sac_segmentation.h"
+#include <pcl/io/io.h>
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::SACSegmentation::onInit ()
+{
+  // Call the super onInit ()
+  PCLNodelet::onInit ();
+
+  // If we're supposed to look for PointIndices (indices)
+  if (use_indices_)
+  {
+    // Subscribe to the input using a filter
+    sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
+    sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
+
+    // when "use_indices" is set to true, and "latched_indices" is set to true,
+    // we'll subscribe and get a separate callback for PointIndices that will 
+    // save the indices internally, and a PointCloud + PointIndices callback 
+    // will take care of meshing the new PointClouds with the old saved indices. 
+    if (latched_indices_)
+    {
+      // Subscribe to a callback that saves the indices
+      sub_indices_filter_.registerCallback (bind (&SACSegmentation::indices_callback, this, _1));
+      // Subscribe to a callback that sets the header of the saved indices to the cloud header
+      sub_input_filter_.registerCallback (bind (&SACSegmentation::input_callback, this, _1));
+
+      // Synchronize the two topics. No need for an approximate synchronizer here, as we'll
+      // match the timestamps exactly
+      sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointIndices> > > (max_queue_size_);
+      sync_input_indices_e_->connectInput (sub_input_filter_, nf_pi_);
+      sync_input_indices_e_->registerCallback (bind (&SACSegmentation::input_indices_callback, this, _1, _2));
+    }
+    // "latched_indices" not set, proceed with regular <input,indices> pairs
+    else
+    {
+      if (approximate_sync_)
+      {
+        sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointIndices> > > (max_queue_size_);
+        sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_);
+        sync_input_indices_a_->registerCallback (bind (&SACSegmentation::input_indices_callback, this, _1, _2));
+      }
+      else
+      {
+        sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointIndices> > > (max_queue_size_);
+        sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_);
+        sync_input_indices_e_->registerCallback (bind (&SACSegmentation::input_indices_callback, this, _1, _2));
+      }
+    }
+  }
+  else
+    // Subscribe in an old fashion to input only (no filters)
+    sub_input_ = pnh_->subscribe<PointCloud> ("input", max_queue_size_,  bind (&SACSegmentation::input_indices_callback, this, _1, PointIndicesConstPtr ()));
+
+  // Advertise the output topics
+  pub_indices_ = pnh_->advertise<PointIndices> ("inliers", max_queue_size_);
+  pub_model_   = pnh_->advertise<ModelCoefficients> ("model", max_queue_size_);
+
+  // ---[ Mandatory parameters
+  int model_type;
+  if (!pnh_->getParam ("model_type", model_type))
+  {
+    NODELET_ERROR ("[onInit] Need a 'model_type' parameter to be set before continuing!");
+    return;
+  }
+  double threshold; // unused - set via dynamic reconfigure in the callback
+  if (!pnh_->getParam ("distance_threshold", threshold))
+  {
+    NODELET_ERROR ("[onInit] Need a 'distance_threshold' parameter to be set before continuing!");
+    return;
+  }
+
+  // ---[ Optional parameters
+  int method_type = 0;
+  pnh_->getParam ("method_type", method_type);
+
+  XmlRpc::XmlRpcValue axis_param;
+  pnh_->getParam ("axis", axis_param);
+  Eigen::Vector3f axis = Eigen::Vector3f::Zero ();
+
+  switch (axis_param.getType ())
+  {
+    case XmlRpc::XmlRpcValue::TypeArray:
+    {
+      if (axis_param.size () != 3)
+      {
+        NODELET_ERROR ("[%s::onInit] Parameter 'axis' given but with a different number of values (%d) than required (3)!", getName ().c_str (), axis_param.size ());
+        return;
+      }
+      for (int i = 0; i < 3; ++i)
+      {
+        if (axis_param[i].getType () != XmlRpc::XmlRpcValue::TypeDouble)
+        {
+          NODELET_ERROR ("[%s::onInit] Need floating point values for 'axis' parameter.", getName ().c_str ());
+          return;
+        }
+        double value = axis_param[i]; axis[i] = value;
+      }
+      break;
+    }
+    default:
+    {
+      break;
+    }
+  }
+
+  // Initialize the random number generator
+  srand (time (0));
+
+  // Enable the dynamic reconfigure service
+  srv_ = boost::make_shared<dynamic_reconfigure::Server<SACSegmentationConfig> >(*pnh_);
+  dynamic_reconfigure::Server<SACSegmentationConfig>::CallbackType f =  boost::bind (&SACSegmentation::config_callback, this, _1, _2);
+  srv_->setCallback (f);
+
+  NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
+                 " - model_type               : %d\n"
+                 " - method_type              : %d\n"
+                 " - model_threshold          : %f\n"
+                 " - axis                     : [%f, %f, %f]\n",
+                 getName ().c_str (), model_type, method_type, threshold, 
+                 axis[0], axis[1], axis[2]);
+
+  // Set given parameters here
+  impl_.setModelType (model_type);
+  impl_.setMethodType (method_type);
+  impl_.setAxis (axis);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::SACSegmentation::config_callback (SACSegmentationConfig &config, uint32_t level)
+{
+  boost::mutex::scoped_lock lock (mutex_);
+
+  if (impl_.getDistanceThreshold () != config.distance_threshold)
+  {
+    //sac_->setDistanceThreshold (threshold_); - done in initSAC
+    impl_.setDistanceThreshold (config.distance_threshold);
+    NODELET_DEBUG ("[%s::config_callback] Setting new distance to model threshold to: %f.", getName ().c_str (), config.distance_threshold);
+  }
+  // The maximum allowed difference between the model normal and the given axis _in radians_
+  if (impl_.getEpsAngle () != config.eps_angle)
+  {
+    impl_.setEpsAngle (config.eps_angle);
+    NODELET_DEBUG ("[%s::config_callback] Setting new epsilon angle to model threshold to: %f (%f degrees).", getName ().c_str (), config.eps_angle, config.eps_angle * 180.0 / M_PI);
+  }
+  
+  // Number of inliers
+  if (min_inliers_ != config.min_inliers)
+  {
+    min_inliers_ = config.min_inliers;
+    NODELET_DEBUG ("[%s::config_callback] Setting new minimum number of inliers to: %d.",  getName ().c_str (), min_inliers_);
+  }
+
+  if (impl_.getMaxIterations () != config.max_iterations)
+  {
+    //sac_->setMaxIterations (max_iterations_); - done in initSAC
+    impl_.setMaxIterations (config.max_iterations);
+    NODELET_DEBUG ("[%s::config_callback] Setting new maximum number of iterations to: %d.",  getName ().c_str (), config.max_iterations);
+  }
+  if (impl_.getProbability () != config.probability)
+  {
+    //sac_->setProbability (probability_); - done in initSAC
+    impl_.setProbability (config.probability);
+    NODELET_DEBUG ("[%s::config_callback] Setting new probability to: %f.", getName ().c_str (), config.probability);
+  }
+  if (impl_.getOptimizeCoefficients () != config.optimize_coefficients)
+  {
+    impl_.setOptimizeCoefficients (config.optimize_coefficients);
+    NODELET_DEBUG ("[%s::config_callback] Setting coefficient optimization to: %s.", getName ().c_str (), (config.optimize_coefficients) ? "true" : "false");
+  }
+
+  double radius_min, radius_max;
+  impl_.getRadiusLimits (radius_min, radius_max);
+  if (radius_min != config.radius_min)
+  {
+    radius_min = config.radius_min;
+    NODELET_DEBUG ("[config_callback] Setting minimum allowable model radius to: %f.", radius_min);
+    impl_.setRadiusLimits (radius_min, radius_max);
+  }
+  if (radius_max != config.radius_max)
+  {
+    radius_max = config.radius_max;
+    NODELET_DEBUG ("[config_callback] Setting maximum allowable model radius to: %f.", radius_max);
+    impl_.setRadiusLimits (radius_min, radius_max);
+  }
+
+  if (tf_input_frame_ != config.input_frame)
+  {
+    tf_input_frame_ = config.input_frame;
+    NODELET_DEBUG ("[config_callback] Setting the input TF frame to: %s.", tf_input_frame_.c_str ());
+    NODELET_WARN ("input_frame TF not implemented yet!");
+  }
+  if (tf_output_frame_ != config.output_frame)
+  {
+    tf_output_frame_ = config.output_frame;
+    NODELET_DEBUG ("[config_callback] Setting the output TF frame to: %s.", tf_output_frame_.c_str ());
+    NODELET_WARN ("output_frame TF not implemented yet!");
+  }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::SACSegmentation::input_indices_callback (const PointCloudConstPtr &cloud, 
+                                                  const PointIndicesConstPtr &indices)
+{
+  boost::mutex::scoped_lock lock (mutex_);
+
+  // No subscribers, no work
+  if (pub_indices_.getNumSubscribers () <= 0 && pub_model_.getNumSubscribers () <= 0)
+    return;
+
+  PointIndices inliers;
+  ModelCoefficients model;
+  // Enforce that the TF frame and the timestamp are copied
+  inliers.header = model.header = cloud->header;
+
+  // If cloud is given, check if it's valid
+  if (!isValid (cloud)) 
+  {
+    NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
+    pub_indices_.publish (boost::make_shared<const PointIndices> (inliers));
+    pub_model_.publish (boost::make_shared<const ModelCoefficients> (model));
+    return;
+  }
+  // If indices are given, check if they are valid
+  if (indices && !isValid (indices))
+  {
+    NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
+    pub_indices_.publish (boost::make_shared<const PointIndices> (inliers));
+    pub_model_.publish (boost::make_shared<const ModelCoefficients> (model));
+    return;
+  }
+
+  /// DEBUG
+  if (indices && !indices->header.frame_id.empty ())
+    NODELET_DEBUG ("[%s::input_indices_callback]\n"
+                   "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
+                   "                                 - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
+                   getName ().c_str (), 
+                   cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
+                   indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
+  else
+    NODELET_DEBUG ("[%s::input_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", 
+                   getName ().c_str (), cloud->width * cloud->height, cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ());
+  ///
+
+  // Check whether the user has given a different input TF frame
+  tf_input_orig_frame_ = cloud->header.frame_id;
+  PointCloudConstPtr cloud_tf;
+/*  if (!tf_input_frame_.empty () && cloud->header.frame_id != tf_input_frame_)
+  {
+    NODELET_DEBUG ("[input_callback] Transforming input dataset from %s to %s.", cloud->header.frame_id.c_str (), tf_input_frame_.c_str ());
+    // Save the original frame ID
+    // Convert the cloud into the different frame
+    PointCloud cloud_transformed;
+    if (!pcl::transformPointCloud (tf_input_frame_, cloud->header.stamp, *cloud, cloud_transformed, tf_listener_))
+      return;
+    cloud_tf.reset (new PointCloud (cloud_transformed));
+  }
+  else*/
+    cloud_tf = cloud;
+
+  IndicesPtr indices_ptr;
+  if (indices && !indices->header.frame_id.empty ())
+    indices_ptr.reset (new std::vector<int> (indices->indices));
+
+  impl_.setInputCloud (cloud_tf);
+  impl_.setIndices (indices_ptr);
+
+  // Final check if the data is empty (remember that indices are set to the size of the data -- if indices* = NULL)
+  if (!cloud->points.empty ())
+    impl_.segment (inliers, model);
+
+  // Probably need to transform the model of the plane here
+
+  // Check if we have enough inliers, clear inliers + model if not
+  if ((int)inliers.indices.size () <= min_inliers_)
+  {
+    inliers.indices.clear (); 
+    model.values.clear ();
+  }
+
+  // Publish
+  pub_indices_.publish (boost::make_shared<const PointIndices> (inliers));
+  pub_model_.publish (boost::make_shared<const ModelCoefficients> (model));
+  NODELET_DEBUG ("[%s::input_indices_callback] Published PointIndices with %zu values on topic %s, and ModelCoefficients with %zu values on topic %s",
+                 getName ().c_str (), inliers.indices.size (), pnh_->resolveName ("inliers").c_str (),
+                 model.values.size (), pnh_->resolveName ("model").c_str ());
+
+  if (inliers.indices.empty ())
+    NODELET_WARN ("[%s::input_indices_callback] No inliers found!", getName ().c_str ());
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::SACSegmentationFromNormals::onInit ()
+{
+  // Call the super onInit ()
+  PCLNodelet::onInit ();
+
+  // Enable the dynamic reconfigure service
+  srv_ = boost::make_shared <dynamic_reconfigure::Server<SACSegmentationFromNormalsConfig> > (*pnh_);
+  dynamic_reconfigure::Server<SACSegmentationFromNormalsConfig>::CallbackType f = boost::bind (&SACSegmentationFromNormals::config_callback, this, _1, _2);
+  srv_->setCallback (f);
+
+  // Subscribe to the input and normals using filters
+  sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
+  sub_normals_filter_.subscribe (*pnh_, "normals", max_queue_size_);
+
+  // Subscribe to an axis direction along which the model search is to be constrained (the first 3 model coefficients will be checked)
+  sub_axis_ = pnh_->subscribe ("axis", 1, &SACSegmentationFromNormals::axis_callback, this);
+
+  if (approximate_sync_)
+    sync_input_normals_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointCloudN, PointIndices> > > (max_queue_size_);
+  else
+    sync_input_normals_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloudN, PointIndices> > > (max_queue_size_);
+
+  // If we're supposed to look for PointIndices (indices)
+  if (use_indices_)
+  {
+    // Subscribe to the input using a filter
+    sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
+
+    if (approximate_sync_)
+      sync_input_normals_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, sub_indices_filter_);
+    else
+      sync_input_normals_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, sub_indices_filter_);
+  }
+  else
+  {
+    // Create a different callback for copying over the timestamp to fake indices
+    sub_input_filter_.registerCallback (bind (&SACSegmentationFromNormals::input_callback, this, _1));
+
+    if (approximate_sync_)
+      sync_input_normals_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, nf_);
+    else
+      sync_input_normals_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, nf_);
+  }
+
+  if (approximate_sync_)
+    sync_input_normals_indices_a_->registerCallback (bind (&SACSegmentationFromNormals::input_normals_indices_callback, this, _1, _2, _3));
+  else
+    sync_input_normals_indices_e_->registerCallback (bind (&SACSegmentationFromNormals::input_normals_indices_callback, this, _1, _2, _3));
+
+  // Advertise the output topics
+  pub_indices_ = pnh_->advertise<PointIndices> ("inliers", max_queue_size_);
+  pub_model_   = pnh_->advertise<ModelCoefficients> ("model", max_queue_size_);
+
+  // ---[ Mandatory parameters
+  int model_type;
+  if (!pnh_->getParam ("model_type", model_type))
+  {
+    NODELET_ERROR ("[%s::onInit] Need a 'model_type' parameter to be set before continuing!", getName ().c_str ());
+    return;
+  }
+  double threshold; // unused - set via dynamic reconfigure in the callback
+  if (!pnh_->getParam ("distance_threshold", threshold))
+  {
+    NODELET_ERROR ("[%s::onInit] Need a 'distance_threshold' parameter to be set before continuing!", getName ().c_str ());
+    return;
+  }
+
+  // ---[ Optional parameters
+  int method_type = 0;
+  pnh_->getParam ("method_type", method_type);
+
+  XmlRpc::XmlRpcValue axis_param;
+  pnh_->getParam ("axis", axis_param);
+  Eigen::Vector3f axis = Eigen::Vector3f::Zero ();
+
+  switch (axis_param.getType ())
+  {
+    case XmlRpc::XmlRpcValue::TypeArray:
+    {
+      if (axis_param.size () != 3)
+      {
+        NODELET_ERROR ("[%s::onInit] Parameter 'axis' given but with a different number of values (%d) than required (3)!", getName ().c_str (), axis_param.size ());
+        return;
+      }
+      for (int i = 0; i < 3; ++i)
+      {
+        if (axis_param[i].getType () != XmlRpc::XmlRpcValue::TypeDouble)
+        {
+          NODELET_ERROR ("[%s::onInit] Need floating point values for 'axis' parameter.", getName ().c_str ());
+          return;
+        }
+        double value = axis_param[i]; axis[i] = value;
+      }
+      break;
+    }
+    default:
+    {
+      break;
+    }
+  }
+
+  // Initialize the random number generator
+  srand (time (0));
+
+  NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
+                 " - model_type               : %d\n"
+                 " - method_type              : %d\n"
+                 " - model_threshold          : %f\n"
+                 " - axis                     : [%f, %f, %f]\n",
+                 getName ().c_str (), model_type, method_type, threshold, 
+                 axis[0], axis[1], axis[2]);
+
+  // Set given parameters here
+  impl_.setModelType (model_type);
+  impl_.setMethodType (method_type);
+  impl_.setAxis (axis);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::SACSegmentationFromNormals::axis_callback (const pcl::ModelCoefficientsConstPtr &model)
+{
+  boost::mutex::scoped_lock lock (mutex_);
+
+  if (model->values.size () < 3)
+  {
+    NODELET_ERROR ("[%s::axis_callback] Invalid axis direction / model coefficients with %zu values sent on %s!", getName ().c_str (), model->values.size (), pnh_->resolveName ("axis").c_str ());
+    return;
+  }
+  NODELET_DEBUG ("[%s::axis_callback] Received axis direction: %f %f %f", getName ().c_str (), model->values[0], model->values[1], model->values[2]);
+
+  Eigen::Vector3f axis (model->values[0], model->values[1], model->values[2]);
+  impl_.setAxis (axis);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::SACSegmentationFromNormals::config_callback (SACSegmentationFromNormalsConfig &config, uint32_t level)
+{
+  boost::mutex::scoped_lock lock (mutex_);
+
+  if (impl_.getDistanceThreshold () != config.distance_threshold)
+  {
+    impl_.setDistanceThreshold (config.distance_threshold);
+    NODELET_DEBUG ("[%s::config_callback] Setting distance to model threshold to: %f.", getName ().c_str (), config.distance_threshold);
+  }
+  // The maximum allowed difference between the model normal and the given axis _in radians_
+  if (impl_.getEpsAngle () != config.eps_angle)
+  {
+    impl_.setEpsAngle (config.eps_angle);
+    NODELET_DEBUG ("[%s::config_callback] Setting new epsilon angle to model threshold to: %f (%f degrees).", getName ().c_str (), config.eps_angle, config.eps_angle * 180.0 / M_PI);
+  }
+
+  if (impl_.getMaxIterations () != config.max_iterations)
+  {
+    impl_.setMaxIterations (config.max_iterations);
+    NODELET_DEBUG ("[%s::config_callback] Setting new maximum number of iterations to: %d.", getName ().c_str (), config.max_iterations);
+  }
+  
+  // Number of inliers
+  if (min_inliers_ != config.min_inliers)
+  {
+    min_inliers_ = config.min_inliers;
+    NODELET_DEBUG ("[%s::config_callback] Setting new minimum number of inliers to: %d.",  getName ().c_str (), min_inliers_);
+  }
+
+
+  if (impl_.getProbability () != config.probability)
+  {
+    impl_.setProbability (config.probability);
+    NODELET_DEBUG ("[%s::config_callback] Setting new probability to: %f.", getName ().c_str (), config.probability);
+  }
+
+  if (impl_.getOptimizeCoefficients () != config.optimize_coefficients)
+  {
+    impl_.setOptimizeCoefficients (config.optimize_coefficients);
+    NODELET_DEBUG ("[%s::config_callback] Setting coefficient optimization to: %s.", getName ().c_str (), (config.optimize_coefficients) ? "true" : "false");
+  }
+
+  if (impl_.getNormalDistanceWeight () != config.normal_distance_weight)
+  {
+    impl_.setNormalDistanceWeight (config.normal_distance_weight);
+    NODELET_DEBUG ("[%s::config_callback] Setting new distance weight to: %f.", getName ().c_str (), config.normal_distance_weight);
+  }
+
+  double radius_min, radius_max;
+  impl_.getRadiusLimits (radius_min, radius_max);
+  if (radius_min != config.radius_min)
+  {
+    radius_min = config.radius_min;
+    NODELET_DEBUG ("[%s::config_callback] Setting minimum allowable model radius to: %f.", getName ().c_str (), radius_min);
+    impl_.setRadiusLimits (radius_min, radius_max);
+  }
+  if (radius_max != config.radius_max)
+  {
+    radius_max = config.radius_max;
+    NODELET_DEBUG ("[%s::config_callback] Setting maximum allowable model radius to: %f.", getName ().c_str (), radius_max);
+    impl_.setRadiusLimits (radius_min, radius_max);
+  }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback (
+      const PointCloudConstPtr &cloud, 
+      const PointCloudNConstPtr &cloud_normals,
+      const PointIndicesConstPtr &indices
+      )
+{
+  boost::mutex::scoped_lock lock (mutex_);
+
+  // No subscribers, no work
+  if (pub_indices_.getNumSubscribers () <= 0 && pub_model_.getNumSubscribers () <= 0)
+    return;
+
+  PointIndices inliers;
+  ModelCoefficients model;
+  // Enforce that the TF frame and the timestamp are copied
+  inliers.header = model.header = cloud->header;
+
+  if (impl_.getModelType () < 0)
+  {
+    NODELET_ERROR ("[%s::input_normals_indices_callback] Model type not set!", getName ().c_str ());
+    pub_indices_.publish (boost::make_shared<const PointIndices> (inliers));
+    pub_model_.publish (boost::make_shared<const ModelCoefficients> (model));
+    return;
+  }
+
+  if (!isValid (cloud))// || !isValid (cloud_normals, "normals")) 
+  {
+    NODELET_ERROR ("[%s::input_normals_indices_callback] Invalid input!", getName ().c_str ());
+    pub_indices_.publish (boost::make_shared<const PointIndices> (inliers));
+    pub_model_.publish (boost::make_shared<const ModelCoefficients> (model));
+    return;
+  }
+  // If indices are given, check if they are valid
+  if (indices && !isValid (indices))
+  {
+    NODELET_ERROR ("[%s::input_normals_indices_callback] Invalid indices!", getName ().c_str ());
+    pub_indices_.publish (boost::make_shared<const PointIndices> (inliers));
+    pub_model_.publish (boost::make_shared<const ModelCoefficients> (model));
+    return;
+  }
+
+  /// DEBUG
+  if (indices && !indices->header.frame_id.empty ())
+    NODELET_DEBUG ("[%s::input_normals_indices_callback]\n"
+                   "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
+                   "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
+                   "                                 - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
+                   getName ().c_str (), 
+                   cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
+                   cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), cloud_normals->header.stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str (),
+                   indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
+  else
+    NODELET_DEBUG ("[%s::input_normals_indices_callback]\n"
+                   "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
+                   "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
+                   getName ().c_str (), 
+                   cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
+                   cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), cloud_normals->header.stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str ());
+  ///
+
+
+  // Extra checks for safety
+  int cloud_nr_points         = cloud->width * cloud->height;
+  int cloud_normals_nr_points = cloud_normals->width * cloud_normals->height;
+  if (cloud_nr_points != cloud_normals_nr_points)
+  {
+    NODELET_ERROR ("[%s::input_normals_indices_callback] Number of points in the input dataset (%d) differs from the number of points in the normals (%d)!", getName ().c_str (), cloud_nr_points, cloud_normals_nr_points);
+    pub_indices_.publish (boost::make_shared<const PointIndices> (inliers));
+    pub_model_.publish (boost::make_shared<const ModelCoefficients> (model));
+    return;
+  }
+
+  impl_.setInputCloud (cloud);
+  impl_.setInputNormals (cloud_normals);
+
+  IndicesPtr indices_ptr;
+  if (indices && !indices->header.frame_id.empty ())
+    indices_ptr.reset (new std::vector<int> (indices->indices));
+
+  impl_.setIndices (indices_ptr);
+
+  // Final check if the data is empty (remember that indices are set to the size of the data -- if indices* = NULL)
+  if (!cloud->points.empty ())
+    impl_.segment (inliers, model);
+
+  // Check if we have enough inliers, clear inliers + model if not
+  if ((int)inliers.indices.size () <= min_inliers_)
+  {
+    inliers.indices.clear (); 
+    model.values.clear ();
+  }
+
+  // Publish
+  pub_indices_.publish (boost::make_shared<const PointIndices> (inliers));
+  pub_model_.publish (boost::make_shared<const ModelCoefficients> (model));
+  NODELET_DEBUG ("[%s::input_normals_callback] Published PointIndices with %zu values on topic %s, and ModelCoefficients with %zu values on topic %s", 
+      getName ().c_str (), inliers.indices.size (), pnh_->resolveName ("inliers").c_str (),
+      model.values.size (), pnh_->resolveName ("model").c_str ());
+  if (inliers.indices.empty ())
+    NODELET_WARN ("[%s::input_indices_callback] No inliers found!", getName ().c_str ());
+}
+
+typedef pcl_ros::SACSegmentation SACSegmentation;
+typedef pcl_ros::SACSegmentationFromNormals SACSegmentationFromNormals;
+PLUGINLIB_DECLARE_CLASS (pcl, SACSegmentation, SACSegmentation, nodelet::Nodelet);
+PLUGINLIB_DECLARE_CLASS (pcl, SACSegmentationFromNormals, SACSegmentationFromNormals, nodelet::Nodelet);
+

pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp

@@ -0,0 +1,128 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2010, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: segment_differences.cpp 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#include <pluginlib/class_list_macros.h>
+#include "pcl_ros/segmentation/segment_differences.h"
+#include <pcl/io/io.h>
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::SegmentDifferences::onInit ()
+{
+  // Call the super onInit ()
+  PCLNodelet::onInit ();
+
+  pub_output_ = pnh_->advertise<PointCloud> ("output", max_queue_size_);
+
+  // Subscribe to the input using a filter
+  sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
+  sub_target_filter_.subscribe (*pnh_, "target", max_queue_size_);
+
+  // Enable the dynamic reconfigure service
+  srv_ = boost::make_shared <dynamic_reconfigure::Server<SegmentDifferencesConfig> > (*pnh_);
+  dynamic_reconfigure::Server<SegmentDifferencesConfig>::CallbackType f =  boost::bind (&SegmentDifferences::config_callback, this, _1, _2);
+  srv_->setCallback (f);
+
+  if (approximate_sync_)
+  {
+    sync_input_target_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointCloud> > > (max_queue_size_);
+    sync_input_target_a_->connectInput (sub_input_filter_, sub_target_filter_);
+    sync_input_target_a_->registerCallback (bind (&SegmentDifferences::input_target_callback, this, _1, _2));
+  }
+  else
+  {
+    sync_input_target_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloud> > > (max_queue_size_);
+    sync_input_target_e_->connectInput (sub_input_filter_, sub_target_filter_);
+    sync_input_target_e_->registerCallback (bind (&SegmentDifferences::input_target_callback, this, _1, _2));
+  }
+
+  NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
+                 " - max_queue_size    : %d",
+                 getName ().c_str (),
+                 max_queue_size_);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::SegmentDifferences::config_callback (SegmentDifferencesConfig &config, uint32_t level)
+{
+  if (impl_.getDistanceThreshold () != config.distance_threshold)
+  {
+    impl_.setDistanceThreshold (config.distance_threshold);
+    NODELET_DEBUG ("[%s::config_callback] Setting new distance threshold to: %f.", getName ().c_str (), config.distance_threshold);
+  }
+}
+
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::SegmentDifferences::input_target_callback (const PointCloudConstPtr &cloud, 
+                                                    const PointCloudConstPtr &cloud_target)
+{
+  if (pub_output_.getNumSubscribers () <= 0)
+    return;
+
+  if (!isValid (cloud) || !isValid (cloud_target, "target")) 
+  {
+    NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
+    PointCloud output;
+    output.header = cloud->header;
+    pub_output_.publish (output.makeShared ());
+    return;
+  }
+
+  NODELET_DEBUG ("[%s::input_indices_callback]\n"
+                 "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
+                 "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
+                 getName ().c_str (),
+                 cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
+                 cloud_target->width * cloud_target->height, pcl::getFieldsList (*cloud_target).c_str (), cloud_target->header.stamp.toSec (), cloud_target->header.frame_id.c_str (), pnh_->resolveName ("target").c_str ());
+
+  impl_.setInputCloud (cloud);
+  impl_.setTargetCloud (cloud_target);
+
+  PointCloud output;
+  impl_.segment (output);
+
+  pub_output_.publish (output.makeShared ());
+  NODELET_DEBUG ("[%s::segmentAndPublish] Published PointCloud2 with %zu points and stamp %f on topic %s", getName ().c_str (),
+                     output.points.size (), output.header.stamp.toSec (), pnh_->resolveName ("output").c_str ());
+}
+
+typedef pcl_ros::SegmentDifferences SegmentDifferences;
+PLUGINLIB_DECLARE_CLASS (pcl, SegmentDifferences, SegmentDifferences, nodelet::Nodelet);
+

pcl_ros/src/pcl_ros/segmentation/segmentation.cpp

@@ -0,0 +1,44 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: segmentation.cpp 34600 2010-12-07 21:12:11Z rusu $
+ *
+ */
+
+// Include the implementations here instead of compiling them separately to speed up compile time
+//#include "extract_clusters.cpp"
+//#include "extract_polygonal_prism_data.cpp"
+//#include "sac_segmentation.cpp"
+//#include "segment_differences.cpp"
+
+

pcl_ros/src/pcl_ros/surface/convex_hull.cpp

@@ -0,0 +1,182 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2010, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: convex_hull.hpp 32993 2010-09-30 23:08:57Z rusu $
+ *
+ */
+
+#include <pluginlib/class_list_macros.h>
+#include <pcl/common/io.h>
+#include "pcl_ros/surface/convex_hull.h"
+#include <geometry_msgs/PolygonStamped.h>
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::ConvexHull2D::onInit ()
+{
+  ros::NodeHandle pnh = getMTPrivateNodeHandle ();
+  pub_output_ = pnh.advertise<PointCloud> ("output", max_queue_size_);
+  pub_plane_  = pnh.advertise<geometry_msgs::PolygonStamped>("output_polygon", max_queue_size_);
+
+  // ---[ Optional parameters
+  pnh.getParam ("use_indices", use_indices_);
+
+  // If we're supposed to look for PointIndices (indices)
+  if (use_indices_)
+  {
+    // Subscribe to the input using a filter
+    sub_input_filter_.subscribe (pnh, "input", 1);
+    // If indices are enabled, subscribe to the indices
+    sub_indices_filter_.subscribe (pnh, "indices", 1);
+
+    if (approximate_sync_)
+    {
+      sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointIndices> > >(max_queue_size_);
+      // surface not enabled, connect the input-indices duo and register
+      sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_);
+      sync_input_indices_a_->registerCallback (bind (&ConvexHull2D::input_indices_callback, this, _1, _2));
+    }
+    else
+    {
+      sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointIndices> > >(max_queue_size_);
+      // surface not enabled, connect the input-indices duo and register
+      sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_);
+      sync_input_indices_e_->registerCallback (bind (&ConvexHull2D::input_indices_callback, this, _1, _2));
+    }
+  }
+  else
+    // Subscribe in an old fashion to input only (no filters)
+    sub_input_ = pnh.subscribe<PointCloud> ("input", 1,  bind (&ConvexHull2D::input_indices_callback, this, _1, PointIndicesConstPtr ()));
+
+  NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
+                 " - use_indices    : %s",
+                 getName ().c_str (), 
+                 (use_indices_) ? "true" : "false");
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+  pcl_ros::ConvexHull2D::input_indices_callback (const PointCloudConstPtr &cloud, 
+                                                 const PointIndicesConstPtr &indices)
+{
+  // No subscribers, no work
+  if (pub_output_.getNumSubscribers () <= 0 && pub_plane_.getNumSubscribers () <= 0)
+    return;
+
+  PointCloud output;
+
+  // If cloud is given, check if it's valid
+  if (!isValid (cloud))
+  {
+    NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
+    // Publish an empty message
+    output.header = cloud->header;
+    pub_output_.publish (output.makeShared ());
+    return;
+  }
+  // If indices are given, check if they are valid
+  if (indices && !isValid (indices, "indices"))
+  {
+    NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
+    // Publish an empty message
+    output.header = cloud->header;
+    pub_output_.publish (output.makeShared ());
+    return;
+  }
+
+  /// DEBUG
+  if (indices)
+    NODELET_DEBUG ("[%s::input_indices_model_callback]\n"
+                   "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
+                   "                                 - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
+                   getName ().c_str (),
+                   cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str (),
+                   indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("indices").c_str ());
+  else
+    NODELET_DEBUG ("[%s::input_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str ());
+
+  // Reset the indices and surface pointers
+  IndicesPtr indices_ptr;
+  if (indices)
+    indices_ptr.reset (new std::vector<int> (indices->indices));
+
+  impl_.setInputCloud (cloud);
+  impl_.setIndices (indices_ptr);
+
+  // Estimate the feature
+  impl_.reconstruct (output);
+
+  // If more than 3 points are present, send a PolygonStamped hull too
+  if (output.points.size () >= 3)
+  {
+    geometry_msgs::PolygonStamped poly;
+    poly.header = output.header;
+    poly.polygon.points.resize (output.points.size ());
+    // Get three consecutive points (without copying)
+    pcl::Vector4fMap O = output.points[1].getVector4fMap ();
+    pcl::Vector4fMap B = output.points[0].getVector4fMap ();
+    pcl::Vector4fMap A = output.points[2].getVector4fMap ();
+    // Check the direction of points -- polygon must have CCW direction
+    Eigen::Vector4f OA = A - O;
+    Eigen::Vector4f OB = B - O;
+    Eigen::Vector4f N = OA.cross3 (OB);
+    double theta = N.dot (O);
+    bool reversed = false;
+    if (theta < (M_PI / 2.0))
+      reversed = true;
+    for (size_t i = 0; i < output.points.size (); ++i)
+    {
+      if (reversed)
+      {
+        size_t j = output.points.size () - i - 1;
+        poly.polygon.points[i].x = output.points[j].x;
+        poly.polygon.points[i].y = output.points[j].y;
+        poly.polygon.points[i].z = output.points[j].z;
+      }
+      else
+      {
+        poly.polygon.points[i].x = output.points[i].x;
+        poly.polygon.points[i].y = output.points[i].y;
+        poly.polygon.points[i].z = output.points[i].z;
+      }
+    }
+    pub_plane_.publish (boost::make_shared<const geometry_msgs::PolygonStamped> (poly));
+  }
+  // Publish a Boost shared ptr const data
+  output.header = cloud->header;
+  pub_output_.publish (output.makeShared ());
+}
+
+typedef pcl_ros::ConvexHull2D ConvexHull2D;
+PLUGINLIB_DECLARE_CLASS (pcl, ConvexHull2D, ConvexHull2D, nodelet::Nodelet);
+

pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp

@@ -0,0 +1,217 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2010, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: moving_least_squares.cpp 36097 2011-02-20 14:18:58Z marton $
+ *
+ */
+
+#include <pluginlib/class_list_macros.h>
+#include "pcl_ros/surface/moving_least_squares.h"
+#include <pcl/io/io.h>
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::MovingLeastSquares::onInit ()
+{
+  PCLNodelet::onInit ();
+  
+  //ros::NodeHandle private_nh = getMTPrivateNodeHandle ();
+  pub_output_ = pnh_->advertise<PointCloudIn> ("output", max_queue_size_);
+  pub_normals_ = pnh_->advertise<NormalCloudOut> ("normals", max_queue_size_);
+  
+  //if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("search_radius", search_radius_))  
+  if (!pnh_->getParam ("search_radius", search_radius_))
+  {
+    //NODELET_ERROR ("[%s::onInit] Neither 'k_search' nor 'search_radius' set! Need to set at least one of these parameters before continuing.", getName ().c_str ());
+    NODELET_ERROR ("[%s::onInit] Need a 'search_radius' parameter to be set before continuing!", getName ().c_str ());
+    return;
+  }
+  if (!pnh_->getParam ("spatial_locator", spatial_locator_type_))
+  { 
+    NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ());
+    return;
+  }
+
+  // Enable the dynamic reconfigure service
+  srv_ = boost::make_shared<dynamic_reconfigure::Server<MLSConfig> > (*pnh_);
+  dynamic_reconfigure::Server<MLSConfig>::CallbackType f = boost::bind (&MovingLeastSquares::config_callback, this, _1, _2 );
+  srv_->setCallback (f); 
+
+  // ---[ Optional parameters
+  pnh_->getParam ("use_indices", use_indices_);
+
+  // If we're supposed to look for PointIndices (indices)
+  if (use_indices_)
+  {
+    // Subscribe to the input using a filter
+    sub_input_filter_.subscribe (*pnh_, "input", 1);
+    // If indices are enabled, subscribe to the indices
+    sub_indices_filter_.subscribe (*pnh_, "indices", 1);
+
+    if (approximate_sync_)
+    {
+      sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<PointCloudIn, PointIndices> > >(max_queue_size_);
+      // surface not enabled, connect the input-indices duo and register
+      sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_);
+      sync_input_indices_a_->registerCallback (bind (&MovingLeastSquares::input_indices_callback, this, _1, _2));
+    }
+    else
+    {
+      sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ExactTime<PointCloudIn, PointIndices> > >(max_queue_size_);
+      // surface not enabled, connect the input-indices duo and register
+      sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_);
+      sync_input_indices_e_->registerCallback (bind (&MovingLeastSquares::input_indices_callback, this, _1, _2));
+    }
+  }
+  else
+    // Subscribe in an old fashion to input only (no filters)
+    sub_input_ = pnh_->subscribe<PointCloudIn> ("input", 1,  bind (&MovingLeastSquares::input_indices_callback, this, _1, PointIndicesConstPtr ()));
+
+
+  NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
+                 " - use_indices    : %s",
+                 getName ().c_str (), 
+                 (use_indices_) ? "true" : "false");
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::MovingLeastSquares::input_indices_callback (const PointCloudInConstPtr &cloud, 
+                                                     const PointIndicesConstPtr &indices)
+{
+  // No subscribers, no work
+  if (pub_output_.getNumSubscribers () <= 0 && pub_normals_.getNumSubscribers () <= 0)
+    return;
+
+  // Output points have the same type as the input, they are only smoothed
+  PointCloudIn output;
+  
+  // Normals are also estimated and published on a separate topic
+  NormalCloudOut::Ptr normals (new NormalCloudOut ());
+
+  // If cloud is given, check if it's valid
+  if (!isValid (cloud)) 
+  {
+    NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
+    output.header = cloud->header;
+    pub_output_.publish (output.makeShared ());
+    return;
+  }
+  // If indices are given, check if they are valid
+  if (indices && !isValid (indices, "indices"))
+  {
+    NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
+    output.header = cloud->header;
+    pub_output_.publish (output.makeShared ());
+    return;
+  }
+
+  /// DEBUG
+  if (indices)
+    NODELET_DEBUG ("[%s::input_indices_model_callback]\n"
+                   "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
+                   "                                 - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
+                   getName ().c_str (),
+                   cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str (),
+                   indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("indices").c_str ());
+  else
+    NODELET_DEBUG ("[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str ());
+  ///
+
+  // Reset the indices and surface pointers
+  impl_.setInputCloud (cloud);
+  impl_.setOutputNormals (normals);
+
+  IndicesPtr indices_ptr;
+  if (indices)
+    indices_ptr.reset (new std::vector<int> (indices->indices));
+
+  impl_.setIndices (indices_ptr);
+
+  // Initialize the spatial locator
+  initTree (spatial_locator_type_, tree_, 0); //k_);
+  impl_.setSearchMethod (tree_);
+  
+  // Do the reconstructon
+  impl_.reconstruct (output);
+
+  // Publish a Boost shared ptr const data
+  // Enforce that the TF frame and the timestamp are copied
+  output.header = cloud->header;
+  pub_output_.publish (output.makeShared ());
+  normals->header = cloud->header;
+  pub_normals_.publish (normals);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::MovingLeastSquares::config_callback (MLSConfig &config, uint32_t level)
+{
+  // \Note Zoli, shouldn't this be implemented in MLS too?
+  /*if (k_ != config.k_search)
+  {
+    k_ = config.k_search;
+    NODELET_DEBUG ("[config_callback] Setting the k_search to: %d.", k_); 
+  }*/
+  if (search_radius_ != config.search_radius)
+  {
+    search_radius_ = config.search_radius;
+    NODELET_DEBUG ("[config_callback] Setting the search radius: %f.", search_radius_);
+    impl_.setSearchRadius (search_radius_);
+  }
+  if (spatial_locator_type_ != config.spatial_locator)
+  {
+    spatial_locator_type_ = config.spatial_locator;
+    NODELET_DEBUG ("[config_callback] Setting the spatial locator to type: %d.", spatial_locator_type_);
+  }
+  if (use_polynomial_fit_ != config.use_polynomial_fit)
+  {
+    use_polynomial_fit_ = config.use_polynomial_fit;
+    NODELET_DEBUG ("[config_callback] Setting the use_polynomial_fit flag to: %d.", use_polynomial_fit_);
+    impl_.setPolynomialFit (use_polynomial_fit_);
+  }
+  if (polynomial_order_ != config.polynomial_order)
+  {
+    polynomial_order_ = config.polynomial_order;
+    NODELET_DEBUG ("[config_callback] Setting the polynomial order to: %d.", polynomial_order_);
+    impl_.setPolynomialOrder (polynomial_order_);
+  }
+  if (gaussian_parameter_ != config.gaussian_parameter)
+  {
+    gaussian_parameter_ = config.gaussian_parameter;
+    NODELET_DEBUG ("[config_callback] Setting the gaussian parameter to: %f.", gaussian_parameter_);
+    impl_.setSqrGaussParam (gaussian_parameter_ * gaussian_parameter_);
+  }
+}
+
+typedef pcl_ros::MovingLeastSquares MovingLeastSquares;
+PLUGINLIB_DECLARE_CLASS (pcl, MovingLeastSquares, MovingLeastSquares, nodelet::Nodelet);

pcl_ros/src/pcl_ros/surface/surface.cpp

@@ -0,0 +1,48 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: surface.cpp 34603 2010-12-07 22:42:10Z rusu $
+ *
+ */
+
+/**
+
+\author Radu Bogdan Rusu
+
+**/
+
+// Include the implementations here instead of compiling them separately to speed up compile time
+//#include "convex_hull.cpp"
+//#include "moving_least_squares.cpp"
+
+