Point Cloud Library (PCL) 1.13.0
supervoxel_clustering.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 *
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of Willow Garage, Inc. nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *
35 * Author : jpapon@gmail.com
36 * Email : jpapon@gmail.com
37 *
38 */
39
40#ifndef PCL_SEGMENTATION_SUPERVOXEL_CLUSTERING_HPP_
41#define PCL_SEGMENTATION_SUPERVOXEL_CLUSTERING_HPP_
42
43#include <pcl/segmentation/supervoxel_clustering.h>
44#include <pcl/common/io.h> // for copyPointCloud
45
46//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
47template <typename PointT>
48pcl::SupervoxelClustering<PointT>::SupervoxelClustering (float voxel_resolution, float seed_resolution) :
49 resolution_ (voxel_resolution),
50 seed_resolution_ (seed_resolution),
51 adjacency_octree_ (),
52 voxel_centroid_cloud_ (),
53 color_importance_ (0.1f),
54 spatial_importance_ (0.4f),
55 normal_importance_ (1.0f),
56 use_default_transform_behaviour_ (true)
57{
58 adjacency_octree_.reset (new OctreeAdjacencyT (resolution_));
59}
60
61//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
62template <typename PointT>
64
65//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
66template <typename PointT> void
68{
69 if ( cloud->empty () )
70 {
71 PCL_ERROR ("[pcl::SupervoxelClustering::setInputCloud] Empty cloud set, doing nothing \n");
72 return;
73 }
74
75 input_ = cloud;
76 adjacency_octree_->setInputCloud (cloud);
77}
78
79//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
80template <typename PointT> void
82{
83 if ( normal_cloud->empty () )
84 {
85 PCL_ERROR ("[pcl::SupervoxelClustering::setNormalCloud] Empty cloud set, doing nothing \n");
86 return;
87 }
88
89 input_normals_ = normal_cloud;
90}
91
92//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
93template <typename PointT> void
94pcl::SupervoxelClustering<PointT>::extract (std::map<std::uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters)
95{
96 //timer_.reset ();
97 //double t_start = timer_.getTime ();
98 //std::cout << "Init compute \n";
99 bool segmentation_is_possible = initCompute ();
100 if ( !segmentation_is_possible )
101 {
102 deinitCompute ();
103 return;
104 }
105
106 //std::cout << "Preparing for segmentation \n";
107 segmentation_is_possible = prepareForSegmentation ();
108 if ( !segmentation_is_possible )
109 {
110 deinitCompute ();
111 return;
112 }
113
114 //double t_prep = timer_.getTime ();
115 //std::cout << "Placing Seeds" << std::endl;
116 Indices seed_indices;
117 selectInitialSupervoxelSeeds (seed_indices);
118 //std::cout << "Creating helpers "<<std::endl;
119 createSupervoxelHelpers (seed_indices);
120 //double t_seeds = timer_.getTime ();
121
122
123 //std::cout << "Expanding the supervoxels" << std::endl;
124 int max_depth = static_cast<int> (1.8f*seed_resolution_/resolution_);
125 expandSupervoxels (max_depth);
126 //double t_iterate = timer_.getTime ();
127
128 //std::cout << "Making Supervoxel structures" << std::endl;
129 makeSupervoxels (supervoxel_clusters);
130 //double t_supervoxels = timer_.getTime ();
131
132 // std::cout << "--------------------------------- Timing Report --------------------------------- \n";
133 // std::cout << "Time to prep (normals, neighbors, voxelization)="<<t_prep-t_start<<" ms\n";
134 // std::cout << "Time to seed clusters ="<<t_seeds-t_prep<<" ms\n";
135 // std::cout << "Time to expand clusters ="<<t_iterate-t_seeds<<" ms\n";
136 // std::cout << "Time to create supervoxel structures ="<<t_supervoxels-t_iterate<<" ms\n";
137 // std::cout << "Total run time ="<<t_supervoxels-t_start<<" ms\n";
138 // std::cout << "--------------------------------------------------------------------------------- \n";
139
140 deinitCompute ();
141}
142
143
144//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
145template <typename PointT> void
146pcl::SupervoxelClustering<PointT>::refineSupervoxels (int num_itr, std::map<std::uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters)
147{
148 if (supervoxel_helpers_.empty ())
149 {
150 PCL_ERROR ("[pcl::SupervoxelClustering::refineVoxelNormals] Supervoxels not extracted, doing nothing - (Call extract first!) \n");
151 return;
152 }
153
154 int max_depth = static_cast<int> (1.8f*seed_resolution_/resolution_);
155 for (int i = 0; i < num_itr; ++i)
156 {
157 for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr)
158 {
159 sv_itr->refineNormals ();
160 }
161
162 reseedSupervoxels ();
163 expandSupervoxels (max_depth);
164 }
165
166
167 makeSupervoxels (supervoxel_clusters);
168
169}
170//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
171//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
172//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
173//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
174//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
175
176
177template <typename PointT> bool
179{
180
181 // if user forgot to pass point cloud or if it is empty
182 if ( input_->points.empty () )
183 return (false);
184
185 //Add the new cloud of data to the octree
186 //std::cout << "Populating adjacency octree with new cloud \n";
187 //double prep_start = timer_.getTime ();
188 if ( (use_default_transform_behaviour_ && input_->isOrganized ())
189 || (!use_default_transform_behaviour_ && use_single_camera_transform_))
190 adjacency_octree_->setTransformFunction ([this] (PointT &p) { transformFunction (p); });
191
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
205template <typename PointT> void
207{
208 voxel_centroid_cloud_.reset (new PointCloudT);
209 voxel_centroid_cloud_->resize (adjacency_octree_->getLeafCount ());
210 auto leaf_itr = adjacency_octree_->begin ();
211 auto 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 auto normal_itr = input_normals_->begin ();
228 for (auto 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_ = nullptr;
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 Indices indices;
261 indices.reserve (81);
262 //Push this point
263 indices.push_back (new_voxel_data.idx_);
264 for (auto neighb_itr=(*leaf_itr)->cbegin (); neighb_itr!=(*leaf_itr)->cend (); ++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 (auto neighb_neighb_itr=(*neighb_itr)->cbegin (); neighb_neighb_itr!=(*neighb_itr)->cend (); ++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_)[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_ = nullptr;
282 new_voxel_data.distance_ = std::numeric_limits<float>::max ();
283 }
284 }
285
286
287}
288
289//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
290template <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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
322template <typename PointT> void
323pcl::SupervoxelClustering<PointT>::makeSupervoxels (std::map<std::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 std::uint32_t label = sv_itr->getLabel ();
329 supervoxel_clusters[label].reset (new 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}
337
338
339//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
340template <typename PointT> void
342{
343
344 supervoxel_helpers_.clear ();
345 for (std::size_t i = 0; i < seed_indices.size (); ++i)
346 {
347 supervoxel_helpers_.push_back (new SupervoxelHelper(i+1,this));
348 //Find which leaf corresponds to this seed index
349 LeafContainerT* seed_leaf = adjacency_octree_->at(seed_indices[i]);//adjacency_octree_->getLeafContainerAtPoint (seed_points[i]);
350 if (seed_leaf)
351 {
352 supervoxel_helpers_.back ().addLeaf (seed_leaf);
353 }
354 else
355 {
356 PCL_WARN ("Could not find leaf in pcl::SupervoxelClustering<PointT>::createSupervoxelHelpers - supervoxel will be deleted \n");
357 }
358 }
359
360}
361//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
362template <typename PointT> void
364{
365 //TODO THIS IS BAD - SEEDING SHOULD BE BETTER
366 //TODO Switch to assigning leaves! Don't use Octree!
367
368 // std::cout << "Size of centroid cloud="<<voxel_centroid_cloud_->size ()<<", seeding resolution="<<seed_resolution_<<"\n";
369 //Initialize octree with voxel centroids
370 pcl::octree::OctreePointCloudSearch <PointT> seed_octree (seed_resolution_);
371 seed_octree.setInputCloud (voxel_centroid_cloud_);
372 seed_octree.addPointsFromInputCloud ();
373 // std::cout << "Size of octree ="<<seed_octree.getLeafCount ()<<"\n";
374 std::vector<PointT, Eigen::aligned_allocator<PointT> > voxel_centers;
375 int num_seeds = seed_octree.getOccupiedVoxelCenters(voxel_centers);
376 //std::cout << "Number of seed points before filtering="<<voxel_centers.size ()<<std::endl;
377
378 std::vector<int> seed_indices_orig;
379 seed_indices_orig.resize (num_seeds, 0);
380 seed_indices.clear ();
381 pcl::Indices closest_index;
382 std::vector<float> distance;
383 closest_index.resize(1,0);
384 distance.resize(1,0);
385 if (!voxel_kdtree_)
386 {
387 voxel_kdtree_.reset (new pcl::search::KdTree<PointT>);
388 voxel_kdtree_ ->setInputCloud (voxel_centroid_cloud_);
389 }
390
391 for (int i = 0; i < num_seeds; ++i)
392 {
393 voxel_kdtree_->nearestKSearch (voxel_centers[i], 1, closest_index, distance);
394 seed_indices_orig[i] = closest_index[0];
395 }
396
397 pcl::Indices neighbors;
398 std::vector<float> sqr_distances;
399 seed_indices.reserve (seed_indices_orig.size ());
400 float search_radius = 0.5f*seed_resolution_;
401 // This is 1/20th of the number of voxels which fit in a planar slice through search volume
402 // Area of planar slice / area of voxel side. (Note: This is smaller than the value mentioned in the original paper)
403 float min_points = 0.05f * (search_radius)*(search_radius) * 3.1415926536f / (resolution_*resolution_);
404 for (const auto &index_orig : seed_indices_orig)
405 {
406 int num = voxel_kdtree_->radiusSearch (index_orig, search_radius , neighbors, sqr_distances);
407 if ( num > min_points)
408 {
409 seed_indices.push_back (index_orig);
410 }
411
412 }
413 // std::cout << "Number of seed points after filtering="<<seed_points.size ()<<std::endl;
414
415}
416
417
418//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
419template <typename PointT> void
421{
422 //Go through each supervoxel and remove all it's leaves
423 for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr)
424 {
425 sv_itr->removeAllLeaves ();
426 }
427
428 Indices closest_index;
429 std::vector<float> distance;
430 //Now go through each supervoxel, find voxel closest to its center, add it in
431 for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr)
432 {
433 PointT point;
434 sv_itr->getXYZ (point.x, point.y, point.z);
435 voxel_kdtree_->nearestKSearch (point, 1, closest_index, distance);
436
437 LeafContainerT* seed_leaf = adjacency_octree_->at (closest_index[0]);
438 if (seed_leaf)
439 {
440 sv_itr->addLeaf (seed_leaf);
441 }
442 else
443 {
444 PCL_WARN ("Could not find leaf in pcl::SupervoxelClustering<PointT>::reseedSupervoxels - supervoxel will be deleted \n");
445 }
446 }
447
448}
449
450//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
451template <typename PointT> void
453{
454 p.x /= p.z;
455 p.y /= p.z;
456 p.z = std::log (p.z);
457}
458
459//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
460template <typename PointT> float
461pcl::SupervoxelClustering<PointT>::voxelDataDistance (const VoxelData &v1, const VoxelData &v2) const
462{
463
464 float spatial_dist = (v1.xyz_ - v2.xyz_).norm () / seed_resolution_;
465 float color_dist = (v1.rgb_ - v2.rgb_).norm () / 255.0f;
466 float cos_angle_normal = 1.0f - std::abs (v1.normal_.dot (v2.normal_));
467 // std::cout << "s="<<spatial_dist<<" c="<<color_dist<<" an="<<cos_angle_normal<<"\n";
468 return cos_angle_normal * normal_importance_ + color_dist * color_importance_+ spatial_dist * spatial_importance_;
469
470}
471
472
473//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
474///////// GETTER FUNCTIONS
475//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
476//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
477//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
478//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
479template <typename PointT> void
481{
482 adjacency_list_arg.clear ();
483 //Add a vertex for each label, store ids in map
484 std::map <std::uint32_t, VoxelID> label_ID_map;
485 for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
486 {
487 VoxelID node_id = add_vertex (adjacency_list_arg);
488 adjacency_list_arg[node_id] = (sv_itr->getLabel ());
489 label_ID_map.insert (std::make_pair (sv_itr->getLabel (), node_id));
490 }
491
492 for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
493 {
494 std::uint32_t label = sv_itr->getLabel ();
495 std::set<std::uint32_t> neighbor_labels;
496 sv_itr->getNeighborLabels (neighbor_labels);
497 for (const unsigned int &neighbor_label : neighbor_labels)
498 {
499 bool edge_added;
500 EdgeID edge;
501 VoxelID u = (label_ID_map.find (label))->second;
502 VoxelID v = (label_ID_map.find (neighbor_label))->second;
503 boost::tie (edge, edge_added) = add_edge (u,v,adjacency_list_arg);
504 //Calc distance between centers, set as edge weight
505 if (edge_added)
506 {
507 VoxelData centroid_data = (sv_itr)->getCentroid ();
508 //Find the neighbhor with this label
509 VoxelData neighb_centroid_data;
510
511 for (typename HelperListT::const_iterator neighb_itr = supervoxel_helpers_.cbegin (); neighb_itr != supervoxel_helpers_.cend (); ++neighb_itr)
512 {
513 if (neighb_itr->getLabel () == neighbor_label)
514 {
515 neighb_centroid_data = neighb_itr->getCentroid ();
516 break;
517 }
518 }
519
520 float length = voxelDataDistance (centroid_data, neighb_centroid_data);
521 adjacency_list_arg[edge] = length;
522 }
523 }
524
525 }
526
527}
528
529//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
530template <typename PointT> void
531pcl::SupervoxelClustering<PointT>::getSupervoxelAdjacency (std::multimap<std::uint32_t, std::uint32_t> &label_adjacency) const
532{
533 label_adjacency.clear ();
534 for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
535 {
536 std::uint32_t label = sv_itr->getLabel ();
537 std::set<std::uint32_t> neighbor_labels;
538 sv_itr->getNeighborLabels (neighbor_labels);
539 for (const unsigned int &neighbor_label : neighbor_labels)
540 label_adjacency.insert (std::pair<std::uint32_t,std::uint32_t> (label, neighbor_label) );
541 //if (neighbor_labels.size () == 0)
542 // std::cout << label<<"(size="<<sv_itr->size () << ") has "<<neighbor_labels.size () << "\n";
543 }
544}
545
546//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
547template <typename PointT> typename pcl::PointCloud<PointT>::Ptr
549{
550 typename PointCloudT::Ptr centroid_copy (new PointCloudT);
551 copyPointCloud (*voxel_centroid_cloud_, *centroid_copy);
552 return centroid_copy;
553}
554
555//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
556template <typename PointT> pcl::PointCloud<pcl::PointXYZL>::Ptr
558{
560 for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
561 {
562 typename PointCloudT::Ptr voxels;
563 sv_itr->getVoxels (voxels);
565 copyPointCloud (*voxels, xyzl_copy);
566
567 auto xyzl_copy_itr = xyzl_copy.begin ();
568 for ( ; xyzl_copy_itr != xyzl_copy.end (); ++xyzl_copy_itr)
569 xyzl_copy_itr->label = sv_itr->getLabel ();
570
571 *labeled_voxel_cloud += xyzl_copy;
572 }
573
574 return labeled_voxel_cloud;
575}
576
577//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
578template <typename PointT> pcl::PointCloud<pcl::PointXYZL>::Ptr
580{
582 pcl::copyPointCloud (*input_,*labeled_cloud);
583
584 auto i_input = input_->begin ();
585 for (auto i_labeled = labeled_cloud->begin (); i_labeled != labeled_cloud->end (); ++i_labeled,++i_input)
586 {
587 if ( !pcl::isFinite<PointT> (*i_input))
588 i_labeled->label = 0;
589 else
590 {
591 i_labeled->label = 0;
592 LeafContainerT *leaf = adjacency_octree_->getLeafContainerAtPoint (*i_input);
593 VoxelData& voxel_data = leaf->getData ();
594 if (voxel_data.owner_)
595 i_labeled->label = voxel_data.owner_->getLabel ();
596
597 }
598
599 }
600
601 return (labeled_cloud);
602}
603
604//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
605template <typename PointT> pcl::PointCloud<pcl::PointNormal>::Ptr
606pcl::SupervoxelClustering<PointT>::makeSupervoxelNormalCloud (std::map<std::uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters)
607{
609 normal_cloud->resize (supervoxel_clusters.size ());
610 auto normal_cloud_itr = normal_cloud->begin ();
611 for (auto sv_itr = supervoxel_clusters.cbegin (), sv_itr_end = supervoxel_clusters.cend ();
612 sv_itr != sv_itr_end; ++sv_itr, ++normal_cloud_itr)
613 {
614 (sv_itr->second)->getCentroidPointNormal (*normal_cloud_itr);
615 }
616 return normal_cloud;
617}
618
619//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
620template <typename PointT> float
622{
623 return (resolution_);
624}
625
626//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
627template <typename PointT> void
629{
630 resolution_ = resolution;
631
632}
633
634//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
635template <typename PointT> float
637{
638 return (seed_resolution_);
639}
640
641//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
642template <typename PointT> void
644{
645 seed_resolution_ = seed_resolution;
646}
647
648
649//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
650template <typename PointT> void
652{
653 color_importance_ = val;
654}
655
656//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
657template <typename PointT> void
659{
660 spatial_importance_ = val;
661}
662
663//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
664template <typename PointT> void
666{
667 normal_importance_ = val;
668}
669
670//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
671template <typename PointT> void
673{
674 use_default_transform_behaviour_ = false;
675 use_single_camera_transform_ = val;
676}
677
678//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
679template <typename PointT> int
681{
682 int max_label = 0;
683 for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
684 {
685 int temp = sv_itr->getLabel ();
686 if (temp > max_label)
687 max_label = temp;
688 }
689 return max_label;
690}
691
692namespace pcl
693{
694 namespace octree
695 {
696 //Explicit overloads for RGB types
697 template<>
698 void
700
701 template<>
702 void
704
705 //Explicit overloads for RGB types
706 template<> void
708
709 template<> void
711
712 //Explicit overloads for XYZ types
713 template<>
714 void
716
717 template<> void
719 }
720}
721
722//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
723//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
724//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
725namespace pcl
726{
727
728 template<> void
730
731 template<> void
733
734 template<typename PointT> void
736 {
737 //XYZ is required or this doesn't make much sense...
738 point_arg.x = xyz_[0];
739 point_arg.y = xyz_[1];
740 point_arg.z = xyz_[2];
741 }
742
743 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
744 template <typename PointT> void
746 {
747 normal_arg.normal_x = normal_[0];
748 normal_arg.normal_y = normal_[1];
749 normal_arg.normal_z = normal_[2];
750 normal_arg.curvature = curvature_;
751 }
752}
753
754//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
755//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
756//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
757template <typename PointT> void
759{
760 leaves_.insert (leaf_arg);
761 VoxelData& voxel_data = leaf_arg->getData ();
762 voxel_data.owner_ = this;
763}
764
765//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
766template <typename PointT> void
768{
769 leaves_.erase (leaf_arg);
770}
771
772//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
773template <typename PointT> void
775{
776 for (auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr)
777 {
778 VoxelData& voxel = ((*leaf_itr)->getData ());
779 voxel.owner_ = nullptr;
780 voxel.distance_ = std::numeric_limits<float>::max ();
781 }
782 leaves_.clear ();
783}
784
785//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
786template <typename PointT> void
788{
789 //std::cout << "Expanding sv "<<label_<<", owns "<<leaves_.size ()<<" voxels\n";
790 //Buffer of new neighbors - initial size is just a guess of most possible
791 std::vector<LeafContainerT*> new_owned;
792 new_owned.reserve (leaves_.size () * 9);
793 //For each leaf belonging to this supervoxel
794 for (auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr)
795 {
796 //for each neighbor of the leaf
797 for (auto neighb_itr=(*leaf_itr)->cbegin (); neighb_itr!=(*leaf_itr)->cend (); ++neighb_itr)
798 {
799 //Get a reference to the data contained in the leaf
800 VoxelData& neighbor_voxel = ((*neighb_itr)->getData ());
801 //TODO this is a shortcut, really we should always recompute distance
802 if(neighbor_voxel.owner_ == this)
803 continue;
804 //Compute distance to the neighbor
805 float dist = parent_->voxelDataDistance (centroid_, neighbor_voxel);
806 //If distance is less than previous, we remove it from its owner's list
807 //and change the owner to this and distance (we *steal* it!)
808 if (dist < neighbor_voxel.distance_)
809 {
810 neighbor_voxel.distance_ = dist;
811 if (neighbor_voxel.owner_ != this)
812 {
813 if (neighbor_voxel.owner_)
814 (neighbor_voxel.owner_)->removeLeaf(*neighb_itr);
815 neighbor_voxel.owner_ = this;
816 new_owned.push_back (*neighb_itr);
817 }
818 }
819 }
820 }
821 //Push all new owned onto the owned leaf set
822 for (auto new_owned_itr = new_owned.cbegin (); new_owned_itr != new_owned.cend (); ++new_owned_itr)
823 {
824 leaves_.insert (*new_owned_itr);
825 }
826}
827
828//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
829template <typename PointT> void
831{
832 //For each leaf belonging to this supervoxel, get its neighbors, build an index vector, compute normal
833 for (auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr)
834 {
835 VoxelData& voxel_data = (*leaf_itr)->getData ();
836 Indices indices;
837 indices.reserve (81);
838 //Push this point
839 indices.push_back (voxel_data.idx_);
840 for (auto neighb_itr=(*leaf_itr)->cbegin (); neighb_itr!=(*leaf_itr)->cend (); ++neighb_itr)
841 {
842 //Get a reference to the data contained in the leaf
843 VoxelData& neighbor_voxel_data = ((*neighb_itr)->getData ());
844 //If the neighbor is in this supervoxel, use it
845 if (neighbor_voxel_data.owner_ == this)
846 {
847 indices.push_back (neighbor_voxel_data.idx_);
848 //Also check its neighbors
849 for (auto neighb_neighb_itr=(*neighb_itr)->cbegin (); neighb_neighb_itr!=(*neighb_itr)->cend (); ++neighb_neighb_itr)
850 {
851 VoxelData& neighb_neighb_voxel_data = (*neighb_neighb_itr)->getData ();
852 if (neighb_neighb_voxel_data.owner_ == this)
853 indices.push_back (neighb_neighb_voxel_data.idx_);
854 }
855
856
857 }
858 }
859 //Compute normal
860 pcl::computePointNormal (*parent_->voxel_centroid_cloud_, indices, voxel_data.normal_, voxel_data.curvature_);
861 pcl::flipNormalTowardsViewpoint ((*parent_->voxel_centroid_cloud_)[voxel_data.idx_], 0.0f,0.0f,0.0f, voxel_data.normal_);
862 voxel_data.normal_[3] = 0.0f;
863 voxel_data.normal_.normalize ();
864 }
865}
866
867//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
868template <typename PointT> void
870{
871 centroid_.normal_ = Eigen::Vector4f::Zero ();
872 centroid_.xyz_ = Eigen::Vector3f::Zero ();
873 centroid_.rgb_ = Eigen::Vector3f::Zero ();
874 for (auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr)
875 {
876 const VoxelData& leaf_data = (*leaf_itr)->getData ();
877 centroid_.normal_ += leaf_data.normal_;
878 centroid_.xyz_ += leaf_data.xyz_;
879 centroid_.rgb_ += leaf_data.rgb_;
880 }
881 centroid_.normal_.normalize ();
882 centroid_.xyz_ /= static_cast<float> (leaves_.size ());
883 centroid_.rgb_ /= static_cast<float> (leaves_.size ());
884}
885
886//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
887template <typename PointT> void
889{
890 voxels.reset (new pcl::PointCloud<PointT>);
891 voxels->clear ();
892 voxels->resize (leaves_.size ());
893 auto voxel_itr = voxels->begin ();
894 for (auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr, ++voxel_itr)
895 {
896 const VoxelData& leaf_data = (*leaf_itr)->getData ();
897 leaf_data.getPoint (*voxel_itr);
898 }
899}
900
901//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
902template <typename PointT> void
904{
905 normals.reset (new pcl::PointCloud<Normal>);
906 normals->clear ();
907 normals->resize (leaves_.size ());
908 auto normal_itr = normals->begin ();
909 for (auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr, ++normal_itr)
910 {
911 const VoxelData& leaf_data = (*leaf_itr)->getData ();
912 leaf_data.getNormal (*normal_itr);
913 }
914}
915
916//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
917template <typename PointT> void
918pcl::SupervoxelClustering<PointT>::SupervoxelHelper::getNeighborLabels (std::set<std::uint32_t> &neighbor_labels) const
919{
920 neighbor_labels.clear ();
921 //For each leaf belonging to this supervoxel
922 for (auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr)
923 {
924 //for each neighbor of the leaf
925 for (auto neighb_itr=(*leaf_itr)->cbegin (); neighb_itr!=(*leaf_itr)->cend (); ++neighb_itr)
926 {
927 //Get a reference to the data contained in the leaf
928 VoxelData& neighbor_voxel = ((*neighb_itr)->getData ());
929 //If it has an owner, and it's not us - get it's owner's label insert into set
930 if (neighbor_voxel.owner_ != this && neighbor_voxel.owner_)
931 {
932 neighbor_labels.insert (neighbor_voxel.owner_->getLabel ());
933 }
934 }
935 }
936}
937
938
939#endif // PCL_SUPERVOXEL_CLUSTERING_HPP_
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
bool empty() const
Definition: point_cloud.h:446
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition: point_cloud.h:462
iterator end() noexcept
Definition: point_cloud.h:430
void clear()
Removes all points in a cloud and sets the width and height to 0.
Definition: point_cloud.h:885
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
iterator begin() noexcept
Definition: point_cloud.h:429
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:414
VoxelData is a structure used for storing data within a pcl::octree::OctreePointCloudAdjacencyContain...
void getPoint(PointT &point_arg) const
Gets the data of in the form of a point.
void getNormal(Normal &normal_arg) const
Gets the data of in the form of a normal.
Implements a supervoxel algorithm based on voxel structure, normals, and rgb values.
static pcl::PointCloud< pcl::PointNormal >::Ptr makeSupervoxelNormalCloud(std::map< std::uint32_t, typename Supervoxel< PointT >::Ptr > &supervoxel_clusters)
Static helper function which returns a pointcloud of normals for the input supervoxels.
VoxelAdjacencyList::edge_descriptor EdgeID
int getMaxLabel() const
Returns the current maximum (highest) label.
virtual void refineSupervoxels(int num_itr, std::map< std::uint32_t, typename Supervoxel< PointT >::Ptr > &supervoxel_clusters)
This method refines the calculated supervoxels - may only be called after extract.
void setSpatialImportance(float val)
Set the importance of spatial distance for supervoxels.
float getVoxelResolution() const
Get the resolution of the octree voxels.
virtual void setNormalCloud(typename NormalCloudT::ConstPtr normal_cloud)
This method sets the normals to be used for supervoxels (should be same size as input cloud)
void setColorImportance(float val)
Set the importance of color for supervoxels.
pcl::PointCloud< pcl::PointXYZL >::Ptr getLabeledVoxelCloud() const
Returns labeled voxelized cloud Points that belong to the same supervoxel have the same label.
void setVoxelResolution(float resolution)
Set the resolution of the octree voxels.
void setInputCloud(const typename pcl::PointCloud< PointT >::ConstPtr &cloud) override
This method sets the cloud to be supervoxelized.
virtual void extract(std::map< std::uint32_t, typename Supervoxel< PointT >::Ptr > &supervoxel_clusters)
This method launches the segmentation algorithm and returns the supervoxels that were obtained during...
boost::adjacency_list< boost::setS, boost::setS, boost::undirectedS, std::uint32_t, float > VoxelAdjacencyList
pcl::PointCloud< PointT >::Ptr getVoxelCentroidCloud() const
Returns a deep copy of the voxel centroid cloud.
void setSeedResolution(float seed_resolution)
Set the resolution of the octree seed voxels.
void getSupervoxelAdjacency(std::multimap< std::uint32_t, std::uint32_t > &label_adjacency) const
Get a multimap which gives supervoxel adjacency.
void setUseSingleCameraTransform(bool val)
Set whether or not to use the single camera transform.
SupervoxelClustering(float voxel_resolution, float seed_resolution)
Constructor that sets default values for member variables.
void getSupervoxelAdjacencyList(VoxelAdjacencyList &adjacency_list_arg) const
Gets the adjacency list (Boost Graph library) which gives connections between supervoxels.
VoxelAdjacencyList::vertex_descriptor VoxelID
~SupervoxelClustering() override
This destructor destroys the cloud, normals and search method used for finding neighbors.
float getSeedResolution() const
Get the resolution of the octree seed voxels.
void setNormalImportance(float val)
Set the importance of scalar normal product for supervoxels.
pcl::PointCloud< PointXYZL >::Ptr getLabeledCloud() const
Returns labeled cloud Points that belong to the same supervoxel have the same label.
shared_ptr< Supervoxel< PointT > > Ptr
Octree adjacency leaf container class- stores a list of pointers to neighbors, number of points added...
DataT & getData()
Returns a reference to the data member to access it without copying.
Octree pointcloud voxel class which maintains adjacency information for its voxels.
Octree pointcloud search class
Definition: octree_search.h:58
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition: kdtree.h:62
void copyPointCloud(const pcl::PointCloud< PointInT > &cloud_in, pcl::PointCloud< PointOutT > &cloud_out)
Copy all the fields from a given point cloud into a new point cloud.
Definition: io.hpp:142
void flipNormalTowardsViewpoint(const PointT &point, float vp_x, float vp_y, float vp_z, Eigen::Matrix< Scalar, 4, 1 > &normal)
Flip (in place) the estimated normal of a point towards a given viewpoint.
Definition: normal_3d.h:122
bool computePointNormal(const pcl::PointCloud< PointT > &cloud, Eigen::Vector4f &plane_parameters, float &curvature)
Compute the Least-Squares plane fit for a given set of points, and return the estimated plane paramet...
Definition: normal_3d.h:61
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
A point structure representing normal coordinates and the surface curvature estimate.
A point structure representing Euclidean xyz coordinates.
A point structure representing Euclidean xyz coordinates, and the RGBA color.
A point structure representing Euclidean xyz coordinates, and the RGB color.