Fawkes API  Fawkes Development Version
comparisons.h
1 
2 /***************************************************************************
3  * comparisons.h - PCL utilities: additional comparison functors
4  *
5  * Created: Tue Nov 08 17:50:07 2011
6  * Copyright 2011 Tim Niemueller [www.niemueller.de]
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 #ifndef __LIBS_PCL_UTILS_COMPARISONS_H_
23 #define __LIBS_PCL_UTILS_COMPARISONS_H_
24 
25 #include <pcl/point_cloud.h>
26 #include <pcl/ModelCoefficients.h>
27 #include <pcl/filters/conditional_removal.h>
28 #include <pcl/segmentation/extract_polygonal_prism_data.h>
29 
30 namespace fawkes {
31  namespace pcl_utils {
32 #if 0 /* just to make Emacs auto-indent happy */
33  }
34 }
35 #endif
36 
37 
38 /** Check if point is inside or outside a given polygon.
39  * This comparison determines if a given point is inside or outside a
40  * given polygon. A flag can be set to have an inside or outside
41  * check. The class uses pcl::isPointIn2DPolygon() to determine if the
42  * point is inside the polygon. Not that we assume planar data, for
43  * example points projected into a segmented plane.
44  * @author Tim Niemueller
45  */
46 template <typename PointT>
47 class PolygonComparison : public pcl::ComparisonBase<PointT>
48 {
49  using pcl::ComparisonBase<PointT>::capable_;
50  public:
51  /// Shared pointer.
52  typedef boost::shared_ptr<PolygonComparison<PointT> > Ptr;
53  /// Constant shared pointer.
54  typedef boost::shared_ptr<const PolygonComparison<PointT> > ConstPtr;
55 
56  /** Constructor.
57  * @param polygon polygon to compare against, it must have at least three points
58  * @param inside if true filter points inside the polygon, false for outside
59  */
60  PolygonComparison(const pcl::PointCloud<PointT> &polygon, bool inside = true)
61  : inside_(inside), polygon_(polygon)
62  {
63  capable_ = (polygon.size() >= 3);
64  }
65  /** Virtual empty destructor. */
66  virtual ~PolygonComparison() {}
67 
68  /** Evaluate for given pixel.
69  * @param point point to compare
70  * @return true if the point is inside/outside (depending on
71  * constructor parameter) the polygon, false otherwise
72  */
73  virtual bool evaluate(const PointT &point) const
74  {
75  if (inside_)
76  return pcl::isPointIn2DPolygon(point, polygon_);
77  else
78  return ! pcl::isPointIn2DPolygon(point, polygon_);
79  }
80 
81  protected:
82  /// Flag to determine whether to do inside or outside check
83  bool inside_;
84  /// The polygon to check against
86 
87  private:
88  PolygonComparison() {} // not allowed
89 };
90 
91 
92 /** Compare points' distance to a plane.
93  * This comparison calculates the distance to a given plane and makes
94  * a decision based on constructor flag and threshold.
95  * @author Tim Niemueller
96  */
97 template <typename PointT>
98 class PlaneDistanceComparison : public pcl::ComparisonBase<PointT>
99 {
100  using pcl::ComparisonBase<PointT>::capable_;
101  public:
102  /// Shared pointer.
103  typedef boost::shared_ptr<PlaneDistanceComparison<PointT> > Ptr;
104  /// Constant shared pointer.
105  typedef boost::shared_ptr<const PlaneDistanceComparison<PointT> > ConstPtr;
106 
107  /** Constructor.
108  * @param coeff planar model coefficients
109  * @param op comparison operation
110  * @param compare_val value to compare against
111  */
112  PlaneDistanceComparison(pcl::ModelCoefficients::ConstPtr coeff,
113  pcl::ComparisonOps::CompareOp op = pcl::ComparisonOps::GT,
114  float compare_val = 0.)
115  : coeff_(coeff), op_(op), compare_val_(compare_val)
116  {
117  capable_ = (coeff_->values.size() == 4);
118  }
119  /** Virtual empty destructor. */
121 
122  /** Evaluate for given pixel.
123  * @param point point to compare
124  * @return true if the setup operation using the compare value evaluates to true, false otherwise
125  */
126  virtual bool evaluate(const PointT &point) const
127  {
128  float val = (coeff_->values[0] * point.x + coeff_->values[1] * point.y +
129  coeff_->values[2] * point.z + coeff_->values[3]) /
130  sqrtf(coeff_->values[0] * coeff_->values[0] +
131  coeff_->values[1] * coeff_->values[1] +
132  coeff_->values[2] * coeff_->values[2]);
133 
134  //printf("%f > %f?: %d\n", val, compare_val_, (val > compare_val_));
135 
136  if (op_ == pcl::ComparisonOps::GT) {
137  return val > compare_val_;
138  } else if (op_ == pcl::ComparisonOps::GE) {
139  return val >= compare_val_;
140  } else if (op_ == pcl::ComparisonOps::LT) {
141  return val < compare_val_;
142  } else if (op_ == pcl::ComparisonOps::LE) {
143  return val <= compare_val_;
144  } else {
145  return val == compare_val_;
146  }
147  }
148 
149  protected:
150  /// Planar model coefficients
151  pcl::ModelCoefficients::ConstPtr coeff_;
152  /// Comparison operation
153  pcl::ComparisonOps::CompareOp op_;
154  /// Value to compare against
156 
157  private:
158  PlaneDistanceComparison() {} // not allowed
159 };
160 
161 } // end namespace pclutils
162 } // end namespace fawkes
163 
164 #endif
Compare points&#39; distance to a plane.
Definition: comparisons.h:98
boost::shared_ptr< PlaneDistanceComparison< PointT > > Ptr
Shared pointer.
Definition: comparisons.h:103
PolygonComparison(const pcl::PointCloud< PointT > &polygon, bool inside=true)
Constructor.
Definition: comparisons.h:60
pcl::ComparisonOps::CompareOp op_
Comparison operation.
Definition: comparisons.h:153
virtual bool evaluate(const PointT &point) const
Evaluate for given pixel.
Definition: comparisons.h:126
const pcl::PointCloud< PointT > & polygon_
The polygon to check against.
Definition: comparisons.h:85
virtual ~PlaneDistanceComparison()
Virtual empty destructor.
Definition: comparisons.h:120
Fawkes library namespace.
PlaneDistanceComparison(pcl::ModelCoefficients::ConstPtr coeff, pcl::ComparisonOps::CompareOp op=pcl::ComparisonOps::GT, float compare_val=0.)
Constructor.
Definition: comparisons.h:112
float compare_val_
Value to compare against.
Definition: comparisons.h:155
Check if point is inside or outside a given polygon.
Definition: comparisons.h:47
bool inside_
Flag to determine whether to do inside or outside check.
Definition: comparisons.h:83
boost::shared_ptr< PolygonComparison< PointT > > Ptr
Shared pointer.
Definition: comparisons.h:52
boost::shared_ptr< const PlaneDistanceComparison< PointT > > ConstPtr
Constant shared pointer.
Definition: comparisons.h:105
virtual bool evaluate(const PointT &point) const
Evaluate for given pixel.
Definition: comparisons.h:73
pcl::ModelCoefficients::ConstPtr coeff_
Planar model coefficients.
Definition: comparisons.h:151
virtual ~PolygonComparison()
Virtual empty destructor.
Definition: comparisons.h:66
boost::shared_ptr< const PolygonComparison< PointT > > ConstPtr
Constant shared pointer.
Definition: comparisons.h:54