Point Cloud Library (PCL)  1.7.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 #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 
46 namespace pcl
47 {
48  /** \brief EuclideanClusterComparator is a comparator used for finding clusters supported by planar surfaces.
49  * This needs to be run as a second pass after extracting planar surfaces, using MultiPlaneSegmentation for example.
50  *
51  * \author Alex Trevor
52  */
53  template<typename PointT, typename PointNT, typename PointLT>
54  class EuclideanClusterComparator: public Comparator<PointT>
55  {
56  public:
59 
61  typedef typename PointCloudN::Ptr PointCloudNPtr;
63 
65  typedef typename PointCloudL::Ptr PointCloudLPtr;
67 
68  typedef boost::shared_ptr<EuclideanClusterComparator<PointT, PointNT, PointLT> > Ptr;
69  typedef boost::shared_ptr<const EuclideanClusterComparator<PointT, PointNT, PointLT> > ConstPtr;
70 
72 
73  /** \brief Empty constructor for EuclideanClusterComparator. */
75  : normals_ ()
76  , angular_threshold_ (0.0f)
77  , distance_threshold_ (0.005f)
78  , depth_dependent_ ()
79  , z_axis_ ()
80  {
81  }
82 
83  /** \brief Destructor for EuclideanClusterComparator. */
84  virtual
86  {
87  }
88 
89  virtual void
91  {
92  input_ = cloud;
93  Eigen::Matrix3f rot = input_->sensor_orientation_.toRotationMatrix ();
94  z_axis_ = rot.col (2);
95  }
96 
97  /** \brief Provide a pointer to the input normals.
98  * \param[in] normals the input normal cloud
99  */
100  inline void
102  {
103  normals_ = normals;
104  }
105 
106  /** \brief Get the input normals. */
107  inline PointCloudNConstPtr
109  {
110  return (normals_);
111  }
112 
113  /** \brief Set the tolerance in radians for difference in normal direction between neighboring points, to be considered part of the same plane.
114  * \param[in] angular_threshold the tolerance in radians
115  */
116  virtual inline void
117  setAngularThreshold (float angular_threshold)
118  {
119  angular_threshold_ = cosf (angular_threshold);
120  }
121 
122  /** \brief Get the angular threshold in radians for difference in normal direction between neighboring points, to be considered part of the same plane. */
123  inline float
125  {
126  return (acos (angular_threshold_) );
127  }
128 
129  /** \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.
130  * \param[in] distance_threshold the tolerance in meters
131  */
132  inline void
133  setDistanceThreshold (float distance_threshold, bool depth_dependent)
134  {
135  distance_threshold_ = distance_threshold;
136  depth_dependent_ = depth_dependent;
137  }
138 
139  /** \brief Get the distance threshold in meters (d component of plane equation) between neighboring points, to be considered part of the same plane. */
140  inline float
142  {
143  return (distance_threshold_);
144  }
145 
146  /** \brief Set label cloud
147  * \param[in] labels The label cloud
148  */
149  void
151  {
152  labels_ = labels;
153  }
154 
155  /** \brief Set labels in the label cloud to exclude.
156  * \param exclude_labels a vector of bools corresponding to whether or not a given label should be considered
157  */
158  void
159  setExcludeLabels (std::vector<bool>& exclude_labels)
160  {
161  exclude_labels_ = boost::make_shared<std::vector<bool> >(exclude_labels);
162  }
163 
164  /** \brief Compare points at two indices by their plane equations. True if the angle between the normals is less than the angular threshold,
165  * and the difference between the d component of the normals is less than distance threshold, else false
166  * \param idx1 The first index for the comparison
167  * \param idx2 The second index for the comparison
168  */
169  virtual bool
170  compare (int idx1, int idx2) const
171  {
172  int label1 = labels_->points[idx1].label;
173  int label2 = labels_->points[idx2].label;
174 
175  if (label1 == -1 || label2 == -1)
176  return false;
177 
178  if ( (*exclude_labels_)[label1] || (*exclude_labels_)[label2])
179  return false;
180 
181  float dist_threshold = distance_threshold_;
182  if (depth_dependent_)
183  {
184  Eigen::Vector3f vec = input_->points[idx1].getVector3fMap ();
185  float z = vec.dot (z_axis_);
186  dist_threshold *= z * z;
187  }
188 
189  float dx = input_->points[idx1].x - input_->points[idx2].x;
190  float dy = input_->points[idx1].y - input_->points[idx2].y;
191  float dz = input_->points[idx1].z - input_->points[idx2].z;
192  float dist = sqrtf (dx*dx + dy*dy + dz*dz);
193 
194  return (dist < dist_threshold);
195  }
196 
197  protected:
200 
201  boost::shared_ptr<std::vector<bool> > exclude_labels_;
205  Eigen::Vector3f z_axis_;
206  };
207 }
208 
209 #endif // PCL_SEGMENTATION_PLANE_COEFFICIENT_COMPARATOR_H_