Point Cloud Library (PCL)  1.9.1-dev
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) :
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  use_default_transform_behaviour_ (true)
56 {
57  adjacency_octree_.reset (new OctreeAdjacencyT (resolution_));
58 }
59 
60 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
61 template <typename PointT>
63 {
64 
65 }
66 
67 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
68 template <typename PointT> void
70 {
71  if ( cloud->empty () )
72  {
73  PCL_ERROR ("[pcl::SupervoxelClustering::setInputCloud] Empty cloud set, doing nothing \n");
74  return;
75  }
76 
77  input_ = cloud;
78  adjacency_octree_->setInputCloud (cloud);
79 }
80 
81 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
82 template <typename PointT> void
84 {
85  if ( normal_cloud->empty () )
86  {
87  PCL_ERROR ("[pcl::SupervoxelClustering::setNormalCloud] Empty cloud set, doing nothing \n");
88  return;
89  }
90 
91  input_normals_ = normal_cloud;
92 }
93 
94 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
95 template <typename PointT> void
96 pcl::SupervoxelClustering<PointT>::extract (std::map<uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters)
97 {
98  //timer_.reset ();
99  //double t_start = timer_.getTime ();
100  //std::cout << "Init compute \n";
101  bool segmentation_is_possible = initCompute ();
102  if ( !segmentation_is_possible )
103  {
104  deinitCompute ();
105  return;
106  }
107 
108  //std::cout << "Preparing for segmentation \n";
109  segmentation_is_possible = prepareForSegmentation ();
110  if ( !segmentation_is_possible )
111  {
112  deinitCompute ();
113  return;
114  }
115 
116  //double t_prep = timer_.getTime ();
117  //std::cout << "Placing Seeds" << std::endl;
118  std::vector<int> seed_indices;
119  selectInitialSupervoxelSeeds (seed_indices);
120  //std::cout << "Creating helpers "<<std::endl;
121  createSupervoxelHelpers (seed_indices);
122  //double t_seeds = timer_.getTime ();
123 
124 
125  //std::cout << "Expanding the supervoxels" << std::endl;
126  int max_depth = static_cast<int> (1.8f*seed_resolution_/resolution_);
127  expandSupervoxels (max_depth);
128  //double t_iterate = timer_.getTime ();
129 
130  //std::cout << "Making Supervoxel structures" << std::endl;
131  makeSupervoxels (supervoxel_clusters);
132  //double t_supervoxels = timer_.getTime ();
133 
134  // std::cout << "--------------------------------- Timing Report --------------------------------- \n";
135  // std::cout << "Time to prep (normals, neighbors, voxelization)="<<t_prep-t_start<<" ms\n";
136  // std::cout << "Time to seed clusters ="<<t_seeds-t_prep<<" ms\n";
137  // std::cout << "Time to expand clusters ="<<t_iterate-t_seeds<<" ms\n";
138  // std::cout << "Time to create supervoxel structures ="<<t_supervoxels-t_iterate<<" ms\n";
139  // std::cout << "Total run time ="<<t_supervoxels-t_start<<" ms\n";
140  // std::cout << "--------------------------------------------------------------------------------- \n";
141 
142  deinitCompute ();
143 }
144 
145 
146 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
147 template <typename PointT> void
148 pcl::SupervoxelClustering<PointT>::refineSupervoxels (int num_itr, std::map<uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters)
149 {
150  if (supervoxel_helpers_.empty ())
151  {
152  PCL_ERROR ("[pcl::SupervoxelClustering::refineVoxelNormals] Supervoxels not extracted, doing nothing - (Call extract first!) \n");
153  return;
154  }
155 
156  int max_depth = static_cast<int> (1.8f*seed_resolution_/resolution_);
157  for (int i = 0; i < num_itr; ++i)
158  {
159  for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr)
160  {
161  sv_itr->refineNormals ();
162  }
163 
164  reseedSupervoxels ();
165  expandSupervoxels (max_depth);
166  }
167 
168 
169  makeSupervoxels (supervoxel_clusters);
170 
171 }
172 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
173 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
174 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
175 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
176 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
177 
178 
179 template <typename PointT> bool
181 {
182 
183  // if user forgot to pass point cloud or if it is empty
184  if ( input_->points.empty () )
185  return (false);
186 
187  //Add the new cloud of data to the octree
188  //std::cout << "Populating adjacency octree with new cloud \n";
189  //double prep_start = timer_.getTime ();
190  if ( (use_default_transform_behaviour_ && input_->isOrganized ())
191  || (!use_default_transform_behaviour_ && use_single_camera_transform_))
192  adjacency_octree_->setTransformFunction ([this] (PointT &p) { transformFunction (p); });
193 
194  adjacency_octree_->addPointsFromInputCloud ();
195  //double prep_end = timer_.getTime ();
196  //std::cout<<"Time elapsed populating octree with next frame ="<<prep_end-prep_start<<" ms\n";
197 
198  //Compute normals and insert data for centroids into data field of octree
199  //double normals_start = timer_.getTime ();
200  computeVoxelData ();
201  //double normals_end = timer_.getTime ();
202  //std::cout << "Time elapsed finding normals and pushing into octree ="<<normals_end-normals_start<<" ms\n";
203 
204  return true;
205 }
206 
207 template <typename PointT> void
209 {
210  voxel_centroid_cloud_.reset (new PointCloudT);
211  voxel_centroid_cloud_->resize (adjacency_octree_->getLeafCount ());
212  typename LeafVectorT::iterator leaf_itr = adjacency_octree_->begin ();
213  typename PointCloudT::iterator cent_cloud_itr = voxel_centroid_cloud_->begin ();
214  for (int idx = 0 ; leaf_itr != adjacency_octree_->end (); ++leaf_itr, ++cent_cloud_itr, ++idx)
215  {
216  VoxelData& new_voxel_data = (*leaf_itr)->getData ();
217  //Add the point to the centroid cloud
218  new_voxel_data.getPoint (*cent_cloud_itr);
219  //voxel_centroid_cloud_->push_back(new_voxel_data.getPoint ());
220  new_voxel_data.idx_ = idx;
221  }
222 
223  //If normals were provided
224  if (input_normals_)
225  {
226  //Verify that input normal cloud size is same as input cloud size
227  assert (input_normals_->size () == input_->size ());
228  //For every point in the input cloud, find its corresponding leaf
229  typename NormalCloudT::const_iterator normal_itr = input_normals_->begin ();
230  for (typename PointCloudT::const_iterator input_itr = input_->begin (); input_itr != input_->end (); ++input_itr, ++normal_itr)
231  {
232  //If the point is not finite we ignore it
233  if ( !pcl::isFinite<PointT> (*input_itr))
234  continue;
235  //Otherwise look up its leaf container
236  LeafContainerT* leaf = adjacency_octree_->getLeafContainerAtPoint (*input_itr);
237 
238  //Get the voxel data object
239  VoxelData& voxel_data = leaf->getData ();
240  //Add this normal in (we will normalize at the end)
241  voxel_data.normal_ += normal_itr->getNormalVector4fMap ();
242  voxel_data.curvature_ += normal_itr->curvature;
243  }
244  //Now iterate through the leaves and normalize
245  for (leaf_itr = adjacency_octree_->begin (); leaf_itr != adjacency_octree_->end (); ++leaf_itr)
246  {
247  VoxelData& voxel_data = (*leaf_itr)->getData ();
248  voxel_data.normal_.normalize ();
249  voxel_data.owner_ = nullptr;
250  voxel_data.distance_ = std::numeric_limits<float>::max ();
251  //Get the number of points in this leaf
252  int num_points = (*leaf_itr)->getPointCounter ();
253  voxel_data.curvature_ /= num_points;
254  }
255  }
256  else //Otherwise just compute the normals
257  {
258  for (leaf_itr = adjacency_octree_->begin (); leaf_itr != adjacency_octree_->end (); ++leaf_itr)
259  {
260  VoxelData& new_voxel_data = (*leaf_itr)->getData ();
261  //For every point, get its neighbors, build an index vector, compute normal
262  std::vector<int> indices;
263  indices.reserve (81);
264  //Push this point
265  indices.push_back (new_voxel_data.idx_);
266  for (typename LeafContainerT::const_iterator neighb_itr=(*leaf_itr)->cbegin (); neighb_itr!=(*leaf_itr)->cend (); ++neighb_itr)
267  {
268  VoxelData& neighb_voxel_data = (*neighb_itr)->getData ();
269  //Push neighbor index
270  indices.push_back (neighb_voxel_data.idx_);
271  //Get neighbors neighbors, push onto cloud
272  for (typename LeafContainerT::const_iterator neighb_neighb_itr=(*neighb_itr)->cbegin (); neighb_neighb_itr!=(*neighb_itr)->cend (); ++neighb_neighb_itr)
273  {
274  VoxelData& neighb2_voxel_data = (*neighb_neighb_itr)->getData ();
275  indices.push_back (neighb2_voxel_data.idx_);
276  }
277  }
278  //Compute normal
279  pcl::computePointNormal (*voxel_centroid_cloud_, indices, new_voxel_data.normal_, new_voxel_data.curvature_);
280  pcl::flipNormalTowardsViewpoint (voxel_centroid_cloud_->points[new_voxel_data.idx_], 0.0f,0.0f,0.0f, new_voxel_data.normal_);
281  new_voxel_data.normal_[3] = 0.0f;
282  new_voxel_data.normal_.normalize ();
283  new_voxel_data.owner_ = nullptr;
284  new_voxel_data.distance_ = std::numeric_limits<float>::max ();
285  }
286  }
287 
288 
289 }
290 
291 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
292 template <typename PointT> void
294 {
295 
296 
297  for (int i = 1; i < depth; ++i)
298  {
299  //Expand the the supervoxels by one iteration
300  for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr)
301  {
302  sv_itr->expand ();
303  }
304 
305  //Update the centers to reflect new centers
306  for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); )
307  {
308  if (sv_itr->size () == 0)
309  {
310  sv_itr = supervoxel_helpers_.erase (sv_itr);
311  }
312  else
313  {
314  sv_itr->updateCentroid ();
315  ++sv_itr;
316  }
317  }
318 
319  }
320 
321 }
322 
323 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
324 template <typename PointT> void
325 pcl::SupervoxelClustering<PointT>::makeSupervoxels (std::map<uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters)
326 {
327  supervoxel_clusters.clear ();
328  for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr)
329  {
330  uint32_t label = sv_itr->getLabel ();
331  supervoxel_clusters[label].reset (new Supervoxel<PointT>);
332  sv_itr->getXYZ (supervoxel_clusters[label]->centroid_.x,supervoxel_clusters[label]->centroid_.y,supervoxel_clusters[label]->centroid_.z);
333  sv_itr->getRGB (supervoxel_clusters[label]->centroid_.rgba);
334  sv_itr->getNormal (supervoxel_clusters[label]->normal_);
335  sv_itr->getVoxels (supervoxel_clusters[label]->voxels_);
336  sv_itr->getNormals (supervoxel_clusters[label]->normals_);
337  }
338 }
339 
340 
341 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
342 template <typename PointT> void
343 pcl::SupervoxelClustering<PointT>::createSupervoxelHelpers (std::vector<int> &seed_indices)
344 {
345 
346  supervoxel_helpers_.clear ();
347  for (size_t i = 0; i < seed_indices.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_->at(seed_indices[i]);//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
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_indices.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_)
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_indices.reserve (seed_indices_orig.size ());
402  float search_radius = 0.5f*seed_resolution_;
403  // This is 1/20th of the number of voxels which fit in a planar slice through search volume
404  // Area of planar slice / area of voxel side. (Note: This is smaller than the value mentioned in the original paper)
405  float min_points = 0.05f * (search_radius)*(search_radius) * 3.1415926536f / (resolution_*resolution_);
406  for (const int &index_orig : seed_indices_orig)
407  {
408  int num = voxel_kdtree_->radiusSearch (index_orig, search_radius , neighbors, sqr_distances);
409  int min_index = index_orig;
410  if ( num > min_points)
411  {
412  seed_indices.push_back (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_->at (closest_index[0]);
441  if (seed_leaf)
442  {
443  sv_itr->addLeaf (seed_leaf);
444  }
445  else
446  {
447  PCL_WARN ("Could not find leaf in pcl::SupervoxelClustering<PointT>::reseedSupervoxels - supervoxel will be deleted \n");
448  }
449  }
450 
451 }
452 
453 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
454 template <typename PointT> void
456 {
457  p.x /= p.z;
458  p.y /= p.z;
459  p.z = std::log (p.z);
460 }
461 
462 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
463 template <typename PointT> float
465 {
466 
467  float spatial_dist = (v1.xyz_ - v2.xyz_).norm () / seed_resolution_;
468  float color_dist = (v1.rgb_ - v2.rgb_).norm () / 255.0f;
469  float cos_angle_normal = 1.0f - std::abs (v1.normal_.dot (v2.normal_));
470  // std::cout << "s="<<spatial_dist<<" c="<<color_dist<<" an="<<cos_angle_normal<<"\n";
471  return cos_angle_normal * normal_importance_ + color_dist * color_importance_+ spatial_dist * spatial_importance_;
472 
473 }
474 
475 
476 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
477 ///////// GETTER FUNCTIONS
478 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
479 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
480 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
481 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
482 template <typename PointT> void
484 {
485  adjacency_list_arg.clear ();
486  //Add a vertex for each label, store ids in map
487  std::map <uint32_t, VoxelID> label_ID_map;
488  for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
489  {
490  VoxelID node_id = add_vertex (adjacency_list_arg);
491  adjacency_list_arg[node_id] = (sv_itr->getLabel ());
492  label_ID_map.insert (std::make_pair (sv_itr->getLabel (), node_id));
493  }
494 
495  for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
496  {
497  uint32_t label = sv_itr->getLabel ();
498  std::set<uint32_t> neighbor_labels;
499  sv_itr->getNeighborLabels (neighbor_labels);
500  for (const unsigned int &neighbor_label : neighbor_labels)
501  {
502  bool edge_added;
503  EdgeID edge;
504  VoxelID u = (label_ID_map.find (label))->second;
505  VoxelID v = (label_ID_map.find (neighbor_label))->second;
506  boost::tie (edge, edge_added) = add_edge (u,v,adjacency_list_arg);
507  //Calc distance between centers, set as edge weight
508  if (edge_added)
509  {
510  VoxelData centroid_data = (sv_itr)->getCentroid ();
511  //Find the neighbhor with this label
512  VoxelData neighb_centroid_data;
513 
514  for (typename HelperListT::const_iterator neighb_itr = supervoxel_helpers_.cbegin (); neighb_itr != supervoxel_helpers_.cend (); ++neighb_itr)
515  {
516  if (neighb_itr->getLabel () == neighbor_label)
517  {
518  neighb_centroid_data = neighb_itr->getCentroid ();
519  break;
520  }
521  }
522 
523  float length = voxelDataDistance (centroid_data, neighb_centroid_data);
524  adjacency_list_arg[edge] = length;
525  }
526  }
527 
528  }
529 
530 }
531 
532 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
533 template <typename PointT> void
534 pcl::SupervoxelClustering<PointT>::getSupervoxelAdjacency (std::multimap<uint32_t, uint32_t> &label_adjacency) const
535 {
536  label_adjacency.clear ();
537  for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
538  {
539  uint32_t label = sv_itr->getLabel ();
540  std::set<uint32_t> neighbor_labels;
541  sv_itr->getNeighborLabels (neighbor_labels);
542  for (const unsigned int &neighbor_label : neighbor_labels)
543  label_adjacency.insert (std::pair<uint32_t,uint32_t> (label, neighbor_label) );
544  //if (neighbor_labels.size () == 0)
545  // std::cout << label<<"(size="<<sv_itr->size () << ") has "<<neighbor_labels.size () << "\n";
546  }
547 }
548 
549 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
550 template <typename PointT> typename pcl::PointCloud<PointT>::Ptr
552 {
553  typename PointCloudT::Ptr centroid_copy (new PointCloudT);
554  copyPointCloud (*voxel_centroid_cloud_, *centroid_copy);
555  return centroid_copy;
556 }
557 
558 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
559 template <typename PointT> pcl::PointCloud<pcl::PointXYZL>::Ptr
561 {
563  for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
564  {
565  typename PointCloudT::Ptr voxels;
566  sv_itr->getVoxels (voxels);
568  copyPointCloud (*voxels, xyzl_copy);
569 
570  pcl::PointCloud<pcl::PointXYZL>::iterator xyzl_copy_itr = xyzl_copy.begin ();
571  for ( ; xyzl_copy_itr != xyzl_copy.end (); ++xyzl_copy_itr)
572  xyzl_copy_itr->label = sv_itr->getLabel ();
573 
574  *labeled_voxel_cloud += xyzl_copy;
575  }
576 
577  return labeled_voxel_cloud;
578 }
579 
580 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
581 template <typename PointT> pcl::PointCloud<pcl::PointXYZL>::Ptr
583 {
585  pcl::copyPointCloud (*input_,*labeled_cloud);
586 
587  typename pcl::PointCloud <PointT>::const_iterator i_input = input_->begin ();
588  std::vector <int> indices;
589  std::vector <float> sqr_distances;
590  for (auto i_labeled = labeled_cloud->begin (); i_labeled != labeled_cloud->end (); ++i_labeled,++i_input)
591  {
592  if ( !pcl::isFinite<PointT> (*i_input))
593  i_labeled->label = 0;
594  else
595  {
596  i_labeled->label = 0;
597  LeafContainerT *leaf = adjacency_octree_->getLeafContainerAtPoint (*i_input);
598  VoxelData& voxel_data = leaf->getData ();
599  if (voxel_data.owner_)
600  i_labeled->label = voxel_data.owner_->getLabel ();
601 
602  }
603 
604  }
605 
606  return (labeled_cloud);
607 }
608 
609 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
610 template <typename PointT> pcl::PointCloud<pcl::PointNormal>::Ptr
612 {
614  normal_cloud->resize (supervoxel_clusters.size ());
615  pcl::PointCloud<pcl::PointNormal>::iterator normal_cloud_itr = normal_cloud->begin ();
616  for (auto sv_itr = supervoxel_clusters.cbegin (), sv_itr_end = supervoxel_clusters.cend ();
617  sv_itr != sv_itr_end; ++sv_itr, ++normal_cloud_itr)
618  {
619  (sv_itr->second)->getCentroidPointNormal (*normal_cloud_itr);
620  }
621  return normal_cloud;
622 }
623 
624 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
625 template <typename PointT> float
627 {
628  return (resolution_);
629 }
630 
631 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
632 template <typename PointT> void
634 {
635  resolution_ = resolution;
636 
637 }
638 
639 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
640 template <typename PointT> float
642 {
643  return (seed_resolution_);
644 }
645 
646 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
647 template <typename PointT> void
649 {
650  seed_resolution_ = seed_resolution;
651 }
652 
653 
654 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
655 template <typename PointT> void
657 {
658  color_importance_ = val;
659 }
660 
661 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
662 template <typename PointT> void
664 {
665  spatial_importance_ = val;
666 }
667 
668 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
669 template <typename PointT> void
671 {
672  normal_importance_ = val;
673 }
674 
675 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
676 template <typename PointT> void
678 {
679  use_default_transform_behaviour_ = false;
680  use_single_camera_transform_ = val;
681 }
682 
683 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
684 template <typename PointT> int
686 {
687  int max_label = 0;
688  for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
689  {
690  int temp = sv_itr->getLabel ();
691  if (temp > max_label)
692  max_label = temp;
693  }
694  return max_label;
695 }
696 
697 namespace pcl
698 {
699  namespace octree
700  {
701  //Explicit overloads for RGB types
702  template<>
703  void
705 
706  template<>
707  void
709 
710  //Explicit overloads for RGB types
711  template<> void
713 
714  template<> void
716 
717  //Explicit overloads for XYZ types
718  template<>
719  void
721 
722  template<> void
724  }
725 }
726 
727 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
728 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
729 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
730 namespace pcl
731 {
732 
733  template<> void
735 
736  template<> void
738 
739  template<typename PointT> void
741  {
742  //XYZ is required or this doesn't make much sense...
743  point_arg.x = xyz_[0];
744  point_arg.y = xyz_[1];
745  point_arg.z = xyz_[2];
746  }
747 
748  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
749  template <typename PointT> void
751  {
752  normal_arg.normal_x = normal_[0];
753  normal_arg.normal_y = normal_[1];
754  normal_arg.normal_z = normal_[2];
755  normal_arg.curvature = curvature_;
756  }
757 }
758 
759 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
760 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
761 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
762 template <typename PointT> void
764 {
765  leaves_.insert (leaf_arg);
766  VoxelData& voxel_data = leaf_arg->getData ();
767  voxel_data.owner_ = this;
768 }
769 
770 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
771 template <typename PointT> void
773 {
774  leaves_.erase (leaf_arg);
775 }
776 
777 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
778 template <typename PointT> void
780 {
781  for (auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr)
782  {
783  VoxelData& voxel = ((*leaf_itr)->getData ());
784  voxel.owner_ = nullptr;
785  voxel.distance_ = std::numeric_limits<float>::max ();
786  }
787  leaves_.clear ();
788 }
789 
790 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
791 template <typename PointT> void
793 {
794  //std::cout << "Expanding sv "<<label_<<", owns "<<leaves_.size ()<<" voxels\n";
795  //Buffer of new neighbors - initial size is just a guess of most possible
796  std::vector<LeafContainerT*> new_owned;
797  new_owned.reserve (leaves_.size () * 9);
798  //For each leaf belonging to this supervoxel
799  for (auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr)
800  {
801  //for each neighbor of the leaf
802  for (typename LeafContainerT::const_iterator neighb_itr=(*leaf_itr)->cbegin (); neighb_itr!=(*leaf_itr)->cend (); ++neighb_itr)
803  {
804  //Get a reference to the data contained in the leaf
805  VoxelData& neighbor_voxel = ((*neighb_itr)->getData ());
806  //TODO this is a shortcut, really we should always recompute distance
807  if(neighbor_voxel.owner_ == this)
808  continue;
809  //Compute distance to the neighbor
810  float dist = parent_->voxelDataDistance (centroid_, neighbor_voxel);
811  //If distance is less than previous, we remove it from its owner's list
812  //and change the owner to this and distance (we *steal* it!)
813  if (dist < neighbor_voxel.distance_)
814  {
815  neighbor_voxel.distance_ = dist;
816  if (neighbor_voxel.owner_ != this)
817  {
818  if (neighbor_voxel.owner_)
819  (neighbor_voxel.owner_)->removeLeaf(*neighb_itr);
820  neighbor_voxel.owner_ = this;
821  new_owned.push_back (*neighb_itr);
822  }
823  }
824  }
825  }
826  //Push all new owned onto the owned leaf set
827  for (auto new_owned_itr = new_owned.cbegin (); new_owned_itr != new_owned.cend (); ++new_owned_itr)
828  {
829  leaves_.insert (*new_owned_itr);
830  }
831 }
832 
833 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
834 template <typename PointT> void
836 {
837  //For each leaf belonging to this supervoxel, get its neighbors, build an index vector, compute normal
838  for (auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr)
839  {
840  VoxelData& voxel_data = (*leaf_itr)->getData ();
841  std::vector<int> indices;
842  indices.reserve (81);
843  //Push this point
844  indices.push_back (voxel_data.idx_);
845  for (typename LeafContainerT::const_iterator neighb_itr=(*leaf_itr)->cbegin (); neighb_itr!=(*leaf_itr)->cend (); ++neighb_itr)
846  {
847  //Get a reference to the data contained in the leaf
848  VoxelData& neighbor_voxel_data = ((*neighb_itr)->getData ());
849  //If the neighbor is in this supervoxel, use it
850  if (neighbor_voxel_data.owner_ == this)
851  {
852  indices.push_back (neighbor_voxel_data.idx_);
853  //Also check its neighbors
854  for (typename LeafContainerT::const_iterator neighb_neighb_itr=(*neighb_itr)->cbegin (); neighb_neighb_itr!=(*neighb_itr)->cend (); ++neighb_neighb_itr)
855  {
856  VoxelData& neighb_neighb_voxel_data = (*neighb_neighb_itr)->getData ();
857  if (neighb_neighb_voxel_data.owner_ == this)
858  indices.push_back (neighb_neighb_voxel_data.idx_);
859  }
860 
861 
862  }
863  }
864  //Compute normal
865  pcl::computePointNormal (*parent_->voxel_centroid_cloud_, indices, voxel_data.normal_, voxel_data.curvature_);
866  pcl::flipNormalTowardsViewpoint (parent_->voxel_centroid_cloud_->points[voxel_data.idx_], 0.0f,0.0f,0.0f, voxel_data.normal_);
867  voxel_data.normal_[3] = 0.0f;
868  voxel_data.normal_.normalize ();
869  }
870 }
871 
872 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
873 template <typename PointT> void
875 {
876  centroid_.normal_ = Eigen::Vector4f::Zero ();
877  centroid_.xyz_ = Eigen::Vector3f::Zero ();
878  centroid_.rgb_ = Eigen::Vector3f::Zero ();
879  for (auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr)
880  {
881  const VoxelData& leaf_data = (*leaf_itr)->getData ();
882  centroid_.normal_ += leaf_data.normal_;
883  centroid_.xyz_ += leaf_data.xyz_;
884  centroid_.rgb_ += leaf_data.rgb_;
885  }
886  centroid_.normal_.normalize ();
887  centroid_.xyz_ /= static_cast<float> (leaves_.size ());
888  centroid_.rgb_ /= static_cast<float> (leaves_.size ());
889 }
890 
891 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
892 template <typename PointT> void
894 {
895  voxels.reset (new pcl::PointCloud<PointT>);
896  voxels->clear ();
897  voxels->resize (leaves_.size ());
898  typename pcl::PointCloud<PointT>::iterator voxel_itr = voxels->begin ();
899  for (auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr, ++voxel_itr)
900  {
901  const VoxelData& leaf_data = (*leaf_itr)->getData ();
902  leaf_data.getPoint (*voxel_itr);
903  }
904 }
905 
906 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
907 template <typename PointT> void
909 {
910  normals.reset (new pcl::PointCloud<Normal>);
911  normals->clear ();
912  normals->resize (leaves_.size ());
913  typename pcl::PointCloud<Normal>::iterator normal_itr = normals->begin ();
914  for (auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr, ++normal_itr)
915  {
916  const VoxelData& leaf_data = (*leaf_itr)->getData ();
917  leaf_data.getNormal (*normal_itr);
918  }
919 }
920 
921 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
922 template <typename PointT> void
923 pcl::SupervoxelClustering<PointT>::SupervoxelHelper::getNeighborLabels (std::set<uint32_t> &neighbor_labels) const
924 {
925  neighbor_labels.clear ();
926  //For each leaf belonging to this supervoxel
927  for (auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr)
928  {
929  //for each neighbor of the leaf
930  for (typename LeafContainerT::const_iterator neighb_itr=(*leaf_itr)->cbegin (); neighb_itr!=(*leaf_itr)->cend (); ++neighb_itr)
931  {
932  //Get a reference to the data contained in the leaf
933  VoxelData& neighbor_voxel = ((*neighb_itr)->getData ());
934  //If it has an owner, and it's not us - get it's owner's label insert into set
935  if (neighbor_voxel.owner_ != this && neighbor_voxel.owner_)
936  {
937  neighbor_labels.insert (neighbor_voxel.owner_->getLabel ());
938  }
939  }
940  }
941 }
942 
943 
944 #endif // PCL_SUPERVOXEL_CLUSTERING_HPP_
A point structure representing normal coordinates and the surface curvature estimate.
void setSeedResolution(float seed_resolution)
Set the resolution of the octree seed voxels.
int radiusSearch(const PointT &point, double radius, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn=0) const override
Search for all the nearest neighbors of the query point in a given radius.
Definition: kdtree.hpp:97
void addPointsFromInputCloud()
Add points from input point cloud to octree.
Octree adjacency leaf container class- stores a list of pointers to neighbors, number of points added...
VoxelAdjacencyList::vertex_descriptor VoxelID
typename VectorType::const_iterator const_iterator
Definition: point_cloud.h:442
Octree pointcloud voxel class which maintains adjacency information for its voxels.
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
void setNormalImportance(float val)
Set the importance of scalar normal product for supervoxels.
iterator end()
Definition: point_cloud.h:444
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.
void setInputCloud(const typename pcl::PointCloud< PointT >::ConstPtr &cloud) override
This method sets the cloud to be supervoxelized.
pcl::PointCloud< PointT >::Ptr getVoxelCentroidCloud() const
Returns a deep copy of the voxel centroid cloud.
void setVoxelResolution(float resolution)
Set the resolution of the octree voxels.
void setInputCloud(const PointCloudConstPtr &cloud_arg, const IndicesConstPtr &indices_arg=IndicesConstPtr())
Provide a pointer to the input data set.
A point structure representing Euclidean xyz coordinates, and the RGBA color.
bool 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
bool initCompute()
This method should get called before starting the actual computation.
Definition: pcl_base.hpp:138
SupervoxelClustering(float voxel_resolution, float seed_resolution)
Constructor that sets default values for member variables.
Supervoxel container class - stores a cluster extracted using supervoxel clustering.
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:121
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.
int getOccupiedVoxelCenters(AlignedPointTVector &voxel_center_list_arg) const
Get a PointT vector of centers of all occupied voxels.
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 setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr()) override
Provide a pointer to the input dataset.
Definition: kdtree.hpp:77
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...
void setUseSingleCameraTransform(bool val)
Set whether or not to use the single camera transform.
VoxelData is a structure used for storing data within a pcl::octree::OctreePointCloudAdjacencyContain...
boost::shared_ptr< Supervoxel< PointT > > Ptr
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.
void getNormal(Normal &normal_arg) const
Gets the data of in the form of a normal.
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:429
void clear()
Removes all points in a cloud and sets the width and height to 0.
Definition: point_cloud.h:606
pcl::PointCloud< pcl::PointXYZL >::Ptr getLabeledVoxelCloud() const
Returns labeled voxelized cloud Points that belong to the same supervoxel have the same label...
bool deinitCompute()
This method should get called after finishing the actual computation.
Definition: pcl_base.hpp:173
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.
VoxelAdjacencyList::edge_descriptor EdgeID
typename VectorType::iterator iterator
Definition: point_cloud.h:441
DataT & getData()
Returns a reference to the data member to access it without copying.
~SupervoxelClustering()
This destructor destroys the cloud, normals and search method used for finding neighbors.
Octree pointcloud search class
Definition: octree_search.h:56
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:430
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.
PointCloudConstPtr input_
The input point cloud dataset.
Definition: pcl_base.h:151
iterator begin()
Definition: point_cloud.h:443
void resize(size_t n)
Resize the cloud.
Definition: point_cloud.h:456
A point structure representing Euclidean xyz coordinates, and the RGB color.
void getSupervoxelAdjacencyList(VoxelAdjacencyList &adjacency_list_arg) const
Gets the adjacency list (Boost Graph library) which gives connections between supervoxels.
int nearestKSearch(const PointT &point, int k, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances) const override
Search for the k-nearest neighbors for the given query point.
Definition: kdtree.hpp:88
boost::adjacency_list< boost::setS, boost::setS, boost::undirectedS, uint32_t, float > VoxelAdjacencyList
bool empty() const
Definition: point_cloud.h:451
float getVoxelResolution() const
Get the resolution of the octree voxels.
void setSpatialImportance(float val)
Set the importance of spatial distance for supervoxels.