Point Cloud Library (PCL)  1.9.1-dev
plane_coefficient_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$
37  *
38  */
39 
40 #pragma once
41 
42 #include <pcl/common/angles.h>
43 #include <pcl/pcl_macros.h>
44 #include <pcl/segmentation/boost.h>
45 #include <pcl/segmentation/comparator.h>
46 
47 namespace pcl
48 {
49  /** \brief PlaneCoefficientComparator is a Comparator that operates on plane coefficients, for use in planar segmentation.
50  * In conjunction with OrganizedConnectedComponentSegmentation, this allows planes to be segmented from organized data.
51  *
52  * \author Alex Trevor
53  */
54  template<typename PointT, typename PointNT>
55  class PlaneCoefficientComparator: public Comparator<PointT>
56  {
57  public:
60 
62  using PointCloudNPtr = typename PointCloudN::Ptr;
64 
65  using Ptr = boost::shared_ptr<PlaneCoefficientComparator<PointT, PointNT> >;
66  using ConstPtr = boost::shared_ptr<const PlaneCoefficientComparator<PointT, PointNT> >;
67 
69 
70  /** \brief Empty constructor for PlaneCoefficientComparator. */
72  : normals_ ()
73  , angular_threshold_ (pcl::deg2rad (2.0f))
74  , distance_threshold_ (0.02f)
75  , depth_dependent_ (true)
76  , z_axis_ (Eigen::Vector3f (0.0, 0.0, 1.0) )
77  {
78  }
79 
80  /** \brief Constructor for PlaneCoefficientComparator.
81  * \param[in] plane_coeff_d a reference to a vector of d coefficients of plane equations. Must be the same size as the input cloud and input normals. a, b, and c coefficients are in the input normals.
82  */
83  PlaneCoefficientComparator (boost::shared_ptr<std::vector<float> >& plane_coeff_d)
84  : normals_ ()
85  , plane_coeff_d_ (plane_coeff_d)
86  , angular_threshold_ (pcl::deg2rad (2.0f))
87  , distance_threshold_ (0.02f)
88  , depth_dependent_ (true)
89  , z_axis_ (Eigen::Vector3f (0.0f, 0.0f, 1.0f) )
90  {
91  }
92 
93  /** \brief Destructor for PlaneCoefficientComparator. */
94 
96  {
97  }
98 
99  void
100  setInputCloud (const PointCloudConstPtr& cloud) override
101  {
102  input_ = cloud;
103  }
104 
105  /** \brief Provide a pointer to the input normals.
106  * \param[in] normals the input normal cloud
107  */
108  inline void
110  {
111  normals_ = normals;
112  }
113 
114  /** \brief Get the input normals. */
115  inline PointCloudNConstPtr
117  {
118  return (normals_);
119  }
120 
121  /** \brief Provide a pointer to a vector of the d-coefficient of the planes' hessian normal form. a, b, and c are provided by the normal cloud.
122  * \param[in] plane_coeff_d a pointer to the plane coefficients.
123  */
124  void
125  setPlaneCoeffD (boost::shared_ptr<std::vector<float> >& plane_coeff_d)
126  {
127  plane_coeff_d_ = plane_coeff_d;
128  }
129 
130  /** \brief Provide a pointer to a vector of the d-coefficient of the planes' hessian normal form. a, b, and c are provided by the normal cloud.
131  * \param[in] plane_coeff_d a pointer to the plane coefficients.
132  */
133  void
134  setPlaneCoeffD (std::vector<float>& plane_coeff_d)
135  {
136  plane_coeff_d_ = boost::make_shared<std::vector<float> >(plane_coeff_d);
137  }
138 
139  /** \brief Get a pointer to the vector of the d-coefficient of the planes' hessian normal form. */
140  const std::vector<float>&
141  getPlaneCoeffD () const
142  {
143  return (*plane_coeff_d_);
144  }
145 
146  /** \brief Set the tolerance in radians for difference in normal direction between neighboring points, to be considered part of the same plane.
147  * \param[in] angular_threshold the tolerance in radians
148  */
149  virtual void
150  setAngularThreshold (float angular_threshold)
151  {
152  angular_threshold_ = std::cos (angular_threshold);
153  }
154 
155  /** \brief Get the angular threshold in radians for difference in normal direction between neighboring points, to be considered part of the same plane. */
156  inline float
158  {
159  return (std::acos (angular_threshold_) );
160  }
161 
162  /** \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.
163  * \param[in] distance_threshold the tolerance in meters (at 1m)
164  * \param[in] depth_dependent whether to scale the threshold based on range from the sensor (default: false)
165  */
166  void
167  setDistanceThreshold (float distance_threshold,
168  bool depth_dependent = false)
169  {
170  distance_threshold_ = distance_threshold;
171  depth_dependent_ = depth_dependent;
172  }
173 
174  /** \brief Get the distance threshold in meters (d component of plane equation) between neighboring points, to be considered part of the same plane. */
175  inline float
177  {
178  return (distance_threshold_);
179  }
180 
181  /** \brief Compare points at two indices by their plane equations. True if the angle between the normals is less than the angular threshold,
182  * and the difference between the d component of the normals is less than distance threshold, else false
183  * \param idx1 The first index for the comparison
184  * \param idx2 The second index for the comparison
185  */
186  bool
187  compare (int idx1, int idx2) const override
188  {
189  float threshold = distance_threshold_;
190  if (depth_dependent_)
191  {
192  Eigen::Vector3f vec = input_->points[idx1].getVector3fMap ();
193 
194  float z = vec.dot (z_axis_);
195  threshold *= z * z;
196  }
197  return ( (std::fabs ((*plane_coeff_d_)[idx1] - (*plane_coeff_d_)[idx2]) < threshold)
198  && (normals_->points[idx1].getNormalVector3fMap ().dot (normals_->points[idx2].getNormalVector3fMap () ) > angular_threshold_ ) );
199  }
200 
201  protected:
203  boost::shared_ptr<std::vector<float> > plane_coeff_d_;
207  Eigen::Vector3f z_axis_;
208 
209  public:
211  };
212 }
float deg2rad(float alpha)
Convert an angle from degrees to radians.
Definition: angles.hpp:67
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: comparator.h:58
void setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input normals.
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
float getDistanceThreshold() const
Get the distance threshold in meters (d component of plane equation) between neighboring points...
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: pcl_macros.h:344
Definition: bfgs.h:9
Comparator is the base class for comparators that compare two points given some function.
Definition: comparator.h:53
~PlaneCoefficientComparator()
Destructor for PlaneCoefficientComparator.
boost::shared_ptr< const Comparator< PointT > > ConstPtr
Definition: comparator.h:61
void setPlaneCoeffD(std::vector< float > &plane_coeff_d)
Provide a pointer to a vector of the d-coefficient of the planes&#39; hessian normal form.
const std::vector< float > & getPlaneCoeffD() const
Get a pointer to the vector of the d-coefficient of the planes&#39; hessian normal form.
boost::shared_ptr< PointCloud< PointNT > > Ptr
Definition: point_cloud.h:441
PlaneCoefficientComparator is a Comparator that operates on plane coefficients, for use in planar seg...
void setInputCloud(const PointCloudConstPtr &cloud) override
Set the input cloud for the comparator.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
boost::shared_ptr< Comparator< PointT > > Ptr
Definition: comparator.h:60
typename PointCloudN::ConstPtr PointCloudNConstPtr
boost::shared_ptr< std::vector< float > > plane_coeff_d_
void setPlaneCoeffD(boost::shared_ptr< std::vector< float > > &plane_coeff_d)
Provide a pointer to a vector of the d-coefficient of the planes&#39; hessian normal form.
void setDistanceThreshold(float distance_threshold, bool depth_dependent=false)
Set the tolerance in meters for difference in perpendicular distance (d component of plane equation) ...
boost::shared_ptr< const PointCloud< PointNT > > ConstPtr
Definition: point_cloud.h:442
PointCloudNConstPtr getInputNormals() const
Get the input normals.
PointCloudConstPtr input_
Definition: comparator.h:99
Define standard C methods to do angle calculations.
PlaneCoefficientComparator(boost::shared_ptr< std::vector< float > > &plane_coeff_d)
Constructor for PlaneCoefficientComparator.
bool compare(int idx1, int idx2) const override
Compare points at two indices by their plane equations.
virtual void setAngularThreshold(float angular_threshold)
Set the tolerance in radians for difference in normal direction between neighboring points...
float getAngularThreshold() const
Get the angular threshold in radians for difference in normal direction between neighboring points...
PlaneCoefficientComparator()
Empty constructor for PlaneCoefficientComparator.
Defines all the PCL and non-PCL macros used.