Point Cloud Library (PCL)  1.14.1
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 #pragma once
41 
42 #include <pcl/segmentation/plane_coefficient_comparator.h>
43 
44 namespace pcl
45 {
46  /** \brief EdgeAwarePlaneComparator is a Comparator that operates on plane coefficients,
47  * for use in planar segmentation.
48  * In conjunction with OrganizedConnectedComponentSegmentation, this allows planes to be segmented from organized data.
49  *
50  * \author Stefan Holzer, Alex Trevor
51  */
52  template<typename PointT, typename PointNT>
54  {
55  public:
58 
60  using PointCloudNPtr = typename PointCloudN::Ptr;
62 
63  using Ptr = shared_ptr<EdgeAwarePlaneComparator<PointT, PointNT> >;
64  using ConstPtr = shared_ptr<const EdgeAwarePlaneComparator<PointT, PointNT> >;
65 
73 
74  /** \brief Empty constructor for PlaneCoefficientComparator. */
77  curvature_threshold_ (0.04f),
79  {
80  }
81 
82  /** \brief Empty constructor for PlaneCoefficientComparator.
83  * \param[in] distance_map the distance map to use
84  */
85  EdgeAwarePlaneComparator (const float *distance_map) :
86  distance_map_ (distance_map),
88  curvature_threshold_ (0.04f),
90  {
91  }
92 
93  /** \brief Destructor for PlaneCoefficientComparator. */
94 
95  ~EdgeAwarePlaneComparator () override
96  = default;
97 
98  /** \brief Set a distance map to use. For an example of a valid distance map see
99  * IntegralImageNormalEstimation::getDistanceMap
100  * \param[in] distance_map the distance map to use
101  */
102  inline void
103  setDistanceMap (const float *distance_map)
104  {
105  distance_map_ = distance_map;
106  }
107 
108  /** \brief Return the distance map used. */
109  const float*
110  getDistanceMap () const
111  {
112  return (distance_map_);
113  }
114 
115  /** \brief Set the curvature threshold for creating a new segment
116  * \param[in] curvature_threshold a threshold for the curvature
117  */
118  void
119  setCurvatureThreshold (float curvature_threshold)
120  {
121  curvature_threshold_ = curvature_threshold;
122  }
123 
124  /** \brief Get the curvature threshold. */
125  inline float
127  {
128  return (curvature_threshold_);
129  }
130 
131  /** \brief Set the distance map threshold -- the number of pixel away from a border / nan
132  * \param[in] distance_map_threshold the distance map threshold
133  */
134  void
135  setDistanceMapThreshold (float distance_map_threshold)
136  {
137  distance_map_threshold_ = distance_map_threshold;
138  }
139 
140  /** \brief Get the distance map threshold (in pixels). */
141  inline float
143  {
144  return (distance_map_threshold_);
145  }
146 
147  /** \brief Set the euclidean distance threshold.
148  * \param[in] euclidean_distance_threshold the euclidean distance threshold in meters
149  */
150  void
151  setEuclideanDistanceThreshold (float euclidean_distance_threshold)
152  {
153  euclidean_distance_threshold_ = euclidean_distance_threshold;
154  }
155 
156  /** \brief Get the euclidean distance threshold. */
157  inline float
159  {
161  }
162 
163  protected:
164  /** \brief Compare two neighboring points, by using normal information, curvature, and euclidean distance information.
165  * \param[in] idx1 The index of the first point.
166  * \param[in] idx2 The index of the second point.
167  */
168  bool
169  compare (int idx1, int idx2) const override
170  {
171  // Note: there are two distance thresholds here that make sense to scale with depth.
172  // dist_threshold is on the perpendicular distance to the plane, as in plane comparator
173  // We additionally check euclidean distance to ensure that we don't have neighboring coplanar points
174  // that aren't close in euclidean space (think two tables separated by a meter, viewed from an angle
175  // where the surfaces are adjacent in image space).
176  float dist_threshold = distance_threshold_;
177  float euclidean_dist_threshold = euclidean_distance_threshold_;
178  if (depth_dependent_)
179  {
180  Eigen::Vector3f vec = (*input_)[idx1].getVector3fMap ();
181  float z = vec.dot (z_axis_);
182  dist_threshold *= z * z;
183  euclidean_dist_threshold *= z * z;
184  }
185 
186  float dx = (*input_)[idx1].x - (*input_)[idx2].x;
187  float dy = (*input_)[idx1].y - (*input_)[idx2].y;
188  float dz = (*input_)[idx1].z - (*input_)[idx2].z;
189  float dist = std::sqrt (dx*dx + dy*dy + dz*dz);
190 
191  bool normal_ok = ((*normals_)[idx1].getNormalVector3fMap ().dot ((*normals_)[idx2].getNormalVector3fMap () ) > angular_threshold_ );
192  bool dist_ok = (dist < euclidean_dist_threshold);
193 
194  bool curvature_ok = (*normals_)[idx1].curvature < curvature_threshold_;
195  bool plane_d_ok = std::abs ((*plane_coeff_d_)[idx1] - (*plane_coeff_d_)[idx2]) < dist_threshold;
196 
198  curvature_ok = false;
199 
200  return (dist_ok && normal_ok && curvature_ok && plane_d_ok);
201  }
202 
203  protected:
204  const float* distance_map_;
208  };
209 }
void setDistanceMap(const float *distance_map)
Set a distance map to use.
typename PointCloudN::Ptr PointCloudNPtr
shared_ptr< PointCloud< PointNT > > Ptr
Definition: point_cloud.h:413
void setCurvatureThreshold(float curvature_threshold)
Set the curvature threshold for creating a new segment.
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: comparator.h:59
EdgeAwarePlaneComparator(const float *distance_map)
Empty constructor for PlaneCoefficientComparator.
float getEuclideanDistanceThreshold() const
Get the euclidean distance threshold.
const float * getDistanceMap() const
Return the distance map used.
float getDistanceMapThreshold() const
Get the distance map threshold (in pixels).
bool compare(int idx1, int idx2) const override
Compare two neighboring points, by using normal information, curvature, and euclidean distance inform...
~EdgeAwarePlaneComparator() override=default
Destructor for PlaneCoefficientComparator.
shared_ptr< const Comparator< PointT > > ConstPtr
Definition: comparator.h:62
EdgeAwarePlaneComparator()
Empty constructor for PlaneCoefficientComparator.
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. ...
Definition: distances.h:55
shared_ptr< std::vector< float > > plane_coeff_d_
EdgeAwarePlaneComparator is a Comparator that operates on plane coefficients, for use in planar segme...
shared_ptr< const PointCloud< PointNT > > ConstPtr
Definition: point_cloud.h:414
typename PointCloudN::ConstPtr PointCloudNConstPtr
shared_ptr< Comparator< PointT > > Ptr
Definition: comparator.h:61
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.
void setEuclideanDistanceThreshold(float euclidean_distance_threshold)
Set the euclidean distance threshold.