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