Point Cloud Library (PCL)  1.15.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>
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 (pcl::search::autoSelectMethod<PointT>(voxel_centroid_cloud_, false));
385  }
386 
387  for (int i = 0; i < num_seeds; ++i)
388  {
389  voxel_kdtree_->nearestKSearch (voxel_centers[i], 1, closest_index, distance);
390  seed_indices_orig[i] = closest_index[0];
391  }
392 
393  pcl::Indices neighbors;
394  std::vector<float> sqr_distances;
395  seed_indices.reserve (seed_indices_orig.size ());
396  float search_radius = 0.5f*seed_resolution_;
397  // This is 1/20th of the number of voxels which fit in a planar slice through search volume
398  // Area of planar slice / area of voxel side. (Note: This is smaller than the value mentioned in the original paper)
399  float min_points = 0.05f * (search_radius)*(search_radius) * 3.1415926536f / (resolution_*resolution_);
400  for (const auto &index_orig : seed_indices_orig)
401  {
402  int num = voxel_kdtree_->radiusSearch (index_orig, search_radius , neighbors, sqr_distances);
403  if ( num > min_points)
404  {
405  seed_indices.push_back (index_orig);
406  }
407 
408  }
409  // std::cout << "Number of seed points after filtering="<<seed_points.size ()<<std::endl;
410 
411 }
412 
413 
414 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
415 template <typename PointT> void
417 {
418  //Go through each supervoxel and remove all it's leaves
419  for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr)
420  {
421  sv_itr->removeAllLeaves ();
422  }
423 
424  Indices closest_index;
425  std::vector<float> distance;
426  //Now go through each supervoxel, find voxel closest to its center, add it in
427  for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr)
428  {
429  PointT point;
430  sv_itr->getXYZ (point.x, point.y, point.z);
431  voxel_kdtree_->nearestKSearch (point, 1, closest_index, distance);
432 
433  LeafContainerT* seed_leaf = adjacency_octree_->at (closest_index[0]);
434  if (seed_leaf)
435  {
436  sv_itr->addLeaf (seed_leaf);
437  }
438  else
439  {
440  PCL_WARN ("Could not find leaf in pcl::SupervoxelClustering<PointT>::reseedSupervoxels - supervoxel will be deleted \n");
441  }
442  }
443 
444 }
445 
446 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
447 template <typename PointT> void
449 {
450  p.x /= p.z;
451  p.y /= p.z;
452  p.z = std::log (p.z);
453 }
454 
455 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
456 template <typename PointT> float
457 pcl::SupervoxelClustering<PointT>::voxelDataDistance (const VoxelData &v1, const VoxelData &v2) const
458 {
459 
460  float spatial_dist = (v1.xyz_ - v2.xyz_).norm () / seed_resolution_;
461  float color_dist = (v1.rgb_ - v2.rgb_).norm () / 255.0f;
462  float cos_angle_normal = 1.0f - std::abs (v1.normal_.dot (v2.normal_));
463  // std::cout << "s="<<spatial_dist<<" c="<<color_dist<<" an="<<cos_angle_normal<<"\n";
464  return cos_angle_normal * normal_importance_ + color_dist * color_importance_+ spatial_dist * spatial_importance_;
465 
466 }
467 
468 
469 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
470 ///////// GETTER FUNCTIONS
471 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
472 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
473 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
474 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
475 template <typename PointT> void
477 {
478  adjacency_list_arg.clear ();
479  //Add a vertex for each label, store ids in map
480  std::map <std::uint32_t, VoxelID> label_ID_map;
481  for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
482  {
483  VoxelID node_id = add_vertex (adjacency_list_arg);
484  adjacency_list_arg[node_id] = (sv_itr->getLabel ());
485  label_ID_map.insert (std::make_pair (sv_itr->getLabel (), node_id));
486  }
487 
488  for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
489  {
490  std::uint32_t label = sv_itr->getLabel ();
491  std::set<std::uint32_t> neighbor_labels;
492  sv_itr->getNeighborLabels (neighbor_labels);
493  for (const unsigned int &neighbor_label : neighbor_labels)
494  {
495  bool edge_added;
496  EdgeID edge;
497  VoxelID u = (label_ID_map.find (label))->second;
498  VoxelID v = (label_ID_map.find (neighbor_label))->second;
499  boost::tie (edge, edge_added) = add_edge (u,v,adjacency_list_arg);
500  //Calc distance between centers, set as edge weight
501  if (edge_added)
502  {
503  VoxelData centroid_data = (sv_itr)->getCentroid ();
504  //Find the neighbor with this label
505  VoxelData neighb_centroid_data;
506 
507  for (typename HelperListT::const_iterator neighb_itr = supervoxel_helpers_.cbegin (); neighb_itr != supervoxel_helpers_.cend (); ++neighb_itr)
508  {
509  if (neighb_itr->getLabel () == neighbor_label)
510  {
511  neighb_centroid_data = neighb_itr->getCentroid ();
512  break;
513  }
514  }
515 
516  float length = voxelDataDistance (centroid_data, neighb_centroid_data);
517  adjacency_list_arg[edge] = length;
518  }
519  }
520 
521  }
522 
523 }
524 
525 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
526 template <typename PointT> void
527 pcl::SupervoxelClustering<PointT>::getSupervoxelAdjacency (std::multimap<std::uint32_t, std::uint32_t> &label_adjacency) const
528 {
529  label_adjacency.clear ();
530  for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
531  {
532  std::uint32_t label = sv_itr->getLabel ();
533  std::set<std::uint32_t> neighbor_labels;
534  sv_itr->getNeighborLabels (neighbor_labels);
535  for (const unsigned int &neighbor_label : neighbor_labels)
536  label_adjacency.insert (std::pair<std::uint32_t,std::uint32_t> (label, neighbor_label) );
537  //if (neighbor_labels.size () == 0)
538  // std::cout << label<<"(size="<<sv_itr->size () << ") has "<<neighbor_labels.size () << "\n";
539  }
540 }
541 
542 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
543 template <typename PointT> typename pcl::PointCloud<PointT>::Ptr
545 {
546  typename PointCloudT::Ptr centroid_copy (new PointCloudT);
547  copyPointCloud (*voxel_centroid_cloud_, *centroid_copy);
548  return centroid_copy;
549 }
550 
551 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
552 template <typename PointT> pcl::PointCloud<pcl::PointXYZL>::Ptr
554 {
556  for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
557  {
558  typename PointCloudT::Ptr voxels;
559  sv_itr->getVoxels (voxels);
561  copyPointCloud (*voxels, xyzl_copy);
562 
563  auto xyzl_copy_itr = xyzl_copy.begin ();
564  for ( ; xyzl_copy_itr != xyzl_copy.end (); ++xyzl_copy_itr)
565  xyzl_copy_itr->label = sv_itr->getLabel ();
566 
567  *labeled_voxel_cloud += xyzl_copy;
568  }
569 
570  return labeled_voxel_cloud;
571 }
572 
573 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
574 template <typename PointT> pcl::PointCloud<pcl::PointXYZL>::Ptr
576 {
578  pcl::copyPointCloud (*input_,*labeled_cloud);
579 
580  auto i_input = input_->begin ();
581  for (auto i_labeled = labeled_cloud->begin (); i_labeled != labeled_cloud->end (); ++i_labeled,++i_input)
582  {
583  if ( !pcl::isFinite<PointT> (*i_input))
584  i_labeled->label = 0;
585  else
586  {
587  i_labeled->label = 0;
588  LeafContainerT *leaf = adjacency_octree_->getLeafContainerAtPoint (*i_input);
589  VoxelData& voxel_data = leaf->getData ();
590  if (voxel_data.owner_)
591  i_labeled->label = voxel_data.owner_->getLabel ();
592 
593  }
594 
595  }
596 
597  return (labeled_cloud);
598 }
599 
600 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
601 template <typename PointT> pcl::PointCloud<pcl::PointNormal>::Ptr
602 pcl::SupervoxelClustering<PointT>::makeSupervoxelNormalCloud (std::map<std::uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters)
603 {
605  normal_cloud->resize (supervoxel_clusters.size ());
606  auto normal_cloud_itr = normal_cloud->begin ();
607  for (auto sv_itr = supervoxel_clusters.cbegin (), sv_itr_end = supervoxel_clusters.cend ();
608  sv_itr != sv_itr_end; ++sv_itr, ++normal_cloud_itr)
609  {
610  (sv_itr->second)->getCentroidPointNormal (*normal_cloud_itr);
611  }
612  return normal_cloud;
613 }
614 
615 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
616 template <typename PointT> float
618 {
619  return (resolution_);
620 }
621 
622 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
623 template <typename PointT> void
625 {
626  resolution_ = resolution;
627 
628 }
629 
630 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
631 template <typename PointT> float
633 {
634  return (seed_resolution_);
635 }
636 
637 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
638 template <typename PointT> void
640 {
641  seed_resolution_ = seed_resolution;
642 }
643 
644 
645 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
646 template <typename PointT> void
648 {
649  color_importance_ = val;
650 }
651 
652 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
653 template <typename PointT> void
655 {
656  spatial_importance_ = val;
657 }
658 
659 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
660 template <typename PointT> void
662 {
663  normal_importance_ = val;
664 }
665 
666 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
667 template <typename PointT> void
669 {
670  use_default_transform_behaviour_ = false;
671  use_single_camera_transform_ = val;
672 }
673 
674 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
675 template <typename PointT> int
677 {
678  int max_label = 0;
679  for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
680  {
681  int temp = sv_itr->getLabel ();
682  if (temp > max_label)
683  max_label = temp;
684  }
685  return max_label;
686 }
687 
688 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
689 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
690 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
691 namespace pcl
692 {
693  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
694  template <typename PointT> void
696  {
697  normal_arg.normal_x = normal_[0];
698  normal_arg.normal_y = normal_[1];
699  normal_arg.normal_z = normal_[2];
700  normal_arg.curvature = curvature_;
701  }
702 }
703 
704 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
705 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
706 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
707 template <typename PointT> void
709 {
710  leaves_.insert (leaf_arg);
711  VoxelData& voxel_data = leaf_arg->getData ();
712  voxel_data.owner_ = this;
713 }
714 
715 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
716 template <typename PointT> void
718 {
719  leaves_.erase (leaf_arg);
720 }
721 
722 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
723 template <typename PointT> void
725 {
726  for (auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr)
727  {
728  VoxelData& voxel = ((*leaf_itr)->getData ());
729  voxel.owner_ = nullptr;
730  voxel.distance_ = std::numeric_limits<float>::max ();
731  }
732  leaves_.clear ();
733 }
734 
735 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
736 template <typename PointT> void
738 {
739  //std::cout << "Expanding sv "<<label_<<", owns "<<leaves_.size ()<<" voxels\n";
740  //Buffer of new neighbors - initial size is just a guess of most possible
741  std::vector<LeafContainerT*> new_owned;
742  new_owned.reserve (leaves_.size () * 9);
743  //For each leaf belonging to this supervoxel
744  for (auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr)
745  {
746  //for each neighbor of the leaf
747  for (auto neighb_itr=(*leaf_itr)->cbegin (); neighb_itr!=(*leaf_itr)->cend (); ++neighb_itr)
748  {
749  //Get a reference to the data contained in the leaf
750  VoxelData& neighbor_voxel = ((*neighb_itr)->getData ());
751  //TODO this is a shortcut, really we should always recompute distance
752  if(neighbor_voxel.owner_ == this)
753  continue;
754  //Compute distance to the neighbor
755  float dist = parent_->voxelDataDistance (centroid_, neighbor_voxel);
756  //If distance is less than previous, we remove it from its owner's list
757  //and change the owner to this and distance (we *steal* it!)
758  if (dist < neighbor_voxel.distance_)
759  {
760  neighbor_voxel.distance_ = dist;
761  if (neighbor_voxel.owner_ != this)
762  {
763  if (neighbor_voxel.owner_)
764  (neighbor_voxel.owner_)->removeLeaf(*neighb_itr);
765  neighbor_voxel.owner_ = this;
766  new_owned.push_back (*neighb_itr);
767  }
768  }
769  }
770  }
771  //Push all new owned onto the owned leaf set
772  for (auto new_owned_itr = new_owned.cbegin (); new_owned_itr != new_owned.cend (); ++new_owned_itr)
773  {
774  leaves_.insert (*new_owned_itr);
775  }
776 }
777 
778 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
779 template <typename PointT> void
781 {
782  //For each leaf belonging to this supervoxel, get its neighbors, build an index vector, compute normal
783  for (auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr)
784  {
785  VoxelData& voxel_data = (*leaf_itr)->getData ();
786  Indices indices;
787  indices.reserve (81);
788  //Push this point
789  indices.push_back (voxel_data.idx_);
790  for (auto neighb_itr=(*leaf_itr)->cbegin (); neighb_itr!=(*leaf_itr)->cend (); ++neighb_itr)
791  {
792  //Get a reference to the data contained in the leaf
793  VoxelData& neighbor_voxel_data = ((*neighb_itr)->getData ());
794  //If the neighbor is in this supervoxel, use it
795  if (neighbor_voxel_data.owner_ == this)
796  {
797  indices.push_back (neighbor_voxel_data.idx_);
798  //Also check its neighbors
799  for (auto neighb_neighb_itr=(*neighb_itr)->cbegin (); neighb_neighb_itr!=(*neighb_itr)->cend (); ++neighb_neighb_itr)
800  {
801  VoxelData& neighb_neighb_voxel_data = (*neighb_neighb_itr)->getData ();
802  if (neighb_neighb_voxel_data.owner_ == this)
803  indices.push_back (neighb_neighb_voxel_data.idx_);
804  }
805 
806 
807  }
808  }
809  //Compute normal
810  pcl::computePointNormal (*parent_->voxel_centroid_cloud_, indices, voxel_data.normal_, voxel_data.curvature_);
811  pcl::flipNormalTowardsViewpoint ((*parent_->voxel_centroid_cloud_)[voxel_data.idx_], 0.0f,0.0f,0.0f, voxel_data.normal_);
812  voxel_data.normal_[3] = 0.0f;
813  voxel_data.normal_.normalize ();
814  }
815 }
816 
817 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
818 template <typename PointT> void
820 {
821  centroid_.normal_ = Eigen::Vector4f::Zero ();
822  centroid_.xyz_ = Eigen::Vector3f::Zero ();
823  centroid_.rgb_ = Eigen::Vector3f::Zero ();
824  for (auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr)
825  {
826  const VoxelData& leaf_data = (*leaf_itr)->getData ();
827  centroid_.normal_ += leaf_data.normal_;
828  centroid_.xyz_ += leaf_data.xyz_;
829  centroid_.rgb_ += leaf_data.rgb_;
830  }
831  centroid_.normal_.normalize ();
832  centroid_.xyz_ /= static_cast<float> (leaves_.size ());
833  centroid_.rgb_ /= static_cast<float> (leaves_.size ());
834 }
835 
836 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
837 template <typename PointT> void
839 {
840  voxels.reset (new pcl::PointCloud<PointT>);
841  voxels->clear ();
842  voxels->resize (leaves_.size ());
843  auto voxel_itr = voxels->begin ();
844  for (auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr, ++voxel_itr)
845  {
846  const VoxelData& leaf_data = (*leaf_itr)->getData ();
847  leaf_data.getPoint (*voxel_itr);
848  }
849 }
850 
851 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
852 template <typename PointT> void
854 {
855  normals.reset (new pcl::PointCloud<Normal>);
856  normals->clear ();
857  normals->resize (leaves_.size ());
858  auto normal_itr = normals->begin ();
859  for (auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr, ++normal_itr)
860  {
861  const VoxelData& leaf_data = (*leaf_itr)->getData ();
862  leaf_data.getNormal (*normal_itr);
863  }
864 }
865 
866 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
867 template <typename PointT> void
868 pcl::SupervoxelClustering<PointT>::SupervoxelHelper::getNeighborLabels (std::set<std::uint32_t> &neighbor_labels) const
869 {
870  neighbor_labels.clear ();
871  //For each leaf belonging to this supervoxel
872  for (auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr)
873  {
874  //for each neighbor of the leaf
875  for (auto neighb_itr=(*leaf_itr)->cbegin (); neighb_itr!=(*leaf_itr)->cend (); ++neighb_itr)
876  {
877  //Get a reference to the data contained in the leaf
878  VoxelData& neighbor_voxel = ((*neighb_itr)->getData ());
879  //If it has an owner, and it's not us - get it's owner's label insert into set
880  if (neighbor_voxel.owner_ != this && neighbor_voxel.owner_)
881  {
882  neighbor_labels.insert (neighbor_voxel.owner_->getLabel ());
883  }
884  }
885  }
886 }
887 
888 
889 #endif // PCL_SUPERVOXEL_CLUSTERING_HPP_
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:174
bool empty() const
Definition: point_cloud.h:447
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition: point_cloud.h:463
iterator end() noexcept
Definition: point_cloud.h:431
void clear()
Removes all points in a cloud and sets the width and height to 0.
Definition: point_cloud.h:886
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:414
iterator begin() noexcept
Definition: point_cloud.h:430
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:415
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.
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::octree::OctreePointCloudAdjacency< PointT, LeafContainerT > OctreeAdjacencyT
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 search class
Definition: octree_search.h:58
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, and the RGB color.