diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 0054a16e..24f506ae 100644 --- a/pcl_ros/CMakeLists.txt +++ b/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} diff --git a/pcl_ros/cfg/EuclideanClusterExtraction.cfg b/pcl_ros/cfg/EuclideanClusterExtraction.cfg new file mode 100755 index 00000000..50ae3729 --- /dev/null +++ b/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")) + diff --git a/pcl_ros/cfg/ExtractPolygonalPrismData.cfg b/pcl_ros/cfg/ExtractPolygonalPrismData.cfg new file mode 100755 index 00000000..109770a9 --- /dev/null +++ b/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")) diff --git a/pcl_ros/cfg/Feature.cfg b/pcl_ros/cfg/Feature.cfg new file mode 100755 index 00000000..21fe0b03 --- /dev/null +++ b/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")) + diff --git a/pcl_ros/cfg/MLS.cfg b/pcl_ros/cfg/MLS.cfg new file mode 100755 index 00000000..db6c2bdf --- /dev/null +++ b/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")) + diff --git a/pcl_ros/cfg/SACSegmentation.cfg b/pcl_ros/cfg/SACSegmentation.cfg new file mode 100755 index 00000000..8068e5c6 --- /dev/null +++ b/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")) + diff --git a/pcl_ros/cfg/SACSegmentationFromNormals.cfg b/pcl_ros/cfg/SACSegmentationFromNormals.cfg new file mode 100755 index 00000000..d9c3a854 --- /dev/null +++ b/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")) diff --git a/pcl_ros/cfg/SACSegmentation_common.py b/pcl_ros/cfg/SACSegmentation_common.py new file mode 100644 index 00000000..8b2c6668 --- /dev/null +++ b/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.", "") + diff --git a/pcl_ros/cfg/SegmentDifferences.cfg b/pcl_ros/cfg/SegmentDifferences.cfg new file mode 100755 index 00000000..0cc5172d --- /dev/null +++ b/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")) + diff --git a/pcl_ros/include/pcl_ros/features/boundary.h b/pcl_ros/include/pcl_ros/features/boundary.h new file mode 100644 index 00000000..bb91cd61 --- /dev/null +++ b/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 +#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 impl_; + + typedef pcl::PointCloud PointCloudOut; + + /** \brief Child initialization routine. Internal method. */ + inline bool + childInit (ros::NodeHandle &nh) + { + // Create the output publisher + pub_output_ = nh.advertise ("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_ + + diff --git a/pcl_ros/include/pcl_ros/features/feature.h b/pcl_ros/include/pcl_ros/features/feature.h new file mode 100644 index 00000000..7ef9d644 --- /dev/null +++ b/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 +#include + +#include "pcl_ros/pcl_nodelet.h" +#include + +// Dynamic reconfigure +#include +#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 KdTree; + typedef pcl::KdTree::Ptr KdTreePtr; + + typedef pcl::PointCloud PointCloudIn; + typedef PointCloudIn::Ptr PointCloudInPtr; + typedef PointCloudIn::ConstPtr PointCloudInConstPtr; + + typedef boost::shared_ptr > IndicesPtr; + typedef boost::shared_ptr > 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 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 > 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 nf_pi_; + message_filters::PassThrough 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 (indices)); + } + + private: + /** \brief Synchronized input, surface, and point indices.*/ + boost::shared_ptr > > sync_input_surface_indices_a_; + boost::shared_ptr > > 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 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 sub_normals_filter_; + + /** \brief Synchronized input, normals, surface, and point indices.*/ + boost::shared_ptr > > sync_input_normals_surface_indices_a_; + boost::shared_ptr > > 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_ diff --git a/pcl_ros/include/pcl_ros/features/fpfh.h b/pcl_ros/include/pcl_ros/features/fpfh.h new file mode 100644 index 00000000..aca59cd3 --- /dev/null +++ b/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 +#include "pcl_ros/features/pfh.h" + +namespace pcl_ros +{ + /** \brief @b FPFHEstimation estimates the Fast Point Feature Histogram (FPFH) descriptor for a given point cloud + * dataset containing points and normals. + * + * @note If you use this code in any academic work, please cite: + * + *
    + *
  • 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. + *
    • + *
  • 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. + *
    • + *
