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


pcl_ros
Author(s): Open Perception, Julius Kammerl , William Woodall
autogenerated on Wed Aug 26 2015 15:25:31