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_EUCLIDEAN_CLUSTER_COMPARATOR_H_ 00041 #define PCL_SEGMENTATION_EUCLIDEAN_CLUSTER_COMPARATOR_H_ 00042 00043 #include <pcl/segmentation/boost.h> 00044 #include <pcl/segmentation/comparator.h> 00045 00046 namespace pcl 00047 { 00048 /** \brief EuclideanClusterComparator is a comparator used for finding clusters supported by planar surfaces. 00049 * This needs to be run as a second pass after extracting planar surfaces, using MultiPlaneSegmentation for example. 00050 * 00051 * \author Alex Trevor 00052 */ 00053 template<typename PointT, typename PointNT, typename PointLT> 00054 class EuclideanClusterComparator: public Comparator<PointT> 00055 { 00056 public: 00057 typedef typename Comparator<PointT>::PointCloud PointCloud; 00058 typedef typename Comparator<PointT>::PointCloudConstPtr PointCloudConstPtr; 00059 00060 typedef typename pcl::PointCloud<PointNT> PointCloudN; 00061 typedef typename PointCloudN::Ptr PointCloudNPtr; 00062 typedef typename PointCloudN::ConstPtr PointCloudNConstPtr; 00063 00064 typedef typename pcl::PointCloud<PointLT> PointCloudL; 00065 typedef typename PointCloudL::Ptr PointCloudLPtr; 00066 typedef typename PointCloudL::ConstPtr PointCloudLConstPtr; 00067 00068 typedef boost::shared_ptr<EuclideanClusterComparator<PointT, PointNT, PointLT> > Ptr; 00069 typedef boost::shared_ptr<const EuclideanClusterComparator<PointT, PointNT, PointLT> > ConstPtr; 00070 00071 using pcl::Comparator<PointT>::input_; 00072 00073 /** \brief Empty constructor for EuclideanClusterComparator. */ 00074 EuclideanClusterComparator () 00075 : normals_ () 00076 , angular_threshold_ (0.0f) 00077 , distance_threshold_ (0.005f) 00078 , depth_dependent_ () 00079 , z_axis_ () 00080 { 00081 } 00082 00083 /** \brief Destructor for EuclideanClusterComparator. */ 00084 virtual 00085 ~EuclideanClusterComparator () 00086 { 00087 } 00088 00089 virtual void 00090 setInputCloud (const PointCloudConstPtr& cloud) 00091 { 00092 input_ = cloud; 00093 Eigen::Matrix3f rot = input_->sensor_orientation_.toRotationMatrix (); 00094 z_axis_ = rot.col (2); 00095 } 00096 00097 /** \brief Provide a pointer to the input normals. 00098 * \param[in] normals the input normal cloud 00099 */ 00100 inline void 00101 setInputNormals (const PointCloudNConstPtr &normals) 00102 { 00103 normals_ = normals; 00104 } 00105 00106 /** \brief Get the input normals. */ 00107 inline PointCloudNConstPtr 00108 getInputNormals () const 00109 { 00110 return (normals_); 00111 } 00112 00113 /** \brief Set the tolerance in radians for difference in normal direction between neighboring points, to be considered part of the same plane. 00114 * \param[in] angular_threshold the tolerance in radians 00115 */ 00116 virtual inline void 00117 setAngularThreshold (float angular_threshold) 00118 { 00119 angular_threshold_ = cosf (angular_threshold); 00120 } 00121 00122 /** \brief Get the angular threshold in radians for difference in normal direction between neighboring points, to be considered part of the same plane. */ 00123 inline float 00124 getAngularThreshold () const 00125 { 00126 return (acos (angular_threshold_) ); 00127 } 00128 00129 /** \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. 00130 * \param[in] distance_threshold the tolerance in meters 00131 */ 00132 inline void 00133 setDistanceThreshold (float distance_threshold, bool depth_dependent) 00134 { 00135 distance_threshold_ = distance_threshold; 00136 depth_dependent_ = depth_dependent; 00137 } 00138 00139 /** \brief Get the distance threshold in meters (d component of plane equation) between neighboring points, to be considered part of the same plane. */ 00140 inline float 00141 getDistanceThreshold () const 00142 { 00143 return (distance_threshold_); 00144 } 00145 00146 /** \brief Set label cloud 00147 * \param[in] labels The label cloud 00148 */ 00149 void 00150 setLabels (PointCloudLPtr& labels) 00151 { 00152 labels_ = labels; 00153 } 00154 00155 /** \brief Set labels in the label cloud to exclude. 00156 * \param exclude_labels a vector of bools corresponding to whether or not a given label should be considered 00157 */ 00158 void 00159 setExcludeLabels (std::vector<bool>& exclude_labels) 00160 { 00161 exclude_labels_ = boost::make_shared<std::vector<bool> >(exclude_labels); 00162 } 00163 00164 /** \brief Compare points at two indices by their plane equations. True if the angle between the normals is less than the angular threshold, 00165 * and the difference between the d component of the normals is less than distance threshold, else false 00166 * \param idx1 The first index for the comparison 00167 * \param idx2 The second index for the comparison 00168 */ 00169 virtual bool 00170 compare (int idx1, int idx2) const 00171 { 00172 int label1 = labels_->points[idx1].label; 00173 int label2 = labels_->points[idx2].label; 00174 00175 if (label1 == -1 || label2 == -1) 00176 return false; 00177 00178 if ( (*exclude_labels_)[label1] || (*exclude_labels_)[label2]) 00179 return false; 00180 00181 float dist_threshold = distance_threshold_; 00182 if (depth_dependent_) 00183 { 00184 Eigen::Vector3f vec = input_->points[idx1].getVector3fMap (); 00185 float z = vec.dot (z_axis_); 00186 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 return (dist < dist_threshold); 00195 } 00196 00197 protected: 00198 PointCloudNConstPtr normals_; 00199 PointCloudLPtr labels_; 00200 00201 boost::shared_ptr<std::vector<bool> > exclude_labels_; 00202 float angular_threshold_; 00203 float distance_threshold_; 00204 bool depth_dependent_; 00205 Eigen::Vector3f z_axis_; 00206 }; 00207 } 00208 00209 #endif // PCL_SEGMENTATION_PLANE_COEFFICIENT_COMPARATOR_H_