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_ros/features/feature.h"
00051 #include <message_filters/null_types.h>
00052 
00054 void
00055 pcl_ros::Feature::onInit ()
00056 {
00057   // Call the super onInit ()
00058   PCLNodelet::onInit ();
00059 
00060   // Call the child init
00061   childInit (*pnh_);
00062 
00063   // Allow each individual class that inherits from us to declare their own Publisher
00064   // This is useful for Publisher<pcl::PointCloud<T> >, as NormalEstimation can publish PointCloud<Normal>, etc
00065   //pub_output_ = pnh_->template advertise<PointCloud2> ("output", max_queue_size_);
00066 
00067   // ---[ Mandatory parameters
00068   if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("radius_search", search_radius_))
00069   {
00070     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 ());
00071     return;
00072   }
00073   if (!pnh_->getParam ("spatial_locator", spatial_locator_type_))
00074   {
00075     NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ());
00076     return;
00077   }
00078 
00079   // ---[ Optional parameters
00080   pnh_->getParam ("use_surface", use_surface_);
00081 
00082   // Enable the dynamic reconfigure service
00083   srv_ = boost::make_shared<dynamic_reconfigure::Server<FeatureConfig> > (*pnh_);
00084   dynamic_reconfigure::Server<FeatureConfig>::CallbackType f = boost::bind (&Feature::config_callback, this, _1, _2);
00085   srv_->setCallback (f);
00086 
00087   // If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages
00088   if (use_indices_ || use_surface_)
00089   {
00090     // Create the objects here
00091     if (approximate_sync_)
00092       sync_input_surface_indices_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn, PointCloudIn, PointIndices> > >(max_queue_size_);
00093     else
00094       sync_input_surface_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, PointCloudIn, PointIndices> > >(max_queue_size_);
00095 
00096     // Subscribe to the input using a filter
00097     sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
00098     if (use_indices_)
00099     {
00100       // If indices are enabled, subscribe to the indices
00101       sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
00102       if (use_surface_)     // Use both indices and surface
00103       {
00104         // If surface is enabled, subscribe to the surface, connect the input-indices-surface trio and register
00105         sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_);
00106         if (approximate_sync_)
00107           sync_input_surface_indices_a_->connectInput (sub_input_filter_, sub_surface_filter_, sub_indices_filter_);
00108         else
00109           sync_input_surface_indices_e_->connectInput (sub_input_filter_, sub_surface_filter_, sub_indices_filter_);
00110       }
00111       else                  // Use only indices
00112       {
00113         sub_input_filter_.registerCallback (bind (&Feature::input_callback, this, _1));
00114         // surface not enabled, connect the input-indices duo and register
00115         if (approximate_sync_)
00116           sync_input_surface_indices_a_->connectInput (sub_input_filter_, nf_pc_, sub_indices_filter_);
00117         else
00118           sync_input_surface_indices_e_->connectInput (sub_input_filter_, nf_pc_, sub_indices_filter_);
00119       }
00120     }
00121     else                    // Use only surface
00122     {
00123       sub_input_filter_.registerCallback (bind (&Feature::input_callback, this, _1));
00124       // indices not enabled, connect the input-surface duo and register
00125       sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_);
00126       if (approximate_sync_)
00127         sync_input_surface_indices_a_->connectInput (sub_input_filter_, sub_surface_filter_, nf_pi_);
00128       else
00129         sync_input_surface_indices_e_->connectInput (sub_input_filter_, sub_surface_filter_, nf_pi_);
00130     }
00131     // Register callbacks
00132     if (approximate_sync_)
00133       sync_input_surface_indices_a_->registerCallback (bind (&Feature::input_surface_indices_callback, this, _1, _2, _3));
00134     else
00135       sync_input_surface_indices_e_->registerCallback (bind (&Feature::input_surface_indices_callback, this, _1, _2, _3));
00136   }
00137   else
00138     // Subscribe in an old fashion to input only (no filters)
00139     sub_input_ = pnh_->subscribe<PointCloudIn> ("input", max_queue_size_,  bind (&Feature::input_surface_indices_callback, this, _1, PointCloudInConstPtr (), PointIndicesConstPtr ()));
00140 
00141   NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
00142                  " - use_surface    : %s\n"
00143                  " - k_search       : %d\n"
00144                  " - radius_search  : %f\n"
00145                  " - spatial_locator: %d",
00146                 getName ().c_str (),
00147                 (use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_);
00148 }
00149 
00151 void
00152 pcl_ros::Feature::config_callback (FeatureConfig &config, uint32_t level)
00153 {
00154   if (k_ != config.k_search)
00155   {
00156     k_ = config.k_search;
00157     NODELET_DEBUG ("[config_callback] Setting the number of K nearest neighbors to use for each point: %d.", k_);
00158   }
00159   if (search_radius_ != config.radius_search)
00160   {
00161     search_radius_ = config.radius_search;
00162     NODELET_DEBUG ("[config_callback] Setting the nearest neighbors search radius for each point: %f.", search_radius_);
00163   }
00164 }
00165 
00167 void
00168 pcl_ros::Feature::input_surface_indices_callback (const PointCloudInConstPtr &cloud, 
00169     const PointCloudInConstPtr &cloud_surface, const PointIndicesConstPtr &indices)
00170 {
00171   // No subscribers, no work
00172   if (pub_output_.getNumSubscribers () <= 0)
00173     return;
00174 
00175   // If cloud is given, check if it's valid
00176   if (!isValid (cloud))
00177   {
00178     NODELET_ERROR ("[%s::input_surface_indices_callback] Invalid input!", getName ().c_str ());
00179     emptyPublish (cloud);
00180     return;
00181   }
00182 
00183   // If surface is given, check if it's valid
00184   if (cloud_surface && !isValid (cloud_surface, "surface"))
00185   {
00186     NODELET_ERROR ("[%s::input_surface_indices_callback] Invalid input surface!", getName ().c_str ());
00187     emptyPublish (cloud);
00188     return;
00189   }
00190     
00191   // If indices are given, check if they are valid
00192   if (indices && !isValid (indices))
00193   {
00194     NODELET_ERROR ("[%s::input_surface_indices_callback] Invalid input indices!", getName ().c_str ());
00195     emptyPublish (cloud);
00196     return;
00197   }
00198 
00200   if (cloud_surface)
00201     if (indices)
00202       NODELET_DEBUG ("[input_surface_indices_callback]\n"
00203                      "                                         - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00204                      "                                         - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00205                      "                                         - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
00206                  cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
00207                  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 (),
00208                  indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
00209     else
00210       NODELET_DEBUG ("[input_surface_indices_callback]\n"
00211                      "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00212                      "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
00213                      cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
00214                      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 ());
00215 
00216   else
00217     if (indices)
00218       NODELET_DEBUG ("[input_surface_indices_callback]\n"
00219                      "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00220                      "                                 - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
00221                      cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
00222                      indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
00223     else
00224       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 ());
00226 
00227 
00228   if ((int)(cloud->width * cloud->height) < k_)
00229   {
00230     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));
00231     emptyPublish (cloud);
00232     return;
00233   }
00234 
00235   // If indices given...
00236   IndicesConstPtr vindices;
00237   if (indices && !indices->header.frame_id.empty ())
00238     vindices = boost::make_shared <const std::vector<int> > (indices->indices);
00239 
00240   computePublish (cloud, cloud_surface, vindices);
00241 }
00242 
00246 
00248 void
00249 pcl_ros::FeatureFromNormals::onInit ()
00250 {
00251   // Call the super onInit ()
00252   PCLNodelet::onInit ();
00253 
00254   // Call the child init
00255   childInit (*pnh_);
00256 
00257   // Allow each individual class that inherits from us to declare their own Publisher
00258   // This is useful for Publisher<pcl::PointCloud<T> >, as NormalEstimation can publish PointCloud<Normal>, etc
00259   //pub_output_ = pnh_->template advertise<PointCloud2> ("output", max_queue_size_);
00260 
00261   // ---[ Mandatory parameters
00262   if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("radius_search", search_radius_))
00263   {
00264     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 ());
00265     return;
00266   }
00267   if (!pnh_->getParam ("spatial_locator", spatial_locator_type_))
00268   {
00269     NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ());
00270     return;
00271   }
00272   // ---[ Optional parameters
00273   pnh_->getParam ("use_surface", use_surface_);
00274 
00275   sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
00276   sub_normals_filter_.subscribe (*pnh_, "normals", max_queue_size_);
00277 
00278   // Enable the dynamic reconfigure service
00279   srv_ = boost::make_shared<dynamic_reconfigure::Server<FeatureConfig> > (*pnh_);
00280   dynamic_reconfigure::Server<FeatureConfig>::CallbackType f = boost::bind (&FeatureFromNormals::config_callback, this, _1, _2);
00281   srv_->setCallback (f);
00282 
00283   // Create the objects here
00284   if (approximate_sync_)
00285     sync_input_normals_surface_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn, PointCloudN, PointCloudIn, PointIndices> > > (max_queue_size_);
00286   else
00287     sync_input_normals_surface_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, PointCloudN, PointCloudIn, PointIndices> > > (max_queue_size_);
00288 
00289   // If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages
00290   if (use_indices_ || use_surface_)
00291   {
00292     if (use_indices_)
00293     {
00294       // If indices are enabled, subscribe to the indices
00295       sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
00296       if (use_surface_)     // Use both indices and surface
00297       {
00298         // If surface is enabled, subscribe to the surface, connect the input-indices-surface trio and register
00299         sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_);
00300         if (approximate_sync_)
00301           sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, sub_indices_filter_);
00302         else
00303           sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, sub_indices_filter_);
00304       }
00305       else                  // Use only indices
00306       {
00307         sub_input_filter_.registerCallback (bind (&FeatureFromNormals::input_callback, this, _1));
00308         if (approximate_sync_)
00309           // surface not enabled, connect the input-indices duo and register
00310           sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, sub_indices_filter_);
00311         else
00312           // surface not enabled, connect the input-indices duo and register
00313           sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, sub_indices_filter_);
00314       }
00315     }
00316     else                    // Use only surface
00317     {
00318       // indices not enabled, connect the input-surface duo and register
00319       sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_);
00320 
00321       sub_input_filter_.registerCallback (bind (&FeatureFromNormals::input_callback, this, _1));
00322       if (approximate_sync_)
00323         sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, nf_pi_);
00324       else
00325         sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, nf_pi_);
00326     }
00327   }
00328   else
00329   {
00330     sub_input_filter_.registerCallback (bind (&FeatureFromNormals::input_callback, this, _1));
00331 
00332     if (approximate_sync_)
00333       sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, nf_pi_);
00334     else
00335       sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, nf_pi_);
00336   }
00337 
00338   // Register callbacks
00339   if (approximate_sync_)
00340     sync_input_normals_surface_indices_a_->registerCallback (bind (&FeatureFromNormals::input_normals_surface_indices_callback, this, _1, _2, _3, _4));
00341   else
00342     sync_input_normals_surface_indices_e_->registerCallback (bind (&FeatureFromNormals::input_normals_surface_indices_callback, this, _1, _2, _3, _4));
00343 
00344   NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
00345                  " - use_surface    : %s\n"
00346                  " - k_search       : %d\n"
00347                  " - radius_search  : %f\n"
00348                  " - spatial_locator: %d",
00349                 getName ().c_str (),
00350                  (use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_);
00351 }
00352 
00354 void
00355 pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback (
00356    const PointCloudInConstPtr &cloud, const PointCloudNConstPtr &cloud_normals,
00357    const PointCloudInConstPtr &cloud_surface, const PointIndicesConstPtr &indices)
00358 {
00359   // No subscribers, no work
00360   if (pub_output_.getNumSubscribers () <= 0)
00361     return;
00362 
00363   // If cloud+normals is given, check if it's valid
00364   if (!isValid (cloud))// || !isValid (cloud_normals, "normals"))
00365   {
00366     NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input!", getName ().c_str ());
00367     emptyPublish (cloud);
00368     return;
00369   }
00370 
00371   // If surface is given, check if it's valid
00372   if (cloud_surface && !isValid (cloud_surface, "surface"))
00373   {
00374     NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input surface!", getName ().c_str ());
00375     emptyPublish (cloud);
00376     return;
00377   }
00378     
00379   // If indices are given, check if they are valid
00380   if (indices && !isValid (indices))
00381   {
00382     NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input indices!", getName ().c_str ());
00383     emptyPublish (cloud);
00384     return;
00385   }
00386 
00388   if (cloud_surface)
00389     if (indices)
00390       NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n"
00391                      "                                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\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                      "                                                 - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
00395                      getName ().c_str (),
00396                      cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
00397                      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 (),
00398                      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 (),
00399                      indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
00400     else
00401       NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n"
00402                      "                                         - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\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.",
00405                      getName ().c_str (), 
00406                      cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
00407                      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 (),
00408                      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 ());
00409   else
00410     if (indices)
00411       NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n"
00412                      "                                         - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00413                      "                                         - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00414                      "                                         - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
00415                      getName ().c_str (),
00416                      cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
00417                      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 (),
00418                      indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
00419     else
00420       NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n"
00421                      "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00422                      "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
00423                      getName ().c_str (), 
00424                      cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
00425                      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 ());
00427 
00428   if ((int)(cloud->width * cloud->height) < k_)
00429   {
00430     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));
00431     emptyPublish (cloud);
00432     return;
00433   }
00434 
00435   // If indices given...
00436   IndicesConstPtr vindices;
00437   if (indices && !indices->header.frame_id.empty ())
00438     vindices = boost::make_shared <const std::vector<int> > (indices->indices);
00439 
00440   computePublish (cloud, cloud_normals, cloud_surface, vindices);
00441 }
00442