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