Point Cloud Library (PCL)  1.15.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 
143  owner_ (nullptr)
144  {}
145 
146 #ifdef DOXYGEN_ONLY
147  /** \brief Gets the data of in the form of a point
148  * \param[out] point_arg Will contain the point value of the voxeldata
149  */
150  void
151  getPoint (PointT &point_arg) const;
152 #else
153  template<typename PointT2 = PointT, traits::HasColor<PointT2> = true> void
154  getPoint (PointT &point_arg) const
155  {
156  point_arg.rgba = static_cast<std::uint32_t>(rgb_[0]) << 16 |
157  static_cast<std::uint32_t>(rgb_[1]) << 8 |
158  static_cast<std::uint32_t>(rgb_[2]);
159  point_arg.x = xyz_[0];
160  point_arg.y = xyz_[1];
161  point_arg.z = xyz_[2];
162  }
163 
164  template<typename PointT2 = PointT, traits::HasNoColor<PointT2> = true> void
165  getPoint (PointT &point_arg ) const
166  {
167  //XYZ is required or this doesn't make much sense...
168  point_arg.x = xyz_[0];
169  point_arg.y = xyz_[1];
170  point_arg.z = xyz_[2];
171  }
172 #endif
173 
174  /** \brief Gets the data of in the form of a normal
175  * \param[out] normal_arg Will contain the normal value of the voxeldata
176  */
177  void
178  getNormal (Normal &normal_arg) const;
179 
180  Eigen::Vector3f xyz_;
181  Eigen::Vector3f rgb_;
182  Eigen::Vector4f normal_;
183  float curvature_{0.0f};
184  float distance_{0.0f};
185  int idx_{0};
186  SupervoxelHelper* owner_;
187 
188  public:
190  };
191 
193  using LeafVectorT = std::vector<LeafContainerT *>;
194 
200 
204 
205  using VoxelAdjacencyList = boost::adjacency_list<boost::setS, boost::setS, boost::undirectedS, std::uint32_t, float>;
206  using VoxelID = VoxelAdjacencyList::vertex_descriptor;
207  using EdgeID = VoxelAdjacencyList::edge_descriptor;
208 
209  public:
210 
211  /** \brief Constructor that sets default values for member variables.
212  * \param[in] voxel_resolution The resolution (in meters) of voxels used
213  * \param[in] seed_resolution The average size (in meters) of resulting supervoxels
214  */
215  SupervoxelClustering (float voxel_resolution, float seed_resolution);
216 
217  /** \brief This destructor destroys the cloud, normals and search method used for
218  * finding neighbors. In other words it frees memory.
219  */
220 
222 
223  /** \brief Set the resolution of the octree voxels */
224  void
225  setVoxelResolution (float resolution);
226 
227  /** \brief Get the resolution of the octree voxels */
228  float
229  getVoxelResolution () const;
230 
231  /** \brief Set the resolution of the octree seed voxels */
232  void
233  setSeedResolution (float seed_resolution);
234 
235  /** \brief Get the resolution of the octree seed voxels */
236  float
237  getSeedResolution () const;
238 
239  /** \brief Set the importance of color for supervoxels */
240  void
241  setColorImportance (float val);
242 
243  /** \brief Set the importance of spatial distance for supervoxels */
244  void
245  setSpatialImportance (float val);
246 
247  /** \brief Set the importance of scalar normal product for supervoxels */
248  void
249  setNormalImportance (float val);
250 
251  /** \brief Set whether or not to use the single camera transform
252  * \note By default it will be used for organized clouds, but not for unorganized - this parameter will override that behavior
253  * The single camera transform scales bin size so that it increases exponentially with depth (z dimension).
254  * This is done to account for the decreasing point density found with depth when using an RGB-D camera.
255  * Without the transform, beyond a certain depth adjacency of voxels breaks down unless the voxel size is set to a large value.
256  * Using the transform allows preserving detail up close, while allowing adjacency at distance.
257  * The specific transform used here is:
258  * x /= z; y /= z; z = ln(z);
259  * This transform is applied when calculating the octree bins in OctreePointCloudAdjacency
260  */
261  void
262  setUseSingleCameraTransform (bool val);
263 
264  /** \brief This method launches the segmentation algorithm and returns the supervoxels that were
265  * obtained during the segmentation.
266  * \param[out] supervoxel_clusters A map of labels to pointers to supervoxel structures
267  */
268  virtual void
269  extract (std::map<std::uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters);
270 
271  /** \brief This method sets the cloud to be supervoxelized
272  * \param[in] cloud The cloud to be supervoxelize
273  */
274  void
275  setInputCloud (const typename pcl::PointCloud<PointT>::ConstPtr& cloud) override;
276 
277  /** \brief This method sets the normals to be used for supervoxels (should be same size as input cloud)
278  * \param[in] normal_cloud The input normals
279  */
280  virtual void
281  setNormalCloud (typename NormalCloudT::ConstPtr normal_cloud);
282 
283  /** \brief This method refines the calculated supervoxels - may only be called after extract
284  * \param[in] num_itr The number of iterations of refinement to be done (2 or 3 is usually sufficient)
285  * \param[out] supervoxel_clusters The resulting refined supervoxels
286  */
287  virtual void
288  refineSupervoxels (int num_itr, std::map<std::uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters);
289 
290  ////////////////////////////////////////////////////////////
291  /** \brief Returns a deep copy of the voxel centroid cloud */
293  getVoxelCentroidCloud () const;
294 
295  /** \brief Returns labeled cloud
296  * Points that belong to the same supervoxel have the same label.
297  * Labels for segments start from 1, unlabeled points have label 0
298  */
300  getLabeledCloud () const;
301 
302  /** \brief Returns labeled voxelized cloud
303  * Points that belong to the same supervoxel have the same label.
304  * Labels for segments start from 1, unlabeled points have label 0
305  */
307  getLabeledVoxelCloud () const;
308 
309  /** \brief Gets the adjacency list (Boost Graph library) which gives connections between supervoxels
310  * \param[out] adjacency_list_arg BGL graph where supervoxel labels are vertices, edges are touching relationships
311  */
312  void
313  getSupervoxelAdjacencyList (VoxelAdjacencyList &adjacency_list_arg) const;
314 
315  /** \brief Get a multimap which gives supervoxel adjacency
316  * \param[out] label_adjacency Multi-Map which maps a supervoxel label to all adjacent supervoxel labels
317  */
318  void
319  getSupervoxelAdjacency (std::multimap<std::uint32_t, std::uint32_t> &label_adjacency) const;
320 
321  /** \brief Static helper function which returns a pointcloud of normals for the input supervoxels
322  * \param[in] supervoxel_clusters Supervoxel cluster map coming from this class
323  * \returns Cloud of PointNormals of the supervoxels
324  *
325  */
327  makeSupervoxelNormalCloud (std::map<std::uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters);
328 
329  /** \brief Returns the current maximum (highest) label */
330  int
331  getMaxLabel () const;
332 
333  private:
334  /** \brief This method simply checks if it is possible to execute the segmentation algorithm with
335  * the current settings. If it is possible then it returns true.
336  */
337  virtual bool
338  prepareForSegmentation ();
339 
340  /** \brief This selects points to use as initial supervoxel centroids
341  * \param[out] seed_indices The selected leaf indices
342  */
343  void
344  selectInitialSupervoxelSeeds (Indices &seed_indices);
345 
346  /** \brief This method creates the internal supervoxel helpers based on the provided seed points
347  * \param[in] seed_indices Indices of the leaves to use as seeds
348  */
349  void
350  createSupervoxelHelpers (Indices &seed_indices);
351 
352  /** \brief This performs the superpixel evolution */
353  void
354  expandSupervoxels (int depth);
355 
356  /** \brief This sets the data of the voxels in the tree */
357  void
358  computeVoxelData ();
359 
360  /** \brief Reseeds the supervoxels by finding the voxel closest to current centroid */
361  void
362  reseedSupervoxels ();
363 
364  /** \brief Constructs the map of supervoxel clusters from the internal supervoxel helpers */
365  void
366  makeSupervoxels (std::map<std::uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters);
367 
368  /** \brief Stores the resolution used in the octree */
369  float resolution_;
370 
371  /** \brief Stores the resolution used to seed the superpixels */
372  float seed_resolution_;
373 
374  /** \brief Distance function used for comparing voxelDatas */
375  float
376  voxelDataDistance (const VoxelData &v1, const VoxelData &v2) const;
377 
378  /** \brief Transform function used to normalize voxel density versus distance from camera */
379  void
380  transformFunction (PointT &p);
381 
382  /** \brief Contains a KDtree for the voxelized cloud */
383  typename pcl::search::Search<PointT>::Ptr voxel_kdtree_;
384 
385  /** \brief Octree Adjacency structure with leaves at voxel resolution */
386  typename OctreeAdjacencyT::Ptr adjacency_octree_;
387 
388  /** \brief Contains the Voxelized centroid Cloud */
389  typename PointCloudT::Ptr voxel_centroid_cloud_;
390 
391  /** \brief Contains the Voxelized centroid Cloud */
392  typename NormalCloudT::ConstPtr input_normals_;
393 
394  /** \brief Importance of color in clustering */
395  float color_importance_{0.1f};
396  /** \brief Importance of distance from seed center in clustering */
397  float spatial_importance_{0.4f};
398  /** \brief Importance of similarity in normals for clustering */
399  float normal_importance_{1.0f};
400 
401  /** \brief Whether or not to use the transform compressing depth in Z
402  * This is only checked if it has been manually set by the user.
403  * The default behavior is to use the transform for organized, and not for unorganized.
404  */
405  bool use_single_camera_transform_;
406  /** \brief Whether to use default transform behavior or not */
407  bool use_default_transform_behaviour_{true};
408 
409  /** \brief Internal storage class for supervoxels
410  * \note Stores pointers to leaves of clustering internal octree,
411  * \note so should not be used outside of clustering class
412  */
413  class SupervoxelHelper
414  {
415  public:
416  /** \brief Comparator for LeafContainerT pointers - used for sorting set of leaves
417  * \note Compares by index in the overall leaf_vector. Order isn't important, so long as it is fixed.
418  */
420  {
421  bool operator() (LeafContainerT* const &left, LeafContainerT* const &right) const
422  {
423  const VoxelData& leaf_data_left = left->getData ();
424  const VoxelData& leaf_data_right = right->getData ();
425  return leaf_data_left.idx_ < leaf_data_right.idx_;
426  }
427  };
428  using LeafSetT = std::set<LeafContainerT*, typename SupervoxelHelper::compareLeaves>;
429  using iterator = typename LeafSetT::iterator;
430  using const_iterator = typename LeafSetT::const_iterator;
431 
432  SupervoxelHelper (std::uint32_t label, SupervoxelClustering* parent_arg):
433  label_ (label),
434  parent_ (parent_arg)
435  { }
436 
437  void
438  addLeaf (LeafContainerT* leaf_arg);
439 
440  void
441  removeLeaf (LeafContainerT* leaf_arg);
442 
443  void
444  removeAllLeaves ();
445 
446  void
447  expand ();
448 
449  void
450  refineNormals ();
451 
452  void
453  updateCentroid ();
454 
455  void
456  getVoxels (typename pcl::PointCloud<PointT>::Ptr &voxels) const;
457 
458  void
459  getNormals (typename pcl::PointCloud<Normal>::Ptr &normals) const;
460 
461  using DistFuncPtr = float (SupervoxelClustering<PointT>::*)(const VoxelData &, const VoxelData &);
462 
463  std::uint32_t
464  getLabel () const
465  { return label_; }
466 
467  Eigen::Vector4f
468  getNormal () const
469  { return centroid_.normal_; }
470 
471  Eigen::Vector3f
472  getRGB () const
473  { return centroid_.rgb_; }
474 
475  Eigen::Vector3f
476  getXYZ () const
477  { return centroid_.xyz_;}
478 
479  void
480  getXYZ (float &x, float &y, float &z) const
481  { x=centroid_.xyz_[0]; y=centroid_.xyz_[1]; z=centroid_.xyz_[2]; }
482 
483  void
484  getRGB (std::uint32_t &rgba) const
485  {
486  rgba = static_cast<std::uint32_t>(centroid_.rgb_[0]) << 16 |
487  static_cast<std::uint32_t>(centroid_.rgb_[1]) << 8 |
488  static_cast<std::uint32_t>(centroid_.rgb_[2]);
489  }
490 
491  void
492  getNormal (pcl::Normal &normal_arg) const
493  {
494  normal_arg.normal_x = centroid_.normal_[0];
495  normal_arg.normal_y = centroid_.normal_[1];
496  normal_arg.normal_z = centroid_.normal_[2];
497  normal_arg.curvature = centroid_.curvature_;
498  }
499 
500  void
501  getNeighborLabels (std::set<std::uint32_t> &neighbor_labels) const;
502 
503  VoxelData
504  getCentroid () const
505  { return centroid_; }
506 
507  std::size_t
508  size () const { return leaves_.size (); }
509  private:
510  //Stores leaves
511  LeafSetT leaves_;
512  std::uint32_t label_;
513  VoxelData centroid_;
514  SupervoxelClustering* parent_;
515  public:
516  //Type VoxelData may have fixed-size Eigen objects inside
518  };
519 
520  //Make boost::ptr_list can access the private class SupervoxelHelper
521 #if BOOST_VERSION >= 107000
522  friend void boost::checked_delete<> (const typename pcl::SupervoxelClustering<PointT>::SupervoxelHelper *) BOOST_NOEXCEPT;
523 #else
524  friend void boost::checked_delete<> (const typename pcl::SupervoxelClustering<PointT>::SupervoxelHelper *);
525 #endif
526 
527  using HelperListT = boost::ptr_list<SupervoxelHelper>;
528  HelperListT supervoxel_helpers_;
529 
530  //TODO DEBUG REMOVE
531  StopWatch timer_;
532  public:
534  };
535 
536 }
537 
538 #ifdef PCL_NO_PRECOMPILE
539 #include <pcl/segmentation/impl/supervoxel_clustering.hpp>
540 #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:174
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:414
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:415
Simple stopwatch.
Definition: time.h:59
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.
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
shared_ptr< pcl::search::Search< PointT > > Ptr
Definition: search.h:81
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:86
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:324
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.