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