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  label_colors_.reserve (MAX_LABEL);
58  label_colors_.push_back (static_cast<uint32_t>(255) << 16 | static_cast<uint32_t>(0) << 8 | static_cast<uint32_t>(0));
59 
60  srand (static_cast<unsigned int> (time (0)));
61  for (size_t i_segment = 0; i_segment < MAX_LABEL - 1; i_segment++)
62  {
63  uint8_t r = static_cast<uint8_t>( (rand () % 256));
64  uint8_t g = static_cast<uint8_t>( (rand () % 256));
65  uint8_t b = static_cast<uint8_t>( (rand () % 256));
66  label_colors_.push_back (static_cast<uint32_t>(r) << 16 | static_cast<uint32_t>(g) << 8 | static_cast<uint32_t>(b));
67  }
68 
69  adjacency_octree_ = boost::make_shared <OctreeAdjacencyT> (resolution_);
70  if (use_single_camera_transform)
71  adjacency_octree_->setTransformFunction (boost::bind (&SupervoxelClustering::transformFunction, this, _1));
72 }
73 
74 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
75 template <typename PointT>
77 {
78 
79 }
80 
81 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
82 template <typename PointT> void
84 {
85  if ( cloud->size () == 0 )
86  {
87  PCL_ERROR ("[pcl::SupervoxelClustering::setInputCloud] Empty cloud set, doing nothing \n");
88  return;
89  }
90 
91  input_ = cloud;
92  adjacency_octree_->setInputCloud (cloud);
93 }
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 
206 template <typename PointT> void
208 {
209  voxel_centroid_cloud_ = boost::make_shared<PointCloudT> ();
210  voxel_centroid_cloud_->resize (adjacency_octree_->getLeafCount ());
211  typename LeafVectorT::iterator leaf_itr = adjacency_octree_->begin ();
212  typename PointCloudT::iterator cent_cloud_itr = voxel_centroid_cloud_->begin ();
213  for (int idx = 0 ; leaf_itr != adjacency_octree_->end (); ++leaf_itr, ++cent_cloud_itr, ++idx)
214  {
215  VoxelData& new_voxel_data = (*leaf_itr)->getData ();
216  //Add the point to the centroid cloud
217  new_voxel_data.getPoint (*cent_cloud_itr);
218  //voxel_centroid_cloud_->push_back(new_voxel_data.getPoint ());
219  new_voxel_data.idx_ = idx;
220  }
221 
222 
223  for (leaf_itr = adjacency_octree_->begin (); leaf_itr != adjacency_octree_->end (); ++leaf_itr)
224  {
225  VoxelData& new_voxel_data = (*leaf_itr)->getData ();
226  //For every point, get its neighbors, build an index vector, compute normal
227  std::vector<int> indices;
228  indices.reserve (81);
229  //Push this point
230  indices.push_back (new_voxel_data.idx_);
231  for (typename LeafContainerT::iterator neighb_itr=(*leaf_itr)->begin (); neighb_itr!=(*leaf_itr)->end (); ++neighb_itr)
232  {
233  VoxelData& neighb_voxel_data = (*neighb_itr)->getData ();
234  //Push neighbor index
235  indices.push_back (neighb_voxel_data.idx_);
236  //Get neighbors neighbors, push onto cloud
237  for (typename LeafContainerT::iterator neighb_neighb_itr=(*neighb_itr)->begin (); neighb_neighb_itr!=(*neighb_itr)->end (); ++neighb_neighb_itr)
238  {
239  VoxelData& neighb2_voxel_data = (*neighb_neighb_itr)->getData ();
240  indices.push_back (neighb2_voxel_data.idx_);
241  }
242  }
243  //Compute normal
244  pcl::computePointNormal (*voxel_centroid_cloud_, indices, new_voxel_data.normal_, new_voxel_data.curvature_);
245  pcl::flipNormalTowardsViewpoint (voxel_centroid_cloud_->points[new_voxel_data.idx_], 0.0f,0.0f,0.0f, new_voxel_data.normal_);
246  new_voxel_data.normal_[3] = 0.0f;
247  new_voxel_data.normal_.normalize ();
248  new_voxel_data.owner_ = 0;
249  new_voxel_data.distance_ = std::numeric_limits<float>::max ();
250  }
251 
252 
253 }
254 
255 
256 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
257 template <typename PointT> void
259 {
260 
261 
262  for (int i = 1; i < depth; ++i)
263  {
264  //Expand the the supervoxels by one iteration
265  for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr)
266  {
267  sv_itr->expand ();
268  }
269 
270  //Update the centers to reflect new centers
271  for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); )
272  {
273  if (sv_itr->size () == 0)
274  {
275  sv_itr = supervoxel_helpers_.erase (sv_itr);
276  }
277  else
278  {
279  sv_itr->updateCentroid ();
280  ++sv_itr;
281  }
282  }
283 
284  }
285 
286 }
287 
288 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
289 template <typename PointT> void
290 pcl::SupervoxelClustering<PointT>::makeSupervoxels (std::map<uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters)
291 {
292  supervoxel_clusters.clear ();
293  for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr)
294  {
295  uint32_t label = sv_itr->getLabel ();
296  supervoxel_clusters[label] = boost::make_shared<Supervoxel<PointT> > ();
297  sv_itr->getXYZ (supervoxel_clusters[label]->centroid_.x,supervoxel_clusters[label]->centroid_.y,supervoxel_clusters[label]->centroid_.z);
298  sv_itr->getRGB (supervoxel_clusters[label]->centroid_.rgba);
299  sv_itr->getNormal (supervoxel_clusters[label]->normal_);
300  sv_itr->getVoxels (supervoxel_clusters[label]->voxels_);
301  sv_itr->getNormals (supervoxel_clusters[label]->normals_);
302  }
303 }
304 
305 
306 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
307 template <typename PointT> void
308 pcl::SupervoxelClustering<PointT>::createSupervoxelHelpers (std::vector<PointT, Eigen::aligned_allocator<PointT> > &seed_points)
309 {
310 
311  supervoxel_helpers_.clear ();
312  for (int i = 0; i < seed_points.size (); ++i)
313  {
314  supervoxel_helpers_.push_back (new SupervoxelHelper(i+1,this));
315  //Find which leaf corresponds to this seed index
316  LeafContainerT* seed_leaf = adjacency_octree_->getLeafContainerAtPoint (seed_points[i]);
317  if (seed_leaf)
318  {
319  supervoxel_helpers_.back ().addLeaf (seed_leaf);
320  }
321  else
322  {
323  PCL_WARN ("Could not find leaf in pcl::SupervoxelClustering<PointT>::createSupervoxelHelpers - supervoxel will be deleted \n");
324  }
325  }
326 
327 }
328 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
329 template <typename PointT> void
330 pcl::SupervoxelClustering<PointT>::selectInitialSupervoxelSeeds (std::vector<PointT, Eigen::aligned_allocator<PointT> > &seed_points)
331 {
332  //TODO THIS IS BAD - SEEDING SHOULD BE BETTER
333  //TODO Switch to assigning leaves! Don't use Octree!
334 
335  // std::cout << "Size of centroid cloud="<<voxel_centroid_cloud_->size ()<<", seeding resolution="<<seed_resolution_<<"\n";
336  //Initialize octree with voxel centroids
337  pcl::octree::OctreePointCloudSearch <PointT> seed_octree (seed_resolution_);
338  seed_octree.setInputCloud (voxel_centroid_cloud_);
339  seed_octree.addPointsFromInputCloud ();
340  // std::cout << "Size of octree ="<<seed_octree.getLeafCount ()<<"\n";
341  std::vector<PointT, Eigen::aligned_allocator<PointT> > voxel_centers;
342  int num_seeds = seed_octree.getOccupiedVoxelCenters(voxel_centers);
343  //std::cout << "Number of seed points before filtering="<<voxel_centers.size ()<<std::endl;
344 
345  std::vector<int> seed_indices_orig;
346  seed_indices_orig.resize (num_seeds, 0);
347  seed_points.clear ();
348  std::vector<int> closest_index;
349  std::vector<float> distance;
350  closest_index.resize(1,0);
351  distance.resize(1,0);
352  if (voxel_kdtree_ == 0)
353  {
354  voxel_kdtree_ = boost::make_shared< pcl::search::KdTree<PointT> >();
355  voxel_kdtree_ ->setInputCloud (voxel_centroid_cloud_);
356  }
357 
358  for (int i = 0; i < num_seeds; ++i)
359  {
360  voxel_kdtree_->nearestKSearch (voxel_centers[i], 1, closest_index, distance);
361  seed_indices_orig[i] = closest_index[0];
362  }
363 
364  std::vector<int> neighbors;
365  std::vector<float> sqr_distances;
366  seed_points.reserve (seed_indices_orig.size ());
367  float search_radius = 0.5f*seed_resolution_;
368  // This is number of voxels which fit in a planar slice through search volume
369  // Area of planar slice / area of voxel side
370  float min_points = 0.05f * (search_radius)*(search_radius) * 3.1415926536f / (resolution_*resolution_);
371  for (int i = 0; i < seed_indices_orig.size (); ++i)
372  {
373  int num = voxel_kdtree_->radiusSearch (seed_indices_orig[i], search_radius , neighbors, sqr_distances);
374  int min_index = seed_indices_orig[i];
375  if ( num > min_points)
376  {
377  seed_points.push_back (voxel_centroid_cloud_->points[min_index]);
378  }
379 
380  }
381  // std::cout << "Number of seed points after filtering="<<seed_points.size ()<<std::endl;
382 
383 }
384 
385 
386 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
387 template <typename PointT> void
389 {
390  //Go through each supervoxel and remove all it's leaves
391  for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr)
392  {
393  sv_itr->removeAllLeaves ();
394  }
395 
396  std::vector<int> closest_index;
397  std::vector<float> distance;
398  //Now go through each supervoxel, find voxel closest to its center, add it in
399  for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr)
400  {
401  PointT point;
402  sv_itr->getXYZ (point.x, point.y, point.z);
403  voxel_kdtree_->nearestKSearch (point, 1, closest_index, distance);
404 
405  LeafContainerT* seed_leaf = adjacency_octree_->getLeafContainerAtPoint (voxel_centroid_cloud_->points[closest_index[0]]);
406  if (seed_leaf)
407  {
408  sv_itr->addLeaf (seed_leaf);
409  }
410  }
411 
412 }
413 
414 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
415 template <typename PointT> void
417 {
418  p.x /= p.z;
419  p.y /= p.z;
420  p.z = std::log (p.z);
421 }
422 
423 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
424 template <typename PointT> float
425 pcl::SupervoxelClustering<PointT>::voxelDataDistance (const VoxelData &v1, const VoxelData &v2) const
426 {
427 
428  float spatial_dist = (v1.xyz_ - v2.xyz_).norm () / seed_resolution_;
429  float color_dist = (v1.rgb_ - v2.rgb_).norm () / 255.0f;
430  float cos_angle_normal = 1.0f - std::abs (v1.normal_.dot (v2.normal_));
431  // std::cout << "s="<<spatial_dist<<" c="<<color_dist<<" an="<<cos_angle_normal<<"\n";
432  return cos_angle_normal * normal_importance_ + color_dist * color_importance_+ spatial_dist * spatial_importance_;
433 
434 }
435 
436 
437 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
438 ///////// GETTER FUNCTIONS
439 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
440 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
441 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
442 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
443 template <typename PointT> void
445 {
446  adjacency_list_arg.clear ();
447  //Add a vertex for each label, store ids in map
448  std::map <uint32_t, VoxelID> label_ID_map;
449  for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
450  {
451  VoxelID node_id = add_vertex (adjacency_list_arg);
452  adjacency_list_arg[node_id] = (sv_itr->getLabel ());
453  label_ID_map.insert (std::make_pair (sv_itr->getLabel (), node_id));
454  }
455 
456  for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
457  {
458  uint32_t label = sv_itr->getLabel ();
459  std::set<uint32_t> neighbor_labels;
460  sv_itr->getNeighborLabels (neighbor_labels);
461  for (std::set<uint32_t>::iterator label_itr = neighbor_labels.begin (); label_itr != neighbor_labels.end (); ++label_itr)
462  {
463  bool edge_added;
464  EdgeID edge;
465  VoxelID u = (label_ID_map.find (label))->second;
466  VoxelID v = (label_ID_map.find (*label_itr))->second;
467  boost::tie (edge, edge_added) = add_edge (u,v,adjacency_list_arg);
468  //Calc distance between centers, set as edge weight
469  if (edge_added)
470  {
471  VoxelData centroid_data = (sv_itr)->getCentroid ();
472  //Find the neighbhor with this label
473  VoxelData neighb_centroid_data;
474 
475  for (typename HelperListT::const_iterator neighb_itr = supervoxel_helpers_.cbegin (); neighb_itr != supervoxel_helpers_.cend (); ++neighb_itr)
476  {
477  if (neighb_itr->getLabel () == (*label_itr))
478  {
479  neighb_centroid_data = neighb_itr->getCentroid ();
480  break;
481  }
482  }
483 
484  float length = voxelDataDistance (centroid_data, neighb_centroid_data);
485  adjacency_list_arg[edge] = length;
486  }
487  }
488 
489  }
490 
491 }
492 
493 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
494 template <typename PointT> void
495 pcl::SupervoxelClustering<PointT>::getSupervoxelAdjacency (std::multimap<uint32_t, uint32_t> &label_adjacency) const
496 {
497  label_adjacency.clear ();
498  for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
499  {
500  uint32_t label = sv_itr->getLabel ();
501  std::set<uint32_t> neighbor_labels;
502  sv_itr->getNeighborLabels (neighbor_labels);
503  for (std::set<uint32_t>::iterator label_itr = neighbor_labels.begin (); label_itr != neighbor_labels.end (); ++label_itr)
504  label_adjacency.insert (std::pair<uint32_t,uint32_t> (label, *label_itr) );
505  //if (neighbor_labels.size () == 0)
506  // std::cout << label<<"(size="<<sv_itr->size () << ") has "<<neighbor_labels.size () << "\n";
507  }
508 
509 }
510 
511 
512 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
513 template <typename PointT> pcl::PointCloud<pcl::PointXYZRGBA>::Ptr
515 {
516 
517  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr colored_cloud = boost::make_shared <pcl::PointCloud<pcl::PointXYZRGBA> >();
518  pcl::copyPointCloud (*input_,*colored_cloud);
519 
521  typename pcl::PointCloud <PointT>::const_iterator i_input = input_->begin ();
522  std::vector <int> indices;
523  std::vector <float> sqr_distances;
524  for (i_colored = colored_cloud->begin (); i_colored != colored_cloud->end (); ++i_colored,++i_input)
525  {
526  if ( !pcl::isFinite<PointT> (*i_input))
527  i_colored->rgb = 0;
528  else
529  {
530  i_colored->rgb = 0;
531  LeafContainerT *leaf = adjacency_octree_->getLeafContainerAtPoint (*i_input);
532  VoxelData& voxel_data = leaf->getData ();
533  if (voxel_data.owner_)
534  i_colored->rgba = label_colors_[voxel_data.owner_->getLabel ()];
535 
536  }
537 
538  }
539 
540  return (colored_cloud);
541 }
542 
543 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
544 template <typename PointT> pcl::PointCloud<pcl::PointXYZRGBA>::Ptr
546 {
547  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr colored_cloud = boost::make_shared< pcl::PointCloud<pcl::PointXYZRGBA> > ();
548  for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
549  {
550  typename PointCloudT::Ptr voxels;
551  sv_itr->getVoxels (voxels);
553  copyPointCloud (*voxels, rgb_copy);
554 
555  pcl::PointCloud<pcl::PointXYZRGBA>::iterator rgb_copy_itr = rgb_copy.begin ();
556  for ( ; rgb_copy_itr != rgb_copy.end (); ++rgb_copy_itr)
557  rgb_copy_itr->rgba = label_colors_ [sv_itr->getLabel ()];
558 
559  *colored_cloud += rgb_copy;
560  }
561 
562  return colored_cloud;
563 }
564 
565 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
566 template <typename PointT> typename pcl::PointCloud<PointT>::Ptr
568 {
569  typename PointCloudT::Ptr centroid_copy = boost::make_shared<PointCloudT> ();
570  copyPointCloud (*voxel_centroid_cloud_, *centroid_copy);
571  return centroid_copy;
572 }
573 
574 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
575 template <typename PointT> pcl::PointCloud<pcl::PointXYZL>::Ptr
577 {
578  pcl::PointCloud<pcl::PointXYZL>::Ptr labeled_voxel_cloud = boost::make_shared< pcl::PointCloud<pcl::PointXYZL> > ();
579  for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
580  {
581  typename PointCloudT::Ptr voxels;
582  sv_itr->getVoxels (voxels);
584  copyPointCloud (*voxels, xyzl_copy);
585 
586  pcl::PointCloud<pcl::PointXYZL>::iterator xyzl_copy_itr = xyzl_copy.begin ();
587  for ( ; xyzl_copy_itr != xyzl_copy.end (); ++xyzl_copy_itr)
588  xyzl_copy_itr->label = sv_itr->getLabel ();
589 
590  *labeled_voxel_cloud += xyzl_copy;
591  }
592 
593  return labeled_voxel_cloud;
594 }
595 
596 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
597 template <typename PointT> pcl::PointCloud<pcl::PointXYZL>::Ptr
599 {
600  pcl::PointCloud<pcl::PointXYZL>::Ptr labeled_cloud = boost::make_shared <pcl::PointCloud<pcl::PointXYZL> >();
601  pcl::copyPointCloud (*input_,*labeled_cloud);
602 
604  typename pcl::PointCloud <PointT>::const_iterator i_input = input_->begin ();
605  std::vector <int> indices;
606  std::vector <float> sqr_distances;
607  for (i_labeled = labeled_cloud->begin (); i_labeled != labeled_cloud->end (); ++i_labeled,++i_input)
608  {
609  if ( !pcl::isFinite<PointT> (*i_input))
610  i_labeled->label = 0;
611  else
612  {
613  i_labeled->label = 0;
614  LeafContainerT *leaf = adjacency_octree_->getLeafContainerAtPoint (*i_input);
615  VoxelData& voxel_data = leaf->getData ();
616  if (voxel_data.owner_)
617  i_labeled->label = voxel_data.owner_->getLabel ();
618 
619  }
620 
621  }
622 
623  return (labeled_cloud);
624 }
625 
626 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
627 template <typename PointT> pcl::PointCloud<pcl::PointNormal>::Ptr
629 {
630  pcl::PointCloud<pcl::PointNormal>::Ptr normal_cloud = boost::make_shared<pcl::PointCloud<pcl::PointNormal> > ();
631  normal_cloud->resize (supervoxel_clusters.size ());
632  typename std::map <uint32_t, typename pcl::Supervoxel<PointT>::Ptr>::iterator sv_itr,sv_itr_end;
633  sv_itr = supervoxel_clusters.begin ();
634  sv_itr_end = supervoxel_clusters.end ();
635  pcl::PointCloud<pcl::PointNormal>::iterator normal_cloud_itr = normal_cloud->begin ();
636  for ( ; sv_itr != sv_itr_end; ++sv_itr, ++normal_cloud_itr)
637  {
638  (sv_itr->second)->getCentroidPointNormal (*normal_cloud_itr);
639  }
640  return normal_cloud;
641 }
642 
643 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
644 template <typename PointT> float
646 {
647  return (resolution_);
648 }
649 
650 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
651 template <typename PointT> void
653 {
654  resolution_ = resolution;
655 
656 }
657 
658 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
659 template <typename PointT> float
661 {
662  return (resolution_);
663 }
664 
665 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
666 template <typename PointT> void
668 {
669  seed_resolution_ = seed_resolution;
670 }
671 
672 
673 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
674 template <typename PointT> void
676 {
677  color_importance_ = val;
678 }
679 
680 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
681 template <typename PointT> void
683 {
684  spatial_importance_ = val;
685 }
686 
687 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
688 template <typename PointT> void
690 {
691  normal_importance_ = val;
692 }
693 
694 
695 
696 namespace pcl
697 {
698 
699  namespace octree
700  {
701  //Explicit overloads for RGB types
702  template<>
703  void
705  {
706  ++num_points_;
707  //Same as before here
708  data_.xyz_[0] += new_point.x;
709  data_.xyz_[1] += new_point.y;
710  data_.xyz_[2] += new_point.z;
711  //Separate sums for r,g,b since we cant sum in uchars
712  data_.rgb_[0] += static_cast<float> (new_point.r);
713  data_.rgb_[1] += static_cast<float> (new_point.g);
714  data_.rgb_[2] += static_cast<float> (new_point.b);
715  }
716 
717  template<>
718  void
720  {
721  ++num_points_;
722  //Same as before here
723  data_.xyz_[0] += new_point.x;
724  data_.xyz_[1] += new_point.y;
725  data_.xyz_[2] += new_point.z;
726  //Separate sums for r,g,b since we cant sum in uchars
727  data_.rgb_[0] += static_cast<float> (new_point.r);
728  data_.rgb_[1] += static_cast<float> (new_point.g);
729  data_.rgb_[2] += static_cast<float> (new_point.b);
730  }
731 
732 
733 
734  //Explicit overloads for RGB types
735  template<> void
737  {
738  data_.rgb_[0] /= (static_cast<float> (num_points_));
739  data_.rgb_[1] /= (static_cast<float> (num_points_));
740  data_.rgb_[2] /= (static_cast<float> (num_points_));
741  data_.xyz_[0] /= (static_cast<float> (num_points_));
742  data_.xyz_[1] /= (static_cast<float> (num_points_));
743  data_.xyz_[2] /= (static_cast<float> (num_points_));
744  }
745 
746  template<> void
748  {
749  data_.rgb_[0] /= (static_cast<float> (num_points_));
750  data_.rgb_[1] /= (static_cast<float> (num_points_));
751  data_.rgb_[2] /= (static_cast<float> (num_points_));
752  data_.xyz_[0] /= (static_cast<float> (num_points_));
753  data_.xyz_[1] /= (static_cast<float> (num_points_));
754  data_.xyz_[2] /= (static_cast<float> (num_points_));
755  }
756 
757  }
758 }
759 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
760 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
761 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
762 namespace pcl
763 {
764 
765  template<> void
767  {
768  point_arg.rgba = static_cast<uint32_t>(rgb_[0]) << 16 |
769  static_cast<uint32_t>(rgb_[1]) << 8 |
770  static_cast<uint32_t>(rgb_[2]);
771  point_arg.x = xyz_[0];
772  point_arg.y = xyz_[1];
773  point_arg.z = xyz_[2];
774  }
775 
776  template<> void
778  {
779  point_arg.rgba = static_cast<uint32_t>(rgb_[0]) << 16 |
780  static_cast<uint32_t>(rgb_[1]) << 8 |
781  static_cast<uint32_t>(rgb_[2]);
782  point_arg.x = xyz_[0];
783  point_arg.y = xyz_[1];
784  point_arg.z = xyz_[2];
785  }
786 
787  template<typename PointT> void
789  {
790  //XYZ is required or this doesn't make much sense...
791  point_arg.x = xyz_[0];
792  point_arg.y = xyz_[1];
793  point_arg.z = xyz_[2];
794  }
795 
796  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
797  template <typename PointT> void
799  {
800  normal_arg.normal_x = normal_[0];
801  normal_arg.normal_y = normal_[1];
802  normal_arg.normal_z = normal_[2];
803  normal_arg.curvature = curvature_;
804  }
805 }
806 
807 
808 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
809 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
810 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
811 template <typename PointT> void
813 {
814  leaves_.insert (leaf_arg);
815  VoxelData& voxel_data = leaf_arg->getData ();
816  voxel_data.owner_ = this;
817 }
818 
819 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
820 template <typename PointT> void
822 {
823  leaves_.erase (leaf_arg);
824 }
825 
826 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
827 template <typename PointT> void
829 {
830  typename std::set<LeafContainerT*>::iterator leaf_itr;
831  for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr)
832  {
833  VoxelData& voxel = ((*leaf_itr)->getData ());
834  voxel.owner_ = 0;
835  voxel.distance_ = std::numeric_limits<float>::max ();
836  }
837  leaves_.clear ();
838 }
839 
840 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
841 template <typename PointT> void
843 {
844  //std::cout << "Expanding sv "<<label_<<", owns "<<leaves_.size ()<<" voxels\n";
845  //Buffer of new neighbors - initial size is just a guess of most possible
846  std::vector<LeafContainerT*> new_owned;
847  new_owned.reserve (leaves_.size () * 9);
848  //For each leaf belonging to this supervoxel
849  typename std::set<LeafContainerT*>::iterator leaf_itr;
850  for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr)
851  {
852  //for each neighbor of the leaf
853  for (typename LeafContainerT::iterator neighb_itr=(*leaf_itr)->begin (); neighb_itr!=(*leaf_itr)->end (); ++neighb_itr)
854  {
855  //Get a reference to the data contained in the leaf
856  VoxelData& neighbor_voxel = ((*neighb_itr)->getData ());
857  //TODO this is a shortcut, really we should always recompute distance
858  if(neighbor_voxel.owner_ == this)
859  continue;
860  //Compute distance to the neighbor
861  float dist = parent_->voxelDataDistance (centroid_, neighbor_voxel);
862  //If distance is less than previous, we remove it from its owner's list
863  //and change the owner to this and distance (we *steal* it!)
864  if (dist < neighbor_voxel.distance_)
865  {
866  neighbor_voxel.distance_ = dist;
867  if (neighbor_voxel.owner_ != this)
868  {
869  if (neighbor_voxel.owner_)
870  (neighbor_voxel.owner_)->removeLeaf(*neighb_itr);
871  neighbor_voxel.owner_ = this;
872  new_owned.push_back (*neighb_itr);
873  }
874  }
875  }
876  }
877  //Push all new owned onto the owned leaf set
878  typename std::vector<LeafContainerT*>::iterator new_owned_itr;
879  for (new_owned_itr=new_owned.begin (); new_owned_itr!=new_owned.end (); ++new_owned_itr)
880  {
881  leaves_.insert (*new_owned_itr);
882  }
883 
884 }
885 
886 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
887 template <typename PointT> void
889 {
890  typename std::set<LeafContainerT*>::iterator leaf_itr;
891  //For each leaf belonging to this supervoxel, get its neighbors, build an index vector, compute normal
892  for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr)
893  {
894  VoxelData& voxel_data = (*leaf_itr)->getData ();
895  std::vector<int> indices;
896  indices.reserve (81);
897  //Push this point
898  indices.push_back (voxel_data.idx_);
899  for (typename LeafContainerT::iterator neighb_itr=(*leaf_itr)->begin (); neighb_itr!=(*leaf_itr)->end (); ++neighb_itr)
900  {
901  //Get a reference to the data contained in the leaf
902  VoxelData& neighbor_voxel_data = ((*neighb_itr)->getData ());
903  //If the neighbor is in this supervoxel, use it
904  if (neighbor_voxel_data.owner_ == this)
905  {
906  indices.push_back (neighbor_voxel_data.idx_);
907  //Also check its neighbors
908  for (typename LeafContainerT::iterator neighb_neighb_itr=(*neighb_itr)->begin (); neighb_neighb_itr!=(*neighb_itr)->end (); ++neighb_neighb_itr)
909  {
910  VoxelData& neighb_neighb_voxel_data = (*neighb_neighb_itr)->getData ();
911  if (neighb_neighb_voxel_data.owner_ == this)
912  indices.push_back (neighb_neighb_voxel_data.idx_);
913  }
914 
915 
916  }
917  }
918  //Compute normal
919  pcl::computePointNormal (*parent_->voxel_centroid_cloud_, indices, voxel_data.normal_, voxel_data.curvature_);
920  pcl::flipNormalTowardsViewpoint (parent_->voxel_centroid_cloud_->points[voxel_data.idx_], 0.0f,0.0f,0.0f, voxel_data.normal_);
921  voxel_data.normal_[3] = 0.0f;
922  voxel_data.normal_.normalize ();
923  }
924 }
925 
926 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
927 template <typename PointT> void
929 {
930  centroid_.normal_ = Eigen::Vector4f::Zero ();
931  centroid_.xyz_ = Eigen::Vector3f::Zero ();
932  centroid_.rgb_ = Eigen::Vector3f::Zero ();
933  typename std::set<LeafContainerT*>::iterator leaf_itr = leaves_.begin ();
934  if (leaves_.size () < 20) //Just a check to see if we have enough points to calculate a good normal
935  {
936  for ( ; leaf_itr!= leaves_.end (); ++leaf_itr)
937  {
938  const VoxelData& leaf_data = (*leaf_itr)->getData ();
939  centroid_.normal_ += leaf_data.normal_;
940  centroid_.xyz_ += leaf_data.xyz_;
941  centroid_.rgb_ += leaf_data.rgb_;
942  }
943  centroid_.normal_.normalize ();
944  }
945  else
946  {
947  std::vector<int> indices;
948  indices.reserve (leaves_.size ());
949  for ( ; leaf_itr!= leaves_.end (); ++leaf_itr)
950  {
951  const VoxelData& leaf_data = (*leaf_itr)->getData ();
952  centroid_.xyz_ += leaf_data.xyz_;
953  centroid_.rgb_ += leaf_data.rgb_;
954  indices.push_back (leaf_data.idx_);
955  }
956  pcl::computePointNormal (*parent_->voxel_centroid_cloud_, indices, centroid_.normal_, centroid_.curvature_);
957  PointT temp;
958  this->getXYZ (temp.x, temp.y, temp.z);
959  pcl::flipNormalTowardsViewpoint (temp, 0.0f,0.0f,0.0f, centroid_.normal_);
960  centroid_.normal_[3] = 0.0f;
961  centroid_.normal_.normalize ();
962  }
963 
964  centroid_.xyz_ /= static_cast<float> (leaves_.size ());
965  centroid_.rgb_ /= static_cast<float> (leaves_.size ());
966 
967 }
968 
969 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
970 template <typename PointT> void
972 {
973  voxels = boost::make_shared<pcl::PointCloud<PointT> > ();
974  voxels->clear ();
975  voxels->resize (leaves_.size ());
976  typename pcl::PointCloud<PointT>::iterator voxel_itr = voxels->begin ();
977  typename std::set<LeafContainerT*>::iterator leaf_itr;
978  for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr, ++voxel_itr)
979  {
980  const VoxelData& leaf_data = (*leaf_itr)->getData ();
981  leaf_data.getPoint (*voxel_itr);
982  }
983 }
984 
985 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
986 template <typename PointT> void
988 {
989  normals = boost::make_shared<pcl::PointCloud<Normal> > ();
990  normals->clear ();
991  normals->resize (leaves_.size ());
992  typename std::set<LeafContainerT*>::iterator leaf_itr;
993  typename pcl::PointCloud<Normal>::iterator normal_itr = normals->begin ();
994  for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr, ++normal_itr)
995  {
996  const VoxelData& leaf_data = (*leaf_itr)->getData ();
997  leaf_data.getNormal (*normal_itr);
998  }
999 }
1000 
1001 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
1002 template <typename PointT> void
1003 pcl::SupervoxelClustering<PointT>::SupervoxelHelper::getNeighborLabels (std::set<uint32_t> &neighbor_labels) const
1004 {
1005  neighbor_labels.clear ();
1006  //For each leaf belonging to this supervoxel
1007  typename std::set<LeafContainerT*>::iterator leaf_itr;
1008  for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr)
1009  {
1010  //for each neighbor of the leaf
1011  for (typename LeafContainerT::iterator neighb_itr=(*leaf_itr)->begin (); neighb_itr!=(*leaf_itr)->end (); ++neighb_itr)
1012  {
1013  //Get a reference to the data contained in the leaf
1014  VoxelData& neighbor_voxel = ((*neighb_itr)->getData ());
1015  //If it has an owner, and it's not us - get it's owner's label insert into set
1016  if (neighbor_voxel.owner_ != this && neighbor_voxel.owner_)
1017  {
1018  neighbor_labels.insert (neighbor_voxel.owner_->getLabel ());
1019  }
1020  }
1021  }
1022 }
1023 
1024 
1025 #endif // PCL_SUPERVOXEL_CLUSTERING_HPP_
1026