pcl_ros: pcl_ros: feature.cpp Source File

00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * $Id: feature.cpp 35422 2011-01-24 20:04:44Z rusu $
00035  *
00036  */
00037 
00038 //#include <pluginlib/class_list_macros.h>
00039 // Include the implementations here instead of compiling them separately to speed up compile time
00040 //#include "normal_3d.cpp"
00041 //#include "boundary.cpp"
00042 //#include "fpfh.cpp"
00043 //#include "fpfh_omp.cpp"
00044 //#include "moment_invariants.cpp"
00045 //#include "normal_3d_omp.cpp"
00046 //#include "normal_3d_tbb.cpp"
00047 //#include "pfh.cpp"
00048 //#include "principal_curvatures.cpp"
00049 //#include "vfh.cpp"
00050 #include <pcl/io/io.h>
00051 #include "pcl_ros/features/feature.h"
00052 #include <message_filters/null_types.h>
00053 
00055 void
00056 pcl_ros::Feature::onInit ()
00057 {
00058   // Call the super onInit ()
00059   PCLNodelet::onInit ();
00060 
00061   // Call the child init
00062   childInit (*pnh_);
00063 
00064   // Allow each individual class that inherits from us to declare their own Publisher
00065   // This is useful for Publisher<pcl::PointCloud<T> >, as NormalEstimation can publish PointCloud<Normal>, etc
00066   //pub_output_ = pnh_->template advertise<PointCloud2> ("output", max_queue_size_);
00067 
00068   // ---[ Mandatory parameters
00069   if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("radius_search", search_radius_))
00070   {
00071     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 ());
00072     return;
00073   }
00074   if (!pnh_->getParam ("spatial_locator", spatial_locator_type_))
00075   {
00076     NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ());
00077     return;
00078   }
00079 
00080   // ---[ Optional parameters
00081   pnh_->getParam ("use_surface", use_surface_);
00082 
00083   // Enable the dynamic reconfigure service
00084   srv_ = boost::make_shared<dynamic_reconfigure::Server<FeatureConfig> > (*pnh_);
00085   dynamic_reconfigure::Server<FeatureConfig>::CallbackType f = boost::bind (&Feature::config_callback, this, _1, _2);
00086   srv_->setCallback (f);
00087 
00088   // If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages
00089   if (use_indices_ || use_surface_)
00090   {
00091     // Create the objects here
00092     if (approximate_sync_)
00093       sync_input_surface_indices_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn, PointCloudIn, PointIndices> > >(max_queue_size_);
00094     else
00095       sync_input_surface_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, PointCloudIn, PointIndices> > >(max_queue_size_);
00096 
00097     // Subscribe to the input using a filter
00098     sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
00099     if (use_indices_)
00100     {
00101       // If indices are enabled, subscribe to the indices
00102       sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
00103       if (use_surface_)     // Use both indices and surface
00104       {
00105         // If surface is enabled, subscribe to the surface, connect the input-indices-surface trio and register
00106         sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_);
00107         if (approximate_sync_)
00108           sync_input_surface_indices_a_->connectInput (sub_input_filter_, sub_surface_filter_, sub_indices_filter_);
00109         else
00110           sync_input_surface_indices_e_->connectInput (sub_input_filter_, sub_surface_filter_, sub_indices_filter_);
00111       }
00112       else                  // Use only indices
00113       {
00114         sub_input_filter_.registerCallback (bind (&Feature::input_callback, this, _1));
00115         // surface not enabled, connect the input-indices duo and register
00116         if (approximate_sync_)
00117           sync_input_surface_indices_a_->connectInput (sub_input_filter_, nf_pc_, sub_indices_filter_);
00118         else
00119           sync_input_surface_indices_e_->connectInput (sub_input_filter_, nf_pc_, sub_indices_filter_);
00120       }
00121     }
00122     else                    // Use only surface
00123     {
00124       sub_input_filter_.registerCallback (bind (&Feature::input_callback, this, _1));
00125       // indices not enabled, connect the input-surface duo and register
00126       sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_);
00127       if (approximate_sync_)
00128         sync_input_surface_indices_a_->connectInput (sub_input_filter_, sub_surface_filter_, nf_pi_);
00129       else
00130         sync_input_surface_indices_e_->connectInput (sub_input_filter_, sub_surface_filter_, nf_pi_);
00131     }
00132     // Register callbacks
00133     if (approximate_sync_)
00134       sync_input_surface_indices_a_->registerCallback (bind (&Feature::input_surface_indices_callback, this, _1, _2, _3));
00135     else
00136       sync_input_surface_indices_e_->registerCallback (bind (&Feature::input_surface_indices_callback, this, _1, _2, _3));
00137   }
00138   else
00139     // Subscribe in an old fashion to input only (no filters)
00140     sub_input_ = pnh_->subscribe<PointCloudIn> ("input", max_queue_size_,  bind (&Feature::input_surface_indices_callback, this, _1, PointCloudInConstPtr (), PointIndicesConstPtr ()));
00141 
00142   NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
00143                  " - use_surface    : %s\n"
00144                  " - k_search       : %d\n"
00145                  " - radius_search  : %f\n"
00146                  " - spatial_locator: %d",
00147                 getName ().c_str (),
00148                 (use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_);
00149 }
00150 
00152 void
00153 pcl_ros::Feature::config_callback (FeatureConfig &config, uint32_t level)
00154 {
00155   if (k_ != config.k_search)
00156   {
00157     k_ = config.k_search;
00158     NODELET_DEBUG ("[config_callback] Setting the number of K nearest neighbors to use for each point: %d.", k_);
00159   }
00160   if (search_radius_ != config.radius_search)
00161   {
00162     search_radius_ = config.radius_search;
00163     NODELET_DEBUG ("[config_callback] Setting the nearest neighbors search radius for each point: %f.", search_radius_);
00164   }
00165 }
00166 
00168 void
00169 pcl_ros::Feature::input_surface_indices_callback (const PointCloudInConstPtr &cloud, 
00170     const PointCloudInConstPtr &cloud_surface, const PointIndicesConstPtr &indices)
00171 {
00172   // No subscribers, no work
00173   if (pub_output_.getNumSubscribers () <= 0)
00174     return;
00175 
00176   // If cloud is given, check if it's valid
00177   if (!isValid (cloud))
00178   {
00179     NODELET_ERROR ("[%s::input_surface_indices_callback] Invalid input!", getName ().c_str ());
00180     emptyPublish (cloud);
00181     return;
00182   }
00183 
00184   // If surface is given, check if it's valid
00185   if (cloud_surface && !isValid (cloud_surface, "surface"))
00186   {
00187     NODELET_ERROR ("[%s::input_surface_indices_callback] Invalid input surface!", getName ().c_str ());
00188     emptyPublish (cloud);
00189     return;
00190   }
00191     
00192   // If indices are given, check if they are valid
00193   if (indices && !isValid (indices))
00194   {
00195     NODELET_ERROR ("[%s::input_surface_indices_callback] Invalid input indices!", getName ().c_str ());
00196     emptyPublish (cloud);
00197     return;
00198   }
00199 
00201   if (cloud_surface)
00202     if (indices)
00203       NODELET_DEBUG ("[input_surface_indices_callback]\n"
00204                      "                                         - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00205                      "                                         - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00206                      "                                         - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
00207                  cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
00208                  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 (),
00209                  indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
00210     else
00211       NODELET_DEBUG ("[input_surface_indices_callback]\n"
00212                      "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00213                      "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
00214                      cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
00215                      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 ());
00216 
00217   else
00218     if (indices)
00219       NODELET_DEBUG ("[input_surface_indices_callback]\n"
00220                      "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00221                      "                                 - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
00222                      cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
00223                      indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
00224     else
00225       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 ());
00227 
00228 
00229   if ((int)(cloud->width * cloud->height) < k_)
00230   {
00231     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));
00232     emptyPublish (cloud);
00233     return;
00234   }
00235 
00236   // If indices given...
00237   IndicesPtr vindices;
00238   if (indices && !indices->header.frame_id.empty ())
00239     vindices.reset (new std::vector<int> (indices->indices));
00240 
00241   computePublish (cloud, cloud_surface, vindices);
00242 }
00243 
00247 
00249 void
00250 pcl_ros::FeatureFromNormals::onInit ()
00251 {
00252   // Call the super onInit ()
00253   PCLNodelet::onInit ();
00254 
00255   // Call the child init
00256   childInit (*pnh_);
00257 
00258   // Allow each individual class that inherits from us to declare their own Publisher
00259   // This is useful for Publisher<pcl::PointCloud<T> >, as NormalEstimation can publish PointCloud<Normal>, etc
00260   //pub_output_ = pnh_->template advertise<PointCloud2> ("output", max_queue_size_);
00261 
00262   // ---[ Mandatory parameters
00263   if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("radius_search", search_radius_))
00264   {
00265     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 ());
00266     return;
00267   }
00268   if (!pnh_->getParam ("spatial_locator", spatial_locator_type_))
00269   {
00270     NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ());
00271     return;
00272   }
00273   // ---[ Optional parameters
00274   pnh_->getParam ("use_surface", use_surface_);
00275 
00276   sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
00277   sub_normals_filter_.subscribe (*pnh_, "normals", max_queue_size_);
00278 
00279   // Enable the dynamic reconfigure service
00280   srv_ = boost::make_shared<dynamic_reconfigure::Server<FeatureConfig> > (*pnh_);
00281   dynamic_reconfigure::Server<FeatureConfig>::CallbackType f = boost::bind (&FeatureFromNormals::config_callback, this, _1, _2);
00282   srv_->setCallback (f);
00283 
00284   // Create the objects here
00285   if (approximate_sync_)
00286     sync_input_normals_surface_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn, PointCloudN, PointCloudIn, PointIndices> > > (max_queue_size_);
00287   else
00288     sync_input_normals_surface_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, PointCloudN, PointCloudIn, PointIndices> > > (max_queue_size_);
00289 
00290   // If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages
00291   if (use_indices_ || use_surface_)
00292   {
00293     if (use_indices_)
00294     {
00295       // If indices are enabled, subscribe to the indices
00296       sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
00297       if (use_surface_)     // Use both indices and surface
00298       {
00299         // If surface is enabled, subscribe to the surface, connect the input-indices-surface trio and register
00300         sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_);
00301         if (approximate_sync_)
00302           sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, sub_indices_filter_);
00303         else
00304           sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, sub_indices_filter_);
00305       }
00306       else                  // Use only indices
00307       {
00308         sub_input_filter_.registerCallback (bind (&FeatureFromNormals::input_callback, this, _1));
00309         if (approximate_sync_)
00310           // surface not enabled, connect the input-indices duo and register
00311           sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, sub_indices_filter_);
00312         else
00313           // surface not enabled, connect the input-indices duo and register
00314           sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, sub_indices_filter_);
00315       }
00316     }
00317     else                    // Use only surface
00318     {
00319       // indices not enabled, connect the input-surface duo and register
00320       sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_);
00321 
00322       sub_input_filter_.registerCallback (bind (&FeatureFromNormals::input_callback, this, _1));
00323       if (approximate_sync_)
00324         sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, nf_pi_);
00325       else
00326         sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, nf_pi_);
00327     }
00328   }
00329   else
00330   {
00331     sub_input_filter_.registerCallback (bind (&FeatureFromNormals::input_callback, this, _1));
00332 
00333     if (approximate_sync_)
00334       sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, nf_pi_);
00335     else
00336       sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, nf_pi_);
00337   }
00338 
00339   // Register callbacks
00340   if (approximate_sync_)
00341     sync_input_normals_surface_indices_a_->registerCallback (bind (&FeatureFromNormals::input_normals_surface_indices_callback, this, _1, _2, _3, _4));
00342   else
00343     sync_input_normals_surface_indices_e_->registerCallback (bind (&FeatureFromNormals::input_normals_surface_indices_callback, this, _1, _2, _3, _4));
00344 
00345   NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
00346                  " - use_surface    : %s\n"
00347                  " - k_search       : %d\n"
00348                  " - radius_search  : %f\n"
00349                  " - spatial_locator: %d",
00350                 getName ().c_str (),
00351                  (use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_);
00352 }
00353 
00355 void
00356 pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback (
00357    const PointCloudInConstPtr &cloud, const PointCloudNConstPtr &cloud_normals,
00358    const PointCloudInConstPtr &cloud_surface, const PointIndicesConstPtr &indices)
00359 {
00360   // No subscribers, no work
00361   if (pub_output_.getNumSubscribers () <= 0)
00362     return;
00363 
00364   // If cloud+normals is given, check if it's valid
00365   if (!isValid (cloud))// || !isValid (cloud_normals, "normals"))
00366   {
00367     NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input!", getName ().c_str ());
00368     emptyPublish (cloud);
00369     return;
00370   }
00371 
00372   // If surface is given, check if it's valid
00373   if (cloud_surface && !isValid (cloud_surface, "surface"))
00374   {
00375     NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input surface!", getName ().c_str ());
00376     emptyPublish (cloud);
00377     return;
00378   }
00379     
00380   // If indices are given, check if they are valid
00381   if (indices && !isValid (indices))
00382   {
00383     NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input indices!", getName ().c_str ());
00384     emptyPublish (cloud);
00385     return;
00386   }
00387 
00389   if (cloud_surface)
00390     if (indices)
00391       NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n"
00392                      "                                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00393                      "                                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00394                      "                                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00395                      "                                                 - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
00396                      getName ().c_str (),
00397                      cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
00398                      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 (),
00399                      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 (),
00400                      indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
00401     else
00402       NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n"
00403                      "                                         - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00404                      "                                         - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00405                      "                                         - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
00406                      getName ().c_str (), 
00407                      cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
00408                      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 (),
00409                      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 ());
00410   else
00411     if (indices)
00412       NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n"
00413                      "                                         - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00414                      "                                         - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00415                      "                                         - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
00416                      getName ().c_str (),
00417                      cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
00418                      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 (),
00419                      indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
00420     else
00421       NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n"
00422                      "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00423                      "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
00424                      getName ().c_str (), 
00425                      cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
00426                      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 ());
00428 
00429   if ((int)(cloud->width * cloud->height) < k_)
00430   {
00431     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));
00432     emptyPublish (cloud);
00433     return;
00434   }
00435 
00436   // If indices given...
00437   IndicesPtr vindices;
00438   if (indices && !indices->header.frame_id.empty ())
00439     vindices.reset (new std::vector<int> (indices->indices));
00440 
00441   computePublish (cloud, cloud_normals, cloud_surface, vindices);
00442 }
00443