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