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