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_ (NULL)
58 std::vector<pcl::PCLPointField> point_fields;
64 if (point_fields.empty ())
66 PCL_WARN (
"[pcl::FieldComparison::FieldComparison] no fields found!\n");
73 for (d = 0; d < point_fields.size (); ++d)
75 if (point_fields[d].name == field_name)
79 if (d == point_fields.size ())
81 PCL_WARN (
"[pcl::FieldComparison::FieldComparison] field not found!\n");
85 uint8_t datatype = point_fields[d].datatype;
86 uint32_t offset = point_fields[d].offset;
93 template <
typename Po
intT>
96 if (point_data_ != NULL)
104 template <
typename Po
intT>
bool 109 PCL_WARN (
"[pcl::FieldComparison::evaluate] invalid comparison!\n");
116 int compare_result = point_data_->compare (point, compare_val_);
121 return (compare_result > 0);
123 return (compare_result >= 0);
125 return (compare_result < 0);
127 return (compare_result <= 0);
129 return (compare_result == 0);
131 PCL_WARN (
"[pcl::FieldComparison::evaluate] unrecognized op_!\n");
139 template <
typename Po
intT>
142 component_name_ (component_name), component_offset_ (), compare_val_ (compare_val)
145 std::vector<pcl::PCLPointField> point_fields;
152 for (d = 0; d < point_fields.size (); ++d)
154 if (point_fields[d].name ==
"rgb" || point_fields[d].name ==
"rgba")
157 if (d == point_fields.size ())
159 PCL_WARN (
"[pcl::PackedRGBComparison::PackedRGBComparison] rgb field not found!\n");
165 uint8_t datatype = point_fields[d].datatype;
170 PCL_WARN (
"[pcl::PackedRGBComparison::PackedRGBComparison] has unusable type!\n");
176 if (component_name ==
"r")
180 else if (component_name ==
"g")
184 else if (component_name ==
"b")
190 PCL_WARN (
"[pcl::PackedRGBComparison::PackedRGBComparison] unrecognized component name!\n");
202 template <
typename Po
intT>
bool 206 const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&point);
207 uint8_t my_val = *(pt_data + component_offset_);
213 return (my_val > this->compare_val_);
215 return (my_val >= this->compare_val_);
217 return (my_val < this->compare_val_);
219 return (my_val <= this->compare_val_);
221 return (my_val == this->compare_val_);
223 PCL_WARN (
"[pcl::PackedRGBComparison::evaluate] unrecognized op_!\n");
231 template <
typename Po
intT>
234 component_name_ (component_name), component_id_ (), compare_val_ (compare_val), rgb_offset_ ()
237 std::vector<pcl::PCLPointField> point_fields;
244 for (d = 0; d < point_fields.size (); ++d)
245 if (point_fields[d].name ==
"rgb" || point_fields[d].name ==
"rgba")
247 if (d == point_fields.size ())
249 PCL_WARN (
"[pcl::PackedHSIComparison::PackedHSIComparison] rgb field not found!\n");
255 uint8_t datatype = point_fields[d].datatype;
260 PCL_WARN (
"[pcl::PackedHSIComparison::PackedHSIComparison] has unusable type!\n");
266 uint32_t offset = point_fields[d].offset;
269 PCL_WARN (
"[pcl::PackedHSIComparison::PackedHSIComparison] rgb field is not 32 bit aligned!\n");
276 if (component_name ==
"h" )
280 else if (component_name ==
"s")
284 else if (component_name ==
"i")
290 PCL_WARN (
"[pcl::PackedHSIComparison::PackedHSIComparison] unrecognized component name!\n");
301 template <
typename Po
intT>
bool 305 static uint32_t rgb_val_ = 0;
306 static uint8_t r_ = 0;
307 static uint8_t g_ = 0;
308 static uint8_t b_ = 0;
309 static int8_t h_ = 0;
310 static uint8_t s_ = 0;
311 static uint8_t i_ = 0;
314 const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&point);
315 const uint32_t* rgb_data = reinterpret_cast<const uint32_t*> (pt_data + rgb_offset_);
316 uint32_t new_rgb_val = *rgb_data;
318 if (rgb_val_ != new_rgb_val)
320 rgb_val_ = new_rgb_val;
322 r_ = static_cast <uint8_t> (rgb_val_ >> 16);
323 g_ = static_cast <uint8_t> (rgb_val_ >> 8);
324 b_ = static_cast <uint8_t> (rgb_val_);
327 float hx = (2.0f * r_ - g_ - b_) / 4.0f;
328 float hy = static_cast<float> (g_ - b_) * 111.0f / 255.0f;
329 h_ = static_cast<int8_t> (atan2(hy, hx) * 128.0f / M_PI);
331 int32_t i = (r_+g_+b_)/3;
332 i_ = static_cast<uint8_t> (i);
335 m = (r_ < g_) ? r_ : g_;
336 m = (m < b_) ? m : b_;
338 s_ = static_cast<uint8_t> ((i == 0) ? 0 : 255 - (m * 255) / i);
343 switch (component_id_)
346 my_val = static_cast <float> (h_);
349 my_val = static_cast <float> (s_);
352 my_val = static_cast <float> (i_);
362 return (my_val > this->compare_val_);
364 return (my_val >= this->compare_val_);
366 return (my_val < this->compare_val_);
368 return (my_val <= this->compare_val_);
370 return (my_val == this->compare_val_);
372 PCL_WARN (
"[pcl::PackedHSIComparison::evaluate] unrecognized op_!\n");
381 template<
typename Po
intT>
383 comp_matr_ (), comp_vect_ (), comp_scalar_ (0.0)
386 std::vector<pcl::PCLPointField> point_fields;
393 for (dX = 0; dX < point_fields.size (); ++dX)
395 if (point_fields[dX].name ==
"x")
398 if (dX == point_fields.size ())
400 PCL_WARN (
"[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] x field not found!\n");
407 for (dY = 0; dY < point_fields.size (); ++dY)
409 if (point_fields[dY].name ==
"y")
412 if (dY == point_fields.size ())
414 PCL_WARN (
"[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] y field not found!\n");
421 for (dZ = 0; dZ < point_fields.size (); ++dZ)
423 if (point_fields[dZ].name ==
"z")
426 if (dZ == point_fields.size ())
428 PCL_WARN (
"[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] z field not found!\n");
433 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;
442 template<
typename Po
intT>
444 const Eigen::Matrix3f &comparison_matrix,
445 const Eigen::Vector3f &comparison_vector,
446 const float &comparison_scalar,
447 const Eigen::Affine3f &comparison_transform) :
448 comp_matr_ (), comp_vect_ (), comp_scalar_ (comparison_scalar)
451 std::vector<pcl::PCLPointField> point_fields;
458 for (dX = 0; dX < point_fields.size (); ++dX)
460 if (point_fields[dX].name ==
"x")
463 if (dX == point_fields.size ())
465 PCL_WARN (
"[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] x field not found!\n");
472 for (dY = 0; dY < point_fields.size (); ++dY)
474 if (point_fields[dY].name ==
"y")
477 if (dY == point_fields.size ())
479 PCL_WARN (
"[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] y field not found!\n");
486 for (dZ = 0; dZ < point_fields.size (); ++dZ)
488 if (point_fields[dZ].name ==
"z")
491 if (dZ == point_fields.size ())
493 PCL_WARN (
"[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] z field not found!\n");
502 if (!comparison_transform.matrix ().isIdentity ())
507 template<
typename Po
intT>
511 Eigen::Vector4f pointAffine;
512 pointAffine << point.x, point.y, point.z, 1;
514 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;
530 PCL_WARN (
"[pcl::TfQuadraticXYZComparison::evaluate] unrecognized op_!\n");
538 template <
typename Po
intT>
int 545 const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&p);
552 memcpy (&pt_val, pt_data + this->offset_,
sizeof (int8_t));
553 return (pt_val > static_cast<int8_t>(val)) - (pt_val < static_cast<int8_t> (val));
558 memcpy (&pt_val, pt_data + this->offset_,
sizeof (uint8_t));
559 return (pt_val > static_cast<uint8_t>(val)) - (pt_val < static_cast<uint8_t> (val));
564 memcpy (&pt_val, pt_data + this->offset_,
sizeof (int16_t));
565 return (pt_val > static_cast<int16_t>(val)) - (pt_val < static_cast<int16_t> (val));
570 memcpy (&pt_val, pt_data + this->offset_,
sizeof (uint16_t));
571 return (pt_val > static_cast<uint16_t> (val)) - (pt_val < static_cast<uint16_t> (val));
576 memcpy (&pt_val, pt_data + this->offset_,
sizeof (int32_t));
577 return (pt_val > static_cast<int32_t> (val)) - (pt_val < static_cast<int32_t> (val));
582 memcpy (&pt_val, pt_data + this->offset_,
sizeof (uint32_t));
583 return (pt_val > static_cast<uint32_t> (val)) - (pt_val < static_cast<uint32_t> (val));
588 memcpy (&pt_val, pt_data + this->offset_,
sizeof (
float));
589 return (pt_val > static_cast<float> (val)) - (pt_val < static_cast<float> (val));
594 memcpy (&pt_val, pt_data + this->offset_,
sizeof (
double));
595 return (pt_val > val) - (pt_val < val);
598 PCL_WARN (
"[pcl::PointDataAtOffset::compare] unknown data_type!\n");
606 template <
typename Po
intT>
void 609 if (!comparison->isCapable ())
611 comparisons_.push_back (comparison);
615 template <
typename Po
intT>
void 618 if (!condition->isCapable ())
620 conditions_.push_back (condition);
626 template <
typename Po
intT>
bool 629 for (
size_t i = 0; i < comparisons_.size (); ++i)
630 if (!comparisons_[i]->evaluate (point))
633 for (
size_t i = 0; i < conditions_.size (); ++i)
634 if (!conditions_[i]->evaluate (point))
643 template <
typename Po
intT>
bool 646 if (comparisons_.empty () && conditions_.empty ())
648 for (
size_t i = 0; i < comparisons_.size (); ++i)
649 if (comparisons_[i]->evaluate(point))
652 for (
size_t i = 0; i < conditions_.size (); ++i)
653 if (conditions_[i]->evaluate (point))
662 template <
typename Po
intT>
void 665 condition_ = condition;
666 capable_ = condition_->isCapable ();
670 template <
typename Po
intT>
void 673 if (capable_ ==
false)
675 PCL_WARN (
"[pcl::%s::applyFilter] not capable!\n", getClassName ().c_str ());
681 PCL_WARN (
"[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ());
685 if (condition_.get () == NULL)
687 PCL_WARN (
"[pcl::%s::applyFilter] No filtering condition given!\n", getClassName ().c_str ());
692 output.
header = input_->header;
693 if (!keep_organized_)
700 output.
height = this->input_->height;
701 output.
width = this->input_->width;
702 output.
is_dense = this->input_->is_dense;
704 output.
points.resize (input_->points.size ());
705 removed_indices_->resize (input_->points.size ());
708 int nr_removed_p = 0;
710 if (!keep_organized_)
712 for (
size_t cp = 0; cp < Filter<PointT>::indices_->size (); ++cp)
719 if (extract_removed_indices_)
734 if (extract_removed_indices_)
743 output.
points.resize (nr_p);
748 std::sort (indices.begin (), indices.end ());
749 bool removed_p =
false;
751 for (
size_t cp = 0; cp < input_->points.size (); ++cp)
753 if (cp == static_cast<size_t> (indices[ci]))
755 if (ci < indices.size () - 1)
758 if (cp == static_cast<size_t> (indices[ci]))
765 if (!condition_->evaluate (input_->points[cp]))
767 output.
points[cp].getVector4fMap ().setConstant (user_filter_value_);
770 if (extract_removed_indices_)
772 (*removed_indices_)[nr_removed_p] = static_cast<int> (cp);
779 output.
points[cp].getVector4fMap ().setConstant (user_filter_value_);
785 if (removed_p && !pcl_isfinite (user_filter_value_))
788 removed_indices_->resize (nr_removed_p);
791 #define PCL_INSTANTIATE_PointDataAtOffset(T) template class PCL_EXPORTS pcl::PointDataAtOffset<T>; 792 #define PCL_INSTANTIATE_ComparisonBase(T) template class PCL_EXPORTS pcl::ComparisonBase<T>; 793 #define PCL_INSTANTIATE_FieldComparison(T) template class PCL_EXPORTS pcl::FieldComparison<T>; 794 #define PCL_INSTANTIATE_PackedRGBComparison(T) template class PCL_EXPORTS pcl::PackedRGBComparison<T>; 795 #define PCL_INSTANTIATE_PackedHSIComparison(T) template class PCL_EXPORTS pcl::PackedHSIComparison<T>; 796 #define PCL_INSTANTIATE_TfQuadraticXYZComparison(T) template class PCL_EXPORTS pcl::TfQuadraticXYZComparison<T>; 797 #define PCL_INSTANTIATE_ConditionBase(T) template class PCL_EXPORTS pcl::ConditionBase<T>; 798 #define PCL_INSTANTIATE_ConditionAnd(T) template class PCL_EXPORTS pcl::ConditionAnd<T>; 799 #define PCL_INSTANTIATE_ConditionOr(T) template class PCL_EXPORTS pcl::ConditionOr<T>; 800 #define PCL_INSTANTIATE_ConditionalRemoval(T) template class PCL_EXPORTS pcl::ConditionalRemoval<T>; void setComparisonMatrix(const Eigen::Matrix3f &matrix)
set the matrix "A" of the comparison "p'Ap + 2v'p + c [OP] 0".
ComparisonBase::ConstPtr ComparisonBaseConstPtr
virtual bool evaluate(const PointT &point) const
Determine the result of this comparison.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
PointDataAtOffset< PointT > * point_data_
The point data to compare.
ComponentId component_id_
The ID of the component.
uint32_t rgb_offset_
The offset of the component.
bool capable_
True if capable.
A datatype that enables type-correct comparisons.
void setCondition(ConditionBasePtr condition)
Set the condition that the filter will use.
virtual bool evaluate(const PointT &point) const
Determine the result of this comparison.
The (abstract) base class for the comparison object.
void setComparisonVector(const Eigen::Vector3f &vector)
set the vector "v" of the comparison "p'Ap + 2v'p + c [OP] 0".
virtual bool evaluate(const PointT &point) const
Determine the result of this comparison.
ComparisonOps::CompareOp op_
The comparison operator type.
uint32_t height
The point cloud height (if organized as an image-structure).
virtual bool evaluate(const PointT &point) const
Determine the result of this comparison.
uint32_t width
The point cloud width (if organized as an image-structure).
Filter represents the base filter class.
int compare(const PointT &p, const double &val)
Compare function.
Eigen::Vector4f comp_vect_
The field-based specialization of the comparison object.
A packed rgb specialization of the comparison object.
uint32_t component_offset_
The offset of the component.
virtual bool evaluate(const PointT &point) const
Determine if a point meets this condition.
void transformComparison(const Eigen::Matrix4f &transform)
transform the coordinate system of the comparison.
std::string field_name_
Field name to compare data on.
CompareOp
The kind of comparison operations that are possible within a comparison object.
TfQuadraticXYZComparison()
Constructor.
PointCloud represents the base class in PCL for storing collections of 3D points.
pcl::PCLHeader header
The point cloud header.
void applyFilter(PointCloud &output)
Filter a Point Cloud.
void addCondition(Ptr condition)
Add a nested condition to this condition.
Eigen::Matrix4f comp_matr_
void addComparison(ComparisonBaseConstPtr comparison)
Add a new comparison.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
ConditionBase::Ptr ConditionBasePtr
A packed HSI specialization of the comparison object.
A point structure representing Euclidean xyz coordinates, and the RGB color.
virtual ~FieldComparison()
Destructor.
void copyPoint(const PointInT &point_in, PointOutT &point_out)
Copy the fields of a source point into a target point.
boost::shared_ptr< ConditionBase< PointT > > Ptr
void getFields(const pcl::PointCloud< PointT > &cloud, std::vector< pcl::PCLPointField > &fields)
Get the list of available fields (i.e., dimension/channel)
virtual bool evaluate(const PointT &point) const
Determine if a point meets this condition.