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/common/copy_point.h>
43 #include <pcl/filters/conditional_removal.h>
48 template <
typename Po
intT>
52 , compare_val_ (compare_val), point_data_ (nullptr)
58 const auto point_fields = pcl::getFields<PointT> ();
61 if (point_fields.empty ())
63 PCL_WARN (
"[pcl::FieldComparison::FieldComparison] no fields found!\n");
70 for (d = 0; d < point_fields.size (); ++d)
72 if (point_fields[d].name == field_name)
76 if (d == point_fields.size ())
78 PCL_WARN (
"[pcl::FieldComparison::FieldComparison] field not found!\n");
90 template <
typename Po
intT>
93 if (point_data_ !=
nullptr)
96 point_data_ =
nullptr;
101 template <
typename Po
intT>
bool
106 PCL_WARN (
"[pcl::FieldComparison::evaluate] invalid comparison!\n");
113 int compare_result = point_data_->compare (point, compare_val_);
118 return (compare_result > 0);
120 return (compare_result >= 0);
122 return (compare_result < 0);
124 return (compare_result <= 0);
126 return (compare_result == 0);
128 PCL_WARN (
"[pcl::FieldComparison::evaluate] unrecognized op_!\n");
136 template <
typename Po
intT>
139 component_name_ (component_name), component_offset_ (), compare_val_ (compare_val)
142 const auto point_fields = pcl::getFields<PointT> ();
146 for (d = 0; d < point_fields.size (); ++d)
148 if (point_fields[d].name ==
"rgb" || point_fields[d].name ==
"rgba")
151 if (d == point_fields.size ())
153 PCL_WARN (
"[pcl::PackedRGBComparison::PackedRGBComparison] rgb field not found!\n");
164 PCL_WARN (
"[pcl::PackedRGBComparison::PackedRGBComparison] has unusable type!\n");
170 if (component_name ==
"r")
174 else if (component_name ==
"g")
178 else if (component_name ==
"b")
184 PCL_WARN (
"[pcl::PackedRGBComparison::PackedRGBComparison] unrecognized component name!\n");
196 template <
typename Po
intT>
bool
200 const std::uint8_t* pt_data = reinterpret_cast<const std::uint8_t*> (&point);
207 return (my_val > this->compare_val_);
209 return (my_val >= this->compare_val_);
211 return (my_val < this->compare_val_);
213 return (my_val <= this->compare_val_);
215 return (my_val == this->compare_val_);
217 PCL_WARN (
"[pcl::PackedRGBComparison::evaluate] unrecognized op_!\n");
225 template <
typename Po
intT>
228 component_name_ (component_name), component_id_ (), compare_val_ (compare_val), rgb_offset_ ()
231 const auto point_fields = pcl::getFields<PointT> ();
235 for (d = 0; d < point_fields.size (); ++d)
236 if (point_fields[d].name ==
"rgb" || point_fields[d].name ==
"rgba")
238 if (d == point_fields.size ())
240 PCL_WARN (
"[pcl::PackedHSIComparison::PackedHSIComparison] rgb field not found!\n");
251 PCL_WARN (
"[pcl::PackedHSIComparison::PackedHSIComparison] has unusable type!\n");
260 PCL_WARN (
"[pcl::PackedHSIComparison::PackedHSIComparison] rgb field is not 32 bit aligned!\n");
267 if (component_name ==
"h" )
271 else if (component_name ==
"s")
275 else if (component_name ==
"i")
281 PCL_WARN (
"[pcl::PackedHSIComparison::PackedHSIComparison] unrecognized component name!\n");
292 template <
typename Po
intT>
bool
305 const std::uint8_t* pt_data = reinterpret_cast<const std::uint8_t*> (&point);
306 const std::uint32_t* rgb_data = reinterpret_cast<const std::uint32_t*> (pt_data + rgb_offset_);
309 if (rgb_val_ != new_rgb_val)
311 rgb_val_ = new_rgb_val;
313 r_ = static_cast <std::uint8_t> (rgb_val_ >> 16);
314 g_ = static_cast <std::uint8_t> (rgb_val_ >> 8);
315 b_ = static_cast <std::uint8_t> (rgb_val_);
318 float hx = (2.0f * r_ - g_ - b_) / 4.0f;
319 float hy = static_cast<float> (g_ - b_) * 111.0f / 255.0f;
320 h_ = static_cast<std::int8_t> (std::atan2(hy, hx) * 128.0f / M_PI);
323 i_ = static_cast<std::uint8_t> (i);
326 m = (r_ < g_) ? r_ : g_;
327 m = (m < b_) ? m : b_;
329 s_ = static_cast<std::uint8_t> ((i == 0) ? 0 : 255 - (m * 255) / i);
334 switch (component_id_)
337 my_val = static_cast <float> (h_);
340 my_val = static_cast <float> (s_);
343 my_val = static_cast <float> (i_);
353 return (my_val > this->compare_val_);
355 return (my_val >= this->compare_val_);
357 return (my_val < this->compare_val_);
359 return (my_val <= this->compare_val_);
361 return (my_val == this->compare_val_);
363 PCL_WARN (
"[pcl::PackedHSIComparison::evaluate] unrecognized op_!\n");
372 template<
typename Po
intT>
377 const auto point_fields = pcl::getFields<PointT> ();
381 for (dX = 0; dX < point_fields.size (); ++dX)
383 if (point_fields[dX].name ==
"x")
386 if (dX == point_fields.size ())
388 PCL_WARN (
"[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] x field not found!\n");
395 for (dY = 0; dY < point_fields.size (); ++dY)
397 if (point_fields[dY].name ==
"y")
400 if (dY == point_fields.size ())
402 PCL_WARN (
"[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] y field not found!\n");
409 for (dZ = 0; dZ < point_fields.size (); ++dZ)
411 if (point_fields[dZ].name ==
"z")
414 if (dZ == point_fields.size ())
416 PCL_WARN (
"[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] z field not found!\n");
421 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;
430 template<
typename Po
intT>
432 const Eigen::Matrix3f &comparison_matrix,
433 const Eigen::Vector3f &comparison_vector,
434 const float &comparison_scalar,
435 const Eigen::Affine3f &comparison_transform) :
436 comp_scalar_ (comparison_scalar)
439 const auto point_fields = pcl::getFields<PointT> ();
443 for (dX = 0; dX < point_fields.size (); ++dX)
445 if (point_fields[dX].name ==
"x")
448 if (dX == point_fields.size ())
450 PCL_WARN (
"[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] x field not found!\n");
457 for (dY = 0; dY < point_fields.size (); ++dY)
459 if (point_fields[dY].name ==
"y")
462 if (dY == point_fields.size ())
464 PCL_WARN (
"[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] y field not found!\n");
471 for (dZ = 0; dZ < point_fields.size (); ++dZ)
473 if (point_fields[dZ].name ==
"z")
476 if (dZ == point_fields.size ())
478 PCL_WARN (
"[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] z field not found!\n");
487 if (!comparison_transform.matrix ().isIdentity ())
492 template<
typename Po
intT>
496 Eigen::Vector4f pointAffine;
497 pointAffine << point.x, point.y, point.z, 1;
499 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;
515 PCL_WARN (
"[pcl::TfQuadraticXYZComparison::evaluate] unrecognized op_!\n");
523 template <
typename Po
intT>
int
530 const std::uint8_t* pt_data = reinterpret_cast<const std::uint8_t*> (&p);
537 memcpy (&pt_val, pt_data + this->offset_,
sizeof (
std::int8_t));
538 return (pt_val > static_cast<std::int8_t>(val)) - (pt_val < static_cast<std::int8_t> (val));
543 memcpy (&pt_val, pt_data + this->offset_,
sizeof (
std::uint8_t));
544 return (pt_val > static_cast<std::uint8_t>(val)) - (pt_val < static_cast<std::uint8_t> (val));
549 memcpy (&pt_val, pt_data + this->offset_,
sizeof (
std::int16_t));
550 return (pt_val > static_cast<std::int16_t>(val)) - (pt_val < static_cast<std::int16_t> (val));
555 memcpy (&pt_val, pt_data + this->offset_,
sizeof (
std::uint16_t));
556 return (pt_val > static_cast<std::uint16_t> (val)) - (pt_val < static_cast<std::uint16_t> (val));
561 memcpy (&pt_val, pt_data + this->offset_,
sizeof (
std::int32_t));
562 return (pt_val > static_cast<std::int32_t> (val)) - (pt_val < static_cast<std::int32_t> (val));
567 memcpy (&pt_val, pt_data + this->offset_,
sizeof (
std::uint32_t));
568 return (pt_val > static_cast<std::uint32_t> (val)) - (pt_val < static_cast<std::uint32_t> (val));
573 memcpy (&pt_val, pt_data + this->offset_,
sizeof (
float));
574 return (pt_val > static_cast<float> (val)) - (pt_val < static_cast<float> (val));
579 memcpy (&pt_val, pt_data + this->offset_,
sizeof (
double));
580 return (pt_val > val) - (pt_val < val);
583 PCL_WARN (
"[pcl::PointDataAtOffset::compare] unknown data_type!\n");
591 template <
typename Po
intT>
void
594 if (!comparison->isCapable ())
596 comparisons_.push_back (comparison);
600 template <
typename Po
intT>
void
603 if (!condition->isCapable ())
605 conditions_.push_back (condition);
611 template <
typename Po
intT>
bool
614 for (std::size_t i = 0; i < comparisons_.size (); ++i)
615 if (!comparisons_[i]->evaluate (point))
618 for (std::size_t i = 0; i < conditions_.size (); ++i)
619 if (!conditions_[i]->evaluate (point))
628 template <
typename Po
intT>
bool
631 if (comparisons_.empty () && conditions_.empty ())
633 for (std::size_t i = 0; i < comparisons_.size (); ++i)
634 if (comparisons_[i]->evaluate(point))
637 for (std::size_t i = 0; i < conditions_.size (); ++i)
638 if (conditions_[i]->evaluate (point))
647 template <
typename Po
intT>
void
650 condition_ = condition;
651 capable_ = condition_->isCapable ();
655 template <
typename Po
intT>
void
658 if (capable_ ==
false)
660 PCL_WARN (
"[pcl::%s::applyFilter] not capable!\n", getClassName ().c_str ());
666 PCL_WARN (
"[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ());
670 if (condition_.get () ==
nullptr)
672 PCL_WARN (
"[pcl::%s::applyFilter] No filtering condition given!\n", getClassName ().c_str ());
677 output.
header = input_->header;
678 if (!keep_organized_)
685 output.
height = this->input_->height;
686 output.
width = this->input_->width;
687 output.
is_dense = this->input_->is_dense;
689 output.
points.resize (input_->points.size ());
690 removed_indices_->resize (input_->points.size ());
693 int nr_removed_p = 0;
695 if (!keep_organized_)
699 const PointT& point = input_->points[index];
701 if (!std::isfinite (point.x)
702 || !std::isfinite (point.y)
703 || !std::isfinite (point.z))
705 if (extract_removed_indices_)
707 (*removed_indices_)[nr_removed_p] = index;
713 if (condition_->evaluate (point))
720 if (extract_removed_indices_)
722 (*removed_indices_)[nr_removed_p] = index;
729 output.
points.resize (nr_p);
734 std::sort (indices.begin (), indices.end ());
735 bool removed_p =
false;
737 for (std::size_t cp = 0; cp < input_->points.size (); ++cp)
739 if (cp == static_cast<std::size_t> (indices[ci]))
741 if (ci < indices.size () - 1)
744 if (cp == static_cast<std::size_t> (indices[ci]))
751 if (!condition_->evaluate (input_->points[cp]))
753 output.
points[cp].getVector4fMap ().setConstant (user_filter_value_);
756 if (extract_removed_indices_)
758 (*removed_indices_)[nr_removed_p] = static_cast<int> (cp);
765 output.
points[cp].getVector4fMap ().setConstant (user_filter_value_);
771 if (removed_p && !std::isfinite (user_filter_value_))
774 removed_indices_->resize (nr_removed_p);
777 #define PCL_INSTANTIATE_PointDataAtOffset(T) template class PCL_EXPORTS pcl::PointDataAtOffset<T>;
778 #define PCL_INSTANTIATE_ComparisonBase(T) template class PCL_EXPORTS pcl::ComparisonBase<T>;
779 #define PCL_INSTANTIATE_FieldComparison(T) template class PCL_EXPORTS pcl::FieldComparison<T>;
780 #define PCL_INSTANTIATE_PackedRGBComparison(T) template class PCL_EXPORTS pcl::PackedRGBComparison<T>;
781 #define PCL_INSTANTIATE_PackedHSIComparison(T) template class PCL_EXPORTS pcl::PackedHSIComparison<T>;
782 #define PCL_INSTANTIATE_TfQuadraticXYZComparison(T) template class PCL_EXPORTS pcl::TfQuadraticXYZComparison<T>;
783 #define PCL_INSTANTIATE_ConditionBase(T) template class PCL_EXPORTS pcl::ConditionBase<T>;
784 #define PCL_INSTANTIATE_ConditionAnd(T) template class PCL_EXPORTS pcl::ConditionAnd<T>;
785 #define PCL_INSTANTIATE_ConditionOr(T) template class PCL_EXPORTS pcl::ConditionOr<T>;
786 #define PCL_INSTANTIATE_ConditionalRemoval(T) template class PCL_EXPORTS pcl::ConditionalRemoval<T>;