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.
boost::shared_ptr< ConditionBase< PointT > > Ptr
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
pcl::PCLHeader header
The point cloud header.
virtual bool evaluate(const PointT &point) const
Determine the result of this comparison.
bool capable_
True if capable.
uint32_t width
The point cloud width (if organized as an image-structure).
PointDataAtOffset< PointT > * point_data_
The point data to compare.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
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_
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.
uint32_t height
The point cloud height (if organized as an image-structure).
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 rgb_offset_
The offset of the component.
Filter represents the base filter class.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
Helper functor structure for concatenate.
virtual bool evaluate(const PointT &point) const
Determine the result of this comparison.
ConditionBase::Ptr ConditionBasePtr