Point Cloud Library (PCL)  1.9.1-dev
supervoxel_clustering.h
1 
2 /*
3  * Software License Agreement (BSD License)
4  *
5  * Point Cloud Library (PCL) - www.pointclouds.org
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage, Inc. nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * Author : jpapon@gmail.com
37  * Email : jpapon@gmail.com
38  *
39  */
40 
41 #pragma once
42 
43 #include <boost/version.hpp>
44 
45 #include <pcl/features/normal_3d.h>
46 #include <pcl/pcl_base.h>
47 #include <pcl/pcl_macros.h>
48 #include <pcl/point_cloud.h>
49 #include <pcl/point_types.h>
50 #include <pcl/octree/octree_search.h>
51 #include <pcl/octree/octree_pointcloud_adjacency.h>
52 #include <pcl/search/search.h>
53 #include <pcl/segmentation/boost.h>
54 
55 
56 
57 //DEBUG TODO REMOVE
58 #include <pcl/common/time.h>
59 
60 
61 namespace pcl
62 {
63  /** \brief Supervoxel container class - stores a cluster extracted using supervoxel clustering
64  */
65  template <typename PointT>
66  class Supervoxel
67  {
68  public:
70  voxels_ (new pcl::PointCloud<PointT> ()),
71  normals_ (new pcl::PointCloud<Normal> ())
72  { }
73 
74  using Ptr = boost::shared_ptr<Supervoxel<PointT> >;
75  using ConstPtr = boost::shared_ptr<const Supervoxel<PointT> >;
76 
77  /** \brief Gets the centroid of the supervoxel
78  * \param[out] centroid_arg centroid of the supervoxel
79  */
80  void
82  {
83  centroid_arg = centroid_;
84  }
85 
86  /** \brief Gets the point normal for the supervoxel
87  * \param[out] normal_arg Point normal of the supervoxel
88  * \note This isn't an average, it is a normal computed using all of the voxels in the supervoxel as support
89  */
90  void
92  {
93  normal_arg.x = centroid_.x;
94  normal_arg.y = centroid_.y;
95  normal_arg.z = centroid_.z;
96  normal_arg.normal_x = normal_.normal_x;
97  normal_arg.normal_y = normal_.normal_y;
98  normal_arg.normal_z = normal_.normal_z;
99  normal_arg.curvature = normal_.curvature;
100  }
101 
102  /** \brief The normal calculated for the voxels contained in the supervoxel */
104  /** \brief The centroid of the supervoxel - average voxel */
106  /** \brief A Pointcloud of the voxels in the supervoxel */
108  /** \brief A Pointcloud of the normals for the points in the supervoxel */
110 
111  public:
113  };
114 
115  /** \brief Implements a supervoxel algorithm based on voxel structure, normals, and rgb values
116  * \note Supervoxels are oversegmented volumetric patches (usually surfaces)
117  * \note Usually, color isn't needed (and can be detrimental)- spatial structure is mainly used
118  * - J. Papon, A. Abramov, M. Schoeler, F. Woergoetter
119  * Voxel Cloud Connectivity Segmentation - Supervoxels from PointClouds
120  * In Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition (CVPR) 2013
121  * \ingroup segmentation
122  * \author Jeremie Papon (jpapon@gmail.com)
123  */
124  template <typename PointT>
125  class PCL_EXPORTS SupervoxelClustering : public pcl::PCLBase<PointT>
126  {
127  //Forward declaration of friended helper class
128  class SupervoxelHelper;
129  friend class SupervoxelHelper;
130  public:
131  /** \brief VoxelData is a structure used for storing data within a pcl::octree::OctreePointCloudAdjacencyContainer
132  * \note It stores xyz, rgb, normal, distance, an index, and an owner.
133  */
134  class VoxelData
135  {
136  public:
138  xyz_ (0.0f, 0.0f, 0.0f),
139  rgb_ (0.0f, 0.0f, 0.0f),
140  normal_ (0.0f, 0.0f, 0.0f, 0.0f),
141  curvature_ (0.0f),
142  owner_ (nullptr)
143  {}
144 
145  /** \brief Gets the data of in the form of a point
146  * \param[out] point_arg Will contain the point value of the voxeldata
147  */
148  void
149  getPoint (PointT &point_arg) const;
150 
151  /** \brief Gets the data of in the form of a normal
152  * \param[out] normal_arg Will contain the normal value of the voxeldata
153  */
154  void
155  getNormal (Normal &normal_arg) const;
156 
157  Eigen::Vector3f xyz_;
158  Eigen::Vector3f rgb_;
159  Eigen::Vector4f normal_;
160  float curvature_;
161  float distance_;
162  int idx_;
163  SupervoxelHelper* owner_;
164 
165  public:
167  };
168 
170  using LeafVectorT = std::vector<LeafContainerT *>;
171 
177  using IndicesPtr = boost::shared_ptr<std::vector<int> >;
178 
182 
183  using VoxelAdjacencyList = boost::adjacency_list<boost::setS, boost::setS, boost::undirectedS, uint32_t, float>;
184  using VoxelID = VoxelAdjacencyList::vertex_descriptor;
185  using EdgeID = VoxelAdjacencyList::edge_descriptor;
186 
187  public:
188 
189  /** \brief Constructor that sets default values for member variables.
190  * \param[in] voxel_resolution The resolution (in meters) of voxels used
191  * \param[in] seed_resolution The average size (in meters) of resulting supervoxels
192  */
193  SupervoxelClustering (float voxel_resolution, float seed_resolution);
194 
195  [[deprecated("constructor with flag for using the single camera transform is deprecated. Default behavior is now to use the transform for organized clouds, and not use it for unorganized. Use setUseSingleCameraTransform() to override the defaults.")]]
196  SupervoxelClustering (float voxel_resolution, float seed_resolution, bool) : SupervoxelClustering (voxel_resolution, seed_resolution) { }
197 
198  /** \brief This destructor destroys the cloud, normals and search method used for
199  * finding neighbors. In other words it frees memory.
200  */
201 
203 
204  /** \brief Set the resolution of the octree voxels */
205  void
206  setVoxelResolution (float resolution);
207 
208  /** \brief Get the resolution of the octree voxels */
209  float
210  getVoxelResolution () const;
211 
212  /** \brief Set the resolution of the octree seed voxels */
213  void
214  setSeedResolution (float seed_resolution);
215 
216  /** \brief Get the resolution of the octree seed voxels */
217  float
218  getSeedResolution () const;
219 
220  /** \brief Set the importance of color for supervoxels */
221  void
222  setColorImportance (float val);
223 
224  /** \brief Set the importance of spatial distance for supervoxels */
225  void
226  setSpatialImportance (float val);
227 
228  /** \brief Set the importance of scalar normal product for supervoxels */
229  void
230  setNormalImportance (float val);
231 
232  /** \brief Set whether or not to use the single camera transform
233  * \note By default it will be used for organized clouds, but not for unorganized - this parameter will override that behavior
234  * The single camera transform scales bin size so that it increases exponentially with depth (z dimension).
235  * This is done to account for the decreasing point density found with depth when using an RGB-D camera.
236  * Without the transform, beyond a certain depth adjacency of voxels breaks down unless the voxel size is set to a large value.
237  * Using the transform allows preserving detail up close, while allowing adjacency at distance.
238  * The specific transform used here is:
239  * x /= z; y /= z; z = ln(z);
240  * This transform is applied when calculating the octree bins in OctreePointCloudAdjacency
241  */
242  void
243  setUseSingleCameraTransform (bool val);
244 
245  /** \brief This method launches the segmentation algorithm and returns the supervoxels that were
246  * obtained during the segmentation.
247  * \param[out] supervoxel_clusters A map of labels to pointers to supervoxel structures
248  */
249  virtual void
250  extract (std::map<uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters);
251 
252  /** \brief This method sets the cloud to be supervoxelized
253  * \param[in] cloud The cloud to be supervoxelize
254  */
255  void
256  setInputCloud (const typename pcl::PointCloud<PointT>::ConstPtr& cloud) override;
257 
258  /** \brief This method sets the normals to be used for supervoxels (should be same size as input cloud)
259  * \param[in] normal_cloud The input normals
260  */
261  virtual void
262  setNormalCloud (typename NormalCloudT::ConstPtr normal_cloud);
263 
264  /** \brief This method refines the calculated supervoxels - may only be called after extract
265  * \param[in] num_itr The number of iterations of refinement to be done (2 or 3 is usually sufficient)
266  * \param[out] supervoxel_clusters The resulting refined supervoxels
267  */
268  virtual void
269  refineSupervoxels (int num_itr, std::map<uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters);
270 
271  ////////////////////////////////////////////////////////////
272  /** \brief Returns an RGB colorized cloud showing superpixels
273  * Otherwise it returns an empty pointer.
274  * Points that belong to the same supervoxel have the same color.
275  * But this function doesn't guarantee that different segments will have different
276  * color(it's random). Points that are unlabeled will be black
277  * \note This will expand the label_colors_ vector so that it can accommodate all labels
278  */
279  [[deprecated("use getLabeledCloud() instead. An example of how to display and save with colorized labels can be found in examples/segmentation/example_supervoxels.cpp")]]
282  {
284  }
285 
286  /** \brief Returns a deep copy of the voxel centroid cloud */
288  getVoxelCentroidCloud () const;
289 
290  /** \brief Returns labeled cloud
291  * Points that belong to the same supervoxel have the same label.
292  * Labels for segments start from 1, unlabled points have label 0
293  */
295  getLabeledCloud () const;
296 
297  /** \brief Returns an RGB colorized voxelized cloud showing superpixels
298  * Otherwise it returns an empty pointer.
299  * Points that belong to the same supervoxel have the same color.
300  * But this function doesn't guarantee that different segments will have different
301  * color(it's random). Points that are unlabeled will be black
302  * \note This will expand the label_colors_ vector so that it can accommodate all labels
303  */
304  [[deprecated("use getLabeledVoxelCloud() instead. An example of how to display and save with colorized labels can be found in examples/segmentation/example_supervoxels.cpp")]]
307  {
309  }
310 
311  /** \brief Returns labeled voxelized cloud
312  * Points that belong to the same supervoxel have the same label.
313  * Labels for segments start from 1, unlabled points have label 0
314  */
316  getLabeledVoxelCloud () const;
317 
318  /** \brief Gets the adjacency list (Boost Graph library) which gives connections between supervoxels
319  * \param[out] adjacency_list_arg BGL graph where supervoxel labels are vertices, edges are touching relationships
320  */
321  void
322  getSupervoxelAdjacencyList (VoxelAdjacencyList &adjacency_list_arg) const;
323 
324  /** \brief Get a multimap which gives supervoxel adjacency
325  * \param[out] label_adjacency Multi-Map which maps a supervoxel label to all adjacent supervoxel labels
326  */
327  void
328  getSupervoxelAdjacency (std::multimap<uint32_t, uint32_t> &label_adjacency) const;
329 
330  /** \brief Static helper function which returns a pointcloud of normals for the input supervoxels
331  * \param[in] supervoxel_clusters Supervoxel cluster map coming from this class
332  * \returns Cloud of PointNormals of the supervoxels
333  *
334  */
336  makeSupervoxelNormalCloud (std::map<uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters);
337 
338  /** \brief Returns the current maximum (highest) label */
339  int
340  getMaxLabel () const;
341 
342  private:
343  /** \brief This method simply checks if it is possible to execute the segmentation algorithm with
344  * the current settings. If it is possible then it returns true.
345  */
346  virtual bool
347  prepareForSegmentation ();
348 
349  /** \brief This selects points to use as initial supervoxel centroids
350  * \param[out] seed_indices The selected leaf indices
351  */
352  void
353  selectInitialSupervoxelSeeds (std::vector<int> &seed_indices);
354 
355  /** \brief This method creates the internal supervoxel helpers based on the provided seed points
356  * \param[in] seed_indices Indices of the leaves to use as seeds
357  */
358  void
359  createSupervoxelHelpers (std::vector<int> &seed_indices);
360 
361  /** \brief This performs the superpixel evolution */
362  void
363  expandSupervoxels (int depth);
364 
365  /** \brief This sets the data of the voxels in the tree */
366  void
367  computeVoxelData ();
368 
369  /** \brief Reseeds the supervoxels by finding the voxel closest to current centroid */
370  void
371  reseedSupervoxels ();
372 
373  /** \brief Constructs the map of supervoxel clusters from the internal supervoxel helpers */
374  void
375  makeSupervoxels (std::map<uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters);
376 
377  /** \brief Stores the resolution used in the octree */
378  float resolution_;
379 
380  /** \brief Stores the resolution used to seed the superpixels */
381  float seed_resolution_;
382 
383  /** \brief Distance function used for comparing voxelDatas */
384  float
385  voxelDataDistance (const VoxelData &v1, const VoxelData &v2) const;
386 
387  /** \brief Transform function used to normalize voxel density versus distance from camera */
388  void
389  transformFunction (PointT &p);
390 
391  /** \brief Contains a KDtree for the voxelized cloud */
392  typename pcl::search::KdTree<PointT>::Ptr voxel_kdtree_;
393 
394  /** \brief Octree Adjacency structure with leaves at voxel resolution */
395  typename OctreeAdjacencyT::Ptr adjacency_octree_;
396 
397  /** \brief Contains the Voxelized centroid Cloud */
398  typename PointCloudT::Ptr voxel_centroid_cloud_;
399 
400  /** \brief Contains the Voxelized centroid Cloud */
401  typename NormalCloudT::ConstPtr input_normals_;
402 
403  /** \brief Importance of color in clustering */
404  float color_importance_;
405  /** \brief Importance of distance from seed center in clustering */
406  float spatial_importance_;
407  /** \brief Importance of similarity in normals for clustering */
408  float normal_importance_;
409 
410  /** \brief Whether or not to use the transform compressing depth in Z
411  * This is only checked if it has been manually set by the user.
412  * The default behavior is to use the transform for organized, and not for unorganized.
413  */
414  bool use_single_camera_transform_;
415  /** \brief Whether to use default transform behavior or not */
416  bool use_default_transform_behaviour_;
417 
418  /** \brief Internal storage class for supervoxels
419  * \note Stores pointers to leaves of clustering internal octree,
420  * \note so should not be used outside of clustering class
421  */
422  class SupervoxelHelper
423  {
424  public:
425  /** \brief Comparator for LeafContainerT pointers - used for sorting set of leaves
426  * \note Compares by index in the overall leaf_vector. Order isn't important, so long as it is fixed.
427  */
429  {
430  bool operator() (LeafContainerT* const &left, LeafContainerT* const &right) const
431  {
432  const VoxelData& leaf_data_left = left->getData ();
433  const VoxelData& leaf_data_right = right->getData ();
434  return leaf_data_left.idx_ < leaf_data_right.idx_;
435  }
436  };
437  using LeafSetT = std::set<LeafContainerT*, typename SupervoxelHelper::compareLeaves>;
438  using iterator = typename LeafSetT::iterator;
439  using const_iterator = typename LeafSetT::const_iterator;
440 
441  SupervoxelHelper (uint32_t label, SupervoxelClustering* parent_arg):
442  label_ (label),
443  parent_ (parent_arg)
444  { }
445 
446  void
447  addLeaf (LeafContainerT* leaf_arg);
448 
449  void
450  removeLeaf (LeafContainerT* leaf_arg);
451 
452  void
453  removeAllLeaves ();
454 
455  void
456  expand ();
457 
458  void
459  refineNormals ();
460 
461  void
462  updateCentroid ();
463 
464  void
465  getVoxels (typename pcl::PointCloud<PointT>::Ptr &voxels) const;
466 
467  void
468  getNormals (typename pcl::PointCloud<Normal>::Ptr &normals) const;
469 
470  using DistFuncPtr = float (SupervoxelClustering<PointT>::*)(const VoxelData &, const VoxelData &);
471 
472  uint32_t
473  getLabel () const
474  { return label_; }
475 
476  Eigen::Vector4f
477  getNormal () const
478  { return centroid_.normal_; }
479 
480  Eigen::Vector3f
481  getRGB () const
482  { return centroid_.rgb_; }
483 
484  Eigen::Vector3f
485  getXYZ () const
486  { return centroid_.xyz_;}
487 
488  void
489  getXYZ (float &x, float &y, float &z) const
490  { x=centroid_.xyz_[0]; y=centroid_.xyz_[1]; z=centroid_.xyz_[2]; }
491 
492  void
493  getRGB (uint32_t &rgba) const
494  {
495  rgba = static_cast<uint32_t>(centroid_.rgb_[0]) << 16 |
496  static_cast<uint32_t>(centroid_.rgb_[1]) << 8 |
497  static_cast<uint32_t>(centroid_.rgb_[2]);
498  }
499 
500  void
501  getNormal (pcl::Normal &normal_arg) const
502  {
503  normal_arg.normal_x = centroid_.normal_[0];
504  normal_arg.normal_y = centroid_.normal_[1];
505  normal_arg.normal_z = centroid_.normal_[2];
506  normal_arg.curvature = centroid_.curvature_;
507  }
508 
509  void
510  getNeighborLabels (std::set<uint32_t> &neighbor_labels) const;
511 
512  VoxelData
513  getCentroid () const
514  { return centroid_; }
515 
516  size_t
517  size () const { return leaves_.size (); }
518  private:
519  //Stores leaves
520  LeafSetT leaves_;
521  uint32_t label_;
523  SupervoxelClustering* parent_;
524  public:
525  //Type VoxelData may have fixed-size Eigen objects inside
527  };
528 
529  //Make boost::ptr_list can access the private class SupervoxelHelper
530 #if BOOST_VERSION >= 107000
531  friend void boost::checked_delete<> (const typename pcl::SupervoxelClustering<PointT>::SupervoxelHelper *) BOOST_NOEXCEPT;
532 #else
533  friend void boost::checked_delete<> (const typename pcl::SupervoxelClustering<PointT>::SupervoxelHelper *);
534 #endif
535 
536  using HelperListT = boost::ptr_list<SupervoxelHelper>;
537  HelperListT supervoxel_helpers_;
538 
539  //TODO DEBUG REMOVE
540  StopWatch timer_;
541  public:
543  };
544 
545 }
546 
547 #ifdef PCL_NO_PRECOMPILE
548 #include <pcl/segmentation/impl/supervoxel_clustering.hpp>
549 #endif
A point structure representing normal coordinates and the surface curvature estimate.
pcl::PointCloud< PointT >::Ptr voxels_
A Pointcloud of the voxels in the supervoxel.
Octree adjacency leaf container class- stores a list of pointers to neighbors, number of points added...
VoxelAdjacencyList::vertex_descriptor VoxelID
pcl::PointXYZRGBA centroid_
The centroid of the supervoxel - average voxel.
Octree pointcloud voxel class which maintains adjacency information for its voxels.
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
void getCentroidPointNormal(PointNormal &normal_arg)
Gets the point normal for the supervoxel.
pcl::Normal normal_
The normal calculated for the voxels contained in the supervoxel.
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: pcl_macros.h:344
A point structure representing Euclidean xyz coordinates, and the RGBA color.
pcl::PointCloud< pcl::PointXYZRGBA >::Ptr getColoredVoxelCloud() const
Returns an RGB colorized voxelized cloud showing superpixels Otherwise it returns an empty pointer...
Supervoxel container class - stores a cluster extracted using supervoxel clustering.
boost::shared_ptr< std::vector< int > > IndicesPtr
std::vector< LeafContainerT * > LeafVectorT
PCL base class.
Definition: pcl_base.h:69
boost::shared_ptr< KdTree< PointT, Tree > > Ptr
Definition: kdtree.h:78
VoxelData is a structure used for storing data within a pcl::octree::OctreePointCloudAdjacencyContain...
boost::shared_ptr< Supervoxel< PointT > > Ptr
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:429
void getCentroidPoint(PointXYZRGBA &centroid_arg)
Gets the centroid of the supervoxel.
boost::shared_ptr< OctreeAdjacencyT > Ptr
PointCloud represents the base class in PCL for storing collections of 3D points. ...
Implements a supervoxel algorithm based on voxel structure, normals, and rgb values.
Comparator for LeafContainerT pointers - used for sorting set of leaves.
A point structure representing Euclidean xyz coordinates, together with normal coordinates and the su...
VoxelAdjacencyList::edge_descriptor EdgeID
DataT & getData()
Returns a reference to the data member to access it without copying.
Octree pointcloud search class
Definition: octree_search.h:56
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:430
Simple stopwatch.
Definition: time.h:58
SupervoxelClustering(float voxel_resolution, float seed_resolution, bool)
boost::shared_ptr< const Supervoxel< PointT > > ConstPtr
pcl::PointCloud< Normal >::Ptr normals_
A Pointcloud of the normals for the points in the supervoxel.
A point structure representing Euclidean xyz coordinates, and the RGB color.
boost::adjacency_list< boost::setS, boost::setS, boost::undirectedS, uint32_t, float > VoxelAdjacencyList
pcl::PointCloud< PointXYZRGBA >::Ptr getColoredCloud() const
Returns an RGB colorized cloud showing superpixels Otherwise it returns an empty pointer.