Point Cloud Library (PCL)  1.7.1
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_ = boost::make_shared <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_ = boost::make_shared<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] = boost::make_shared<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 (int 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_ = boost::make_shared< 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 (int 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 {
551  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr colored_cloud = boost::make_shared <pcl::PointCloud<pcl::PointXYZRGBA> >();
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 {
581  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr colored_cloud = boost::make_shared< pcl::PointCloud<pcl::PointXYZRGBA> > ();
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 = boost::make_shared<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 {
612  pcl::PointCloud<pcl::PointXYZL>::Ptr labeled_voxel_cloud = boost::make_shared< pcl::PointCloud<pcl::PointXYZL> > ();
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 {
634  pcl::PointCloud<pcl::PointXYZL>::Ptr labeled_cloud = boost::make_shared <pcl::PointCloud<pcl::PointXYZL> >();
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 {
664  pcl::PointCloud<pcl::PointNormal>::Ptr normal_cloud = boost::make_shared<pcl::PointCloud<pcl::PointNormal> > ();
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  int max_label = 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  }
826 }
827 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
828 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
829 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
830 namespace pcl
831 {
832 
833  template<> void
835  {
836  point_arg.rgba = static_cast<uint32_t>(rgb_[0]) << 16 |
837  static_cast<uint32_t>(rgb_[1]) << 8 |
838  static_cast<uint32_t>(rgb_[2]);
839  point_arg.x = xyz_[0];
840  point_arg.y = xyz_[1];
841  point_arg.z = xyz_[2];
842  }
843 
844  template<> void
846  {
847  point_arg.rgba = static_cast<uint32_t>(rgb_[0]) << 16 |
848  static_cast<uint32_t>(rgb_[1]) << 8 |
849  static_cast<uint32_t>(rgb_[2]);
850  point_arg.x = xyz_[0];
851  point_arg.y = xyz_[1];
852  point_arg.z = xyz_[2];
853  }
854 
855  template<typename PointT> void
857  {
858  //XYZ is required or this doesn't make much sense...
859  point_arg.x = xyz_[0];
860  point_arg.y = xyz_[1];
861  point_arg.z = xyz_[2];
862  }
863 
864  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
865  template <typename PointT> void
867  {
868  normal_arg.normal_x = normal_[0];
869  normal_arg.normal_y = normal_[1];
870  normal_arg.normal_z = normal_[2];
871  normal_arg.curvature = curvature_;
872  }
873 }
874 
875 
876 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
877 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
878 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
879 template <typename PointT> void
881 {
882  leaves_.insert (leaf_arg);
883  VoxelData& voxel_data = leaf_arg->getData ();
884  voxel_data.owner_ = this;
885 }
886 
887 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
888 template <typename PointT> void
890 {
891  leaves_.erase (leaf_arg);
892 }
893 
894 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
895 template <typename PointT> void
897 {
898  typename std::set<LeafContainerT*>::iterator leaf_itr;
899  for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr)
900  {
901  VoxelData& voxel = ((*leaf_itr)->getData ());
902  voxel.owner_ = 0;
903  voxel.distance_ = std::numeric_limits<float>::max ();
904  }
905  leaves_.clear ();
906 }
907 
908 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
909 template <typename PointT> void
911 {
912  //std::cout << "Expanding sv "<<label_<<", owns "<<leaves_.size ()<<" voxels\n";
913  //Buffer of new neighbors - initial size is just a guess of most possible
914  std::vector<LeafContainerT*> new_owned;
915  new_owned.reserve (leaves_.size () * 9);
916  //For each leaf belonging to this supervoxel
917  typename std::set<LeafContainerT*>::iterator leaf_itr;
918  for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr)
919  {
920  //for each neighbor of the leaf
921  for (typename LeafContainerT::iterator neighb_itr=(*leaf_itr)->begin (); neighb_itr!=(*leaf_itr)->end (); ++neighb_itr)
922  {
923  //Get a reference to the data contained in the leaf
924  VoxelData& neighbor_voxel = ((*neighb_itr)->getData ());
925  //TODO this is a shortcut, really we should always recompute distance
926  if(neighbor_voxel.owner_ == this)
927  continue;
928  //Compute distance to the neighbor
929  float dist = parent_->voxelDataDistance (centroid_, neighbor_voxel);
930  //If distance is less than previous, we remove it from its owner's list
931  //and change the owner to this and distance (we *steal* it!)
932  if (dist < neighbor_voxel.distance_)
933  {
934  neighbor_voxel.distance_ = dist;
935  if (neighbor_voxel.owner_ != this)
936  {
937  if (neighbor_voxel.owner_)
938  (neighbor_voxel.owner_)->removeLeaf(*neighb_itr);
939  neighbor_voxel.owner_ = this;
940  new_owned.push_back (*neighb_itr);
941  }
942  }
943  }
944  }
945  //Push all new owned onto the owned leaf set
946  typename std::vector<LeafContainerT*>::iterator new_owned_itr;
947  for (new_owned_itr=new_owned.begin (); new_owned_itr!=new_owned.end (); ++new_owned_itr)
948  {
949  leaves_.insert (*new_owned_itr);
950  }
951 
952 }
953 
954 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
955 template <typename PointT> void
957 {
958  typename std::set<LeafContainerT*>::iterator leaf_itr;
959  //For each leaf belonging to this supervoxel, get its neighbors, build an index vector, compute normal
960  for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr)
961  {
962  VoxelData& voxel_data = (*leaf_itr)->getData ();
963  std::vector<int> indices;
964  indices.reserve (81);
965  //Push this point
966  indices.push_back (voxel_data.idx_);
967  for (typename LeafContainerT::iterator neighb_itr=(*leaf_itr)->begin (); neighb_itr!=(*leaf_itr)->end (); ++neighb_itr)
968  {
969  //Get a reference to the data contained in the leaf
970  VoxelData& neighbor_voxel_data = ((*neighb_itr)->getData ());
971  //If the neighbor is in this supervoxel, use it
972  if (neighbor_voxel_data.owner_ == this)
973  {
974  indices.push_back (neighbor_voxel_data.idx_);
975  //Also check its neighbors
976  for (typename LeafContainerT::iterator neighb_neighb_itr=(*neighb_itr)->begin (); neighb_neighb_itr!=(*neighb_itr)->end (); ++neighb_neighb_itr)
977  {
978  VoxelData& neighb_neighb_voxel_data = (*neighb_neighb_itr)->getData ();
979  if (neighb_neighb_voxel_data.owner_ == this)
980  indices.push_back (neighb_neighb_voxel_data.idx_);
981  }
982 
983 
984  }
985  }
986  //Compute normal
987  pcl::computePointNormal (*parent_->voxel_centroid_cloud_, indices, voxel_data.normal_, voxel_data.curvature_);
988  pcl::flipNormalTowardsViewpoint (parent_->voxel_centroid_cloud_->points[voxel_data.idx_], 0.0f,0.0f,0.0f, voxel_data.normal_);
989  voxel_data.normal_[3] = 0.0f;
990  voxel_data.normal_.normalize ();
991  }
992 }
993 
994 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
995 template <typename PointT> void
997 {
998  centroid_.normal_ = Eigen::Vector4f::Zero ();
999  centroid_.xyz_ = Eigen::Vector3f::Zero ();
1000  centroid_.rgb_ = Eigen::Vector3f::Zero ();
1001  typename std::set<LeafContainerT*>::iterator leaf_itr = leaves_.begin ();
1002  for ( ; leaf_itr!= leaves_.end (); ++leaf_itr)
1003  {
1004  const VoxelData& leaf_data = (*leaf_itr)->getData ();
1005  centroid_.normal_ += leaf_data.normal_;
1006  centroid_.xyz_ += leaf_data.xyz_;
1007  centroid_.rgb_ += leaf_data.rgb_;
1008  }
1009  centroid_.normal_.normalize ();
1010  centroid_.xyz_ /= static_cast<float> (leaves_.size ());
1011  centroid_.rgb_ /= static_cast<float> (leaves_.size ());
1012 
1013 }
1014 
1015 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
1016 template <typename PointT> void
1018 {
1019  voxels = boost::make_shared<pcl::PointCloud<PointT> > ();
1020  voxels->clear ();
1021  voxels->resize (leaves_.size ());
1022  typename pcl::PointCloud<PointT>::iterator voxel_itr = voxels->begin ();
1023  typename std::set<LeafContainerT*>::iterator leaf_itr;
1024  for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr, ++voxel_itr)
1025  {
1026  const VoxelData& leaf_data = (*leaf_itr)->getData ();
1027  leaf_data.getPoint (*voxel_itr);
1028  }
1029 }
1030 
1031 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
1032 template <typename PointT> void
1034 {
1035  normals = boost::make_shared<pcl::PointCloud<Normal> > ();
1036  normals->clear ();
1037  normals->resize (leaves_.size ());
1038  typename std::set<LeafContainerT*>::iterator leaf_itr;
1039  typename pcl::PointCloud<Normal>::iterator normal_itr = normals->begin ();
1040  for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr, ++normal_itr)
1041  {
1042  const VoxelData& leaf_data = (*leaf_itr)->getData ();
1043  leaf_data.getNormal (*normal_itr);
1044  }
1045 }
1046 
1047 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
1048 template <typename PointT> void
1049 pcl::SupervoxelClustering<PointT>::SupervoxelHelper::getNeighborLabels (std::set<uint32_t> &neighbor_labels) const
1050 {
1051  neighbor_labels.clear ();
1052  //For each leaf belonging to this supervoxel
1053  typename std::set<LeafContainerT*>::iterator leaf_itr;
1054  for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr)
1055  {
1056  //for each neighbor of the leaf
1057  for (typename LeafContainerT::iterator neighb_itr=(*leaf_itr)->begin (); neighb_itr!=(*leaf_itr)->end (); ++neighb_itr)
1058  {
1059  //Get a reference to the data contained in the leaf
1060  VoxelData& neighbor_voxel = ((*neighb_itr)->getData ());
1061  //If it has an owner, and it's not us - get it's owner's label insert into set
1062  if (neighbor_voxel.owner_ != this && neighbor_voxel.owner_)
1063  {
1064  neighbor_labels.insert (neighbor_voxel.owner_->getLabel ());
1065  }
1066  }
1067  }
1068 }
1069 
1070 
1071 #endif // PCL_SUPERVOXEL_CLUSTERING_HPP_
1072 
void setVoxelResolution(float resolution)
Set the resolution of the octree voxels.
pcl::PointCloud< pcl::PointXYZRGBA >::Ptr getColoredVoxelCloud() const
Returns an RGB colorized voxelized cloud showing superpixels Otherwise it returns an empty pointer...
int getMaxLabel() const
Returns the current maximum (highest) 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.
iterator end()
Definition: point_cloud.h:435
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.
VectorType::iterator iterator
Definition: point_cloud.h:432
VoxelAdjacencyList::edge_descriptor EdgeID
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
Octree adjacency leaf container class- stores set of pointers to neighbors, number of points added...
pcl::PointCloud< PointXYZRGBA >::Ptr getColoredCloud() const
Returns an RGB colorized cloud showing superpixels Otherwise it returns an empty pointer.
float getSeedResolution() const
Get the resolution of the octree seed voxels.
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...
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.
pcl::PointCloud< pcl::PointXYZL >::Ptr getLabeledVoxelCloud() const
Returns labeled voxelized cloud Points that belong to the same supervoxel have the same label...
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
float getVoxelResolution() const
Get the resolution of the octree voxels.
void setSpatialImportance(float val)
Set the importance of spatial distance for supervoxels.
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) ...
void getNormal(Normal &normal_arg) const
Gets the data of in the form of a normal.
VectorType::const_iterator const_iterator
Definition: point_cloud.h:433
A point structure representing normal coordinates and the surface curvature estimate.
VoxelAdjacencyList::vertex_descriptor VoxelID
void setColorImportance(float val)
Set the importance of color for supervoxels.
boost::adjacency_list< boost::setS, boost::setS, boost::undirectedS, uint32_t, float > VoxelAdjacencyList
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, and the RGBA color.
iterator begin()
Definition: point_cloud.h:434
void getPoint(PointT &point_arg) const
Gets the data of in the form of a point.
void setSeedResolution(float seed_resolution)
Set the resolution of the octree seed voxels.
Octree pointcloud search class
Definition: octree_search.h:58
void resize(size_t n)
Resize the cloud.
Definition: point_cloud.h:447
void getSupervoxelAdjacency(std::multimap< uint32_t, uint32_t > &label_adjacency) const
Get a multimap which gives supervoxel adjacency.
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
pcl::PointCloud< PointXYZL >::Ptr getLabeledCloud() const
Returns labeled cloud Points that belong to the same supervoxel have the same label.
VoxelData is a structure used for storing data within a pcl::octree::OctreePointCloudAdjacencyContain...
boost::shared_ptr< Supervoxel< PointT > > Ptr
Implements a supervoxel algorithm based on voxel structure, normals, and rgb values.
A point structure representing Euclidean xyz coordinates, and the RGB color.
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 ~SupervoxelClustering()
This destructor destroys the cloud, normals and search method used for finding neighbors.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void clear()
Removes all points in a cloud and sets the width and height to 0.
Definition: point_cloud.h:567
void getSupervoxelAdjacencyList(VoxelAdjacencyList &adjacency_list_arg) const
Gets the adjacency list (Boost Graph library) which gives connections between supervoxels.
pcl::PointCloud< PointT >::Ptr getVoxelCentroidCloud() const
Returns a deep copy of the voxel centroid cloud.
virtual void setInputCloud(typename pcl::PointCloud< PointT >::ConstPtr cloud)
This method sets the cloud to be supervoxelized.
size_t size() const
Definition: point_cloud.h:440
void setNormalImportance(float val)
Set the importance of scalar normal product for supervoxels.