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
32namespace fawkes {
33namespace 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 */
43template <typename PointT>
44class PolygonComparison : public pcl::ComparisonBase<PointT>
45{
46 using pcl::ComparisonBase<PointT>::capable_;
47
48public:
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
82protected:
83 /// Flag to determine whether to do inside or outside check
84 bool inside_;
85 /// The polygon to check against
87
88private:
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 */
99template <typename PointT>
100class PlaneDistanceComparison : public pcl::ComparisonBase<PointT>
101{
102 using pcl::ComparisonBase<PointT>::capable_;
103
104public:
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
155protected:
156 /// Planar model coefficients
157 pcl::ModelCoefficients::ConstPtr coeff_;
158 /// Comparison operation
159 pcl::ComparisonOps::CompareOp op_;
160 /// Value to compare against
162
163private:
165 {
166 } // not allowed
167};
168
169} // namespace pcl_utils
170} // end namespace fawkes
171
172#endif
Compare points' distance to a plane.
Definition: comparisons.h:101
float compare_val_
Value to compare against.
Definition: comparisons.h:161
pcl::ComparisonOps::CompareOp op_
Comparison operation.
Definition: comparisons.h:159
pcl::shared_ptr< const PlaneDistanceComparison< PointT > > ConstPtr
Constant shared pointer.
Definition: comparisons.h:108
pcl::shared_ptr< PlaneDistanceComparison< PointT > > Ptr
Shared pointer.
Definition: comparisons.h:106
virtual ~PlaneDistanceComparison()
Virtual empty destructor.
Definition: comparisons.h:123
virtual bool evaluate(const PointT &point) const
Evaluate for given pixel.
Definition: comparisons.h:132
PlaneDistanceComparison(pcl::ModelCoefficients::ConstPtr coeff, pcl::ComparisonOps::CompareOp op=pcl::ComparisonOps::GT, float compare_val=0.)
Constructor.
Definition: comparisons.h:115
pcl::ModelCoefficients::ConstPtr coeff_
Planar model coefficients.
Definition: comparisons.h:157
Check if point is inside or outside a given polygon.
Definition: comparisons.h:45
const pcl::PointCloud< PointT > & polygon_
The polygon to check against.
Definition: comparisons.h:86
virtual bool evaluate(const PointT &point) const
Evaluate for given pixel.
Definition: comparisons.h:74
pcl::shared_ptr< PolygonComparison< PointT > > Ptr
Shared pointer.
Definition: comparisons.h:50
bool inside_
Flag to determine whether to do inside or outside check.
Definition: comparisons.h:84
PolygonComparison(const pcl::PointCloud< PointT > &polygon, bool inside=true)
Constructor.
Definition: comparisons.h:58
virtual ~PolygonComparison()
Virtual empty destructor.
Definition: comparisons.h:64
pcl::shared_ptr< const PolygonComparison< PointT > > ConstPtr
Constant shared pointer.
Definition: comparisons.h:52
Fawkes library namespace.