Point Cloud Library (PCL)  1.9.1
supervoxel_clustering.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  *
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Willow Garage, Inc. nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *
35  * Author : jpapon@gmail.com
36  * Email : jpapon@gmail.com
37  *
38  */
39 
40 #ifndef PCL_SEGMENTATION_SUPERVOXEL_CLUSTERING_HPP_
41 #define PCL_SEGMENTATION_SUPERVOXEL_CLUSTERING_HPP_
42 
43 #include <pcl/segmentation/supervoxel_clustering.h>
44 
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.
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_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.
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
int getMaxLabel() const
Returns the current maximum (highest) label.
VectorType::iterator iterator
Definition: point_cloud.h:440
pcl::PointCloud< pcl::PointXYZL >::Ptr getLabeledVoxelCloud() const
Returns labeled voxelized cloud Points that belong to the same supervoxel have the same label...
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.
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.
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.
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...
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
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:61
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 clear()
Removes all points in a cloud and sets the width and height to 0.
Definition: point_cloud.h:576
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
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.
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
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
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.
boost::shared_ptr< Supervoxel< PointT > > Ptr
boost::adjacency_list< boost::setS, boost::setS, boost::undirectedS, uint32_t, float > VoxelAdjacencyList
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.
void getSupervoxelAdjacency(std::multimap< uint32_t, uint32_t > &label_adjacency) const
Get a multimap which gives supervoxel adjacency.
size_t size() const
Definition: point_cloud.h:448
VectorType::const_iterator const_iterator
Definition: point_cloud.h:441
void setSpatialImportance(float val)
Set the importance of spatial distance for supervoxels.