Point Cloud Library (PCL)  1.10.0
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 #pragma once
41 
42 #include <pcl/common/angles.h>
43 #include <pcl/pcl_macros.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  using PointCloudNPtr = typename PointCloudN::Ptr;
64 
67 
69 
70  /** \brief Empty constructor for GroundPlaneComparator. */
72  : normals_ ()
73  , angular_threshold_ (std::cos (pcl::deg2rad (2.0f)))
74  , road_angular_threshold_ ( std::cos(pcl::deg2rad (10.0f)))
75  , distance_threshold_ (0.1f)
76  , depth_dependent_ (true)
77  , z_axis_ (Eigen::Vector3f (0.0, 0.0, 1.0) )
78  , desired_road_axis_ (Eigen::Vector3f(0.0, -1.0, 0.0))
79  {
80  }
81 
82  /** \brief Constructor for GroundPlaneComparator.
83  * \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.
84  */
85  GroundPlaneComparator (shared_ptr<std::vector<float> >& plane_coeff_d)
86  : normals_ ()
87  , plane_coeff_d_ (plane_coeff_d)
88  , angular_threshold_ (std::cos (pcl::deg2rad (3.0f)))
89  , distance_threshold_ (0.1f)
90  , depth_dependent_ (true)
91  , z_axis_ (Eigen::Vector3f (0.0f, 0.0f, 1.0f))
92  , road_angular_threshold_ ( std::cos(pcl::deg2rad (40.0f)))
93  , desired_road_axis_ (Eigen::Vector3f(0.0, -1.0, 0.0))
94  {
95  }
96 
97  /** \brief Destructor for GroundPlaneComparator. */
98 
100  {
101  }
102  /** \brief Provide the input cloud.
103  * \param[in] cloud the input point cloud.
104  */
105  void
106  setInputCloud (const PointCloudConstPtr& cloud) override
107  {
108  input_ = cloud;
109  }
110 
111  /** \brief Provide a pointer to the input normals.
112  * \param[in] normals the input normal cloud.
113  */
114  inline void
116  {
117  normals_ = normals;
118  }
119 
120  /** \brief Get the input normals. */
121  inline PointCloudNConstPtr
123  {
124  return (normals_);
125  }
126 
127  /** \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.
128  * \param[in] plane_coeff_d a pointer to the plane coefficients.
129  */
130  void
131  setPlaneCoeffD (shared_ptr<std::vector<float> >& plane_coeff_d)
132  {
133  plane_coeff_d_ = plane_coeff_d;
134  }
135 
136  /** \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.
137  * \param[in] plane_coeff_d a pointer to the plane coefficients.
138  */
139  void
140  setPlaneCoeffD (std::vector<float>& plane_coeff_d)
141  {
142  plane_coeff_d_ = boost::make_shared<std::vector<float> >(plane_coeff_d);
143  }
144 
145  /** \brief Get a pointer to the vector of the d-coefficient of the planes' hessian normal form. */
146  const std::vector<float>&
147  getPlaneCoeffD () const
148  {
149  return (*plane_coeff_d_);
150  }
151 
152  /** \brief Set the tolerance in radians for difference in normal direction between neighboring points, to be considered part of the same plane.
153  * \param[in] angular_threshold the tolerance in radians
154  */
155  virtual void
156  setAngularThreshold (float angular_threshold)
157  {
158  angular_threshold_ = std::cos (angular_threshold);
159  }
160 
161  /** \brief Set the tolerance in radians for difference in normal direction between a point and the expected ground normal.
162  * \param[in] angular_threshold the
163  */
164  virtual void
165  setGroundAngularThreshold (float angular_threshold)
166  {
167  road_angular_threshold_ = std::cos (angular_threshold);
168  }
169 
170  /** \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.
171  * \param[in] normal The normal direction of the expected ground plane.
172  */
173  void
174  setExpectedGroundNormal (Eigen::Vector3f normal)
175  {
176  desired_road_axis_ = normal;
177  }
178 
179 
180  /** \brief Get the angular threshold in radians for difference in normal direction between neighboring points, to be considered part of the same plane. */
181  inline float
183  {
184  return (std::acos (angular_threshold_) );
185  }
186 
187  /** \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.
188  * \param[in] distance_threshold the tolerance in meters (at 1m)
189  * \param[in] depth_dependent whether to scale the threshold based on range from the sensor (default: false)
190  */
191  void
192  setDistanceThreshold (float distance_threshold,
193  bool depth_dependent = false)
194  {
195  distance_threshold_ = distance_threshold;
196  depth_dependent_ = depth_dependent;
197  }
198 
199  /** \brief Get the distance threshold in meters (d component of plane equation) between neighboring points, to be considered part of the same plane. */
200  inline float
202  {
203  return distance_threshold_;
204  }
205 
206  /** \brief Compare points at two indices by their plane equations. True if the angle between the normals is less than the angular threshold,
207  * and the difference between the d component of the normals is less than distance threshold, else false
208  * \param idx1 The first index for the comparison
209  * \param idx2 The second index for the comparison
210  */
211  bool
212  compare (int idx1, int idx2) const override
213  {
214  // Normal must be similar to neighbor
215  // Normal must be similar to expected normal
216  float threshold = distance_threshold_;
217  if (depth_dependent_)
218  {
219  Eigen::Vector3f vec = input_->points[idx1].getVector3fMap ();
220 
221  float z = vec.dot (z_axis_);
222  threshold *= z * z;
223  }
224 
225  return ( (normals_->points[idx1].getNormalVector3fMap ().dot (desired_road_axis_) > road_angular_threshold_) &&
226  (normals_->points[idx1].getNormalVector3fMap ().dot (normals_->points[idx2].getNormalVector3fMap () ) > angular_threshold_ ));
227 
228  // Euclidean proximity of neighbors does not seem to be required -- pixel adjacency handles this well enough
229  //return ( (normals_->points[idx1].getNormalVector3fMap ().dot (desired_road_axis_) > road_angular_threshold_) &&
230  // (normals_->points[idx1].getNormalVector3fMap ().dot (normals_->points[idx2].getNormalVector3fMap () ) > angular_threshold_ ) &&
231  // (pcl::euclideanDistance (input_->points[idx1], input_->points[idx2]) < distance_threshold_ ));
232  }
233 
234  protected:
241  Eigen::Vector3f z_axis_;
242  Eigen::Vector3f desired_road_axis_;
243 
244  public:
246  };
247 }
pcl_macros.h
Defines all the PCL and non-PCL macros used.
pcl::GroundPlaneComparator::setGroundAngularThreshold
virtual void setGroundAngularThreshold(float angular_threshold)
Set the tolerance in radians for difference in normal direction between a point and the expected grou...
Definition: ground_plane_comparator.h:165
pcl
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
pcl::GroundPlaneComparator::getInputNormals
PointCloudNConstPtr getInputNormals() const
Get the input normals.
Definition: ground_plane_comparator.h:122
pcl::GroundPlaneComparator::PointCloudNPtr
typename PointCloudN::Ptr PointCloudNPtr
Definition: ground_plane_comparator.h:62
Eigen
Definition: bfgs.h:9
pcl::GroundPlaneComparator::setInputNormals
void setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input normals.
Definition: ground_plane_comparator.h:115
pcl::GroundPlaneComparator::angular_threshold_
float angular_threshold_
Definition: ground_plane_comparator.h:237
pcl::GroundPlaneComparator::getAngularThreshold
float getAngularThreshold() const
Get the angular threshold in radians for difference in normal direction between neighboring points,...
Definition: ground_plane_comparator.h:182
pcl::GroundPlaneComparator::desired_road_axis_
Eigen::Vector3f desired_road_axis_
Definition: ground_plane_comparator.h:242
pcl::GroundPlaneComparator
GroundPlaneComparator is a Comparator for detecting smooth surfaces suitable for driving.
Definition: ground_plane_comparator.h:55
pcl::GroundPlaneComparator::road_angular_threshold_
float road_angular_threshold_
Definition: ground_plane_comparator.h:238
pcl::GroundPlaneComparator::GroundPlaneComparator
GroundPlaneComparator()
Empty constructor for GroundPlaneComparator.
Definition: ground_plane_comparator.h:71
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: projection_matrix.h:52
pcl::GroundPlaneComparator::~GroundPlaneComparator
~GroundPlaneComparator()
Destructor for GroundPlaneComparator.
Definition: ground_plane_comparator.h:99
pcl::Comparator::ConstPtr
shared_ptr< const Comparator< PointT > > ConstPtr
Definition: comparator.h:61
angles.h
pcl::GroundPlaneComparator::setInputCloud
void setInputCloud(const PointCloudConstPtr &cloud) override
Provide the input cloud.
Definition: ground_plane_comparator.h:106
pcl::GroundPlaneComparator::compare
bool compare(int idx1, int idx2) const override
Compare points at two indices by their plane equations.
Definition: ground_plane_comparator.h:212
pcl::GroundPlaneComparator::normals_
PointCloudNConstPtr normals_
Definition: ground_plane_comparator.h:235
pcl::GroundPlaneComparator::setAngularThreshold
virtual void setAngularThreshold(float angular_threshold)
Set the tolerance in radians for difference in normal direction between neighboring points,...
Definition: ground_plane_comparator.h:156
pcl::GroundPlaneComparator::plane_coeff_d_
shared_ptr< std::vector< float > > plane_coeff_d_
Definition: ground_plane_comparator.h:236
pcl::GroundPlaneComparator::z_axis_
Eigen::Vector3f z_axis_
Definition: ground_plane_comparator.h:241
pcl::deg2rad
float deg2rad(float alpha)
Convert an angle from degrees to radians.
Definition: angles.hpp:67
PCL_MAKE_ALIGNED_OPERATOR_NEW
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: pcl_macros.h:371
pcl::Comparator
Comparator is the base class for comparators that compare two points given some function.
Definition: comparator.h:53
pcl::GroundPlaneComparator::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: ground_plane_comparator.h:192
pcl::GroundPlaneComparator::getDistanceThreshold
float getDistanceThreshold() const
Get the distance threshold in meters (d component of plane equation) between neighboring points,...
Definition: ground_plane_comparator.h:201
pcl::GroundPlaneComparator::getPlaneCoeffD
const std::vector< float > & getPlaneCoeffD() const
Get a pointer to the vector of the d-coefficient of the planes' hessian normal form.
Definition: ground_plane_comparator.h:147
pcl::Comparator::PointCloudConstPtr
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: comparator.h:58
pcl::GroundPlaneComparator::setExpectedGroundNormal
void setExpectedGroundNormal(Eigen::Vector3f normal)
Set the expected ground plane normal with respect to the sensor.
Definition: ground_plane_comparator.h:174
pcl::PointCloud< PointNT >::Ptr
shared_ptr< PointCloud< PointNT > > Ptr
Definition: point_cloud.h:415
pcl::GroundPlaneComparator::setPlaneCoeffD
void setPlaneCoeffD(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: ground_plane_comparator.h:131
pcl::GroundPlaneComparator::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: ground_plane_comparator.h:140
pcl::GroundPlaneComparator::PointCloudNConstPtr
typename PointCloudN::ConstPtr PointCloudNConstPtr
Definition: ground_plane_comparator.h:63
pcl::PointCloud< PointNT >::ConstPtr
shared_ptr< const PointCloud< PointNT > > ConstPtr
Definition: point_cloud.h:416
pcl::GroundPlaneComparator::distance_threshold_
float distance_threshold_
Definition: ground_plane_comparator.h:239
pcl::GroundPlaneComparator::GroundPlaneComparator
GroundPlaneComparator(shared_ptr< std::vector< float > > &plane_coeff_d)
Constructor for GroundPlaneComparator.
Definition: ground_plane_comparator.h:85
pcl::Comparator::input_
PointCloudConstPtr input_
Definition: comparator.h:99
pcl::Comparator::Ptr
shared_ptr< Comparator< PointT > > Ptr
Definition: comparator.h:60
pcl::GroundPlaneComparator::depth_dependent_
bool depth_dependent_
Definition: ground_plane_comparator.h:240
pcl::shared_ptr
boost::shared_ptr< T > shared_ptr
Alias for boost::shared_ptr.
Definition: pcl_macros.h:90