copy features/segmentation/surface from fuerte-devel
This commit is contained in:
Kei Okada
Paul Bovbel
@@ -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);
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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"
|
||||
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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);
|
||||
@@ -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"
|
||||
|
||||
|
||||
Reference in New Issue
Block a user