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$ 00037 * 00038 */ 00039 00040 #ifndef PCL_SEGMENTATION_PLANE_COEFFICIENT_COMPARATOR_H_ 00041 #define PCL_SEGMENTATION_PLANE_COEFFICIENT_COMPARATOR_H_ 00042 00043 #include <pcl/common/angles.h> 00044 #include <pcl/segmentation/boost.h> 00045 #include <pcl/segmentation/comparator.h> 00046 00047 namespace pcl 00048 { 00049 /** \brief PlaneCoefficientComparator is a Comparator that operates on plane coefficients, for use in planar segmentation. 00050 * In conjunction with OrganizedConnectedComponentSegmentation, this allows planes to be segmented from organized data. 00051 * 00052 * \author Alex Trevor 00053 */ 00054 template<typename PointT, typename PointNT> 00055 class PlaneCoefficientComparator: 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<PlaneCoefficientComparator<PointT, PointNT> > Ptr; 00066 typedef boost::shared_ptr<const PlaneCoefficientComparator<PointT, PointNT> > ConstPtr; 00067 00068 using pcl::Comparator<PointT>::input_; 00069 00070 /** \brief Empty constructor for PlaneCoefficientComparator. */ 00071 PlaneCoefficientComparator () 00072 : normals_ () 00073 , plane_coeff_d_ () 00074 , angular_threshold_ (pcl::deg2rad (2.0f)) 00075 , distance_threshold_ (0.02f) 00076 , depth_dependent_ (true) 00077 , z_axis_ (Eigen::Vector3f (0.0, 0.0, 1.0) ) 00078 { 00079 } 00080 00081 /** \brief Constructor for PlaneCoefficientComparator. 00082 * \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. 00083 */ 00084 PlaneCoefficientComparator (boost::shared_ptr<std::vector<float> >& plane_coeff_d) 00085 : normals_ () 00086 , plane_coeff_d_ (plane_coeff_d) 00087 , angular_threshold_ (pcl::deg2rad (2.0f)) 00088 , distance_threshold_ (0.02f) 00089 , depth_dependent_ (true) 00090 , z_axis_ (Eigen::Vector3f (0.0f, 0.0f, 1.0f) ) 00091 { 00092 } 00093 00094 /** \brief Destructor for PlaneCoefficientComparator. */ 00095 virtual 00096 ~PlaneCoefficientComparator () 00097 { 00098 } 00099 00100 virtual void 00101 setInputCloud (const PointCloudConstPtr& cloud) 00102 { 00103 input_ = cloud; 00104 } 00105 00106 /** \brief Provide a pointer to the input normals. 00107 * \param[in] normals the input normal cloud 00108 */ 00109 inline void 00110 setInputNormals (const PointCloudNConstPtr &normals) 00111 { 00112 normals_ = normals; 00113 } 00114 00115 /** \brief Get the input normals. */ 00116 inline PointCloudNConstPtr 00117 getInputNormals () const 00118 { 00119 return (normals_); 00120 } 00121 00122 /** \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. 00123 * \param[in] plane_coeff_d a pointer to the plane coefficients. 00124 */ 00125 void 00126 setPlaneCoeffD (boost::shared_ptr<std::vector<float> >& plane_coeff_d) 00127 { 00128 plane_coeff_d_ = plane_coeff_d; 00129 } 00130 00131 /** \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. 00132 * \param[in] plane_coeff_d a pointer to the plane coefficients. 00133 */ 00134 void 00135 setPlaneCoeffD (std::vector<float>& plane_coeff_d) 00136 { 00137 plane_coeff_d_ = boost::make_shared<std::vector<float> >(plane_coeff_d); 00138 } 00139 00140 /** \brief Get a pointer to the vector of the d-coefficient of the planes' hessian normal form. */ 00141 const std::vector<float>& 00142 getPlaneCoeffD () const 00143 { 00144 return (plane_coeff_d_); 00145 } 00146 00147 /** \brief Set the tolerance in radians for difference in normal direction between neighboring points, to be considered part of the same plane. 00148 * \param[in] angular_threshold the tolerance in radians 00149 */ 00150 virtual void 00151 setAngularThreshold (float angular_threshold) 00152 { 00153 angular_threshold_ = cosf (angular_threshold); 00154 } 00155 00156 /** \brief Get the angular threshold in radians for difference in normal direction between neighboring points, to be considered part of the same plane. */ 00157 inline float 00158 getAngularThreshold () const 00159 { 00160 return (acosf (angular_threshold_) ); 00161 } 00162 00163 /** \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. 00164 * \param[in] distance_threshold the tolerance in meters (at 1m) 00165 * \param[in] depth_dependent whether to scale the threshold based on range from the sensor (default: false) 00166 */ 00167 void 00168 setDistanceThreshold (float distance_threshold, 00169 bool depth_dependent = false) 00170 { 00171 distance_threshold_ = distance_threshold; 00172 depth_dependent_ = depth_dependent; 00173 } 00174 00175 /** \brief Get the distance threshold in meters (d component of plane equation) between neighboring points, to be considered part of the same plane. */ 00176 inline float 00177 getDistanceThreshold () const 00178 { 00179 return (distance_threshold_); 00180 } 00181 00182 /** \brief Compare points at two indices by their plane equations. True if the angle between the normals is less than the angular threshold, 00183 * and the difference between the d component of the normals is less than distance threshold, else false 00184 * \param idx1 The first index for the comparison 00185 * \param idx2 The second index for the comparison 00186 */ 00187 virtual bool 00188 compare (int idx1, int idx2) const 00189 { 00190 float threshold = distance_threshold_; 00191 if (depth_dependent_) 00192 { 00193 Eigen::Vector3f vec = input_->points[idx1].getVector3fMap (); 00194 00195 float z = vec.dot (z_axis_); 00196 threshold *= z * z; 00197 } 00198 return ( (fabs ((*plane_coeff_d_)[idx1] - (*plane_coeff_d_)[idx2]) < threshold) 00199 && (normals_->points[idx1].getNormalVector3fMap ().dot (normals_->points[idx2].getNormalVector3fMap () ) > angular_threshold_ ) ); 00200 } 00201 00202 protected: 00203 PointCloudNConstPtr normals_; 00204 boost::shared_ptr<std::vector<float> > plane_coeff_d_; 00205 float angular_threshold_; 00206 float distance_threshold_; 00207 bool depth_dependent_; 00208 Eigen::Vector3f z_axis_; 00209 00210 public: 00211 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00212 }; 00213 } 00214 00215 #endif // PCL_SEGMENTATION_PLANE_COEFFICIENT_COMPARATOR_H_