00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #ifndef PCL_FILTER_FIELD_VAL_CONDITION_H_
00039 #define PCL_FILTER_FIELD_VAL_CONDITION_H_
00040
00041 #include <pcl/filters/filter.h>
00042
00043 namespace pcl
00044 {
00046 namespace ComparisonOps
00047 {
00051 typedef enum
00052 {
00053 GT,
00054 GE,
00055 LT,
00056 LE,
00057 EQ
00058 } CompareOp;
00059 }
00060
00062
00063 template <typename PointT>
00064 class PointDataAtOffset
00065 {
00066 public:
00068 PointDataAtOffset (uint8_t datatype, uint32_t offset) : datatype_(datatype), offset_(offset) {}
00069
00074 int compare (const PointT& p, const double& val);
00075 protected:
00077 uint8_t datatype_;
00078
00080 uint32_t offset_;
00081 private:
00082 PointDataAtOffset () {}
00083 };
00084
00086
00087 template <typename PointT>
00088 class ComparisonBase
00089 {
00090 public:
00091 typedef boost::shared_ptr<ComparisonBase<PointT> > Ptr;
00092 typedef boost::shared_ptr<const ComparisonBase<PointT> > ConstPtr;
00093
00095 inline bool isCapable () const { return (capable_); }
00096
00098 virtual bool evaluate (const PointT &point) const = 0;
00099
00100 protected:
00102 bool capable_;
00103
00105 std::string field_name_;
00106
00108 uint32_t offset_;
00109
00111 ComparisonOps::CompareOp op_;
00112 };
00113
00115
00116 template <typename PointT>
00117 class FieldComparison : public ComparisonBase<PointT>
00118 {
00119 using ComparisonBase<PointT>::field_name_;
00120 using ComparisonBase<PointT>::op_;
00121 using ComparisonBase<PointT>::capable_;
00122
00123 public:
00124 typedef boost::shared_ptr<FieldComparison<PointT> > Ptr;
00125 typedef boost::shared_ptr<const FieldComparison<PointT> > ConstPtr;
00126
00132 FieldComparison (std::string field_name, ComparisonOps::CompareOp op, double compare_val);
00133
00135 virtual ~FieldComparison();
00136
00141 virtual inline bool
00142 evaluate (const PointT &point) const;
00143
00144 protected:
00146 double compare_val_;
00147
00149 PointDataAtOffset<PointT>* point_data_;
00150
00151 private:
00152 FieldComparison () {}
00153 };
00154
00156
00157 template <typename PointT>
00158 class PackedRGBComparison : public ComparisonBase<PointT>
00159 {
00160 using ComparisonBase<PointT>::capable_;
00161 using ComparisonBase<PointT>::op_;
00162
00163 public:
00169 PackedRGBComparison (std::string component_name, ComparisonOps::CompareOp op, double compare_val);
00170
00175 virtual inline bool
00176 evaluate (const PointT &point) const;
00177
00178 protected:
00180 std::string component_name_;
00181
00183 uint32_t component_offset_;
00184
00186 double compare_val_;
00187
00188 private:
00189 PackedRGBComparison () {}
00190
00191 };
00192
00194
00195 template <typename PointT>
00196 class PackedHSIComparison : public ComparisonBase<PointT>
00197 {
00198 using ComparisonBase<PointT>::capable_;
00199 using ComparisonBase<PointT>::op_;
00200
00201 public:
00207 PackedHSIComparison (std::string component_name, ComparisonOps::CompareOp op, double compare_val);
00208
00213 virtual inline bool
00214 evaluate (const PointT &point) const;
00215
00216 typedef enum
00217 {
00218 H,
00219 S,
00220 I,
00221 } ComponentId;
00222
00223 protected:
00225 std::string component_name_;
00226
00228 ComponentId component_id_;
00229
00231 double compare_val_;
00232
00234 uint32_t rgb_offset_;
00235
00236 private:
00237 PackedHSIComparison () {}
00238 };
00239
00241
00242 template <typename PointT>
00243 class ConditionBase
00244 {
00245 public:
00246 typedef typename pcl::ComparisonBase<PointT> ComparisonBase;
00247 typedef typename ComparisonBase::Ptr ComparisonBasePtr;
00248 typedef typename ComparisonBase::ConstPtr ComparisonBaseConstPtr;
00249
00250 typedef boost::shared_ptr<ConditionBase<PointT> > Ptr;
00251 typedef boost::shared_ptr<const ConditionBase<PointT> > ConstPtr;
00252
00254 ConditionBase () : capable_(true) {}
00255
00257 virtual ~ConditionBase ()
00258 {
00259
00260 comparisons_.clear ();
00261
00262
00263 conditions_.clear ();
00264 }
00265
00269 void
00270 addComparison (ComparisonBaseConstPtr comparison);
00271
00275 void
00276 addCondition (ConstPtr condition);
00277
00280 inline bool
00281 isCapable () const { return (capable_); }
00282
00286 virtual bool
00287 evaluate (const PointT &point) const = 0;
00288
00289 protected:
00291 bool capable_;
00292
00294 std::vector<ComparisonBaseConstPtr> comparisons_;
00295
00297 std::vector<Ptr> conditions_;
00298 };
00299
00301
00302 template <typename PointT>
00303 class ConditionAnd: public ConditionBase<PointT>
00304 {
00305 using ConditionBase<PointT>::conditions_;
00306 using ConditionBase<PointT>::comparisons_;
00307
00308 public:
00309 typedef boost::shared_ptr<ConditionAnd<PointT> > Ptr;
00310 typedef boost::shared_ptr<const ConditionAnd<PointT> > ConstPtr;
00311
00313 ConditionAnd () : ConditionBase<PointT>() {}
00314
00322 virtual inline bool
00323 evaluate (const PointT &point) const;
00324 };
00325
00327
00328 template <typename PointT>
00329 class ConditionOr: public ConditionBase<PointT>
00330 {
00331 using ConditionBase<PointT>::conditions_;
00332 using ConditionBase<PointT>::comparisons_;
00333
00334 public:
00335 typedef boost::shared_ptr<ConditionOr<PointT> > Ptr;
00336 typedef boost::shared_ptr<const ConditionOr<PointT> > ConstPtr;
00337
00339 ConditionOr () : ConditionBase<PointT>() {}
00340
00348 virtual inline bool
00349 evaluate (const PointT &point) const;
00350 };
00351
00353
00382 template <typename PointT>
00383 class ConditionalRemoval: public Filter<PointT>
00384 {
00385 using Filter<PointT>::input_;
00386 using Filter<PointT>::filter_name_;
00387 using Filter<PointT>::getClassName;
00388
00389 typedef typename Filter<PointT>::PointCloud PointCloud;
00390 typedef typename PointCloud::Ptr PointCloudPtr;
00391 typedef typename PointCloud::ConstPtr PointCloudConstPtr;
00392
00393 public:
00394 typedef typename pcl::ConditionBase<PointT> ConditionBase;
00395 typedef typename ConditionBase::Ptr ConditionBasePtr;
00396 typedef typename ConditionBase::ConstPtr ConditionBaseConstPtr;
00397
00403 ConditionalRemoval () : keep_organized_(false), condition_()
00404 {
00405 filter_name_ = "ConditionalRemoval";
00406 }
00407
00412 ConditionalRemoval (ConditionBasePtr condition) : keep_organized_(false), condition_()
00413 {
00414 filter_name_ = "ConditionalRemoval";
00415 setCondition (condition);
00416 }
00417
00426 inline void setKeepOrganized (bool val) { keep_organized_ = val; }
00427
00428 inline bool getKeepOrganized () { return (keep_organized_); }
00429
00436 void
00437 setCondition (ConditionBasePtr condition);
00438
00439 protected:
00443 void applyFilter(PointCloud &output);
00444
00445 typedef typename pcl::traits::fieldList<PointT>::type FieldList;
00446
00448 bool capable_;
00449
00453 bool keep_organized_;
00454
00456 ConditionBasePtr condition_;
00457 };
00458 }
00459
00460 #include <pcl/filters/impl/conditional_removal.hpp>
00461
00462 #endif