Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2010-2012, Willow Garage, Inc. 00006 * 00007 * All rights reserved. 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * * Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * * Redistributions in binary form must reproduce the above 00016 * copyright notice, this list of conditions and the following 00017 * disclaimer in the documentation and/or other materials provided 00018 * with the distribution. 00019 * * Neither the name of the copyright holder(s) nor the names of its 00020 * contributors may be used to endorse or promote products derived 00021 * from this software without specific prior written permission. 00022 * 00023 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 * POSSIBILITY OF SUCH DAMAGE. 00035 * 00036 * 00037 * 00038 */ 00039 00040 #ifndef PCL_SEGMENTATION_GROUND_PLANE_COMPARATOR_H_ 00041 #define PCL_SEGMENTATION_GROUND_PLANE_COMPARATOR_H_ 00042 00043 #include <pcl/common/angles.h> 00044 #include <pcl/segmentation/comparator.h> 00045 #include <boost/make_shared.hpp> 00046 00047 namespace pcl 00048 { 00049 /** \brief GroundPlaneComparator is a Comparator for detecting smooth surfaces suitable for driving. 00050 * In conjunction with OrganizedConnectedComponentSegmentation, this allows smooth groundplanes / road surfaces to be segmented from point clouds. 00051 * 00052 * \author Alex Trevor 00053 */ 00054 template<typename PointT, typename PointNT> 00055 class GroundPlaneComparator: public Comparator<PointT> 00056 { 00057 public: 00058 typedef typename Comparator<PointT>::PointCloud PointCloud; 00059 typedef typename Comparator<PointT>::PointCloudConstPtr PointCloudConstPtr; 00060 00061 typedef typename pcl::PointCloud<PointNT> PointCloudN; 00062 typedef typename PointCloudN::Ptr PointCloudNPtr; 00063 typedef typename PointCloudN::ConstPtr PointCloudNConstPtr; 00064 00065 typedef boost::shared_ptr<GroundPlaneComparator<PointT, PointNT> > Ptr; 00066 typedef boost::shared_ptr<const GroundPlaneComparator<PointT, PointNT> > ConstPtr; 00067 00068 using pcl::Comparator<PointT>::input_; 00069 00070 /** \brief Empty constructor for GroundPlaneComparator. */ 00071 GroundPlaneComparator () 00072 : normals_ () 00073 , plane_coeff_d_ () 00074 , angular_threshold_ (cosf (pcl::deg2rad (2.0f))) 00075 , road_angular_threshold_ ( cosf(pcl::deg2rad (10.0f))) 00076 , distance_threshold_ (0.1f) 00077 , depth_dependent_ (true) 00078 , z_axis_ (Eigen::Vector3f (0.0, 0.0, 1.0) ) 00079 , desired_road_axis_ (Eigen::Vector3f(0.0, -1.0, 0.0)) 00080 { 00081 } 00082 00083 /** \brief Constructor for GroundPlaneComparator. 00084 * \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. 00085 */ 00086 GroundPlaneComparator (boost::shared_ptr<std::vector<float> >& plane_coeff_d) 00087 : normals_ () 00088 , plane_coeff_d_ (plane_coeff_d) 00089 , angular_threshold_ (cosf (pcl::deg2rad (3.0f))) 00090 , distance_threshold_ (0.1f) 00091 , depth_dependent_ (true) 00092 , z_axis_ (Eigen::Vector3f (0.0f, 0.0f, 1.0f)) 00093 , road_angular_threshold_ ( cosf(pcl::deg2rad (40.0f))) 00094 , desired_road_axis_ (Eigen::Vector3f(0.0, -1.0, 0.0)) 00095 { 00096 } 00097 00098 /** \brief Destructor for GroundPlaneComparator. */ 00099 virtual 00100 ~GroundPlaneComparator () 00101 { 00102 } 00103 /** \brief Provide the input cloud. 00104 * \param[in] cloud the input point cloud. 00105 */ 00106 virtual void 00107 setInputCloud (const PointCloudConstPtr& cloud) 00108 { 00109 input_ = cloud; 00110 } 00111 00112 /** \brief Provide a pointer to the input normals. 00113 * \param[in] normals the input normal cloud. 00114 */ 00115 inline void 00116 setInputNormals (const PointCloudNConstPtr &normals) 00117 { 00118 normals_ = normals; 00119 } 00120 00121 /** \brief Get the input normals. */ 00122 inline PointCloudNConstPtr 00123 getInputNormals () const 00124 { 00125 return (normals_); 00126 } 00127 00128 /** \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. 00129 * \param[in] plane_coeff_d a pointer to the plane coefficients. 00130 */ 00131 void 00132 setPlaneCoeffD (boost::shared_ptr<std::vector<float> >& plane_coeff_d) 00133 { 00134 plane_coeff_d_ = plane_coeff_d; 00135 } 00136 00137 /** \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. 00138 * \param[in] plane_coeff_d a pointer to the plane coefficients. 00139 */ 00140 void 00141 setPlaneCoeffD (std::vector<float>& plane_coeff_d) 00142 { 00143 plane_coeff_d_ = boost::make_shared<std::vector<float> >(plane_coeff_d); 00144 } 00145 00146 /** \brief Get a pointer to the vector of the d-coefficient of the planes' hessian normal form. */ 00147 const std::vector<float>& 00148 getPlaneCoeffD () const 00149 { 00150 return (plane_coeff_d_); 00151 } 00152 00153 /** \brief Set the tolerance in radians for difference in normal direction between neighboring points, to be considered part of the same plane. 00154 * \param[in] angular_threshold the tolerance in radians 00155 */ 00156 virtual void 00157 setAngularThreshold (float angular_threshold) 00158 { 00159 angular_threshold_ = cosf (angular_threshold); 00160 } 00161 00162 /** \brief Set the tolerance in radians for difference in normal direction between a point and the expected ground normal. 00163 * \param[in] angular_threshold the 00164 */ 00165 virtual void 00166 setGroundAngularThreshold (float angular_threshold) 00167 { 00168 road_angular_threshold_ = cosf (angular_threshold); 00169 } 00170 00171 /** \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. 00172 * \param[in] normal The normal direction of the expected ground plane. 00173 */ 00174 void 00175 setExpectedGroundNormal (Eigen::Vector3f normal) 00176 { 00177 desired_road_axis_ = normal; 00178 } 00179 00180 00181 /** \brief Get the angular threshold in radians for difference in normal direction between neighboring points, to be considered part of the same plane. */ 00182 inline float 00183 getAngularThreshold () const 00184 { 00185 return (acosf (angular_threshold_) ); 00186 } 00187 00188 /** \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. 00189 * \param[in] distance_threshold the tolerance in meters (at 1m) 00190 * \param[in] depth_dependent whether to scale the threshold based on range from the sensor (default: false) 00191 */ 00192 void 00193 setDistanceThreshold (float distance_threshold, 00194 bool depth_dependent = false) 00195 { 00196 distance_threshold_ = distance_threshold; 00197 depth_dependent_ = depth_dependent; 00198 } 00199 00200 /** \brief Get the distance threshold in meters (d component of plane equation) between neighboring points, to be considered part of the same plane. */ 00201 inline float 00202 getDistanceThreshold () const 00203 { 00204 return distance_threshold_; 00205 } 00206 00207 /** \brief Compare points at two indices by their plane equations. True if the angle between the normals is less than the angular threshold, 00208 * and the difference between the d component of the normals is less than distance threshold, else false 00209 * \param idx1 The first index for the comparison 00210 * \param idx2 The second index for the comparison 00211 */ 00212 virtual bool 00213 compare (int idx1, int idx2) const 00214 { 00215 // Normal must be similar to neighbor 00216 // Normal must be similar to expected normal 00217 float threshold = distance_threshold_; 00218 if (depth_dependent_) 00219 { 00220 Eigen::Vector3f vec = input_->points[idx1].getVector3fMap (); 00221 00222 float z = vec.dot (z_axis_); 00223 threshold *= z * z; 00224 } 00225 00226 return ( (normals_->points[idx1].getNormalVector3fMap ().dot (desired_road_axis_) > road_angular_threshold_) && 00227 (normals_->points[idx1].getNormalVector3fMap ().dot (normals_->points[idx2].getNormalVector3fMap () ) > angular_threshold_ )); 00228 00229 // Euclidean proximity of neighbors does not seem to be required -- pixel adjacency handles this well enough 00230 //return ( (normals_->points[idx1].getNormalVector3fMap ().dot (desired_road_axis_) > road_angular_threshold_) && 00231 // (normals_->points[idx1].getNormalVector3fMap ().dot (normals_->points[idx2].getNormalVector3fMap () ) > angular_threshold_ ) && 00232 // (pcl::euclideanDistance (input_->points[idx1], input_->points[idx2]) < distance_threshold_ )); 00233 } 00234 00235 protected: 00236 PointCloudNConstPtr normals_; 00237 boost::shared_ptr<std::vector<float> > plane_coeff_d_; 00238 float angular_threshold_; 00239 float road_angular_threshold_; 00240 float distance_threshold_; 00241 bool depth_dependent_; 00242 Eigen::Vector3f z_axis_; 00243 Eigen::Vector3f desired_road_axis_; 00244 00245 public: 00246 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00247 }; 00248 } 00249 00250 #endif // PCL_SEGMENTATION_GROUND_PLANE_COMPARATOR_H_