Point Cloud Library (PCL)  1.10.0
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/segmentation/boost.h>
43 #include <pcl/segmentation/comparator.h>
44 #include <pcl/point_types.h>
45 
46 namespace pcl
47 {
48  namespace experimental
49  {
50  template<typename PointT, typename PointLT = pcl::Label>
52  {
53  protected:
54 
56 
57  public:
58  using typename Comparator<PointT>::PointCloud;
60 
62  using PointCloudLPtr = typename PointCloudL::Ptr;
64 
67 
68  using ExcludeLabelSet = std::set<std::uint32_t>;
71 
72  /** \brief Default constructor for EuclideanClusterComparator. */
74  : distance_threshold_ (0.005f)
75  , depth_dependent_ ()
76  {}
77 
78  void
79  setInputCloud (const PointCloudConstPtr& cloud) override
80  {
81  input_ = cloud;
82  Eigen::Matrix3f rot = input_->sensor_orientation_.toRotationMatrix ();
83  z_axis_ = rot.col (2);
84  }
85 
86  /** \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.
87  * \param[in] distance_threshold the tolerance in meters
88  * \param depth_dependent
89  */
90  inline void
91  setDistanceThreshold (float distance_threshold, bool depth_dependent)
92  {
93  distance_threshold_ = distance_threshold;
94  depth_dependent_ = depth_dependent;
95  }
96 
97  /** \brief Get the distance threshold in meters (d component of plane equation) between neighboring points, to be considered part of the same plane. */
98  inline float
100  {
101  return (distance_threshold_);
102  }
103 
104  /** \brief Set label cloud
105  * \param[in] labels The label cloud
106  */
107  void
108  setLabels (const PointCloudLPtr& labels)
109  {
110  labels_ = labels;
111  }
112 
115  {
116  return exclude_labels_;
117  }
118 
119  /** \brief Set labels in the label cloud to exclude.
120  * \param exclude_labels a vector of bools corresponding to whether or not a given label should be considered
121  */
122  void
124  {
125  exclude_labels_ = exclude_labels;
126  }
127 
128  /** \brief Compare points at two indices by their euclidean distance
129  * \param idx1 The first index for the comparison
130  * \param idx2 The second index for the comparison
131  */
132  bool
133  compare (int idx1, int idx2) const override
134  {
135  if (labels_ && exclude_labels_)
136  {
137  assert (labels_->size () == input_->size ());
138  const std::uint32_t &label1 = (*labels_)[idx1].label;
139  const std::uint32_t &label2 = (*labels_)[idx2].label;
140 
141  const std::set<std::uint32_t>::const_iterator it1 = exclude_labels_->find (label1);
142  if (it1 == exclude_labels_->end ())
143  return false;
144 
145  const std::set<std::uint32_t>::const_iterator it2 = exclude_labels_->find (label2);
146  if (it2 == exclude_labels_->end ())
147  return false;
148  }
149 
150  float dist_threshold = distance_threshold_;
151  if (depth_dependent_)
152  {
153  Eigen::Vector3f vec = input_->points[idx1].getVector3fMap ();
154  float z = vec.dot (z_axis_);
155  dist_threshold *= z * z;
156  }
157 
158  const float dist = ((*input_)[idx1].getVector3fMap ()
159  - (*input_)[idx2].getVector3fMap ()).norm ();
160  return (dist < dist_threshold);
161  }
162 
163  protected:
164 
165 
166  /** \brief Set of labels with similar size as the input point cloud,
167  * aggregating points into groups based on a similar label identifier.
168  *
169  * It needs to be set in conjunction with the \ref exclude_labels_
170  * member in order to provided a masking functionality.
171  */
173 
174  /** \brief Specifies which labels should be excluded com being clustered.
175  *
176  * If a label is not specified, it's assumed by default that it's
177  * intended be excluded
178  */
180 
182 
184 
185  Eigen::Vector3f z_axis_;
186  };
187  } // namespace experimental
188 
189 
190  /** \brief EuclideanClusterComparator is a comparator used for finding clusters based on euclidian distance.
191  *
192  * \author Alex Trevor
193  */
194  template<typename PointT, typename PointNT, typename PointLT = deprecated::T>
196  {
197  protected:
198 
200 
201  public:
202 
206 
209 
211 
212  /** \brief Default constructor for EuclideanClusterComparator. */
213  [[deprecated("remove PointNT from template parameters")]]
215  : normals_ ()
216  , angular_threshold_ (0.0f)
217  {}
218 
219  /** \brief Provide a pointer to the input normals.
220  * \param[in] normals the input normal cloud
221  */
222  [[deprecated("EuclideadClusterComparator never actually used normals and angular threshold, this function has no effect on the behavior of the comparator. It is deprecated and will be removed in future releases.")]]
223  inline void
224  setInputNormals (const PointCloudNConstPtr& normals) { normals_ = normals; }
225 
226  /** \brief Get the input normals. */
227  [[deprecated("EuclideadClusterComparator never actually used normals and angular threshold, this function has no effect on the behavior of the comparator. It is deprecated and will be removed in future releases.")]]
228  inline PointCloudNConstPtr
229  getInputNormals () const { return (normals_); }
230 
231  /** \brief Set the tolerance in radians for difference in normal direction between neighboring points, to be considered part of the same plane.
232  * \param[in] angular_threshold the tolerance in radians
233  */
234  [[deprecated("EuclideadClusterComparator never actually used normals and angular threshold, this function has no effect on the behavior of the comparator. It is deprecated and will be removed in future releases.")]]
235  inline void
236  setAngularThreshold (float angular_threshold)
237  {
238  angular_threshold_ = std::cos (angular_threshold);
239  }
240 
241  /** \brief Get the angular threshold in radians for difference in normal direction between neighboring points, to be considered part of the same plane. */
242  [[deprecated("EuclideadClusterComparator never actually used normals and angular threshold, this function has no effect on the behavior of the comparator. It is deprecated and will be removed in future releases.")]]
243  inline float
244  getAngularThreshold () const { return (std::acos (angular_threshold_) ); }
245 
246  /** \brief Set labels in the label cloud to exclude.
247  * \param[in] exclude_labels a vector of bools corresponding to whether or not a given label should be considered
248  */
249  [[deprecated("use setExcludeLabels(const ExcludeLabelSetConstPtr &) instead")]]
250  void
251  setExcludeLabels (const std::vector<bool>& exclude_labels)
252  {
253  exclude_labels_ = boost::make_shared<std::set<std::uint32_t> > ();
254  for (std::size_t i = 0; i < exclude_labels.size (); ++i)
255  if (exclude_labels[i])
256  exclude_labels_->insert (i);
257  }
258 
259  protected:
260 
262 
264  };
265 
266  template<typename PointT, typename PointLT>
267  class EuclideanClusterComparator<PointT, PointLT, deprecated::T>
268  : public experimental::EuclideanClusterComparator<PointT, PointLT> {};
269 }
pcl
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
point_types.h
pcl::experimental::EuclideanClusterComparator::compare
bool compare(int idx1, int idx2) const override
Compare points at two indices by their euclidean distance.
Definition: euclidean_cluster_comparator.h:133
pcl::uint32_t
std::uint32_t uint32_t
Definition: pcl_macros.h:96
pcl::experimental::EuclideanClusterComparator::getDistanceThreshold
float getDistanceThreshold() const
Get the distance threshold in meters (d component of plane equation) between neighboring points,...
Definition: euclidean_cluster_comparator.h:99
pcl::EuclideanClusterComparator::PointCloudNConstPtr
typename PointCloudN::ConstPtr PointCloudNConstPtr
Definition: euclidean_cluster_comparator.h:205
pcl::experimental::EuclideanClusterComparator::ExcludeLabelSet
std::set< std::uint32_t > ExcludeLabelSet
Definition: euclidean_cluster_comparator.h:68
pcl::experimental::EuclideanClusterComparator::PointCloudLPtr
typename PointCloudL::Ptr PointCloudLPtr
Definition: euclidean_cluster_comparator.h:62
pcl::experimental::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:91
pcl::experimental::EuclideanClusterComparator::PointCloudLConstPtr
typename PointCloudL::ConstPtr PointCloudLConstPtr
Definition: euclidean_cluster_comparator.h:63
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: projection_matrix.h:52
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:623
pcl::Comparator::ConstPtr
shared_ptr< const Comparator< PointT > > ConstPtr
Definition: comparator.h:61
pcl::experimental::EuclideanClusterComparator::getExcludeLabels
const ExcludeLabelSetConstPtr & getExcludeLabels() const
Definition: euclidean_cluster_comparator.h:114
pcl::experimental::EuclideanClusterComparator
Definition: euclidean_cluster_comparator.h:51
pcl::experimental::EuclideanClusterComparator::z_axis_
Eigen::Vector3f z_axis_
Definition: euclidean_cluster_comparator.h:185
pcl::experimental::EuclideanClusterComparator::distance_threshold_
float distance_threshold_
Definition: euclidean_cluster_comparator.h:181
pcl::experimental::EuclideanClusterComparator::EuclideanClusterComparator
EuclideanClusterComparator()
Default constructor for EuclideanClusterComparator.
Definition: euclidean_cluster_comparator.h:73
pcl::experimental::EuclideanClusterComparator::exclude_labels_
ExcludeLabelSetConstPtr exclude_labels_
Specifies which labels should be excluded com being clustered.
Definition: euclidean_cluster_comparator.h:179
pcl::experimental::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:172
pcl::experimental::EuclideanClusterComparator::setExcludeLabels
void setExcludeLabels(const ExcludeLabelSetConstPtr &exclude_labels)
Set labels in the label cloud to exclude.
Definition: euclidean_cluster_comparator.h:123
pcl::experimental::EuclideanClusterComparator::ExcludeLabelSetConstPtr
shared_ptr< const ExcludeLabelSet > ExcludeLabelSetConstPtr
Definition: euclidean_cluster_comparator.h:70
pcl::experimental::EuclideanClusterComparator::depth_dependent_
bool depth_dependent_
Definition: euclidean_cluster_comparator.h:183
pcl::Comparator
Comparator is the base class for comparators that compare two points given some function.
Definition: comparator.h:53
pcl::EuclideanClusterComparator::getAngularThreshold
float getAngularThreshold() const
Get the angular threshold in radians for difference in normal direction between neighboring points,...
Definition: euclidean_cluster_comparator.h:244
pcl::experimental::EuclideanClusterComparator::setInputCloud
void setInputCloud(const PointCloudConstPtr &cloud) override
Set the input cloud for the comparator.
Definition: euclidean_cluster_comparator.h:79
pcl::EuclideanClusterComparator
EuclideanClusterComparator is a comparator used for finding clusters based on euclidian distance.
Definition: euclidean_cluster_comparator.h:195
pcl::Comparator::PointCloudConstPtr
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: comparator.h:58
pcl::PointCloud::Ptr
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:415
pcl::EuclideanClusterComparator::PointCloudNPtr
typename PointCloudN::Ptr PointCloudNPtr
Definition: euclidean_cluster_comparator.h:204
pcl::EuclideanClusterComparator::getInputNormals
PointCloudNConstPtr getInputNormals() const
Get the input normals.
Definition: euclidean_cluster_comparator.h:229
pcl::EuclideanClusterComparator::setExcludeLabels
void setExcludeLabels(const std::vector< bool > &exclude_labels)
Set labels in the label cloud to exclude.
Definition: euclidean_cluster_comparator.h:251
pcl::experimental::EuclideanClusterComparator::ExcludeLabelSetPtr
shared_ptr< ExcludeLabelSet > ExcludeLabelSetPtr
Definition: euclidean_cluster_comparator.h:69
pcl::EuclideanClusterComparator::EuclideanClusterComparator
EuclideanClusterComparator()
Default constructor for EuclideanClusterComparator.
Definition: euclidean_cluster_comparator.h:214
pcl::PointCloud::ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:416
pcl::EuclideanClusterComparator::setAngularThreshold
void setAngularThreshold(float angular_threshold)
Set the tolerance in radians for difference in normal direction between neighboring points,...
Definition: euclidean_cluster_comparator.h:236
pcl::EuclideanClusterComparator::angular_threshold_
float angular_threshold_
Definition: euclidean_cluster_comparator.h:263
pcl::Comparator::input_
PointCloudConstPtr input_
Definition: comparator.h:99
pcl::Comparator::Ptr
shared_ptr< Comparator< PointT > > Ptr
Definition: comparator.h:60
pcl::EuclideanClusterComparator::normals_
PointCloudNConstPtr normals_
Definition: euclidean_cluster_comparator.h:261
pcl::EuclideanClusterComparator::setInputNormals
void setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input normals.
Definition: euclidean_cluster_comparator.h:224
pcl::shared_ptr
boost::shared_ptr< T > shared_ptr
Alias for boost::shared_ptr.
Definition: pcl_macros.h:90
pcl::experimental::EuclideanClusterComparator::setLabels
void setLabels(const PointCloudLPtr &labels)
Set label cloud.
Definition: euclidean_cluster_comparator.h:108