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