Fawkes API  Fawkes Development Version
comparisons.h
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