Point Cloud Library (PCL)  1.11.1-dev
euclidean_cluster_comparator.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, Willow Garage, Inc.
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 the copyright holder(s) 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  *
37  *
38  */
39 
40 #pragma once
41 
42 #include <pcl/memory.h>
43 #include <pcl/pcl_macros.h>
44 #include <pcl/point_types.h>
45 #include <pcl/segmentation/boost.h>
46 #include <pcl/segmentation/comparator.h>
47 
48 
49 namespace pcl
50 {
51  /** \brief EuclideanClusterComparator is a comparator used for finding clusters based on euclidian distance.
52  *
53  * \author Alex Trevor
54  */
55  template<typename PointT, typename PointLT = pcl::Label>
57  {
58  protected:
59 
61 
62  public:
63  using typename Comparator<PointT>::PointCloud;
65 
67  using PointCloudLPtr = typename PointCloudL::Ptr;
69 
70  using Ptr = shared_ptr<EuclideanClusterComparator<PointT, PointLT> >;
71  using ConstPtr = shared_ptr<const EuclideanClusterComparator<PointT, PointLT> >;
72 
73  using ExcludeLabelSet = std::set<std::uint32_t>;
74  using ExcludeLabelSetPtr = shared_ptr<ExcludeLabelSet>;
75  using ExcludeLabelSetConstPtr = shared_ptr<const ExcludeLabelSet>;
76 
77  /** \brief Default constructor for EuclideanClusterComparator. */
78  EuclideanClusterComparator() = default;
79 
80  void
81  setInputCloud (const PointCloudConstPtr& cloud) override
82  {
83  input_ = cloud;
84  Eigen::Matrix3f rot = input_->sensor_orientation_.toRotationMatrix ();
85  z_axis_ = rot.col (2);
86  }
87 
88  /** \brief Set the tolerance in meters for difference in perpendicular distance (d component of plane equation) to the plane between neighboring points, to be considered part of the same plane.
89  * \param[in] distance_threshold the tolerance in meters
90  * \param depth_dependent
91  */
92  inline void
93  setDistanceThreshold (float distance_threshold, bool depth_dependent)
94  {
95  distance_threshold_ = distance_threshold;
96  depth_dependent_ = depth_dependent;
97  }
98 
99  /** \brief Get the distance threshold in meters (d component of plane equation) between neighboring points, to be considered part of the same plane. */
100  inline float
102  {
103  return distance_threshold_;
104  }
105 
106  /** \brief Get if depth dependent */
107  inline bool
109  {
110  return depth_dependent_;
111  }
112 
113  /** \brief Set label cloud
114  * \param[in] labels The label cloud
115  */
116  void
117  setLabels (const PointCloudLPtr& labels)
118  {
119  labels_ = labels;
120  }
121 
122  /** \brief Get labels */
123  const PointCloudLPtr&
124  getLabels() const
125  {
126  return labels_;
127  }
128 
129  /** \brief Get exlude labels */
132  {
133  return exclude_labels_;
134  }
135 
136  /** \brief Set labels in the label cloud to exclude.
137  * \param exclude_labels a vector of bools corresponding to whether or not a given label should be considered
138  */
139  void
141  {
142  exclude_labels_ = exclude_labels;
143  }
144 
145  /** \brief Compare points at two indices by their euclidean distance
146  * \param idx1 The first index for the comparison
147  * \param idx2 The second index for the comparison
148  */
149  bool
150  compare (int idx1, int idx2) const override
151  {
152  if (labels_ && exclude_labels_)
153  {
154  assert (labels_->size () == input_->size ());
155  const std::uint32_t &label1 = (*labels_)[idx1].label;
156  const std::uint32_t &label2 = (*labels_)[idx2].label;
157 
158  const std::set<std::uint32_t>::const_iterator it1 = exclude_labels_->find (label1);
159  if (it1 == exclude_labels_->end ())
160  return false;
161 
162  const std::set<std::uint32_t>::const_iterator it2 = exclude_labels_->find (label2);
163  if (it2 == exclude_labels_->end ())
164  return false;
165  }
166 
167  float dist_threshold = distance_threshold_;
168  if (depth_dependent_)
169  {
170  Eigen::Vector3f vec = (*input_)[idx1].getVector3fMap ();
171  float z = vec.dot (z_axis_);
172  dist_threshold *= z * z;
173  }
174 
175  const float dist = ((*input_)[idx1].getVector3fMap ()
176  - (*input_)[idx2].getVector3fMap ()).norm ();
177  return (dist < dist_threshold);
178  }
179 
180  protected:
181 
182 
183  /** \brief Set of labels with similar size as the input point cloud,
184  * aggregating points into groups based on a similar label identifier.
185  *
186  * It needs to be set in conjunction with the \ref exclude_labels_
187  * member in order to provided a masking functionality.
188  */
190 
191  /** \brief Specifies which labels should be excluded com being clustered.
192  *
193  * If a label is not specified, it's assumed by default that it's
194  * intended be excluded
195  */
197 
198  float distance_threshold_ = 0.005f;
199 
200  bool depth_dependent_ = false;
201 
202  Eigen::Vector3f z_axis_;
203  };
204 }
pcl::EuclideanClusterComparator::getExcludeLabels
const ExcludeLabelSetConstPtr & getExcludeLabels() const
Get exlude labels.
Definition: euclidean_cluster_comparator.h:131
pcl::EuclideanClusterComparator::getDepthDependent
bool getDepthDependent() const
Get if depth dependent.
Definition: euclidean_cluster_comparator.h:108
pcl_macros.h
Defines all the PCL and non-PCL macros used.
pcl
Definition: convolution.h:46
point_types.h
pcl::EuclideanClusterComparator::z_axis_
Eigen::Vector3f z_axis_
Definition: euclidean_cluster_comparator.h:202
pcl::EuclideanClusterComparator::setInputCloud
void setInputCloud(const PointCloudConstPtr &cloud) override
Set the input cloud for the comparator.
Definition: euclidean_cluster_comparator.h:81
pcl::EuclideanClusterComparator::EuclideanClusterComparator
EuclideanClusterComparator()=default
Default constructor for EuclideanClusterComparator.
pcl::EuclideanClusterComparator::getLabels
const PointCloudLPtr & getLabels() const
Get labels.
Definition: euclidean_cluster_comparator.h:124
pcl::EuclideanClusterComparator::exclude_labels_
ExcludeLabelSetConstPtr exclude_labels_
Specifies which labels should be excluded com being clustered.
Definition: euclidean_cluster_comparator.h:196
pcl::EuclideanClusterComparator::ExcludeLabelSetPtr
shared_ptr< ExcludeLabelSet > ExcludeLabelSetPtr
Definition: euclidean_cluster_comparator.h:74
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: distances.h:55
pcl::EuclideanClusterComparator::depth_dependent_
bool depth_dependent_
Definition: euclidean_cluster_comparator.h:200
pcl::Comparator::ConstPtr
shared_ptr< const Comparator< PointT > > ConstPtr
Definition: comparator.h:62
pcl::EuclideanClusterComparator::ExcludeLabelSet
std::set< std::uint32_t > ExcludeLabelSet
Definition: euclidean_cluster_comparator.h:73
pcl::EuclideanClusterComparator::distance_threshold_
float distance_threshold_
Definition: euclidean_cluster_comparator.h:198
pcl::EuclideanClusterComparator::getDistanceThreshold
float getDistanceThreshold() const
Get the distance threshold in meters (d component of plane equation) between neighboring points,...
Definition: euclidean_cluster_comparator.h:101
pcl::EuclideanClusterComparator::setExcludeLabels
void setExcludeLabels(const ExcludeLabelSetConstPtr &exclude_labels)
Set labels in the label cloud to exclude.
Definition: euclidean_cluster_comparator.h:140
pcl::EuclideanClusterComparator::setDistanceThreshold
void setDistanceThreshold(float distance_threshold, bool depth_dependent)
Set the tolerance in meters for difference in perpendicular distance (d component of plane equation) ...
Definition: euclidean_cluster_comparator.h:93
pcl::EuclideanClusterComparator::compare
bool compare(int idx1, int idx2) const override
Compare points at two indices by their euclidean distance.
Definition: euclidean_cluster_comparator.h:150
pcl::EuclideanClusterComparator::setLabels
void setLabels(const PointCloudLPtr &labels)
Set label cloud.
Definition: euclidean_cluster_comparator.h:117
pcl::EuclideanClusterComparator::labels_
PointCloudLPtr labels_
Set of labels with similar size as the input point cloud, aggregating points into groups based on a s...
Definition: euclidean_cluster_comparator.h:189
pcl::EuclideanClusterComparator::ExcludeLabelSetConstPtr
shared_ptr< const ExcludeLabelSet > ExcludeLabelSetConstPtr
Definition: euclidean_cluster_comparator.h:75
pcl::Comparator
Comparator is the base class for comparators that compare two points given some function.
Definition: comparator.h:54
pcl::EuclideanClusterComparator
EuclideanClusterComparator is a comparator used for finding clusters based on euclidian distance.
Definition: euclidean_cluster_comparator.h:56
pcl::EuclideanClusterComparator::PointCloudLConstPtr
typename PointCloudL::ConstPtr PointCloudLConstPtr
Definition: euclidean_cluster_comparator.h:68
pcl::Comparator::PointCloudConstPtr
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: comparator.h:59
pcl::PointCloud::Ptr
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:406
pcl::PointCloud::ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:407
pcl::Comparator::input_
PointCloudConstPtr input_
Definition: comparator.h:100
pcl::Comparator::Ptr
shared_ptr< Comparator< PointT > > Ptr
Definition: comparator.h:61
memory.h
Defines functions, macros and traits for allocating and using memory.
pcl::EuclideanClusterComparator::PointCloudLPtr
typename PointCloudL::Ptr PointCloudLPtr
Definition: euclidean_cluster_comparator.h:67