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/ModelCoefficients.h>
26 #include <pcl/filters/conditional_removal.h>
27 #include <pcl/point_cloud.h>
28 #include <pcl/segmentation/extract_polygonal_prism_data.h>
29 
30 namespace fawkes {
31 namespace pcl_utils {
32 
33 /** Check if point is inside or outside a given polygon.
34  * This comparison determines if a given point is inside or outside a
35  * given polygon. A flag can be set to have an inside or outside
36  * check. The class uses pcl::isPointIn2DPolygon() to determine if the
37  * point is inside the polygon. Not that we assume planar data, for
38  * example points projected into a segmented plane.
39  * @author Tim Niemueller
40  */
41 template <typename PointT>
42 class PolygonComparison : public pcl::ComparisonBase<PointT>
43 {
44  using pcl::ComparisonBase<PointT>::capable_;
45 
46 public:
47  /// Shared pointer.
48  typedef boost::shared_ptr<PolygonComparison<PointT>> Ptr;
49  /// Constant shared pointer.
50  typedef boost::shared_ptr<const PolygonComparison<PointT>> ConstPtr;
51 
52  /** Constructor.
53  * @param polygon polygon to compare against, it must have at least three points
54  * @param inside if true filter points inside the polygon, false for outside
55  */
56  PolygonComparison(const pcl::PointCloud<PointT> &polygon, bool inside = true)
57  : inside_(inside), polygon_(polygon)
58  {
59  capable_ = (polygon.size() >= 3);
60  }
61  /** Virtual empty destructor. */
62  virtual ~PolygonComparison()
63  {
64  }
65 
66  /** Evaluate for given pixel.
67  * @param point point to compare
68  * @return true if the point is inside/outside (depending on
69  * constructor parameter) the polygon, false otherwise
70  */
71  virtual bool
72  evaluate(const PointT &point) const
73  {
74  if (inside_)
75  return pcl::isPointIn2DPolygon(point, polygon_);
76  else
77  return !pcl::isPointIn2DPolygon(point, polygon_);
78  }
79 
80 protected:
81  /// Flag to determine whether to do inside or outside check
82  bool inside_;
83  /// The polygon to check against
85 
86 private:
88  {
89  } // not allowed
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 
102 public:
103  /// Shared pointer.
104  typedef boost::shared_ptr<PlaneDistanceComparison<PointT>> Ptr;
105  /// Constant shared pointer.
106  typedef boost::shared_ptr<const PlaneDistanceComparison<PointT>> ConstPtr;
107 
108  /** Constructor.
109  * @param coeff planar model coefficients
110  * @param op comparison operation
111  * @param compare_val value to compare against
112  */
113  PlaneDistanceComparison(pcl::ModelCoefficients::ConstPtr coeff,
114  pcl::ComparisonOps::CompareOp op = pcl::ComparisonOps::GT,
115  float compare_val = 0.)
116  : coeff_(coeff), op_(op), compare_val_(compare_val)
117  {
118  capable_ = (coeff_->values.size() == 4);
119  }
120  /** Virtual empty destructor. */
121  virtual ~PlaneDistanceComparison()
122  {
123  }
124 
125  /** Evaluate for given pixel.
126  * @param point point to compare
127  * @return true if the setup operation using the compare value evaluates to true, false otherwise
128  */
129  virtual bool
130  evaluate(const PointT &point) const
131  {
132  float val =
133  (coeff_->values[0] * point.x + coeff_->values[1] * point.y + coeff_->values[2] * point.z
134  + coeff_->values[3])
135  / sqrtf(coeff_->values[0] * coeff_->values[0] + coeff_->values[1] * coeff_->values[1]
136  + coeff_->values[2] * coeff_->values[2]);
137 
138  //printf("%f > %f?: %d\n", val, compare_val_, (val > compare_val_));
139 
140  if (op_ == pcl::ComparisonOps::GT) {
141  return val > compare_val_;
142  } else if (op_ == pcl::ComparisonOps::GE) {
143  return val >= compare_val_;
144  } else if (op_ == pcl::ComparisonOps::LT) {
145  return val < compare_val_;
146  } else if (op_ == pcl::ComparisonOps::LE) {
147  return val <= compare_val_;
148  } else {
149  return val == compare_val_;
150  }
151  }
152 
153 protected:
154  /// Planar model coefficients
155  pcl::ModelCoefficients::ConstPtr coeff_;
156  /// Comparison operation
157  pcl::ComparisonOps::CompareOp op_;
158  /// Value to compare against
159  float compare_val_;
160 
161 private:
163  {
164  } // not allowed
165 };
166 
167 } // namespace pcl_utils
168 } // end namespace fawkes
169 
170 #endif
fawkes::pcl_utils::PolygonComparison::~PolygonComparison
virtual ~PolygonComparison()
Virtual empty destructor.
Definition: comparisons.h:77
fawkes::pcl_utils::PolygonComparison::ConstPtr
boost::shared_ptr< const PolygonComparison< PointT > > ConstPtr
Constant shared pointer.
Definition: comparisons.h:65
fawkes::pcl_utils::PolygonComparison::Ptr
boost::shared_ptr< PolygonComparison< PointT > > Ptr
Shared pointer.
Definition: comparisons.h:63
fawkes::pcl_utils::PlaneDistanceComparison::evaluate
virtual bool evaluate(const PointT &point) const
Evaluate for given pixel.
Definition: comparisons.h:140
fawkes::pcl_utils::PlaneDistanceComparison::op_
pcl::ComparisonOps::CompareOp op_
Comparison operation.
Definition: comparisons.h:167
fawkes::pcl_utils::PolygonComparison::polygon_
const pcl::PointCloud< PointT > & polygon_
The polygon to check against.
Definition: comparisons.h:99
fawkes::pcl_utils::PlaneDistanceComparison::compare_val_
float compare_val_
Value to compare against.
Definition: comparisons.h:169
fawkes::pcl_utils::PlaneDistanceComparison::PlaneDistanceComparison
PlaneDistanceComparison(pcl::ModelCoefficients::ConstPtr coeff, pcl::ComparisonOps::CompareOp op=pcl::ComparisonOps::GT, float compare_val=0.)
Constructor.
Definition: comparisons.h:123
fawkes::pcl_utils::PlaneDistanceComparison::coeff_
pcl::ModelCoefficients::ConstPtr coeff_
Planar model coefficients.
Definition: comparisons.h:165
fawkes
pcl::PointCloud
Definition: pointcloud.h:38
fawkes::pcl_utils::PolygonComparison::inside_
bool inside_
Flag to determine whether to do inside or outside check.
Definition: comparisons.h:97
fawkes::pcl_utils::PlaneDistanceComparison::ConstPtr
boost::shared_ptr< const PlaneDistanceComparison< PointT > > ConstPtr
Constant shared pointer.
Definition: comparisons.h:116
fawkes::pcl_utils::PlaneDistanceComparison
Compare points' distance to a plane.
Definition: comparisons.h:108
fawkes::pcl_utils::PolygonComparison
Check if point is inside or outside a given polygon.
Definition: comparisons.h:52
fawkes::pcl_utils::PolygonComparison::evaluate
virtual bool evaluate(const PointT &point) const
Evaluate for given pixel.
Definition: comparisons.h:87
fawkes::pcl_utils::PlaneDistanceComparison::Ptr
boost::shared_ptr< PlaneDistanceComparison< PointT > > Ptr
Shared pointer.
Definition: comparisons.h:114
fawkes::pcl_utils::PlaneDistanceComparison::~PlaneDistanceComparison
virtual ~PlaneDistanceComparison()
Virtual empty destructor.
Definition: comparisons.h:131