diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index c64b6a20..3192e07c 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -105,6 +105,8 @@ add_library(pcl_ros_features src/pcl_ros/features/boundary.cpp src/pcl_ros/features/fpfh.cpp src/pcl_ros/features/fpfh_omp.cpp + src/pcl_ros/features/shot.cpp + src/pcl_ros/features/shot_omp.cpp src/pcl_ros/features/moment_invariants.cpp src/pcl_ros/features/normal_3d.cpp src/pcl_ros/features/normal_3d_omp.cpp diff --git a/pcl_ros/include/pcl_ros/features/shot.h b/pcl_ros/include/pcl_ros/features/shot.h new file mode 100644 index 00000000..73c0d81c --- /dev/null +++ b/pcl_ros/include/pcl_ros/features/shot.h @@ -0,0 +1,80 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, Ryohei Ueda, JSK Lab + * 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 JSK Lab. 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. + * + * + */ + +#ifndef PCL_ROS_SHOT_H_ +#define PCL_ROS_SHOT_H_ + +#include +#include "pcl_ros/features/pfh.h" + +namespace pcl_ros +{ + /** \brief @b SHOTEstimation estimates SHOT descriptor. + * + */ + class SHOTEstimation : public FeatureFromNormals + { + private: + pcl::SHOTEstimation 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_SHOT_H_ + + diff --git a/pcl_ros/include/pcl_ros/features/shot_omp.h b/pcl_ros/include/pcl_ros/features/shot_omp.h new file mode 100644 index 00000000..bc28939c --- /dev/null +++ b/pcl_ros/include/pcl_ros/features/shot_omp.h @@ -0,0 +1,78 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, Ryohei Ueda, JSK Lab + * 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 JSK Lab. 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. + * + * + */ + +#ifndef PCL_ROS_SHOT_OMP_H_ +#define PCL_ROS_SHOT_OMP_H_ + +#include +#include "pcl_ros/features/shot.h" + +namespace pcl_ros +{ + /** \brief @b SHOTEstimation estimates SHOT descriptor using OpenMP. + */ + class SHOTEstimationOMP : public FeatureFromNormals + { + private: + pcl::SHOTEstimationOMP 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_SHOT_OMP_H_ + diff --git a/pcl_ros/pcl_nodelets.xml b/pcl_ros/pcl_nodelets.xml index 2be8cace..64fa27d8 100644 --- a/pcl_ros/pcl_nodelets.xml +++ b/pcl_ros/pcl_nodelets.xml @@ -20,6 +20,20 @@ containing points and normals, in parallel, using the OpenMP standard. + + + + SHOTEstimation estimates SHOT descriptor for a given point cloud dataset + containing points and normals. + + + + + + SHOTEstimationOMP estimates SHOT descriptor for a given point cloud dataset + containing points and normals, in parallel, using the OpenMP standard. + + diff --git a/pcl_ros/src/pcl_ros/features/shot.cpp b/pcl_ros/src/pcl_ros/features/shot.cpp new file mode 100644 index 00000000..82122c8d --- /dev/null +++ b/pcl_ros/src/pcl_ros/features/shot.cpp @@ -0,0 +1,75 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, Ryohei Ueda, JSK Lab + * 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 JSK Lab. 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. + * + * + */ + +#include +#include "pcl_ros/features/shot.h" + +void +pcl_ros::SHOTEstimation::emptyPublish (const PointCloudInConstPtr &cloud) +{ + PointCloudOut output; + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); +} + +void +pcl_ros::SHOTEstimation::computePublish (const PointCloudInConstPtr &cloud, + const PointCloudNConstPtr &normals, + const PointCloudInConstPtr &surface, + const IndicesPtr &indices) +{ + // Set the parameters + impl_.setKSearch (k_); + impl_.setRadiusSearch (search_radius_); + + // 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::SHOTEstimation SHOTEstimation; +PLUGINLIB_DECLARE_CLASS (pcl, SHOTEstimation, SHOTEstimation, nodelet::Nodelet); + diff --git a/pcl_ros/src/pcl_ros/features/shot_omp.cpp b/pcl_ros/src/pcl_ros/features/shot_omp.cpp new file mode 100644 index 00000000..887e4b5d --- /dev/null +++ b/pcl_ros/src/pcl_ros/features/shot_omp.cpp @@ -0,0 +1,75 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, Ryohei Ueda, JSK Lab + * 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 JSK Lab. 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. + * + * + */ + +#include +#include "pcl_ros/features/shot_omp.h" + +void +pcl_ros::SHOTEstimationOMP::emptyPublish (const PointCloudInConstPtr &cloud) +{ + PointCloudOut output; + output.header = cloud->header; + pub_output_.publish (output.makeShared ()); +} + +void +pcl_ros::SHOTEstimationOMP::computePublish (const PointCloudInConstPtr &cloud, + const PointCloudNConstPtr &normals, + const PointCloudInConstPtr &surface, + const IndicesPtr &indices) +{ + // Set the parameters + impl_.setKSearch (k_); + impl_.setRadiusSearch (search_radius_); + + // 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::SHOTEstimationOMP SHOTEstimationOMP; +PLUGINLIB_DECLARE_CLASS (pcl, SHOTEstimationOMP, SHOTEstimationOMP, nodelet::Nodelet); + diff --git a/pcl_ros/src/pcl_ros/filters/filter.cpp b/pcl_ros/src/pcl_ros/filters/filter.cpp index 30104d94..0d39a03e 100644 --- a/pcl_ros/src/pcl_ros/filters/filter.cpp +++ b/pcl_ros/src/pcl_ros/filters/filter.cpp @@ -95,6 +95,9 @@ pcl_ros::Filter::computePublish (const PointCloud2::ConstPtr &input, const Indic cloud_tf.reset (new PointCloud2 (cloud_transformed)); } + // Copy timestamp to keep it + cloud_tf->header.stamp = input->header.stamp; + // Publish a boost shared ptr pub_output_.publish (cloud_tf); }