copy features/segmentation/surface from fuerte-devel
This commit is contained in:
Kei Okada
Paul Bovbel
@@ -0,0 +1,86 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2009, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: boundary.h 35361 2011-01-20 04:34:49Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef PCL_ROS_BOUNDARY_H_
|
||||
#define PCL_ROS_BOUNDARY_H_
|
||||
|
||||
#include <pcl/features/boundary.h>
|
||||
#include "pcl_ros/features/feature.h"
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
/** \brief @b BoundaryEstimation estimates whether a set of points is lying on surface boundaries using an angle
|
||||
* criterion. The code makes use of the estimated surface normals at each point in the input dataset.
|
||||
*
|
||||
* @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
|
||||
* \a NormalEstimationOpenMP and \a NormalEstimationTBB for examples on how to extend this to parallel implementations.
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class BoundaryEstimation: public FeatureFromNormals
|
||||
{
|
||||
private:
|
||||
pcl::BoundaryEstimation<pcl::PointXYZ, pcl::Normal, pcl::Boundary> impl_;
|
||||
|
||||
typedef pcl::PointCloud<pcl::Boundary> PointCloudOut;
|
||||
|
||||
/** \brief Child initialization routine. Internal method. */
|
||||
inline bool
|
||||
childInit (ros::NodeHandle &nh)
|
||||
{
|
||||
// Create the output publisher
|
||||
pub_output_ = nh.advertise<PointCloudOut> ("output", max_queue_size_);
|
||||
return (true);
|
||||
}
|
||||
|
||||
/** \brief Publish an empty point cloud of the feature output type. */
|
||||
void emptyPublish (const PointCloudInConstPtr &cloud);
|
||||
|
||||
/** \brief Compute the feature and publish it. */
|
||||
void computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudNConstPtr &normals,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices);
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
}
|
||||
|
||||
#endif //#ifndef PCL_ROS_BOUNDARY_H_
|
||||
|
||||
|
||||
@@ -0,0 +1,240 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2009, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: feature.h 35422 2011-01-24 20:04:44Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef PCL_ROS_FEATURE_H_
|
||||
#define PCL_ROS_FEATURE_H_
|
||||
|
||||
// PCL includes
|
||||
#include <pcl/features/feature.h>
|
||||
#include <pcl/PointIndices.h>
|
||||
|
||||
#include "pcl_ros/pcl_nodelet.h"
|
||||
#include <message_filters/pass_through.h>
|
||||
|
||||
// Dynamic reconfigure
|
||||
#include <dynamic_reconfigure/server.h>
|
||||
#include "pcl_ros/FeatureConfig.h"
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
namespace sync_policies = message_filters::sync_policies;
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////////////////
|
||||
///////////////////////////////////////////////////////////////////////////////////////////
|
||||
///////////////////////////////////////////////////////////////////////////////////////////
|
||||
/** \brief @b Feature represents the base feature class. Some generic 3D operations that
|
||||
* are applicable to all features are defined here as static methods.
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class Feature : public PCLNodelet
|
||||
{
|
||||
public:
|
||||
typedef pcl::KdTree<pcl::PointXYZ> KdTree;
|
||||
typedef pcl::KdTree<pcl::PointXYZ>::Ptr KdTreePtr;
|
||||
|
||||
typedef pcl::PointCloud<pcl::PointXYZ> PointCloudIn;
|
||||
typedef PointCloudIn::Ptr PointCloudInPtr;
|
||||
typedef PointCloudIn::ConstPtr PointCloudInConstPtr;
|
||||
|
||||
typedef boost::shared_ptr <std::vector<int> > IndicesPtr;
|
||||
typedef boost::shared_ptr <const std::vector<int> > IndicesConstPtr;
|
||||
|
||||
/** \brief Empty constructor. */
|
||||
Feature () : /*input_(), indices_(), surface_(), */tree_(), k_(0), search_radius_(0),
|
||||
use_surface_(false), spatial_locator_type_(-1)
|
||||
{};
|
||||
|
||||
protected:
|
||||
/** \brief The input point cloud dataset. */
|
||||
//PointCloudInConstPtr input_;
|
||||
|
||||
/** \brief A pointer to the vector of point indices to use. */
|
||||
//IndicesConstPtr indices_;
|
||||
|
||||
/** \brief An input point cloud describing the surface that is to be used for nearest neighbors estimation. */
|
||||
//PointCloudInConstPtr surface_;
|
||||
|
||||
/** \brief A pointer to the spatial search object. */
|
||||
KdTreePtr tree_;
|
||||
|
||||
/** \brief The number of K nearest neighbors to use for each point. */
|
||||
int k_;
|
||||
|
||||
/** \brief The nearest neighbors search radius for each point. */
|
||||
double search_radius_;
|
||||
|
||||
// ROS nodelet attributes
|
||||
/** \brief The surface PointCloud subscriber filter. */
|
||||
message_filters::Subscriber<PointCloudIn> sub_surface_filter_;
|
||||
|
||||
/** \brief The input PointCloud subscriber. */
|
||||
ros::Subscriber sub_input_;
|
||||
|
||||
/** \brief Set to true if the nodelet needs to listen for incoming point clouds representing the search surface. */
|
||||
bool use_surface_;
|
||||
|
||||
/** \brief Parameter for the spatial locator tree. By convention, the values represent:
|
||||
* 0: ANN (Approximate Nearest Neigbor library) kd-tree
|
||||
* 1: FLANN (Fast Library for Approximate Nearest Neighbors) kd-tree
|
||||
* 2: Organized spatial dataset index
|
||||
*/
|
||||
int spatial_locator_type_;
|
||||
|
||||
/** \brief Pointer to a dynamic reconfigure service. */
|
||||
boost::shared_ptr <dynamic_reconfigure::Server<FeatureConfig> > srv_;
|
||||
|
||||
/** \brief Child initialization routine. Internal method. */
|
||||
virtual bool childInit (ros::NodeHandle &nh) = 0;
|
||||
|
||||
/** \brief Publish an empty point cloud of the feature output type. */
|
||||
virtual void emptyPublish (const PointCloudInConstPtr &cloud) = 0;
|
||||
|
||||
/** \brief Compute the feature and publish it. Internal method. */
|
||||
virtual void computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices) = 0;
|
||||
|
||||
/** \brief Dynamic reconfigure callback
|
||||
* \param config the config object
|
||||
* \param level the dynamic reconfigure level
|
||||
*/
|
||||
void config_callback (FeatureConfig &config, uint32_t level);
|
||||
|
||||
/** \brief Null passthrough filter, used for pushing empty elements in the
|
||||
* synchronizer */
|
||||
message_filters::PassThrough<PointIndices> nf_pi_;
|
||||
message_filters::PassThrough<PointCloudIn> nf_pc_;
|
||||
|
||||
/** \brief Input point cloud callback.
|
||||
* Because we want to use the same synchronizer object, we push back
|
||||
* empty elements with the same timestamp.
|
||||
*/
|
||||
inline void
|
||||
input_callback (const PointCloudInConstPtr &input)
|
||||
{
|
||||
PointIndices indices;
|
||||
indices.header.stamp = input->header.stamp;
|
||||
PointCloudIn cloud;
|
||||
cloud.header.stamp = input->header.stamp;
|
||||
nf_pc_.add (cloud.makeShared ());
|
||||
nf_pi_.add (boost::make_shared<PointIndices> (indices));
|
||||
}
|
||||
|
||||
private:
|
||||
/** \brief Synchronized input, surface, and point indices.*/
|
||||
boost::shared_ptr <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn, PointCloudIn, PointIndices> > > sync_input_surface_indices_a_;
|
||||
boost::shared_ptr <message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, PointCloudIn, PointIndices> > > sync_input_surface_indices_e_;
|
||||
|
||||
/** \brief Nodelet initialization routine. */
|
||||
virtual void onInit ();
|
||||
|
||||
/** \brief Input point cloud callback. Used when \a use_indices and \a use_surface are set.
|
||||
* \param cloud the pointer to the input point cloud
|
||||
* \param cloud_surface the pointer to the surface point cloud
|
||||
* \param indices the pointer to the input point cloud indices
|
||||
*/
|
||||
void input_surface_indices_callback (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudInConstPtr &cloud_surface,
|
||||
const PointIndicesConstPtr &indices);
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////
|
||||
//////////////////////////////////////////////////////////////////////////////////////////
|
||||
//////////////////////////////////////////////////////////////////////////////////////////
|
||||
class FeatureFromNormals : public Feature
|
||||
{
|
||||
public:
|
||||
typedef sensor_msgs::PointCloud2 PointCloud2;
|
||||
|
||||
typedef pcl::PointCloud<pcl::Normal> PointCloudN;
|
||||
typedef PointCloudN::Ptr PointCloudNPtr;
|
||||
typedef PointCloudN::ConstPtr PointCloudNConstPtr;
|
||||
|
||||
FeatureFromNormals () : normals_() {};
|
||||
|
||||
protected:
|
||||
/** \brief A pointer to the input dataset that contains the point normals of the XYZ dataset. */
|
||||
PointCloudNConstPtr normals_;
|
||||
|
||||
/** \brief Child initialization routine. Internal method. */
|
||||
virtual bool childInit (ros::NodeHandle &nh) = 0;
|
||||
|
||||
/** \brief Publish an empty point cloud of the feature output type. */
|
||||
virtual void emptyPublish (const PointCloudInConstPtr &cloud) = 0;
|
||||
|
||||
/** \brief Compute the feature and publish it. */
|
||||
virtual void computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudNConstPtr &normals,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices) = 0;
|
||||
|
||||
private:
|
||||
/** \brief The normals PointCloud subscriber filter. */
|
||||
message_filters::Subscriber<PointCloudN> sub_normals_filter_;
|
||||
|
||||
/** \brief Synchronized input, normals, surface, and point indices.*/
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn, PointCloudN, PointCloudIn, PointIndices> > > sync_input_normals_surface_indices_a_;
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, PointCloudN, PointCloudIn, PointIndices> > > sync_input_normals_surface_indices_e_;
|
||||
|
||||
/** \brief Internal method. */
|
||||
void computePublish (const PointCloudInConstPtr &,
|
||||
const PointCloudInConstPtr &,
|
||||
const IndicesPtr &) {} // This should never be called
|
||||
|
||||
/** \brief Nodelet initialization routine. */
|
||||
virtual void onInit ();
|
||||
|
||||
/** \brief Input point cloud callback. Used when \a use_indices and \a use_surface are set.
|
||||
* \param cloud the pointer to the input point cloud
|
||||
* \param cloud_normals the pointer to the input point cloud normals
|
||||
* \param cloud_surface the pointer to the surface point cloud
|
||||
* \param indices the pointer to the input point cloud indices
|
||||
*/
|
||||
void input_normals_surface_indices_callback (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudNConstPtr &cloud_normals,
|
||||
const PointCloudInConstPtr &cloud_surface,
|
||||
const PointIndicesConstPtr &indices);
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
}
|
||||
|
||||
#endif //#ifndef PCL_ROS_FEATURE_H_
|
||||
@@ -0,0 +1,100 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2009, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: fpfh.h 35361 2011-01-20 04:34:49Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef PCL_ROS_FPFH_H_
|
||||
#define PCL_ROS_FPFH_H_
|
||||
|
||||
#include <pcl/features/fpfh.h>
|
||||
#include "pcl_ros/features/pfh.h"
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
/** \brief @b FPFHEstimation estimates the <b>Fast Point Feature Histogram (FPFH)</b> descriptor for a given point cloud
|
||||
* dataset containing points and normals.
|
||||
*
|
||||
* @note If you use this code in any academic work, please cite:
|
||||
*
|
||||
* <ul>
|
||||
* <li> R.B. Rusu, N. Blodow, M. Beetz.
|
||||
* Fast Point Feature Histograms (FPFH) for 3D Registration.
|
||||
* In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA),
|
||||
* Kobe, Japan, May 12-17 2009.
|
||||
* </li>
|
||||
* <li> R.B. Rusu, A. Holzbach, N. Blodow, M. Beetz.
|
||||
* Fast Geometric Point Labeling using Conditional Random Fields.
|
||||
* In Proceedings of the 22nd IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS),
|
||||
* St. Louis, MO, USA, October 11-15 2009.
|
||||
* </li>
|
||||
* </ul>
|
||||
*
|
||||
* @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
|
||||
* \a FPFHEstimationOpenMP for examples on parallel implementations of the FPFH (Fast Point Feature Histogram).
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class FPFHEstimation : public FeatureFromNormals
|
||||
{
|
||||
private:
|
||||
pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> impl_;
|
||||
|
||||
typedef pcl::PointCloud<pcl::FPFHSignature33> PointCloudOut;
|
||||
|
||||
/** \brief Child initialization routine. Internal method. */
|
||||
inline bool
|
||||
childInit (ros::NodeHandle &nh)
|
||||
{
|
||||
// Create the output publisher
|
||||
pub_output_ = nh.advertise<PointCloudOut> ("output", max_queue_size_);
|
||||
return (true);
|
||||
}
|
||||
|
||||
/** \brief Publish an empty point cloud of the feature output type. */
|
||||
void emptyPublish (const PointCloudInConstPtr &cloud);
|
||||
|
||||
/** \brief Compute the feature and publish it. */
|
||||
void computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudNConstPtr &normals,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices);
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
}
|
||||
|
||||
#endif //#ifndef PCL_FPFH_H_
|
||||
|
||||
|
||||
@@ -0,0 +1,96 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2009, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: fpfh_omp.h 35361 2011-01-20 04:34:49Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef PCL_ROS_FPFH_OMP_H_
|
||||
#define PCL_ROS_FPFH_OMP_H_
|
||||
|
||||
#include <pcl/features/fpfh_omp.h>
|
||||
#include "pcl_ros/features/fpfh.h"
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
/** \brief @b FPFHEstimationOMP estimates the Fast Point Feature Histogram (FPFH) descriptor for a given point cloud
|
||||
* dataset containing points and normals, in parallel, using the OpenMP standard.
|
||||
*
|
||||
* @note If you use this code in any academic work, please cite:
|
||||
*
|
||||
* <ul>
|
||||
* <li> R.B. Rusu, N. Blodow, M. Beetz.
|
||||
* Fast Point Feature Histograms (FPFH) for 3D Registration.
|
||||
* In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA),
|
||||
* Kobe, Japan, May 12-17 2009.
|
||||
* </li>
|
||||
* <li> R.B. Rusu, A. Holzbach, N. Blodow, M. Beetz.
|
||||
* Fast Geometric Point Labeling using Conditional Random Fields.
|
||||
* In Proceedings of the 22nd IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS),
|
||||
* St. Louis, MO, USA, October 11-15 2009.
|
||||
* </li>
|
||||
* </ul>
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class FPFHEstimationOMP : public FeatureFromNormals
|
||||
{
|
||||
private:
|
||||
pcl::FPFHEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> impl_;
|
||||
|
||||
typedef pcl::PointCloud<pcl::FPFHSignature33> PointCloudOut;
|
||||
|
||||
/** \brief Child initialization routine. Internal method. */
|
||||
inline bool
|
||||
childInit (ros::NodeHandle &nh)
|
||||
{
|
||||
// Create the output publisher
|
||||
pub_output_ = nh.advertise<PointCloudOut> ("output", max_queue_size_);
|
||||
return (true);
|
||||
}
|
||||
|
||||
/** \brief Publish an empty point cloud of the feature output type. */
|
||||
void emptyPublish (const PointCloudInConstPtr &cloud);
|
||||
|
||||
/** \brief Compute the feature and publish it. */
|
||||
void computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudNConstPtr &normals,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices);
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
}
|
||||
|
||||
#endif //#ifndef PCL_ROS_FPFH_OMP_H_
|
||||
|
||||
@@ -0,0 +1,83 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2009, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: moment_invariants.h 35361 2011-01-20 04:34:49Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef PCL_ROS_MOMENT_INVARIANTS_H_
|
||||
#define PCL_ROS_MOMENT_INVARIANTS_H_
|
||||
|
||||
#include <pcl/features/moment_invariants.h>
|
||||
#include "pcl_ros/features/feature.h"
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
/** \brief MomentInvariantsEstimation estimates the 3 moment invariants (j1, j2, j3) at each 3D point.
|
||||
*
|
||||
* @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
|
||||
* \a NormalEstimationOpenMP and \a NormalEstimationTBB for examples on how to extend this to parallel implementations.
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class MomentInvariantsEstimation: public Feature
|
||||
{
|
||||
private:
|
||||
pcl::MomentInvariantsEstimation<pcl::PointXYZ, pcl::MomentInvariants> impl_;
|
||||
|
||||
typedef pcl::PointCloud<pcl::MomentInvariants> PointCloudOut;
|
||||
|
||||
/** \brief Child initialization routine. Internal method. */
|
||||
inline bool
|
||||
childInit (ros::NodeHandle &nh)
|
||||
{
|
||||
// Create the output publisher
|
||||
pub_output_ = nh.advertise<PointCloudOut> ("output", max_queue_size_);
|
||||
return (true);
|
||||
}
|
||||
|
||||
/** \brief Publish an empty point cloud of the feature output type. */
|
||||
void emptyPublish (const PointCloudInConstPtr &cloud);
|
||||
|
||||
/** \brief Compute the feature and publish it. */
|
||||
void computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices);
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
}
|
||||
|
||||
#endif //#ifndef PCL_ROS_MOMENT_INVARIANTS_H_
|
||||
|
||||
|
||||
@@ -0,0 +1,86 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2009, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: normal_3d.h 35361 2011-01-20 04:34:49Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef PCL_ROS_NORMAL_3D_H_
|
||||
#define PCL_ROS_NORMAL_3D_H_
|
||||
|
||||
#include <pcl/features/normal_3d.h>
|
||||
#include "pcl_ros/features/feature.h"
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
/** \brief @b NormalEstimation estimates local surface properties at each 3D point, such as surface normals and
|
||||
* curvatures.
|
||||
*
|
||||
* @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
|
||||
* \a NormalEstimationOpenMP and \a NormalEstimationTBB for parallel implementations.
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class NormalEstimation: public Feature
|
||||
{
|
||||
private:
|
||||
/** \brief PCL implementation object. */
|
||||
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> impl_;
|
||||
|
||||
typedef pcl::PointCloud<pcl::Normal> PointCloudOut;
|
||||
|
||||
/** \brief Child initialization routine. Internal method. */
|
||||
inline bool
|
||||
childInit (ros::NodeHandle &nh)
|
||||
{
|
||||
// Create the output publisher
|
||||
pub_output_ = nh.advertise<PointCloudOut> ("output", max_queue_size_);
|
||||
return (true);
|
||||
}
|
||||
|
||||
/** \brief Publish an empty point cloud of the feature output type.
|
||||
* \param cloud the input point cloud to copy the header from.
|
||||
*/
|
||||
void emptyPublish (const PointCloudInConstPtr &cloud);
|
||||
|
||||
/** \brief Compute the feature and publish it. */
|
||||
void computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices);
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
}
|
||||
|
||||
#endif //#ifndef PCL_ROS_NORMAL_3D_H_
|
||||
|
||||
@@ -0,0 +1,79 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2009, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: normal_3d_omp.h 35361 2011-01-20 04:34:49Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef PCL_ROS_NORMAL_3D_OMP_H_
|
||||
#define PCL_ROS_NORMAL_3D_OMP_H_
|
||||
|
||||
#include <pcl/features/normal_3d_omp.h>
|
||||
#include "pcl_ros/features/normal_3d.h"
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
/** \brief @b NormalEstimationOMP estimates local surface properties at each 3D point, such as surface normals and
|
||||
* curvatures, in parallel, using the OpenMP standard.
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class NormalEstimationOMP: public Feature
|
||||
{
|
||||
private:
|
||||
pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> impl_;
|
||||
|
||||
typedef pcl::PointCloud<pcl::Normal> PointCloudOut;
|
||||
|
||||
/** \brief Child initialization routine. Internal method. */
|
||||
inline bool
|
||||
childInit (ros::NodeHandle &nh)
|
||||
{
|
||||
// Create the output publisher
|
||||
pub_output_ = nh.advertise<PointCloudOut> ("output", max_queue_size_);
|
||||
return (true);
|
||||
}
|
||||
|
||||
/** \brief Publish an empty point cloud of the feature output type. */
|
||||
void emptyPublish (const PointCloudInConstPtr &cloud);
|
||||
|
||||
/** \brief Compute the feature and publish it. */
|
||||
void computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices);
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
}
|
||||
|
||||
#endif //#ifndef PCL_ROS_NORMAL_3D_OMP_H_
|
||||
@@ -0,0 +1,86 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2009, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: normal_3d_tbb.h 35661 2011-02-01 06:04:14Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef PCL_ROS_NORMAL_3D_TBB_H_
|
||||
#define PCL_ROS_NORMAL_3D_TBB_H_
|
||||
|
||||
//#include "pcl_ros/pcl_ros_config.h"
|
||||
//#if defined(HAVE_TBB)
|
||||
|
||||
#include <pcl/features/normal_3d_tbb.h>
|
||||
#include "pcl_ros/features/normal_3d.h"
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
/** \brief @b NormalEstimationTBB estimates local surface properties at each 3D point, such as surface normals and
|
||||
* curvatures, in parallel, using Intel's Threading Building Blocks library.
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class NormalEstimationTBB: public Feature
|
||||
{
|
||||
private:
|
||||
pcl::NormalEstimationTBB<pcl::PointXYZ, pcl::Normal> impl_;
|
||||
|
||||
typedef pcl::PointCloud<pcl::Normal> PointCloudOut;
|
||||
|
||||
/** \brief Child initialization routine. Internal method. */
|
||||
inline bool
|
||||
childInit (ros::NodeHandle &nh)
|
||||
{
|
||||
// Create the output publisher
|
||||
pub_output_ = nh.advertise<PointCloud> ("output", max_queue_size_);
|
||||
return (true);
|
||||
}
|
||||
|
||||
/** \brief Publish an empty point cloud of the feature output type. */
|
||||
void emptyPublish (const PointCloudInConstPtr &cloud);
|
||||
|
||||
/** \brief Compute the feature and publish it. */
|
||||
void computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices);
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
}
|
||||
|
||||
//#endif // HAVE_TBB
|
||||
|
||||
#endif //#ifndef PCL_ROS_NORMAL_3D_TBB_H_
|
||||
|
||||
|
||||
@@ -0,0 +1,100 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2009, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: pfh.h 35361 2011-01-20 04:34:49Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef PCL_ROS_PFH_H_
|
||||
#define PCL_ROS_PFH_H_
|
||||
|
||||
#include <pcl/features/pfh.h>
|
||||
#include "pcl_ros/features/feature.h"
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
/** \brief @b PFHEstimation estimates the Point Feature Histogram (PFH) descriptor for a given point cloud dataset
|
||||
* containing points and normals.
|
||||
*
|
||||
* @note If you use this code in any academic work, please cite:
|
||||
*
|
||||
* <ul>
|
||||
* <li> R.B. Rusu, N. Blodow, Z.C. Marton, M. Beetz.
|
||||
* Aligning Point Cloud Views using Persistent Feature Histograms.
|
||||
* In Proceedings of the 21st IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS),
|
||||
* Nice, France, September 22-26 2008.
|
||||
* </li>
|
||||
* <li> R.B. Rusu, Z.C. Marton, N. Blodow, M. Beetz.
|
||||
* Learning Informative Point Classes for the Acquisition of Object Model Maps.
|
||||
* In Proceedings of the 10th International Conference on Control, Automation, Robotics and Vision (ICARCV),
|
||||
* Hanoi, Vietnam, December 17-20 2008.
|
||||
* </li>
|
||||
* </ul>
|
||||
*
|
||||
* @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
|
||||
* \a FPFHEstimationOpenMP for examples on parallel implementations of the FPFH (Fast Point Feature Histogram).
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class PFHEstimation : public FeatureFromNormals
|
||||
{
|
||||
private:
|
||||
pcl::PFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::PFHSignature125> impl_;
|
||||
|
||||
typedef pcl::PointCloud<pcl::PFHSignature125> PointCloudOut;
|
||||
|
||||
/** \brief Child initialization routine. Internal method. */
|
||||
inline bool
|
||||
childInit (ros::NodeHandle &nh)
|
||||
{
|
||||
// Create the output publisher
|
||||
pub_output_ = nh.advertise<PointCloudOut> ("output", max_queue_size_);
|
||||
return (true);
|
||||
}
|
||||
|
||||
/** \brief Publish an empty point cloud of the feature output type. */
|
||||
void emptyPublish (const PointCloudInConstPtr &cloud);
|
||||
|
||||
/** \brief Compute the feature and publish it. */
|
||||
void computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudNConstPtr &normals,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices);
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
}
|
||||
|
||||
#endif //#ifndef PCL_ROS_PFH_H_
|
||||
|
||||
|
||||
@@ -0,0 +1,83 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2009, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: principal_curvatures.h 35361 2011-01-20 04:34:49Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef PCL_ROS_PRINCIPAL_CURVATURES_H_
|
||||
#define PCL_ROS_PRINCIPAL_CURVATURES_H_
|
||||
|
||||
#include <pcl/features/principal_curvatures.h>
|
||||
#include "pcl_ros/features/feature.h"
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
/** \brief @b PrincipalCurvaturesEstimation estimates the directions (eigenvectors) and magnitudes (eigenvalues) of
|
||||
* principal surface curvatures for a given point cloud dataset containing points and normals.
|
||||
*
|
||||
* @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
|
||||
* \a NormalEstimationOpenMP and \a NormalEstimationTBB for examples on how to extend this to parallel implementations.
|
||||
* \author Radu Bogdan Rusu, Jared Glover
|
||||
*/
|
||||
class PrincipalCurvaturesEstimation : public FeatureFromNormals
|
||||
{
|
||||
private:
|
||||
pcl::PrincipalCurvaturesEstimation<pcl::PointXYZ, pcl::Normal, pcl::PrincipalCurvatures> impl_;
|
||||
|
||||
typedef pcl::PointCloud<pcl::PrincipalCurvatures> PointCloudOut;
|
||||
|
||||
/** \brief Child initialization routine. Internal method. */
|
||||
inline bool
|
||||
childInit (ros::NodeHandle &nh)
|
||||
{
|
||||
// Create the output publisher
|
||||
pub_output_ = nh.advertise<PointCloudOut> ("output", max_queue_size_);
|
||||
return (true);
|
||||
}
|
||||
|
||||
/** \brief Publish an empty point cloud of the feature output type. */
|
||||
void emptyPublish (const PointCloudInConstPtr &cloud);
|
||||
|
||||
/** \brief Compute the feature and publish it. */
|
||||
void computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudNConstPtr &normals,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices);
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
}
|
||||
|
||||
#endif //#ifndef PCL_ROS_PRINCIPAL_CURVATURES_H_
|
||||
@@ -0,0 +1,83 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2009, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: vfh.h 35361 2011-01-20 04:34:49Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef PCL_ROS_FEATURES_VFH_H_
|
||||
#define PCL_ROS_FEATURES_VFH_H_
|
||||
|
||||
#include <pcl/features/vfh.h>
|
||||
#include "pcl_ros/features/fpfh.h"
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
/** \brief @b VFHEstimation estimates the <b>Viewpoint Feature Histogram (VFH)</b> descriptor for a given point cloud
|
||||
* dataset containing points and normals.
|
||||
*
|
||||
* @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
|
||||
* \a FPFHEstimationOpenMP for examples on parallel implementations of the FPFH (Fast Point Feature Histogram).
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class VFHEstimation : public FeatureFromNormals
|
||||
{
|
||||
private:
|
||||
pcl::VFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::VFHSignature308> impl_;
|
||||
|
||||
typedef pcl::PointCloud<pcl::VFHSignature308> PointCloudOut;
|
||||
|
||||
/** \brief Child initialization routine. Internal method. */
|
||||
inline bool
|
||||
childInit (ros::NodeHandle &nh)
|
||||
{
|
||||
// Create the output publisher
|
||||
pub_output_ = nh.advertise<PointCloudOut> ("output", max_queue_size_);
|
||||
return (true);
|
||||
}
|
||||
|
||||
/** \brief Publish an empty point cloud of the feature output type. */
|
||||
void emptyPublish (const PointCloudInConstPtr &cloud);
|
||||
|
||||
/** \brief Compute the feature and publish it. */
|
||||
void computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudNConstPtr &normals,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices);
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
}
|
||||
|
||||
#endif //#ifndef PCL_ROS_FEATURES_VFH_H_
|
||||
@@ -0,0 +1,105 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2010, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: extract_clusters.h 35361 2011-01-20 04:34:49Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef PCL_ROS_EXTRACT_CLUSTERS_H_
|
||||
#define PCL_ROS_EXTRACT_CLUSTERS_H_
|
||||
|
||||
#include <pcl/segmentation/extract_clusters.h>
|
||||
#include "pcl_ros/pcl_nodelet.h"
|
||||
|
||||
// Dynamic reconfigure
|
||||
#include <dynamic_reconfigure/server.h>
|
||||
#include "pcl_ros/EuclideanClusterExtractionConfig.h"
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
namespace sync_policies = message_filters::sync_policies;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////
|
||||
////////////////////////////////////////////////////////////////////////////////////////////
|
||||
/** \brief @b EuclideanClusterExtraction represents a segmentation class for cluster extraction in an Euclidean sense.
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class EuclideanClusterExtraction : public PCLNodelet
|
||||
{
|
||||
public:
|
||||
/** \brief Empty constructor. */
|
||||
EuclideanClusterExtraction () : publish_indices_ (false), max_clusters_ (std::numeric_limits<int>::max ()) {};
|
||||
|
||||
protected:
|
||||
// ROS nodelet attributes
|
||||
/** \brief Publish indices or convert to PointCloud clusters. Default: false */
|
||||
bool publish_indices_;
|
||||
|
||||
/** \brief Maximum number of clusters to publish. */
|
||||
int max_clusters_;
|
||||
|
||||
/** \brief Pointer to a dynamic reconfigure service. */
|
||||
boost::shared_ptr<dynamic_reconfigure::Server<EuclideanClusterExtractionConfig> > srv_;
|
||||
|
||||
/** \brief Nodelet initialization routine. */
|
||||
void onInit ();
|
||||
|
||||
/** \brief Dynamic reconfigure callback
|
||||
* \param config the config object
|
||||
* \param level the dynamic reconfigure level
|
||||
*/
|
||||
void config_callback (EuclideanClusterExtractionConfig &config, uint32_t level);
|
||||
|
||||
/** \brief Input point cloud callback.
|
||||
* \param cloud the pointer to the input point cloud
|
||||
* \param indices the pointer to the input point cloud indices
|
||||
*/
|
||||
void input_indices_callback (const PointCloudConstPtr &cloud, const PointIndicesConstPtr &indices);
|
||||
|
||||
private:
|
||||
/** \brief The PCL implementation used. */
|
||||
pcl::EuclideanClusterExtraction<pcl::PointXYZ> impl_;
|
||||
|
||||
/** \brief The input PointCloud subscriber. */
|
||||
ros::Subscriber sub_input_;
|
||||
|
||||
/** \brief Synchronized input, and indices.*/
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointIndices> > > sync_input_indices_e_;
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointIndices> > > sync_input_indices_a_;
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
}
|
||||
|
||||
#endif //#ifndef PCL_ROS_EXTRACT_CLUSTERS_H_
|
||||
@@ -0,0 +1,126 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2010, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: extract_polygonal_prism_data.h 35361 2011-01-20 04:34:49Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef PCL_ROS_EXTRACT_POLYGONAL_PRISM_DATA_H_
|
||||
#define PCL_ROS_EXTRACT_POLYGONAL_PRISM_DATA_H_
|
||||
|
||||
#include "pcl_ros/pcl_nodelet.h"
|
||||
#include <message_filters/sync_policies/exact_time.h>
|
||||
#include <message_filters/sync_policies/approximate_time.h>
|
||||
#include <message_filters/pass_through.h>
|
||||
|
||||
// PCL includes
|
||||
#include <pcl/segmentation/extract_polygonal_prism_data.h>
|
||||
|
||||
// Dynamic reconfigure
|
||||
#include <dynamic_reconfigure/server.h>
|
||||
#include "pcl_ros/ExtractPolygonalPrismDataConfig.h"
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
namespace sync_policies = message_filters::sync_policies;
|
||||
|
||||
/** \brief @b ExtractPolygonalPrismData uses a set of point indices that represent a planar model, and together with
|
||||
* a given height, generates a 3D polygonal prism. The polygonal prism is then used to segment all points lying
|
||||
* inside it.
|
||||
*
|
||||
* An example of its usage is to extract the data lying within a set of 3D boundaries (e.g., objects supported by a plane).
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class ExtractPolygonalPrismData : public PCLNodelet
|
||||
{
|
||||
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
|
||||
typedef PointCloud::Ptr PointCloudPtr;
|
||||
typedef PointCloud::ConstPtr PointCloudConstPtr;
|
||||
|
||||
protected:
|
||||
/** \brief The output PointIndices publisher. */
|
||||
ros::Publisher pub_output_;
|
||||
|
||||
/** \brief The message filter subscriber for PointCloud2. */
|
||||
message_filters::Subscriber<PointCloud> sub_hull_filter_;
|
||||
|
||||
/** \brief Synchronized input, planar hull, and indices.*/
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloud, PointIndices> > > sync_input_hull_indices_e_;
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointCloud, PointIndices> > > sync_input_hull_indices_a_;
|
||||
|
||||
/** \brief Pointer to a dynamic reconfigure service. */
|
||||
boost::shared_ptr<dynamic_reconfigure::Server<ExtractPolygonalPrismDataConfig> > srv_;
|
||||
|
||||
/** \brief Null passthrough filter, used for pushing empty elements in the
|
||||
* synchronizer */
|
||||
message_filters::PassThrough<PointIndices> nf_;
|
||||
|
||||
/** \brief Input point cloud callback.
|
||||
* Because we want to use the same synchronizer object, we push back
|
||||
* empty elements with the same timestamp.
|
||||
*/
|
||||
inline void
|
||||
input_callback (const PointCloudConstPtr &input)
|
||||
{
|
||||
PointIndices cloud;
|
||||
cloud.header.stamp = input->header.stamp;
|
||||
nf_.add (boost::make_shared<PointIndices> (cloud));
|
||||
}
|
||||
|
||||
/** \brief Nodelet initialization routine. */
|
||||
void onInit ();
|
||||
|
||||
/** \brief Dynamic reconfigure callback
|
||||
* \param config the config object
|
||||
* \param level the dynamic reconfigure level
|
||||
*/
|
||||
void config_callback (ExtractPolygonalPrismDataConfig &config, uint32_t level);
|
||||
|
||||
/** \brief Input point cloud callback. Used when \a use_indices is set.
|
||||
* \param cloud the pointer to the input point cloud
|
||||
* \param hull the pointer to the planar hull point cloud
|
||||
* \param indices the pointer to the input point cloud indices
|
||||
*/
|
||||
void input_hull_indices_callback (const PointCloudConstPtr &cloud,
|
||||
const PointCloudConstPtr &hull,
|
||||
const PointIndicesConstPtr &indices);
|
||||
|
||||
private:
|
||||
/** \brief The PCL implementation used. */
|
||||
pcl::ExtractPolygonalPrismData<pcl::PointXYZ> impl_;
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
}
|
||||
|
||||
#endif //#ifndef PCL_EXTRACT_POLYGONAL_PRISM_DATA_H_
|
||||
@@ -0,0 +1,277 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2009, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: sac_segmentation.h 35564 2011-01-27 07:32:12Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef PCL_ROS_SAC_SEGMENTATION_H_
|
||||
#define PCL_ROS_SAC_SEGMENTATION_H_
|
||||
|
||||
#include "pcl_ros/pcl_nodelet.h"
|
||||
#include <message_filters/pass_through.h>
|
||||
|
||||
// PCL includes
|
||||
#include <pcl/segmentation/sac_segmentation.h>
|
||||
|
||||
// Dynamic reconfigure
|
||||
#include <dynamic_reconfigure/server.h>
|
||||
#include "pcl_ros/SACSegmentationConfig.h"
|
||||
#include "pcl_ros/SACSegmentationFromNormalsConfig.h"
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
namespace sync_policies = message_filters::sync_policies;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////
|
||||
/** \brief @b SACSegmentation represents the Nodelet segmentation class for Sample Consensus methods and models, in
|
||||
* the sense that it just creates a Nodelet wrapper for generic-purpose SAC-based segmentation.
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class SACSegmentation : public PCLNodelet
|
||||
{
|
||||
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
|
||||
typedef PointCloud::Ptr PointCloudPtr;
|
||||
typedef PointCloud::ConstPtr PointCloudConstPtr;
|
||||
|
||||
public:
|
||||
/** \brief Constructor. */
|
||||
SACSegmentation () : min_inliers_(0) {}
|
||||
|
||||
/** \brief Set the input TF frame the data should be transformed into before processing, if input.header.frame_id is different.
|
||||
* \param tf_frame the TF frame the input PointCloud should be transformed into before processing
|
||||
*/
|
||||
inline void setInputTFframe (std::string tf_frame) { tf_input_frame_ = tf_frame; }
|
||||
|
||||
/** \brief Get the TF frame the input PointCloud should be transformed into before processing. */
|
||||
inline std::string getInputTFframe () { return (tf_input_frame_); }
|
||||
|
||||
/** \brief Set the output TF frame the data should be transformed into after processing.
|
||||
* \param tf_frame the TF frame the PointCloud should be transformed into after processing
|
||||
*/
|
||||
inline void setOutputTFframe (std::string tf_frame) { tf_output_frame_ = tf_frame; }
|
||||
|
||||
/** \brief Get the TF frame the PointCloud should be transformed into after processing. */
|
||||
inline std::string getOutputTFframe () { return (tf_output_frame_); }
|
||||
|
||||
protected:
|
||||
// The minimum number of inliers a model must have in order to be considered valid.
|
||||
int min_inliers_;
|
||||
|
||||
// ROS nodelet attributes
|
||||
/** \brief The output PointIndices publisher. */
|
||||
ros::Publisher pub_indices_;
|
||||
|
||||
/** \brief The output ModelCoefficients publisher. */
|
||||
ros::Publisher pub_model_;
|
||||
|
||||
/** \brief The input PointCloud subscriber. */
|
||||
ros::Subscriber sub_input_;
|
||||
|
||||
/** \brief Pointer to a dynamic reconfigure service. */
|
||||
boost::shared_ptr<dynamic_reconfigure::Server<SACSegmentationConfig> > srv_;
|
||||
|
||||
/** \brief The input TF frame the data should be transformed into, if input.header.frame_id is different. */
|
||||
std::string tf_input_frame_;
|
||||
|
||||
/** \brief The original data input TF frame. */
|
||||
std::string tf_input_orig_frame_;
|
||||
|
||||
/** \brief The output TF frame the data should be transformed into, if input.header.frame_id is different. */
|
||||
std::string tf_output_frame_;
|
||||
|
||||
/** \brief Null passthrough filter, used for pushing empty elements in the
|
||||
* synchronizer */
|
||||
message_filters::PassThrough<PointIndices> nf_pi_;
|
||||
|
||||
/** \brief Nodelet initialization routine. */
|
||||
virtual void onInit ();
|
||||
|
||||
/** \brief Dynamic reconfigure callback
|
||||
* \param config the config object
|
||||
* \param level the dynamic reconfigure level
|
||||
*/
|
||||
void config_callback (SACSegmentationConfig &config, uint32_t level);
|
||||
|
||||
/** \brief Input point cloud callback. Used when \a use_indices is set.
|
||||
* \param cloud the pointer to the input point cloud
|
||||
* \param indices the pointer to the input point cloud indices
|
||||
*/
|
||||
void input_indices_callback (const PointCloudConstPtr &cloud, const PointIndicesConstPtr &indices);
|
||||
|
||||
/** \brief Pointer to a set of indices stored internally.
|
||||
* (used when \a latched_indices_ is set).
|
||||
*/
|
||||
PointIndices indices_;
|
||||
|
||||
/** \brief Indices callback. Used when \a latched_indices_ is set.
|
||||
* \param indices the pointer to the input point cloud indices
|
||||
*/
|
||||
inline void
|
||||
indices_callback (const PointIndicesConstPtr &indices)
|
||||
{
|
||||
indices_ = *indices;
|
||||
}
|
||||
|
||||
/** \brief Input callback. Used when \a latched_indices_ is set.
|
||||
* \param input the pointer to the input point cloud
|
||||
*/
|
||||
inline void
|
||||
input_callback (const PointCloudConstPtr &input)
|
||||
{
|
||||
indices_.header = input->header;
|
||||
PointIndicesConstPtr indices;
|
||||
indices.reset (new PointIndices (indices_));
|
||||
nf_pi_.add (indices);
|
||||
}
|
||||
|
||||
private:
|
||||
/** \brief Internal mutex. */
|
||||
boost::mutex mutex_;
|
||||
|
||||
/** \brief The PCL implementation used. */
|
||||
pcl::SACSegmentation<pcl::PointXYZ> impl_;
|
||||
|
||||
/** \brief Synchronized input, and indices.*/
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointIndices> > > sync_input_indices_e_;
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointIndices> > > sync_input_indices_a_;
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////
|
||||
/** \brief @b SACSegmentationFromNormals represents the PCL nodelet segmentation class for Sample Consensus methods and
|
||||
* models that require the use of surface normals for estimation.
|
||||
*/
|
||||
class SACSegmentationFromNormals: public SACSegmentation
|
||||
{
|
||||
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
|
||||
typedef PointCloud::Ptr PointCloudPtr;
|
||||
typedef PointCloud::ConstPtr PointCloudConstPtr;
|
||||
|
||||
typedef pcl::PointCloud<pcl::Normal> PointCloudN;
|
||||
typedef PointCloudN::Ptr PointCloudNPtr;
|
||||
typedef PointCloudN::ConstPtr PointCloudNConstPtr;
|
||||
|
||||
public:
|
||||
/** \brief Set the input TF frame the data should be transformed into before processing, if input.header.frame_id is different.
|
||||
* \param tf_frame the TF frame the input PointCloud should be transformed into before processing
|
||||
*/
|
||||
inline void setInputTFframe (std::string tf_frame) { tf_input_frame_ = tf_frame; }
|
||||
|
||||
/** \brief Get the TF frame the input PointCloud should be transformed into before processing. */
|
||||
inline std::string getInputTFframe () { return (tf_input_frame_); }
|
||||
|
||||
/** \brief Set the output TF frame the data should be transformed into after processing.
|
||||
* \param tf_frame the TF frame the PointCloud should be transformed into after processing
|
||||
*/
|
||||
inline void setOutputTFframe (std::string tf_frame) { tf_output_frame_ = tf_frame; }
|
||||
|
||||
/** \brief Get the TF frame the PointCloud should be transformed into after processing. */
|
||||
inline std::string getOutputTFframe () { return (tf_output_frame_); }
|
||||
|
||||
protected:
|
||||
// ROS nodelet attributes
|
||||
/** \brief The normals PointCloud subscriber filter. */
|
||||
message_filters::Subscriber<PointCloudN> sub_normals_filter_;
|
||||
|
||||
/** \brief The input PointCloud subscriber. */
|
||||
ros::Subscriber sub_axis_;
|
||||
|
||||
/** \brief Pointer to a dynamic reconfigure service. */
|
||||
boost::shared_ptr<dynamic_reconfigure::Server<SACSegmentationFromNormalsConfig> > srv_;
|
||||
|
||||
/** \brief Input point cloud callback.
|
||||
* Because we want to use the same synchronizer object, we push back
|
||||
* empty elements with the same timestamp.
|
||||
*/
|
||||
inline void
|
||||
input_callback (const PointCloudConstPtr &cloud)
|
||||
{
|
||||
PointIndices indices;
|
||||
indices.header.stamp = cloud->header.stamp;
|
||||
nf_.add (boost::make_shared<PointIndices> (indices));
|
||||
}
|
||||
|
||||
/** \brief Null passthrough filter, used for pushing empty elements in the
|
||||
* synchronizer */
|
||||
message_filters::PassThrough<PointIndices> nf_;
|
||||
|
||||
/** \brief The input TF frame the data should be transformed into, if input.header.frame_id is different. */
|
||||
std::string tf_input_frame_;
|
||||
/** \brief The original data input TF frame. */
|
||||
std::string tf_input_orig_frame_;
|
||||
/** \brief The output TF frame the data should be transformed into, if input.header.frame_id is different. */
|
||||
std::string tf_output_frame_;
|
||||
|
||||
/** \brief Nodelet initialization routine. */
|
||||
virtual void onInit ();
|
||||
|
||||
/** \brief Model callback
|
||||
* \param model the sample consensus model found
|
||||
*/
|
||||
void axis_callback (const pcl::ModelCoefficientsConstPtr &model);
|
||||
|
||||
/** \brief Dynamic reconfigure callback
|
||||
* \param config the config object
|
||||
* \param level the dynamic reconfigure level
|
||||
*/
|
||||
void config_callback (SACSegmentationFromNormalsConfig &config, uint32_t level);
|
||||
|
||||
/** \brief Input point cloud callback.
|
||||
* \param cloud the pointer to the input point cloud
|
||||
* \param cloud_normals the pointer to the input point cloud normals
|
||||
* \param indices the pointer to the input point cloud indices
|
||||
*/
|
||||
void input_normals_indices_callback (const PointCloudConstPtr &cloud,
|
||||
const PointCloudNConstPtr &cloud_normals,
|
||||
const PointIndicesConstPtr &indices);
|
||||
|
||||
private:
|
||||
/** \brief Internal mutex. */
|
||||
boost::mutex mutex_;
|
||||
|
||||
/** \brief The PCL implementation used. */
|
||||
pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> impl_;
|
||||
|
||||
/** \brief Synchronized input, normals, and indices.*/
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointCloudN, PointIndices> > > sync_input_normals_indices_a_;
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloudN, PointIndices> > > sync_input_normals_indices_e_;
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
}
|
||||
|
||||
#endif //#ifndef PCL_ROS_SAC_SEGMENTATION_H_
|
||||
@@ -0,0 +1,106 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2010, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: segment_differences.h 35361 2011-01-20 04:34:49Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef PCL_ROS_SEGMENT_DIFFERENCES_H_
|
||||
#define PCL_ROS_SEGMENT_DIFFERENCES_H_
|
||||
|
||||
#include <pcl/segmentation/segment_differences.h>
|
||||
#include "pcl_ros/pcl_nodelet.h"
|
||||
|
||||
// Dynamic reconfigure
|
||||
#include <dynamic_reconfigure/server.h>
|
||||
#include "pcl_ros/SegmentDifferencesConfig.h"
|
||||
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
namespace sync_policies = message_filters::sync_policies;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////
|
||||
////////////////////////////////////////////////////////////////////////////////////////////
|
||||
////////////////////////////////////////////////////////////////////////////////////////////
|
||||
/** \brief @b SegmentDifferences obtains the difference between two spatially aligned point clouds and returns the
|
||||
* difference between them for a maximum given distance threshold.
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class SegmentDifferences : public PCLNodelet
|
||||
{
|
||||
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
|
||||
typedef PointCloud::Ptr PointCloudPtr;
|
||||
typedef PointCloud::ConstPtr PointCloudConstPtr;
|
||||
|
||||
public:
|
||||
/** \brief Empty constructor. */
|
||||
SegmentDifferences () {};
|
||||
|
||||
protected:
|
||||
/** \brief The message filter subscriber for PointCloud2. */
|
||||
message_filters::Subscriber<PointCloud> sub_target_filter_;
|
||||
|
||||
/** \brief Synchronized input, and planar hull.*/
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloud> > > sync_input_target_e_;
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointCloud> > > sync_input_target_a_;
|
||||
|
||||
/** \brief Pointer to a dynamic reconfigure service. */
|
||||
boost::shared_ptr<dynamic_reconfigure::Server<SegmentDifferencesConfig> > srv_;
|
||||
|
||||
/** \brief Nodelet initialization routine. */
|
||||
void onInit ();
|
||||
|
||||
/** \brief Dynamic reconfigure callback
|
||||
* \param config the config object
|
||||
* \param level the dynamic reconfigure level
|
||||
*/
|
||||
void config_callback (SegmentDifferencesConfig &config, uint32_t level);
|
||||
|
||||
/** \brief Input point cloud callback.
|
||||
* \param cloud the pointer to the input point cloud
|
||||
* \param cloud_target the pointcloud that we want to segment \a cloud from
|
||||
*/
|
||||
void input_target_callback (const PointCloudConstPtr &cloud,
|
||||
const PointCloudConstPtr &cloud_target);
|
||||
|
||||
private:
|
||||
/** \brief The PCL implementation used. */
|
||||
pcl::SegmentDifferences<pcl::PointXYZ> impl_;
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
}
|
||||
|
||||
#endif //#ifndef PCL_ROS_SEGMENT_DIFFERENCES_H_
|
||||
@@ -0,0 +1,92 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2010, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: convex_hull.h 36116 2011-02-22 00:05:23Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef PCL_ROS_CONVEX_HULL_2D_H_
|
||||
#define PCL_ROS_CONVEX_HULL_2D_H_
|
||||
|
||||
#include "pcl_ros/pcl_nodelet.h"
|
||||
|
||||
// PCL includes
|
||||
#include <pcl/surface/convex_hull.h>
|
||||
|
||||
// Dynamic reconfigure
|
||||
#include <dynamic_reconfigure/server.h>
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
namespace sync_policies = message_filters::sync_policies;
|
||||
|
||||
/** \brief @b ConvexHull2D represents a 2D ConvexHull implementation.
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class ConvexHull2D : public PCLNodelet
|
||||
{
|
||||
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
|
||||
typedef PointCloud::Ptr PointCloudPtr;
|
||||
typedef PointCloud::ConstPtr PointCloudConstPtr;
|
||||
|
||||
private:
|
||||
/** \brief Nodelet initialization routine. */
|
||||
virtual void onInit ();
|
||||
|
||||
/** \brief Input point cloud callback.
|
||||
* \param cloud the pointer to the input point cloud
|
||||
* \param indices the pointer to the input point cloud indices
|
||||
*/
|
||||
void input_indices_callback (const PointCloudConstPtr &cloud,
|
||||
const PointIndicesConstPtr &indices);
|
||||
|
||||
private:
|
||||
/** \brief The PCL implementation used. */
|
||||
pcl::ConvexHull<pcl::PointXYZ> impl_;
|
||||
|
||||
/** \brief The input PointCloud subscriber. */
|
||||
ros::Subscriber sub_input_;
|
||||
|
||||
/** \brief Publisher for PolygonStamped. */
|
||||
ros::Publisher pub_plane_;
|
||||
|
||||
/** \brief Synchronized input, and indices.*/
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointIndices> > > sync_input_indices_e_;
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointIndices> > > sync_input_indices_a_;
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
}
|
||||
|
||||
#endif //#ifndef PCL_ROS_CONVEX_HULL_2D_H_
|
||||
@@ -0,0 +1,145 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2010, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: moving_least_squares.h 36097 2011-02-20 14:18:58Z marton $
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef PCL_ROS_MOVING_LEAST_SQUARES_H_
|
||||
#define PCL_ROS_MOVING_LEAST_SQUARES_H_
|
||||
|
||||
#include "pcl_ros/pcl_nodelet.h"
|
||||
|
||||
// PCL includes
|
||||
#include <pcl/surface/mls.h>
|
||||
|
||||
// Dynamic reconfigure
|
||||
#include <dynamic_reconfigure/server.h>
|
||||
#include "pcl_ros/MLSConfig.h"
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
namespace sync_policies = message_filters::sync_policies;
|
||||
|
||||
/** \brief @b MovingLeastSquares represents a nodelet using the MovingLeastSquares implementation.
|
||||
* The type of the output is the same as the input, it only smooths the XYZ coordinates according to the parameters.
|
||||
* Normals are estimated at each point as well and published on a separate topic.
|
||||
* \author Radu Bogdan Rusu, Zoltan-Csaba Marton
|
||||
*/
|
||||
class MovingLeastSquares : public PCLNodelet
|
||||
{
|
||||
typedef pcl::PointXYZ PointIn;
|
||||
typedef pcl::Normal NormalOut;
|
||||
|
||||
typedef pcl::PointCloud<PointIn> PointCloudIn;
|
||||
typedef PointCloudIn::Ptr PointCloudInPtr;
|
||||
typedef PointCloudIn::ConstPtr PointCloudInConstPtr;
|
||||
typedef pcl::PointCloud<NormalOut> NormalCloudOut;
|
||||
|
||||
typedef pcl::KdTree<PointIn> KdTree;
|
||||
typedef pcl::KdTree<PointIn>::Ptr KdTreePtr;
|
||||
|
||||
protected:
|
||||
/** \brief An input point cloud describing the surface that is to be used for nearest neighbors estimation. */
|
||||
PointCloudInConstPtr surface_;
|
||||
|
||||
/** \brief A pointer to the spatial search object. */
|
||||
KdTreePtr tree_;
|
||||
|
||||
/** \brief The nearest neighbors search radius for each point. */
|
||||
double search_radius_;
|
||||
|
||||
/** \brief The number of K nearest neighbors to use for each point. */
|
||||
//int k_;
|
||||
|
||||
/** \brief Whether to use a polynomial fit. */
|
||||
bool use_polynomial_fit_;
|
||||
|
||||
/** \brief The order of the polynomial to be fit. */
|
||||
int polynomial_order_;
|
||||
|
||||
/** \brief How 'flat' should the neighbor weighting gaussian be (the smaller, the more local the fit). */
|
||||
double gaussian_parameter_;
|
||||
|
||||
// ROS nodelet attributes
|
||||
/** \brief The surface PointCloud subscriber filter. */
|
||||
message_filters::Subscriber<PointCloudIn> sub_surface_filter_;
|
||||
|
||||
/** \brief Parameter for the spatial locator tree. By convention, the values represent:
|
||||
* 0: ANN (Approximate Nearest Neigbor library) kd-tree
|
||||
* 1: FLANN (Fast Library for Approximate Nearest Neighbors) kd-tree
|
||||
* 2: Organized spatial dataset index
|
||||
*/
|
||||
int spatial_locator_type_;
|
||||
|
||||
/** \brief Pointer to a dynamic reconfigure service. */
|
||||
boost::shared_ptr <dynamic_reconfigure::Server<MLSConfig> > srv_;
|
||||
|
||||
/** \brief Dynamic reconfigure callback
|
||||
* \param config the config object
|
||||
* \param level the dynamic reconfigure level
|
||||
*/
|
||||
void config_callback (MLSConfig &config, uint32_t level);
|
||||
|
||||
private:
|
||||
/** \brief Nodelet initialization routine. */
|
||||
virtual void onInit ();
|
||||
|
||||
/** \brief Input point cloud callback.
|
||||
* \param cloud the pointer to the input point cloud
|
||||
* \param indices the pointer to the input point cloud indices
|
||||
*/
|
||||
void input_indices_callback (const PointCloudInConstPtr &cloud,
|
||||
const PointIndicesConstPtr &indices);
|
||||
|
||||
|
||||
private:
|
||||
/** \brief The PCL implementation used. */
|
||||
pcl::MovingLeastSquares<PointIn, NormalOut> impl_;
|
||||
|
||||
/** \brief The input PointCloud subscriber. */
|
||||
ros::Subscriber sub_input_;
|
||||
|
||||
/** \brief The output PointCloud (containing the normals) publisher. */
|
||||
ros::Publisher pub_normals_;
|
||||
|
||||
/** \brief Synchronized input, and indices.*/
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, PointIndices> > > sync_input_indices_e_;
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn, PointIndices> > > sync_input_indices_a_;
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
}
|
||||
|
||||
#endif //#ifndef PCL_ROS_MOVING_LEAST_SQUARES_H_
|
||||
Reference in New Issue
Block a user