Point Cloud Library (PCL)  1.9.1-dev
supervoxel_clustering.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  *
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Willow Garage, Inc. nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *
35  * Author : jpapon@gmail.com
36  * Email : jpapon@gmail.com
37  *
38  */
39 
40 #ifndef PCL_SEGMENTATION_SUPERVOXEL_CLUSTERING_HPP_
41 #define PCL_SEGMENTATION_SUPERVOXEL_CLUSTERING_HPP_
42 
43 #include <pcl/segmentation/supervoxel_clustering.h>
44 
45 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
46 template <typename PointT>
47 pcl::SupervoxelClustering<PointT>::SupervoxelClustering (float voxel_resolution, float seed_resolution) :
48  resolution_ (voxel_resolution),
49  seed_resolution_ (seed_resolution),
50  adjacency_octree_ (),
51  voxel_centroid_cloud_ (),
52  color_importance_ (0.1f),
53  spatial_importance_ (0.4f),
54  normal_importance_ (1.0f),
55  use_default_transform_behaviour_ (true)
56 {
57  adjacency_octree_.reset (new OctreeAdjacencyT (resolution_));
58 }
59 
60 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
61 template <typename PointT>
63 {
64 
65 }
66 
67 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
68 template <typename PointT> void
70 {
71  if ( cloud->empty () )
72  {
73  PCL_ERROR ("[pcl::SupervoxelClustering::setInputCloud] Empty cloud set, doing nothing \n");
74  return;
75  }
76 
77  input_ = cloud;
78  adjacency_octree_->setInputCloud (cloud);
79 }
80 
81 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
82 template <typename PointT> void
84 {
85  if ( normal_cloud->empty () )
86  {
87  PCL_ERROR ("[pcl::SupervoxelClustering::setNormalCloud] Empty cloud set, doing nothing \n");
88  return;
89  }
90 
91  input_normals_ = normal_cloud;
92 }
93 
94 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
95 template <typename PointT> void
96 pcl::SupervoxelClustering<PointT>::extract (std::map<std::uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters)
97 {
98  //timer_.reset ();
99  //double t_start = timer_.getTime ();
100  //std::cout << "Init compute \n";
101  bool segmentation_is_possible = initCompute ();
102  if ( !segmentation_is_possible )
103  {
104  deinitCompute ();
105  return;
106  }
107 
108  //std::cout << "Preparing for segmentation \n";
109  segmentation_is_possible = prepareForSegmentation ();
110  if ( !segmentation_is_possible )
111  {
112  deinitCompute ();
113  return;
114  }
115 
116  //double t_prep = timer_.getTime ();
117  //std::cout << "Placing Seeds" << std::endl;
118  std::vector<int> seed_indices;
119  selectInitialSupervoxelSeeds (seed_indices);
120  //std::cout << "Creating helpers "<<std::endl;
121  createSupervoxelHelpers (seed_indices);
122  //double t_seeds = timer_.getTime ();
123 
124 
125  //std::cout << "Expanding the supervoxels" << std::endl;
126  int max_depth = static_cast<int> (1.8f*seed_resolution_/resolution_);
127  expandSupervoxels (max_depth);
128  //double t_iterate = timer_.getTime ();
129 
130  //std::cout << "Making Supervoxel structures" << std::endl;
131  makeSupervoxels (supervoxel_clusters);
132  //double t_supervoxels = timer_.getTime ();
133 
134  // std::cout << "--------------------------------- Timing Report --------------------------------- \n";
135  // std::cout << "Time to prep (normals, neighbors, voxelization)="<<t_prep-t_start<<" ms\n";
136  // std::cout << "Time to seed clusters ="<<t_seeds-t_prep<<" ms\n";
137  // std::cout << "Time to expand clusters ="<<t_iterate-t_seeds<<" ms\n";
138  // std::cout << "Time to create supervoxel structures ="<<t_supervoxels-t_iterate<<" ms\n";
139  // std::cout << "Total run time ="<<t_supervoxels-t_start<<" ms\n";
140  // std::cout << "--------------------------------------------------------------------------------- \n";
141 
142  deinitCompute ();
143 }
144 
145 
146 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
147 template <typename PointT> void
148 pcl::SupervoxelClustering<PointT>::refineSupervoxels (int num_itr, std::map<std::uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters)
149 {
150  if (supervoxel_helpers_.empty ())
151  {
152  PCL_ERROR ("[pcl::SupervoxelClustering::refineVoxelNormals] Supervoxels not extracted, doing nothing - (Call extract first!) \n");
153  return;
154  }
155 
156  int max_depth = static_cast<int> (1.8f*seed_resolution_/resolution_);
157  for (int i = 0; i < num_itr; ++i)
158  {
159  for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr)
160  {
161  sv_itr->refineNormals ();
162  }
163 
164  reseedSupervoxels ();
165  expandSupervoxels (max_depth);
166  }
167 
168 
169  makeSupervoxels (supervoxel_clusters);
170 
171 }
172 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
173 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
174 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
175 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
176 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
177 
178 
179 template <typename PointT> bool
181 {
182 
183  // if user forgot to pass point cloud or if it is empty
184  if ( input_->points.empty () )
185  return (false);
186 
187  //Add the new cloud of data to the octree
188  //std::cout << "Populating adjacency octree with new cloud \n";
189  //double prep_start = timer_.getTime ();
190  if ( (use_default_transform_behaviour_ && input_->isOrganized ())
191  || (!use_default_transform_behaviour_ && use_single_camera_transform_))
192  adjacency_octree_->setTransformFunction ([this] (PointT &p) { transformFunction (p); });
193 
194  adjacency_octree_->addPointsFromInputCloud ();
195  //double prep_end = timer_.getTime ();
196  //std::cout<<"Time elapsed populating octree with next frame ="<<prep_end-prep_start<<" ms\n";
197 
198  //Compute normals and insert data for centroids into data field of octree
199  //double normals_start = timer_.getTime ();
200  computeVoxelData ();
201  //double normals_end = timer_.getTime ();
202  //std::cout << "Time elapsed finding normals and pushing into octree ="<<normals_end-normals_start<<" ms\n";
203 
204  return true;
205 }
206 
207 template <typename PointT> void
209 {
210  voxel_centroid_cloud_.reset (new PointCloudT);
211  voxel_centroid_cloud_->resize (adjacency_octree_->getLeafCount ());
212  typename LeafVectorT::iterator leaf_itr = adjacency_octree_->begin ();
213  typename PointCloudT::iterator cent_cloud_itr = voxel_centroid_cloud_->begin ();
214  for (int idx = 0 ; leaf_itr != adjacency_octree_->end (); ++leaf_itr, ++cent_cloud_itr, ++idx)
215  {
216  VoxelData& new_voxel_data = (*leaf_itr)->getData ();
217  //Add the point to the centroid cloud
218  new_voxel_data.getPoint (*cent_cloud_itr);
219  //voxel_centroid_cloud_->push_back(new_voxel_data.getPoint ());
220  new_voxel_data.idx_ = idx;
221  }
222 
223  //If normals were provided
224  if (input_normals_)
225  {
226  //Verify that input normal cloud size is same as input cloud size
227  assert (input_normals_->size () == input_->size ());
228  //For every point in the input cloud, find its corresponding leaf
229  typename NormalCloudT::const_iterator normal_itr = input_normals_->begin ();
230  for (typename PointCloudT::const_iterator input_itr = input_->begin (); input_itr != input_->end (); ++input_itr, ++normal_itr)
231  {
232  //If the point is not finite we ignore it
233  if ( !pcl::isFinite<PointT> (*input_itr))
234  continue;
235  //Otherwise look up its leaf container
236  LeafContainerT* leaf = adjacency_octree_->getLeafContainerAtPoint (*input_itr);
237 
238  //Get the voxel data object
239  VoxelData& voxel_data = leaf->getData ();
240  //Add this normal in (we will normalize at the end)
241  voxel_data.normal_ += normal_itr->getNormalVector4fMap ();
242  voxel_data.curvature_ += normal_itr->curvature;
243  }
244  //Now iterate through the leaves and normalize
245  for (leaf_itr = adjacency_octree_->begin (); leaf_itr != adjacency_octree_->end (); ++leaf_itr)
246  {
247  VoxelData& voxel_data = (*leaf_itr)->getData ();
248  voxel_data.normal_.normalize ();
249  voxel_data.owner_ = nullptr;
250  voxel_data.distance_ = std::numeric_limits<float>::max ();
251  //Get the number of points in this leaf
252  int num_points = (*leaf_itr)->getPointCounter ();
253  voxel_data.curvature_ /= num_points;
254  }
255  }
256  else //Otherwise just compute the normals
257  {
258  for (leaf_itr = adjacency_octree_->begin (); leaf_itr != adjacency_octree_->end (); ++leaf_itr)
259  {
260  VoxelData& new_voxel_data = (*leaf_itr)->getData ();
261  //For every point, get its neighbors, build an index vector, compute normal
262  std::vector<int> indices;
263  indices.reserve (81);
264  //Push this point
265  indices.push_back (new_voxel_data.idx_);
266  for (typename LeafContainerT::const_iterator neighb_itr=(*leaf_itr)->cbegin (); neighb_itr!=(*leaf_itr)->cend (); ++neighb_itr)
267  {
268  VoxelData& neighb_voxel_data = (*neighb_itr)->getData ();
269  //Push neighbor index
270  indices.push_back (neighb_voxel_data.idx_);
271  //Get neighbors neighbors, push onto cloud
272  for (typename LeafContainerT::const_iterator neighb_neighb_itr=(*neighb_itr)->cbegin (); neighb_neighb_itr!=(*neighb_itr)->cend (); ++neighb_neighb_itr)
273  {
274  VoxelData& neighb2_voxel_data = (*neighb_neighb_itr)->getData ();
275  indices.push_back (neighb2_voxel_data.idx_);
276  }
277  }
278  //Compute normal
279  pcl::computePointNormal (*voxel_centroid_cloud_, indices, new_voxel_data.normal_, new_voxel_data.curvature_);
280  pcl::flipNormalTowardsViewpoint (voxel_centroid_cloud_->points[new_voxel_data.idx_], 0.0f,0.0f,0.0f, new_voxel_data.normal_);
281  new_voxel_data.normal_[3] = 0.0f;
282  new_voxel_data.normal_.normalize ();
283  new_voxel_data.owner_ = nullptr;
284  new_voxel_data.distance_ = std::numeric_limits<float>::max ();
285  }
286  }
287 
288 
289 }
290 
291 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
292 template <typename PointT> void
294 {
295 
296 
297  for (int i = 1; i < depth; ++i)
298  {
299  //Expand the the supervoxels by one iteration
300  for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr)
301  {
302  sv_itr->expand ();
303  }
304 
305  //Update the centers to reflect new centers
306  for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); )
307  {
308  if (sv_itr->size () == 0)
309  {
310  sv_itr = supervoxel_helpers_.erase (sv_itr);
311  }
312  else
313  {
314  sv_itr->updateCentroid ();
315  ++sv_itr;
316  }
317  }
318 
319  }
320 
321 }
322 
323 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
324 template <typename PointT> void
325 pcl::SupervoxelClustering<PointT>::makeSupervoxels (std::map<std::uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters)
326 {
327  supervoxel_clusters.clear ();
328  for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr)
329  {
330  std::uint32_t label = sv_itr->getLabel ();
331  supervoxel_clusters[label].reset (new Supervoxel<PointT>);
332  sv_itr->getXYZ (supervoxel_clusters[label]->centroid_.x,supervoxel_clusters[label]->centroid_.y,supervoxel_clusters[label]->centroid_.z);
333  sv_itr->getRGB (supervoxel_clusters[label]->centroid_.rgba);
334  sv_itr->getNormal (supervoxel_clusters[label]->normal_);
335  sv_itr->getVoxels (supervoxel_clusters[label]->voxels_);
336  sv_itr->getNormals (supervoxel_clusters[label]->normals_);
337  }
338 }
339 
340 
341 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
342 template <typename PointT> void
343 pcl::SupervoxelClustering<PointT>::createSupervoxelHelpers (std::vector<int> &seed_indices)
344 {
345 
346  supervoxel_helpers_.clear ();
347  for (std::size_t i = 0; i < seed_indices.size (); ++i)
348  {
349  supervoxel_helpers_.push_back (new SupervoxelHelper(i+1,this));
350  //Find which leaf corresponds to this seed index
351  LeafContainerT* seed_leaf = adjacency_octree_->at(seed_indices[i]);//adjacency_octree_->getLeafContainerAtPoint (seed_points[i]);
352  if (seed_leaf)
353  {
354  supervoxel_helpers_.back ().addLeaf (seed_leaf);
355  }
356  else
357  {
358  PCL_WARN ("Could not find leaf in pcl::SupervoxelClustering<PointT>::createSupervoxelHelpers - supervoxel will be deleted \n");
359  }
360  }
361 
362 }
363 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
364 template <typename PointT> void
366 {
367  //TODO THIS IS BAD - SEEDING SHOULD BE BETTER
368  //TODO Switch to assigning leaves! Don't use Octree!
369 
370  // std::cout << "Size of centroid cloud="<<voxel_centroid_cloud_->size ()<<", seeding resolution="<<seed_resolution_<<"\n";
371  //Initialize octree with voxel centroids
372  pcl::octree::OctreePointCloudSearch <PointT> seed_octree (seed_resolution_);
373  seed_octree.setInputCloud (voxel_centroid_cloud_);
374  seed_octree.addPointsFromInputCloud ();
375  // std::cout << "Size of octree ="<<seed_octree.getLeafCount ()<<"\n";
376  std::vector<PointT, Eigen::aligned_allocator<PointT> > voxel_centers;
377  int num_seeds = seed_octree.getOccupiedVoxelCenters(voxel_centers);
378  //std::cout << "Number of seed points before filtering="<<voxel_centers.size ()<<std::endl;
379 
380  std::vector<int> seed_indices_orig;
381  seed_indices_orig.resize (num_seeds, 0);
382  seed_indices.clear ();
383  std::vector<int> closest_index;
384  std::vector<float> distance;
385  closest_index.resize(1,0);
386  distance.resize(1,0);
387  if (!voxel_kdtree_)
388  {
389  voxel_kdtree_.reset (new pcl::search::KdTree<PointT>);
390  voxel_kdtree_ ->setInputCloud (voxel_centroid_cloud_);
391  }
392 
393  for (int i = 0; i < num_seeds; ++i)
394  {
395  voxel_kdtree_->nearestKSearch (voxel_centers[i], 1, closest_index, distance);
396  seed_indices_orig[i] = closest_index[0];
397  }
398 
399  std::vector<int> neighbors;
400  std::vector<float> sqr_distances;
401  seed_indices.reserve (seed_indices_orig.size ());
402  float search_radius = 0.5f*seed_resolution_;
403  // This is 1/20th of the number of voxels which fit in a planar slice through search volume
404  // Area of planar slice / area of voxel side. (Note: This is smaller than the value mentioned in the original paper)
405  float min_points = 0.05f * (search_radius)*(search_radius) * 3.1415926536f / (resolution_*resolution_);
406  for (const int &index_orig : seed_indices_orig)
407  {
408  int num = voxel_kdtree_->radiusSearch (index_orig, search_radius , neighbors, sqr_distances);
409  int min_index = index_orig;
410  if ( num > min_points)
411  {
412  seed_indices.push_back (min_index);
413  }
414 
415  }
416  // std::cout << "Number of seed points after filtering="<<seed_points.size ()<<std::endl;
417 
418 }
419 
420 
421 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
422 template <typename PointT> void
424 {
425  //Go through each supervoxel and remove all it's leaves
426  for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr)
427  {
428  sv_itr->removeAllLeaves ();
429  }
430 
431  std::vector<int> closest_index;
432  std::vector<float> distance;
433  //Now go through each supervoxel, find voxel closest to its center, add it in
434  for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr)
435  {
436  PointT point;
437  sv_itr->getXYZ (point.x, point.y, point.z);
438  voxel_kdtree_->nearestKSearch (point, 1, closest_index, distance);
439 
440  LeafContainerT* seed_leaf = adjacency_octree_->at (closest_index[0]);
441  if (seed_leaf)
442  {
443  sv_itr->addLeaf (seed_leaf);
444  }
445  else
446  {
447  PCL_WARN ("Could not find leaf in pcl::SupervoxelClustering<PointT>::reseedSupervoxels - supervoxel will be deleted \n");
448  }
449  }
450 
451 }
452 
453 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
454 template <typename PointT> void
456 {
457  p.x /= p.z;
458  p.y /= p.z;
459  p.z = std::log (p.z);
460 }
461 
462 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
463 template <typename PointT> float
465 {
466 
467  float spatial_dist = (v1.xyz_ - v2.xyz_).norm () / seed_resolution_;
468  float color_dist = (v1.rgb_ - v2.rgb_).norm () / 255.0f;
469  float cos_angle_normal = 1.0f - std::abs (v1.normal_.dot (v2.normal_));
470  // std::cout << "s="<<spatial_dist<<" c="<<color_dist<<" an="<<cos_angle_normal<<"\n";
471  return cos_angle_normal * normal_importance_ + color_dist * color_importance_+ spatial_dist * spatial_importance_;
472 
473 }
474 
475 
476 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
477 ///////// GETTER FUNCTIONS
478 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
479 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
480 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
481 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
482 template <typename PointT> void
484 {
485  adjacency_list_arg.clear ();
486  //Add a vertex for each label, store ids in map
487  std::map <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  std::vector <int> indices;
589  std::vector <float> sqr_distances;
590  for (auto i_labeled = labeled_cloud->begin (); i_labeled != labeled_cloud->end (); ++i_labeled,++i_input)
591  {
592  if ( !pcl::isFinite<PointT> (*i_input))
593  i_labeled->label = 0;
594  else
595  {
596  i_labeled->label = 0;
597  LeafContainerT *leaf = adjacency_octree_->getLeafContainerAtPoint (*i_input);
598  VoxelData& voxel_data = leaf->getData ();
599  if (voxel_data.owner_)
600  i_labeled->label = voxel_data.owner_->getLabel ();
601 
602  }
603 
604  }
605 
606  return (labeled_cloud);
607 }
608 
609 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
610 template <typename PointT> pcl::PointCloud<pcl::PointNormal>::Ptr
611 pcl::SupervoxelClustering<PointT>::makeSupervoxelNormalCloud (std::map<std::uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters)
612 {
614  normal_cloud->resize (supervoxel_clusters.size ());
615  pcl::PointCloud<pcl::PointNormal>::iterator normal_cloud_itr = normal_cloud->begin ();
616  for (auto sv_itr = supervoxel_clusters.cbegin (), sv_itr_end = supervoxel_clusters.cend ();
617  sv_itr != sv_itr_end; ++sv_itr, ++normal_cloud_itr)
618  {
619  (sv_itr->second)->getCentroidPointNormal (*normal_cloud_itr);
620  }
621  return normal_cloud;
622 }
623 
624 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
625 template <typename PointT> float
627 {
628  return (resolution_);
629 }
630 
631 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
632 template <typename PointT> void
634 {
635  resolution_ = resolution;
636 
637 }
638 
639 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
640 template <typename PointT> float
642 {
643  return (seed_resolution_);
644 }
645 
646 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
647 template <typename PointT> void
649 {
650  seed_resolution_ = seed_resolution;
651 }
652 
653 
654 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
655 template <typename PointT> void
657 {
658  color_importance_ = val;
659 }
660 
661 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
662 template <typename PointT> void
664 {
665  spatial_importance_ = val;
666 }
667 
668 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
669 template <typename PointT> void
671 {
672  normal_importance_ = val;
673 }
674 
675 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
676 template <typename PointT> void
678 {
679  use_default_transform_behaviour_ = false;
680  use_single_camera_transform_ = val;
681 }
682 
683 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
684 template <typename PointT> int
686 {
687  int max_label = 0;
688  for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
689  {
690  int temp = sv_itr->getLabel ();
691  if (temp > max_label)
692  max_label = temp;
693  }
694  return max_label;
695 }
696 
697 namespace pcl
698 {
699  namespace octree
700  {
701  //Explicit overloads for RGB types
702  template<>
703  void
705 
706  template<>
707  void
709 
710  //Explicit overloads for RGB types
711  template<> void
713 
714  template<> void
716 
717  //Explicit overloads for XYZ types
718  template<>
719  void
721 
722  template<> void
724  }
725 }
726 
727 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
728 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
729 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
730 namespace pcl
731 {
732 
733  template<> void
735 
736  template<> void
738 
739  template<typename PointT> void
741  {
742  //XYZ is required or this doesn't make much sense...
743  point_arg.x = xyz_[0];
744  point_arg.y = xyz_[1];
745  point_arg.z = xyz_[2];
746  }
747 
748  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
749  template <typename PointT> void
751  {
752  normal_arg.normal_x = normal_[0];
753  normal_arg.normal_y = normal_[1];
754  normal_arg.normal_z = normal_[2];
755  normal_arg.curvature = curvature_;
756  }
757 }
758 
759 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
760 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
761 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
762 template <typename PointT> void
764 {
765  leaves_.insert (leaf_arg);
766  VoxelData& voxel_data = leaf_arg->getData ();
767  voxel_data.owner_ = this;
768 }
769 
770 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
771 template <typename PointT> void
773 {
774  leaves_.erase (leaf_arg);
775 }
776 
777 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
778 template <typename PointT> void
780 {
781  for (auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr)
782  {
783  VoxelData& voxel = ((*leaf_itr)->getData ());
784  voxel.owner_ = nullptr;
785  voxel.distance_ = std::numeric_limits<float>::max ();
786  }
787  leaves_.clear ();
788 }
789 
790 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
791 template <typename PointT> void
793 {
794  //std::cout << "Expanding sv "<<label_<<", owns "<<leaves_.size ()<<" voxels\n";
795  //Buffer of new neighbors - initial size is just a guess of most possible
796  std::vector<LeafContainerT*> new_owned;
797  new_owned.reserve (leaves_.size () * 9);
798  //For each leaf belonging to this supervoxel
799  for (auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr)
800  {
801  //for each neighbor of the leaf
802  for (typename LeafContainerT::const_iterator neighb_itr=(*leaf_itr)->cbegin (); neighb_itr!=(*leaf_itr)->cend (); ++neighb_itr)
803  {
804  //Get a reference to the data contained in the leaf
805  VoxelData& neighbor_voxel = ((*neighb_itr)->getData ());
806  //TODO this is a shortcut, really we should always recompute distance
807  if(neighbor_voxel.owner_ == this)
808  continue;
809  //Compute distance to the neighbor
810  float dist = parent_->voxelDataDistance (centroid_, neighbor_voxel);
811  //If distance is less than previous, we remove it from its owner's list
812  //and change the owner to this and distance (we *steal* it!)
813  if (dist < neighbor_voxel.distance_)
814  {
815  neighbor_voxel.distance_ = dist;
816  if (neighbor_voxel.owner_ != this)
817  {
818  if (neighbor_voxel.owner_)
819  (neighbor_voxel.owner_)->removeLeaf(*neighb_itr);
820  neighbor_voxel.owner_ = this;
821  new_owned.push_back (*neighb_itr);
822  }
823  }
824  }
825  }
826  //Push all new owned onto the owned leaf set
827  for (auto new_owned_itr = new_owned.cbegin (); new_owned_itr != new_owned.cend (); ++new_owned_itr)
828  {
829  leaves_.insert (*new_owned_itr);
830  }
831 }
832 
833 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
834 template <typename PointT> void
836 {
837  //For each leaf belonging to this supervoxel, get its neighbors, build an index vector, compute normal
838  for (auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr)
839  {
840  VoxelData& voxel_data = (*leaf_itr)->getData ();
841  std::vector<int> indices;
842  indices.reserve (81);
843  //Push this point
844  indices.push_back (voxel_data.idx_);
845  for (typename LeafContainerT::const_iterator neighb_itr=(*leaf_itr)->cbegin (); neighb_itr!=(*leaf_itr)->cend (); ++neighb_itr)
846  {
847  //Get a reference to the data contained in the leaf
848  VoxelData& neighbor_voxel_data = ((*neighb_itr)->getData ());
849  //If the neighbor is in this supervoxel, use it
850  if (neighbor_voxel_data.owner_ == this)
851  {
852  indices.push_back (neighbor_voxel_data.idx_);
853  //Also check its neighbors
854  for (typename LeafContainerT::const_iterator neighb_neighb_itr=(*neighb_itr)->cbegin (); neighb_neighb_itr!=(*neighb_itr)->cend (); ++neighb_neighb_itr)
855  {
856  VoxelData& neighb_neighb_voxel_data = (*neighb_neighb_itr)->getData ();
857  if (neighb_neighb_voxel_data.owner_ == this)
858  indices.push_back (neighb_neighb_voxel_data.idx_);
859  }
860 
861 
862  }
863  }
864  //Compute normal
865  pcl::computePointNormal (*parent_->voxel_centroid_cloud_, indices, voxel_data.normal_, voxel_data.curvature_);
866  pcl::flipNormalTowardsViewpoint (parent_->voxel_centroid_cloud_->points[voxel_data.idx_], 0.0f,0.0f,0.0f, voxel_data.normal_);
867  voxel_data.normal_[3] = 0.0f;
868  voxel_data.normal_.normalize ();
869  }
870 }
871 
872 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
873 template <typename PointT> void
875 {
876  centroid_.normal_ = Eigen::Vector4f::Zero ();
877  centroid_.xyz_ = Eigen::Vector3f::Zero ();
878  centroid_.rgb_ = Eigen::Vector3f::Zero ();
879  for (auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr)
880  {
881  const VoxelData& leaf_data = (*leaf_itr)->getData ();
882  centroid_.normal_ += leaf_data.normal_;
883  centroid_.xyz_ += leaf_data.xyz_;
884  centroid_.rgb_ += leaf_data.rgb_;
885  }
886  centroid_.normal_.normalize ();
887  centroid_.xyz_ /= static_cast<float> (leaves_.size ());
888  centroid_.rgb_ /= static_cast<float> (leaves_.size ());
889 }
890 
891 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
892 template <typename PointT> void
894 {
895  voxels.reset (new pcl::PointCloud<PointT>);
896  voxels->clear ();
897  voxels->resize (leaves_.size ());
898  typename pcl::PointCloud<PointT>::iterator voxel_itr = voxels->begin ();
899  for (auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr, ++voxel_itr)
900  {
901  const VoxelData& leaf_data = (*leaf_itr)->getData ();
902  leaf_data.getPoint (*voxel_itr);
903  }
904 }
905 
906 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
907 template <typename PointT> void
909 {
910  normals.reset (new pcl::PointCloud<Normal>);
911  normals->clear ();
912  normals->resize (leaves_.size ());
913  typename pcl::PointCloud<Normal>::iterator normal_itr = normals->begin ();
914  for (auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr, ++normal_itr)
915  {
916  const VoxelData& leaf_data = (*leaf_itr)->getData ();
917  leaf_data.getNormal (*normal_itr);
918  }
919 }
920 
921 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
922 template <typename PointT> void
923 pcl::SupervoxelClustering<PointT>::SupervoxelHelper::getNeighborLabels (std::set<std::uint32_t> &neighbor_labels) const
924 {
925  neighbor_labels.clear ();
926  //For each leaf belonging to this supervoxel
927  for (auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr)
928  {
929  //for each neighbor of the leaf
930  for (typename LeafContainerT::const_iterator neighb_itr=(*leaf_itr)->cbegin (); neighb_itr!=(*leaf_itr)->cend (); ++neighb_itr)
931  {
932  //Get a reference to the data contained in the leaf
933  VoxelData& neighbor_voxel = ((*neighb_itr)->getData ());
934  //If it has an owner, and it's not us - get it's owner's label insert into set
935  if (neighbor_voxel.owner_ != this && neighbor_voxel.owner_)
936  {
937  neighbor_labels.insert (neighbor_voxel.owner_->getLabel ());
938  }
939  }
940  }
941 }
942 
943 
944 #endif // PCL_SUPERVOXEL_CLUSTERING_HPP_
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.
void setSeedResolution(float seed_resolution)
Set the resolution of the octree seed voxels.
int radiusSearch(const PointT &point, double radius, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn=0) const override
Search for all the nearest neighbors of the query point in a given radius.
Definition: kdtree.hpp:97
void addPointsFromInputCloud()
Add points from input point cloud to octree.
Octree adjacency leaf container class- stores a list of pointers to neighbors, number of points added...
VoxelAdjacencyList::vertex_descriptor VoxelID
typename VectorType::const_iterator const_iterator
Definition: point_cloud.h:425
Octree pointcloud voxel class which maintains adjacency information for its voxels.
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
void resize(std::size_t n)
Resize the cloud.
Definition: point_cloud.h:439
void setNormalImportance(float val)
Set the importance of scalar normal product for supervoxels.
iterator end()
Definition: point_cloud.h:427
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_EXPORTS void copyPointCloud(const pcl::PCLPointCloud2 &cloud_in, const std::vector< int > &indices, pcl::PCLPointCloud2 &cloud_out)
Extract the indices of a given point cloud as a new point cloud.
void setInputCloud(const typename pcl::PointCloud< PointT >::ConstPtr &cloud) override
This method sets the cloud to be supervoxelized.
int getMaxLabel() const
Returns the current maximum (highest) label.
pcl::PointCloud< pcl::PointXYZL >::Ptr getLabeledVoxelCloud() const
Returns labeled voxelized cloud Points that belong to the same supervoxel have the same label...
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.
void setInputCloud(const PointCloudConstPtr &cloud_arg, const IndicesConstPtr &indices_arg=IndicesConstPtr())
Provide a pointer to the input data set.
A point structure representing Euclidean xyz coordinates, and the RGBA color.
bool computePointNormal(const pcl::PointCloud< PointT > &cloud, Eigen::Vector4f &plane_parameters, float &curvature)
Compute the Least-Squares plane fit for a given set of points, and return the estimated plane paramet...
Definition: normal_3d.h:60
bool initCompute()
This method should get called before starting the actual computation.
Definition: pcl_base.hpp:138
SupervoxelClustering(float voxel_resolution, float seed_resolution)
Constructor that sets default values for member variables.
Supervoxel container class - stores a cluster extracted using supervoxel clustering.
void flipNormalTowardsViewpoint(const PointT &point, float vp_x, float vp_y, float vp_z, Eigen::Matrix< Scalar, 4, 1 > &normal)
Flip (in place) the estimated normal of a point towards a given viewpoint.
Definition: normal_3d.h:121
virtual void setNormalCloud(typename NormalCloudT::ConstPtr normal_cloud)
This method sets the normals to be used for supervoxels (should be same size as input cloud) ...
pcl::PointCloud< PointXYZL >::Ptr getLabeledCloud() const
Returns labeled cloud Points that belong to the same supervoxel have the same label.
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr()) override
Provide a pointer to the input dataset.
Definition: kdtree.hpp:77
A point structure representing Euclidean xyz coordinates.
pcl::PointCloud< PointT >::Ptr getVoxelCentroidCloud() const
Returns a deep copy of the voxel centroid cloud.
void getNormal(Normal &normal_arg) const
Gets the data of in the form of a normal.
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...
boost::shared_ptr< Supervoxel< PointT > > Ptr
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:412
void clear()
Removes all points in a cloud and sets the width and height to 0.
Definition: point_cloud.h:589
bool deinitCompute()
This method should get called after finishing the actual computation.
Definition: pcl_base.hpp:171
PointCloud represents the base class in PCL for storing collections of 3D points. ...
Implements a supervoxel algorithm based on voxel structure, normals, and rgb values.
VoxelAdjacencyList::edge_descriptor EdgeID
typename VectorType::iterator iterator
Definition: point_cloud.h:424
DataT & getData()
Returns a reference to the data member to access it without copying.
float getSeedResolution() const
Get the resolution of the octree seed voxels.
~SupervoxelClustering()
This destructor destroys the cloud, normals and search method used for finding neighbors.
Octree pointcloud search class
Definition: octree_search.h:56
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:413
void getPoint(PointT &point_arg) const
Gets the data of in the form of a point.
PointCloudConstPtr input_
The input point cloud dataset.
Definition: pcl_base.h:151
iterator begin()
Definition: point_cloud.h:426
void getSupervoxelAdjacencyList(VoxelAdjacencyList &adjacency_list_arg) const
Gets the adjacency list (Boost Graph library) which gives connections between supervoxels.
A point structure representing Euclidean xyz coordinates, and the RGB color.
void getSupervoxelAdjacency(std::multimap< std::uint32_t, std::uint32_t > &label_adjacency) const
Get a multimap which gives supervoxel adjacency.
int nearestKSearch(const PointT &point, int k, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances) const override
Search for the k-nearest neighbors for the given query point.
Definition: kdtree.hpp:88
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.
int getOccupiedVoxelCenters(AlignedPointTVector &voxel_center_list_arg) const
Get a PointT vector of centers of all occupied voxels.
float getVoxelResolution() const
Get the resolution of the octree voxels.
bool empty() const
Definition: point_cloud.h:434
void setSpatialImportance(float val)
Set the importance of spatial distance for supervoxels.