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