Point Cloud Library (PCL)  1.12.0
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 #include <pcl/common/io.h> // for copyPointCloud
45 
46 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
47 template <typename PointT>
48 pcl::SupervoxelClustering<PointT>::SupervoxelClustering (float voxel_resolution, float seed_resolution) :
49  resolution_ (voxel_resolution),
50  seed_resolution_ (seed_resolution),
51  adjacency_octree_ (),
52  voxel_centroid_cloud_ (),
53  color_importance_ (0.1f),
54  spatial_importance_ (0.4f),
55  normal_importance_ (1.0f),
56  use_default_transform_behaviour_ (true)
57 {
58  adjacency_octree_.reset (new OctreeAdjacencyT (resolution_));
59 }
60 
61 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
62 template <typename PointT>
64 {
65 
66 }
67 
68 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
69 template <typename PointT> void
71 {
72  if ( cloud->empty () )
73  {
74  PCL_ERROR ("[pcl::SupervoxelClustering::setInputCloud] Empty cloud set, doing nothing \n");
75  return;
76  }
77 
78  input_ = cloud;
79  adjacency_octree_->setInputCloud (cloud);
80 }
81 
82 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
83 template <typename PointT> void
85 {
86  if ( normal_cloud->empty () )
87  {
88  PCL_ERROR ("[pcl::SupervoxelClustering::setNormalCloud] Empty cloud set, doing nothing \n");
89  return;
90  }
91 
92  input_normals_ = normal_cloud;
93 }
94 
95 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
96 template <typename PointT> void
97 pcl::SupervoxelClustering<PointT>::extract (std::map<std::uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters)
98 {
99  //timer_.reset ();
100  //double t_start = timer_.getTime ();
101  //std::cout << "Init compute \n";
102  bool segmentation_is_possible = initCompute ();
103  if ( !segmentation_is_possible )
104  {
105  deinitCompute ();
106  return;
107  }
108 
109  //std::cout << "Preparing for segmentation \n";
110  segmentation_is_possible = prepareForSegmentation ();
111  if ( !segmentation_is_possible )
112  {
113  deinitCompute ();
114  return;
115  }
116 
117  //double t_prep = timer_.getTime ();
118  //std::cout << "Placing Seeds" << std::endl;
119  Indices seed_indices;
120  selectInitialSupervoxelSeeds (seed_indices);
121  //std::cout << "Creating helpers "<<std::endl;
122  createSupervoxelHelpers (seed_indices);
123  //double t_seeds = timer_.getTime ();
124 
125 
126  //std::cout << "Expanding the supervoxels" << std::endl;
127  int max_depth = static_cast<int> (1.8f*seed_resolution_/resolution_);
128  expandSupervoxels (max_depth);
129  //double t_iterate = timer_.getTime ();
130 
131  //std::cout << "Making Supervoxel structures" << std::endl;
132  makeSupervoxels (supervoxel_clusters);
133  //double t_supervoxels = timer_.getTime ();
134 
135  // std::cout << "--------------------------------- Timing Report --------------------------------- \n";
136  // std::cout << "Time to prep (normals, neighbors, voxelization)="<<t_prep-t_start<<" ms\n";
137  // std::cout << "Time to seed clusters ="<<t_seeds-t_prep<<" ms\n";
138  // std::cout << "Time to expand clusters ="<<t_iterate-t_seeds<<" ms\n";
139  // std::cout << "Time to create supervoxel structures ="<<t_supervoxels-t_iterate<<" ms\n";
140  // std::cout << "Total run time ="<<t_supervoxels-t_start<<" ms\n";
141  // std::cout << "--------------------------------------------------------------------------------- \n";
142 
143  deinitCompute ();
144 }
145 
146 
147 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
148 template <typename PointT> void
149 pcl::SupervoxelClustering<PointT>::refineSupervoxels (int num_itr, std::map<std::uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters)
150 {
151  if (supervoxel_helpers_.empty ())
152  {
153  PCL_ERROR ("[pcl::SupervoxelClustering::refineVoxelNormals] Supervoxels not extracted, doing nothing - (Call extract first!) \n");
154  return;
155  }
156 
157  int max_depth = static_cast<int> (1.8f*seed_resolution_/resolution_);
158  for (int i = 0; i < num_itr; ++i)
159  {
160  for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr)
161  {
162  sv_itr->refineNormals ();
163  }
164 
165  reseedSupervoxels ();
166  expandSupervoxels (max_depth);
167  }
168 
169 
170  makeSupervoxels (supervoxel_clusters);
171 
172 }
173 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
174 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
175 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
176 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
177 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
178 
179 
180 template <typename PointT> bool
182 {
183 
184  // if user forgot to pass point cloud or if it is empty
185  if ( input_->points.empty () )
186  return (false);
187 
188  //Add the new cloud of data to the octree
189  //std::cout << "Populating adjacency octree with new cloud \n";
190  //double prep_start = timer_.getTime ();
191  if ( (use_default_transform_behaviour_ && input_->isOrganized ())
192  || (!use_default_transform_behaviour_ && use_single_camera_transform_))
193  adjacency_octree_->setTransformFunction ([this] (PointT &p) { transformFunction (p); });
194 
195  adjacency_octree_->addPointsFromInputCloud ();
196  //double prep_end = timer_.getTime ();
197  //std::cout<<"Time elapsed populating octree with next frame ="<<prep_end-prep_start<<" ms\n";
198 
199  //Compute normals and insert data for centroids into data field of octree
200  //double normals_start = timer_.getTime ();
201  computeVoxelData ();
202  //double normals_end = timer_.getTime ();
203  //std::cout << "Time elapsed finding normals and pushing into octree ="<<normals_end-normals_start<<" ms\n";
204 
205  return true;
206 }
207 
208 template <typename PointT> void
210 {
211  voxel_centroid_cloud_.reset (new PointCloudT);
212  voxel_centroid_cloud_->resize (adjacency_octree_->getLeafCount ());
213  typename LeafVectorT::iterator leaf_itr = adjacency_octree_->begin ();
214  typename PointCloudT::iterator cent_cloud_itr = voxel_centroid_cloud_->begin ();
215  for (int idx = 0 ; leaf_itr != adjacency_octree_->end (); ++leaf_itr, ++cent_cloud_itr, ++idx)
216  {
217  VoxelData& new_voxel_data = (*leaf_itr)->getData ();
218  //Add the point to the centroid cloud
219  new_voxel_data.getPoint (*cent_cloud_itr);
220  //voxel_centroid_cloud_->push_back(new_voxel_data.getPoint ());
221  new_voxel_data.idx_ = idx;
222  }
223 
224  //If normals were provided
225  if (input_normals_)
226  {
227  //Verify that input normal cloud size is same as input cloud size
228  assert (input_normals_->size () == input_->size ());
229  //For every point in the input cloud, find its corresponding leaf
230  typename NormalCloudT::const_iterator normal_itr = input_normals_->begin ();
231  for (typename PointCloudT::const_iterator input_itr = input_->begin (); input_itr != input_->end (); ++input_itr, ++normal_itr)
232  {
233  //If the point is not finite we ignore it
234  if ( !pcl::isFinite<PointT> (*input_itr))
235  continue;
236  //Otherwise look up its leaf container
237  LeafContainerT* leaf = adjacency_octree_->getLeafContainerAtPoint (*input_itr);
238 
239  //Get the voxel data object
240  VoxelData& voxel_data = leaf->getData ();
241  //Add this normal in (we will normalize at the end)
242  voxel_data.normal_ += normal_itr->getNormalVector4fMap ();
243  voxel_data.curvature_ += normal_itr->curvature;
244  }
245  //Now iterate through the leaves and normalize
246  for (leaf_itr = adjacency_octree_->begin (); leaf_itr != adjacency_octree_->end (); ++leaf_itr)
247  {
248  VoxelData& voxel_data = (*leaf_itr)->getData ();
249  voxel_data.normal_.normalize ();
250  voxel_data.owner_ = nullptr;
251  voxel_data.distance_ = std::numeric_limits<float>::max ();
252  //Get the number of points in this leaf
253  int num_points = (*leaf_itr)->getPointCounter ();
254  voxel_data.curvature_ /= num_points;
255  }
256  }
257  else //Otherwise just compute the normals
258  {
259  for (leaf_itr = adjacency_octree_->begin (); leaf_itr != adjacency_octree_->end (); ++leaf_itr)
260  {
261  VoxelData& new_voxel_data = (*leaf_itr)->getData ();
262  //For every point, get its neighbors, build an index vector, compute normal
263  Indices indices;
264  indices.reserve (81);
265  //Push this point
266  indices.push_back (new_voxel_data.idx_);
267  for (typename LeafContainerT::const_iterator neighb_itr=(*leaf_itr)->cbegin (); neighb_itr!=(*leaf_itr)->cend (); ++neighb_itr)
268  {
269  VoxelData& neighb_voxel_data = (*neighb_itr)->getData ();
270  //Push neighbor index
271  indices.push_back (neighb_voxel_data.idx_);
272  //Get neighbors neighbors, push onto cloud
273  for (typename LeafContainerT::const_iterator neighb_neighb_itr=(*neighb_itr)->cbegin (); neighb_neighb_itr!=(*neighb_itr)->cend (); ++neighb_neighb_itr)
274  {
275  VoxelData& neighb2_voxel_data = (*neighb_neighb_itr)->getData ();
276  indices.push_back (neighb2_voxel_data.idx_);
277  }
278  }
279  //Compute normal
280  pcl::computePointNormal (*voxel_centroid_cloud_, indices, new_voxel_data.normal_, new_voxel_data.curvature_);
281  pcl::flipNormalTowardsViewpoint ((*voxel_centroid_cloud_)[new_voxel_data.idx_], 0.0f,0.0f,0.0f, new_voxel_data.normal_);
282  new_voxel_data.normal_[3] = 0.0f;
283  new_voxel_data.normal_.normalize ();
284  new_voxel_data.owner_ = nullptr;
285  new_voxel_data.distance_ = std::numeric_limits<float>::max ();
286  }
287  }
288 
289 
290 }
291 
292 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
293 template <typename PointT> void
295 {
296 
297 
298  for (int i = 1; i < depth; ++i)
299  {
300  //Expand the the supervoxels by one iteration
301  for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr)
302  {
303  sv_itr->expand ();
304  }
305 
306  //Update the centers to reflect new centers
307  for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); )
308  {
309  if (sv_itr->size () == 0)
310  {
311  sv_itr = supervoxel_helpers_.erase (sv_itr);
312  }
313  else
314  {
315  sv_itr->updateCentroid ();
316  ++sv_itr;
317  }
318  }
319 
320  }
321 
322 }
323 
324 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
325 template <typename PointT> void
326 pcl::SupervoxelClustering<PointT>::makeSupervoxels (std::map<std::uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters)
327 {
328  supervoxel_clusters.clear ();
329  for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr)
330  {
331  std::uint32_t label = sv_itr->getLabel ();
332  supervoxel_clusters[label].reset (new Supervoxel<PointT>);
333  sv_itr->getXYZ (supervoxel_clusters[label]->centroid_.x,supervoxel_clusters[label]->centroid_.y,supervoxel_clusters[label]->centroid_.z);
334  sv_itr->getRGB (supervoxel_clusters[label]->centroid_.rgba);
335  sv_itr->getNormal (supervoxel_clusters[label]->normal_);
336  sv_itr->getVoxels (supervoxel_clusters[label]->voxels_);
337  sv_itr->getNormals (supervoxel_clusters[label]->normals_);
338  }
339 }
340 
341 
342 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
343 template <typename PointT> void
345 {
346 
347  supervoxel_helpers_.clear ();
348  for (std::size_t i = 0; i < seed_indices.size (); ++i)
349  {
350  supervoxel_helpers_.push_back (new SupervoxelHelper(i+1,this));
351  //Find which leaf corresponds to this seed index
352  LeafContainerT* seed_leaf = adjacency_octree_->at(seed_indices[i]);//adjacency_octree_->getLeafContainerAtPoint (seed_points[i]);
353  if (seed_leaf)
354  {
355  supervoxel_helpers_.back ().addLeaf (seed_leaf);
356  }
357  else
358  {
359  PCL_WARN ("Could not find leaf in pcl::SupervoxelClustering<PointT>::createSupervoxelHelpers - supervoxel will be deleted \n");
360  }
361  }
362 
363 }
364 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
365 template <typename PointT> void
367 {
368  //TODO THIS IS BAD - SEEDING SHOULD BE BETTER
369  //TODO Switch to assigning leaves! Don't use Octree!
370 
371  // std::cout << "Size of centroid cloud="<<voxel_centroid_cloud_->size ()<<", seeding resolution="<<seed_resolution_<<"\n";
372  //Initialize octree with voxel centroids
373  pcl::octree::OctreePointCloudSearch <PointT> seed_octree (seed_resolution_);
374  seed_octree.setInputCloud (voxel_centroid_cloud_);
375  seed_octree.addPointsFromInputCloud ();
376  // std::cout << "Size of octree ="<<seed_octree.getLeafCount ()<<"\n";
377  std::vector<PointT, Eigen::aligned_allocator<PointT> > voxel_centers;
378  int num_seeds = seed_octree.getOccupiedVoxelCenters(voxel_centers);
379  //std::cout << "Number of seed points before filtering="<<voxel_centers.size ()<<std::endl;
380 
381  std::vector<int> seed_indices_orig;
382  seed_indices_orig.resize (num_seeds, 0);
383  seed_indices.clear ();
384  pcl::Indices closest_index;
385  std::vector<float> distance;
386  closest_index.resize(1,0);
387  distance.resize(1,0);
388  if (!voxel_kdtree_)
389  {
390  voxel_kdtree_.reset (new pcl::search::KdTree<PointT>);
391  voxel_kdtree_ ->setInputCloud (voxel_centroid_cloud_);
392  }
393 
394  for (int i = 0; i < num_seeds; ++i)
395  {
396  voxel_kdtree_->nearestKSearch (voxel_centers[i], 1, closest_index, distance);
397  seed_indices_orig[i] = closest_index[0];
398  }
399 
400  pcl::Indices neighbors;
401  std::vector<float> sqr_distances;
402  seed_indices.reserve (seed_indices_orig.size ());
403  float search_radius = 0.5f*seed_resolution_;
404  // This is 1/20th of the number of voxels which fit in a planar slice through search volume
405  // Area of planar slice / area of voxel side. (Note: This is smaller than the value mentioned in the original paper)
406  float min_points = 0.05f * (search_radius)*(search_radius) * 3.1415926536f / (resolution_*resolution_);
407  for (const auto &index_orig : seed_indices_orig)
408  {
409  int num = voxel_kdtree_->radiusSearch (index_orig, search_radius , neighbors, sqr_distances);
410  if ( num > min_points)
411  {
412  seed_indices.push_back (index_orig);
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  Indices 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
464 pcl::SupervoxelClustering<PointT>::voxelDataDistance (const VoxelData &v1, const VoxelData &v2) const
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 <std::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  std::uint32_t label = sv_itr->getLabel ();
498  std::set<std::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<std::uint32_t, std::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  std::uint32_t label = sv_itr->getLabel ();
540  std::set<std::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<std::uint32_t,std::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  for (auto i_labeled = labeled_cloud->begin (); i_labeled != labeled_cloud->end (); ++i_labeled,++i_input)
589  {
590  if ( !pcl::isFinite<PointT> (*i_input))
591  i_labeled->label = 0;
592  else
593  {
594  i_labeled->label = 0;
595  LeafContainerT *leaf = adjacency_octree_->getLeafContainerAtPoint (*i_input);
596  VoxelData& voxel_data = leaf->getData ();
597  if (voxel_data.owner_)
598  i_labeled->label = voxel_data.owner_->getLabel ();
599 
600  }
601 
602  }
603 
604  return (labeled_cloud);
605 }
606 
607 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
608 template <typename PointT> pcl::PointCloud<pcl::PointNormal>::Ptr
609 pcl::SupervoxelClustering<PointT>::makeSupervoxelNormalCloud (std::map<std::uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters)
610 {
612  normal_cloud->resize (supervoxel_clusters.size ());
613  pcl::PointCloud<pcl::PointNormal>::iterator normal_cloud_itr = normal_cloud->begin ();
614  for (auto sv_itr = supervoxel_clusters.cbegin (), sv_itr_end = supervoxel_clusters.cend ();
615  sv_itr != sv_itr_end; ++sv_itr, ++normal_cloud_itr)
616  {
617  (sv_itr->second)->getCentroidPointNormal (*normal_cloud_itr);
618  }
619  return normal_cloud;
620 }
621 
622 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
623 template <typename PointT> float
625 {
626  return (resolution_);
627 }
628 
629 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
630 template <typename PointT> void
632 {
633  resolution_ = resolution;
634 
635 }
636 
637 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
638 template <typename PointT> float
640 {
641  return (seed_resolution_);
642 }
643 
644 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
645 template <typename PointT> void
647 {
648  seed_resolution_ = seed_resolution;
649 }
650 
651 
652 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
653 template <typename PointT> void
655 {
656  color_importance_ = val;
657 }
658 
659 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
660 template <typename PointT> void
662 {
663  spatial_importance_ = val;
664 }
665 
666 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
667 template <typename PointT> void
669 {
670  normal_importance_ = val;
671 }
672 
673 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
674 template <typename PointT> void
676 {
677  use_default_transform_behaviour_ = false;
678  use_single_camera_transform_ = val;
679 }
680 
681 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
682 template <typename PointT> int
684 {
685  int max_label = 0;
686  for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
687  {
688  int temp = sv_itr->getLabel ();
689  if (temp > max_label)
690  max_label = temp;
691  }
692  return max_label;
693 }
694 
695 namespace pcl
696 {
697  namespace octree
698  {
699  //Explicit overloads for RGB types
700  template<>
701  void
703 
704  template<>
705  void
707 
708  //Explicit overloads for RGB types
709  template<> void
711 
712  template<> void
714 
715  //Explicit overloads for XYZ types
716  template<>
717  void
719 
720  template<> void
722  }
723 }
724 
725 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
726 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
727 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
728 namespace pcl
729 {
730 
731  template<> void
733 
734  template<> void
736 
737  template<typename PointT> void
739  {
740  //XYZ is required or this doesn't make much sense...
741  point_arg.x = xyz_[0];
742  point_arg.y = xyz_[1];
743  point_arg.z = xyz_[2];
744  }
745 
746  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
747  template <typename PointT> void
749  {
750  normal_arg.normal_x = normal_[0];
751  normal_arg.normal_y = normal_[1];
752  normal_arg.normal_z = normal_[2];
753  normal_arg.curvature = curvature_;
754  }
755 }
756 
757 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
758 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
759 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
760 template <typename PointT> void
762 {
763  leaves_.insert (leaf_arg);
764  VoxelData& voxel_data = leaf_arg->getData ();
765  voxel_data.owner_ = this;
766 }
767 
768 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
769 template <typename PointT> void
771 {
772  leaves_.erase (leaf_arg);
773 }
774 
775 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
776 template <typename PointT> void
778 {
779  for (auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr)
780  {
781  VoxelData& voxel = ((*leaf_itr)->getData ());
782  voxel.owner_ = nullptr;
783  voxel.distance_ = std::numeric_limits<float>::max ();
784  }
785  leaves_.clear ();
786 }
787 
788 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
789 template <typename PointT> void
791 {
792  //std::cout << "Expanding sv "<<label_<<", owns "<<leaves_.size ()<<" voxels\n";
793  //Buffer of new neighbors - initial size is just a guess of most possible
794  std::vector<LeafContainerT*> new_owned;
795  new_owned.reserve (leaves_.size () * 9);
796  //For each leaf belonging to this supervoxel
797  for (auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr)
798  {
799  //for each neighbor of the leaf
800  for (typename LeafContainerT::const_iterator neighb_itr=(*leaf_itr)->cbegin (); neighb_itr!=(*leaf_itr)->cend (); ++neighb_itr)
801  {
802  //Get a reference to the data contained in the leaf
803  VoxelData& neighbor_voxel = ((*neighb_itr)->getData ());
804  //TODO this is a shortcut, really we should always recompute distance
805  if(neighbor_voxel.owner_ == this)
806  continue;
807  //Compute distance to the neighbor
808  float dist = parent_->voxelDataDistance (centroid_, neighbor_voxel);
809  //If distance is less than previous, we remove it from its owner's list
810  //and change the owner to this and distance (we *steal* it!)
811  if (dist < neighbor_voxel.distance_)
812  {
813  neighbor_voxel.distance_ = dist;
814  if (neighbor_voxel.owner_ != this)
815  {
816  if (neighbor_voxel.owner_)
817  (neighbor_voxel.owner_)->removeLeaf(*neighb_itr);
818  neighbor_voxel.owner_ = this;
819  new_owned.push_back (*neighb_itr);
820  }
821  }
822  }
823  }
824  //Push all new owned onto the owned leaf set
825  for (auto new_owned_itr = new_owned.cbegin (); new_owned_itr != new_owned.cend (); ++new_owned_itr)
826  {
827  leaves_.insert (*new_owned_itr);
828  }
829 }
830 
831 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
832 template <typename PointT> void
834 {
835  //For each leaf belonging to this supervoxel, get its neighbors, build an index vector, compute normal
836  for (auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr)
837  {
838  VoxelData& voxel_data = (*leaf_itr)->getData ();
839  Indices indices;
840  indices.reserve (81);
841  //Push this point
842  indices.push_back (voxel_data.idx_);
843  for (typename LeafContainerT::const_iterator neighb_itr=(*leaf_itr)->cbegin (); neighb_itr!=(*leaf_itr)->cend (); ++neighb_itr)
844  {
845  //Get a reference to the data contained in the leaf
846  VoxelData& neighbor_voxel_data = ((*neighb_itr)->getData ());
847  //If the neighbor is in this supervoxel, use it
848  if (neighbor_voxel_data.owner_ == this)
849  {
850  indices.push_back (neighbor_voxel_data.idx_);
851  //Also check its neighbors
852  for (typename LeafContainerT::const_iterator neighb_neighb_itr=(*neighb_itr)->cbegin (); neighb_neighb_itr!=(*neighb_itr)->cend (); ++neighb_neighb_itr)
853  {
854  VoxelData& neighb_neighb_voxel_data = (*neighb_neighb_itr)->getData ();
855  if (neighb_neighb_voxel_data.owner_ == this)
856  indices.push_back (neighb_neighb_voxel_data.idx_);
857  }
858 
859 
860  }
861  }
862  //Compute normal
863  pcl::computePointNormal (*parent_->voxel_centroid_cloud_, indices, voxel_data.normal_, voxel_data.curvature_);
864  pcl::flipNormalTowardsViewpoint ((*parent_->voxel_centroid_cloud_)[voxel_data.idx_], 0.0f,0.0f,0.0f, voxel_data.normal_);
865  voxel_data.normal_[3] = 0.0f;
866  voxel_data.normal_.normalize ();
867  }
868 }
869 
870 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
871 template <typename PointT> void
873 {
874  centroid_.normal_ = Eigen::Vector4f::Zero ();
875  centroid_.xyz_ = Eigen::Vector3f::Zero ();
876  centroid_.rgb_ = Eigen::Vector3f::Zero ();
877  for (auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr)
878  {
879  const VoxelData& leaf_data = (*leaf_itr)->getData ();
880  centroid_.normal_ += leaf_data.normal_;
881  centroid_.xyz_ += leaf_data.xyz_;
882  centroid_.rgb_ += leaf_data.rgb_;
883  }
884  centroid_.normal_.normalize ();
885  centroid_.xyz_ /= static_cast<float> (leaves_.size ());
886  centroid_.rgb_ /= static_cast<float> (leaves_.size ());
887 }
888 
889 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
890 template <typename PointT> void
892 {
893  voxels.reset (new pcl::PointCloud<PointT>);
894  voxels->clear ();
895  voxels->resize (leaves_.size ());
896  typename pcl::PointCloud<PointT>::iterator voxel_itr = voxels->begin ();
897  for (auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr, ++voxel_itr)
898  {
899  const VoxelData& leaf_data = (*leaf_itr)->getData ();
900  leaf_data.getPoint (*voxel_itr);
901  }
902 }
903 
904 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
905 template <typename PointT> void
907 {
908  normals.reset (new pcl::PointCloud<Normal>);
909  normals->clear ();
910  normals->resize (leaves_.size ());
911  typename pcl::PointCloud<Normal>::iterator normal_itr = normals->begin ();
912  for (auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr, ++normal_itr)
913  {
914  const VoxelData& leaf_data = (*leaf_itr)->getData ();
915  leaf_data.getNormal (*normal_itr);
916  }
917 }
918 
919 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
920 template <typename PointT> void
921 pcl::SupervoxelClustering<PointT>::SupervoxelHelper::getNeighborLabels (std::set<std::uint32_t> &neighbor_labels) const
922 {
923  neighbor_labels.clear ();
924  //For each leaf belonging to this supervoxel
925  for (auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr)
926  {
927  //for each neighbor of the leaf
928  for (typename LeafContainerT::const_iterator neighb_itr=(*leaf_itr)->cbegin (); neighb_itr!=(*leaf_itr)->cend (); ++neighb_itr)
929  {
930  //Get a reference to the data contained in the leaf
931  VoxelData& neighbor_voxel = ((*neighb_itr)->getData ());
932  //If it has an owner, and it's not us - get it's owner's label insert into set
933  if (neighbor_voxel.owner_ != this && neighbor_voxel.owner_)
934  {
935  neighbor_labels.insert (neighbor_voxel.owner_->getLabel ());
936  }
937  }
938  }
939 }
940 
941 
942 #endif // PCL_SUPERVOXEL_CLUSTERING_HPP_
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
bool empty() const
Definition: point_cloud.h:446
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition: point_cloud.h:462
typename VectorType::iterator iterator
Definition: point_cloud.h:425
typename VectorType::const_iterator const_iterator
Definition: point_cloud.h:426
iterator end() noexcept
Definition: point_cloud.h:430
void clear()
Removes all points in a cloud and sets the width and height to 0.
Definition: point_cloud.h:874
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
iterator begin() noexcept
Definition: point_cloud.h:429
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:414
VoxelData is a structure used for storing data within a pcl::octree::OctreePointCloudAdjacencyContain...
void getPoint(PointT &point_arg) const
Gets the data of in the form of a point.
void getNormal(Normal &normal_arg) const
Gets the data of in the form of a normal.
Implements a supervoxel algorithm based on voxel structure, normals, and rgb values.
static pcl::PointCloud< pcl::PointNormal >::Ptr makeSupervoxelNormalCloud(std::map< std::uint32_t, typename Supervoxel< PointT >::Ptr > &supervoxel_clusters)
Static helper function which returns a pointcloud of normals for the input supervoxels.
VoxelAdjacencyList::edge_descriptor EdgeID
int getMaxLabel() const
Returns the current maximum (highest) label.
virtual void refineSupervoxels(int num_itr, std::map< std::uint32_t, typename Supervoxel< PointT >::Ptr > &supervoxel_clusters)
This method refines the calculated supervoxels - may only be called after extract.
void setSpatialImportance(float val)
Set the importance of spatial distance for supervoxels.
float getVoxelResolution() const
Get the resolution of the octree 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 setColorImportance(float val)
Set the importance of color for supervoxels.
pcl::PointCloud< pcl::PointXYZL >::Ptr getLabeledVoxelCloud() const
Returns labeled voxelized cloud Points that belong to the same supervoxel have the same label.
void setVoxelResolution(float resolution)
Set the resolution of the octree voxels.
void setInputCloud(const typename pcl::PointCloud< PointT >::ConstPtr &cloud) override
This method sets the cloud to be supervoxelized.
virtual void extract(std::map< std::uint32_t, typename Supervoxel< PointT >::Ptr > &supervoxel_clusters)
This method launches the segmentation algorithm and returns the supervoxels that were obtained during...
boost::adjacency_list< boost::setS, boost::setS, boost::undirectedS, std::uint32_t, float > VoxelAdjacencyList
pcl::PointCloud< PointT >::Ptr getVoxelCentroidCloud() const
Returns a deep copy of the voxel centroid cloud.
void setSeedResolution(float seed_resolution)
Set the resolution of the octree seed voxels.
void getSupervoxelAdjacency(std::multimap< std::uint32_t, std::uint32_t > &label_adjacency) const
Get a multimap which gives supervoxel adjacency.
void setUseSingleCameraTransform(bool val)
Set whether or not to use the single camera transform.
SupervoxelClustering(float voxel_resolution, float seed_resolution)
Constructor that sets default values for member variables.
void getSupervoxelAdjacencyList(VoxelAdjacencyList &adjacency_list_arg) const
Gets the adjacency list (Boost Graph library) which gives connections between supervoxels.
~SupervoxelClustering()
This destructor destroys the cloud, normals and search method used for finding neighbors.
VoxelAdjacencyList::vertex_descriptor VoxelID
float getSeedResolution() const
Get the resolution of the octree seed voxels.
void setNormalImportance(float val)
Set the importance of scalar normal product for supervoxels.
pcl::PointCloud< PointXYZL >::Ptr getLabeledCloud() const
Returns labeled cloud Points that belong to the same supervoxel have the same label.
shared_ptr< Supervoxel< PointT > > Ptr
Octree adjacency leaf container class- stores a list of pointers to neighbors, number of points added...
DataT & getData()
Returns a reference to the data member to access it without copying.
Octree pointcloud voxel class which maintains adjacency information for its voxels.
Octree pointcloud search class
Definition: octree_search.h:58
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition: kdtree.h:62
void copyPointCloud(const pcl::PointCloud< PointInT > &cloud_in, pcl::PointCloud< PointOutT > &cloud_out)
Copy all the fields from a given point cloud into a new point cloud.
Definition: io.hpp:144
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:122
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:61
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
A point structure representing normal coordinates and the surface curvature estimate.
A point structure representing Euclidean xyz coordinates.
A point structure representing Euclidean xyz coordinates, and the RGBA color.
A point structure representing Euclidean xyz coordinates, and the RGB color.