Point Cloud Library (PCL)  1.13.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/memory.h>
47 #include <pcl/pcl_base.h>
48 #include <pcl/pcl_macros.h>
49 #include <pcl/point_cloud.h>
50 #include <pcl/point_types.h>
51 #include <pcl/octree/octree_search.h>
52 #include <pcl/octree/octree_pointcloud_adjacency.h>
53 #include <pcl/search/search.h>
54 #include <boost/ptr_container/ptr_list.hpp> // for ptr_list
55 
56 
57 
58 //DEBUG TODO REMOVE
59 #include <pcl/common/time.h>
60 
61 
62 namespace pcl
63 {
64  /** \brief Supervoxel container class - stores a cluster extracted using supervoxel clustering
65  */
66  template <typename PointT>
67  class Supervoxel
68  {
69  public:
71  voxels_ (new pcl::PointCloud<PointT> ()),
72  normals_ (new pcl::PointCloud<Normal> ())
73  { }
74 
75  using Ptr = shared_ptr<Supervoxel<PointT> >;
76  using ConstPtr = shared_ptr<const Supervoxel<PointT> >;
77 
78  /** \brief Gets the centroid of the supervoxel
79  * \param[out] centroid_arg centroid of the supervoxel
80  */
81  void
83  {
84  centroid_arg = centroid_;
85  }
86 
87  /** \brief Gets the point normal for the supervoxel
88  * \param[out] normal_arg Point normal of the supervoxel
89  * \note This isn't an average, it is a normal computed using all of the voxels in the supervoxel as support
90  */
91  void
93  {
94  normal_arg.x = centroid_.x;
95  normal_arg.y = centroid_.y;
96  normal_arg.z = centroid_.z;
97  normal_arg.normal_x = normal_.normal_x;
98  normal_arg.normal_y = normal_.normal_y;
99  normal_arg.normal_z = normal_.normal_z;
100  normal_arg.curvature = normal_.curvature;
101  }
102 
103  /** \brief The normal calculated for the voxels contained in the supervoxel */
105  /** \brief The centroid of the supervoxel - average voxel */
107  /** \brief A Pointcloud of the voxels in the supervoxel */
109  /** \brief A Pointcloud of the normals for the points in the supervoxel */
111 
112  public:
114  };
115 
116  /** \brief Implements a supervoxel algorithm based on voxel structure, normals, and rgb values
117  * \note Supervoxels are oversegmented volumetric patches (usually surfaces)
118  * \note Usually, color isn't needed (and can be detrimental)- spatial structure is mainly used
119  * - J. Papon, A. Abramov, M. Schoeler, F. Woergoetter
120  * Voxel Cloud Connectivity Segmentation - Supervoxels from PointClouds
121  * In Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition (CVPR) 2013
122  * \ingroup segmentation
123  * \author Jeremie Papon (jpapon@gmail.com)
124  */
125  template <typename PointT>
127  {
128  //Forward declaration of friended helper class
129  class SupervoxelHelper;
130  friend class SupervoxelHelper;
131  public:
132  /** \brief VoxelData is a structure used for storing data within a pcl::octree::OctreePointCloudAdjacencyContainer
133  * \note It stores xyz, rgb, normal, distance, an index, and an owner.
134  */
135  class VoxelData
136  {
137  public:
139  xyz_ (0.0f, 0.0f, 0.0f),
140  rgb_ (0.0f, 0.0f, 0.0f),
141  normal_ (0.0f, 0.0f, 0.0f, 0.0f),
142  curvature_ (0.0f),
143  distance_(0),
144  idx_(0),
145  owner_ (nullptr)
146  {}
147 
148  /** \brief Gets the data of in the form of a point
149  * \param[out] point_arg Will contain the point value of the voxeldata
150  */
151  void
152  getPoint (PointT &point_arg) const;
153 
154  /** \brief Gets the data of in the form of a normal
155  * \param[out] normal_arg Will contain the normal value of the voxeldata
156  */
157  void
158  getNormal (Normal &normal_arg) const;
159 
160  Eigen::Vector3f xyz_;
161  Eigen::Vector3f rgb_;
162  Eigen::Vector4f normal_;
163  float curvature_;
164  float distance_;
165  int idx_;
166  SupervoxelHelper* owner_;
167 
168  public:
170  };
171 
173  using LeafVectorT = std::vector<LeafContainerT *>;
174 
181 
185 
186  using VoxelAdjacencyList = boost::adjacency_list<boost::setS, boost::setS, boost::undirectedS, std::uint32_t, float>;
187  using VoxelID = VoxelAdjacencyList::vertex_descriptor;
188  using EdgeID = VoxelAdjacencyList::edge_descriptor;
189 
190  public:
191 
192  /** \brief Constructor that sets default values for member variables.
193  * \param[in] voxel_resolution The resolution (in meters) of voxels used
194  * \param[in] seed_resolution The average size (in meters) of resulting supervoxels
195  */
196  SupervoxelClustering (float voxel_resolution, float 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<std::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<std::uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters);
270 
271  ////////////////////////////////////////////////////////////
272  /** \brief Returns a deep copy of the voxel centroid cloud */
274  getVoxelCentroidCloud () const;
275 
276  /** \brief Returns labeled cloud
277  * Points that belong to the same supervoxel have the same label.
278  * Labels for segments start from 1, unlabeled points have label 0
279  */
281  getLabeledCloud () const;
282 
283  /** \brief Returns labeled voxelized cloud
284  * Points that belong to the same supervoxel have the same label.
285  * Labels for segments start from 1, unlabeled points have label 0
286  */
288  getLabeledVoxelCloud () const;
289 
290  /** \brief Gets the adjacency list (Boost Graph library) which gives connections between supervoxels
291  * \param[out] adjacency_list_arg BGL graph where supervoxel labels are vertices, edges are touching relationships
292  */
293  void
294  getSupervoxelAdjacencyList (VoxelAdjacencyList &adjacency_list_arg) const;
295 
296  /** \brief Get a multimap which gives supervoxel adjacency
297  * \param[out] label_adjacency Multi-Map which maps a supervoxel label to all adjacent supervoxel labels
298  */
299  void
300  getSupervoxelAdjacency (std::multimap<std::uint32_t, std::uint32_t> &label_adjacency) const;
301 
302  /** \brief Static helper function which returns a pointcloud of normals for the input supervoxels
303  * \param[in] supervoxel_clusters Supervoxel cluster map coming from this class
304  * \returns Cloud of PointNormals of the supervoxels
305  *
306  */
308  makeSupervoxelNormalCloud (std::map<std::uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters);
309 
310  /** \brief Returns the current maximum (highest) label */
311  int
312  getMaxLabel () const;
313 
314  private:
315  /** \brief This method simply checks if it is possible to execute the segmentation algorithm with
316  * the current settings. If it is possible then it returns true.
317  */
318  virtual bool
319  prepareForSegmentation ();
320 
321  /** \brief This selects points to use as initial supervoxel centroids
322  * \param[out] seed_indices The selected leaf indices
323  */
324  void
325  selectInitialSupervoxelSeeds (Indices &seed_indices);
326 
327  /** \brief This method creates the internal supervoxel helpers based on the provided seed points
328  * \param[in] seed_indices Indices of the leaves to use as seeds
329  */
330  void
331  createSupervoxelHelpers (Indices &seed_indices);
332 
333  /** \brief This performs the superpixel evolution */
334  void
335  expandSupervoxels (int depth);
336 
337  /** \brief This sets the data of the voxels in the tree */
338  void
339  computeVoxelData ();
340 
341  /** \brief Reseeds the supervoxels by finding the voxel closest to current centroid */
342  void
343  reseedSupervoxels ();
344 
345  /** \brief Constructs the map of supervoxel clusters from the internal supervoxel helpers */
346  void
347  makeSupervoxels (std::map<std::uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters);
348 
349  /** \brief Stores the resolution used in the octree */
350  float resolution_;
351 
352  /** \brief Stores the resolution used to seed the superpixels */
353  float seed_resolution_;
354 
355  /** \brief Distance function used for comparing voxelDatas */
356  float
357  voxelDataDistance (const VoxelData &v1, const VoxelData &v2) const;
358 
359  /** \brief Transform function used to normalize voxel density versus distance from camera */
360  void
361  transformFunction (PointT &p);
362 
363  /** \brief Contains a KDtree for the voxelized cloud */
364  typename pcl::search::KdTree<PointT>::Ptr voxel_kdtree_;
365 
366  /** \brief Octree Adjacency structure with leaves at voxel resolution */
367  typename OctreeAdjacencyT::Ptr adjacency_octree_;
368 
369  /** \brief Contains the Voxelized centroid Cloud */
370  typename PointCloudT::Ptr voxel_centroid_cloud_;
371 
372  /** \brief Contains the Voxelized centroid Cloud */
373  typename NormalCloudT::ConstPtr input_normals_;
374 
375  /** \brief Importance of color in clustering */
376  float color_importance_;
377  /** \brief Importance of distance from seed center in clustering */
378  float spatial_importance_;
379  /** \brief Importance of similarity in normals for clustering */
380  float normal_importance_;
381 
382  /** \brief Whether or not to use the transform compressing depth in Z
383  * This is only checked if it has been manually set by the user.
384  * The default behavior is to use the transform for organized, and not for unorganized.
385  */
386  bool use_single_camera_transform_;
387  /** \brief Whether to use default transform behavior or not */
388  bool use_default_transform_behaviour_;
389 
390  /** \brief Internal storage class for supervoxels
391  * \note Stores pointers to leaves of clustering internal octree,
392  * \note so should not be used outside of clustering class
393  */
394  class SupervoxelHelper
395  {
396  public:
397  /** \brief Comparator for LeafContainerT pointers - used for sorting set of leaves
398  * \note Compares by index in the overall leaf_vector. Order isn't important, so long as it is fixed.
399  */
401  {
402  bool operator() (LeafContainerT* const &left, LeafContainerT* const &right) const
403  {
404  const VoxelData& leaf_data_left = left->getData ();
405  const VoxelData& leaf_data_right = right->getData ();
406  return leaf_data_left.idx_ < leaf_data_right.idx_;
407  }
408  };
409  using LeafSetT = std::set<LeafContainerT*, typename SupervoxelHelper::compareLeaves>;
410  using iterator = typename LeafSetT::iterator;
411  using const_iterator = typename LeafSetT::const_iterator;
412 
413  SupervoxelHelper (std::uint32_t label, SupervoxelClustering* parent_arg):
414  label_ (label),
415  parent_ (parent_arg)
416  { }
417 
418  void
419  addLeaf (LeafContainerT* leaf_arg);
420 
421  void
422  removeLeaf (LeafContainerT* leaf_arg);
423 
424  void
425  removeAllLeaves ();
426 
427  void
428  expand ();
429 
430  void
431  refineNormals ();
432 
433  void
434  updateCentroid ();
435 
436  void
437  getVoxels (typename pcl::PointCloud<PointT>::Ptr &voxels) const;
438 
439  void
440  getNormals (typename pcl::PointCloud<Normal>::Ptr &normals) const;
441 
442  using DistFuncPtr = float (SupervoxelClustering<PointT>::*)(const VoxelData &, const VoxelData &);
443 
444  std::uint32_t
445  getLabel () const
446  { return label_; }
447 
448  Eigen::Vector4f
449  getNormal () const
450  { return centroid_.normal_; }
451 
452  Eigen::Vector3f
453  getRGB () const
454  { return centroid_.rgb_; }
455 
456  Eigen::Vector3f
457  getXYZ () const
458  { return centroid_.xyz_;}
459 
460  void
461  getXYZ (float &x, float &y, float &z) const
462  { x=centroid_.xyz_[0]; y=centroid_.xyz_[1]; z=centroid_.xyz_[2]; }
463 
464  void
465  getRGB (std::uint32_t &rgba) const
466  {
467  rgba = static_cast<std::uint32_t>(centroid_.rgb_[0]) << 16 |
468  static_cast<std::uint32_t>(centroid_.rgb_[1]) << 8 |
469  static_cast<std::uint32_t>(centroid_.rgb_[2]);
470  }
471 
472  void
473  getNormal (pcl::Normal &normal_arg) const
474  {
475  normal_arg.normal_x = centroid_.normal_[0];
476  normal_arg.normal_y = centroid_.normal_[1];
477  normal_arg.normal_z = centroid_.normal_[2];
478  normal_arg.curvature = centroid_.curvature_;
479  }
480 
481  void
482  getNeighborLabels (std::set<std::uint32_t> &neighbor_labels) const;
483 
484  VoxelData
485  getCentroid () const
486  { return centroid_; }
487 
488  std::size_t
489  size () const { return leaves_.size (); }
490  private:
491  //Stores leaves
492  LeafSetT leaves_;
493  std::uint32_t label_;
494  VoxelData centroid_;
495  SupervoxelClustering* parent_;
496  public:
497  //Type VoxelData may have fixed-size Eigen objects inside
499  };
500 
501  //Make boost::ptr_list can access the private class SupervoxelHelper
502 #if BOOST_VERSION >= 107000
503  friend void boost::checked_delete<> (const typename pcl::SupervoxelClustering<PointT>::SupervoxelHelper *) BOOST_NOEXCEPT;
504 #else
505  friend void boost::checked_delete<> (const typename pcl::SupervoxelClustering<PointT>::SupervoxelHelper *);
506 #endif
507 
508  using HelperListT = boost::ptr_list<SupervoxelHelper>;
509  HelperListT supervoxel_helpers_;
510 
511  //TODO DEBUG REMOVE
512  StopWatch timer_;
513  public:
515  };
516 
517 }
518 
519 #ifdef PCL_NO_PRECOMPILE
520 #include <pcl/segmentation/impl/supervoxel_clustering.hpp>
521 #endif
PCL base class.
Definition: pcl_base.h:70
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:414
Simple stopwatch.
Definition: time.h:59
VoxelData is a structure used for storing data within a pcl::octree::OctreePointCloudAdjacencyContain...
Implements a supervoxel algorithm based on voxel structure, normals, and rgb values.
VoxelAdjacencyList::edge_descriptor EdgeID
std::vector< LeafContainerT * > LeafVectorT
boost::adjacency_list< boost::setS, boost::setS, boost::undirectedS, std::uint32_t, float > VoxelAdjacencyList
VoxelAdjacencyList::vertex_descriptor VoxelID
~SupervoxelClustering() override
This destructor destroys the cloud, normals and search method used for finding neighbors.
Supervoxel container class - stores a cluster extracted using supervoxel clustering.
pcl::PointXYZRGBA centroid_
The centroid of the supervoxel - average voxel.
pcl::PointCloud< Normal >::Ptr normals_
A Pointcloud of the normals for the points in the supervoxel.
shared_ptr< const Supervoxel< PointT > > ConstPtr
void getCentroidPoint(PointXYZRGBA &centroid_arg)
Gets the centroid of the supervoxel.
shared_ptr< Supervoxel< PointT > > Ptr
pcl::PointCloud< PointT >::Ptr voxels_
A Pointcloud of the voxels in the supervoxel.
pcl::Normal normal_
The normal calculated for the voxels contained in the supervoxel.
void getCentroidPointNormal(PointNormal &normal_arg)
Gets the point normal for the supervoxel.
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
shared_ptr< KdTree< PointT, Tree > > Ptr
Definition: kdtree.h:75
Defines all the PCL implemented PointT point type structures.
Define methods for measuring time spent in code blocks.
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: memory.h:63
Defines functions, macros and traits for allocating and using memory.
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
shared_ptr< Indices > IndicesPtr
Definition: pcl_base.h:58
Defines all the PCL and non-PCL macros used.
#define PCL_EXPORTS
Definition: pcl_macros.h:323
A point structure representing normal coordinates and the surface curvature estimate.
A point structure representing Euclidean xyz coordinates, together with normal coordinates and the su...
A point structure representing Euclidean xyz coordinates, and the RGBA color.
A point structure representing Euclidean xyz coordinates, and the RGB color.
Comparator for LeafContainerT pointers - used for sorting set of leaves.