+ * + * @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 impl_; + + typedef pcl::PointCloud PointCloudOut; + + /** \brief Child initialization routine. Internal method. */ + inline bool + childInit (ros::NodeHandle &nh) + { + // Create the output publisher + pub_output_ = nh.advertise ("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_ + + diff --git a/pcl_ros/include/pcl_ros/features/fpfh_omp.h b/pcl_ros/include/pcl_ros/features/fpfh_omp.h new file mode 100644 index 00000000..2db8eb18 --- /dev/null +++ b/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 +#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: + * + *
    + *
  • 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. + *
    • + *
  • 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. + *
    • + *
+ * \author Radu Bogdan Rusu + */ + class FPFHEstimationOMP : public FeatureFromNormals + { + private: + pcl::FPFHEstimationOMP impl_; + + typedef pcl::PointCloud PointCloudOut; + + /** \brief Child initialization routine. Internal method. */ + inline bool + childInit (ros::NodeHandle &nh) + { + // Create the output publisher + pub_output_ = nh.advertise ("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_ + diff --git a/pcl_ros/include/pcl_ros/features/moment_invariants.h b/pcl_ros/include/pcl_ros/features/moment_invariants.h new file mode 100644 index 00000000..ef6df20a --- /dev/null +++ b/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 +#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 impl_; + + typedef pcl::PointCloud PointCloudOut; + + /** \brief Child initialization routine. Internal method. */ + inline bool + childInit (ros::NodeHandle &nh) + { + // Create the output publisher + pub_output_ = nh.advertise ("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_ + + diff --git a/pcl_ros/include/pcl_ros/features/normal_3d.h b/pcl_ros/include/pcl_ros/features/normal_3d.h new file mode 100644 index 00000000..7a1506e4 --- /dev/null +++ b/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 +#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 impl_; + + typedef pcl::PointCloud PointCloudOut; + + /** \brief Child initialization routine. Internal method. */ + inline bool + childInit (ros::NodeHandle &nh) + { + // Create the output publisher + pub_output_ = nh.advertise ("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_ + diff --git a/pcl_ros/include/pcl_ros/features/normal_3d_omp.h b/pcl_ros/include/pcl_ros/features/normal_3d_omp.h new file mode 100644 index 00000000..ab265162 --- /dev/null +++ b/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 +#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 impl_; + + typedef pcl::PointCloud PointCloudOut; + + /** \brief Child initialization routine. Internal method. */ + inline bool + childInit (ros::NodeHandle &nh) + { + // Create the output publisher + pub_output_ = nh.advertise ("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_ diff --git a/pcl_ros/include/pcl_ros/features/normal_3d_tbb.h b/pcl_ros/include/pcl_ros/features/normal_3d_tbb.h new file mode 100644 index 00000000..ec570f75 --- /dev/null +++ b/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 +#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 impl_; + + typedef pcl::PointCloud PointCloudOut; + + /** \brief Child initialization routine. Internal method. */ + inline bool + childInit (ros::NodeHandle &nh) + { + // Create the output publisher + pub_output_ = nh.advertise ("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_ + + diff --git a/pcl_ros/include/pcl_ros/features/pfh.h b/pcl_ros/include/pcl_ros/features/pfh.h new file mode 100644 index 00000000..262ce37c --- /dev/null +++ b/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 +#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: + * + *
    + *
  • 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. + *
    • + *
  • 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. + *
    • + *
+ * + * @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 impl_; + + typedef pcl::PointCloud PointCloudOut; + + /** \brief Child initialization routine. Internal method. */ + inline bool + childInit (ros::NodeHandle &nh) + { + // Create the output publisher + pub_output_ = nh.advertise ("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_ + + diff --git a/pcl_ros/include/pcl_ros/features/principal_curvatures.h b/pcl_ros/include/pcl_ros/features/principal_curvatures.h new file mode 100644 index 00000000..7a0ddfb6 --- /dev/null +++ b/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 +#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 impl_; + + typedef pcl::PointCloud PointCloudOut; + + /** \brief Child initialization routine. Internal method. */ + inline bool + childInit (ros::NodeHandle &nh) + { + // Create the output publisher + pub_output_ = nh.advertise ("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_ diff --git a/pcl_ros/include/pcl_ros/features/vfh.h b/pcl_ros/include/pcl_ros/features/vfh.h new file mode 100644 index 00000000..90e588f1 --- /dev/null +++ b/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 +#include "pcl_ros/features/fpfh.h" + +namespace pcl_ros +{ + /** \brief @b VFHEstimation estimates the Viewpoint Feature Histogram (VFH) 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 impl_; + + typedef pcl::PointCloud PointCloudOut; + + /** \brief Child initialization routine. Internal method. */ + inline bool + childInit (ros::NodeHandle &nh) + { + // Create the output publisher + pub_output_ = nh.advertise ("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_ diff --git a/pcl_ros/include/pcl_ros/segmentation/extract_clusters.h b/pcl_ros/include/pcl_ros/segmentation/extract_clusters.h new file mode 100644 index 00000000..38a6d379 --- /dev/null +++ b/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 +#include "pcl_ros/pcl_nodelet.h" + +// Dynamic reconfigure +#include +#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::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 > 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 impl_; + + /** \brief The input PointCloud subscriber. */ + ros::Subscriber sub_input_; + + /** \brief Synchronized input, and indices.*/ + boost::shared_ptr > > sync_input_indices_e_; + boost::shared_ptr > > sync_input_indices_a_; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#endif //#ifndef PCL_ROS_EXTRACT_CLUSTERS_H_ diff --git a/pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.h b/pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.h new file mode 100644 index 00000000..33050a41 --- /dev/null +++ b/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 +#include +#include + +// PCL includes +#include + +// Dynamic reconfigure +#include +#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 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 sub_hull_filter_; + + /** \brief Synchronized input, planar hull, and indices.*/ + boost::shared_ptr > > sync_input_hull_indices_e_; + boost::shared_ptr > > sync_input_hull_indices_a_; + + /** \brief Pointer to a dynamic reconfigure service. */ + boost::shared_ptr > srv_; + + /** \brief Null passthrough filter, used for pushing empty elements in the + * synchronizer */ + message_filters::PassThrough 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 (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 impl_; + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#endif //#ifndef PCL_EXTRACT_POLYGONAL_PRISM_DATA_H_ diff --git a/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.h b/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.h new file mode 100644 index 00000000..7ff369b6 --- /dev/null +++ b/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 + +// PCL includes +#include + +// Dynamic reconfigure +#include +#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 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 > 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 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 impl_; + + /** \brief Synchronized input, and indices.*/ + boost::shared_ptr > > sync_input_indices_e_; + boost::shared_ptr > > 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 PointCloud; + typedef PointCloud::Ptr PointCloudPtr; + typedef PointCloud::ConstPtr PointCloudConstPtr; + + typedef pcl::PointCloud 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 sub_normals_filter_; + + /** \brief The input PointCloud subscriber. */ + ros::Subscriber sub_axis_; + + /** \brief Pointer to a dynamic reconfigure service. */ + boost::shared_ptr > 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 (indices)); + } + + /** \brief Null passthrough filter, used for pushing empty elements in the + * synchronizer */ + message_filters::PassThrough 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 impl_; + + /** \brief Synchronized input, normals, and indices.*/ + boost::shared_ptr > > sync_input_normals_indices_a_; + boost::shared_ptr > > sync_input_normals_indices_e_; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#endif //#ifndef PCL_ROS_SAC_SEGMENTATION_H_ diff --git a/pcl_ros/include/pcl_ros/segmentation/segment_differences.h b/pcl_ros/include/pcl_ros/segmentation/segment_differences.h new file mode 100644 index 00000000..287d8fb2 --- /dev/null +++ b/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 +#include "pcl_ros/pcl_nodelet.h" + +// Dynamic reconfigure +#include +#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 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 sub_target_filter_; + + /** \brief Synchronized input, and planar hull.*/ + boost::shared_ptr > > sync_input_target_e_; + boost::shared_ptr > > sync_input_target_a_; + + /** \brief Pointer to a dynamic reconfigure service. */ + boost::shared_ptr > 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 impl_; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#endif //#ifndef PCL_ROS_SEGMENT_DIFFERENCES_H_ diff --git a/pcl_ros/include/pcl_ros/surface/convex_hull.h b/pcl_ros/include/pcl_ros/surface/convex_hull.h new file mode 100644 index 00000000..59fa4f6f --- /dev/null +++ b/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 + +// Dynamic reconfigure +#include + +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 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 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 > > sync_input_indices_e_; + boost::shared_ptr > > sync_input_indices_a_; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#endif //#ifndef PCL_ROS_CONVEX_HULL_2D_H_ diff --git a/pcl_ros/include/pcl_ros/surface/moving_least_squares.h b/pcl_ros/include/pcl_ros/surface/moving_least_squares.h new file mode 100644 index 00000000..87aa2662 --- /dev/null +++ b/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 + +// Dynamic reconfigure +#include +#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 PointCloudIn; + typedef PointCloudIn::Ptr PointCloudInPtr; + typedef PointCloudIn::ConstPtr PointCloudInConstPtr; + typedef pcl::PointCloud NormalCloudOut; + + typedef pcl::KdTree KdTree; + typedef pcl::KdTree::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 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 > 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 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 > > sync_input_indices_e_; + boost::shared_ptr > > sync_input_indices_a_; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#endif //#ifndef PCL_ROS_MOVING_LEAST_SQUARES_H_ diff --git a/pcl_ros/src/pcl_ros/features/boundary.cpp b/pcl_ros/src/pcl_ros/features/boundary.cpp new file mode 100644 index 00000000..0e17c5fd --- /dev/null +++ b/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 +#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); diff --git a/pcl_ros/src/pcl_ros/features/feature.cpp b/pcl_ros/src/pcl_ros/features/feature.cpp new file mode 100644 index 00000000..434707ee --- /dev/null +++ b/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 +// 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 +#include "pcl_ros/features/feature.h" +#include + +//////////////////////////////////////////////////////////////////////////////////////////// +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 >, as NormalEstimation can publish PointCloud, etc + //pub_output_ = pnh_->template advertise ("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 > (*pnh_); + dynamic_reconfigure::Server::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 > >(max_queue_size_); + else + sync_input_surface_indices_e_ = boost::make_shared > >(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 ("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 (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 >, as NormalEstimation can publish PointCloud, etc + //pub_output_ = pnh_->template advertise ("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 > (*pnh_); + dynamic_reconfigure::Server::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 > > (max_queue_size_); + else + sync_input_normals_surface_indices_e_ = boost::make_shared > > (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 (indices->indices)); + + computePublish (cloud, cloud_normals, cloud_surface, vindices); +} + diff --git a/pcl_ros/src/pcl_ros/features/fpfh.cpp b/pcl_ros/src/pcl_ros/features/fpfh.cpp new file mode 100644 index 00000000..7782fde9 --- /dev/null +++ b/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 +#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); + diff --git a/pcl_ros/src/pcl_ros/features/fpfh_omp.cpp b/pcl_ros/src/pcl_ros/features/fpfh_omp.cpp new file mode 100644 index 00000000..51880a9f --- /dev/null +++ b/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 +#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); + diff --git a/pcl_ros/src/pcl_ros/features/moment_invariants.cpp b/pcl_ros/src/pcl_ros/features/moment_invariants.cpp new file mode 100644 index 00000000..85bd209a --- /dev/null +++ b/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 +#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); + diff --git a/pcl_ros/src/pcl_ros/features/normal_3d.cpp b/pcl_ros/src/pcl_ros/features/normal_3d.cpp new file mode 100644 index 00000000..a512c41f --- /dev/null +++ b/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 +#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); + diff --git a/pcl_ros/src/pcl_ros/features/normal_3d_omp.cpp b/pcl_ros/src/pcl_ros/features/normal_3d_omp.cpp new file mode 100644 index 00000000..e53fe9c7 --- /dev/null +++ b/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 +#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); + diff --git a/pcl_ros/src/pcl_ros/features/normal_3d_tbb.cpp b/pcl_ros/src/pcl_ros/features/normal_3d_tbb.cpp new file mode 100644 index 00000000..d6636f73 --- /dev/null +++ b/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 +#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 + diff --git a/pcl_ros/src/pcl_ros/features/pfh.cpp b/pcl_ros/src/pcl_ros/features/pfh.cpp new file mode 100644 index 00000000..f40e0bab --- /dev/null +++ b/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 +#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); + diff --git a/pcl_ros/src/pcl_ros/features/principal_curvatures.cpp b/pcl_ros/src/pcl_ros/features/principal_curvatures.cpp new file mode 100644 index 00000000..d6431160 --- /dev/null +++ b/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 +#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); + diff --git a/pcl_ros/src/pcl_ros/features/vfh.cpp b/pcl_ros/src/pcl_ros/features/vfh.cpp new file mode 100644 index 00000000..f13028e3 --- /dev/null +++ b/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 +#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); + diff --git a/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp b/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp new file mode 100644 index 00000000..3c067f0a --- /dev/null +++ b/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 +#include +#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 ("output", max_queue_size_); + else + pub_output_ = pnh_->advertise ("output", max_queue_size_); + + // Enable the dynamic reconfigure service + srv_ = boost::make_shared > (*pnh_); + dynamic_reconfigure::Server::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 > > (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 > > (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 ("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 (indices->indices)); + + impl_.setInputCloud (cloud); + impl_.setIndices (indices_ptr); + + std::vector 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 (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); + diff --git a/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp b/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp new file mode 100644 index 00000000..23ab6d12 --- /dev/null +++ b/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 +#include "pcl_ros/transforms.h" +#include "pcl_ros/segmentation/extract_polygonal_prism_data.h" +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +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 > (*pnh_); + dynamic_reconfigure::Server::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 > > (max_queue_size_); + else + sync_input_hull_indices_e_ = boost::make_shared > > (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 ("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 (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 (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 (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 (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 (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); + diff --git a/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp b/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp new file mode 100644 index 00000000..8478c71d --- /dev/null +++ b/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 +#include "pcl_ros/segmentation/sac_segmentation.h" +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +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 > > (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 pairs + else + { + if (approximate_sync_) + { + sync_input_indices_a_ = boost::make_shared > > (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 > > (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 ("input", max_queue_size_, bind (&SACSegmentation::input_indices_callback, this, _1, PointIndicesConstPtr ())); + + // Advertise the output topics + pub_indices_ = pnh_->advertise ("inliers", max_queue_size_); + pub_model_ = pnh_->advertise ("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 >(*pnh_); + dynamic_reconfigure::Server::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 (inliers)); + pub_model_.publish (boost::make_shared (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 (inliers)); + pub_model_.publish (boost::make_shared (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 (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 (inliers)); + pub_model_.publish (boost::make_shared (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 > (*pnh_); + dynamic_reconfigure::Server::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 > > (max_queue_size_); + else + sync_input_normals_indices_e_ = boost::make_shared > > (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 ("inliers", max_queue_size_); + pub_model_ = pnh_->advertise ("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 (inliers)); + pub_model_.publish (boost::make_shared (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 (inliers)); + pub_model_.publish (boost::make_shared (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 (inliers)); + pub_model_.publish (boost::make_shared (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 (inliers)); + pub_model_.publish (boost::make_shared (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 (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 (inliers)); + pub_model_.publish (boost::make_shared (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); + diff --git a/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp b/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp new file mode 100644 index 00000000..f8fbae11 --- /dev/null +++ b/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 +#include "pcl_ros/segmentation/segment_differences.h" +#include + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::SegmentDifferences::onInit () +{ + // Call the super onInit () + PCLNodelet::onInit (); + + pub_output_ = pnh_->advertise ("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 > (*pnh_); + dynamic_reconfigure::Server::CallbackType f = boost::bind (&SegmentDifferences::config_callback, this, _1, _2); + srv_->setCallback (f); + + if (approximate_sync_) + { + sync_input_target_a_ = boost::make_shared > > (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 > > (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); + diff --git a/pcl_ros/src/pcl_ros/segmentation/segmentation.cpp b/pcl_ros/src/pcl_ros/segmentation/segmentation.cpp new file mode 100644 index 00000000..8c261926 --- /dev/null +++ b/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" + + diff --git a/pcl_ros/src/pcl_ros/surface/convex_hull.cpp b/pcl_ros/src/pcl_ros/surface/convex_hull.cpp new file mode 100644 index 00000000..94dc5336 --- /dev/null +++ b/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 +#include +#include "pcl_ros/surface/convex_hull.h" +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::ConvexHull2D::onInit () +{ + ros::NodeHandle pnh = getMTPrivateNodeHandle (); + pub_output_ = pnh.advertise ("output", max_queue_size_); + pub_plane_ = pnh.advertise("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 > >(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 > >(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 ("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 (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 (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); + diff --git a/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp b/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp new file mode 100644 index 00000000..c79c0195 --- /dev/null +++ b/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 +#include "pcl_ros/surface/moving_least_squares.h" +#include +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::MovingLeastSquares::onInit () +{ + PCLNodelet::onInit (); + + //ros::NodeHandle private_nh = getMTPrivateNodeHandle (); + pub_output_ = pnh_->advertise ("output", max_queue_size_); + pub_normals_ = pnh_->advertise ("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 > (*pnh_); + dynamic_reconfigure::Server::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 > >(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 > >(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 ("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 (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); diff --git a/pcl_ros/src/pcl_ros/surface/surface.cpp b/pcl_ros/src/pcl_ros/surface/surface.cpp new file mode 100644 index 00000000..13976f90 --- /dev/null +++ b/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" + +