sac_segmentation.cpp
Go to the documentation of this 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 OWNER 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: sac_segmentation.hpp 33195 2010-10-10 14:12:19Z marton $
00035  *
00036  */
00037 
00038 #include <pluginlib/class_list_macros.h>
00039 #include "pcl_ros/segmentation/sac_segmentation.h"
00040 #include <pcl/io/io.h>
00041 
00043 void
00044 pcl_ros::SACSegmentation::onInit ()
00045 {
00046   // Call the super onInit ()
00047   PCLNodelet::onInit ();
00048 
00049   // If we're supposed to look for PointIndices (indices)
00050   if (use_indices_)
00051   {
00052     // Subscribe to the input using a filter
00053     sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
00054     sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
00055 
00056     // when "use_indices" is set to true, and "latched_indices" is set to true,
00057     // we'll subscribe and get a separate callback for PointIndices that will 
00058     // save the indices internally, and a PointCloud + PointIndices callback 
00059     // will take care of meshing the new PointClouds with the old saved indices. 
00060     if (latched_indices_)
00061     {
00062       // Subscribe to a callback that saves the indices
00063       sub_indices_filter_.registerCallback (bind (&SACSegmentation::indices_callback, this, _1));
00064       // Subscribe to a callback that sets the header of the saved indices to the cloud header
00065       sub_input_filter_.registerCallback (bind (&SACSegmentation::input_callback, this, _1));
00066 
00067       // Synchronize the two topics. No need for an approximate synchronizer here, as we'll
00068       // match the timestamps exactly
00069       sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointIndices> > > (max_queue_size_);
00070       sync_input_indices_e_->connectInput (sub_input_filter_, nf_pi_);
00071       sync_input_indices_e_->registerCallback (bind (&SACSegmentation::input_indices_callback, this, _1, _2));
00072     }
00073     // "latched_indices" not set, proceed with regular <input,indices> pairs
00074     else
00075     {
00076       if (approximate_sync_)
00077       {
00078         sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointIndices> > > (max_queue_size_);
00079         sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_);
00080         sync_input_indices_a_->registerCallback (bind (&SACSegmentation::input_indices_callback, this, _1, _2));
00081       }
00082       else
00083       {
00084         sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointIndices> > > (max_queue_size_);
00085         sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_);
00086         sync_input_indices_e_->registerCallback (bind (&SACSegmentation::input_indices_callback, this, _1, _2));
00087       }
00088     }
00089   }
00090   else
00091     // Subscribe in an old fashion to input only (no filters)
00092     sub_input_ = pnh_->subscribe<PointCloud> ("input", max_queue_size_,  bind (&SACSegmentation::input_indices_callback, this, _1, PointIndicesConstPtr ()));
00093 
00094   // Advertise the output topics
00095   pub_indices_ = pnh_->advertise<PointIndices> ("inliers", max_queue_size_);
00096   pub_model_   = pnh_->advertise<ModelCoefficients> ("model", max_queue_size_);
00097 
00098   // ---[ Mandatory parameters
00099   int model_type;
00100   if (!pnh_->getParam ("model_type", model_type))
00101   {
00102     NODELET_ERROR ("[onInit] Need a 'model_type' parameter to be set before continuing!");
00103     return;
00104   }
00105   double threshold; // unused - set via dynamic reconfigure in the callback
00106   if (!pnh_->getParam ("distance_threshold", threshold))
00107   {
00108     NODELET_ERROR ("[onInit] Need a 'distance_threshold' parameter to be set before continuing!");
00109     return;
00110   }
00111 
00112   // ---[ Optional parameters
00113   int method_type = 0;
00114   pnh_->getParam ("method_type", method_type);
00115 
00116   XmlRpc::XmlRpcValue axis_param;
00117   pnh_->getParam ("axis", axis_param);
00118   Eigen::Vector3f axis = Eigen::Vector3f::Zero ();
00119 
00120   switch (axis_param.getType ())
00121   {
00122     case XmlRpc::XmlRpcValue::TypeArray:
00123     {
00124       if (axis_param.size () != 3)
00125       {
00126         NODELET_ERROR ("[%s::onInit] Parameter 'axis' given but with a different number of values (%d) than required (3)!", getName ().c_str (), axis_param.size ());
00127         return;
00128       }
00129       for (int i = 0; i < 3; ++i)
00130       {
00131         if (axis_param[i].getType () != XmlRpc::XmlRpcValue::TypeDouble)
00132         {
00133           NODELET_ERROR ("[%s::onInit] Need floating point values for 'axis' parameter.", getName ().c_str ());
00134           return;
00135         }
00136         double value = axis_param[i]; axis[i] = value;
00137       }
00138       break;
00139     }
00140     default:
00141     {
00142       break;
00143     }
00144   }
00145 
00146   // Initialize the random number generator
00147   srand (time (0));
00148 
00149   // Enable the dynamic reconfigure service
00150   srv_ = boost::make_shared<dynamic_reconfigure::Server<SACSegmentationConfig> >(*pnh_);
00151   dynamic_reconfigure::Server<SACSegmentationConfig>::CallbackType f =  boost::bind (&SACSegmentation::config_callback, this, _1, _2);
00152   srv_->setCallback (f);
00153 
00154   NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
00155                  " - model_type               : %d\n"
00156                  " - method_type              : %d\n"
00157                  " - model_threshold          : %f\n"
00158                  " - axis                     : [%f, %f, %f]\n",
00159                  getName ().c_str (), model_type, method_type, threshold, 
00160                  axis[0], axis[1], axis[2]);
00161 
00162   // Set given parameters here
00163   impl_.setModelType (model_type);
00164   impl_.setMethodType (method_type);
00165   impl_.setAxis (axis);
00166 }
00167 
00169 void
00170 pcl_ros::SACSegmentation::config_callback (SACSegmentationConfig &config, uint32_t level)
00171 {
00172   boost::mutex::scoped_lock lock (mutex_);
00173 
00174   if (impl_.getDistanceThreshold () != config.distance_threshold)
00175   {
00176     //sac_->setDistanceThreshold (threshold_); - done in initSAC
00177     impl_.setDistanceThreshold (config.distance_threshold);
00178     NODELET_DEBUG ("[%s::config_callback] Setting new distance to model threshold to: %f.", getName ().c_str (), config.distance_threshold);
00179   }
00180   // The maximum allowed difference between the model normal and the given axis _in radians_
00181   if (impl_.getEpsAngle () != config.eps_angle)
00182   {
00183     impl_.setEpsAngle (config.eps_angle);
00184     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);
00185   }
00186   
00187   // Number of inliers
00188   if (min_inliers_ != config.min_inliers)
00189   {
00190     min_inliers_ = config.min_inliers;
00191     NODELET_DEBUG ("[%s::config_callback] Setting new minimum number of inliers to: %d.",  getName ().c_str (), min_inliers_);
00192   }
00193 
00194   if (impl_.getMaxIterations () != config.max_iterations)
00195   {
00196     //sac_->setMaxIterations (max_iterations_); - done in initSAC
00197     impl_.setMaxIterations (config.max_iterations);
00198     NODELET_DEBUG ("[%s::config_callback] Setting new maximum number of iterations to: %d.",  getName ().c_str (), config.max_iterations);
00199   }
00200   if (impl_.getProbability () != config.probability)
00201   {
00202     //sac_->setProbability (probability_); - done in initSAC
00203     impl_.setProbability (config.probability);
00204     NODELET_DEBUG ("[%s::config_callback] Setting new probability to: %f.", getName ().c_str (), config.probability);
00205   }
00206   if (impl_.getOptimizeCoefficients () != config.optimize_coefficients)
00207   {
00208     impl_.setOptimizeCoefficients (config.optimize_coefficients);
00209     NODELET_DEBUG ("[%s::config_callback] Setting coefficient optimization to: %s.", getName ().c_str (), (config.optimize_coefficients) ? "true" : "false");
00210   }
00211 
00212   double radius_min, radius_max;
00213   impl_.getRadiusLimits (radius_min, radius_max);
00214   if (radius_min != config.radius_min)
00215   {
00216     radius_min = config.radius_min;
00217     NODELET_DEBUG ("[config_callback] Setting minimum allowable model radius to: %f.", radius_min);
00218     impl_.setRadiusLimits (radius_min, radius_max);
00219   }
00220   if (radius_max != config.radius_max)
00221   {
00222     radius_max = config.radius_max;
00223     NODELET_DEBUG ("[config_callback] Setting maximum allowable model radius to: %f.", radius_max);
00224     impl_.setRadiusLimits (radius_min, radius_max);
00225   }
00226 
00227   if (tf_input_frame_ != config.input_frame)
00228   {
00229     tf_input_frame_ = config.input_frame;
00230     NODELET_DEBUG ("[config_callback] Setting the input TF frame to: %s.", tf_input_frame_.c_str ());
00231     NODELET_WARN ("input_frame TF not implemented yet!");
00232   }
00233   if (tf_output_frame_ != config.output_frame)
00234   {
00235     tf_output_frame_ = config.output_frame;
00236     NODELET_DEBUG ("[config_callback] Setting the output TF frame to: %s.", tf_output_frame_.c_str ());
00237     NODELET_WARN ("output_frame TF not implemented yet!");
00238   }
00239 }
00240 
00242 void
00243 pcl_ros::SACSegmentation::input_indices_callback (const PointCloudConstPtr &cloud, 
00244                                                   const PointIndicesConstPtr &indices)
00245 {
00246   boost::mutex::scoped_lock lock (mutex_);
00247 
00248   // No subscribers, no work
00249   if (pub_indices_.getNumSubscribers () <= 0 && pub_model_.getNumSubscribers () <= 0)
00250     return;
00251 
00252   PointIndices inliers;
00253   ModelCoefficients model;
00254   // Enforce that the TF frame and the timestamp are copied
00255   inliers.header = model.header = cloud->header;
00256 
00257   // If cloud is given, check if it's valid
00258   if (!isValid (cloud)) 
00259   {
00260     NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
00261     pub_indices_.publish (boost::make_shared<const PointIndices> (inliers));
00262     pub_model_.publish (boost::make_shared<const ModelCoefficients> (model));
00263     return;
00264   }
00265   // If indices are given, check if they are valid
00266   if (indices && !isValid (indices))
00267   {
00268     NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
00269     pub_indices_.publish (boost::make_shared<const PointIndices> (inliers));
00270     pub_model_.publish (boost::make_shared<const ModelCoefficients> (model));
00271     return;
00272   }
00273 
00275   if (indices && !indices->header.frame_id.empty ())
00276     NODELET_DEBUG ("[%s::input_indices_callback]\n"
00277                    "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00278                    "                                 - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
00279                    getName ().c_str (), 
00280                    cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
00281                    indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
00282   else
00283     NODELET_DEBUG ("[%s::input_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", 
00284                    getName ().c_str (), cloud->width * cloud->height, cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ());
00286 
00287   // Check whether the user has given a different input TF frame
00288   tf_input_orig_frame_ = cloud->header.frame_id;
00289   PointCloudConstPtr cloud_tf;
00290 /*  if (!tf_input_frame_.empty () && cloud->header.frame_id != tf_input_frame_)
00291   {
00292     NODELET_DEBUG ("[input_callback] Transforming input dataset from %s to %s.", cloud->header.frame_id.c_str (), tf_input_frame_.c_str ());
00293     // Save the original frame ID
00294     // Convert the cloud into the different frame
00295     PointCloud cloud_transformed;
00296     if (!pcl::transformPointCloud (tf_input_frame_, cloud->header.stamp, *cloud, cloud_transformed, tf_listener_))
00297       return;
00298     cloud_tf.reset (new PointCloud (cloud_transformed));
00299   }
00300   else*/
00301     cloud_tf = cloud;
00302 
00303   IndicesPtr indices_ptr;
00304   if (indices && !indices->header.frame_id.empty ())
00305     indices_ptr.reset (new std::vector<int> (indices->indices));
00306 
00307   impl_.setInputCloud (cloud_tf);
00308   impl_.setIndices (indices_ptr);
00309 
00310   // Final check if the data is empty (remember that indices are set to the size of the data -- if indices* = NULL)
00311   if (!cloud->points.empty ())
00312     impl_.segment (inliers, model);
00313 
00314   // Probably need to transform the model of the plane here
00315 
00316   // Check if we have enough inliers, clear inliers + model if not
00317   if ((int)inliers.indices.size () <= min_inliers_)
00318   {
00319     inliers.indices.clear (); 
00320     model.values.clear ();
00321   }
00322 
00323   // Publish
00324   pub_indices_.publish (boost::make_shared<const PointIndices> (inliers));
00325   pub_model_.publish (boost::make_shared<const ModelCoefficients> (model));
00326   NODELET_DEBUG ("[%s::input_indices_callback] Published PointIndices with %zu values on topic %s, and ModelCoefficients with %zu values on topic %s",
00327                  getName ().c_str (), inliers.indices.size (), pnh_->resolveName ("inliers").c_str (),
00328                  model.values.size (), pnh_->resolveName ("model").c_str ());
00329 
00330   if (inliers.indices.empty ())
00331     NODELET_WARN ("[%s::input_indices_callback] No inliers found!", getName ().c_str ());
00332 }
00333 
00335 void
00336 pcl_ros::SACSegmentationFromNormals::onInit ()
00337 {
00338   // Call the super onInit ()
00339   PCLNodelet::onInit ();
00340 
00341   // Enable the dynamic reconfigure service
00342   srv_ = boost::make_shared <dynamic_reconfigure::Server<SACSegmentationFromNormalsConfig> > (*pnh_);
00343   dynamic_reconfigure::Server<SACSegmentationFromNormalsConfig>::CallbackType f = boost::bind (&SACSegmentationFromNormals::config_callback, this, _1, _2);
00344   srv_->setCallback (f);
00345 
00346   // Subscribe to the input and normals using filters
00347   sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
00348   sub_normals_filter_.subscribe (*pnh_, "normals", max_queue_size_);
00349 
00350   // Subscribe to an axis direction along which the model search is to be constrained (the first 3 model coefficients will be checked)
00351   sub_axis_ = pnh_->subscribe ("axis", 1, &SACSegmentationFromNormals::axis_callback, this);
00352 
00353   if (approximate_sync_)
00354     sync_input_normals_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointCloudN, PointIndices> > > (max_queue_size_);
00355   else
00356     sync_input_normals_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloudN, PointIndices> > > (max_queue_size_);
00357 
00358   // If we're supposed to look for PointIndices (indices)
00359   if (use_indices_)
00360   {
00361     // Subscribe to the input using a filter
00362     sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
00363 
00364     if (approximate_sync_)
00365       sync_input_normals_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, sub_indices_filter_);
00366     else
00367       sync_input_normals_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, sub_indices_filter_);
00368   }
00369   else
00370   {
00371     // Create a different callback for copying over the timestamp to fake indices
00372     sub_input_filter_.registerCallback (bind (&SACSegmentationFromNormals::input_callback, this, _1));
00373 
00374     if (approximate_sync_)
00375       sync_input_normals_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, nf_);
00376     else
00377       sync_input_normals_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, nf_);
00378   }
00379 
00380   if (approximate_sync_)
00381     sync_input_normals_indices_a_->registerCallback (bind (&SACSegmentationFromNormals::input_normals_indices_callback, this, _1, _2, _3));
00382   else
00383     sync_input_normals_indices_e_->registerCallback (bind (&SACSegmentationFromNormals::input_normals_indices_callback, this, _1, _2, _3));
00384 
00385   // Advertise the output topics
00386   pub_indices_ = pnh_->advertise<PointIndices> ("inliers", max_queue_size_);
00387   pub_model_   = pnh_->advertise<ModelCoefficients> ("model", max_queue_size_);
00388 
00389   // ---[ Mandatory parameters
00390   int model_type;
00391   if (!pnh_->getParam ("model_type", model_type))
00392   {
00393     NODELET_ERROR ("[%s::onInit] Need a 'model_type' parameter to be set before continuing!", getName ().c_str ());
00394     return;
00395   }
00396   double threshold; // unused - set via dynamic reconfigure in the callback
00397   if (!pnh_->getParam ("distance_threshold", threshold))
00398   {
00399     NODELET_ERROR ("[%s::onInit] Need a 'distance_threshold' parameter to be set before continuing!", getName ().c_str ());
00400     return;
00401   }
00402 
00403   // ---[ Optional parameters
00404   int method_type = 0;
00405   pnh_->getParam ("method_type", method_type);
00406 
00407   XmlRpc::XmlRpcValue axis_param;
00408   pnh_->getParam ("axis", axis_param);
00409   Eigen::Vector3f axis = Eigen::Vector3f::Zero ();
00410 
00411   switch (axis_param.getType ())
00412   {
00413     case XmlRpc::XmlRpcValue::TypeArray:
00414     {
00415       if (axis_param.size () != 3)
00416       {
00417         NODELET_ERROR ("[%s::onInit] Parameter 'axis' given but with a different number of values (%d) than required (3)!", getName ().c_str (), axis_param.size ());
00418         return;
00419       }
00420       for (int i = 0; i < 3; ++i)
00421       {
00422         if (axis_param[i].getType () != XmlRpc::XmlRpcValue::TypeDouble)
00423         {
00424           NODELET_ERROR ("[%s::onInit] Need floating point values for 'axis' parameter.", getName ().c_str ());
00425           return;
00426         }
00427         double value = axis_param[i]; axis[i] = value;
00428       }
00429       break;
00430     }
00431     default:
00432     {
00433       break;
00434     }
00435   }
00436 
00437   // Initialize the random number generator
00438   srand (time (0));
00439 
00440   NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
00441                  " - model_type               : %d\n"
00442                  " - method_type              : %d\n"
00443                  " - model_threshold          : %f\n"
00444                  " - axis                     : [%f, %f, %f]\n",
00445                  getName ().c_str (), model_type, method_type, threshold, 
00446                  axis[0], axis[1], axis[2]);
00447 
00448   // Set given parameters here
00449   impl_.setModelType (model_type);
00450   impl_.setMethodType (method_type);
00451   impl_.setAxis (axis);
00452 }
00453 
00455 void
00456 pcl_ros::SACSegmentationFromNormals::axis_callback (const pcl::ModelCoefficientsConstPtr &model)
00457 {
00458   boost::mutex::scoped_lock lock (mutex_);
00459 
00460   if (model->values.size () < 3)
00461   {
00462     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 ());
00463     return;
00464   }
00465   NODELET_DEBUG ("[%s::axis_callback] Received axis direction: %f %f %f", getName ().c_str (), model->values[0], model->values[1], model->values[2]);
00466 
00467   Eigen::Vector3f axis (model->values[0], model->values[1], model->values[2]);
00468   impl_.setAxis (axis);
00469 }
00470 
00472 void
00473 pcl_ros::SACSegmentationFromNormals::config_callback (SACSegmentationFromNormalsConfig &config, uint32_t level)
00474 {
00475   boost::mutex::scoped_lock lock (mutex_);
00476 
00477   if (impl_.getDistanceThreshold () != config.distance_threshold)
00478   {
00479     impl_.setDistanceThreshold (config.distance_threshold);
00480     NODELET_DEBUG ("[%s::config_callback] Setting distance to model threshold to: %f.", getName ().c_str (), config.distance_threshold);
00481   }
00482   // The maximum allowed difference between the model normal and the given axis _in radians_
00483   if (impl_.getEpsAngle () != config.eps_angle)
00484   {
00485     impl_.setEpsAngle (config.eps_angle);
00486     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);
00487   }
00488 
00489   if (impl_.getMaxIterations () != config.max_iterations)
00490   {
00491     impl_.setMaxIterations (config.max_iterations);
00492     NODELET_DEBUG ("[%s::config_callback] Setting new maximum number of iterations to: %d.", getName ().c_str (), config.max_iterations);
00493   }
00494   
00495   // Number of inliers
00496   if (min_inliers_ != config.min_inliers)
00497   {
00498     min_inliers_ = config.min_inliers;
00499     NODELET_DEBUG ("[%s::config_callback] Setting new minimum number of inliers to: %d.",  getName ().c_str (), min_inliers_);
00500   }
00501 
00502 
00503   if (impl_.getProbability () != config.probability)
00504   {
00505     impl_.setProbability (config.probability);
00506     NODELET_DEBUG ("[%s::config_callback] Setting new probability to: %f.", getName ().c_str (), config.probability);
00507   }
00508 
00509   if (impl_.getOptimizeCoefficients () != config.optimize_coefficients)
00510   {
00511     impl_.setOptimizeCoefficients (config.optimize_coefficients);
00512     NODELET_DEBUG ("[%s::config_callback] Setting coefficient optimization to: %s.", getName ().c_str (), (config.optimize_coefficients) ? "true" : "false");
00513   }
00514 
00515   if (impl_.getNormalDistanceWeight () != config.normal_distance_weight)
00516   {
00517     impl_.setNormalDistanceWeight (config.normal_distance_weight);
00518     NODELET_DEBUG ("[%s::config_callback] Setting new distance weight to: %f.", getName ().c_str (), config.normal_distance_weight);
00519   }
00520 
00521   double radius_min, radius_max;
00522   impl_.getRadiusLimits (radius_min, radius_max);
00523   if (radius_min != config.radius_min)
00524   {
00525     radius_min = config.radius_min;
00526     NODELET_DEBUG ("[%s::config_callback] Setting minimum allowable model radius to: %f.", getName ().c_str (), radius_min);
00527     impl_.setRadiusLimits (radius_min, radius_max);
00528   }
00529   if (radius_max != config.radius_max)
00530   {
00531     radius_max = config.radius_max;
00532     NODELET_DEBUG ("[%s::config_callback] Setting maximum allowable model radius to: %f.", getName ().c_str (), radius_max);
00533     impl_.setRadiusLimits (radius_min, radius_max);
00534   }
00535 }
00536 
00538 void
00539 pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback (
00540       const PointCloudConstPtr &cloud, 
00541       const PointCloudNConstPtr &cloud_normals,
00542       const PointIndicesConstPtr &indices
00543       )
00544 {
00545   boost::mutex::scoped_lock lock (mutex_);
00546 
00547   // No subscribers, no work
00548   if (pub_indices_.getNumSubscribers () <= 0 && pub_model_.getNumSubscribers () <= 0)
00549     return;
00550 
00551   PointIndices inliers;
00552   ModelCoefficients model;
00553   // Enforce that the TF frame and the timestamp are copied
00554   inliers.header = model.header = cloud->header;
00555 
00556   if (impl_.getModelType () < 0)
00557   {
00558     NODELET_ERROR ("[%s::input_normals_indices_callback] Model type not set!", getName ().c_str ());
00559     pub_indices_.publish (boost::make_shared<const PointIndices> (inliers));
00560     pub_model_.publish (boost::make_shared<const ModelCoefficients> (model));
00561     return;
00562   }
00563 
00564   if (!isValid (cloud))// || !isValid (cloud_normals, "normals")) 
00565   {
00566     NODELET_ERROR ("[%s::input_normals_indices_callback] Invalid input!", getName ().c_str ());
00567     pub_indices_.publish (boost::make_shared<const PointIndices> (inliers));
00568     pub_model_.publish (boost::make_shared<const ModelCoefficients> (model));
00569     return;
00570   }
00571   // If indices are given, check if they are valid
00572   if (indices && !isValid (indices))
00573   {
00574     NODELET_ERROR ("[%s::input_normals_indices_callback] Invalid indices!", getName ().c_str ());
00575     pub_indices_.publish (boost::make_shared<const PointIndices> (inliers));
00576     pub_model_.publish (boost::make_shared<const ModelCoefficients> (model));
00577     return;
00578   }
00579 
00581   if (indices && !indices->header.frame_id.empty ())
00582     NODELET_DEBUG ("[%s::input_normals_indices_callback]\n"
00583                    "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00584                    "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00585                    "                                 - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
00586                    getName ().c_str (), 
00587                    cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
00588                    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 (),
00589                    indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
00590   else
00591     NODELET_DEBUG ("[%s::input_normals_indices_callback]\n"
00592                    "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00593                    "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
00594                    getName ().c_str (), 
00595                    cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
00596                    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 ());
00598 
00599 
00600   // Extra checks for safety
00601   int cloud_nr_points         = cloud->width * cloud->height;
00602   int cloud_normals_nr_points = cloud_normals->width * cloud_normals->height;
00603   if (cloud_nr_points != cloud_normals_nr_points)
00604   {
00605     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);
00606     pub_indices_.publish (boost::make_shared<const PointIndices> (inliers));
00607     pub_model_.publish (boost::make_shared<const ModelCoefficients> (model));
00608     return;
00609   }
00610 
00611   impl_.setInputCloud (cloud);
00612   impl_.setInputNormals (cloud_normals);
00613 
00614   IndicesPtr indices_ptr;
00615   if (indices && !indices->header.frame_id.empty ())
00616     indices_ptr.reset (new std::vector<int> (indices->indices));
00617 
00618   impl_.setIndices (indices_ptr);
00619 
00620   // Final check if the data is empty (remember that indices are set to the size of the data -- if indices* = NULL)
00621   if (!cloud->points.empty ())
00622     impl_.segment (inliers, model);
00623 
00624   // Check if we have enough inliers, clear inliers + model if not
00625   if ((int)inliers.indices.size () <= min_inliers_)
00626   {
00627     inliers.indices.clear (); 
00628     model.values.clear ();
00629   }
00630 
00631   // Publish
00632   pub_indices_.publish (boost::make_shared<const PointIndices> (inliers));
00633   pub_model_.publish (boost::make_shared<const ModelCoefficients> (model));
00634   NODELET_DEBUG ("[%s::input_normals_callback] Published PointIndices with %zu values on topic %s, and ModelCoefficients with %zu values on topic %s", 
00635       getName ().c_str (), inliers.indices.size (), pnh_->resolveName ("inliers").c_str (),
00636       model.values.size (), pnh_->resolveName ("model").c_str ());
00637   if (inliers.indices.empty ())
00638     NODELET_WARN ("[%s::input_indices_callback] No inliers found!", getName ().c_str ());
00639 }
00640 
00641 typedef pcl_ros::SACSegmentation SACSegmentation;
00642 typedef pcl_ros::SACSegmentationFromNormals SACSegmentationFromNormals;
00643 PLUGINLIB_DECLARE_CLASS (pcl, SACSegmentation, SACSegmentation, nodelet::Nodelet);
00644 PLUGINLIB_DECLARE_CLASS (pcl, SACSegmentationFromNormals, SACSegmentationFromNormals, nodelet::Nodelet);
00645 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


pcl_ros
Author(s): Maintained by Radu Bogdan Rusu
autogenerated on Tue Mar 5 2013 13:54:41