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 * $Id: extract_clusters.h 5027 2012-03-12 03:10:45Z rusu $ 00037 * 00038 */ 00039 00040 #ifndef PCL_SEGMENTATION_EDGE_AWARE_PLANE_COMPARATOR_H_ 00041 #define PCL_SEGMENTATION_EDGE_AWARE_PLANE_COMPARATOR_H_ 00042 00043 #include <pcl/segmentation/boost.h> 00044 #include <pcl/segmentation/plane_coefficient_comparator.h> 00045 00046 namespace pcl 00047 { 00048 /** \brief EdgeAwarePlaneComparator is a Comparator that operates on plane coefficients, 00049 * for use in planar segmentation. 00050 * In conjunction with OrganizedConnectedComponentSegmentation, this allows planes to be segmented from organized data. 00051 * 00052 * \author Stefan Holzer, Alex Trevor 00053 */ 00054 template<typename PointT, typename PointNT> 00055 class EdgeAwarePlaneComparator: public PlaneCoefficientComparator<PointT, PointNT> 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<EdgeAwarePlaneComparator<PointT, PointNT> > Ptr; 00066 typedef boost::shared_ptr<const EdgeAwarePlaneComparator<PointT, PointNT> > ConstPtr; 00067 00068 using pcl::PlaneCoefficientComparator<PointT, PointNT>::input_; 00069 using pcl::PlaneCoefficientComparator<PointT, PointNT>::normals_; 00070 using pcl::PlaneCoefficientComparator<PointT, PointNT>::plane_coeff_d_; 00071 using pcl::PlaneCoefficientComparator<PointT, PointNT>::angular_threshold_; 00072 using pcl::PlaneCoefficientComparator<PointT, PointNT>::distance_threshold_; 00073 using pcl::PlaneCoefficientComparator<PointT, PointNT>::depth_dependent_; 00074 using pcl::PlaneCoefficientComparator<PointT, PointNT>::z_axis_; 00075 00076 /** \brief Empty constructor for PlaneCoefficientComparator. */ 00077 EdgeAwarePlaneComparator () : 00078 distance_map_threshold_ (5), 00079 curvature_threshold_ (0.04f), 00080 euclidean_distance_threshold_ (0.04f) 00081 { 00082 } 00083 00084 /** \brief Empty constructor for PlaneCoefficientComparator. 00085 * \param[in] distance_map the distance map to use 00086 */ 00087 EdgeAwarePlaneComparator (const float *distance_map) : 00088 distance_map_ (distance_map), 00089 distance_map_threshold_ (5), 00090 curvature_threshold_ (0.04f), 00091 euclidean_distance_threshold_ (0.04f) 00092 { 00093 } 00094 00095 /** \brief Destructor for PlaneCoefficientComparator. */ 00096 virtual 00097 ~EdgeAwarePlaneComparator () 00098 { 00099 } 00100 00101 /** \brief Set a distance map to use. For an example of a valid distance map see 00102 * \ref OrganizedIntegralImageNormalEstimation 00103 * \param[in] distance_map the distance map to use 00104 */ 00105 inline void 00106 setDistanceMap (const float *distance_map) 00107 { 00108 distance_map_ = distance_map; 00109 } 00110 00111 /** \brief Return the distance map used. */ 00112 const float* 00113 getDistanceMap () const 00114 { 00115 return (distance_map_); 00116 } 00117 00118 /** \brief Set the curvature threshold for creating a new segment 00119 * \param[in] curvature_threshold a threshold for the curvature 00120 */ 00121 void 00122 setCurvatureThreshold (float curvature_threshold) 00123 { 00124 curvature_threshold_ = curvature_threshold; 00125 } 00126 00127 /** \brief Get the curvature threshold. */ 00128 inline float 00129 getCurvatureThreshold () const 00130 { 00131 return (curvature_threshold_); 00132 } 00133 00134 /** \brief Set the distance map threshold -- the number of pixel away from a border / nan 00135 * \param[in] distance_map_threshold the distance map threshold 00136 */ 00137 void 00138 setDistanceMapThreshold (float distance_map_threshold) 00139 { 00140 distance_map_threshold_ = distance_map_threshold; 00141 } 00142 00143 /** \brief Get the distance map threshold (in pixels). */ 00144 inline float 00145 getDistanceMapThreshold () const 00146 { 00147 return (distance_map_threshold_); 00148 } 00149 00150 /** \brief Set the euclidean distance threshold. 00151 * \param[in] euclidean_distance_threshold the euclidean distance threshold in meters 00152 */ 00153 void 00154 setEuclideanDistanceThreshold (float euclidean_distance_threshold) 00155 { 00156 euclidean_distance_threshold_ = euclidean_distance_threshold; 00157 } 00158 00159 /** \brief Get the euclidean distance threshold. */ 00160 inline float 00161 getEuclideanDistanceThreshold () const 00162 { 00163 return (euclidean_distance_threshold_); 00164 } 00165 00166 protected: 00167 /** \brief Compare two neighboring points, by using normal information, curvature, and euclidean distance information. 00168 * \param[in] idx1 The index of the first point. 00169 * \param[in] idx2 The index of the second point. 00170 */ 00171 bool 00172 compare (int idx1, int idx2) const 00173 { 00174 // Note: there are two distance thresholds here that make sense to scale with depth. 00175 // dist_threshold is on the perpendicular distance to the plane, as in plane comparator 00176 // We additionally check euclidean distance to ensure that we don't have neighboring coplanar points 00177 // that aren't close in euclidean space (think two tables separated by a meter, viewed from an angle 00178 // where the surfaces are adjacent in image space). 00179 float dist_threshold = distance_threshold_; 00180 float euclidean_dist_threshold = euclidean_distance_threshold_; 00181 if (depth_dependent_) 00182 { 00183 Eigen::Vector3f vec = input_->points[idx1].getVector3fMap (); 00184 float z = vec.dot (z_axis_); 00185 dist_threshold *= z * z; 00186 euclidean_dist_threshold *= z * z; 00187 } 00188 00189 float dx = input_->points[idx1].x - input_->points[idx2].x; 00190 float dy = input_->points[idx1].y - input_->points[idx2].y; 00191 float dz = input_->points[idx1].z - input_->points[idx2].z; 00192 float dist = sqrtf (dx*dx + dy*dy + dz*dz); 00193 00194 bool normal_ok = (normals_->points[idx1].getNormalVector3fMap ().dot (normals_->points[idx2].getNormalVector3fMap () ) > angular_threshold_ ); 00195 bool dist_ok = (dist < euclidean_dist_threshold); 00196 00197 bool curvature_ok = normals_->points[idx1].curvature < curvature_threshold_; 00198 bool plane_d_ok = fabs ((*plane_coeff_d_)[idx1] - (*plane_coeff_d_)[idx2]) < dist_threshold; 00199 00200 if (distance_map_[idx1] < distance_map_threshold_) 00201 curvature_ok = false; 00202 00203 return (dist_ok && normal_ok && curvature_ok && plane_d_ok); 00204 } 00205 00206 protected: 00207 const float* distance_map_; 00208 int distance_map_threshold_; 00209 float curvature_threshold_; 00210 float euclidean_distance_threshold_; 00211 }; 00212 } 00213 00214 #endif // PCL_SEGMENTATION_PLANE_COEFFICIENT_COMPARATOR_H_