Point Cloud Library (PCL)  1.8.1
ground_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  *
37  *
38  */
39 
40 #ifndef PCL_SEGMENTATION_GROUND_PLANE_COMPARATOR_H_
41 #define PCL_SEGMENTATION_GROUND_PLANE_COMPARATOR_H_
42 
43 #include <pcl/common/angles.h>
44 #include <pcl/segmentation/comparator.h>
45 #include <boost/make_shared.hpp>
46 
47 namespace pcl
48 {
49  /** \brief GroundPlaneComparator is a Comparator for detecting smooth surfaces suitable for driving.
50  * In conjunction with OrganizedConnectedComponentSegmentation, this allows smooth groundplanes / road surfaces to be segmented from point clouds.
51  *
52  * \author Alex Trevor
53  */
54  template<typename PointT, typename PointNT>
55  class GroundPlaneComparator: public Comparator<PointT>
56  {
57  public:
60 
62  typedef typename PointCloudN::Ptr PointCloudNPtr;
64 
65  typedef boost::shared_ptr<GroundPlaneComparator<PointT, PointNT> > Ptr;
66  typedef boost::shared_ptr<const GroundPlaneComparator<PointT, PointNT> > ConstPtr;
67 
69 
70  /** \brief Empty constructor for GroundPlaneComparator. */
72  : normals_ ()
73  , plane_coeff_d_ ()
74  , angular_threshold_ (cosf (pcl::deg2rad (2.0f)))
75  , road_angular_threshold_ ( cosf(pcl::deg2rad (10.0f)))
76  , distance_threshold_ (0.1f)
77  , depth_dependent_ (true)
78  , z_axis_ (Eigen::Vector3f (0.0, 0.0, 1.0) )
79  , desired_road_axis_ (Eigen::Vector3f(0.0, -1.0, 0.0))
80  {
81  }
82 
83  /** \brief Constructor for GroundPlaneComparator.
84  * \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.
85  */
86  GroundPlaneComparator (boost::shared_ptr<std::vector<float> >& plane_coeff_d)
87  : normals_ ()
88  , plane_coeff_d_ (plane_coeff_d)
89  , angular_threshold_ (cosf (pcl::deg2rad (3.0f)))
90  , distance_threshold_ (0.1f)
91  , depth_dependent_ (true)
92  , z_axis_ (Eigen::Vector3f (0.0f, 0.0f, 1.0f))
93  , road_angular_threshold_ ( cosf(pcl::deg2rad (40.0f)))
94  , desired_road_axis_ (Eigen::Vector3f(0.0, -1.0, 0.0))
95  {
96  }
97 
98  /** \brief Destructor for GroundPlaneComparator. */
99  virtual
101  {
102  }
103  /** \brief Provide the input cloud.
104  * \param[in] cloud the input point cloud.
105  */
106  virtual void
107  setInputCloud (const PointCloudConstPtr& cloud)
108  {
109  input_ = cloud;
110  }
111 
112  /** \brief Provide a pointer to the input normals.
113  * \param[in] normals the input normal cloud.
114  */
115  inline void
116  setInputNormals (const PointCloudNConstPtr &normals)
117  {
118  normals_ = normals;
119  }
120 
121  /** \brief Get the input normals. */
122  inline PointCloudNConstPtr
124  {
125  return (normals_);
126  }
127 
128  /** \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.
129  * \param[in] plane_coeff_d a pointer to the plane coefficients.
130  */
131  void
132  setPlaneCoeffD (boost::shared_ptr<std::vector<float> >& plane_coeff_d)
133  {
134  plane_coeff_d_ = plane_coeff_d;
135  }
136 
137  /** \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.
138  * \param[in] plane_coeff_d a pointer to the plane coefficients.
139  */
140  void
141  setPlaneCoeffD (std::vector<float>& plane_coeff_d)
142  {
143  plane_coeff_d_ = boost::make_shared<std::vector<float> >(plane_coeff_d);
144  }
145 
146  /** \brief Get a pointer to the vector of the d-coefficient of the planes' hessian normal form. */
147  const std::vector<float>&
148  getPlaneCoeffD () const
149  {
150  return (*plane_coeff_d_);
151  }
152 
153  /** \brief Set the tolerance in radians for difference in normal direction between neighboring points, to be considered part of the same plane.
154  * \param[in] angular_threshold the tolerance in radians
155  */
156  virtual void
157  setAngularThreshold (float angular_threshold)
158  {
159  angular_threshold_ = cosf (angular_threshold);
160  }
161 
162  /** \brief Set the tolerance in radians for difference in normal direction between a point and the expected ground normal.
163  * \param[in] angular_threshold the
164  */
165  virtual void
166  setGroundAngularThreshold (float angular_threshold)
167  {
168  road_angular_threshold_ = cosf (angular_threshold);
169  }
170 
171  /** \brief Set the expected ground plane normal with respect to the sensor. Pixels labeled as ground must be within ground_angular_threshold radians of this normal to be labeled as ground.
172  * \param[in] normal The normal direction of the expected ground plane.
173  */
174  void
175  setExpectedGroundNormal (Eigen::Vector3f normal)
176  {
177  desired_road_axis_ = normal;
178  }
179 
180 
181  /** \brief Get the angular threshold in radians for difference in normal direction between neighboring points, to be considered part of the same plane. */
182  inline float
184  {
185  return (acosf (angular_threshold_) );
186  }
187 
188  /** \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.
189  * \param[in] distance_threshold the tolerance in meters (at 1m)
190  * \param[in] depth_dependent whether to scale the threshold based on range from the sensor (default: false)
191  */
192  void
193  setDistanceThreshold (float distance_threshold,
194  bool depth_dependent = false)
195  {
196  distance_threshold_ = distance_threshold;
197  depth_dependent_ = depth_dependent;
198  }
199 
200  /** \brief Get the distance threshold in meters (d component of plane equation) between neighboring points, to be considered part of the same plane. */
201  inline float
203  {
204  return distance_threshold_;
205  }
206 
207  /** \brief Compare points at two indices by their plane equations. True if the angle between the normals is less than the angular threshold,
208  * and the difference between the d component of the normals is less than distance threshold, else false
209  * \param idx1 The first index for the comparison
210  * \param idx2 The second index for the comparison
211  */
212  virtual bool
213  compare (int idx1, int idx2) const
214  {
215  // Normal must be similar to neighbor
216  // Normal must be similar to expected normal
217  float threshold = distance_threshold_;
218  if (depth_dependent_)
219  {
220  Eigen::Vector3f vec = input_->points[idx1].getVector3fMap ();
221 
222  float z = vec.dot (z_axis_);
223  threshold *= z * z;
224  }
225 
226  return ( (normals_->points[idx1].getNormalVector3fMap ().dot (desired_road_axis_) > road_angular_threshold_) &&
227  (normals_->points[idx1].getNormalVector3fMap ().dot (normals_->points[idx2].getNormalVector3fMap () ) > angular_threshold_ ));
228 
229  // Euclidean proximity of neighbors does not seem to be required -- pixel adjacency handles this well enough
230  //return ( (normals_->points[idx1].getNormalVector3fMap ().dot (desired_road_axis_) > road_angular_threshold_) &&
231  // (normals_->points[idx1].getNormalVector3fMap ().dot (normals_->points[idx2].getNormalVector3fMap () ) > angular_threshold_ ) &&
232  // (pcl::euclideanDistance (input_->points[idx1], input_->points[idx2]) < distance_threshold_ ));
233  }
234 
235  protected:
236  PointCloudNConstPtr normals_;
237  boost::shared_ptr<std::vector<float> > plane_coeff_d_;
242  Eigen::Vector3f z_axis_;
243  Eigen::Vector3f desired_road_axis_;
244 
245  public:
246  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
247  };
248 }
249 
250 #endif // PCL_SEGMENTATION_GROUND_PLANE_COMPARATOR_H_
boost::shared_ptr< std::vector< float > > plane_coeff_d_
PointCloud::ConstPtr PointCloudConstPtr
Definition: comparator.h:58
float deg2rad(float alpha)
Convert an angle from degrees to radians.
Definition: angles.hpp:67
void setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input normals.
virtual void setAngularThreshold(float angular_threshold)
Set the tolerance in radians for difference in normal direction between neighboring points...
GroundPlaneComparator is a Comparator for detecting smooth surfaces suitable for driving.
float getAngularThreshold() const
Get the angular threshold in radians for difference in normal direction between neighboring points...
GroundPlaneComparator(boost::shared_ptr< std::vector< float > > &plane_coeff_d)
Constructor for GroundPlaneComparator.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide the input cloud.
PointCloudN::ConstPtr PointCloudNConstPtr
Definition: bfgs.h:10
Comparator is the base class for comparators that compare two points given some function.
Definition: comparator.h:53
boost::shared_ptr< PointCloud< PointNT > > Ptr
Definition: point_cloud.h:428
virtual bool compare(int idx1, int idx2) const
Compare points at two indices by their plane equations.
boost::shared_ptr< const GroundPlaneComparator< PointT, PointNT > > ConstPtr
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.
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:429
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.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void setExpectedGroundNormal(Eigen::Vector3f normal)
Set the expected ground plane normal with respect to the sensor.
pcl::PointCloud< PointNT > PointCloudN
GroundPlaneComparator()
Empty constructor for GroundPlaneComparator.
virtual void setGroundAngularThreshold(float angular_threshold)
Set the tolerance in radians for difference in normal direction between a point and the expected grou...
PointCloudNConstPtr getInputNormals() const
Get the input normals.
PointCloudConstPtr input_
Definition: comparator.h:99
boost::shared_ptr< GroundPlaneComparator< PointT, PointNT > > Ptr
Define standard C methods to do angle calculations.
float getDistanceThreshold() const
Get the distance threshold in meters (d component of plane equation) between neighboring points...
Comparator< PointT >::PointCloud PointCloud
virtual ~GroundPlaneComparator()
Destructor for GroundPlaneComparator.
Comparator< PointT >::PointCloudConstPtr PointCloudConstPtr
const std::vector< float > & getPlaneCoeffD() const
Get a pointer to the vector of the d-coefficient of the planes&#39; hessian normal form.