Point Cloud Library (PCL)  1.7.1
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 #ifndef PCL_SEGMENTATION_SUPERVOXEL_CLUSTERING_H_
42 #define PCL_SEGMENTATION_SUPERVOXEL_CLUSTERING_H_
43 
44 #include <pcl/features/normal_3d.h>
45 #include <pcl/pcl_base.h>
46 #include <pcl/point_cloud.h>
47 #include <pcl/point_types.h>
48 #include <pcl/octree/octree.h>
49 #include <pcl/octree/octree_pointcloud_adjacency.h>
50 #include <pcl/search/search.h>
51 #include <pcl/segmentation/boost.h>
52 
53 
54 
55 //DEBUG TODO REMOVE
56 #include <pcl/common/time.h>
57 
58 
59 namespace pcl
60 {
61  /** \brief Supervoxel container class - stores a cluster extracted using supervoxel clustering
62  */
63  template <typename PointT>
64  class Supervoxel
65  {
66  public:
68  voxels_ (new pcl::PointCloud<PointT> ()),
69  normals_ (new pcl::PointCloud<Normal> ())
70  { }
71 
72  typedef boost::shared_ptr<Supervoxel<PointT> > Ptr;
73  typedef boost::shared_ptr<const Supervoxel<PointT> > ConstPtr;
74 
75  /** \brief Gets the centroid of the supervoxel
76  * \param[out] centroid_arg centroid of the supervoxel
77  */
78  void
80  {
81  centroid_arg = centroid_;
82  }
83 
84  /** \brief Gets the point normal for the supervoxel
85  * \param[out] normal_arg Point normal of the supervoxel
86  * \note This isn't an average, it is a normal computed using all of the voxels in the supervoxel as support
87  */
88  void
90  {
91  normal_arg.x = centroid_.x;
92  normal_arg.y = centroid_.y;
93  normal_arg.z = centroid_.z;
94  normal_arg.normal_x = normal_.normal_x;
95  normal_arg.normal_y = normal_.normal_y;
96  normal_arg.normal_z = normal_.normal_z;
97  normal_arg.curvature = normal_.curvature;
98  }
99 
100  /** \brief The normal calculated for the voxels contained in the supervoxel */
102  /** \brief The centroid of the supervoxel - average voxel */
104  /** \brief A Pointcloud of the voxels in the supervoxel */
106  /** \brief A Pointcloud of the normals for the points in the supervoxel */
108 
109  public:
110  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
111  };
112 
113  /** \brief Implements a supervoxel algorithm based on voxel structure, normals, and rgb values
114  * \note Supervoxels are oversegmented volumetric patches (usually surfaces)
115  * \note Usually, color isn't needed (and can be detrimental)- spatial structure is mainly used
116  * - J. Papon, A. Abramov, M. Schoeler, F. Woergoetter
117  * Voxel Cloud Connectivity Segmentation - Supervoxels from PointClouds
118  * In Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition (CVPR) 2013
119  * \author Jeremie Papon (jpapon@gmail.com)
120  */
121  template <typename PointT>
122  class PCL_EXPORTS SupervoxelClustering : public pcl::PCLBase<PointT>
123  {
124  //Forward declaration of friended helper class
125  class SupervoxelHelper;
126  friend class SupervoxelHelper;
127  public:
128  /** \brief VoxelData is a structure used for storing data within a pcl::octree::OctreePointCloudAdjacencyContainer
129  * \note It stores xyz, rgb, normal, distance, an index, and an owner.
130  */
131  class VoxelData
132  {
133  public:
135  xyz_ (0.0f, 0.0f, 0.0f),
136  rgb_ (0.0f, 0.0f, 0.0f),
137  normal_ (0.0f, 0.0f, 0.0f, 0.0f),
138  curvature_ (0.0f),
139  owner_ (0)
140  {}
141 
142  /** \brief Gets the data of in the form of a point
143  * \param[out] point_arg Will contain the point value of the voxeldata
144  */
145  void
146  getPoint (PointT &point_arg) const;
147 
148  /** \brief Gets the data of in the form of a normal
149  * \param[out] normal_arg Will contain the normal value of the voxeldata
150  */
151  void
152  getNormal (Normal &normal_arg) const;
153 
154  Eigen::Vector3f xyz_;
155  Eigen::Vector3f rgb_;
156  Eigen::Vector4f normal_;
157  float curvature_;
158  float distance_;
159  int idx_;
160  SupervoxelHelper* owner_;
161 
162  public:
163  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
164  };
165 
167  typedef std::vector <LeafContainerT*> LeafVectorT;
168 
174  typedef boost::shared_ptr<std::vector<int> > IndicesPtr;
175 
179 
180  typedef boost::adjacency_list<boost::setS, boost::setS, boost::undirectedS, uint32_t, float> VoxelAdjacencyList;
181  typedef VoxelAdjacencyList::vertex_descriptor VoxelID;
182  typedef VoxelAdjacencyList::edge_descriptor EdgeID;
183 
184 
185  public:
186 
187  /** \brief Constructor that sets default values for member variables.
188  * \param[in] voxel_resolution The resolution (in meters) of voxels used
189  * \param[in] seed_resolution The average size (in meters) of resulting supervoxels
190  * \param[in] use_single_camera_transform Set to true if point density in cloud falls off with distance from origin (such as with a cloud coming from one stationary camera), set false if input cloud is from multiple captures from multiple locations.
191  */
192  SupervoxelClustering (float voxel_resolution, float seed_resolution, bool use_single_camera_transform = true);
193 
194  /** \brief This destructor destroys the cloud, normals and search method used for
195  * finding neighbors. In other words it frees memory.
196  */
197  virtual
199 
200  /** \brief Set the resolution of the octree voxels */
201  void
202  setVoxelResolution (float resolution);
203 
204  /** \brief Get the resolution of the octree voxels */
205  float
206  getVoxelResolution () const;
207 
208  /** \brief Set the resolution of the octree seed voxels */
209  void
210  setSeedResolution (float seed_resolution);
211 
212  /** \brief Get the resolution of the octree seed voxels */
213  float
214  getSeedResolution () const;
215 
216  /** \brief Set the importance of color for supervoxels */
217  void
218  setColorImportance (float val);
219 
220  /** \brief Set the importance of spatial distance for supervoxels */
221  void
222  setSpatialImportance (float val);
223 
224  /** \brief Set the importance of scalar normal product for supervoxels */
225  void
226  setNormalImportance (float val);
227 
228  /** \brief This method launches the segmentation algorithm and returns the supervoxels that were
229  * obtained during the segmentation.
230  * \param[out] supervoxel_clusters A map of labels to pointers to supervoxel structures
231  */
232  virtual void
233  extract (std::map<uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters);
234 
235  /** \brief This method sets the cloud to be supervoxelized
236  * \param[in] cloud The cloud to be supervoxelize
237  */
238  virtual void
239  setInputCloud (typename pcl::PointCloud<PointT>::ConstPtr cloud);
240 
241  /** \brief This method sets the normals to be used for supervoxels (should be same size as input cloud)
242  * \param[in] cloud The input normals
243  */
244  virtual void
245  setNormalCloud (typename NormalCloudT::ConstPtr normal_cloud);
246 
247  /** \brief This method refines the calculated supervoxels - may only be called after extract
248  * \param[in] num_itr The number of iterations of refinement to be done (2 or 3 is usually sufficient)
249  * \param[out] supervoxel_clusters The resulting refined supervoxels
250  */
251  virtual void
252  refineSupervoxels (int num_itr, std::map<uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters);
253 
254  ////////////////////////////////////////////////////////////
255  /** \brief Returns an RGB colorized cloud showing superpixels
256  * Otherwise it returns an empty pointer.
257  * Points that belong to the same supervoxel have the same color.
258  * But this function doesn't guarantee that different segments will have different
259  * color(it's random). Points that are unlabeled will be black
260  * \note This will expand the label_colors_ vector so that it can accomodate all labels
261  */
263  getColoredCloud () const;
264 
265  /** \brief Returns a deep copy of the voxel centroid cloud */
267  getVoxelCentroidCloud () const;
268 
269  /** \brief Returns labeled cloud
270  * Points that belong to the same supervoxel have the same label.
271  * Labels for segments start from 1, unlabled points have label 0
272  */
274  getLabeledCloud () const;
275 
276  /** \brief Returns an RGB colorized voxelized cloud showing superpixels
277  * Otherwise it returns an empty pointer.
278  * Points that belong to the same supervoxel have the same color.
279  * But this function doesn't guarantee that different segments will have different
280  * color(it's random). Points that are unlabeled will be black
281  * \note This will expand the label_colors_ vector so that it can accomodate all labels
282  */
284  getColoredVoxelCloud () const;
285 
286  /** \brief Returns labeled voxelized cloud
287  * Points that belong to the same supervoxel have the same label.
288  * Labels for segments start from 1, unlabled points have label 0
289  */
291  getLabeledVoxelCloud () const;
292 
293  /** \brief Gets the adjacency list (Boost Graph library) which gives connections between supervoxels
294  * \param[out] adjacency_list_arg BGL graph where supervoxel labels are vertices, edges are touching relationships
295  */
296  void
297  getSupervoxelAdjacencyList (VoxelAdjacencyList &adjacency_list_arg) const;
298 
299  /** \brief Get a multimap which gives supervoxel adjacency
300  * \param[out] label_adjacency Multi-Map which maps a supervoxel label to all adjacent supervoxel labels
301  */
302  void
303  getSupervoxelAdjacency (std::multimap<uint32_t, uint32_t> &label_adjacency) const;
304 
305  /** \brief Static helper function which returns a pointcloud of normals for the input supervoxels
306  * \param[in] supervoxel_clusters Supervoxel cluster map coming from this class
307  * \returns Cloud of PointNormals of the supervoxels
308  *
309  */
311  makeSupervoxelNormalCloud (std::map<uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters);
312 
313  /** \brief Returns the current maximum (highest) label */
314  int
315  getMaxLabel () const;
316 
317  private:
318 
319  /** \brief This method initializes the label_colors_ vector (assigns random colors to labels)
320  * \note Checks to see if it is already big enough - if so, does not reinitialize it
321  */
322  void
323  initializeLabelColors ();
324 
325  /** \brief This method simply checks if it is possible to execute the segmentation algorithm with
326  * the current settings. If it is possible then it returns true.
327  */
328  virtual bool
329  prepareForSegmentation ();
330 
331  /** \brief This selects points to use as initial supervoxel centroids
332  * \param[out] seed_points The selected points
333  */
334  void
335  selectInitialSupervoxelSeeds (std::vector<PointT, Eigen::aligned_allocator<PointT> > &seed_points);
336 
337  /** \brief This method creates the internal supervoxel helpers based on the provided seed points
338  * \param[in] seed_points The selected points
339  */
340  void
341  createSupervoxelHelpers (std::vector<PointT, Eigen::aligned_allocator<PointT> > &seed_points);
342 
343  /** \brief This performs the superpixel evolution */
344  void
345  expandSupervoxels (int depth);
346 
347  /** \brief This sets the data of the voxels in the tree */
348  void
349  computeVoxelData ();
350 
351  /** \brief Reseeds the supervoxels by finding the voxel closest to current centroid */
352  void
353  reseedSupervoxels ();
354 
355  /** \brief Constructs the map of supervoxel clusters from the internal supervoxel helpers */
356  void
357  makeSupervoxels (std::map<uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters);
358 
359  /** \brief Stores the resolution used in the octree */
360  float resolution_;
361 
362  /** \brief Stores the resolution used to seed the superpixels */
363  float seed_resolution_;
364 
365  /** \brief Distance function used for comparing voxelDatas */
366  float
367  voxelDataDistance (const VoxelData &v1, const VoxelData &v2) const;
368 
369  /** \brief Transform function used to normalize voxel density versus distance from camera */
370  void
371  transformFunction (PointT &p);
372 
373  /** \brief Contains a KDtree for the voxelized cloud */
374  typename pcl::search::KdTree<PointT>::Ptr voxel_kdtree_;
375 
376  /** \brief Octree Adjacency structure with leaves at voxel resolution */
377  typename OctreeAdjacencyT::Ptr adjacency_octree_;
378 
379  /** \brief Contains the Voxelized centroid Cloud */
380  typename PointCloudT::Ptr voxel_centroid_cloud_;
381 
382  /** \brief Contains the Voxelized centroid Cloud */
383  typename NormalCloudT::ConstPtr input_normals_;
384 
385  /** \brief Importance of color in clustering */
386  float color_importance_;
387  /** \brief Importance of distance from seed center in clustering */
388  float spatial_importance_;
389  /** \brief Importance of similarity in normals for clustering */
390  float normal_importance_;
391 
392  /** \brief Stores the colors used for the superpixel labels*/
393  std::vector<uint32_t> label_colors_;
394 
395  /** \brief Internal storage class for supervoxels
396  * \note Stores pointers to leaves of clustering internal octree,
397  * \note so should not be used outside of clustering class
398  */
399  class SupervoxelHelper
400  {
401  public:
402  SupervoxelHelper (uint32_t label, SupervoxelClustering* parent_arg):
403  label_ (label),
404  parent_ (parent_arg)
405  { }
406 
407  void
408  addLeaf (LeafContainerT* leaf_arg);
409 
410  void
411  removeLeaf (LeafContainerT* leaf_arg);
412 
413  void
414  removeAllLeaves ();
415 
416  void
417  expand ();
418 
419  void
420  refineNormals ();
421 
422  void
423  updateCentroid ();
424 
425  void
426  getVoxels (typename pcl::PointCloud<PointT>::Ptr &voxels) const;
427 
428  void
429  getNormals (typename pcl::PointCloud<Normal>::Ptr &normals) const;
430 
431  typedef float (SupervoxelClustering::*DistFuncPtr)(const VoxelData &v1, const VoxelData &v2);
432 
433  uint32_t
434  getLabel () const
435  { return label_; }
436 
437  Eigen::Vector4f
438  getNormal () const
439  { return centroid_.normal_; }
440 
441  Eigen::Vector3f
442  getRGB () const
443  { return centroid_.rgb_; }
444 
445  Eigen::Vector3f
446  getXYZ () const
447  { return centroid_.xyz_;}
448 
449  void
450  getXYZ (float &x, float &y, float &z) const
451  { x=centroid_.xyz_[0]; y=centroid_.xyz_[1]; z=centroid_.xyz_[2]; }
452 
453  void
454  getRGB (uint32_t &rgba) const
455  {
456  rgba = static_cast<uint32_t>(centroid_.rgb_[0]) << 16 |
457  static_cast<uint32_t>(centroid_.rgb_[1]) << 8 |
458  static_cast<uint32_t>(centroid_.rgb_[2]);
459  }
460 
461  void
462  getNormal (pcl::Normal &normal_arg) const
463  {
464  normal_arg.normal_x = centroid_.normal_[0];
465  normal_arg.normal_y = centroid_.normal_[1];
466  normal_arg.normal_z = centroid_.normal_[2];
467  normal_arg.curvature = centroid_.curvature_;
468  }
469 
470  void
471  getNeighborLabels (std::set<uint32_t> &neighbor_labels) const;
472 
473  VoxelData
474  getCentroid () const
475  { return centroid_; }
476 
477 
478  size_t
479  size () const { return leaves_.size (); }
480  private:
481  //Stores leaves
482  std::set<LeafContainerT*> leaves_;
483  uint32_t label_;
484  VoxelData centroid_;
485  SupervoxelClustering* parent_;
486 
487 
488  };
489 
490  typedef boost::ptr_list<SupervoxelHelper> HelperListT;
491  HelperListT supervoxel_helpers_;
492 
493  //TODO DEBUG REMOVE
494  StopWatch timer_;
495  public:
496  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
497 
498 
499 
500  };
501 
502 }
503 
504 #ifdef PCL_NO_PRECOMPILE
505 #include <pcl/segmentation/impl/supervoxel_clustering.hpp>
506 #endif
507 
508 #endif
Implements a supervoxel algorithm based on voxel structure, normals, and rgb values.
pcl::PointCloud< Normal >::Ptr normals_
A Pointcloud of the normals for the points in the supervoxel.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
VoxelData is a structure used for storing data within a pcl::octree::OctreePointCloudAdjacencyContain...
void getCentroidPoint(PointXYZRGBA &centroid_arg)
Gets the centroid of the supervoxel.
pcl::PointCloud< Normal > NormalCloudT
Octree pointcloud voxel class used for adjacency calculation
pcl::octree::OctreePointCloudAdjacencyContainer< PointT, VoxelData > LeafContainerT
pcl::Normal normal_
The normal calculated for the voxels contained in the supervoxel.
Supervoxel container class - stores a cluster extracted using supervoxel clustering.
boost::shared_ptr< OctreeAdjacencyT > Ptr
pcl::PointCloud< PointT > PointCloudT
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition: kdtree.h:62
pcl::octree::OctreePointCloudSearch< PointT > OctreeSearchT
VoxelAdjacencyList::vertex_descriptor VoxelID
boost::shared_ptr< std::vector< int > > IndicesPtr
PCL base class.
Definition: pcl_base.h:68
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
A point structure representing normal coordinates and the surface curvature estimate.
boost::shared_ptr< KdTree< PointT > > Ptr
Definition: kdtree.h:79
boost::adjacency_list< boost::setS, boost::setS, boost::undirectedS, uint32_t, float > VoxelAdjacencyList
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
A point structure representing Euclidean xyz coordinates, and the RGBA color.
Octree pointcloud search class
Definition: octree_search.h:58
pcl::octree::OctreePointCloudAdjacency< PointT, LeafContainerT > OctreeAdjacencyT
std::vector< LeafContainerT * > LeafVectorT
boost::shared_ptr< Supervoxel< PointT > > Ptr
pcl::search::KdTree< PointT > KdTreeT
Octree adjacency leaf container class- stores set of pointers to neighbors, number of points added...
void getCentroidPointNormal(PointNormal &normal_arg)
Gets the point normal for the supervoxel.
pcl::PointXYZRGBA centroid_
The centroid of the supervoxel - average voxel.
A point structure representing Euclidean xyz coordinates, together with normal coordinates and the su...
boost::shared_ptr< const Supervoxel< PointT > > ConstPtr
A point structure representing Euclidean xyz coordinates, and the RGB color.
VoxelAdjacencyList::edge_descriptor EdgeID
pcl::PointCloud< PointT >::Ptr voxels_
A Pointcloud of the voxels in the supervoxel.