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