copy features/segmentation/surface from fuerte-devel
This commit is contained in:
Kei Okada
Paul Bovbel
parent
669c2d2b04
commit
f2553aac6c
@ -18,8 +18,15 @@ include_directories(include)
|
||||
link_directories(${catkin_LIBRARY_DIRS} ${Boost_LIBRARY_DIRS} ${Eigen_LIBRARY_DIRS} ${PCL_LIBRARY_DIRS})
|
||||
|
||||
# generate the dynamic_reconfigure config file
|
||||
generate_dynamic_reconfigure_options(cfg/Filter.cfg
|
||||
generate_dynamic_reconfigure_options(cfg/Feature.cfg
|
||||
cfg/Filter.cfg
|
||||
cfg/EuclideanClusterExtraction.cfg
|
||||
cfg/ExtractIndices.cfg
|
||||
cfg/ExtractPolygonalPrismData.cfg
|
||||
cfg/MLS.cfg
|
||||
cfg/SACSegmentation.cfg
|
||||
cfg/SACSegmentationFromNormals.cfg
|
||||
cfg/SegmentDifferences.cfg
|
||||
cfg/StatisticalOutlierRemoval.cfg
|
||||
cfg/VoxelGrid.cfg
|
||||
|
||||
@ -53,6 +60,26 @@ target_link_libraries (pcl_ros_io pcl_ros_tf ${PCL_LIBRARIES} ${Boost_LIBRARIES}
|
||||
|
||||
class_loader_hide_library_symbols(pcl_ros_io)
|
||||
|
||||
# ---[ PCL ROS - Features
|
||||
add_library (pcl_ros_features
|
||||
src/pcl_ros/features/feature.cpp
|
||||
# Compilation is much faster if we include all the following CPP files in feature.cpp
|
||||
# src/pcl_ros/features/boundary.cpp
|
||||
# src/pcl_ros/features/fpfh.cpp
|
||||
# src/pcl_ros/features/fpfh_omp.cpp
|
||||
# src/pcl_ros/features/moment_invariants.cpp
|
||||
# src/pcl_ros/features/normal_3d.cpp
|
||||
# src/pcl_ros/features/normal_3d_omp.cpp
|
||||
# src/pcl_ros/features/pfh.cpp
|
||||
# src/pcl_ros/features/principal_curvatures.cpp
|
||||
# src/pcl_ros/features/vfh.cpp
|
||||
)
|
||||
#rosbuild_add_compile_flags (pcl_ros_features ${SSE_FLAGS})
|
||||
target_link_libraries (pcl_ros_features ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES})
|
||||
|
||||
class_loader_hide_library_symbols(pcl_ros_features)
|
||||
|
||||
|
||||
# ---[ PCL ROS - Filters
|
||||
add_library (pcl_ros_filters
|
||||
src/pcl_ros/filters/filter.cpp
|
||||
@ -68,6 +95,31 @@ target_link_libraries (pcl_ros_filters pcl_ros_tf ${PCL_LIBRARIES} ${Boost_LIBRA
|
||||
|
||||
class_loader_hide_library_symbols(pcl_ros_filters)
|
||||
|
||||
# ---[ Point Cloud Library - Segmentation
|
||||
add_library (pcl_ros_segmentation
|
||||
src/pcl_ros/segmentation/segmentation.cpp
|
||||
src/pcl_ros/segmentation/segment_differences.cpp
|
||||
# src/pcl_ros/segmentation/extract_clusters.cpp
|
||||
src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp
|
||||
src/pcl_ros/segmentation/sac_segmentation.cpp
|
||||
)
|
||||
#rosbuild_add_compile_flags (pcl_ros_segmentation ${SSE_FLAGS})
|
||||
target_link_libraries (pcl_ros_segmentation pcl_segmentation pcl_ros_tf ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES})
|
||||
|
||||
class_loader_hide_library_symbols(pcl_ros_segmentation)
|
||||
|
||||
# ---[ Point Cloud Library - Surface
|
||||
add_library (pcl_ros_surface
|
||||
src/pcl_ros/surface/surface.cpp
|
||||
# Compilation is much faster if we include all the following CPP files in surface.cpp
|
||||
src/pcl_ros/surface/convex_hull.cpp
|
||||
#src/pcl_ros/surface/moving_least_squares.cpp
|
||||
)
|
||||
#rosbuild_add_compile_flags (pcl_ros_surface ${SSE_FLAGS})
|
||||
target_link_libraries (pcl_ros_surface ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES})
|
||||
|
||||
class_loader_hide_library_symbols(pcl_ros_surface)
|
||||
|
||||
############ TOOLS
|
||||
|
||||
add_executable (pcd_to_pointcloud tools/pcd_to_pointcloud.cpp)
|
||||
@ -90,7 +142,7 @@ install(DIRECTORY include/${PROJECT_NAME}/
|
||||
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
|
||||
)
|
||||
|
||||
install(TARGETS pcl_ros_tf pcl_ros_io pcl_ros_filters pcd_to_pointcloud pointcloud_to_pcd bag_to_pcd convert_pcd_to_image convert_pointcloud_to_image
|
||||
install(TARGETS pcl_ros_tf pcl_ros_io pcl_ros_features pcl_ros_filters pcl_ros_surface pcl_ros_segmentation pcd_to_pointcloud pointcloud_to_pcd bag_to_pcd convert_pcd_to_image convert_pointcloud_to_image
|
||||
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
|
||||
19
pcl_ros/cfg/EuclideanClusterExtraction.cfg
Executable file
19
pcl_ros/cfg/EuclideanClusterExtraction.cfg
Executable file
@ -0,0 +1,19 @@
|
||||
#! /usr/bin/env python
|
||||
|
||||
# set up parameters that we care about
|
||||
PACKAGE = 'pcl_ros'
|
||||
|
||||
import roslib;
|
||||
roslib.load_manifest (PACKAGE);
|
||||
from dynamic_reconfigure.parameter_generator import *;
|
||||
gen = ParameterGenerator ()
|
||||
|
||||
# enabling/disabling the unit limits
|
||||
# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""):
|
||||
gen.add ("cluster_tolerance", double_t, 0, "The spatial tolerance as a measure in the L2 Euclidean space", 0.05, 0.0, 2.0)
|
||||
gen.add ("cluster_min_size", int_t, 0, "The minimum number of points that a cluster must contain in order to be accepted", 1, 0, 1000)
|
||||
gen.add ("cluster_max_size", int_t, 0, "The maximum number of points that a cluster must contain in order to be accepted", 2147483647, 0, 2147483647)
|
||||
gen.add ("max_clusters", int_t, 0, "The maximum number of clusters to extract.", 2147483647, 1, 2147483647)
|
||||
|
||||
exit (gen.generate (PACKAGE, "pcl_ros", "EuclideanClusterExtraction"))
|
||||
|
||||
15
pcl_ros/cfg/ExtractPolygonalPrismData.cfg
Executable file
15
pcl_ros/cfg/ExtractPolygonalPrismData.cfg
Executable file
@ -0,0 +1,15 @@
|
||||
#! /usr/bin/env python
|
||||
|
||||
# set up parameters that we care about
|
||||
PACKAGE = 'pcl_ros'
|
||||
|
||||
import roslib; roslib.load_manifest (PACKAGE);
|
||||
from dynamic_reconfigure.parameter_generator import *;
|
||||
import roslib.packages
|
||||
|
||||
gen = ParameterGenerator ()
|
||||
# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""):
|
||||
gen.add ("height_min", double_t, 0, "The minimum allowed distance to the plane model value a point will be considered from", 0.0, -10.0, 10.0)
|
||||
gen.add ("height_max", double_t, 0, "The maximum allowed distance to the plane model value a point will be considered from", 0.5, -10.0, 10.0)
|
||||
|
||||
exit (gen.generate (PACKAGE, "pcl_ros", "ExtractPolygonalPrismData"))
|
||||
19
pcl_ros/cfg/Feature.cfg
Executable file
19
pcl_ros/cfg/Feature.cfg
Executable file
@ -0,0 +1,19 @@
|
||||
#! /usr/bin/env python
|
||||
|
||||
# set up parameters that we care about
|
||||
PACKAGE = 'pcl_ros'
|
||||
|
||||
import roslib;
|
||||
roslib.load_manifest (PACKAGE);
|
||||
from dynamic_reconfigure.parameter_generator import *;
|
||||
gen = ParameterGenerator ()
|
||||
|
||||
#enum = gen.enum ([ gen.const("ANN", int_t, 0, "ANN"), gen.const("FLANN", int_t, 1, "FLANN"), gen.const("organized", int_t, 2, "Dense/organized data locator") ], "Set the spatial locator")
|
||||
# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""):
|
||||
gen.add ("k_search", int_t, 0, "Number of k-nearest neighbors to search for", 10, 0, 1000)
|
||||
gen.add ("radius_search", double_t, 0, "Sphere radius for nearest neighbor search", 0.0, 0.0, 0.5)
|
||||
#gen.add ("spatial_locator", int_t, 0, "Set the spatial locator", 0, 0, 2, enum)
|
||||
#gen.add ("spatial_locator", str_t, 0, "Set the spatial locator", "ANN")
|
||||
|
||||
exit (gen.generate (PACKAGE, "pcl_ros", "Feature"))
|
||||
|
||||
22
pcl_ros/cfg/MLS.cfg
Executable file
22
pcl_ros/cfg/MLS.cfg
Executable file
@ -0,0 +1,22 @@
|
||||
#! /usr/bin/env python
|
||||
|
||||
# set up parameters that we care about
|
||||
PACKAGE = 'pcl_ros'
|
||||
|
||||
import roslib;
|
||||
roslib.load_manifest (PACKAGE);
|
||||
from dynamic_reconfigure.parameter_generator import *;
|
||||
gen = ParameterGenerator ()
|
||||
|
||||
enum = gen.enum ([ gen.const("ANN", int_t, 0, "ANN"), gen.const("FLANN", int_t, 1, "FLANN"), gen.const("organized", int_t, 2, "Dense/organized data locator") ], "Set the spatial locator")
|
||||
# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""):
|
||||
gen.add ("spatial_locator", int_t, 0, "Set the spatial locator", 0, 0, 2, enum)
|
||||
gen.add ("search_radius", double_t, 0, "Sphere radius for nearest neighbor search", 0.02, 0.0, 0.5)
|
||||
#gen.add ("k_search", int_t, 0, "Number of k-nearest neighbors to search for", 10, 0, 1000)
|
||||
gen.add ("use_polynomial_fit", bool_t, 0, "Whether to use polynomial fit", True)
|
||||
gen.add ("polynomial_order", int_t, 0, "Set the spatial locator", 2, 0, 5)
|
||||
gen.add ("gaussian_parameter", double_t, 0, "How 'flat' should the neighbor weighting gaussian be (the smaller, the more local the fit)", 0.02, 0.0, 0.5)
|
||||
#gen.add ("spatial_locator", str_t, 0, "Set the spatial locator", "ANN")
|
||||
|
||||
exit (gen.generate (PACKAGE, "pcl_ros", "MLS"))
|
||||
|
||||
14
pcl_ros/cfg/SACSegmentation.cfg
Executable file
14
pcl_ros/cfg/SACSegmentation.cfg
Executable file
@ -0,0 +1,14 @@
|
||||
#! /usr/bin/env python
|
||||
|
||||
# set up parameters that we care about
|
||||
PACKAGE = 'pcl_ros'
|
||||
|
||||
import roslib; roslib.load_manifest (PACKAGE);
|
||||
from dynamic_reconfigure.parameter_generator import *;
|
||||
import SACSegmentation_common as common
|
||||
|
||||
gen = ParameterGenerator ()
|
||||
common.add_common_parameters (gen)
|
||||
|
||||
exit (gen.generate (PACKAGE, "pcl_ros", "SACSegmentation"))
|
||||
|
||||
16
pcl_ros/cfg/SACSegmentationFromNormals.cfg
Executable file
16
pcl_ros/cfg/SACSegmentationFromNormals.cfg
Executable file
@ -0,0 +1,16 @@
|
||||
#! /usr/bin/env python
|
||||
|
||||
# set up parameters that we care about
|
||||
PACKAGE = 'pcl_ros'
|
||||
|
||||
import roslib; roslib.load_manifest (PACKAGE);
|
||||
from dynamic_reconfigure.parameter_generator import *;
|
||||
import roslib.packages
|
||||
import SACSegmentation_common as common
|
||||
|
||||
gen = ParameterGenerator ()
|
||||
# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""):
|
||||
gen.add ("normal_distance_weight", double_t, 0, "The relative weight (between 0 and 1) to give to the angular distance (0 to pi/2) between point normals and the plane normal.", 0.1, 0, 1.0)
|
||||
common.add_common_parameters (gen)
|
||||
|
||||
exit (gen.generate (PACKAGE, "pcl_ros", "SACSegmentationFromNormals"))
|
||||
21
pcl_ros/cfg/SACSegmentation_common.py
Normal file
21
pcl_ros/cfg/SACSegmentation_common.py
Normal file
@ -0,0 +1,21 @@
|
||||
#! /usr/bin/env python
|
||||
|
||||
# set up parameters that we care about
|
||||
PACKAGE = 'pcl_ros'
|
||||
|
||||
import roslib; roslib.load_manifest (PACKAGE);
|
||||
from dynamic_reconfigure.parameter_generator import *;
|
||||
|
||||
def add_common_parameters (gen):
|
||||
# add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = "")
|
||||
gen.add ("max_iterations", int_t, 0, "The maximum number of iterations the algorithm will run for", 50, 0, 100000)
|
||||
gen.add ("probability", double_t, 0, "The desired probability of choosing at least one sample free from outliers", 0.99, 0.5, 0.99)
|
||||
gen.add ("distance_threshold", double_t, 0, "The distance to model threshold", 0.02, 0, 1.0)
|
||||
gen.add ("optimize_coefficients", bool_t, 0, "Model coefficient refinement", True)
|
||||
gen.add ("radius_min", double_t, 0, "The minimum allowed model radius (where applicable)", 0.0, 0, 1.0)
|
||||
gen.add ("radius_max", double_t, 0, "The maximum allowed model radius (where applicable)", 0.05, 0, 1.0)
|
||||
gen.add ("eps_angle", double_t, 0, "The maximum allowed difference between the model normal and the given axis in radians.", 0.17, 0.0, 1.5707)
|
||||
gen.add ("min_inliers", int_t, 0, "The minimum number of inliers a model must have in order to be considered valid.", 0, 0, 100000)
|
||||
gen.add ("input_frame", str_t, 0, "The input TF frame the data should be transformed into, if input.header.frame_id is different.", "")
|
||||
gen.add ("output_frame", str_t, 0, "The output TF frame the data should be transformed into, if input.header.frame_id is different.", "")
|
||||
|
||||
16
pcl_ros/cfg/SegmentDifferences.cfg
Executable file
16
pcl_ros/cfg/SegmentDifferences.cfg
Executable file
@ -0,0 +1,16 @@
|
||||
#! /usr/bin/env python
|
||||
|
||||
# set up parameters that we care about
|
||||
PACKAGE = 'pcl_ros'
|
||||
|
||||
import roslib;
|
||||
roslib.load_manifest (PACKAGE);
|
||||
from dynamic_reconfigure.parameter_generator import *;
|
||||
gen = ParameterGenerator ()
|
||||
|
||||
# enabling/disabling the unit limits
|
||||
# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""):
|
||||
gen.add ("distance_threshold", double_t, 0, "The distance tolerance as a measure in the L2 Euclidean space between corresponding points.", 0.0, 0.0, 2.0)
|
||||
|
||||
exit (gen.generate (PACKAGE, "pcl_ros", "SegmentDifferences"))
|
||||
|
||||
86
pcl_ros/include/pcl_ros/features/boundary.h
Normal file
86
pcl_ros/include/pcl_ros/features/boundary.h
Normal file
@ -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_
|
||||
|
||||
|
||||
240
pcl_ros/include/pcl_ros/features/feature.h
Normal file
240
pcl_ros/include/pcl_ros/features/feature.h
Normal file
@ -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_
|
||||
100
pcl_ros/include/pcl_ros/features/fpfh.h
Normal file
100
pcl_ros/include/pcl_ros/features/fpfh.h
Normal file
@ -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_
|
||||
|
||||
|
||||
96
pcl_ros/include/pcl_ros/features/fpfh_omp.h
Normal file
96
pcl_ros/include/pcl_ros/features/fpfh_omp.h
Normal file
@ -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_
|
||||
|
||||
83
pcl_ros/include/pcl_ros/features/moment_invariants.h
Normal file
83
pcl_ros/include/pcl_ros/features/moment_invariants.h
Normal file
@ -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_
|
||||
|
||||
|
||||
86
pcl_ros/include/pcl_ros/features/normal_3d.h
Normal file
86
pcl_ros/include/pcl_ros/features/normal_3d.h
Normal file
@ -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_
|
||||
|
||||
79
pcl_ros/include/pcl_ros/features/normal_3d_omp.h
Normal file
79
pcl_ros/include/pcl_ros/features/normal_3d_omp.h
Normal file
@ -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_
|
||||
86
pcl_ros/include/pcl_ros/features/normal_3d_tbb.h
Normal file
86
pcl_ros/include/pcl_ros/features/normal_3d_tbb.h
Normal file
@ -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_
|
||||
|
||||
|
||||
100
pcl_ros/include/pcl_ros/features/pfh.h
Normal file
100
pcl_ros/include/pcl_ros/features/pfh.h
Normal file
@ -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_
|
||||
|
||||
|
||||
83
pcl_ros/include/pcl_ros/features/principal_curvatures.h
Normal file
83
pcl_ros/include/pcl_ros/features/principal_curvatures.h
Normal file
@ -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_
|
||||
83
pcl_ros/include/pcl_ros/features/vfh.h
Normal file
83
pcl_ros/include/pcl_ros/features/vfh.h
Normal file
@ -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_
|
||||
105
pcl_ros/include/pcl_ros/segmentation/extract_clusters.h
Normal file
105
pcl_ros/include/pcl_ros/segmentation/extract_clusters.h
Normal file
@ -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_
|
||||
277
pcl_ros/include/pcl_ros/segmentation/sac_segmentation.h
Normal file
277
pcl_ros/include/pcl_ros/segmentation/sac_segmentation.h
Normal file
@ -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_
|
||||
106
pcl_ros/include/pcl_ros/segmentation/segment_differences.h
Normal file
106
pcl_ros/include/pcl_ros/segmentation/segment_differences.h
Normal file
@ -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_
|
||||
92
pcl_ros/include/pcl_ros/surface/convex_hull.h
Normal file
92
pcl_ros/include/pcl_ros/surface/convex_hull.h
Normal file
@ -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_
|
||||
145
pcl_ros/include/pcl_ros/surface/moving_least_squares.h
Normal file
145
pcl_ros/include/pcl_ros/surface/moving_least_squares.h
Normal file
@ -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_
|
||||
77
pcl_ros/src/pcl_ros/features/boundary.cpp
Normal file
77
pcl_ros/src/pcl_ros/features/boundary.cpp
Normal file
@ -0,0 +1,77 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2009, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: boundary.cpp 35361 2011-01-20 04:34:49Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/features/boundary.h"
|
||||
|
||||
void
|
||||
pcl_ros::BoundaryEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
|
||||
{
|
||||
PointCloudOut output;
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
}
|
||||
|
||||
void
|
||||
pcl_ros::BoundaryEstimation::computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudNConstPtr &normals,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices)
|
||||
{
|
||||
// Set the parameters
|
||||
impl_.setKSearch (k_);
|
||||
impl_.setRadiusSearch (search_radius_);
|
||||
// Initialize the spatial locator
|
||||
initTree (spatial_locator_type_, tree_, k_);
|
||||
impl_.setSearchMethod (tree_);
|
||||
|
||||
// Set the inputs
|
||||
impl_.setInputCloud (cloud);
|
||||
impl_.setIndices (indices);
|
||||
impl_.setSearchSurface (surface);
|
||||
impl_.setInputNormals (normals);
|
||||
// Estimate the feature
|
||||
PointCloudOut output;
|
||||
impl_.compute (output);
|
||||
|
||||
// Enforce that the TF frame and the timestamp are copied
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
}
|
||||
|
||||
typedef pcl_ros::BoundaryEstimation BoundaryEstimation;
|
||||
PLUGINLIB_DECLARE_CLASS (pcl, BoundaryEstimation, BoundaryEstimation, nodelet::Nodelet);
|
||||
443
pcl_ros/src/pcl_ros/features/feature.cpp
Normal file
443
pcl_ros/src/pcl_ros/features/feature.cpp
Normal file
@ -0,0 +1,443 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2009, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: feature.cpp 35422 2011-01-24 20:04:44Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
//#include <pluginlib/class_list_macros.h>
|
||||
// Include the implementations here instead of compiling them separately to speed up compile time
|
||||
//#include "normal_3d.cpp"
|
||||
//#include "boundary.cpp"
|
||||
//#include "fpfh.cpp"
|
||||
//#include "fpfh_omp.cpp"
|
||||
//#include "moment_invariants.cpp"
|
||||
//#include "normal_3d_omp.cpp"
|
||||
//#include "normal_3d_tbb.cpp"
|
||||
//#include "pfh.cpp"
|
||||
//#include "principal_curvatures.cpp"
|
||||
//#include "vfh.cpp"
|
||||
#include <pcl/io/io.h>
|
||||
#include "pcl_ros/features/feature.h"
|
||||
#include <message_filters/null_types.h>
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::Feature::onInit ()
|
||||
{
|
||||
// Call the super onInit ()
|
||||
PCLNodelet::onInit ();
|
||||
|
||||
// Call the child init
|
||||
childInit (*pnh_);
|
||||
|
||||
// Allow each individual class that inherits from us to declare their own Publisher
|
||||
// This is useful for Publisher<pcl::PointCloud<T> >, as NormalEstimation can publish PointCloud<Normal>, etc
|
||||
//pub_output_ = pnh_->template advertise<PointCloud2> ("output", max_queue_size_);
|
||||
|
||||
// ---[ Mandatory parameters
|
||||
if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("radius_search", search_radius_))
|
||||
{
|
||||
NODELET_ERROR ("[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these parameters before continuing.", getName ().c_str ());
|
||||
return;
|
||||
}
|
||||
if (!pnh_->getParam ("spatial_locator", spatial_locator_type_))
|
||||
{
|
||||
NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ());
|
||||
return;
|
||||
}
|
||||
|
||||
// ---[ Optional parameters
|
||||
pnh_->getParam ("use_surface", use_surface_);
|
||||
|
||||
// Enable the dynamic reconfigure service
|
||||
srv_ = boost::make_shared<dynamic_reconfigure::Server<FeatureConfig> > (*pnh_);
|
||||
dynamic_reconfigure::Server<FeatureConfig>::CallbackType f = boost::bind (&Feature::config_callback, this, _1, _2);
|
||||
srv_->setCallback (f);
|
||||
|
||||
// If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages
|
||||
if (use_indices_ || use_surface_)
|
||||
{
|
||||
// Create the objects here
|
||||
if (approximate_sync_)
|
||||
sync_input_surface_indices_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn, PointCloudIn, PointIndices> > >(max_queue_size_);
|
||||
else
|
||||
sync_input_surface_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, PointCloudIn, PointIndices> > >(max_queue_size_);
|
||||
|
||||
// Subscribe to the input using a filter
|
||||
sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
|
||||
if (use_indices_)
|
||||
{
|
||||
// If indices are enabled, subscribe to the indices
|
||||
sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
|
||||
if (use_surface_) // Use both indices and surface
|
||||
{
|
||||
// If surface is enabled, subscribe to the surface, connect the input-indices-surface trio and register
|
||||
sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_);
|
||||
if (approximate_sync_)
|
||||
sync_input_surface_indices_a_->connectInput (sub_input_filter_, sub_surface_filter_, sub_indices_filter_);
|
||||
else
|
||||
sync_input_surface_indices_e_->connectInput (sub_input_filter_, sub_surface_filter_, sub_indices_filter_);
|
||||
}
|
||||
else // Use only indices
|
||||
{
|
||||
sub_input_filter_.registerCallback (bind (&Feature::input_callback, this, _1));
|
||||
// surface not enabled, connect the input-indices duo and register
|
||||
if (approximate_sync_)
|
||||
sync_input_surface_indices_a_->connectInput (sub_input_filter_, nf_pc_, sub_indices_filter_);
|
||||
else
|
||||
sync_input_surface_indices_e_->connectInput (sub_input_filter_, nf_pc_, sub_indices_filter_);
|
||||
}
|
||||
}
|
||||
else // Use only surface
|
||||
{
|
||||
sub_input_filter_.registerCallback (bind (&Feature::input_callback, this, _1));
|
||||
// indices not enabled, connect the input-surface duo and register
|
||||
sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_);
|
||||
if (approximate_sync_)
|
||||
sync_input_surface_indices_a_->connectInput (sub_input_filter_, sub_surface_filter_, nf_pi_);
|
||||
else
|
||||
sync_input_surface_indices_e_->connectInput (sub_input_filter_, sub_surface_filter_, nf_pi_);
|
||||
}
|
||||
// Register callbacks
|
||||
if (approximate_sync_)
|
||||
sync_input_surface_indices_a_->registerCallback (bind (&Feature::input_surface_indices_callback, this, _1, _2, _3));
|
||||
else
|
||||
sync_input_surface_indices_e_->registerCallback (bind (&Feature::input_surface_indices_callback, this, _1, _2, _3));
|
||||
}
|
||||
else
|
||||
// Subscribe in an old fashion to input only (no filters)
|
||||
sub_input_ = pnh_->subscribe<PointCloudIn> ("input", max_queue_size_, bind (&Feature::input_surface_indices_callback, this, _1, PointCloudInConstPtr (), PointIndicesConstPtr ()));
|
||||
|
||||
NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
|
||||
" - use_surface : %s\n"
|
||||
" - k_search : %d\n"
|
||||
" - radius_search : %f\n"
|
||||
" - spatial_locator: %d",
|
||||
getName ().c_str (),
|
||||
(use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_);
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::Feature::config_callback (FeatureConfig &config, uint32_t level)
|
||||
{
|
||||
if (k_ != config.k_search)
|
||||
{
|
||||
k_ = config.k_search;
|
||||
NODELET_DEBUG ("[config_callback] Setting the number of K nearest neighbors to use for each point: %d.", k_);
|
||||
}
|
||||
if (search_radius_ != config.radius_search)
|
||||
{
|
||||
search_radius_ = config.radius_search;
|
||||
NODELET_DEBUG ("[config_callback] Setting the nearest neighbors search radius for each point: %f.", search_radius_);
|
||||
}
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::Feature::input_surface_indices_callback (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudInConstPtr &cloud_surface, const PointIndicesConstPtr &indices)
|
||||
{
|
||||
// No subscribers, no work
|
||||
if (pub_output_.getNumSubscribers () <= 0)
|
||||
return;
|
||||
|
||||
// If cloud is given, check if it's valid
|
||||
if (!isValid (cloud))
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_surface_indices_callback] Invalid input!", getName ().c_str ());
|
||||
emptyPublish (cloud);
|
||||
return;
|
||||
}
|
||||
|
||||
// If surface is given, check if it's valid
|
||||
if (cloud_surface && !isValid (cloud_surface, "surface"))
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_surface_indices_callback] Invalid input surface!", getName ().c_str ());
|
||||
emptyPublish (cloud);
|
||||
return;
|
||||
}
|
||||
|
||||
// If indices are given, check if they are valid
|
||||
if (indices && !isValid (indices))
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_surface_indices_callback] Invalid input indices!", getName ().c_str ());
|
||||
emptyPublish (cloud);
|
||||
return;
|
||||
}
|
||||
|
||||
/// DEBUG
|
||||
if (cloud_surface)
|
||||
if (indices)
|
||||
NODELET_DEBUG ("[input_surface_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
|
||||
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
|
||||
cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), cloud_surface->header.stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str (),
|
||||
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
|
||||
else
|
||||
NODELET_DEBUG ("[input_surface_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
|
||||
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
|
||||
cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), cloud_surface->header.stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str ());
|
||||
|
||||
else
|
||||
if (indices)
|
||||
NODELET_DEBUG ("[input_surface_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
|
||||
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
|
||||
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
|
||||
else
|
||||
NODELET_DEBUG ("[input_surface_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", cloud->width * cloud->height, cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ());
|
||||
///
|
||||
|
||||
|
||||
if ((int)(cloud->width * cloud->height) < k_)
|
||||
{
|
||||
NODELET_ERROR ("[input_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger than the PointCloud size (%d)!", k_, (int)(cloud->width * cloud->height));
|
||||
emptyPublish (cloud);
|
||||
return;
|
||||
}
|
||||
|
||||
// If indices given...
|
||||
IndicesPtr vindices;
|
||||
if (indices && !indices->header.frame_id.empty ())
|
||||
vindices.reset (new std::vector<int> (indices->indices));
|
||||
|
||||
computePublish (cloud, cloud_surface, vindices);
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::FeatureFromNormals::onInit ()
|
||||
{
|
||||
// Call the super onInit ()
|
||||
PCLNodelet::onInit ();
|
||||
|
||||
// Call the child init
|
||||
childInit (*pnh_);
|
||||
|
||||
// Allow each individual class that inherits from us to declare their own Publisher
|
||||
// This is useful for Publisher<pcl::PointCloud<T> >, as NormalEstimation can publish PointCloud<Normal>, etc
|
||||
//pub_output_ = pnh_->template advertise<PointCloud2> ("output", max_queue_size_);
|
||||
|
||||
// ---[ Mandatory parameters
|
||||
if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("radius_search", search_radius_))
|
||||
{
|
||||
NODELET_ERROR ("[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these parameters before continuing.", getName ().c_str ());
|
||||
return;
|
||||
}
|
||||
if (!pnh_->getParam ("spatial_locator", spatial_locator_type_))
|
||||
{
|
||||
NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ());
|
||||
return;
|
||||
}
|
||||
// ---[ Optional parameters
|
||||
pnh_->getParam ("use_surface", use_surface_);
|
||||
|
||||
sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
|
||||
sub_normals_filter_.subscribe (*pnh_, "normals", max_queue_size_);
|
||||
|
||||
// Enable the dynamic reconfigure service
|
||||
srv_ = boost::make_shared<dynamic_reconfigure::Server<FeatureConfig> > (*pnh_);
|
||||
dynamic_reconfigure::Server<FeatureConfig>::CallbackType f = boost::bind (&FeatureFromNormals::config_callback, this, _1, _2);
|
||||
srv_->setCallback (f);
|
||||
|
||||
// Create the objects here
|
||||
if (approximate_sync_)
|
||||
sync_input_normals_surface_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn, PointCloudN, PointCloudIn, PointIndices> > > (max_queue_size_);
|
||||
else
|
||||
sync_input_normals_surface_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, PointCloudN, PointCloudIn, PointIndices> > > (max_queue_size_);
|
||||
|
||||
// If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages
|
||||
if (use_indices_ || use_surface_)
|
||||
{
|
||||
if (use_indices_)
|
||||
{
|
||||
// If indices are enabled, subscribe to the indices
|
||||
sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
|
||||
if (use_surface_) // Use both indices and surface
|
||||
{
|
||||
// If surface is enabled, subscribe to the surface, connect the input-indices-surface trio and register
|
||||
sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_);
|
||||
if (approximate_sync_)
|
||||
sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, sub_indices_filter_);
|
||||
else
|
||||
sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, sub_indices_filter_);
|
||||
}
|
||||
else // Use only indices
|
||||
{
|
||||
sub_input_filter_.registerCallback (bind (&FeatureFromNormals::input_callback, this, _1));
|
||||
if (approximate_sync_)
|
||||
// surface not enabled, connect the input-indices duo and register
|
||||
sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, sub_indices_filter_);
|
||||
else
|
||||
// surface not enabled, connect the input-indices duo and register
|
||||
sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, sub_indices_filter_);
|
||||
}
|
||||
}
|
||||
else // Use only surface
|
||||
{
|
||||
// indices not enabled, connect the input-surface duo and register
|
||||
sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_);
|
||||
|
||||
sub_input_filter_.registerCallback (bind (&FeatureFromNormals::input_callback, this, _1));
|
||||
if (approximate_sync_)
|
||||
sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, nf_pi_);
|
||||
else
|
||||
sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, nf_pi_);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
sub_input_filter_.registerCallback (bind (&FeatureFromNormals::input_callback, this, _1));
|
||||
|
||||
if (approximate_sync_)
|
||||
sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, nf_pi_);
|
||||
else
|
||||
sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, nf_pi_);
|
||||
}
|
||||
|
||||
// Register callbacks
|
||||
if (approximate_sync_)
|
||||
sync_input_normals_surface_indices_a_->registerCallback (bind (&FeatureFromNormals::input_normals_surface_indices_callback, this, _1, _2, _3, _4));
|
||||
else
|
||||
sync_input_normals_surface_indices_e_->registerCallback (bind (&FeatureFromNormals::input_normals_surface_indices_callback, this, _1, _2, _3, _4));
|
||||
|
||||
NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
|
||||
" - use_surface : %s\n"
|
||||
" - k_search : %d\n"
|
||||
" - radius_search : %f\n"
|
||||
" - spatial_locator: %d",
|
||||
getName ().c_str (),
|
||||
(use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_);
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback (
|
||||
const PointCloudInConstPtr &cloud, const PointCloudNConstPtr &cloud_normals,
|
||||
const PointCloudInConstPtr &cloud_surface, const PointIndicesConstPtr &indices)
|
||||
{
|
||||
// No subscribers, no work
|
||||
if (pub_output_.getNumSubscribers () <= 0)
|
||||
return;
|
||||
|
||||
// If cloud+normals is given, check if it's valid
|
||||
if (!isValid (cloud))// || !isValid (cloud_normals, "normals"))
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input!", getName ().c_str ());
|
||||
emptyPublish (cloud);
|
||||
return;
|
||||
}
|
||||
|
||||
// If surface is given, check if it's valid
|
||||
if (cloud_surface && !isValid (cloud_surface, "surface"))
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input surface!", getName ().c_str ());
|
||||
emptyPublish (cloud);
|
||||
return;
|
||||
}
|
||||
|
||||
// If indices are given, check if they are valid
|
||||
if (indices && !isValid (indices))
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input indices!", getName ().c_str ());
|
||||
emptyPublish (cloud);
|
||||
return;
|
||||
}
|
||||
|
||||
/// DEBUG
|
||||
if (cloud_surface)
|
||||
if (indices)
|
||||
NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
|
||||
getName ().c_str (),
|
||||
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
|
||||
cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), cloud_surface->header.stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str (),
|
||||
cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), cloud_normals->header.stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str (),
|
||||
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
|
||||
else
|
||||
NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
|
||||
getName ().c_str (),
|
||||
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
|
||||
cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), cloud_surface->header.stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str (),
|
||||
cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), cloud_normals->header.stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str ());
|
||||
else
|
||||
if (indices)
|
||||
NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
|
||||
getName ().c_str (),
|
||||
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
|
||||
cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), cloud_normals->header.stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str (),
|
||||
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
|
||||
else
|
||||
NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
|
||||
getName ().c_str (),
|
||||
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
|
||||
cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), cloud_normals->header.stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str ());
|
||||
///
|
||||
|
||||
if ((int)(cloud->width * cloud->height) < k_)
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger than the PointCloud size (%d)!", getName ().c_str (), k_, (int)(cloud->width * cloud->height));
|
||||
emptyPublish (cloud);
|
||||
return;
|
||||
}
|
||||
|
||||
// If indices given...
|
||||
IndicesPtr vindices;
|
||||
if (indices && !indices->header.frame_id.empty ())
|
||||
vindices.reset (new std::vector<int> (indices->indices));
|
||||
|
||||
computePublish (cloud, cloud_normals, cloud_surface, vindices);
|
||||
}
|
||||
|
||||
79
pcl_ros/src/pcl_ros/features/fpfh.cpp
Normal file
79
pcl_ros/src/pcl_ros/features/fpfh.cpp
Normal file
@ -0,0 +1,79 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2009, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: fpfh.cpp 35361 2011-01-20 04:34:49Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/features/fpfh.h"
|
||||
|
||||
void
|
||||
pcl_ros::FPFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
|
||||
{
|
||||
PointCloudOut output;
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
}
|
||||
|
||||
void
|
||||
pcl_ros::FPFHEstimation::computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudNConstPtr &normals,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices)
|
||||
{
|
||||
// Set the parameters
|
||||
impl_.setKSearch (k_);
|
||||
impl_.setRadiusSearch (search_radius_);
|
||||
// Initialize the spatial locator
|
||||
initTree (spatial_locator_type_, tree_, k_);
|
||||
impl_.setSearchMethod (tree_);
|
||||
|
||||
// Set the inputs
|
||||
impl_.setInputCloud (cloud);
|
||||
impl_.setIndices (indices);
|
||||
impl_.setSearchSurface (surface);
|
||||
impl_.setInputNormals (normals);
|
||||
// Estimate the feature
|
||||
PointCloudOut output;
|
||||
impl_.compute (output);
|
||||
|
||||
// Publish a Boost shared ptr const data
|
||||
// Enforce that the TF frame and the timestamp are copied
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
}
|
||||
|
||||
typedef pcl_ros::FPFHEstimation FPFHEstimation;
|
||||
PLUGINLIB_DECLARE_CLASS (pcl, FPFHEstimation, FPFHEstimation, nodelet::Nodelet);
|
||||
|
||||
79
pcl_ros/src/pcl_ros/features/fpfh_omp.cpp
Normal file
79
pcl_ros/src/pcl_ros/features/fpfh_omp.cpp
Normal file
@ -0,0 +1,79 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2009, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: fpfh_omp.cpp 35361 2011-01-20 04:34:49Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/features/fpfh_omp.h"
|
||||
|
||||
void
|
||||
pcl_ros::FPFHEstimationOMP::emptyPublish (const PointCloudInConstPtr &cloud)
|
||||
{
|
||||
PointCloudOut output;
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
}
|
||||
|
||||
void
|
||||
pcl_ros::FPFHEstimationOMP::computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudNConstPtr &normals,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices)
|
||||
{
|
||||
// Set the parameters
|
||||
impl_.setKSearch (k_);
|
||||
impl_.setRadiusSearch (search_radius_);
|
||||
// Initialize the spatial locator
|
||||
initTree (spatial_locator_type_, tree_, k_);
|
||||
impl_.setSearchMethod (tree_);
|
||||
|
||||
// Set the inputs
|
||||
impl_.setInputCloud (cloud);
|
||||
impl_.setIndices (indices);
|
||||
impl_.setSearchSurface (surface);
|
||||
impl_.setInputNormals (normals);
|
||||
// Estimate the feature
|
||||
PointCloudOut output;
|
||||
impl_.compute (output);
|
||||
|
||||
// Publish a Boost shared ptr const data
|
||||
// Enforce that the TF frame and the timestamp are copied
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
}
|
||||
|
||||
typedef pcl_ros::FPFHEstimationOMP FPFHEstimationOMP;
|
||||
PLUGINLIB_DECLARE_CLASS (pcl, FPFHEstimationOMP, FPFHEstimationOMP, nodelet::Nodelet);
|
||||
|
||||
77
pcl_ros/src/pcl_ros/features/moment_invariants.cpp
Normal file
77
pcl_ros/src/pcl_ros/features/moment_invariants.cpp
Normal file
@ -0,0 +1,77 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2009, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: moment_invariants.cpp 35361 2011-01-20 04:34:49Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/features/moment_invariants.h"
|
||||
|
||||
void
|
||||
pcl_ros::MomentInvariantsEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
|
||||
{
|
||||
PointCloudOut output;
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
}
|
||||
|
||||
void
|
||||
pcl_ros::MomentInvariantsEstimation::computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices)
|
||||
{
|
||||
// Set the parameters
|
||||
impl_.setKSearch (k_);
|
||||
impl_.setRadiusSearch (search_radius_);
|
||||
// Initialize the spatial locator
|
||||
initTree (spatial_locator_type_, tree_, k_);
|
||||
impl_.setSearchMethod (tree_);
|
||||
|
||||
// Set the inputs
|
||||
impl_.setInputCloud (cloud);
|
||||
impl_.setIndices (indices);
|
||||
impl_.setSearchSurface (surface);
|
||||
// Estimate the feature
|
||||
PointCloudOut output;
|
||||
impl_.compute (output);
|
||||
|
||||
// Publish a Boost shared ptr const data
|
||||
// Enforce that the TF frame and the timestamp are copied
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
}
|
||||
|
||||
typedef pcl_ros::MomentInvariantsEstimation MomentInvariantsEstimation;
|
||||
PLUGINLIB_DECLARE_CLASS (pcl, MomentInvariantsEstimation, MomentInvariantsEstimation, nodelet::Nodelet);
|
||||
|
||||
77
pcl_ros/src/pcl_ros/features/normal_3d.cpp
Normal file
77
pcl_ros/src/pcl_ros/features/normal_3d.cpp
Normal file
@ -0,0 +1,77 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2009, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: normal_3d.cpp 35361 2011-01-20 04:34:49Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/features/normal_3d.h"
|
||||
|
||||
void
|
||||
pcl_ros::NormalEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
|
||||
{
|
||||
PointCloudOut output;
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
}
|
||||
|
||||
void
|
||||
pcl_ros::NormalEstimation::computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices)
|
||||
{
|
||||
// Set the parameters
|
||||
impl_.setKSearch (k_);
|
||||
impl_.setRadiusSearch (search_radius_);
|
||||
// Initialize the spatial locator
|
||||
initTree (spatial_locator_type_, tree_, k_);
|
||||
impl_.setSearchMethod (tree_);
|
||||
|
||||
// Set the inputs
|
||||
impl_.setInputCloud (cloud);
|
||||
impl_.setIndices (indices);
|
||||
impl_.setSearchSurface (surface);
|
||||
// Estimate the feature
|
||||
PointCloudOut output;
|
||||
impl_.compute (output);
|
||||
|
||||
// Publish a Boost shared ptr const data
|
||||
// Enforce that the TF frame and the timestamp are copied
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
}
|
||||
|
||||
typedef pcl_ros::NormalEstimation NormalEstimation;
|
||||
PLUGINLIB_DECLARE_CLASS (pcl, NormalEstimation, NormalEstimation, nodelet::Nodelet);
|
||||
|
||||
77
pcl_ros/src/pcl_ros/features/normal_3d_omp.cpp
Normal file
77
pcl_ros/src/pcl_ros/features/normal_3d_omp.cpp
Normal file
@ -0,0 +1,77 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2009, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: normal_3d_omp.cpp 35361 2011-01-20 04:34:49Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/features/normal_3d_omp.h"
|
||||
|
||||
void
|
||||
pcl_ros::NormalEstimationOMP::emptyPublish (const PointCloudInConstPtr &cloud)
|
||||
{
|
||||
PointCloudOut output;
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
}
|
||||
|
||||
void
|
||||
pcl_ros::NormalEstimationOMP::computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices)
|
||||
{
|
||||
// Set the parameters
|
||||
impl_.setKSearch (k_);
|
||||
impl_.setRadiusSearch (search_radius_);
|
||||
// Initialize the spatial locator
|
||||
initTree (spatial_locator_type_, tree_, k_);
|
||||
impl_.setSearchMethod (tree_);
|
||||
|
||||
// Set the inputs
|
||||
impl_.setInputCloud (cloud);
|
||||
impl_.setIndices (indices);
|
||||
impl_.setSearchSurface (surface);
|
||||
// Estimate the feature
|
||||
PointCloudOut output;
|
||||
impl_.compute (output);
|
||||
|
||||
// Publish a Boost shared ptr const data
|
||||
// Enforce that the TF frame and the timestamp are copied
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
}
|
||||
|
||||
typedef pcl_ros::NormalEstimationOMP NormalEstimationOMP;
|
||||
PLUGINLIB_DECLARE_CLASS (pcl, NormalEstimationOMP, NormalEstimationOMP, nodelet::Nodelet);
|
||||
|
||||
81
pcl_ros/src/pcl_ros/features/normal_3d_tbb.cpp
Normal file
81
pcl_ros/src/pcl_ros/features/normal_3d_tbb.cpp
Normal file
@ -0,0 +1,81 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2009, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: normal_3d_tbb.cpp 35625 2011-01-31 07:56:13Z gbiggs $
|
||||
*
|
||||
*/
|
||||
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/features/normal_3d_tbb.h"
|
||||
|
||||
#if defined HAVE_TBB
|
||||
|
||||
void
|
||||
pcl_ros::NormalEstimationTBB::emptyPublish (const PointCloudInConstPtr &cloud)
|
||||
{
|
||||
PointCloud output;
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
}
|
||||
|
||||
void
|
||||
pcl_ros::NormalEstimationTBB::computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices)
|
||||
{
|
||||
// Set the parameters
|
||||
impl_.setKSearch (k_);
|
||||
impl_.setRadiusSearch (search_radius_);
|
||||
// Initialize the spatial locator
|
||||
initTree (spatial_locator_type_, tree_, k_);
|
||||
impl_.setSearchMethod (tree_);
|
||||
|
||||
// Set the inputs
|
||||
impl_.setInputCloud (cloud);
|
||||
impl_.setIndices (indices);
|
||||
impl_.setSearchSurface (surface);
|
||||
// Estimate the feature
|
||||
PointCloudOut output;
|
||||
impl_.compute (output);
|
||||
|
||||
// Publish a Boost shared ptr const data
|
||||
// Enforce that the TF frame and the timestamp are copied
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
}
|
||||
|
||||
typedef pcl_ros::NormalEstimationTBB NormalEstimationTBB;
|
||||
PLUGINLIB_DECLARE_CLASS (pcl, NormalEstimationTBB, NormalEstimationTBB, nodelet::Nodelet);
|
||||
|
||||
#endif // HAVE_TBB
|
||||
|
||||
79
pcl_ros/src/pcl_ros/features/pfh.cpp
Normal file
79
pcl_ros/src/pcl_ros/features/pfh.cpp
Normal file
@ -0,0 +1,79 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2009, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: pfh.cpp 35361 2011-01-20 04:34:49Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/features/pfh.h"
|
||||
|
||||
void
|
||||
pcl_ros::PFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
|
||||
{
|
||||
PointCloudOut output;
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
}
|
||||
|
||||
void
|
||||
pcl_ros::PFHEstimation::computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudNConstPtr &normals,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices)
|
||||
{
|
||||
// Set the parameters
|
||||
impl_.setKSearch (k_);
|
||||
impl_.setRadiusSearch (search_radius_);
|
||||
// Initialize the spatial locator
|
||||
initTree (spatial_locator_type_, tree_, k_);
|
||||
impl_.setSearchMethod (tree_);
|
||||
|
||||
// Set the inputs
|
||||
impl_.setInputCloud (cloud);
|
||||
impl_.setIndices (indices);
|
||||
impl_.setSearchSurface (surface);
|
||||
impl_.setInputNormals (normals);
|
||||
// Estimate the feature
|
||||
PointCloudOut output;
|
||||
impl_.compute (output);
|
||||
|
||||
// Publish a Boost shared ptr const data
|
||||
// Enforce that the TF frame and the timestamp are copied
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
}
|
||||
|
||||
typedef pcl_ros::PFHEstimation PFHEstimation;
|
||||
PLUGINLIB_DECLARE_CLASS (pcl, PFHEstimation, PFHEstimation, nodelet::Nodelet);
|
||||
|
||||
79
pcl_ros/src/pcl_ros/features/principal_curvatures.cpp
Normal file
79
pcl_ros/src/pcl_ros/features/principal_curvatures.cpp
Normal file
@ -0,0 +1,79 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2009, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: principal_curvatures.cpp 35361 2011-01-20 04:34:49Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/features/principal_curvatures.h"
|
||||
|
||||
void
|
||||
pcl_ros::PrincipalCurvaturesEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
|
||||
{
|
||||
PointCloudOut output;
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
}
|
||||
|
||||
void
|
||||
pcl_ros::PrincipalCurvaturesEstimation::computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudNConstPtr &normals,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices)
|
||||
{
|
||||
// Set the parameters
|
||||
impl_.setKSearch (k_);
|
||||
impl_.setRadiusSearch (search_radius_);
|
||||
// Initialize the spatial locator
|
||||
initTree (spatial_locator_type_, tree_, k_);
|
||||
impl_.setSearchMethod (tree_);
|
||||
|
||||
// Set the inputs
|
||||
impl_.setInputCloud (cloud);
|
||||
impl_.setIndices (indices);
|
||||
impl_.setSearchSurface (surface);
|
||||
impl_.setInputNormals (normals);
|
||||
// Estimate the feature
|
||||
PointCloudOut output;
|
||||
impl_.compute (output);
|
||||
|
||||
// Publish a Boost shared ptr const data
|
||||
// Enforce that the TF frame and the timestamp are copied
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
}
|
||||
|
||||
typedef pcl_ros::PrincipalCurvaturesEstimation PrincipalCurvaturesEstimation;
|
||||
PLUGINLIB_DECLARE_CLASS (pcl, PrincipalCurvaturesEstimation, PrincipalCurvaturesEstimation, nodelet::Nodelet);
|
||||
|
||||
79
pcl_ros/src/pcl_ros/features/vfh.cpp
Normal file
79
pcl_ros/src/pcl_ros/features/vfh.cpp
Normal file
@ -0,0 +1,79 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2009, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: vfh.cpp 35361 2011-01-20 04:34:49Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/features/vfh.h"
|
||||
|
||||
void
|
||||
pcl_ros::VFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
|
||||
{
|
||||
PointCloudOut output;
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
}
|
||||
|
||||
void
|
||||
pcl_ros::VFHEstimation::computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudNConstPtr &normals,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices)
|
||||
{
|
||||
// Set the parameters
|
||||
impl_.setKSearch (k_);
|
||||
impl_.setRadiusSearch (search_radius_);
|
||||
// Initialize the spatial locator
|
||||
initTree (spatial_locator_type_, tree_, k_);
|
||||
impl_.setSearchMethod (tree_);
|
||||
|
||||
// Set the inputs
|
||||
impl_.setInputCloud (cloud);
|
||||
impl_.setIndices (indices);
|
||||
impl_.setSearchSurface (surface);
|
||||
impl_.setInputNormals (normals);
|
||||
// Estimate the feature
|
||||
PointCloudOut output;
|
||||
impl_.compute (output);
|
||||
|
||||
// Publish a Boost shared ptr const data
|
||||
// Enforce that the TF frame and the timestamp are copied
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
}
|
||||
|
||||
typedef pcl_ros::VFHEstimation VFHEstimation;
|
||||
PLUGINLIB_DECLARE_CLASS (pcl, VFHEstimation, VFHEstimation, nodelet::Nodelet);
|
||||
|
||||
220
pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp
Normal file
220
pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp
Normal file
@ -0,0 +1,220 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2010, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: extract_clusters.hpp 32052 2010-08-27 02:19:30Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include <pcl/io/io.h>
|
||||
#include "pcl_ros/segmentation/extract_clusters.h"
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::EuclideanClusterExtraction::onInit ()
|
||||
{
|
||||
// Call the super onInit ()
|
||||
PCLNodelet::onInit ();
|
||||
|
||||
// ---[ Mandatory parameters
|
||||
double cluster_tolerance;
|
||||
if (!pnh_->getParam ("cluster_tolerance", cluster_tolerance))
|
||||
{
|
||||
NODELET_ERROR ("[%s::onInit] Need a 'cluster_tolerance' parameter to be set before continuing!", getName ().c_str ());
|
||||
return;
|
||||
}
|
||||
int spatial_locator;
|
||||
if (!pnh_->getParam ("spatial_locator", spatial_locator))
|
||||
{
|
||||
NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ());
|
||||
return;
|
||||
}
|
||||
|
||||
//private_nh.getParam ("use_indices", use_indices_);
|
||||
pnh_->getParam ("publish_indices", publish_indices_);
|
||||
|
||||
if (publish_indices_)
|
||||
pub_output_ = pnh_->advertise<PointIndices> ("output", max_queue_size_);
|
||||
else
|
||||
pub_output_ = pnh_->advertise<PointCloud> ("output", max_queue_size_);
|
||||
|
||||
// Enable the dynamic reconfigure service
|
||||
srv_ = boost::make_shared <dynamic_reconfigure::Server<EuclideanClusterExtractionConfig> > (*pnh_);
|
||||
dynamic_reconfigure::Server<EuclideanClusterExtractionConfig>::CallbackType f = boost::bind (&EuclideanClusterExtraction::config_callback, this, _1, _2);
|
||||
srv_->setCallback (f);
|
||||
|
||||
// If we're supposed to look for PointIndices (indices)
|
||||
if (use_indices_)
|
||||
{
|
||||
// Subscribe to the input using a filter
|
||||
sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
|
||||
sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
|
||||
|
||||
if (approximate_sync_)
|
||||
{
|
||||
sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<PointCloud, PointIndices> > > (max_queue_size_);
|
||||
sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_);
|
||||
sync_input_indices_a_->registerCallback (bind (&EuclideanClusterExtraction::input_indices_callback, this, _1, _2));
|
||||
}
|
||||
else
|
||||
{
|
||||
sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ExactTime<PointCloud, PointIndices> > > (max_queue_size_);
|
||||
sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_);
|
||||
sync_input_indices_e_->registerCallback (bind (&EuclideanClusterExtraction::input_indices_callback, this, _1, _2));
|
||||
}
|
||||
}
|
||||
else
|
||||
// Subscribe in an old fashion to input only (no filters)
|
||||
sub_input_ = pnh_->subscribe<PointCloud> ("input", max_queue_size_, bind (&EuclideanClusterExtraction::input_indices_callback, this, _1, PointIndicesConstPtr ()));
|
||||
|
||||
NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
|
||||
" - max_queue_size : %d\n"
|
||||
" - use_indices : %s\n"
|
||||
" - cluster_tolerance : %f\n"
|
||||
" - spatial_locator : %d",
|
||||
getName ().c_str (),
|
||||
max_queue_size_,
|
||||
(use_indices_) ? "true" : "false", cluster_tolerance, spatial_locator);
|
||||
|
||||
// Set given parameters here
|
||||
impl_.setSpatialLocator (spatial_locator);
|
||||
impl_.setClusterTolerance (cluster_tolerance);
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::EuclideanClusterExtraction::config_callback (EuclideanClusterExtractionConfig &config, uint32_t level)
|
||||
{
|
||||
if (impl_.getClusterTolerance () != config.cluster_tolerance)
|
||||
{
|
||||
impl_.setClusterTolerance (config.cluster_tolerance);
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting new clustering tolerance to: %f.", getName ().c_str (), config.cluster_tolerance);
|
||||
}
|
||||
if (impl_.getMinClusterSize () != config.cluster_min_size)
|
||||
{
|
||||
impl_.setMinClusterSize (config.cluster_min_size);
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting the minimum cluster size to: %d.", getName ().c_str (), config.cluster_min_size);
|
||||
}
|
||||
if (impl_.getMaxClusterSize () != config.cluster_max_size)
|
||||
{
|
||||
impl_.setMaxClusterSize (config.cluster_max_size);
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting the maximum cluster size to: %d.", getName ().c_str (), config.cluster_max_size);
|
||||
}
|
||||
if (max_clusters_ != config.max_clusters)
|
||||
{
|
||||
max_clusters_ = config.max_clusters;
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting the maximum number of clusters to extract to: %d.", getName ().c_str (), config.max_clusters);
|
||||
}
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::EuclideanClusterExtraction::input_indices_callback (
|
||||
const PointCloudConstPtr &cloud, const PointIndicesConstPtr &indices)
|
||||
{
|
||||
// No subscribers, no work
|
||||
if (pub_output_.getNumSubscribers () <= 0)
|
||||
return;
|
||||
|
||||
// If cloud is given, check if it's valid
|
||||
if (!isValid (cloud))
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
|
||||
return;
|
||||
}
|
||||
// If indices are given, check if they are valid
|
||||
if (indices && !isValid (indices))
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
|
||||
return;
|
||||
}
|
||||
|
||||
/// DEBUG
|
||||
if (indices)
|
||||
NODELET_DEBUG ("[%s::input_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
|
||||
getName ().c_str (),
|
||||
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
|
||||
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
|
||||
else
|
||||
NODELET_DEBUG ("[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ());
|
||||
///
|
||||
|
||||
IndicesPtr indices_ptr;
|
||||
if (indices)
|
||||
indices_ptr.reset (new std::vector<int> (indices->indices));
|
||||
|
||||
impl_.setInputCloud (cloud);
|
||||
impl_.setIndices (indices_ptr);
|
||||
|
||||
std::vector<PointIndices> clusters;
|
||||
impl_.extract (clusters);
|
||||
|
||||
if (publish_indices_)
|
||||
{
|
||||
for (size_t i = 0; i < clusters.size (); ++i)
|
||||
{
|
||||
if ((int)i >= max_clusters_)
|
||||
break;
|
||||
// TODO: HACK!!! We need to change the PointCloud2 message to add for an incremental sequence ID number.
|
||||
clusters[i].header.stamp += ros::Duration (i * 0.001);
|
||||
pub_output_.publish (boost::make_shared<const PointIndices> (clusters[i]));
|
||||
}
|
||||
|
||||
NODELET_DEBUG ("[segmentAndPublish] Published %zu clusters (PointIndices) on topic %s", clusters.size (), pnh_->resolveName ("output").c_str ());
|
||||
}
|
||||
else
|
||||
{
|
||||
for (size_t i = 0; i < clusters.size (); ++i)
|
||||
{
|
||||
if ((int)i >= max_clusters_)
|
||||
break;
|
||||
PointCloud output;
|
||||
copyPointCloud (*cloud, clusters[i].indices, output);
|
||||
|
||||
//PointCloud output_blob; // Convert from the templated output to the PointCloud blob
|
||||
//pcl::toROSMsg (output, output_blob);
|
||||
// TODO: HACK!!! We need to change the PointCloud2 message to add for an incremental sequence ID number.
|
||||
output.header.stamp += ros::Duration (i * 0.001);
|
||||
// Publish a Boost shared ptr const data
|
||||
pub_output_.publish (output.makeShared ());
|
||||
NODELET_DEBUG ("[segmentAndPublish] Published cluster %zu (with %zu values and stamp %f) on topic %s",
|
||||
i, clusters[i].indices.size (), output.header.stamp.toSec (), pnh_->resolveName ("output").c_str ());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
typedef pcl_ros::EuclideanClusterExtraction EuclideanClusterExtraction;
|
||||
PLUGINLIB_DECLARE_CLASS (pcl, EuclideanClusterExtraction, EuclideanClusterExtraction, nodelet::Nodelet);
|
||||
|
||||
@ -0,0 +1,193 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2010, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: extract_polygonal_prism_data.hpp 32996 2010-09-30 23:42:11Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/transforms.h"
|
||||
#include "pcl_ros/segmentation/extract_polygonal_prism_data.h"
|
||||
#include <pcl/io/io.h>
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::ExtractPolygonalPrismData::onInit ()
|
||||
{
|
||||
// Call the super onInit ()
|
||||
PCLNodelet::onInit ();
|
||||
|
||||
sub_hull_filter_.subscribe (*pnh_, "planar_hull", max_queue_size_);
|
||||
sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
|
||||
|
||||
// Enable the dynamic reconfigure service
|
||||
srv_ = boost::make_shared <dynamic_reconfigure::Server<ExtractPolygonalPrismDataConfig> > (*pnh_);
|
||||
dynamic_reconfigure::Server<ExtractPolygonalPrismDataConfig>::CallbackType f = boost::bind (&ExtractPolygonalPrismData::config_callback, this, _1, _2);
|
||||
srv_->setCallback (f);
|
||||
|
||||
// Create the objects here
|
||||
if (approximate_sync_)
|
||||
sync_input_hull_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointCloud, PointIndices> > > (max_queue_size_);
|
||||
else
|
||||
sync_input_hull_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloud, PointIndices> > > (max_queue_size_);
|
||||
|
||||
if (use_indices_)
|
||||
{
|
||||
sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
|
||||
if (approximate_sync_)
|
||||
sync_input_hull_indices_a_->connectInput (sub_input_filter_, sub_hull_filter_, sub_indices_filter_);
|
||||
else
|
||||
sync_input_hull_indices_e_->connectInput (sub_input_filter_, sub_hull_filter_, sub_indices_filter_);
|
||||
}
|
||||
else
|
||||
{
|
||||
sub_input_filter_.registerCallback (bind (&ExtractPolygonalPrismData::input_callback, this, _1));
|
||||
|
||||
if (approximate_sync_)
|
||||
sync_input_hull_indices_a_->connectInput (sub_input_filter_, sub_hull_filter_, nf_);
|
||||
else
|
||||
sync_input_hull_indices_e_->connectInput (sub_input_filter_, sub_hull_filter_, nf_);
|
||||
}
|
||||
// Register callbacks
|
||||
if (approximate_sync_)
|
||||
sync_input_hull_indices_a_->registerCallback (bind (&ExtractPolygonalPrismData::input_hull_indices_callback, this, _1, _2, _3));
|
||||
else
|
||||
sync_input_hull_indices_e_->registerCallback (bind (&ExtractPolygonalPrismData::input_hull_indices_callback, this, _1, _2, _3));
|
||||
|
||||
// Advertise the output topics
|
||||
pub_output_ = pnh_->advertise<PointIndices> ("output", max_queue_size_);
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::ExtractPolygonalPrismData::config_callback (ExtractPolygonalPrismDataConfig &config, uint32_t level)
|
||||
{
|
||||
double height_min, height_max;
|
||||
impl_.getHeightLimits (height_min, height_max);
|
||||
if (height_min != config.height_min)
|
||||
{
|
||||
height_min = config.height_min;
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting new minimum height to the planar model to: %f.", getName ().c_str (), height_min);
|
||||
impl_.setHeightLimits (height_min, height_max);
|
||||
}
|
||||
if (height_max != config.height_max)
|
||||
{
|
||||
height_max = config.height_max;
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting new maximum height to the planar model to: %f.", getName ().c_str (), height_max);
|
||||
impl_.setHeightLimits (height_min, height_max);
|
||||
}
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback (
|
||||
const PointCloudConstPtr &cloud,
|
||||
const PointCloudConstPtr &hull,
|
||||
const PointIndicesConstPtr &indices)
|
||||
{
|
||||
// No subscribers, no work
|
||||
if (pub_output_.getNumSubscribers () <= 0)
|
||||
return;
|
||||
|
||||
// Copy the header (stamp + frame_id)
|
||||
pcl::PointIndices inliers;
|
||||
inliers.header = cloud->header;
|
||||
|
||||
// If cloud is given, check if it's valid
|
||||
if (!isValid (cloud) || !isValid (hull, "planar_hull"))
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_hull_indices_callback] Invalid input!", getName ().c_str ());
|
||||
pub_output_.publish (boost::make_shared<const pcl::PointIndices> (inliers));
|
||||
return;
|
||||
}
|
||||
// If indices are given, check if they are valid
|
||||
if (indices && !isValid (indices))
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_hull_indices_callback] Invalid indices!", getName ().c_str ());
|
||||
pub_output_.publish (boost::make_shared<const pcl::PointIndices> (inliers));
|
||||
return;
|
||||
}
|
||||
|
||||
/// DEBUG
|
||||
if (indices)
|
||||
NODELET_DEBUG ("[%s::input_indices_hull_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
|
||||
getName ().c_str (),
|
||||
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
|
||||
hull->width * hull->height, pcl::getFieldsList (*hull).c_str (), hull->header.stamp.toSec (), hull->header.frame_id.c_str (), pnh_->resolveName ("planar_hull").c_str (),
|
||||
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
|
||||
else
|
||||
NODELET_DEBUG ("[%s::input_indices_hull_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
|
||||
getName ().c_str (),
|
||||
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
|
||||
hull->width * hull->height, pcl::getFieldsList (*hull).c_str (), hull->header.stamp.toSec (), hull->header.frame_id.c_str (), pnh_->resolveName ("planar_hull").c_str ());
|
||||
///
|
||||
|
||||
if (cloud->header.frame_id != hull->header.frame_id)
|
||||
{
|
||||
NODELET_DEBUG ("[%s::input_hull_callback] Planar hull has a different TF frame (%s) than the input point cloud (%s)! Using TF to transform.", getName ().c_str (), hull->header.frame_id.c_str (), cloud->header.frame_id.c_str ());
|
||||
PointCloud planar_hull;
|
||||
if (!pcl_ros::transformPointCloud (cloud->header.frame_id, *hull, planar_hull, tf_listener_))
|
||||
{
|
||||
// Publish empty before return
|
||||
pub_output_.publish (boost::make_shared<const pcl::PointIndices> (inliers));
|
||||
return;
|
||||
}
|
||||
impl_.setInputPlanarHull (planar_hull.makeShared ());
|
||||
}
|
||||
else
|
||||
impl_.setInputPlanarHull (hull);
|
||||
|
||||
IndicesPtr indices_ptr;
|
||||
if (indices && !indices->header.frame_id.empty ())
|
||||
indices_ptr.reset (new std::vector<int> (indices->indices));
|
||||
|
||||
impl_.setInputCloud (cloud);
|
||||
impl_.setIndices (indices_ptr);
|
||||
|
||||
// Final check if the data is empty (remember that indices are set to the size of the data -- if indices* = NULL)
|
||||
if (!cloud->points.empty ())
|
||||
impl_.segment (inliers);
|
||||
// Enforce that the TF frame and the timestamp are copied
|
||||
inliers.header = cloud->header;
|
||||
pub_output_.publish (boost::make_shared<const PointIndices> (inliers));
|
||||
NODELET_DEBUG ("[%s::input_hull_callback] Publishing %zu indices.", getName ().c_str (), inliers.indices.size ());
|
||||
}
|
||||
|
||||
typedef pcl_ros::ExtractPolygonalPrismData ExtractPolygonalPrismData;
|
||||
PLUGINLIB_DECLARE_CLASS (pcl, ExtractPolygonalPrismData, ExtractPolygonalPrismData, nodelet::Nodelet);
|
||||
|
||||
645
pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp
Normal file
645
pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp
Normal file
@ -0,0 +1,645 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2009, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: sac_segmentation.hpp 33195 2010-10-10 14:12:19Z marton $
|
||||
*
|
||||
*/
|
||||
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/segmentation/sac_segmentation.h"
|
||||
#include <pcl/io/io.h>
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::SACSegmentation::onInit ()
|
||||
{
|
||||
// Call the super onInit ()
|
||||
PCLNodelet::onInit ();
|
||||
|
||||
// If we're supposed to look for PointIndices (indices)
|
||||
if (use_indices_)
|
||||
{
|
||||
// Subscribe to the input using a filter
|
||||
sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
|
||||
sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
|
||||
|
||||
// when "use_indices" is set to true, and "latched_indices" is set to true,
|
||||
// we'll subscribe and get a separate callback for PointIndices that will
|
||||
// save the indices internally, and a PointCloud + PointIndices callback
|
||||
// will take care of meshing the new PointClouds with the old saved indices.
|
||||
if (latched_indices_)
|
||||
{
|
||||
// Subscribe to a callback that saves the indices
|
||||
sub_indices_filter_.registerCallback (bind (&SACSegmentation::indices_callback, this, _1));
|
||||
// Subscribe to a callback that sets the header of the saved indices to the cloud header
|
||||
sub_input_filter_.registerCallback (bind (&SACSegmentation::input_callback, this, _1));
|
||||
|
||||
// Synchronize the two topics. No need for an approximate synchronizer here, as we'll
|
||||
// match the timestamps exactly
|
||||
sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointIndices> > > (max_queue_size_);
|
||||
sync_input_indices_e_->connectInput (sub_input_filter_, nf_pi_);
|
||||
sync_input_indices_e_->registerCallback (bind (&SACSegmentation::input_indices_callback, this, _1, _2));
|
||||
}
|
||||
// "latched_indices" not set, proceed with regular <input,indices> pairs
|
||||
else
|
||||
{
|
||||
if (approximate_sync_)
|
||||
{
|
||||
sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointIndices> > > (max_queue_size_);
|
||||
sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_);
|
||||
sync_input_indices_a_->registerCallback (bind (&SACSegmentation::input_indices_callback, this, _1, _2));
|
||||
}
|
||||
else
|
||||
{
|
||||
sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointIndices> > > (max_queue_size_);
|
||||
sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_);
|
||||
sync_input_indices_e_->registerCallback (bind (&SACSegmentation::input_indices_callback, this, _1, _2));
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
// Subscribe in an old fashion to input only (no filters)
|
||||
sub_input_ = pnh_->subscribe<PointCloud> ("input", max_queue_size_, bind (&SACSegmentation::input_indices_callback, this, _1, PointIndicesConstPtr ()));
|
||||
|
||||
// Advertise the output topics
|
||||
pub_indices_ = pnh_->advertise<PointIndices> ("inliers", max_queue_size_);
|
||||
pub_model_ = pnh_->advertise<ModelCoefficients> ("model", max_queue_size_);
|
||||
|
||||
// ---[ Mandatory parameters
|
||||
int model_type;
|
||||
if (!pnh_->getParam ("model_type", model_type))
|
||||
{
|
||||
NODELET_ERROR ("[onInit] Need a 'model_type' parameter to be set before continuing!");
|
||||
return;
|
||||
}
|
||||
double threshold; // unused - set via dynamic reconfigure in the callback
|
||||
if (!pnh_->getParam ("distance_threshold", threshold))
|
||||
{
|
||||
NODELET_ERROR ("[onInit] Need a 'distance_threshold' parameter to be set before continuing!");
|
||||
return;
|
||||
}
|
||||
|
||||
// ---[ Optional parameters
|
||||
int method_type = 0;
|
||||
pnh_->getParam ("method_type", method_type);
|
||||
|
||||
XmlRpc::XmlRpcValue axis_param;
|
||||
pnh_->getParam ("axis", axis_param);
|
||||
Eigen::Vector3f axis = Eigen::Vector3f::Zero ();
|
||||
|
||||
switch (axis_param.getType ())
|
||||
{
|
||||
case XmlRpc::XmlRpcValue::TypeArray:
|
||||
{
|
||||
if (axis_param.size () != 3)
|
||||
{
|
||||
NODELET_ERROR ("[%s::onInit] Parameter 'axis' given but with a different number of values (%d) than required (3)!", getName ().c_str (), axis_param.size ());
|
||||
return;
|
||||
}
|
||||
for (int i = 0; i < 3; ++i)
|
||||
{
|
||||
if (axis_param[i].getType () != XmlRpc::XmlRpcValue::TypeDouble)
|
||||
{
|
||||
NODELET_ERROR ("[%s::onInit] Need floating point values for 'axis' parameter.", getName ().c_str ());
|
||||
return;
|
||||
}
|
||||
double value = axis_param[i]; axis[i] = value;
|
||||
}
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// Initialize the random number generator
|
||||
srand (time (0));
|
||||
|
||||
// Enable the dynamic reconfigure service
|
||||
srv_ = boost::make_shared<dynamic_reconfigure::Server<SACSegmentationConfig> >(*pnh_);
|
||||
dynamic_reconfigure::Server<SACSegmentationConfig>::CallbackType f = boost::bind (&SACSegmentation::config_callback, this, _1, _2);
|
||||
srv_->setCallback (f);
|
||||
|
||||
NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
|
||||
" - model_type : %d\n"
|
||||
" - method_type : %d\n"
|
||||
" - model_threshold : %f\n"
|
||||
" - axis : [%f, %f, %f]\n",
|
||||
getName ().c_str (), model_type, method_type, threshold,
|
||||
axis[0], axis[1], axis[2]);
|
||||
|
||||
// Set given parameters here
|
||||
impl_.setModelType (model_type);
|
||||
impl_.setMethodType (method_type);
|
||||
impl_.setAxis (axis);
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::SACSegmentation::config_callback (SACSegmentationConfig &config, uint32_t level)
|
||||
{
|
||||
boost::mutex::scoped_lock lock (mutex_);
|
||||
|
||||
if (impl_.getDistanceThreshold () != config.distance_threshold)
|
||||
{
|
||||
//sac_->setDistanceThreshold (threshold_); - done in initSAC
|
||||
impl_.setDistanceThreshold (config.distance_threshold);
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting new distance to model threshold to: %f.", getName ().c_str (), config.distance_threshold);
|
||||
}
|
||||
// The maximum allowed difference between the model normal and the given axis _in radians_
|
||||
if (impl_.getEpsAngle () != config.eps_angle)
|
||||
{
|
||||
impl_.setEpsAngle (config.eps_angle);
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting new epsilon angle to model threshold to: %f (%f degrees).", getName ().c_str (), config.eps_angle, config.eps_angle * 180.0 / M_PI);
|
||||
}
|
||||
|
||||
// Number of inliers
|
||||
if (min_inliers_ != config.min_inliers)
|
||||
{
|
||||
min_inliers_ = config.min_inliers;
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting new minimum number of inliers to: %d.", getName ().c_str (), min_inliers_);
|
||||
}
|
||||
|
||||
if (impl_.getMaxIterations () != config.max_iterations)
|
||||
{
|
||||
//sac_->setMaxIterations (max_iterations_); - done in initSAC
|
||||
impl_.setMaxIterations (config.max_iterations);
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting new maximum number of iterations to: %d.", getName ().c_str (), config.max_iterations);
|
||||
}
|
||||
if (impl_.getProbability () != config.probability)
|
||||
{
|
||||
//sac_->setProbability (probability_); - done in initSAC
|
||||
impl_.setProbability (config.probability);
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting new probability to: %f.", getName ().c_str (), config.probability);
|
||||
}
|
||||
if (impl_.getOptimizeCoefficients () != config.optimize_coefficients)
|
||||
{
|
||||
impl_.setOptimizeCoefficients (config.optimize_coefficients);
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting coefficient optimization to: %s.", getName ().c_str (), (config.optimize_coefficients) ? "true" : "false");
|
||||
}
|
||||
|
||||
double radius_min, radius_max;
|
||||
impl_.getRadiusLimits (radius_min, radius_max);
|
||||
if (radius_min != config.radius_min)
|
||||
{
|
||||
radius_min = config.radius_min;
|
||||
NODELET_DEBUG ("[config_callback] Setting minimum allowable model radius to: %f.", radius_min);
|
||||
impl_.setRadiusLimits (radius_min, radius_max);
|
||||
}
|
||||
if (radius_max != config.radius_max)
|
||||
{
|
||||
radius_max = config.radius_max;
|
||||
NODELET_DEBUG ("[config_callback] Setting maximum allowable model radius to: %f.", radius_max);
|
||||
impl_.setRadiusLimits (radius_min, radius_max);
|
||||
}
|
||||
|
||||
if (tf_input_frame_ != config.input_frame)
|
||||
{
|
||||
tf_input_frame_ = config.input_frame;
|
||||
NODELET_DEBUG ("[config_callback] Setting the input TF frame to: %s.", tf_input_frame_.c_str ());
|
||||
NODELET_WARN ("input_frame TF not implemented yet!");
|
||||
}
|
||||
if (tf_output_frame_ != config.output_frame)
|
||||
{
|
||||
tf_output_frame_ = config.output_frame;
|
||||
NODELET_DEBUG ("[config_callback] Setting the output TF frame to: %s.", tf_output_frame_.c_str ());
|
||||
NODELET_WARN ("output_frame TF not implemented yet!");
|
||||
}
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::SACSegmentation::input_indices_callback (const PointCloudConstPtr &cloud,
|
||||
const PointIndicesConstPtr &indices)
|
||||
{
|
||||
boost::mutex::scoped_lock lock (mutex_);
|
||||
|
||||
// No subscribers, no work
|
||||
if (pub_indices_.getNumSubscribers () <= 0 && pub_model_.getNumSubscribers () <= 0)
|
||||
return;
|
||||
|
||||
PointIndices inliers;
|
||||
ModelCoefficients model;
|
||||
// Enforce that the TF frame and the timestamp are copied
|
||||
inliers.header = model.header = cloud->header;
|
||||
|
||||
// If cloud is given, check if it's valid
|
||||
if (!isValid (cloud))
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
|
||||
pub_indices_.publish (boost::make_shared<const PointIndices> (inliers));
|
||||
pub_model_.publish (boost::make_shared<const ModelCoefficients> (model));
|
||||
return;
|
||||
}
|
||||
// If indices are given, check if they are valid
|
||||
if (indices && !isValid (indices))
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
|
||||
pub_indices_.publish (boost::make_shared<const PointIndices> (inliers));
|
||||
pub_model_.publish (boost::make_shared<const ModelCoefficients> (model));
|
||||
return;
|
||||
}
|
||||
|
||||
/// DEBUG
|
||||
if (indices && !indices->header.frame_id.empty ())
|
||||
NODELET_DEBUG ("[%s::input_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
|
||||
getName ().c_str (),
|
||||
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
|
||||
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
|
||||
else
|
||||
NODELET_DEBUG ("[%s::input_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.",
|
||||
getName ().c_str (), cloud->width * cloud->height, cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ());
|
||||
///
|
||||
|
||||
// Check whether the user has given a different input TF frame
|
||||
tf_input_orig_frame_ = cloud->header.frame_id;
|
||||
PointCloudConstPtr cloud_tf;
|
||||
/* if (!tf_input_frame_.empty () && cloud->header.frame_id != tf_input_frame_)
|
||||
{
|
||||
NODELET_DEBUG ("[input_callback] Transforming input dataset from %s to %s.", cloud->header.frame_id.c_str (), tf_input_frame_.c_str ());
|
||||
// Save the original frame ID
|
||||
// Convert the cloud into the different frame
|
||||
PointCloud cloud_transformed;
|
||||
if (!pcl::transformPointCloud (tf_input_frame_, cloud->header.stamp, *cloud, cloud_transformed, tf_listener_))
|
||||
return;
|
||||
cloud_tf.reset (new PointCloud (cloud_transformed));
|
||||
}
|
||||
else*/
|
||||
cloud_tf = cloud;
|
||||
|
||||
IndicesPtr indices_ptr;
|
||||
if (indices && !indices->header.frame_id.empty ())
|
||||
indices_ptr.reset (new std::vector<int> (indices->indices));
|
||||
|
||||
impl_.setInputCloud (cloud_tf);
|
||||
impl_.setIndices (indices_ptr);
|
||||
|
||||
// Final check if the data is empty (remember that indices are set to the size of the data -- if indices* = NULL)
|
||||
if (!cloud->points.empty ())
|
||||
impl_.segment (inliers, model);
|
||||
|
||||
// Probably need to transform the model of the plane here
|
||||
|
||||
// Check if we have enough inliers, clear inliers + model if not
|
||||
if ((int)inliers.indices.size () <= min_inliers_)
|
||||
{
|
||||
inliers.indices.clear ();
|
||||
model.values.clear ();
|
||||
}
|
||||
|
||||
// Publish
|
||||
pub_indices_.publish (boost::make_shared<const PointIndices> (inliers));
|
||||
pub_model_.publish (boost::make_shared<const ModelCoefficients> (model));
|
||||
NODELET_DEBUG ("[%s::input_indices_callback] Published PointIndices with %zu values on topic %s, and ModelCoefficients with %zu values on topic %s",
|
||||
getName ().c_str (), inliers.indices.size (), pnh_->resolveName ("inliers").c_str (),
|
||||
model.values.size (), pnh_->resolveName ("model").c_str ());
|
||||
|
||||
if (inliers.indices.empty ())
|
||||
NODELET_WARN ("[%s::input_indices_callback] No inliers found!", getName ().c_str ());
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::SACSegmentationFromNormals::onInit ()
|
||||
{
|
||||
// Call the super onInit ()
|
||||
PCLNodelet::onInit ();
|
||||
|
||||
// Enable the dynamic reconfigure service
|
||||
srv_ = boost::make_shared <dynamic_reconfigure::Server<SACSegmentationFromNormalsConfig> > (*pnh_);
|
||||
dynamic_reconfigure::Server<SACSegmentationFromNormalsConfig>::CallbackType f = boost::bind (&SACSegmentationFromNormals::config_callback, this, _1, _2);
|
||||
srv_->setCallback (f);
|
||||
|
||||
// Subscribe to the input and normals using filters
|
||||
sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
|
||||
sub_normals_filter_.subscribe (*pnh_, "normals", max_queue_size_);
|
||||
|
||||
// Subscribe to an axis direction along which the model search is to be constrained (the first 3 model coefficients will be checked)
|
||||
sub_axis_ = pnh_->subscribe ("axis", 1, &SACSegmentationFromNormals::axis_callback, this);
|
||||
|
||||
if (approximate_sync_)
|
||||
sync_input_normals_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointCloudN, PointIndices> > > (max_queue_size_);
|
||||
else
|
||||
sync_input_normals_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloudN, PointIndices> > > (max_queue_size_);
|
||||
|
||||
// If we're supposed to look for PointIndices (indices)
|
||||
if (use_indices_)
|
||||
{
|
||||
// Subscribe to the input using a filter
|
||||
sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
|
||||
|
||||
if (approximate_sync_)
|
||||
sync_input_normals_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, sub_indices_filter_);
|
||||
else
|
||||
sync_input_normals_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, sub_indices_filter_);
|
||||
}
|
||||
else
|
||||
{
|
||||
// Create a different callback for copying over the timestamp to fake indices
|
||||
sub_input_filter_.registerCallback (bind (&SACSegmentationFromNormals::input_callback, this, _1));
|
||||
|
||||
if (approximate_sync_)
|
||||
sync_input_normals_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, nf_);
|
||||
else
|
||||
sync_input_normals_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, nf_);
|
||||
}
|
||||
|
||||
if (approximate_sync_)
|
||||
sync_input_normals_indices_a_->registerCallback (bind (&SACSegmentationFromNormals::input_normals_indices_callback, this, _1, _2, _3));
|
||||
else
|
||||
sync_input_normals_indices_e_->registerCallback (bind (&SACSegmentationFromNormals::input_normals_indices_callback, this, _1, _2, _3));
|
||||
|
||||
// Advertise the output topics
|
||||
pub_indices_ = pnh_->advertise<PointIndices> ("inliers", max_queue_size_);
|
||||
pub_model_ = pnh_->advertise<ModelCoefficients> ("model", max_queue_size_);
|
||||
|
||||
// ---[ Mandatory parameters
|
||||
int model_type;
|
||||
if (!pnh_->getParam ("model_type", model_type))
|
||||
{
|
||||
NODELET_ERROR ("[%s::onInit] Need a 'model_type' parameter to be set before continuing!", getName ().c_str ());
|
||||
return;
|
||||
}
|
||||
double threshold; // unused - set via dynamic reconfigure in the callback
|
||||
if (!pnh_->getParam ("distance_threshold", threshold))
|
||||
{
|
||||
NODELET_ERROR ("[%s::onInit] Need a 'distance_threshold' parameter to be set before continuing!", getName ().c_str ());
|
||||
return;
|
||||
}
|
||||
|
||||
// ---[ Optional parameters
|
||||
int method_type = 0;
|
||||
pnh_->getParam ("method_type", method_type);
|
||||
|
||||
XmlRpc::XmlRpcValue axis_param;
|
||||
pnh_->getParam ("axis", axis_param);
|
||||
Eigen::Vector3f axis = Eigen::Vector3f::Zero ();
|
||||
|
||||
switch (axis_param.getType ())
|
||||
{
|
||||
case XmlRpc::XmlRpcValue::TypeArray:
|
||||
{
|
||||
if (axis_param.size () != 3)
|
||||
{
|
||||
NODELET_ERROR ("[%s::onInit] Parameter 'axis' given but with a different number of values (%d) than required (3)!", getName ().c_str (), axis_param.size ());
|
||||
return;
|
||||
}
|
||||
for (int i = 0; i < 3; ++i)
|
||||
{
|
||||
if (axis_param[i].getType () != XmlRpc::XmlRpcValue::TypeDouble)
|
||||
{
|
||||
NODELET_ERROR ("[%s::onInit] Need floating point values for 'axis' parameter.", getName ().c_str ());
|
||||
return;
|
||||
}
|
||||
double value = axis_param[i]; axis[i] = value;
|
||||
}
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// Initialize the random number generator
|
||||
srand (time (0));
|
||||
|
||||
NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
|
||||
" - model_type : %d\n"
|
||||
" - method_type : %d\n"
|
||||
" - model_threshold : %f\n"
|
||||
" - axis : [%f, %f, %f]\n",
|
||||
getName ().c_str (), model_type, method_type, threshold,
|
||||
axis[0], axis[1], axis[2]);
|
||||
|
||||
// Set given parameters here
|
||||
impl_.setModelType (model_type);
|
||||
impl_.setMethodType (method_type);
|
||||
impl_.setAxis (axis);
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::SACSegmentationFromNormals::axis_callback (const pcl::ModelCoefficientsConstPtr &model)
|
||||
{
|
||||
boost::mutex::scoped_lock lock (mutex_);
|
||||
|
||||
if (model->values.size () < 3)
|
||||
{
|
||||
NODELET_ERROR ("[%s::axis_callback] Invalid axis direction / model coefficients with %zu values sent on %s!", getName ().c_str (), model->values.size (), pnh_->resolveName ("axis").c_str ());
|
||||
return;
|
||||
}
|
||||
NODELET_DEBUG ("[%s::axis_callback] Received axis direction: %f %f %f", getName ().c_str (), model->values[0], model->values[1], model->values[2]);
|
||||
|
||||
Eigen::Vector3f axis (model->values[0], model->values[1], model->values[2]);
|
||||
impl_.setAxis (axis);
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::SACSegmentationFromNormals::config_callback (SACSegmentationFromNormalsConfig &config, uint32_t level)
|
||||
{
|
||||
boost::mutex::scoped_lock lock (mutex_);
|
||||
|
||||
if (impl_.getDistanceThreshold () != config.distance_threshold)
|
||||
{
|
||||
impl_.setDistanceThreshold (config.distance_threshold);
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting distance to model threshold to: %f.", getName ().c_str (), config.distance_threshold);
|
||||
}
|
||||
// The maximum allowed difference between the model normal and the given axis _in radians_
|
||||
if (impl_.getEpsAngle () != config.eps_angle)
|
||||
{
|
||||
impl_.setEpsAngle (config.eps_angle);
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting new epsilon angle to model threshold to: %f (%f degrees).", getName ().c_str (), config.eps_angle, config.eps_angle * 180.0 / M_PI);
|
||||
}
|
||||
|
||||
if (impl_.getMaxIterations () != config.max_iterations)
|
||||
{
|
||||
impl_.setMaxIterations (config.max_iterations);
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting new maximum number of iterations to: %d.", getName ().c_str (), config.max_iterations);
|
||||
}
|
||||
|
||||
// Number of inliers
|
||||
if (min_inliers_ != config.min_inliers)
|
||||
{
|
||||
min_inliers_ = config.min_inliers;
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting new minimum number of inliers to: %d.", getName ().c_str (), min_inliers_);
|
||||
}
|
||||
|
||||
|
||||
if (impl_.getProbability () != config.probability)
|
||||
{
|
||||
impl_.setProbability (config.probability);
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting new probability to: %f.", getName ().c_str (), config.probability);
|
||||
}
|
||||
|
||||
if (impl_.getOptimizeCoefficients () != config.optimize_coefficients)
|
||||
{
|
||||
impl_.setOptimizeCoefficients (config.optimize_coefficients);
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting coefficient optimization to: %s.", getName ().c_str (), (config.optimize_coefficients) ? "true" : "false");
|
||||
}
|
||||
|
||||
if (impl_.getNormalDistanceWeight () != config.normal_distance_weight)
|
||||
{
|
||||
impl_.setNormalDistanceWeight (config.normal_distance_weight);
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting new distance weight to: %f.", getName ().c_str (), config.normal_distance_weight);
|
||||
}
|
||||
|
||||
double radius_min, radius_max;
|
||||
impl_.getRadiusLimits (radius_min, radius_max);
|
||||
if (radius_min != config.radius_min)
|
||||
{
|
||||
radius_min = config.radius_min;
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting minimum allowable model radius to: %f.", getName ().c_str (), radius_min);
|
||||
impl_.setRadiusLimits (radius_min, radius_max);
|
||||
}
|
||||
if (radius_max != config.radius_max)
|
||||
{
|
||||
radius_max = config.radius_max;
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting maximum allowable model radius to: %f.", getName ().c_str (), radius_max);
|
||||
impl_.setRadiusLimits (radius_min, radius_max);
|
||||
}
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback (
|
||||
const PointCloudConstPtr &cloud,
|
||||
const PointCloudNConstPtr &cloud_normals,
|
||||
const PointIndicesConstPtr &indices
|
||||
)
|
||||
{
|
||||
boost::mutex::scoped_lock lock (mutex_);
|
||||
|
||||
// No subscribers, no work
|
||||
if (pub_indices_.getNumSubscribers () <= 0 && pub_model_.getNumSubscribers () <= 0)
|
||||
return;
|
||||
|
||||
PointIndices inliers;
|
||||
ModelCoefficients model;
|
||||
// Enforce that the TF frame and the timestamp are copied
|
||||
inliers.header = model.header = cloud->header;
|
||||
|
||||
if (impl_.getModelType () < 0)
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_normals_indices_callback] Model type not set!", getName ().c_str ());
|
||||
pub_indices_.publish (boost::make_shared<const PointIndices> (inliers));
|
||||
pub_model_.publish (boost::make_shared<const ModelCoefficients> (model));
|
||||
return;
|
||||
}
|
||||
|
||||
if (!isValid (cloud))// || !isValid (cloud_normals, "normals"))
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_normals_indices_callback] Invalid input!", getName ().c_str ());
|
||||
pub_indices_.publish (boost::make_shared<const PointIndices> (inliers));
|
||||
pub_model_.publish (boost::make_shared<const ModelCoefficients> (model));
|
||||
return;
|
||||
}
|
||||
// If indices are given, check if they are valid
|
||||
if (indices && !isValid (indices))
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_normals_indices_callback] Invalid indices!", getName ().c_str ());
|
||||
pub_indices_.publish (boost::make_shared<const PointIndices> (inliers));
|
||||
pub_model_.publish (boost::make_shared<const ModelCoefficients> (model));
|
||||
return;
|
||||
}
|
||||
|
||||
/// DEBUG
|
||||
if (indices && !indices->header.frame_id.empty ())
|
||||
NODELET_DEBUG ("[%s::input_normals_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
|
||||
getName ().c_str (),
|
||||
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
|
||||
cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), cloud_normals->header.stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str (),
|
||||
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
|
||||
else
|
||||
NODELET_DEBUG ("[%s::input_normals_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
|
||||
getName ().c_str (),
|
||||
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
|
||||
cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), cloud_normals->header.stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str ());
|
||||
///
|
||||
|
||||
|
||||
// Extra checks for safety
|
||||
int cloud_nr_points = cloud->width * cloud->height;
|
||||
int cloud_normals_nr_points = cloud_normals->width * cloud_normals->height;
|
||||
if (cloud_nr_points != cloud_normals_nr_points)
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_normals_indices_callback] Number of points in the input dataset (%d) differs from the number of points in the normals (%d)!", getName ().c_str (), cloud_nr_points, cloud_normals_nr_points);
|
||||
pub_indices_.publish (boost::make_shared<const PointIndices> (inliers));
|
||||
pub_model_.publish (boost::make_shared<const ModelCoefficients> (model));
|
||||
return;
|
||||
}
|
||||
|
||||
impl_.setInputCloud (cloud);
|
||||
impl_.setInputNormals (cloud_normals);
|
||||
|
||||
IndicesPtr indices_ptr;
|
||||
if (indices && !indices->header.frame_id.empty ())
|
||||
indices_ptr.reset (new std::vector<int> (indices->indices));
|
||||
|
||||
impl_.setIndices (indices_ptr);
|
||||
|
||||
// Final check if the data is empty (remember that indices are set to the size of the data -- if indices* = NULL)
|
||||
if (!cloud->points.empty ())
|
||||
impl_.segment (inliers, model);
|
||||
|
||||
// Check if we have enough inliers, clear inliers + model if not
|
||||
if ((int)inliers.indices.size () <= min_inliers_)
|
||||
{
|
||||
inliers.indices.clear ();
|
||||
model.values.clear ();
|
||||
}
|
||||
|
||||
// Publish
|
||||
pub_indices_.publish (boost::make_shared<const PointIndices> (inliers));
|
||||
pub_model_.publish (boost::make_shared<const ModelCoefficients> (model));
|
||||
NODELET_DEBUG ("[%s::input_normals_callback] Published PointIndices with %zu values on topic %s, and ModelCoefficients with %zu values on topic %s",
|
||||
getName ().c_str (), inliers.indices.size (), pnh_->resolveName ("inliers").c_str (),
|
||||
model.values.size (), pnh_->resolveName ("model").c_str ());
|
||||
if (inliers.indices.empty ())
|
||||
NODELET_WARN ("[%s::input_indices_callback] No inliers found!", getName ().c_str ());
|
||||
}
|
||||
|
||||
typedef pcl_ros::SACSegmentation SACSegmentation;
|
||||
typedef pcl_ros::SACSegmentationFromNormals SACSegmentationFromNormals;
|
||||
PLUGINLIB_DECLARE_CLASS (pcl, SACSegmentation, SACSegmentation, nodelet::Nodelet);
|
||||
PLUGINLIB_DECLARE_CLASS (pcl, SACSegmentationFromNormals, SACSegmentationFromNormals, nodelet::Nodelet);
|
||||
|
||||
128
pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp
Normal file
128
pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp
Normal file
@ -0,0 +1,128 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2010, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: segment_differences.cpp 35361 2011-01-20 04:34:49Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/segmentation/segment_differences.h"
|
||||
#include <pcl/io/io.h>
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::SegmentDifferences::onInit ()
|
||||
{
|
||||
// Call the super onInit ()
|
||||
PCLNodelet::onInit ();
|
||||
|
||||
pub_output_ = pnh_->advertise<PointCloud> ("output", max_queue_size_);
|
||||
|
||||
// Subscribe to the input using a filter
|
||||
sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
|
||||
sub_target_filter_.subscribe (*pnh_, "target", max_queue_size_);
|
||||
|
||||
// Enable the dynamic reconfigure service
|
||||
srv_ = boost::make_shared <dynamic_reconfigure::Server<SegmentDifferencesConfig> > (*pnh_);
|
||||
dynamic_reconfigure::Server<SegmentDifferencesConfig>::CallbackType f = boost::bind (&SegmentDifferences::config_callback, this, _1, _2);
|
||||
srv_->setCallback (f);
|
||||
|
||||
if (approximate_sync_)
|
||||
{
|
||||
sync_input_target_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointCloud> > > (max_queue_size_);
|
||||
sync_input_target_a_->connectInput (sub_input_filter_, sub_target_filter_);
|
||||
sync_input_target_a_->registerCallback (bind (&SegmentDifferences::input_target_callback, this, _1, _2));
|
||||
}
|
||||
else
|
||||
{
|
||||
sync_input_target_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloud> > > (max_queue_size_);
|
||||
sync_input_target_e_->connectInput (sub_input_filter_, sub_target_filter_);
|
||||
sync_input_target_e_->registerCallback (bind (&SegmentDifferences::input_target_callback, this, _1, _2));
|
||||
}
|
||||
|
||||
NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
|
||||
" - max_queue_size : %d",
|
||||
getName ().c_str (),
|
||||
max_queue_size_);
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::SegmentDifferences::config_callback (SegmentDifferencesConfig &config, uint32_t level)
|
||||
{
|
||||
if (impl_.getDistanceThreshold () != config.distance_threshold)
|
||||
{
|
||||
impl_.setDistanceThreshold (config.distance_threshold);
|
||||
NODELET_DEBUG ("[%s::config_callback] Setting new distance threshold to: %f.", getName ().c_str (), config.distance_threshold);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::SegmentDifferences::input_target_callback (const PointCloudConstPtr &cloud,
|
||||
const PointCloudConstPtr &cloud_target)
|
||||
{
|
||||
if (pub_output_.getNumSubscribers () <= 0)
|
||||
return;
|
||||
|
||||
if (!isValid (cloud) || !isValid (cloud_target, "target"))
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
|
||||
PointCloud output;
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
return;
|
||||
}
|
||||
|
||||
NODELET_DEBUG ("[%s::input_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
|
||||
getName ().c_str (),
|
||||
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
|
||||
cloud_target->width * cloud_target->height, pcl::getFieldsList (*cloud_target).c_str (), cloud_target->header.stamp.toSec (), cloud_target->header.frame_id.c_str (), pnh_->resolveName ("target").c_str ());
|
||||
|
||||
impl_.setInputCloud (cloud);
|
||||
impl_.setTargetCloud (cloud_target);
|
||||
|
||||
PointCloud output;
|
||||
impl_.segment (output);
|
||||
|
||||
pub_output_.publish (output.makeShared ());
|
||||
NODELET_DEBUG ("[%s::segmentAndPublish] Published PointCloud2 with %zu points and stamp %f on topic %s", getName ().c_str (),
|
||||
output.points.size (), output.header.stamp.toSec (), pnh_->resolveName ("output").c_str ());
|
||||
}
|
||||
|
||||
typedef pcl_ros::SegmentDifferences SegmentDifferences;
|
||||
PLUGINLIB_DECLARE_CLASS (pcl, SegmentDifferences, SegmentDifferences, nodelet::Nodelet);
|
||||
|
||||
44
pcl_ros/src/pcl_ros/segmentation/segmentation.cpp
Normal file
44
pcl_ros/src/pcl_ros/segmentation/segmentation.cpp
Normal file
@ -0,0 +1,44 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2009, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: segmentation.cpp 34600 2010-12-07 21:12:11Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
// Include the implementations here instead of compiling them separately to speed up compile time
|
||||
//#include "extract_clusters.cpp"
|
||||
//#include "extract_polygonal_prism_data.cpp"
|
||||
//#include "sac_segmentation.cpp"
|
||||
//#include "segment_differences.cpp"
|
||||
|
||||
|
||||
182
pcl_ros/src/pcl_ros/surface/convex_hull.cpp
Normal file
182
pcl_ros/src/pcl_ros/surface/convex_hull.cpp
Normal file
@ -0,0 +1,182 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2010, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: convex_hull.hpp 32993 2010-09-30 23:08:57Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include <pcl/common/io.h>
|
||||
#include "pcl_ros/surface/convex_hull.h"
|
||||
#include <geometry_msgs/PolygonStamped.h>
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::ConvexHull2D::onInit ()
|
||||
{
|
||||
ros::NodeHandle pnh = getMTPrivateNodeHandle ();
|
||||
pub_output_ = pnh.advertise<PointCloud> ("output", max_queue_size_);
|
||||
pub_plane_ = pnh.advertise<geometry_msgs::PolygonStamped>("output_polygon", max_queue_size_);
|
||||
|
||||
// ---[ Optional parameters
|
||||
pnh.getParam ("use_indices", use_indices_);
|
||||
|
||||
// If we're supposed to look for PointIndices (indices)
|
||||
if (use_indices_)
|
||||
{
|
||||
// Subscribe to the input using a filter
|
||||
sub_input_filter_.subscribe (pnh, "input", 1);
|
||||
// If indices are enabled, subscribe to the indices
|
||||
sub_indices_filter_.subscribe (pnh, "indices", 1);
|
||||
|
||||
if (approximate_sync_)
|
||||
{
|
||||
sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointIndices> > >(max_queue_size_);
|
||||
// surface not enabled, connect the input-indices duo and register
|
||||
sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_);
|
||||
sync_input_indices_a_->registerCallback (bind (&ConvexHull2D::input_indices_callback, this, _1, _2));
|
||||
}
|
||||
else
|
||||
{
|
||||
sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointIndices> > >(max_queue_size_);
|
||||
// surface not enabled, connect the input-indices duo and register
|
||||
sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_);
|
||||
sync_input_indices_e_->registerCallback (bind (&ConvexHull2D::input_indices_callback, this, _1, _2));
|
||||
}
|
||||
}
|
||||
else
|
||||
// Subscribe in an old fashion to input only (no filters)
|
||||
sub_input_ = pnh.subscribe<PointCloud> ("input", 1, bind (&ConvexHull2D::input_indices_callback, this, _1, PointIndicesConstPtr ()));
|
||||
|
||||
NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
|
||||
" - use_indices : %s",
|
||||
getName ().c_str (),
|
||||
(use_indices_) ? "true" : "false");
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::ConvexHull2D::input_indices_callback (const PointCloudConstPtr &cloud,
|
||||
const PointIndicesConstPtr &indices)
|
||||
{
|
||||
// No subscribers, no work
|
||||
if (pub_output_.getNumSubscribers () <= 0 && pub_plane_.getNumSubscribers () <= 0)
|
||||
return;
|
||||
|
||||
PointCloud output;
|
||||
|
||||
// If cloud is given, check if it's valid
|
||||
if (!isValid (cloud))
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
|
||||
// Publish an empty message
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
return;
|
||||
}
|
||||
// If indices are given, check if they are valid
|
||||
if (indices && !isValid (indices, "indices"))
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
|
||||
// Publish an empty message
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
return;
|
||||
}
|
||||
|
||||
/// DEBUG
|
||||
if (indices)
|
||||
NODELET_DEBUG ("[%s::input_indices_model_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
|
||||
getName ().c_str (),
|
||||
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str (),
|
||||
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("indices").c_str ());
|
||||
else
|
||||
NODELET_DEBUG ("[%s::input_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str ());
|
||||
|
||||
// Reset the indices and surface pointers
|
||||
IndicesPtr indices_ptr;
|
||||
if (indices)
|
||||
indices_ptr.reset (new std::vector<int> (indices->indices));
|
||||
|
||||
impl_.setInputCloud (cloud);
|
||||
impl_.setIndices (indices_ptr);
|
||||
|
||||
// Estimate the feature
|
||||
impl_.reconstruct (output);
|
||||
|
||||
// If more than 3 points are present, send a PolygonStamped hull too
|
||||
if (output.points.size () >= 3)
|
||||
{
|
||||
geometry_msgs::PolygonStamped poly;
|
||||
poly.header = output.header;
|
||||
poly.polygon.points.resize (output.points.size ());
|
||||
// Get three consecutive points (without copying)
|
||||
pcl::Vector4fMap O = output.points[1].getVector4fMap ();
|
||||
pcl::Vector4fMap B = output.points[0].getVector4fMap ();
|
||||
pcl::Vector4fMap A = output.points[2].getVector4fMap ();
|
||||
// Check the direction of points -- polygon must have CCW direction
|
||||
Eigen::Vector4f OA = A - O;
|
||||
Eigen::Vector4f OB = B - O;
|
||||
Eigen::Vector4f N = OA.cross3 (OB);
|
||||
double theta = N.dot (O);
|
||||
bool reversed = false;
|
||||
if (theta < (M_PI / 2.0))
|
||||
reversed = true;
|
||||
for (size_t i = 0; i < output.points.size (); ++i)
|
||||
{
|
||||
if (reversed)
|
||||
{
|
||||
size_t j = output.points.size () - i - 1;
|
||||
poly.polygon.points[i].x = output.points[j].x;
|
||||
poly.polygon.points[i].y = output.points[j].y;
|
||||
poly.polygon.points[i].z = output.points[j].z;
|
||||
}
|
||||
else
|
||||
{
|
||||
poly.polygon.points[i].x = output.points[i].x;
|
||||
poly.polygon.points[i].y = output.points[i].y;
|
||||
poly.polygon.points[i].z = output.points[i].z;
|
||||
}
|
||||
}
|
||||
pub_plane_.publish (boost::make_shared<const geometry_msgs::PolygonStamped> (poly));
|
||||
}
|
||||
// Publish a Boost shared ptr const data
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
}
|
||||
|
||||
typedef pcl_ros::ConvexHull2D ConvexHull2D;
|
||||
PLUGINLIB_DECLARE_CLASS (pcl, ConvexHull2D, ConvexHull2D, nodelet::Nodelet);
|
||||
|
||||
217
pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp
Normal file
217
pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp
Normal file
@ -0,0 +1,217 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2010, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: moving_least_squares.cpp 36097 2011-02-20 14:18:58Z marton $
|
||||
*
|
||||
*/
|
||||
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/surface/moving_least_squares.h"
|
||||
#include <pcl/io/io.h>
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::MovingLeastSquares::onInit ()
|
||||
{
|
||||
PCLNodelet::onInit ();
|
||||
|
||||
//ros::NodeHandle private_nh = getMTPrivateNodeHandle ();
|
||||
pub_output_ = pnh_->advertise<PointCloudIn> ("output", max_queue_size_);
|
||||
pub_normals_ = pnh_->advertise<NormalCloudOut> ("normals", max_queue_size_);
|
||||
|
||||
//if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("search_radius", search_radius_))
|
||||
if (!pnh_->getParam ("search_radius", search_radius_))
|
||||
{
|
||||
//NODELET_ERROR ("[%s::onInit] Neither 'k_search' nor 'search_radius' set! Need to set at least one of these parameters before continuing.", getName ().c_str ());
|
||||
NODELET_ERROR ("[%s::onInit] Need a 'search_radius' parameter to be set before continuing!", getName ().c_str ());
|
||||
return;
|
||||
}
|
||||
if (!pnh_->getParam ("spatial_locator", spatial_locator_type_))
|
||||
{
|
||||
NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ());
|
||||
return;
|
||||
}
|
||||
|
||||
// Enable the dynamic reconfigure service
|
||||
srv_ = boost::make_shared<dynamic_reconfigure::Server<MLSConfig> > (*pnh_);
|
||||
dynamic_reconfigure::Server<MLSConfig>::CallbackType f = boost::bind (&MovingLeastSquares::config_callback, this, _1, _2 );
|
||||
srv_->setCallback (f);
|
||||
|
||||
// ---[ Optional parameters
|
||||
pnh_->getParam ("use_indices", use_indices_);
|
||||
|
||||
// If we're supposed to look for PointIndices (indices)
|
||||
if (use_indices_)
|
||||
{
|
||||
// Subscribe to the input using a filter
|
||||
sub_input_filter_.subscribe (*pnh_, "input", 1);
|
||||
// If indices are enabled, subscribe to the indices
|
||||
sub_indices_filter_.subscribe (*pnh_, "indices", 1);
|
||||
|
||||
if (approximate_sync_)
|
||||
{
|
||||
sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<PointCloudIn, PointIndices> > >(max_queue_size_);
|
||||
// surface not enabled, connect the input-indices duo and register
|
||||
sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_);
|
||||
sync_input_indices_a_->registerCallback (bind (&MovingLeastSquares::input_indices_callback, this, _1, _2));
|
||||
}
|
||||
else
|
||||
{
|
||||
sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ExactTime<PointCloudIn, PointIndices> > >(max_queue_size_);
|
||||
// surface not enabled, connect the input-indices duo and register
|
||||
sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_);
|
||||
sync_input_indices_e_->registerCallback (bind (&MovingLeastSquares::input_indices_callback, this, _1, _2));
|
||||
}
|
||||
}
|
||||
else
|
||||
// Subscribe in an old fashion to input only (no filters)
|
||||
sub_input_ = pnh_->subscribe<PointCloudIn> ("input", 1, bind (&MovingLeastSquares::input_indices_callback, this, _1, PointIndicesConstPtr ()));
|
||||
|
||||
|
||||
NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
|
||||
" - use_indices : %s",
|
||||
getName ().c_str (),
|
||||
(use_indices_) ? "true" : "false");
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::MovingLeastSquares::input_indices_callback (const PointCloudInConstPtr &cloud,
|
||||
const PointIndicesConstPtr &indices)
|
||||
{
|
||||
// No subscribers, no work
|
||||
if (pub_output_.getNumSubscribers () <= 0 && pub_normals_.getNumSubscribers () <= 0)
|
||||
return;
|
||||
|
||||
// Output points have the same type as the input, they are only smoothed
|
||||
PointCloudIn output;
|
||||
|
||||
// Normals are also estimated and published on a separate topic
|
||||
NormalCloudOut::Ptr normals (new NormalCloudOut ());
|
||||
|
||||
// If cloud is given, check if it's valid
|
||||
if (!isValid (cloud))
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
return;
|
||||
}
|
||||
// If indices are given, check if they are valid
|
||||
if (indices && !isValid (indices, "indices"))
|
||||
{
|
||||
NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
return;
|
||||
}
|
||||
|
||||
/// DEBUG
|
||||
if (indices)
|
||||
NODELET_DEBUG ("[%s::input_indices_model_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
|
||||
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
|
||||
getName ().c_str (),
|
||||
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str (),
|
||||
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("indices").c_str ());
|
||||
else
|
||||
NODELET_DEBUG ("[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str ());
|
||||
///
|
||||
|
||||
// Reset the indices and surface pointers
|
||||
impl_.setInputCloud (cloud);
|
||||
impl_.setOutputNormals (normals);
|
||||
|
||||
IndicesPtr indices_ptr;
|
||||
if (indices)
|
||||
indices_ptr.reset (new std::vector<int> (indices->indices));
|
||||
|
||||
impl_.setIndices (indices_ptr);
|
||||
|
||||
// Initialize the spatial locator
|
||||
initTree (spatial_locator_type_, tree_, 0); //k_);
|
||||
impl_.setSearchMethod (tree_);
|
||||
|
||||
// Do the reconstructon
|
||||
impl_.reconstruct (output);
|
||||
|
||||
// Publish a Boost shared ptr const data
|
||||
// Enforce that the TF frame and the timestamp are copied
|
||||
output.header = cloud->header;
|
||||
pub_output_.publish (output.makeShared ());
|
||||
normals->header = cloud->header;
|
||||
pub_normals_.publish (normals);
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::MovingLeastSquares::config_callback (MLSConfig &config, uint32_t level)
|
||||
{
|
||||
// \Note Zoli, shouldn't this be implemented in MLS too?
|
||||
/*if (k_ != config.k_search)
|
||||
{
|
||||
k_ = config.k_search;
|
||||
NODELET_DEBUG ("[config_callback] Setting the k_search to: %d.", k_);
|
||||
}*/
|
||||
if (search_radius_ != config.search_radius)
|
||||
{
|
||||
search_radius_ = config.search_radius;
|
||||
NODELET_DEBUG ("[config_callback] Setting the search radius: %f.", search_radius_);
|
||||
impl_.setSearchRadius (search_radius_);
|
||||
}
|
||||
if (spatial_locator_type_ != config.spatial_locator)
|
||||
{
|
||||
spatial_locator_type_ = config.spatial_locator;
|
||||
NODELET_DEBUG ("[config_callback] Setting the spatial locator to type: %d.", spatial_locator_type_);
|
||||
}
|
||||
if (use_polynomial_fit_ != config.use_polynomial_fit)
|
||||
{
|
||||
use_polynomial_fit_ = config.use_polynomial_fit;
|
||||
NODELET_DEBUG ("[config_callback] Setting the use_polynomial_fit flag to: %d.", use_polynomial_fit_);
|
||||
impl_.setPolynomialFit (use_polynomial_fit_);
|
||||
}
|
||||
if (polynomial_order_ != config.polynomial_order)
|
||||
{
|
||||
polynomial_order_ = config.polynomial_order;
|
||||
NODELET_DEBUG ("[config_callback] Setting the polynomial order to: %d.", polynomial_order_);
|
||||
impl_.setPolynomialOrder (polynomial_order_);
|
||||
}
|
||||
if (gaussian_parameter_ != config.gaussian_parameter)
|
||||
{
|
||||
gaussian_parameter_ = config.gaussian_parameter;
|
||||
NODELET_DEBUG ("[config_callback] Setting the gaussian parameter to: %f.", gaussian_parameter_);
|
||||
impl_.setSqrGaussParam (gaussian_parameter_ * gaussian_parameter_);
|
||||
}
|
||||
}
|
||||
|
||||
typedef pcl_ros::MovingLeastSquares MovingLeastSquares;
|
||||
PLUGINLIB_DECLARE_CLASS (pcl, MovingLeastSquares, MovingLeastSquares, nodelet::Nodelet);
|
||||
48
pcl_ros/src/pcl_ros/surface/surface.cpp
Normal file
48
pcl_ros/src/pcl_ros/surface/surface.cpp
Normal file
@ -0,0 +1,48 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2009, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: surface.cpp 34603 2010-12-07 22:42:10Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
/**
|
||||
|
||||
\author Radu Bogdan Rusu
|
||||
|
||||
**/
|
||||
|
||||
// Include the implementations here instead of compiling them separately to speed up compile time
|
||||
//#include "convex_hull.cpp"
|
||||
//#include "moving_least_squares.cpp"
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user