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