Point Cloud Library (PCL)  1.7.2
edge_aware_plane_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  * $Id: extract_clusters.h 5027 2012-03-12 03:10:45Z rusu $
37  *
38  */
39 
40 #ifndef PCL_SEGMENTATION_EDGE_AWARE_PLANE_COMPARATOR_H_
41 #define PCL_SEGMENTATION_EDGE_AWARE_PLANE_COMPARATOR_H_
42 
43 #include <pcl/segmentation/boost.h>
44 #include <pcl/segmentation/plane_coefficient_comparator.h>
45 
46 namespace pcl
47 {
48  /** \brief EdgeAwarePlaneComparator is a Comparator that operates on plane coefficients,
49  * for use in planar segmentation.
50  * In conjunction with OrganizedConnectedComponentSegmentation, this allows planes to be segmented from organized data.
51  *
52  * \author Stefan Holzer, Alex Trevor
53  */
54  template<typename PointT, typename PointNT>
56  {
57  public:
60 
62  typedef typename PointCloudN::Ptr PointCloudNPtr;
64 
65  typedef boost::shared_ptr<EdgeAwarePlaneComparator<PointT, PointNT> > Ptr;
66  typedef boost::shared_ptr<const EdgeAwarePlaneComparator<PointT, PointNT> > ConstPtr;
67 
75 
76  /** \brief Empty constructor for PlaneCoefficientComparator. */
79  curvature_threshold_ (0.04f),
81  {
82  }
83 
84  /** \brief Empty constructor for PlaneCoefficientComparator.
85  * \param[in] distance_map the distance map to use
86  */
87  EdgeAwarePlaneComparator (const float *distance_map) :
88  distance_map_ (distance_map),
90  curvature_threshold_ (0.04f),
92  {
93  }
94 
95  /** \brief Destructor for PlaneCoefficientComparator. */
96  virtual
98  {
99  }
100 
101  /** \brief Set a distance map to use. For an example of a valid distance map see
102  * \ref OrganizedIntegralImageNormalEstimation
103  * \param[in] distance_map the distance map to use
104  */
105  inline void
106  setDistanceMap (const float *distance_map)
107  {
108  distance_map_ = distance_map;
109  }
110 
111  /** \brief Return the distance map used. */
112  const float*
113  getDistanceMap () const
114  {
115  return (distance_map_);
116  }
117 
118  /** \brief Set the curvature threshold for creating a new segment
119  * \param[in] curvature_threshold a threshold for the curvature
120  */
121  void
122  setCurvatureThreshold (float curvature_threshold)
123  {
124  curvature_threshold_ = curvature_threshold;
125  }
126 
127  /** \brief Get the curvature threshold. */
128  inline float
130  {
131  return (curvature_threshold_);
132  }
133 
134  /** \brief Set the distance map threshold -- the number of pixel away from a border / nan
135  * \param[in] distance_map_threshold the distance map threshold
136  */
137  void
138  setDistanceMapThreshold (float distance_map_threshold)
139  {
140  distance_map_threshold_ = distance_map_threshold;
141  }
142 
143  /** \brief Get the distance map threshold (in pixels). */
144  inline float
146  {
147  return (distance_map_threshold_);
148  }
149 
150  /** \brief Set the euclidean distance threshold.
151  * \param[in] euclidean_distance_threshold the euclidean distance threshold in meters
152  */
153  void
154  setEuclideanDistanceThreshold (float euclidean_distance_threshold)
155  {
156  euclidean_distance_threshold_ = euclidean_distance_threshold;
157  }
158 
159  /** \brief Get the euclidean distance threshold. */
160  inline float
162  {
164  }
165 
166  protected:
167  /** \brief Compare two neighboring points, by using normal information, curvature, and euclidean distance information.
168  * \param[in] idx1 The index of the first point.
169  * \param[in] idx2 The index of the second point.
170  */
171  bool
172  compare (int idx1, int idx2) const
173  {
174  // Note: there are two distance thresholds here that make sense to scale with depth.
175  // dist_threshold is on the perpendicular distance to the plane, as in plane comparator
176  // We additionally check euclidean distance to ensure that we don't have neighboring coplanar points
177  // that aren't close in euclidean space (think two tables separated by a meter, viewed from an angle
178  // where the surfaces are adjacent in image space).
179  float dist_threshold = distance_threshold_;
180  float euclidean_dist_threshold = euclidean_distance_threshold_;
181  if (depth_dependent_)
182  {
183  Eigen::Vector3f vec = input_->points[idx1].getVector3fMap ();
184  float z = vec.dot (z_axis_);
185  dist_threshold *= z * z;
186  euclidean_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  bool normal_ok = (normals_->points[idx1].getNormalVector3fMap ().dot (normals_->points[idx2].getNormalVector3fMap () ) > angular_threshold_ );
195  bool dist_ok = (dist < euclidean_dist_threshold);
196 
197  bool curvature_ok = normals_->points[idx1].curvature < curvature_threshold_;
198  bool plane_d_ok = fabs ((*plane_coeff_d_)[idx1] - (*plane_coeff_d_)[idx2]) < dist_threshold;
199 
201  curvature_ok = false;
202 
203  return (dist_ok && normal_ok && curvature_ok && plane_d_ok);
204  }
205 
206  protected:
207  const float* distance_map_;
211  };
212 }
213 
214 #endif // PCL_SEGMENTATION_PLANE_COEFFICIENT_COMPARATOR_H_
PointCloud::ConstPtr PointCloudConstPtr
Definition: comparator.h:58
void setDistanceMap(const float *distance_map)
Set a distance map to use.
void setCurvatureThreshold(float curvature_threshold)
Set the curvature threshold for creating a new segment.
EdgeAwarePlaneComparator(const float *distance_map)
Empty constructor for PlaneCoefficientComparator.
bool compare(int idx1, int idx2) const
Compare two neighboring points, by using normal information, curvature, and euclidean distance inform...
float getEuclideanDistanceThreshold() const
Get the euclidean distance threshold.
pcl::PointCloud< PointNT > PointCloudN
const float * getDistanceMap() const
Return the distance map used.
float getDistanceMapThreshold() const
Get the distance map threshold (in pixels).
boost::shared_ptr< PointCloud< PointNT > > Ptr
Definition: point_cloud.h:428
boost::shared_ptr< const EdgeAwarePlaneComparator< PointT, PointNT > > ConstPtr
virtual ~EdgeAwarePlaneComparator()
Destructor for PlaneCoefficientComparator.
Comparator< PointT >::PointCloud PointCloud
EdgeAwarePlaneComparator()
Empty constructor for PlaneCoefficientComparator.
boost::shared_ptr< const PointCloud< PointNT > > ConstPtr
Definition: point_cloud.h:429
PlaneCoefficientComparator is a Comparator that operates on plane coefficients, for use in planar seg...
PointCloud represents the base class in PCL for storing collections of 3D points. ...
boost::shared_ptr< std::vector< float > > plane_coeff_d_
EdgeAwarePlaneComparator is a Comparator that operates on plane coefficients, for use in planar segme...
PointCloudConstPtr input_
Definition: comparator.h:99
float getCurvatureThreshold() const
Get the curvature threshold.
void setDistanceMapThreshold(float distance_map_threshold)
Set the distance map threshold – the number of pixel away from a border / nan.
boost::shared_ptr< EdgeAwarePlaneComparator< PointT, PointNT > > Ptr
Comparator< PointT >::PointCloudConstPtr PointCloudConstPtr
void setEuclideanDistanceThreshold(float euclidean_distance_threshold)
Set the euclidean distance threshold.