Point Cloud Library (PCL)  1.7.2
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
90  setInputCloud (const PointCloudConstPtr& cloud)
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
101  setInputNormals (const PointCloudNConstPtr &normals)
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  * \param depth_dependent
132  */
133  inline void
134  setDistanceThreshold (float distance_threshold, bool depth_dependent)
135  {
136  distance_threshold_ = distance_threshold;
137  depth_dependent_ = depth_dependent;
138  }
139 
140  /** \brief Get the distance threshold in meters (d component of plane equation) between neighboring points, to be considered part of the same plane. */
141  inline float
143  {
144  return (distance_threshold_);
145  }
146 
147  /** \brief Set label cloud
148  * \param[in] labels The label cloud
149  */
150  void
151  setLabels (PointCloudLPtr& labels)
152  {
153  labels_ = labels;
154  }
155 
156  /** \brief Set labels in the label cloud to exclude.
157  * \param exclude_labels a vector of bools corresponding to whether or not a given label should be considered
158  */
159  void
160  setExcludeLabels (std::vector<bool>& exclude_labels)
161  {
162  exclude_labels_ = boost::make_shared<std::vector<bool> >(exclude_labels);
163  }
164 
165  /** \brief Compare points at two indices by their plane equations. True if the angle between the normals is less than the angular threshold,
166  * and the difference between the d component of the normals is less than distance threshold, else false
167  * \param idx1 The first index for the comparison
168  * \param idx2 The second index for the comparison
169  */
170  virtual bool
171  compare (int idx1, int idx2) const
172  {
173  int label1 = labels_->points[idx1].label;
174  int label2 = labels_->points[idx2].label;
175 
176  if (label1 == -1 || label2 == -1)
177  return false;
178 
179  if ( (*exclude_labels_)[label1] || (*exclude_labels_)[label2])
180  return false;
181 
182  float dist_threshold = distance_threshold_;
183  if (depth_dependent_)
184  {
185  Eigen::Vector3f vec = input_->points[idx1].getVector3fMap ();
186  float z = vec.dot (z_axis_);
187  dist_threshold *= z * z;
188  }
189 
190  float dx = input_->points[idx1].x - input_->points[idx2].x;
191  float dy = input_->points[idx1].y - input_->points[idx2].y;
192  float dz = input_->points[idx1].z - input_->points[idx2].z;
193  float dist = sqrtf (dx*dx + dy*dy + dz*dz);
194 
195  return (dist < dist_threshold);
196  }
197 
198  protected:
199  PointCloudNConstPtr normals_;
200  PointCloudLPtr labels_;
201 
202  boost::shared_ptr<std::vector<bool> > exclude_labels_;
206  Eigen::Vector3f z_axis_;
207  };
208 }
209 
210 #endif // PCL_SEGMENTATION_PLANE_COEFFICIENT_COMPARATOR_H_
PointCloud::ConstPtr PointCloudConstPtr
Definition: comparator.h:58
boost::shared_ptr< PointCloud< PointNT > > Ptr
Definition: point_cloud.h:428
float getAngularThreshold() const
Get the angular threshold in radians for difference in normal direction between neighboring points...
Comparator< PointT >::PointCloudConstPtr PointCloudConstPtr
virtual bool compare(int idx1, int idx2) const
Compare points at two indices by their plane equations.
void setExcludeLabels(std::vector< bool > &exclude_labels)
Set labels in the label cloud to exclude.
PointCloudNConstPtr getInputNormals() const
Get the input normals.
Comparator is the base class for comparators that compare two points given some function.
Definition: comparator.h:53
boost::shared_ptr< const PointCloud< PointNT > > ConstPtr
Definition: point_cloud.h:429
boost::shared_ptr< const EuclideanClusterComparator< PointT, PointNT, PointLT > > ConstPtr
void setDistanceThreshold(float distance_threshold, bool depth_dependent)
Set the tolerance in meters for difference in perpendicular distance (d component of plane equation) ...
virtual ~EuclideanClusterComparator()
Destructor for EuclideanClusterComparator.
virtual void setAngularThreshold(float angular_threshold)
Set the tolerance in radians for difference in normal direction between neighboring points...
EuclideanClusterComparator()
Empty constructor for EuclideanClusterComparator.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Set the input cloud for the comparator.
boost::shared_ptr< std::vector< bool > > exclude_labels_
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
PointCloudConstPtr input_
Definition: comparator.h:99
EuclideanClusterComparator is a comparator used for finding clusters supported by planar surfaces...
void setLabels(PointCloudLPtr &labels)
Set label cloud.
Comparator< PointT >::PointCloud PointCloud
void setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input normals.