Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * comparisons.h - PCL utilities: additional comparison functors 00004 * 00005 * Created: Tue Nov 08 17:50:07 2011 00006 * Copyright 2011 Tim Niemueller [www.niemueller.de] 00007 ****************************************************************************/ 00008 00009 /* This program is free software; you can redistribute it and/or modify 00010 * it under the terms of the GNU General Public License as published by 00011 * the Free Software Foundation; either version 2 of the License, or 00012 * (at your option) any later version. 00013 * 00014 * This program is distributed in the hope that it will be useful, 00015 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00016 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00017 * GNU Library General Public License for more details. 00018 * 00019 * Read the full text in the LICENSE.GPL file in the doc directory. 00020 */ 00021 00022 #ifndef __LIBS_PCL_UTILS_COMPARISONS__H_ 00023 #define __LIBS_PCL_UTILS_COMPARISONS_H_ 00024 00025 #include <pcl/point_cloud.h> 00026 #include <pcl/ModelCoefficients.h> 00027 #include <pcl/filters/conditional_removal.h> 00028 #include <pcl/segmentation/extract_polygonal_prism_data.h> 00029 00030 namespace fawkes { 00031 namespace pcl_utils { 00032 #if 0 /* just to make Emacs auto-indent happy */ 00033 } 00034 } 00035 #endif 00036 00037 00038 /** Check if point is inside or outside a given polygon. 00039 * This comparison determines if a given point is inside or outside a 00040 * given polygon. A flag can be set to have an inside or outside 00041 * check. The class uses pcl::isPointIn2DPolygon() to determine if the 00042 * point is inside the polygon. Not that we assume planar data, for 00043 * example points projected into a segmented plane. 00044 * @author Tim Niemueller 00045 */ 00046 template <typename PointT> 00047 class PolygonComparison : public pcl::ComparisonBase<PointT> 00048 { 00049 using pcl::ComparisonBase<PointT>::capable_; 00050 public: 00051 /// Shared pointer. 00052 typedef boost::shared_ptr<PolygonComparison<PointT> > Ptr; 00053 /// Constant shared pointer. 00054 typedef boost::shared_ptr<const PolygonComparison<PointT> > ConstPtr; 00055 00056 /** Constructor. 00057 * @param polygon polygon to compare against, it must have at least three points 00058 * @param inside if true filter points inside the polygon, false for outside 00059 */ 00060 PolygonComparison(const pcl::PointCloud<PointT> &polygon, bool inside = true) 00061 : inside_(inside), polygon_(polygon) 00062 { 00063 capable_ = (polygon.size() >= 3); 00064 } 00065 /** Virtual empty destructor. */ 00066 virtual ~PolygonComparison() {} 00067 00068 /** Evaluate for given pixel. 00069 * @param point point to compare 00070 * @return true if the point is inside/outside (depending on 00071 * constructor parameter) the polygon, false otherwise 00072 */ 00073 virtual bool evaluate(const PointT &point) const 00074 { 00075 if (inside_) 00076 return pcl::isPointIn2DPolygon(point, polygon_); 00077 else 00078 return ! pcl::isPointIn2DPolygon(point, polygon_); 00079 } 00080 00081 protected: 00082 /// Flag to determine whether to do inside or outside check 00083 bool inside_; 00084 /// The polygon to check against 00085 const pcl::PointCloud<PointT> &polygon_; 00086 00087 private: 00088 PolygonComparison() {} // not allowed 00089 }; 00090 00091 00092 /** Compare points' distance to a plane. 00093 * This comparison calculates the distance to a given plane and makes 00094 * a decision based on constructor flag and threshold. 00095 * @author Tim Niemueller 00096 */ 00097 template <typename PointT> 00098 class PlaneDistanceComparison : public pcl::ComparisonBase<PointT> 00099 { 00100 using pcl::ComparisonBase<PointT>::capable_; 00101 public: 00102 /// Shared pointer. 00103 typedef boost::shared_ptr<PlaneDistanceComparison<PointT> > Ptr; 00104 /// Constant shared pointer. 00105 typedef boost::shared_ptr<const PlaneDistanceComparison<PointT> > ConstPtr; 00106 00107 /** Constructor. 00108 * @param coeff planar model coefficients 00109 * @param op comparison operation 00110 * @param compare_val value to compare against 00111 */ 00112 PlaneDistanceComparison(pcl::ModelCoefficients::ConstPtr coeff, 00113 pcl::ComparisonOps::CompareOp op = pcl::ComparisonOps::GT, 00114 float compare_val = 0.) 00115 : coeff_(coeff), op_(op), compare_val_(compare_val) 00116 { 00117 capable_ = (coeff_->values.size() == 4); 00118 } 00119 /** Virtual empty destructor. */ 00120 virtual ~PlaneDistanceComparison() {} 00121 00122 /** Evaluate for given pixel. 00123 * @param point point to compare 00124 * @return true if the setup operation using the compare value evaluates to true, false otherwise 00125 */ 00126 virtual bool evaluate(const PointT &point) const 00127 { 00128 float val = (coeff_->values[0] * point.x + coeff_->values[1] * point.y + 00129 coeff_->values[2] * point.z + coeff_->values[3]) / 00130 sqrtf(coeff_->values[0] * coeff_->values[0] + 00131 coeff_->values[1] * coeff_->values[1] + 00132 coeff_->values[2] * coeff_->values[2]); 00133 00134 //printf("%f > %f?: %d\n", val, compare_val_, (val > compare_val_)); 00135 00136 if (op_ == pcl::ComparisonOps::GT) { 00137 return val > compare_val_; 00138 } else if (op_ == pcl::ComparisonOps::GE) { 00139 return val >= compare_val_; 00140 } else if (op_ == pcl::ComparisonOps::LT) { 00141 return val < compare_val_; 00142 } else if (op_ == pcl::ComparisonOps::LE) { 00143 return val <= compare_val_; 00144 } else { 00145 return val == compare_val_; 00146 } 00147 } 00148 00149 protected: 00150 /// Planar model coefficients 00151 pcl::ModelCoefficients::ConstPtr coeff_; 00152 /// Comparison operation 00153 pcl::ComparisonOps::CompareOp op_; 00154 /// Value to compare against 00155 float compare_val_; 00156 00157 private: 00158 PlaneDistanceComparison() {} // not allowed 00159 }; 00160 00161 } // end namespace pclutils 00162 } // end namespace fawkes 00163 00164 #endif