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