seeded_hue_segmentation.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2010-2011, Willow Garage, Inc.
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of the copyright holder(s) nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * $id $
00037  */
00038 
00039 #ifndef PCL_SEGMENTATION_IMPL_SEEDED_HUE_SEGMENTATION_H_
00040 #define PCL_SEGMENTATION_IMPL_SEEDED_HUE_SEGMENTATION_H_
00041 
00042 #include <pcl/segmentation/seeded_hue_segmentation.h>
00043 
00045 void
00046 pcl::seededHueSegmentation (const PointCloud<PointXYZRGB>                         &cloud, 
00047                             const boost::shared_ptr<search::Search<PointXYZRGB> > &tree,
00048                             float                                                 tolerance, 
00049                             PointIndices                                          &indices_in,
00050                             PointIndices                                          &indices_out,
00051                             float                                                 delta_hue)
00052 {
00053   if (tree->getInputCloud ()->points.size () != cloud.points.size ())
00054   {
00055     PCL_ERROR ("[pcl::seededHueSegmentation] Tree built for a different point cloud dataset (%zu) than the input cloud (%zu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ());
00056     return;
00057   }
00058   // Create a bool vector of processed point indices, and initialize it to false
00059   std::vector<bool> processed (cloud.points.size (), false);
00060 
00061   std::vector<int> nn_indices;
00062   std::vector<float> nn_distances;
00063 
00064   // Process all points in the indices vector
00065   for (size_t k = 0; k < indices_in.indices.size (); ++k)
00066   {
00067     int i = indices_in.indices[k];
00068     if (processed[i])
00069       continue;
00070 
00071     processed[i] = true;
00072 
00073     std::vector<int> seed_queue;
00074     int sq_idx = 0;
00075     seed_queue.push_back (i);
00076 
00077     PointXYZRGB  p;
00078     p = cloud.points[i];
00079     PointXYZHSV h;
00080     PointXYZRGBtoXYZHSV(p, h);
00081 
00082     while (sq_idx < static_cast<int> (seed_queue.size ()))
00083     {
00084       int ret = tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances, std::numeric_limits<int>::max());
00085       if(ret == -1)
00086         PCL_ERROR("[pcl::seededHueSegmentation] radiusSearch returned error code -1");
00087       // Search for sq_idx
00088       if (!ret)
00089       {
00090         sq_idx++;
00091         continue;
00092       }
00093 
00094       for (size_t j = 1; j < nn_indices.size (); ++j)             // nn_indices[0] should be sq_idx
00095       {
00096         if (processed[nn_indices[j]])                             // Has this point been processed before ?
00097           continue;
00098 
00099         PointXYZRGB  p_l;
00100         p_l = cloud.points[nn_indices[j]];
00101         PointXYZHSV h_l;
00102         PointXYZRGBtoXYZHSV(p_l, h_l);
00103 
00104         if (fabs(h_l.h - h.h) < delta_hue)
00105         {
00106           seed_queue.push_back (nn_indices[j]);
00107           processed[nn_indices[j]] = true;
00108         }
00109       }
00110 
00111       sq_idx++;
00112     }
00113     // Copy the seed queue into the output indices
00114     for (size_t l = 0; l < seed_queue.size (); ++l)
00115       indices_out.indices.push_back(seed_queue[l]);
00116   }
00117   // This is purely esthetical, can be removed for speed purposes
00118   std::sort (indices_out.indices.begin (), indices_out.indices.end ());
00119 }
00121 void
00122 pcl::seededHueSegmentation (const PointCloud<PointXYZRGB>                           &cloud, 
00123                             const boost::shared_ptr<search::Search<PointXYZRGBL> >  &tree,
00124                             float                                                   tolerance, 
00125                             PointIndices                                            &indices_in,
00126                             PointIndices                                            &indices_out,
00127                             float                                                   delta_hue)
00128 {
00129   if (tree->getInputCloud ()->points.size () != cloud.points.size ())
00130   {
00131     PCL_ERROR ("[pcl::seededHueSegmentation] Tree built for a different point cloud dataset (%zu) than the input cloud (%zu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ());
00132     return;
00133   }
00134   // Create a bool vector of processed point indices, and initialize it to false
00135   std::vector<bool> processed (cloud.points.size (), false);
00136 
00137   std::vector<int> nn_indices;
00138   std::vector<float> nn_distances;
00139 
00140   // Process all points in the indices vector
00141   for (size_t k = 0; k < indices_in.indices.size (); ++k)
00142   {
00143     int i = indices_in.indices[k];
00144     if (processed[i])
00145       continue;
00146 
00147     processed[i] = true;
00148 
00149     std::vector<int> seed_queue;
00150     int sq_idx = 0;
00151     seed_queue.push_back (i);
00152 
00153     PointXYZRGB  p;
00154     p = cloud.points[i];
00155     PointXYZHSV h;
00156     PointXYZRGBtoXYZHSV(p, h);
00157 
00158     while (sq_idx < static_cast<int> (seed_queue.size ()))
00159     {
00160       int ret = tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances, std::numeric_limits<int>::max());
00161       if(ret == -1)
00162         PCL_ERROR("[pcl::seededHueSegmentation] radiusSearch returned error code -1");
00163       // Search for sq_idx
00164       if (!ret)
00165       {
00166         sq_idx++;
00167         continue;
00168       }
00169       for (size_t j = 1; j < nn_indices.size (); ++j)             // nn_indices[0] should be sq_idx
00170       {
00171         if (processed[nn_indices[j]])                             // Has this point been processed before ?
00172           continue;
00173 
00174         PointXYZRGB  p_l;
00175         p_l = cloud.points[nn_indices[j]];
00176         PointXYZHSV h_l;
00177         PointXYZRGBtoXYZHSV(p_l, h_l);
00178 
00179         if (fabs(h_l.h - h.h) < delta_hue)
00180         {
00181           seed_queue.push_back (nn_indices[j]);
00182           processed[nn_indices[j]] = true;
00183         }
00184       }
00185 
00186       sq_idx++;
00187     }
00188     // Copy the seed queue into the output indices
00189     for (size_t l = 0; l < seed_queue.size (); ++l)
00190       indices_out.indices.push_back(seed_queue[l]);
00191   }
00192   // This is purely esthetical, can be removed for speed purposes
00193   std::sort (indices_out.indices.begin (), indices_out.indices.end ());
00194 }
00197 
00198 void 
00199 pcl::SeededHueSegmentation::segment (PointIndices &indices_in, PointIndices &indices_out)
00200 {
00201   if (!initCompute () || 
00202       (input_ != 0   && input_->points.empty ()) ||
00203       (indices_ != 0 && indices_->empty ()))
00204   {
00205     indices_out.indices.clear ();
00206     return;
00207   }
00208 
00209   // Initialize the spatial locator
00210   if (!tree_)
00211   {
00212     if (input_->isOrganized ())
00213       tree_.reset (new pcl::search::OrganizedNeighbor<PointXYZRGB> ());
00214     else
00215       tree_.reset (new pcl::search::KdTree<PointXYZRGB> (false));
00216   }
00217 
00218   // Send the input dataset to the spatial locator
00219   tree_->setInputCloud (input_);
00220   seededHueSegmentation (*input_, tree_, static_cast<float> (cluster_tolerance_), indices_in, indices_out, delta_hue_);
00221   deinitCompute ();
00222 }
00223 
00224 #endif        // PCL_EXTRACT_CLUSTERS_IMPL_H_


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:33:12