Point Cloud Library (PCL)  1.9.1
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 #ifndef PCL_SEGMENTATION_PLANE_COEFFICIENT_COMPARATOR_H_
41 #define PCL_SEGMENTATION_PLANE_COEFFICIENT_COMPARATOR_H_
42 
43 #include <pcl/common/angles.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  typedef typename PointCloudN::Ptr PointCloudNPtr;
64 
65  typedef boost::shared_ptr<PlaneCoefficientComparator<PointT, PointNT> > Ptr;
66  typedef boost::shared_ptr<const PlaneCoefficientComparator<PointT, PointNT> > ConstPtr;
67 
69 
70  /** \brief Empty constructor for PlaneCoefficientComparator. */
72  : normals_ ()
73  , plane_coeff_d_ ()
74  , angular_threshold_ (pcl::deg2rad (2.0f))
75  , distance_threshold_ (0.02f)
76  , depth_dependent_ (true)
77  , z_axis_ (Eigen::Vector3f (0.0, 0.0, 1.0) )
78  {
79  }
80 
81  /** \brief Constructor for PlaneCoefficientComparator.
82  * \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.
83  */
84  PlaneCoefficientComparator (boost::shared_ptr<std::vector<float> >& plane_coeff_d)
85  : normals_ ()
86  , plane_coeff_d_ (plane_coeff_d)
87  , angular_threshold_ (pcl::deg2rad (2.0f))
88  , distance_threshold_ (0.02f)
89  , depth_dependent_ (true)
90  , z_axis_ (Eigen::Vector3f (0.0f, 0.0f, 1.0f) )
91  {
92  }
93 
94  /** \brief Destructor for PlaneCoefficientComparator. */
95  virtual
97  {
98  }
99 
100  virtual void
102  {
103  input_ = cloud;
104  }
105 
106  /** \brief Provide a pointer to the input normals.
107  * \param[in] normals the input normal cloud
108  */
109  inline void
111  {
112  normals_ = normals;
113  }
114 
115  /** \brief Get the input normals. */
116  inline PointCloudNConstPtr
118  {
119  return (normals_);
120  }
121 
122  /** \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.
123  * \param[in] plane_coeff_d a pointer to the plane coefficients.
124  */
125  void
126  setPlaneCoeffD (boost::shared_ptr<std::vector<float> >& plane_coeff_d)
127  {
128  plane_coeff_d_ = plane_coeff_d;
129  }
130 
131  /** \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.
132  * \param[in] plane_coeff_d a pointer to the plane coefficients.
133  */
134  void
135  setPlaneCoeffD (std::vector<float>& plane_coeff_d)
136  {
137  plane_coeff_d_ = boost::make_shared<std::vector<float> >(plane_coeff_d);
138  }
139 
140  /** \brief Get a pointer to the vector of the d-coefficient of the planes' hessian normal form. */
141  const std::vector<float>&
142  getPlaneCoeffD () const
143  {
144  return (*plane_coeff_d_);
145  }
146 
147  /** \brief Set the tolerance in radians for difference in normal direction between neighboring points, to be considered part of the same plane.
148  * \param[in] angular_threshold the tolerance in radians
149  */
150  virtual void
151  setAngularThreshold (float angular_threshold)
152  {
153  angular_threshold_ = cosf (angular_threshold);
154  }
155 
156  /** \brief Get the angular threshold in radians for difference in normal direction between neighboring points, to be considered part of the same plane. */
157  inline float
159  {
160  return (acosf (angular_threshold_) );
161  }
162 
163  /** \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.
164  * \param[in] distance_threshold the tolerance in meters (at 1m)
165  * \param[in] depth_dependent whether to scale the threshold based on range from the sensor (default: false)
166  */
167  void
168  setDistanceThreshold (float distance_threshold,
169  bool depth_dependent = false)
170  {
171  distance_threshold_ = distance_threshold;
172  depth_dependent_ = depth_dependent;
173  }
174 
175  /** \brief Get the distance threshold in meters (d component of plane equation) between neighboring points, to be considered part of the same plane. */
176  inline float
178  {
179  return (distance_threshold_);
180  }
181 
182  /** \brief Compare points at two indices by their plane equations. True if the angle between the normals is less than the angular threshold,
183  * and the difference between the d component of the normals is less than distance threshold, else false
184  * \param idx1 The first index for the comparison
185  * \param idx2 The second index for the comparison
186  */
187  virtual bool
188  compare (int idx1, int idx2) const
189  {
190  float threshold = distance_threshold_;
191  if (depth_dependent_)
192  {
193  Eigen::Vector3f vec = input_->points[idx1].getVector3fMap ();
194 
195  float z = vec.dot (z_axis_);
196  threshold *= z * z;
197  }
198  return ( (fabs ((*plane_coeff_d_)[idx1] - (*plane_coeff_d_)[idx2]) < threshold)
199  && (normals_->points[idx1].getNormalVector3fMap ().dot (normals_->points[idx2].getNormalVector3fMap () ) > angular_threshold_ ) );
200  }
201 
202  protected:
204  boost::shared_ptr<std::vector<float> > plane_coeff_d_;
208  Eigen::Vector3f z_axis_;
209 
210  public:
211  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
212  };
213 }
214 
215 #endif // PCL_SEGMENTATION_PLANE_COEFFICIENT_COMPARATOR_H_
pcl::PlaneCoefficientComparator::PointCloudNPtr
PointCloudN::Ptr PointCloudNPtr
Definition: plane_coefficient_comparator.h:62
pcl
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
pcl::Comparator::PointCloudConstPtr
PointCloud::ConstPtr PointCloudConstPtr
Definition: comparator.h:58
pcl::PointCloud< PointNT >::Ptr
boost::shared_ptr< PointCloud< PointNT > > Ptr
Definition: point_cloud.h:428
Eigen
Definition: bfgs.h:10
pcl::PlaneCoefficientComparator::~PlaneCoefficientComparator
virtual ~PlaneCoefficientComparator()
Destructor for PlaneCoefficientComparator.
Definition: plane_coefficient_comparator.h:96
pcl::PlaneCoefficientComparator::Ptr
boost::shared_ptr< PlaneCoefficientComparator< PointT, PointNT > > Ptr
Definition: plane_coefficient_comparator.h:65
pcl::PlaneCoefficientComparator::PointCloudConstPtr
Comparator< PointT >::PointCloudConstPtr PointCloudConstPtr
Definition: plane_coefficient_comparator.h:59
pcl::PlaneCoefficientComparator::PlaneCoefficientComparator
PlaneCoefficientComparator(boost::shared_ptr< std::vector< float > > &plane_coeff_d)
Constructor for PlaneCoefficientComparator.
Definition: plane_coefficient_comparator.h:84
pcl::PlaneCoefficientComparator::PointCloud
Comparator< PointT >::PointCloud PointCloud
Definition: plane_coefficient_comparator.h:58
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: projection_matrix.h:53
pcl::PlaneCoefficientComparator::PlaneCoefficientComparator
PlaneCoefficientComparator()
Empty constructor for PlaneCoefficientComparator.
Definition: plane_coefficient_comparator.h:71
pcl::PlaneCoefficientComparator::distance_threshold_
float distance_threshold_
Definition: plane_coefficient_comparator.h:206
angles.h
pcl::PlaneCoefficientComparator::normals_
PointCloudNConstPtr normals_
Definition: plane_coefficient_comparator.h:203
pcl::PlaneCoefficientComparator::depth_dependent_
bool depth_dependent_
Definition: plane_coefficient_comparator.h:207
pcl::PlaneCoefficientComparator::compare
virtual bool compare(int idx1, int idx2) const
Compare points at two indices by their plane equations.
Definition: plane_coefficient_comparator.h:188
pcl::deg2rad
float deg2rad(float alpha)
Convert an angle from degrees to radians.
Definition: angles.hpp:67
pcl::Comparator
Comparator is the base class for comparators that compare two points given some function.
Definition: comparator.h:53
pcl::PlaneCoefficientComparator::setInputCloud
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Set the input cloud for the comparator.
Definition: plane_coefficient_comparator.h:101
pcl::PlaneCoefficientComparator::getPlaneCoeffD
const std::vector< float > & getPlaneCoeffD() const
Get a pointer to the vector of the d-coefficient of the planes' hessian normal form.
Definition: plane_coefficient_comparator.h:142
pcl::PlaneCoefficientComparator::getAngularThreshold
float getAngularThreshold() const
Get the angular threshold in radians for difference in normal direction between neighboring points,...
Definition: plane_coefficient_comparator.h:158
pcl::PlaneCoefficientComparator::ConstPtr
boost::shared_ptr< const PlaneCoefficientComparator< PointT, PointNT > > ConstPtr
Definition: plane_coefficient_comparator.h:66
pcl::PlaneCoefficientComparator::z_axis_
Eigen::Vector3f z_axis_
Definition: plane_coefficient_comparator.h:208
pcl::PointCloud< PointNT >::ConstPtr
boost::shared_ptr< const PointCloud< PointNT > > ConstPtr
Definition: point_cloud.h:429
pcl::PlaneCoefficientComparator::getInputNormals
PointCloudNConstPtr getInputNormals() const
Get the input normals.
Definition: plane_coefficient_comparator.h:117
pcl::PlaneCoefficientComparator::PointCloudNConstPtr
PointCloudN::ConstPtr PointCloudNConstPtr
Definition: plane_coefficient_comparator.h:63
pcl::PlaneCoefficientComparator::setDistanceThreshold
void setDistanceThreshold(float distance_threshold, bool depth_dependent=false)
Set the tolerance in meters for difference in perpendicular distance (d component of plane equation) ...
Definition: plane_coefficient_comparator.h:168
pcl::PlaneCoefficientComparator::angular_threshold_
float angular_threshold_
Definition: plane_coefficient_comparator.h:205
pcl::PlaneCoefficientComparator::setPlaneCoeffD
void setPlaneCoeffD(std::vector< float > &plane_coeff_d)
Provide a pointer to a vector of the d-coefficient of the planes' hessian normal form.
Definition: plane_coefficient_comparator.h:135
pcl::PlaneCoefficientComparator::setInputNormals
void setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input normals.
Definition: plane_coefficient_comparator.h:110
pcl::Comparator::input_
PointCloudConstPtr input_
Definition: comparator.h:99
pcl::PlaneCoefficientComparator::setPlaneCoeffD
void setPlaneCoeffD(boost::shared_ptr< std::vector< float > > &plane_coeff_d)
Provide a pointer to a vector of the d-coefficient of the planes' hessian normal form.
Definition: plane_coefficient_comparator.h:126
pcl::PlaneCoefficientComparator::plane_coeff_d_
boost::shared_ptr< std::vector< float > > plane_coeff_d_
Definition: plane_coefficient_comparator.h:204
pcl::PlaneCoefficientComparator::getDistanceThreshold
float getDistanceThreshold() const
Get the distance threshold in meters (d component of plane equation) between neighboring points,...
Definition: plane_coefficient_comparator.h:177
pcl::PlaneCoefficientComparator::PointCloudN
pcl::PointCloud< PointNT > PointCloudN
Definition: plane_coefficient_comparator.h:61
pcl::PlaneCoefficientComparator::setAngularThreshold
virtual void setAngularThreshold(float angular_threshold)
Set the tolerance in radians for difference in normal direction between neighboring points,...
Definition: plane_coefficient_comparator.h:151
pcl::PlaneCoefficientComparator
PlaneCoefficientComparator is a Comparator that operates on plane coefficients, for use in planar seg...
Definition: plane_coefficient_comparator.h:55