38 #ifndef PCL_FILTER_IMPL_FIELD_VAL_CONDITION_H_
39 #define PCL_FILTER_IMPL_FIELD_VAL_CONDITION_H_
41 #include <pcl/common/io.h>
42 #include <pcl/filters/conditional_removal.h>
47 template <
typename Po
intT>
51 , compare_val_ (compare_val), point_data_ (NULL)
57 std::vector<pcl::PCLPointField> point_fields;
63 if (point_fields.empty ())
65 PCL_WARN (
"[pcl::FieldComparison::FieldComparison] no fields found!\n");
72 for (d = 0; d < point_fields.size (); ++d)
74 if (point_fields[d].name == field_name)
78 if (d == point_fields.size ())
80 PCL_WARN (
"[pcl::FieldComparison::FieldComparison] field not found!\n");
84 uint8_t datatype = point_fields[d].datatype;
85 uint32_t offset = point_fields[d].offset;
92 template <
typename Po
intT>
95 if (point_data_ != NULL)
103 template <
typename Po
intT>
bool
108 PCL_WARN (
"[pcl::FieldComparison::evaluate] invalid compariosn!\n");
115 int compare_result = point_data_->compare (point, compare_val_);
120 return (compare_result > 0);
122 return (compare_result >= 0);
124 return (compare_result < 0);
126 return (compare_result <= 0);
128 return (compare_result == 0);
130 PCL_WARN (
"[pcl::FieldComparison::evaluate] unrecognized op_!\n");
138 template <
typename Po
intT>
141 component_name_ (component_name), component_offset_ (), compare_val_ (compare_val)
144 std::vector<pcl::PCLPointField> point_fields;
151 for (d = 0; d < point_fields.size (); ++d)
153 if (point_fields[d].name ==
"rgb" || point_fields[d].name ==
"rgba")
156 if (d == point_fields.size ())
158 PCL_WARN (
"[pcl::PackedRGBComparison::PackedRGBComparison] rgb field not found!\n");
164 uint8_t datatype = point_fields[d].datatype;
169 PCL_WARN (
"[pcl::PackedRGBComparison::PackedRGBComparison] has unusable type!\n");
175 if (component_name ==
"r")
179 else if (component_name ==
"g")
183 else if (component_name ==
"b")
189 PCL_WARN (
"[pcl::PackedRGBComparison::PackedRGBComparison] unrecognized component name!\n");
201 template <
typename Po
intT>
bool
205 const uint8_t* pt_data =
reinterpret_cast<const uint8_t*
> (&point);
206 uint8_t my_val = *(pt_data + component_offset_);
212 return (my_val > this->compare_val_);
214 return (my_val >= this->compare_val_);
216 return (my_val < this->compare_val_);
218 return (my_val <= this->compare_val_);
220 return (my_val == this->compare_val_);
222 PCL_WARN (
"[pcl::PackedRGBComparison::evaluate] unrecognized op_!\n");
230 template <
typename Po
intT>
233 component_name_ (component_name), component_id_ (), compare_val_ (compare_val), rgb_offset_ ()
236 std::vector<pcl::PCLPointField> point_fields;
243 for (d = 0; d < point_fields.size (); ++d)
244 if (point_fields[d].name ==
"rgb" || point_fields[d].name ==
"rgba")
246 if (d == point_fields.size ())
248 PCL_WARN (
"[pcl::PackedHSIComparison::PackedHSIComparison] rgb field not found!\n");
254 uint8_t datatype = point_fields[d].datatype;
259 PCL_WARN (
"[pcl::PackedHSIComparison::PackedHSIComparison] has unusable type!\n");
265 uint32_t offset = point_fields[d].offset;
268 PCL_WARN (
"[pcl::PackedHSIComparison::PackedHSIComparison] rgb field is not 32 bit aligned!\n");
275 if (component_name ==
"h" )
279 else if (component_name ==
"s")
283 else if (component_name ==
"i")
289 PCL_WARN (
"[pcl::PackedRGBComparison::PackedRGBComparison] unrecognized component name!\n");
300 template <
typename Po
intT>
bool
304 static uint32_t rgb_val_ = 0;
305 static uint8_t r_ = 0;
306 static uint8_t g_ = 0;
307 static uint8_t b_ = 0;
308 static int8_t h_ = 0;
309 static uint8_t s_ = 0;
310 static uint8_t i_ = 0;
313 const uint8_t* pt_data =
reinterpret_cast<const uint8_t*
> (&point);
314 const uint32_t* rgb_data =
reinterpret_cast<const uint32_t*
> (pt_data + rgb_offset_);
315 uint32_t new_rgb_val = *rgb_data;
317 if (rgb_val_ != new_rgb_val)
319 rgb_val_ = new_rgb_val;
321 r_ = static_cast <uint8_t> (rgb_val_ >> 16);
322 g_ = static_cast <uint8_t> (rgb_val_ >> 8);
323 b_ = static_cast <uint8_t> (rgb_val_);
326 float hx = (2.0f * r_ - g_ - b_) / 4.0f;
327 float hy =
static_cast<float> (g_ - b_) * 111.0f / 255.0f;
328 h_ =
static_cast<int8_t
> (atan2(hy, hx) * 128.0f / M_PI);
330 int32_t i = (r_+g_+b_)/3;
331 i_ =
static_cast<uint8_t
> (i);
334 m = (r_ < g_) ? r_ : g_;
335 m = (m < b_) ? m : b_;
337 s_ =
static_cast<uint8_t
> ((i == 0) ? 0 : 255 - (m * 255) / i);
342 switch (component_id_)
345 my_val = static_cast <
float> (h_);
348 my_val = static_cast <
float> (s_);
351 my_val = static_cast <
float> (i_);
361 return (my_val > this->compare_val_);
363 return (my_val >= this->compare_val_);
365 return (my_val < this->compare_val_);
367 return (my_val <= this->compare_val_);
369 return (my_val == this->compare_val_);
371 PCL_WARN (
"[pcl::PackedHSIComparison::evaluate] unrecognized op_!\n");
380 template<
typename Po
intT>
382 comp_matr_ (), comp_vect_ (), comp_scalar_ (0.0)
385 std::vector<pcl::PCLPointField> point_fields;
392 for (dX = 0; dX < point_fields.size (); ++dX)
394 if (point_fields[dX].name ==
"x")
397 if (dX == point_fields.size ())
399 PCL_WARN (
"[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] x field not found!\n");
406 for (dY = 0; dY < point_fields.size (); ++dY)
408 if (point_fields[dY].name ==
"y")
411 if (dY == point_fields.size ())
413 PCL_WARN (
"[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] y field not found!\n");
420 for (dZ = 0; dZ < point_fields.size (); ++dZ)
422 if (point_fields[dZ].name ==
"z")
425 if (dZ == point_fields.size ())
427 PCL_WARN (
"[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] z field not found!\n");
432 comp_matr_ << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0;
441 template<
typename Po
intT>
443 const Eigen::Matrix3f &comparison_matrix,
444 const Eigen::Vector3f &comparison_vector,
445 const float &comparison_scalar,
446 const Eigen::Affine3f &comparison_transform) :
447 comp_matr_ (), comp_vect_ (), comp_scalar_ (comparison_scalar)
450 std::vector<pcl::PCLPointField> point_fields;
457 for (dX = 0; dX < point_fields.size (); ++dX)
459 if (point_fields[dX].name ==
"x")
462 if (dX == point_fields.size ())
464 PCL_WARN (
"[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] x field not found!\n");
471 for (dY = 0; dY < point_fields.size (); ++dY)
473 if (point_fields[dY].name ==
"y")
476 if (dY == point_fields.size ())
478 PCL_WARN (
"[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] y field not found!\n");
485 for (dZ = 0; dZ < point_fields.size (); ++dZ)
487 if (point_fields[dZ].name ==
"z")
490 if (dZ == point_fields.size ())
492 PCL_WARN (
"[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] z field not found!\n");
501 if (!comparison_transform.matrix ().isIdentity ())
506 template<
typename Po
intT>
510 Eigen::Vector4f pointAffine;
511 pointAffine << point.x, point.y, point.z, 1;
513 float myVal =
static_cast<float>(2.0f * tf_comp_vect_.transpose () * pointAffine) + static_cast<float>(pointAffine.transpose () * tf_comp_matr_ * pointAffine) + comp_scalar_ - 3.0f;
529 PCL_WARN (
"[pcl::transformableQuadricXYZComparison::evaluate] unrecognized op_!\n");
537 template <
typename Po
intT>
int
544 const uint8_t* pt_data =
reinterpret_cast<const uint8_t*
> (&p);
551 memcpy (&pt_val, pt_data + this->offset_,
sizeof (int8_t));
552 return (pt_val > static_cast<int8_t>(val)) - (pt_val < static_cast<int8_t> (val));
557 memcpy (&pt_val, pt_data + this->offset_,
sizeof (uint8_t));
558 return (pt_val > static_cast<uint8_t>(val)) - (pt_val < static_cast<uint8_t> (val));
563 memcpy (&pt_val, pt_data + this->offset_,
sizeof (int16_t));
564 return (pt_val > static_cast<int16_t>(val)) - (pt_val < static_cast<int16_t> (val));
569 memcpy (&pt_val, pt_data + this->offset_,
sizeof (uint16_t));
570 return (pt_val > static_cast<uint16_t> (val)) - (pt_val < static_cast<uint16_t> (val));
575 memcpy (&pt_val, pt_data + this->offset_,
sizeof (int32_t));
576 return (pt_val > static_cast<int32_t> (val)) - (pt_val < static_cast<int32_t> (val));
581 memcpy (&pt_val, pt_data + this->offset_,
sizeof (uint32_t));
582 return (pt_val > static_cast<uint32_t> (val)) - (pt_val < static_cast<uint32_t> (val));
587 memcpy (&pt_val, pt_data + this->offset_,
sizeof (
float));
588 return (pt_val > static_cast<float> (val)) - (pt_val < static_cast<float> (val));
593 memcpy (&pt_val, pt_data + this->offset_,
sizeof (
double));
594 return (pt_val > val) - (pt_val < val);
597 PCL_WARN (
"[pcl::pcl::PointDataAtOffset::compare] unknown data_type!\n");
605 template <
typename Po
intT>
void
608 if (!comparison->isCapable ())
610 comparisons_.push_back (comparison);
614 template <
typename Po
intT>
void
617 if (!condition->isCapable ())
619 conditions_.push_back (condition);
625 template <
typename Po
intT>
bool
628 for (
size_t i = 0; i < comparisons_.size (); ++i)
629 if (!comparisons_[i]->evaluate (point))
632 for (
size_t i = 0; i < conditions_.size (); ++i)
633 if (!conditions_[i]->evaluate (point))
642 template <
typename Po
intT>
bool
645 if (comparisons_.empty () && conditions_.empty ())
647 for (
size_t i = 0; i < comparisons_.size (); ++i)
648 if (comparisons_[i]->evaluate(point))
651 for (
size_t i = 0; i < conditions_.size (); ++i)
652 if (conditions_[i]->evaluate (point))
661 template <
typename Po
intT>
void
664 condition_ = condition;
665 capable_ = condition_->isCapable ();
669 template <
typename Po
intT>
void
672 if (capable_ ==
false)
674 PCL_WARN (
"[pcl::%s::applyFilter] not capable!\n", getClassName ().c_str ());
680 PCL_WARN (
"[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ());
684 if (condition_.get () == NULL)
686 PCL_WARN (
"[pcl::%s::applyFilter] No filtering condition given!\n", getClassName ().c_str ());
691 output.
header = input_->header;
692 if (!keep_organized_)
699 output.
height = this->input_->height;
700 output.
width = this->input_->width;
701 output.
is_dense = this->input_->is_dense;
703 output.
points.resize (input_->points.size ());
704 removed_indices_->resize (input_->points.size ());
707 int nr_removed_p = 0;
709 if (!keep_organized_)
711 for (
size_t cp = 0; cp < Filter<PointT>::indices_->size (); ++cp)
718 if (extract_removed_indices_)
728 pcl::for_each_type<FieldList> (
736 if (extract_removed_indices_)
745 output.
points.resize (nr_p);
750 std::sort (indices.begin (), indices.end ());
751 bool removed_p =
false;
753 for (
size_t cp = 0; cp < input_->points.size (); ++cp)
755 if (cp == static_cast<size_t> (indices[ci]))
757 if (ci < indices.size () - 1)
760 if (cp == static_cast<size_t> (indices[ci]))
767 if (!condition_->evaluate (input_->points[cp]))
769 output.
points[cp].getVector4fMap ().setConstant (user_filter_value_);
772 if (extract_removed_indices_)
774 (*removed_indices_)[nr_removed_p] =
static_cast<int> (cp);
781 output.
points[cp].getVector4fMap ().setConstant (user_filter_value_);
787 if (removed_p && !pcl_isfinite (user_filter_value_))
790 removed_indices_->resize (nr_removed_p);
793 #define PCL_INSTANTIATE_PointDataAtOffset(T) template class PCL_EXPORTS pcl::PointDataAtOffset<T>;
794 #define PCL_INSTANTIATE_ComparisonBase(T) template class PCL_EXPORTS pcl::ComparisonBase<T>;
795 #define PCL_INSTANTIATE_FieldComparison(T) template class PCL_EXPORTS pcl::FieldComparison<T>;
796 #define PCL_INSTANTIATE_PackedRGBComparison(T) template class PCL_EXPORTS pcl::PackedRGBComparison<T>;
797 #define PCL_INSTANTIATE_PackedHSIComparison(T) template class PCL_EXPORTS pcl::PackedHSIComparison<T>;
798 #define PCL_INSTANTIATE_TfQuadraticXYZComparison(T) template class PCL_EXPORTS pcl::TfQuadraticXYZComparison<T>;
799 #define PCL_INSTANTIATE_ConditionBase(T) template class PCL_EXPORTS pcl::ConditionBase<T>;
800 #define PCL_INSTANTIATE_ConditionAnd(T) template class PCL_EXPORTS pcl::ConditionAnd<T>;
801 #define PCL_INSTANTIATE_ConditionOr(T) template class PCL_EXPORTS pcl::ConditionOr<T>;
802 #define PCL_INSTANTIATE_ConditionalRemoval(T) template class PCL_EXPORTS pcl::ConditionalRemoval<T>;
virtual bool evaluate(const PointT &point) const
Determine if a point meets this condition.
virtual ~FieldComparison()
Destructor.
void setComparisonVector(const Eigen::Vector3f &vector)
set the vector "v" of the comparison "p'Ap + 2v'p + c [OP] 0".
uint32_t component_offset_
The offset of the component.
ComparisonBase::ConstPtr ComparisonBaseConstPtr
void addCondition(Ptr condition)
Add a nested condition to this condition.
pcl::PCLHeader header
The point cloud header.
boost::shared_ptr< ConditionBase< PointT > > Ptr
virtual bool evaluate(const PointT &point) const
Determine the result of this comparison.
uint32_t height
The point cloud height (if organized as an image-structure).
bool capable_
True if capable.
PointDataAtOffset< PointT > * point_data_
The point data to compare.
void getFields(const pcl::PointCloud< PointT > &cloud, std::vector< pcl::PCLPointField > &fields)
Get the list of available fields (i.e., dimension/channel)
void addComparison(ComparisonBaseConstPtr comparison)
Add a new comparison.
CompareOp
The kind of comparison operations that are possible within a comparison object.
Eigen::Vector4f comp_vect_
Eigen::Matrix4f comp_matr_
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
ComponentId component_id_
The ID of the component.
The (abstract) base class for the comparison object.
A datatype that enables type-correct comparisons.
A packed HSI specialization of the comparison object.
int compare(const PointT &p, const double &val)
Compare function.
virtual bool evaluate(const PointT &point) const
Determine the result of this comparison.
void applyFilter(PointCloud &output)
Filter a Point Cloud.
void transformComparison(const Eigen::Matrix4f &transform)
transform the coordinate system of the comparison.
virtual bool evaluate(const PointT &point) const
Determine the result of this comparison.
void setComparisonMatrix(const Eigen::Matrix3f &matrix)
set the matrix "A" of the comparison "p'Ap + 2v'p + c [OP] 0".
ComparisonOps::CompareOp op_
The comparison operator type.
virtual bool evaluate(const PointT &point) const
Determine if a point meets this condition.
The field-based specialization of the comparison object.
A packed rgb specialization of the comparison object.
void setCondition(ConditionBasePtr condition)
Set the condition that the filter will use.
TfQuadraticXYZComparison()
Constructor.
std::string field_name_
Field name to compare data on.
A point structure representing Euclidean xyz coordinates, and the RGB color.
uint32_t width
The point cloud width (if organized as an image-structure).
uint32_t rgb_offset_
The offset of the component.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Filter represents the base filter class.
Helper functor structure for concatenate.
virtual bool evaluate(const PointT &point) const
Determine the result of this comparison.
ConditionBase::Ptr ConditionBasePtr