Point Cloud Library (PCL)  1.7.2
supervoxel_clustering.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  *
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Willow Garage, Inc. nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *
35  * Author : jpapon@gmail.com
36  * Email : jpapon@gmail.com
37  *
38  */
39 
40 #ifndef PCL_SEGMENTATION_SUPERVOXEL_CLUSTERING_HPP_
41 #define PCL_SEGMENTATION_SUPERVOXEL_CLUSTERING_HPP_
42 
43 #include <pcl/segmentation/supervoxel_clustering.h>
44 
45 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
46 template <typename PointT>
47 pcl::SupervoxelClustering<PointT>::SupervoxelClustering (float voxel_resolution, float seed_resolution, bool use_single_camera_transform) :
48  resolution_ (voxel_resolution),
49  seed_resolution_ (seed_resolution),
50  adjacency_octree_ (),
51  voxel_centroid_cloud_ (),
52  color_importance_(0.1f),
53  spatial_importance_(0.4f),
54  normal_importance_(1.0f),
55  label_colors_ (0)
56 {
57  adjacency_octree_.reset (new OctreeAdjacencyT (resolution_));
58  if (use_single_camera_transform)
59  adjacency_octree_->setTransformFunction (boost::bind (&SupervoxelClustering::transformFunction, this, _1));
60 }
61 
62 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
63 template <typename PointT>
65 {
66 
67 }
68 
69 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
70 template <typename PointT> void
72 {
73  if ( cloud->size () == 0 )
74  {
75  PCL_ERROR ("[pcl::SupervoxelClustering::setInputCloud] Empty cloud set, doing nothing \n");
76  return;
77  }
78 
79  input_ = cloud;
80  adjacency_octree_->setInputCloud (cloud);
81 }
82 
83 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
84 template <typename PointT> void
86 {
87  if ( normal_cloud->size () == 0 )
88  {
89  PCL_ERROR ("[pcl::SupervoxelClustering::setNormalCloud] Empty cloud set, doing nothing \n");
90  return;
91  }
92 
93  input_normals_ = normal_cloud;
94 }
95 
96 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
97 template <typename PointT> void
98 pcl::SupervoxelClustering<PointT>::extract (std::map<uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters)
99 {
100  //timer_.reset ();
101  //double t_start = timer_.getTime ();
102  //std::cout << "Init compute \n";
103  bool segmentation_is_possible = initCompute ();
104  if ( !segmentation_is_possible )
105  {
106  deinitCompute ();
107  return;
108  }
109 
110  //std::cout << "Preparing for segmentation \n";
111  segmentation_is_possible = prepareForSegmentation ();
112  if ( !segmentation_is_possible )
113  {
114  deinitCompute ();
115  return;
116  }
117 
118  //double t_prep = timer_.getTime ();
119  //std::cout << "Placing Seeds" << std::endl;
120  std::vector<PointT, Eigen::aligned_allocator<PointT> > seed_points;
121  selectInitialSupervoxelSeeds (seed_points);
122  //std::cout << "Creating helpers "<<std::endl;
123  createSupervoxelHelpers (seed_points);
124  //double t_seeds = timer_.getTime ();
125 
126 
127  //std::cout << "Expanding the supervoxels" << std::endl;
128  int max_depth = static_cast<int> (1.8f*seed_resolution_/resolution_);
129  expandSupervoxels (max_depth);
130  //double t_iterate = timer_.getTime ();
131 
132  //std::cout << "Making Supervoxel structures" << std::endl;
133  makeSupervoxels (supervoxel_clusters);
134  //double t_supervoxels = timer_.getTime ();
135 
136  // std::cout << "--------------------------------- Timing Report --------------------------------- \n";
137  // std::cout << "Time to prep (normals, neighbors, voxelization)="<<t_prep-t_start<<" ms\n";
138  // std::cout << "Time to seed clusters ="<<t_seeds-t_prep<<" ms\n";
139  // std::cout << "Time to expand clusters ="<<t_iterate-t_seeds<<" ms\n";
140  // std::cout << "Time to create supervoxel structures ="<<t_supervoxels-t_iterate<<" ms\n";
141  // std::cout << "Total run time ="<<t_supervoxels-t_start<<" ms\n";
142  // std::cout << "--------------------------------------------------------------------------------- \n";
143 
144  deinitCompute ();
145 }
146 
147 
148 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
149 template <typename PointT> void
150 pcl::SupervoxelClustering<PointT>::refineSupervoxels (int num_itr, std::map<uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters)
151 {
152  if (supervoxel_helpers_.size () == 0)
153  {
154  PCL_ERROR ("[pcl::SupervoxelClustering::refineVoxelNormals] Supervoxels not extracted, doing nothing - (Call extract first!) \n");
155  return;
156  }
157 
158  int max_depth = static_cast<int> (1.8f*seed_resolution_/resolution_);
159  for (int i = 0; i < num_itr; ++i)
160  {
161  for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr)
162  {
163  sv_itr->refineNormals ();
164  }
165 
166  reseedSupervoxels ();
167  expandSupervoxels (max_depth);
168  }
169 
170 
171  makeSupervoxels (supervoxel_clusters);
172 
173 }
174 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
175 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
176 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
177 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
178 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
179 
180 
181 template <typename PointT> bool
183 {
184 
185  // if user forgot to pass point cloud or if it is empty
186  if ( input_->points.size () == 0 )
187  return (false);
188 
189  //Add the new cloud of data to the octree
190  //std::cout << "Populating adjacency octree with new cloud \n";
191  //double prep_start = timer_.getTime ();
192  adjacency_octree_->addPointsFromInputCloud ();
193  //double prep_end = timer_.getTime ();
194  //std::cout<<"Time elapsed populating octree with next frame ="<<prep_end-prep_start<<" ms\n";
195 
196  //Compute normals and insert data for centroids into data field of octree
197  //double normals_start = timer_.getTime ();
198  computeVoxelData ();
199  //double normals_end = timer_.getTime ();
200  //std::cout << "Time elapsed finding normals and pushing into octree ="<<normals_end-normals_start<<" ms\n";
201 
202  return true;
203 }
204 
205 template <typename PointT> void
207 {
208  voxel_centroid_cloud_.reset (new PointCloudT);
209  voxel_centroid_cloud_->resize (adjacency_octree_->getLeafCount ());
210  typename LeafVectorT::iterator leaf_itr = adjacency_octree_->begin ();
211  typename PointCloudT::iterator cent_cloud_itr = voxel_centroid_cloud_->begin ();
212  for (int idx = 0 ; leaf_itr != adjacency_octree_->end (); ++leaf_itr, ++cent_cloud_itr, ++idx)
213  {
214  VoxelData& new_voxel_data = (*leaf_itr)->getData ();
215  //Add the point to the centroid cloud
216  new_voxel_data.getPoint (*cent_cloud_itr);
217  //voxel_centroid_cloud_->push_back(new_voxel_data.getPoint ());
218  new_voxel_data.idx_ = idx;
219  }
220 
221  //If normals were provided
222  if (input_normals_)
223  {
224  //Verify that input normal cloud size is same as input cloud size
225  assert (input_normals_->size () == input_->size ());
226  //For every point in the input cloud, find its corresponding leaf
227  typename NormalCloudT::const_iterator normal_itr = input_normals_->begin ();
228  for (typename PointCloudT::const_iterator input_itr = input_->begin (); input_itr != input_->end (); ++input_itr, ++normal_itr)
229  {
230  //If the point is not finite we ignore it
231  if ( !pcl::isFinite<PointT> (*input_itr))
232  continue;
233  //Otherwise look up its leaf container
234  LeafContainerT* leaf = adjacency_octree_->getLeafContainerAtPoint (*input_itr);
235 
236  //Get the voxel data object
237  VoxelData& voxel_data = leaf->getData ();
238  //Add this normal in (we will normalize at the end)
239  voxel_data.normal_ += normal_itr->getNormalVector4fMap ();
240  voxel_data.curvature_ += normal_itr->curvature;
241  }
242  //Now iterate through the leaves and normalize
243  for (leaf_itr = adjacency_octree_->begin (); leaf_itr != adjacency_octree_->end (); ++leaf_itr)
244  {
245  VoxelData& voxel_data = (*leaf_itr)->getData ();
246  voxel_data.normal_.normalize ();
247  voxel_data.owner_ = 0;
248  voxel_data.distance_ = std::numeric_limits<float>::max ();
249  //Get the number of points in this leaf
250  int num_points = (*leaf_itr)->getPointCounter ();
251  voxel_data.curvature_ /= num_points;
252  }
253  }
254  else //Otherwise just compute the normals
255  {
256  for (leaf_itr = adjacency_octree_->begin (); leaf_itr != adjacency_octree_->end (); ++leaf_itr)
257  {
258  VoxelData& new_voxel_data = (*leaf_itr)->getData ();
259  //For every point, get its neighbors, build an index vector, compute normal
260  std::vector<int> indices;
261  indices.reserve (81);
262  //Push this point
263  indices.push_back (new_voxel_data.idx_);
264  for (typename LeafContainerT::iterator neighb_itr=(*leaf_itr)->begin (); neighb_itr!=(*leaf_itr)->end (); ++neighb_itr)
265  {
266  VoxelData& neighb_voxel_data = (*neighb_itr)->getData ();
267  //Push neighbor index
268  indices.push_back (neighb_voxel_data.idx_);
269  //Get neighbors neighbors, push onto cloud
270  for (typename LeafContainerT::iterator neighb_neighb_itr=(*neighb_itr)->begin (); neighb_neighb_itr!=(*neighb_itr)->end (); ++neighb_neighb_itr)
271  {
272  VoxelData& neighb2_voxel_data = (*neighb_neighb_itr)->getData ();
273  indices.push_back (neighb2_voxel_data.idx_);
274  }
275  }
276  //Compute normal
277  pcl::computePointNormal (*voxel_centroid_cloud_, indices, new_voxel_data.normal_, new_voxel_data.curvature_);
278  pcl::flipNormalTowardsViewpoint (voxel_centroid_cloud_->points[new_voxel_data.idx_], 0.0f,0.0f,0.0f, new_voxel_data.normal_);
279  new_voxel_data.normal_[3] = 0.0f;
280  new_voxel_data.normal_.normalize ();
281  new_voxel_data.owner_ = 0;
282  new_voxel_data.distance_ = std::numeric_limits<float>::max ();
283  }
284  }
285 
286 
287 }
288 
289 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
290 template <typename PointT> void
292 {
293 
294 
295  for (int i = 1; i < depth; ++i)
296  {
297  //Expand the the supervoxels by one iteration
298  for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr)
299  {
300  sv_itr->expand ();
301  }
302 
303  //Update the centers to reflect new centers
304  for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); )
305  {
306  if (sv_itr->size () == 0)
307  {
308  sv_itr = supervoxel_helpers_.erase (sv_itr);
309  }
310  else
311  {
312  sv_itr->updateCentroid ();
313  ++sv_itr;
314  }
315  }
316 
317  }
318 
319 }
320 
321 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
322 template <typename PointT> void
323 pcl::SupervoxelClustering<PointT>::makeSupervoxels (std::map<uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters)
324 {
325  supervoxel_clusters.clear ();
326  for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr)
327  {
328  uint32_t label = sv_itr->getLabel ();
329  supervoxel_clusters[label].reset (new Supervoxel<PointT>);
330  sv_itr->getXYZ (supervoxel_clusters[label]->centroid_.x,supervoxel_clusters[label]->centroid_.y,supervoxel_clusters[label]->centroid_.z);
331  sv_itr->getRGB (supervoxel_clusters[label]->centroid_.rgba);
332  sv_itr->getNormal (supervoxel_clusters[label]->normal_);
333  sv_itr->getVoxels (supervoxel_clusters[label]->voxels_);
334  sv_itr->getNormals (supervoxel_clusters[label]->normals_);
335  }
336  //Make sure that color vector is big enough
337  initializeLabelColors ();
338 }
339 
340 
341 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
342 template <typename PointT> void
343 pcl::SupervoxelClustering<PointT>::createSupervoxelHelpers (std::vector<PointT, Eigen::aligned_allocator<PointT> > &seed_points)
344 {
345 
346  supervoxel_helpers_.clear ();
347  for (size_t i = 0; i < seed_points.size (); ++i)
348  {
349  supervoxel_helpers_.push_back (new SupervoxelHelper(i+1,this));
350  //Find which leaf corresponds to this seed index
351  LeafContainerT* seed_leaf = adjacency_octree_->getLeafContainerAtPoint (seed_points[i]);
352  if (seed_leaf)
353  {
354  supervoxel_helpers_.back ().addLeaf (seed_leaf);
355  }
356  else
357  {
358  PCL_WARN ("Could not find leaf in pcl::SupervoxelClustering<PointT>::createSupervoxelHelpers - supervoxel will be deleted \n");
359  }
360  }
361 
362 }
363 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
364 template <typename PointT> void
365 pcl::SupervoxelClustering<PointT>::selectInitialSupervoxelSeeds (std::vector<PointT, Eigen::aligned_allocator<PointT> > &seed_points)
366 {
367  //TODO THIS IS BAD - SEEDING SHOULD BE BETTER
368  //TODO Switch to assigning leaves! Don't use Octree!
369 
370  // std::cout << "Size of centroid cloud="<<voxel_centroid_cloud_->size ()<<", seeding resolution="<<seed_resolution_<<"\n";
371  //Initialize octree with voxel centroids
372  pcl::octree::OctreePointCloudSearch <PointT> seed_octree (seed_resolution_);
373  seed_octree.setInputCloud (voxel_centroid_cloud_);
374  seed_octree.addPointsFromInputCloud ();
375  // std::cout << "Size of octree ="<<seed_octree.getLeafCount ()<<"\n";
376  std::vector<PointT, Eigen::aligned_allocator<PointT> > voxel_centers;
377  int num_seeds = seed_octree.getOccupiedVoxelCenters(voxel_centers);
378  //std::cout << "Number of seed points before filtering="<<voxel_centers.size ()<<std::endl;
379 
380  std::vector<int> seed_indices_orig;
381  seed_indices_orig.resize (num_seeds, 0);
382  seed_points.clear ();
383  std::vector<int> closest_index;
384  std::vector<float> distance;
385  closest_index.resize(1,0);
386  distance.resize(1,0);
387  if (voxel_kdtree_ == 0)
388  {
389  voxel_kdtree_.reset (new pcl::search::KdTree<PointT>);
390  voxel_kdtree_ ->setInputCloud (voxel_centroid_cloud_);
391  }
392 
393  for (int i = 0; i < num_seeds; ++i)
394  {
395  voxel_kdtree_->nearestKSearch (voxel_centers[i], 1, closest_index, distance);
396  seed_indices_orig[i] = closest_index[0];
397  }
398 
399  std::vector<int> neighbors;
400  std::vector<float> sqr_distances;
401  seed_points.reserve (seed_indices_orig.size ());
402  float search_radius = 0.5f*seed_resolution_;
403  // This is number of voxels which fit in a planar slice through search volume
404  // Area of planar slice / area of voxel side
405  float min_points = 0.05f * (search_radius)*(search_radius) * 3.1415926536f / (resolution_*resolution_);
406  for (size_t i = 0; i < seed_indices_orig.size (); ++i)
407  {
408  int num = voxel_kdtree_->radiusSearch (seed_indices_orig[i], search_radius , neighbors, sqr_distances);
409  int min_index = seed_indices_orig[i];
410  if ( num > min_points)
411  {
412  seed_points.push_back (voxel_centroid_cloud_->points[min_index]);
413  }
414 
415  }
416  // std::cout << "Number of seed points after filtering="<<seed_points.size ()<<std::endl;
417 
418 }
419 
420 
421 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
422 template <typename PointT> void
424 {
425  //Go through each supervoxel and remove all it's leaves
426  for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr)
427  {
428  sv_itr->removeAllLeaves ();
429  }
430 
431  std::vector<int> closest_index;
432  std::vector<float> distance;
433  //Now go through each supervoxel, find voxel closest to its center, add it in
434  for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr)
435  {
436  PointT point;
437  sv_itr->getXYZ (point.x, point.y, point.z);
438  voxel_kdtree_->nearestKSearch (point, 1, closest_index, distance);
439 
440  LeafContainerT* seed_leaf = adjacency_octree_->getLeafContainerAtPoint (voxel_centroid_cloud_->points[closest_index[0]]);
441  if (seed_leaf)
442  {
443  sv_itr->addLeaf (seed_leaf);
444  }
445  }
446 
447 }
448 
449 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
450 template <typename PointT> void
452 {
453  p.x /= p.z;
454  p.y /= p.z;
455  p.z = std::log (p.z);
456 }
457 
458 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
459 template <typename PointT> float
460 pcl::SupervoxelClustering<PointT>::voxelDataDistance (const VoxelData &v1, const VoxelData &v2) const
461 {
462 
463  float spatial_dist = (v1.xyz_ - v2.xyz_).norm () / seed_resolution_;
464  float color_dist = (v1.rgb_ - v2.rgb_).norm () / 255.0f;
465  float cos_angle_normal = 1.0f - std::abs (v1.normal_.dot (v2.normal_));
466  // std::cout << "s="<<spatial_dist<<" c="<<color_dist<<" an="<<cos_angle_normal<<"\n";
467  return cos_angle_normal * normal_importance_ + color_dist * color_importance_+ spatial_dist * spatial_importance_;
468 
469 }
470 
471 
472 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
473 ///////// GETTER FUNCTIONS
474 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
475 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
476 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
477 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
478 template <typename PointT> void
480 {
481  adjacency_list_arg.clear ();
482  //Add a vertex for each label, store ids in map
483  std::map <uint32_t, VoxelID> label_ID_map;
484  for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
485  {
486  VoxelID node_id = add_vertex (adjacency_list_arg);
487  adjacency_list_arg[node_id] = (sv_itr->getLabel ());
488  label_ID_map.insert (std::make_pair (sv_itr->getLabel (), node_id));
489  }
490 
491  for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
492  {
493  uint32_t label = sv_itr->getLabel ();
494  std::set<uint32_t> neighbor_labels;
495  sv_itr->getNeighborLabels (neighbor_labels);
496  for (std::set<uint32_t>::iterator label_itr = neighbor_labels.begin (); label_itr != neighbor_labels.end (); ++label_itr)
497  {
498  bool edge_added;
499  EdgeID edge;
500  VoxelID u = (label_ID_map.find (label))->second;
501  VoxelID v = (label_ID_map.find (*label_itr))->second;
502  boost::tie (edge, edge_added) = add_edge (u,v,adjacency_list_arg);
503  //Calc distance between centers, set as edge weight
504  if (edge_added)
505  {
506  VoxelData centroid_data = (sv_itr)->getCentroid ();
507  //Find the neighbhor with this label
508  VoxelData neighb_centroid_data;
509 
510  for (typename HelperListT::const_iterator neighb_itr = supervoxel_helpers_.cbegin (); neighb_itr != supervoxel_helpers_.cend (); ++neighb_itr)
511  {
512  if (neighb_itr->getLabel () == (*label_itr))
513  {
514  neighb_centroid_data = neighb_itr->getCentroid ();
515  break;
516  }
517  }
518 
519  float length = voxelDataDistance (centroid_data, neighb_centroid_data);
520  adjacency_list_arg[edge] = length;
521  }
522  }
523 
524  }
525 
526 }
527 
528 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
529 template <typename PointT> void
530 pcl::SupervoxelClustering<PointT>::getSupervoxelAdjacency (std::multimap<uint32_t, uint32_t> &label_adjacency) const
531 {
532  label_adjacency.clear ();
533  for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
534  {
535  uint32_t label = sv_itr->getLabel ();
536  std::set<uint32_t> neighbor_labels;
537  sv_itr->getNeighborLabels (neighbor_labels);
538  for (std::set<uint32_t>::iterator label_itr = neighbor_labels.begin (); label_itr != neighbor_labels.end (); ++label_itr)
539  label_adjacency.insert (std::pair<uint32_t,uint32_t> (label, *label_itr) );
540  //if (neighbor_labels.size () == 0)
541  // std::cout << label<<"(size="<<sv_itr->size () << ") has "<<neighbor_labels.size () << "\n";
542  }
543 
544 }
545 
546 
547 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
548 template <typename PointT> pcl::PointCloud<pcl::PointXYZRGBA>::Ptr
550 {
552  pcl::copyPointCloud (*input_,*colored_cloud);
553 
555  typename pcl::PointCloud <PointT>::const_iterator i_input = input_->begin ();
556  std::vector <int> indices;
557  std::vector <float> sqr_distances;
558  for (i_colored = colored_cloud->begin (); i_colored != colored_cloud->end (); ++i_colored,++i_input)
559  {
560  if ( !pcl::isFinite<PointT> (*i_input))
561  i_colored->rgb = 0;
562  else
563  {
564  i_colored->rgb = 0;
565  LeafContainerT *leaf = adjacency_octree_->getLeafContainerAtPoint (*i_input);
566  VoxelData& voxel_data = leaf->getData ();
567  if (voxel_data.owner_)
568  i_colored->rgba = label_colors_[voxel_data.owner_->getLabel ()];
569 
570  }
571 
572  }
573 
574  return (colored_cloud);
575 }
576 
577 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
578 template <typename PointT> pcl::PointCloud<pcl::PointXYZRGBA>::Ptr
580 {
582  for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
583  {
584  typename PointCloudT::Ptr voxels;
585  sv_itr->getVoxels (voxels);
587  copyPointCloud (*voxels, rgb_copy);
588 
589  pcl::PointCloud<pcl::PointXYZRGBA>::iterator rgb_copy_itr = rgb_copy.begin ();
590  for ( ; rgb_copy_itr != rgb_copy.end (); ++rgb_copy_itr)
591  rgb_copy_itr->rgba = label_colors_ [sv_itr->getLabel ()];
592 
593  *colored_cloud += rgb_copy;
594  }
595 
596  return colored_cloud;
597 }
598 
599 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
600 template <typename PointT> typename pcl::PointCloud<PointT>::Ptr
602 {
603  typename PointCloudT::Ptr centroid_copy (new PointCloudT);
604  copyPointCloud (*voxel_centroid_cloud_, *centroid_copy);
605  return centroid_copy;
606 }
607 
608 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
609 template <typename PointT> pcl::PointCloud<pcl::PointXYZL>::Ptr
611 {
613  for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
614  {
615  typename PointCloudT::Ptr voxels;
616  sv_itr->getVoxels (voxels);
618  copyPointCloud (*voxels, xyzl_copy);
619 
620  pcl::PointCloud<pcl::PointXYZL>::iterator xyzl_copy_itr = xyzl_copy.begin ();
621  for ( ; xyzl_copy_itr != xyzl_copy.end (); ++xyzl_copy_itr)
622  xyzl_copy_itr->label = sv_itr->getLabel ();
623 
624  *labeled_voxel_cloud += xyzl_copy;
625  }
626 
627  return labeled_voxel_cloud;
628 }
629 
630 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
631 template <typename PointT> pcl::PointCloud<pcl::PointXYZL>::Ptr
633 {
635  pcl::copyPointCloud (*input_,*labeled_cloud);
636 
638  typename pcl::PointCloud <PointT>::const_iterator i_input = input_->begin ();
639  std::vector <int> indices;
640  std::vector <float> sqr_distances;
641  for (i_labeled = labeled_cloud->begin (); i_labeled != labeled_cloud->end (); ++i_labeled,++i_input)
642  {
643  if ( !pcl::isFinite<PointT> (*i_input))
644  i_labeled->label = 0;
645  else
646  {
647  i_labeled->label = 0;
648  LeafContainerT *leaf = adjacency_octree_->getLeafContainerAtPoint (*i_input);
649  VoxelData& voxel_data = leaf->getData ();
650  if (voxel_data.owner_)
651  i_labeled->label = voxel_data.owner_->getLabel ();
652 
653  }
654 
655  }
656 
657  return (labeled_cloud);
658 }
659 
660 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
661 template <typename PointT> pcl::PointCloud<pcl::PointNormal>::Ptr
663 {
665  normal_cloud->resize (supervoxel_clusters.size ());
666  typename std::map <uint32_t, typename pcl::Supervoxel<PointT>::Ptr>::iterator sv_itr,sv_itr_end;
667  sv_itr = supervoxel_clusters.begin ();
668  sv_itr_end = supervoxel_clusters.end ();
669  pcl::PointCloud<pcl::PointNormal>::iterator normal_cloud_itr = normal_cloud->begin ();
670  for ( ; sv_itr != sv_itr_end; ++sv_itr, ++normal_cloud_itr)
671  {
672  (sv_itr->second)->getCentroidPointNormal (*normal_cloud_itr);
673  }
674  return normal_cloud;
675 }
676 
677 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
678 template <typename PointT> float
680 {
681  return (resolution_);
682 }
683 
684 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
685 template <typename PointT> void
687 {
688  resolution_ = resolution;
689 
690 }
691 
692 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
693 template <typename PointT> float
695 {
696  return (resolution_);
697 }
698 
699 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
700 template <typename PointT> void
702 {
703  seed_resolution_ = seed_resolution;
704 }
705 
706 
707 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
708 template <typename PointT> void
710 {
711  color_importance_ = val;
712 }
713 
714 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
715 template <typename PointT> void
717 {
718  spatial_importance_ = val;
719 }
720 
721 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
722 template <typename PointT> void
724 {
725  normal_importance_ = val;
726 }
727 
728 
729 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
730 template <typename PointT> void
732 {
733  uint32_t max_label = static_cast<uint32_t> (getMaxLabel ());
734  //If we already have enough colors, return
735  if (label_colors_.size () > max_label)
736  return;
737 
738  //Otherwise, generate new colors until we have enough
739  label_colors_.reserve (max_label + 1);
740  srand (static_cast<unsigned int> (time (0)));
741  while (label_colors_.size () <= max_label )
742  {
743  uint8_t r = static_cast<uint8_t>( (rand () % 256));
744  uint8_t g = static_cast<uint8_t>( (rand () % 256));
745  uint8_t b = static_cast<uint8_t>( (rand () % 256));
746  label_colors_.push_back (static_cast<uint32_t>(r) << 16 | static_cast<uint32_t>(g) << 8 | static_cast<uint32_t>(b));
747  }
748 }
749 
750 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
751 template <typename PointT> int
753 {
754  int max_label = 0;
755  for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
756  {
757  int temp = sv_itr->getLabel ();
758  if (temp > max_label)
759  max_label = temp;
760  }
761  return max_label;
762 }
763 
764 namespace pcl
765 {
766 
767  namespace octree
768  {
769  //Explicit overloads for RGB types
770  template<>
771  void
773  {
774  ++num_points_;
775  //Same as before here
776  data_.xyz_[0] += new_point.x;
777  data_.xyz_[1] += new_point.y;
778  data_.xyz_[2] += new_point.z;
779  //Separate sums for r,g,b since we cant sum in uchars
780  data_.rgb_[0] += static_cast<float> (new_point.r);
781  data_.rgb_[1] += static_cast<float> (new_point.g);
782  data_.rgb_[2] += static_cast<float> (new_point.b);
783  }
784 
785  template<>
786  void
788  {
789  ++num_points_;
790  //Same as before here
791  data_.xyz_[0] += new_point.x;
792  data_.xyz_[1] += new_point.y;
793  data_.xyz_[2] += new_point.z;
794  //Separate sums for r,g,b since we cant sum in uchars
795  data_.rgb_[0] += static_cast<float> (new_point.r);
796  data_.rgb_[1] += static_cast<float> (new_point.g);
797  data_.rgb_[2] += static_cast<float> (new_point.b);
798  }
799 
800 
801 
802  //Explicit overloads for RGB types
803  template<> void
805  {
806  data_.rgb_[0] /= (static_cast<float> (num_points_));
807  data_.rgb_[1] /= (static_cast<float> (num_points_));
808  data_.rgb_[2] /= (static_cast<float> (num_points_));
809  data_.xyz_[0] /= (static_cast<float> (num_points_));
810  data_.xyz_[1] /= (static_cast<float> (num_points_));
811  data_.xyz_[2] /= (static_cast<float> (num_points_));
812  }
813 
814  template<> void
816  {
817  data_.rgb_[0] /= (static_cast<float> (num_points_));
818  data_.rgb_[1] /= (static_cast<float> (num_points_));
819  data_.rgb_[2] /= (static_cast<float> (num_points_));
820  data_.xyz_[0] /= (static_cast<float> (num_points_));
821  data_.xyz_[1] /= (static_cast<float> (num_points_));
822  data_.xyz_[2] /= (static_cast<float> (num_points_));
823  }
824 
825  //Explicit overloads for XYZ types
826  template<>
827  void
829  {
830  ++num_points_;
831  //Same as before here
832  data_.xyz_[0] += new_point.x;
833  data_.xyz_[1] += new_point.y;
834  data_.xyz_[2] += new_point.z;
835  }
836 
837  template<> void
839  {
840  data_.xyz_[0] /= (static_cast<float> (num_points_));
841  data_.xyz_[1] /= (static_cast<float> (num_points_));
842  data_.xyz_[2] /= (static_cast<float> (num_points_));
843  }
844 
845  }
846 }
847 
848 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
849 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
850 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
851 namespace pcl
852 {
853 
854  template<> void
856  {
857  point_arg.rgba = static_cast<uint32_t>(rgb_[0]) << 16 |
858  static_cast<uint32_t>(rgb_[1]) << 8 |
859  static_cast<uint32_t>(rgb_[2]);
860  point_arg.x = xyz_[0];
861  point_arg.y = xyz_[1];
862  point_arg.z = xyz_[2];
863  }
864 
865  template<> void
867  {
868  point_arg.rgba = static_cast<uint32_t>(rgb_[0]) << 16 |
869  static_cast<uint32_t>(rgb_[1]) << 8 |
870  static_cast<uint32_t>(rgb_[2]);
871  point_arg.x = xyz_[0];
872  point_arg.y = xyz_[1];
873  point_arg.z = xyz_[2];
874  }
875 
876  template<typename PointT> void
878  {
879  //XYZ is required or this doesn't make much sense...
880  point_arg.x = xyz_[0];
881  point_arg.y = xyz_[1];
882  point_arg.z = xyz_[2];
883  }
884 
885  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
886  template <typename PointT> void
888  {
889  normal_arg.normal_x = normal_[0];
890  normal_arg.normal_y = normal_[1];
891  normal_arg.normal_z = normal_[2];
892  normal_arg.curvature = curvature_;
893  }
894 }
895 
896 
897 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
898 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
899 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
900 template <typename PointT> void
902 {
903  leaves_.insert (leaf_arg);
904  VoxelData& voxel_data = leaf_arg->getData ();
905  voxel_data.owner_ = this;
906 }
907 
908 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
909 template <typename PointT> void
911 {
912  leaves_.erase (leaf_arg);
913 }
914 
915 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
916 template <typename PointT> void
918 {
919  typename std::set<LeafContainerT*>::iterator leaf_itr;
920  for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr)
921  {
922  VoxelData& voxel = ((*leaf_itr)->getData ());
923  voxel.owner_ = 0;
924  voxel.distance_ = std::numeric_limits<float>::max ();
925  }
926  leaves_.clear ();
927 }
928 
929 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
930 template <typename PointT> void
932 {
933  //std::cout << "Expanding sv "<<label_<<", owns "<<leaves_.size ()<<" voxels\n";
934  //Buffer of new neighbors - initial size is just a guess of most possible
935  std::vector<LeafContainerT*> new_owned;
936  new_owned.reserve (leaves_.size () * 9);
937  //For each leaf belonging to this supervoxel
938  typename std::set<LeafContainerT*>::iterator leaf_itr;
939  for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr)
940  {
941  //for each neighbor of the leaf
942  for (typename LeafContainerT::iterator neighb_itr=(*leaf_itr)->begin (); neighb_itr!=(*leaf_itr)->end (); ++neighb_itr)
943  {
944  //Get a reference to the data contained in the leaf
945  VoxelData& neighbor_voxel = ((*neighb_itr)->getData ());
946  //TODO this is a shortcut, really we should always recompute distance
947  if(neighbor_voxel.owner_ == this)
948  continue;
949  //Compute distance to the neighbor
950  float dist = parent_->voxelDataDistance (centroid_, neighbor_voxel);
951  //If distance is less than previous, we remove it from its owner's list
952  //and change the owner to this and distance (we *steal* it!)
953  if (dist < neighbor_voxel.distance_)
954  {
955  neighbor_voxel.distance_ = dist;
956  if (neighbor_voxel.owner_ != this)
957  {
958  if (neighbor_voxel.owner_)
959  (neighbor_voxel.owner_)->removeLeaf(*neighb_itr);
960  neighbor_voxel.owner_ = this;
961  new_owned.push_back (*neighb_itr);
962  }
963  }
964  }
965  }
966  //Push all new owned onto the owned leaf set
967  typename std::vector<LeafContainerT*>::iterator new_owned_itr;
968  for (new_owned_itr=new_owned.begin (); new_owned_itr!=new_owned.end (); ++new_owned_itr)
969  {
970  leaves_.insert (*new_owned_itr);
971  }
972 
973 }
974 
975 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
976 template <typename PointT> void
978 {
979  typename std::set<LeafContainerT*>::iterator leaf_itr;
980  //For each leaf belonging to this supervoxel, get its neighbors, build an index vector, compute normal
981  for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr)
982  {
983  VoxelData& voxel_data = (*leaf_itr)->getData ();
984  std::vector<int> indices;
985  indices.reserve (81);
986  //Push this point
987  indices.push_back (voxel_data.idx_);
988  for (typename LeafContainerT::iterator neighb_itr=(*leaf_itr)->begin (); neighb_itr!=(*leaf_itr)->end (); ++neighb_itr)
989  {
990  //Get a reference to the data contained in the leaf
991  VoxelData& neighbor_voxel_data = ((*neighb_itr)->getData ());
992  //If the neighbor is in this supervoxel, use it
993  if (neighbor_voxel_data.owner_ == this)
994  {
995  indices.push_back (neighbor_voxel_data.idx_);
996  //Also check its neighbors
997  for (typename LeafContainerT::iterator neighb_neighb_itr=(*neighb_itr)->begin (); neighb_neighb_itr!=(*neighb_itr)->end (); ++neighb_neighb_itr)
998  {
999  VoxelData& neighb_neighb_voxel_data = (*neighb_neighb_itr)->getData ();
1000  if (neighb_neighb_voxel_data.owner_ == this)
1001  indices.push_back (neighb_neighb_voxel_data.idx_);
1002  }
1003 
1004 
1005  }
1006  }
1007  //Compute normal
1008  pcl::computePointNormal (*parent_->voxel_centroid_cloud_, indices, voxel_data.normal_, voxel_data.curvature_);
1009  pcl::flipNormalTowardsViewpoint (parent_->voxel_centroid_cloud_->points[voxel_data.idx_], 0.0f,0.0f,0.0f, voxel_data.normal_);
1010  voxel_data.normal_[3] = 0.0f;
1011  voxel_data.normal_.normalize ();
1012  }
1013 }
1014 
1015 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
1016 template <typename PointT> void
1018 {
1019  centroid_.normal_ = Eigen::Vector4f::Zero ();
1020  centroid_.xyz_ = Eigen::Vector3f::Zero ();
1021  centroid_.rgb_ = Eigen::Vector3f::Zero ();
1022  typename std::set<LeafContainerT*>::iterator leaf_itr = leaves_.begin ();
1023  for ( ; leaf_itr!= leaves_.end (); ++leaf_itr)
1024  {
1025  const VoxelData& leaf_data = (*leaf_itr)->getData ();
1026  centroid_.normal_ += leaf_data.normal_;
1027  centroid_.xyz_ += leaf_data.xyz_;
1028  centroid_.rgb_ += leaf_data.rgb_;
1029  }
1030  centroid_.normal_.normalize ();
1031  centroid_.xyz_ /= static_cast<float> (leaves_.size ());
1032  centroid_.rgb_ /= static_cast<float> (leaves_.size ());
1033 
1034 }
1035 
1036 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
1037 template <typename PointT> void
1039 {
1040  voxels.reset (new pcl::PointCloud<PointT>);
1041  voxels->clear ();
1042  voxels->resize (leaves_.size ());
1043  typename pcl::PointCloud<PointT>::iterator voxel_itr = voxels->begin ();
1044  //typename std::set<LeafContainerT*>::iterator leaf_itr;
1045  for (typename std::set<LeafContainerT*>::const_iterator leaf_itr = leaves_.begin ();
1046  leaf_itr != leaves_.end ();
1047  ++leaf_itr, ++voxel_itr)
1048  {
1049  const VoxelData& leaf_data = (*leaf_itr)->getData ();
1050  leaf_data.getPoint (*voxel_itr);
1051  }
1052 }
1053 
1054 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
1055 template <typename PointT> void
1057 {
1058  normals.reset (new pcl::PointCloud<Normal>);
1059  normals->clear ();
1060  normals->resize (leaves_.size ());
1061  typename std::set<LeafContainerT*>::const_iterator leaf_itr;
1062  typename pcl::PointCloud<Normal>::iterator normal_itr = normals->begin ();
1063  for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr, ++normal_itr)
1064  {
1065  const VoxelData& leaf_data = (*leaf_itr)->getData ();
1066  leaf_data.getNormal (*normal_itr);
1067  }
1068 }
1069 
1070 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
1071 template <typename PointT> void
1072 pcl::SupervoxelClustering<PointT>::SupervoxelHelper::getNeighborLabels (std::set<uint32_t> &neighbor_labels) const
1073 {
1074  neighbor_labels.clear ();
1075  //For each leaf belonging to this supervoxel
1076  typename std::set<LeafContainerT*>::const_iterator leaf_itr;
1077  for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr)
1078  {
1079  //for each neighbor of the leaf
1080  for (typename LeafContainerT::iterator neighb_itr=(*leaf_itr)->begin (); neighb_itr!=(*leaf_itr)->end (); ++neighb_itr)
1081  {
1082  //Get a reference to the data contained in the leaf
1083  VoxelData& neighbor_voxel = ((*neighb_itr)->getData ());
1084  //If it has an owner, and it's not us - get it's owner's label insert into set
1085  if (neighbor_voxel.owner_ != this && neighbor_voxel.owner_)
1086  {
1087  neighbor_labels.insert (neighbor_voxel.owner_->getLabel ());
1088  }
1089  }
1090  }
1091 }
1092 
1093 
1094 #endif // PCL_SUPERVOXEL_CLUSTERING_HPP_
1095 
A point structure representing normal coordinates and the surface curvature estimate.
void setSeedResolution(float seed_resolution)
Set the resolution of the octree seed voxels.
size_t size() const
Definition: point_cloud.h:440
Octree adjacency leaf container class- stores set of pointers to neighbors, number of points added...
Octree pointcloud voxel class which maintains adjacency information for its voxels.
void setNormalImportance(float val)
Set the importance of scalar normal product for supervoxels.
iterator end()
Definition: point_cloud.h:435
void setColorImportance(float val)
Set the importance of color for supervoxels.
pcl::PointCloud< PointXYZL >::Ptr getLabeledCloud() const
Returns labeled cloud Points that belong to the same supervoxel have the same label.
PCL_EXPORTS void copyPointCloud(const pcl::PCLPointCloud2 &cloud_in, const std::vector< int > &indices, pcl::PCLPointCloud2 &cloud_out)
Extract the indices of a given point cloud as a new point cloud.
VectorType::iterator iterator
Definition: point_cloud.h:432
pcl::PointCloud< PointT >::Ptr getVoxelCentroidCloud() const
Returns a deep copy of the voxel centroid cloud.
VoxelAdjacencyList::vertex_descriptor VoxelID
void setVoxelResolution(float resolution)
Set the resolution of the octree voxels.
A point structure representing Euclidean xyz coordinates, and the RGBA color.
virtual void setInputCloud(const typename pcl::PointCloud< PointT >::ConstPtr &cloud)
This method sets the cloud to be supervoxelized.
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
void flipNormalTowardsViewpoint(const PointT &point, float vp_x, float vp_y, float vp_z, Eigen::Matrix< Scalar, 4, 1 > &normal)
Flip (in place) the estimated normal of a point towards a given viewpoint.
Definition: normal_3d.h:119
virtual void refineSupervoxels(int num_itr, std::map< uint32_t, typename Supervoxel< PointT >::Ptr > &supervoxel_clusters)
This method refines the calculated supervoxels - may only be called after extract.
virtual void setNormalCloud(typename NormalCloudT::ConstPtr normal_cloud)
This method sets the normals to be used for supervoxels (should be same size as input cloud) ...
SupervoxelClustering(float voxel_resolution, float seed_resolution, bool use_single_camera_transform=true)
Constructor that sets default values for member variables.
A point structure representing Euclidean xyz coordinates.
virtual void extract(std::map< uint32_t, typename Supervoxel< PointT >::Ptr > &supervoxel_clusters)
This method launches the segmentation algorithm and returns the supervoxels that were obtained during...
VoxelData is a structure used for storing data within a pcl::octree::OctreePointCloudAdjacencyContain...
static pcl::PointCloud< pcl::PointNormal >::Ptr makeSupervoxelNormalCloud(std::map< uint32_t, typename Supervoxel< PointT >::Ptr > &supervoxel_clusters)
Static helper function which returns a pointcloud of normals for the input supervoxels.
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
void getNormal(Normal &normal_arg) const
Gets the data of in the form of a normal.
void clear()
Removes all points in a cloud and sets the width and height to 0.
Definition: point_cloud.h:567
pcl::PointCloud< pcl::PointXYZRGBA >::Ptr getColoredVoxelCloud() const
Returns an RGB colorized voxelized cloud showing superpixels Otherwise it returns an empty pointer...
pcl::PointCloud< pcl::PointXYZL >::Ptr getLabeledVoxelCloud() const
Returns labeled voxelized cloud Points that belong to the same supervoxel have the same label...
PointCloud represents the base class in PCL for storing collections of 3D points. ...
Implements a supervoxel algorithm based on voxel structure, normals, and rgb values.
void getSupervoxelAdjacency(std::multimap< uint32_t, uint32_t > &label_adjacency) const
Get a multimap which gives supervoxel adjacency.
virtual ~SupervoxelClustering()
This destructor destroys the cloud, normals and search method used for finding neighbors.
Octree pointcloud search class
Definition: octree_search.h:58
VoxelAdjacencyList::edge_descriptor EdgeID
float getSeedResolution() const
Get the resolution of the octree seed voxels.
int getMaxLabel() const
Returns the current maximum (highest) label.
void getPoint(PointT &point_arg) const
Gets the data of in the form of a point.
iterator begin()
Definition: point_cloud.h:434
void resize(size_t n)
Resize the cloud.
Definition: point_cloud.h:447
void computePointNormal(const pcl::PointCloud< PointT > &cloud, Eigen::Vector4f &plane_parameters, float &curvature)
Compute the Least-Squares plane fit for a given set of points, and return the estimated plane paramet...
Definition: normal_3d.h:60
A point structure representing Euclidean xyz coordinates, and the RGB color.
boost::shared_ptr< Supervoxel< PointT > > Ptr
void getSupervoxelAdjacencyList(VoxelAdjacencyList &adjacency_list_arg) const
Gets the adjacency list (Boost Graph library) which gives connections between supervoxels.
boost::adjacency_list< boost::setS, boost::setS, boost::undirectedS, uint32_t, float > VoxelAdjacencyList
pcl::PointCloud< PointXYZRGBA >::Ptr getColoredCloud() const
Returns an RGB colorized cloud showing superpixels Otherwise it returns an empty pointer.
float getVoxelResolution() const
Get the resolution of the octree voxels.
VectorType::const_iterator const_iterator
Definition: point_cloud.h:433
void setSpatialImportance(float val)
Set the importance of spatial distance for supervoxels.