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>
48template <
typename Po
intT>
63 PCL_WARN (
"[pcl::FieldComparison::FieldComparison] no fields found!\n");
78 PCL_WARN (
"[pcl::FieldComparison::FieldComparison] field not found!\n");
90template <
typename Po
intT>
97template <
typename Po
intT>
bool
102 PCL_WARN (
"[pcl::FieldComparison::evaluate] invalid comparison!\n");
124 PCL_WARN (
"[pcl::FieldComparison::evaluate] unrecognized op_!\n");
132template <
typename Po
intT>
149 PCL_WARN (
"[pcl::PackedRGBComparison::PackedRGBComparison] rgb field not found!\n");
160 PCL_WARN (
"[pcl::PackedRGBComparison::PackedRGBComparison] has unusable type!\n");
180 PCL_WARN (
"[pcl::PackedRGBComparison::PackedRGBComparison] unrecognized component name!\n");
192template <
typename Po
intT>
bool
196 const std::uint8_t*
pt_data =
reinterpret_cast<const std::uint8_t*
> (&point);
203 return (
my_val > this->compare_val_);
205 return (
my_val >= this->compare_val_);
211 return (
my_val == this->compare_val_);
213 PCL_WARN (
"[pcl::PackedRGBComparison::evaluate] unrecognized op_!\n");
221template <
typename Po
intT>
236 PCL_WARN (
"[pcl::PackedHSIComparison::PackedHSIComparison] rgb field not found!\n");
247 PCL_WARN (
"[pcl::PackedHSIComparison::PackedHSIComparison] has unusable type!\n");
256 PCL_WARN (
"[pcl::PackedHSIComparison::PackedHSIComparison] rgb field is not 32 bit aligned!\n");
277 PCL_WARN (
"[pcl::PackedHSIComparison::PackedHSIComparison] unrecognized component name!\n");
288template <
typename Po
intT>
bool
293 static std::uint8_t r_ = 0;
294 static std::uint8_t g_ = 0;
295 static std::uint8_t b_ = 0;
296 static std::int8_t h_ = 0;
297 static std::uint8_t
s_ = 0;
298 static std::uint8_t
i_ = 0;
301 const std::uint8_t*
pt_data =
reinterpret_cast<const std::uint8_t*
> (&point);
302 const std::uint32_t*
rgb_data =
reinterpret_cast<const std::uint32_t*
> (
pt_data + rgb_offset_);
309 r_ =
static_cast <std::uint8_t
> (
rgb_val_ >> 16);
310 g_ =
static_cast <std::uint8_t
> (
rgb_val_ >> 8);
311 b_ =
static_cast <std::uint8_t
> (
rgb_val_);
314 float hx = (2.0f * r_ - g_ - b_) / 4.0f;
315 float hy =
static_cast<float> (g_ - b_) * 111.0f / 255.0f;
316 h_ =
static_cast<std::int8_t
> (std::atan2(
hy,
hx) * 128.0f /
M_PI);
318 std::int32_t i = (r_+g_+b_)/3;
319 i_ =
static_cast<std::uint8_t
> (i);
322 m = (r_ < g_) ? r_ : g_;
323 m = (
m < b_) ?
m : b_;
325 s_ =
static_cast<std::uint8_t
> ((i == 0) ? 0 : 255 - (
m * 255) / i);
330 switch (component_id_)
333 my_val =
static_cast <float> (h_);
349 return (
my_val > this->compare_val_);
351 return (
my_val >= this->compare_val_);
357 return (
my_val == this->compare_val_);
359 PCL_WARN (
"[pcl::PackedHSIComparison::evaluate] unrecognized op_!\n");
368template<
typename Po
intT>
384 PCL_WARN (
"[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] x field not found!\n");
398 PCL_WARN (
"[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] y field not found!\n");
412 PCL_WARN (
"[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] z field not found!\n");
417 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;
426template<
typename Po
intT>
446 PCL_WARN (
"[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] x field not found!\n");
460 PCL_WARN (
"[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] y field not found!\n");
474 PCL_WARN (
"[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] z field not found!\n");
488template<
typename Po
intT>
511 PCL_WARN (
"[pcl::TfQuadraticXYZComparison::evaluate] unrecognized op_!\n");
519template <
typename Po
intT>
int
526 const std::uint8_t*
pt_data =
reinterpret_cast<const std::uint8_t*
> (&p);
579 PCL_WARN (
"[pcl::PointDataAtOffset::compare] unknown data_type!\n");
587template <
typename Po
intT>
void
596template <
typename Po
intT>
void
607template <
typename Po
intT>
bool
610 for (std::size_t i = 0; i < comparisons_.size (); ++i)
611 if (!comparisons_[i]->evaluate (point))
614 for (std::size_t i = 0; i < conditions_.size (); ++i)
615 if (!conditions_[i]->evaluate (point))
624template <
typename Po
intT>
bool
627 if (comparisons_.empty () && conditions_.empty ())
629 for (std::size_t i = 0; i < comparisons_.size (); ++i)
630 if (comparisons_[i]->evaluate(point))
633 for (std::size_t i = 0; i < conditions_.size (); ++i)
634 if (conditions_[i]->evaluate (point))
643template <
typename Po
intT>
void
647 capable_ = condition_->isCapable ();
651template <
typename Po
intT>
void
654 if (capable_ ==
false)
656 PCL_WARN (
"[pcl::%s::applyFilter] not capable!\n", getClassName ().
c_str ());
662 PCL_WARN (
"[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().
c_str ());
666 if (condition_.get () ==
nullptr)
668 PCL_WARN (
"[pcl::%s::applyFilter] No filtering condition given!\n", getClassName ().
c_str ());
673 output.header = input_->header;
674 if (!keep_organized_)
681 output.height = this->input_->height;
682 output.width = this->input_->width;
683 output.is_dense = this->input_->is_dense;
685 output.resize (input_->size ());
686 removed_indices_->resize (input_->size ());
690 if (!keep_organized_)
696 const PointT& point = (*input_)[index];
698 if (!std::isfinite (point.x)
699 || !std::isfinite (point.y)
700 || !std::isfinite (point.z))
702 if (extract_removed_indices_)
710 if (condition_->evaluate (point))
717 if (extract_removed_indices_)
731 std::sort (indices.begin (), indices.end ());
734 for (std::size_t cp = 0; cp < input_->
size (); ++cp)
736 if (cp ==
static_cast<std::size_t
> (indices[
ci]))
738 if (
ci < indices.
size () - 1)
741 if (cp ==
static_cast<std::size_t
> (indices[
ci]))
748 if (!condition_->evaluate ((*input_)[cp]))
750 output[cp].getVector4fMap ().setConstant (user_filter_value_);
753 if (extract_removed_indices_)
755 (*removed_indices_)[
nr_removed_p] =
static_cast<int> (cp);
762 output[cp].getVector4fMap ().setConstant (user_filter_value_);
768 if (
removed_p && !std::isfinite (user_filter_value_))
774#define PCL_INSTANTIATE_PointDataAtOffset(T) template class PCL_EXPORTS pcl::PointDataAtOffset<T>;
775#define PCL_INSTANTIATE_ComparisonBase(T) template class PCL_EXPORTS pcl::ComparisonBase<T>;
776#define PCL_INSTANTIATE_FieldComparison(T) template class PCL_EXPORTS pcl::FieldComparison<T>;
777#define PCL_INSTANTIATE_PackedRGBComparison(T) template class PCL_EXPORTS pcl::PackedRGBComparison<T>;
778#define PCL_INSTANTIATE_PackedHSIComparison(T) template class PCL_EXPORTS pcl::PackedHSIComparison<T>;
779#define PCL_INSTANTIATE_TfQuadraticXYZComparison(T) template class PCL_EXPORTS pcl::TfQuadraticXYZComparison<T>;
780#define PCL_INSTANTIATE_ConditionBase(T) template class PCL_EXPORTS pcl::ConditionBase<T>;
781#define PCL_INSTANTIATE_ConditionAnd(T) template class PCL_EXPORTS pcl::ConditionAnd<T>;
782#define PCL_INSTANTIATE_ConditionOr(T) template class PCL_EXPORTS pcl::ConditionOr<T>;
783#define PCL_INSTANTIATE_ConditionalRemoval(T) template class PCL_EXPORTS pcl::ConditionalRemoval<T>;
The (abstract) base class for the comparison object.
ComparisonOps::CompareOp op_
The comparison operator type.
bool capable_
True if capable.
std::string field_name_
Field name to compare data on.
bool evaluate(const PointT &point) const override
Determine if a point meets this condition.
void addCondition(Ptr condition)
Add a nested condition to this condition.
typename ComparisonBase::ConstPtr ComparisonBaseConstPtr
void addComparison(ComparisonBaseConstPtr comparison)
Add a new comparison.
shared_ptr< ConditionBase< PointT > > Ptr
bool evaluate(const PointT &point) const override
Determine if a point meets this condition.
void applyFilter(PointCloud &output) override
Filter a Point Cloud.
void setCondition(ConditionBasePtr condition)
Set the condition that the filter will use.
typename ConditionBase::Ptr ConditionBasePtr
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
The field-based specialization of the comparison object.
~FieldComparison()
Destructor.
PointDataAtOffset< PointT > * point_data_
The point data to compare.
bool evaluate(const PointT &point) const override
Determine the result of this comparison.
Filter represents the base filter class.
A packed HSI specialization of the comparison object.
std::uint32_t rgb_offset_
The offset of the component.
bool evaluate(const PointT &point) const override
Determine the result of this comparison.
ComponentId component_id_
The ID of the component.
A packed rgb specialization of the comparison object.
bool evaluate(const PointT &point) const override
Determine the result of this comparison.
std::uint32_t component_offset_
The offset of the component.
int compare(const PointT &p, const double &val)
Compare function.
TfQuadraticXYZComparison()
Constructor.
void setComparisonVector(const Eigen::Vector3f &vector)
set the vector "v" of the comparison "p'Ap + 2v'p + c [OP] 0".
Eigen::Matrix4f comp_matr_
void transformComparison(const Eigen::Matrix4f &transform)
transform the coordinate system of the comparison.
Eigen::Vector4f comp_vect_
void setComparisonMatrix(const Eigen::Matrix3f &matrix)
set the matrix "A" of the comparison "p'Ap + 2v'p + c [OP] 0".
bool evaluate(const PointT &point) const override
Determine the result of this comparison.
void copyPoint(const PointInT &point_in, PointOutT &point_out)
Copy the fields of a source point into a target point.
CompareOp
The kind of comparison operations that are possible within a comparison object.
A point structure representing Euclidean xyz coordinates, and the RGB color.