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_RGB_SEGMENTATION_PLANE_COEFFICIENT_COMPARATOR_H_ 00041 #define PCL_RGB_SEGMENTATION_PLANE_COEFFICIENT_COMPARATOR_H_ 00042 00043 #include <pcl/segmentation/boost.h> 00044 #include <pcl/segmentation/plane_coefficient_comparator.h> 00045 00046 namespace pcl 00047 { 00048 /** \brief RGBPlaneCoefficientComparator is a Comparator that operates on plane coefficients, 00049 * for use in planar segmentation. Also takes into account RGB, so we can segmented different colored co-planar regions. 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 RGBPlaneCoefficientComparator: 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<RGBPlaneCoefficientComparator<PointT, PointNT> > Ptr; 00066 typedef boost::shared_ptr<const RGBPlaneCoefficientComparator<PointT, PointNT> > ConstPtr; 00067 00068 using pcl::Comparator<PointT>::input_; 00069 using pcl::PlaneCoefficientComparator<PointT, PointNT>::normals_; 00070 using pcl::PlaneCoefficientComparator<PointT, PointNT>::angular_threshold_; 00071 using pcl::PlaneCoefficientComparator<PointT, PointNT>::distance_threshold_; 00072 00073 /** \brief Empty constructor for RGBPlaneCoefficientComparator. */ 00074 RGBPlaneCoefficientComparator () 00075 : color_threshold_ (50.0f) 00076 { 00077 } 00078 00079 /** \brief Constructor for RGBPlaneCoefficientComparator. 00080 * \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. 00081 */ 00082 RGBPlaneCoefficientComparator (boost::shared_ptr<std::vector<float> >& plane_coeff_d) 00083 : PlaneCoefficientComparator<PointT, PointNT> (plane_coeff_d), color_threshold_ (50.0f) 00084 { 00085 } 00086 00087 /** \brief Destructor for RGBPlaneCoefficientComparator. */ 00088 virtual 00089 ~RGBPlaneCoefficientComparator () 00090 { 00091 } 00092 00093 /** \brief Set the tolerance in color space between neighboring points, to be considered part of the same plane. 00094 * \param[in] color_threshold The distance in color space 00095 */ 00096 inline void 00097 setColorThreshold (float color_threshold) 00098 { 00099 color_threshold_ = color_threshold * color_threshold; 00100 } 00101 00102 /** \brief Get the color threshold between neighboring points, to be considered part of the same plane. */ 00103 inline float 00104 getColorThreshold () const 00105 { 00106 return (color_threshold_); 00107 } 00108 00109 /** \brief Compare two neighboring points, by using normal information, euclidean distance, and color information. 00110 * \param[in] idx1 The index of the first point. 00111 * \param[in] idx2 The index of the second point. 00112 */ 00113 bool 00114 compare (int idx1, int idx2) const 00115 { 00116 float dx = input_->points[idx1].x - input_->points[idx2].x; 00117 float dy = input_->points[idx1].y - input_->points[idx2].y; 00118 float dz = input_->points[idx1].z - input_->points[idx2].z; 00119 float dist = sqrtf (dx*dx + dy*dy + dz*dz); 00120 int dr = input_->points[idx1].r - input_->points[idx2].r; 00121 int dg = input_->points[idx1].g - input_->points[idx2].g; 00122 int db = input_->points[idx1].b - input_->points[idx2].b; 00123 //Note: This is not the best metric for color comparisons, we should probably use HSV space. 00124 float color_dist = static_cast<float> (dr*dr + dg*dg + db*db); 00125 return ( (dist < distance_threshold_) 00126 && (normals_->points[idx1].getNormalVector3fMap ().dot (normals_->points[idx2].getNormalVector3fMap () ) > angular_threshold_ ) 00127 && (color_dist < color_threshold_)); 00128 } 00129 00130 protected: 00131 float color_threshold_; 00132 }; 00133 } 00134 00135 #endif // PCL_SEGMENTATION_PLANE_COEFFICIENT_COMPARATOR_H_