Point Cloud Library (PCL)  1.11.1-dev
conditional_removal.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2010, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the copyright holder(s) nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *
34  * $Id$
35  *
36  */
37 
38 #ifndef PCL_FILTER_IMPL_FIELD_VAL_CONDITION_H_
39 #define PCL_FILTER_IMPL_FIELD_VAL_CONDITION_H_
40 
41 #include <pcl/common/io.h>
42 #include <pcl/common/copy_point.h>
43 #include <pcl/filters/conditional_removal.h>
44 
45 //////////////////////////////////////////////////////////////////////////
46 //////////////////////////////////////////////////////////////////////////
47 //////////////////////////////////////////////////////////////////////////
48 template <typename PointT>
50  const std::string &field_name, ComparisonOps::CompareOp op, double compare_val)
52  , compare_val_ (compare_val), point_data_ (nullptr)
53 {
54  field_name_ = field_name;
55  op_ = op;
56 
57  // Get all fields
58  const auto point_fields = pcl::getFields<PointT> ();
59 
60  // Find field_name
61  if (point_fields.empty ())
62  {
63  PCL_WARN ("[pcl::FieldComparison::FieldComparison] no fields found!\n");
64  capable_ = false;
65  return;
66  }
67 
68  // Get the field index
69  std::size_t d;
70  for (d = 0; d < point_fields.size (); ++d)
71  {
72  if (point_fields[d].name == field_name)
73  break;
74  }
75 
76  if (d == point_fields.size ())
77  {
78  PCL_WARN ("[pcl::FieldComparison::FieldComparison] field not found!\n");
79  capable_ = false;
80  return;
81  }
82  std::uint8_t datatype = point_fields[d].datatype;
83  std::uint32_t offset = point_fields[d].offset;
84 
85  point_data_ = new PointDataAtOffset<PointT>(datatype, offset);
86  capable_ = true;
87 }
88 
89 //////////////////////////////////////////////////////////////////////////
90 template <typename PointT>
92 {
93  delete point_data_;
94 }
95 
96 //////////////////////////////////////////////////////////////////////////
97 template <typename PointT> bool
99 {
100  if (!this->capable_)
101  {
102  PCL_WARN ("[pcl::FieldComparison::evaluate] invalid comparison!\n");
103  return (false);
104  }
105 
106  // if p(data) > val then compare_result = 1
107  // if p(data) == val then compare_result = 0
108  // if p(data) < ival then compare_result = -1
109  int compare_result = point_data_->compare (point, compare_val_);
110 
111  switch (this->op_)
112  {
114  return (compare_result > 0);
116  return (compare_result >= 0);
118  return (compare_result < 0);
120  return (compare_result <= 0);
122  return (compare_result == 0);
123  default:
124  PCL_WARN ("[pcl::FieldComparison::evaluate] unrecognized op_!\n");
125  return (false);
126  }
127 }
128 
129 //////////////////////////////////////////////////////////////////////////
130 //////////////////////////////////////////////////////////////////////////
131 //////////////////////////////////////////////////////////////////////////
132 template <typename PointT>
134  const std::string &component_name, ComparisonOps::CompareOp op, double compare_val) :
135  component_name_ (component_name), component_offset_ (), compare_val_ (compare_val)
136 {
137  // get all the fields
138  const auto point_fields = pcl::getFields<PointT> ();
139 
140  // Locate the "rgb" field
141  std::size_t d;
142  for (d = 0; d < point_fields.size (); ++d)
143  {
144  if (point_fields[d].name == "rgb" || point_fields[d].name == "rgba")
145  break;
146  }
147  if (d == point_fields.size ())
148  {
149  PCL_WARN ("[pcl::PackedRGBComparison::PackedRGBComparison] rgb field not found!\n");
150  capable_ = false;
151  return;
152  }
153 
154  // Verify the datatype
155  std::uint8_t datatype = point_fields[d].datatype;
156  if (datatype != pcl::PCLPointField::FLOAT32 &&
157  datatype != pcl::PCLPointField::UINT32 &&
158  datatype != pcl::PCLPointField::INT32)
159  {
160  PCL_WARN ("[pcl::PackedRGBComparison::PackedRGBComparison] has unusable type!\n");
161  capable_ = false;
162  return;
163  }
164 
165  // Verify the component name
166  if (component_name == "r")
167  {
168  component_offset_ = point_fields[d].offset + 2;
169  }
170  else if (component_name == "g")
171  {
172  component_offset_ = point_fields[d].offset + 1;
173  }
174  else if (component_name == "b")
175  {
176  component_offset_ = point_fields[d].offset;
177  }
178  else
179  {
180  PCL_WARN ("[pcl::PackedRGBComparison::PackedRGBComparison] unrecognized component name!\n");
181  capable_ = false;
182  return;
183  }
184 
185  // save the rest of the context
186  capable_ = true;
187  op_ = op;
188 }
189 
190 
191 //////////////////////////////////////////////////////////////////////////
192 template <typename PointT> bool
194 {
195  // extract the component value
196  const std::uint8_t* pt_data = reinterpret_cast<const std::uint8_t*> (&point);
197  std::uint8_t my_val = *(pt_data + component_offset_);
198 
199  // now do the comparison
200  switch (this->op_)
201  {
203  return (my_val > this->compare_val_);
205  return (my_val >= this->compare_val_);
207  return (my_val < this->compare_val_);
209  return (my_val <= this->compare_val_);
211  return (my_val == this->compare_val_);
212  default:
213  PCL_WARN ("[pcl::PackedRGBComparison::evaluate] unrecognized op_!\n");
214  return (false);
215  }
216 }
217 
218 //////////////////////////////////////////////////////////////////////////
219 //////////////////////////////////////////////////////////////////////////
220 //////////////////////////////////////////////////////////////////////////
221 template <typename PointT>
223  const std::string &component_name, ComparisonOps::CompareOp op, double compare_val) :
224  component_name_ (component_name), component_id_ (), compare_val_ (compare_val), rgb_offset_ ()
225 {
226  // Get all the fields
227  const auto point_fields = pcl::getFields<PointT> ();
228 
229  // Locate the "rgb" field
230  std::size_t d;
231  for (d = 0; d < point_fields.size (); ++d)
232  if (point_fields[d].name == "rgb" || point_fields[d].name == "rgba")
233  break;
234  if (d == point_fields.size ())
235  {
236  PCL_WARN ("[pcl::PackedHSIComparison::PackedHSIComparison] rgb field not found!\n");
237  capable_ = false;
238  return;
239  }
240 
241  // Verify the datatype
242  std::uint8_t datatype = point_fields[d].datatype;
243  if (datatype != pcl::PCLPointField::FLOAT32 &&
244  datatype != pcl::PCLPointField::UINT32 &&
245  datatype != pcl::PCLPointField::INT32)
246  {
247  PCL_WARN ("[pcl::PackedHSIComparison::PackedHSIComparison] has unusable type!\n");
248  capable_ = false;
249  return;
250  }
251 
252  // verify the offset
253  std::uint32_t offset = point_fields[d].offset;
254  if (offset % 4 != 0)
255  {
256  PCL_WARN ("[pcl::PackedHSIComparison::PackedHSIComparison] rgb field is not 32 bit aligned!\n");
257  capable_ = false;
258  return;
259  }
260  rgb_offset_ = point_fields[d].offset;
261 
262  // verify the component name
263  if (component_name == "h" )
264  {
265  component_id_ = H;
266  }
267  else if (component_name == "s")
268  {
269  component_id_ = S;
270  }
271  else if (component_name == "i")
272  {
273  component_id_ = I;
274  }
275  else
276  {
277  PCL_WARN ("[pcl::PackedHSIComparison::PackedHSIComparison] unrecognized component name!\n");
278  capable_ = false;
279  return;
280  }
281 
282  // Save the context
283  capable_ = true;
284  op_ = op;
285 }
286 
287 //////////////////////////////////////////////////////////////////////////
288 template <typename PointT> bool
290 {
291  // Since this is a const function, we can't make these data members because we change them here
292  static std::uint32_t rgb_val_ = 0;
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;
299 
300  // We know that rgb data is 32 bit aligned (verified in the ctor) so...
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_);
303  std::uint32_t new_rgb_val = *rgb_data;
304 
305  if (rgb_val_ != new_rgb_val)
306  { // avoid having to redo this calc, if possible
307  rgb_val_ = new_rgb_val;
308  // extract r,g,b
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_);
312 
313  // definitions taken from http://en.wikipedia.org/wiki/HSL_and_HSI
314  float hx = (2.0f * r_ - g_ - b_) / 4.0f; // hue x component -127 to 127
315  float hy = static_cast<float> (g_ - b_) * 111.0f / 255.0f; // hue y component -111 to 111
316  h_ = static_cast<std::int8_t> (std::atan2(hy, hx) * 128.0f / M_PI);
317 
318  std::int32_t i = (r_+g_+b_)/3; // 0 to 255
319  i_ = static_cast<std::uint8_t> (i);
320 
321  std::int32_t m; // min(r,g,b)
322  m = (r_ < g_) ? r_ : g_;
323  m = (m < b_) ? m : b_;
324 
325  s_ = static_cast<std::uint8_t> ((i == 0) ? 0 : 255 - (m * 255) / i); // saturation 0 to 255
326  }
327 
328  float my_val = 0;
329 
330  switch (component_id_)
331  {
332  case H:
333  my_val = static_cast <float> (h_);
334  break;
335  case S:
336  my_val = static_cast <float> (s_);
337  break;
338  case I:
339  my_val = static_cast <float> (i_);
340  break;
341  default:
342  assert (false);
343  }
344 
345  // now do the comparison
346  switch (this->op_)
347  {
349  return (my_val > this->compare_val_);
351  return (my_val >= this->compare_val_);
353  return (my_val < this->compare_val_);
355  return (my_val <= this->compare_val_);
357  return (my_val == this->compare_val_);
358  default:
359  PCL_WARN ("[pcl::PackedHSIComparison::evaluate] unrecognized op_!\n");
360  return (false);
361  }
362 }
363 
364 
365 //////////////////////////////////////////////////////////////////////////
366 //////////////////////////////////////////////////////////////////////////
367 //////////////////////////////////////////////////////////////////////////
368 template<typename PointT>
370  comp_scalar_ (0.0)
371 {
372  // get all the fields
373  const auto point_fields = pcl::getFields<PointT> ();
374 
375  // Locate the "x" field
376  std::size_t dX;
377  for (dX = 0; dX < point_fields.size (); ++dX)
378  {
379  if (point_fields[dX].name == "x")
380  break;
381  }
382  if (dX == point_fields.size ())
383  {
384  PCL_WARN ("[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] x field not found!\n");
385  capable_ = false;
386  return;
387  }
388 
389  // Locate the "y" field
390  std::size_t dY;
391  for (dY = 0; dY < point_fields.size (); ++dY)
392  {
393  if (point_fields[dY].name == "y")
394  break;
395  }
396  if (dY == point_fields.size ())
397  {
398  PCL_WARN ("[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] y field not found!\n");
399  capable_ = false;
400  return;
401  }
402 
403  // Locate the "z" field
404  std::size_t dZ;
405  for (dZ = 0; dZ < point_fields.size (); ++dZ)
406  {
407  if (point_fields[dZ].name == "z")
408  break;
409  }
410  if (dZ == point_fields.size ())
411  {
412  PCL_WARN ("[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] z field not found!\n");
413  capable_ = false;
414  return;
415  }
416 
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;
418  comp_vect_ << 0.0, 0.0, 0.0, 1.0;
419  tf_comp_matr_ = comp_matr_;
420  tf_comp_vect_ = comp_vect_;
422  capable_ = true;
423 }
424 
425 //////////////////////////////////////////////////////////////////////////
426 template<typename PointT>
428  const Eigen::Matrix3f &comparison_matrix,
429  const Eigen::Vector3f &comparison_vector,
430  const float &comparison_scalar,
431  const Eigen::Affine3f &comparison_transform) :
432  comp_scalar_ (comparison_scalar)
433 {
434  // get all the fields
435  const auto point_fields = pcl::getFields<PointT> ();
436 
437  // Locate the "x" field
438  std::size_t dX;
439  for (dX = 0; dX < point_fields.size (); ++dX)
440  {
441  if (point_fields[dX].name == "x")
442  break;
443  }
444  if (dX == point_fields.size ())
445  {
446  PCL_WARN ("[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] x field not found!\n");
447  capable_ = false;
448  return;
449  }
450 
451  // Locate the "y" field
452  std::size_t dY;
453  for (dY = 0; dY < point_fields.size (); ++dY)
454  {
455  if (point_fields[dY].name == "y")
456  break;
457  }
458  if (dY == point_fields.size ())
459  {
460  PCL_WARN ("[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] y field not found!\n");
461  capable_ = false;
462  return;
463  }
464 
465  // Locate the "z" field
466  std::size_t dZ;
467  for (dZ = 0; dZ < point_fields.size (); ++dZ)
468  {
469  if (point_fields[dZ].name == "z")
470  break;
471  }
472  if (dZ == point_fields.size ())
473  {
474  PCL_WARN ("[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] z field not found!\n");
475  capable_ = false;
476  return;
477  }
478 
479  capable_ = true;
480  op_ = op;
481  setComparisonMatrix (comparison_matrix);
482  setComparisonVector (comparison_vector);
483  if (!comparison_transform.matrix ().isIdentity ())
484  transformComparison (comparison_transform);
485 }
486 
487 //////////////////////////////////////////////////////////////////////////
488 template<typename PointT>
489 bool
491 {
492  Eigen::Vector4f pointAffine;
493  pointAffine << point.x, point.y, point.z, 1;
494 
495  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;
496 
497  // now do the comparison
498  switch (this->op_)
499  {
501  return (myVal > 0);
503  return (myVal >= 0);
505  return (myVal < 0);
507  return (myVal <= 0);
509  return (myVal == 0);
510  default:
511  PCL_WARN ("[pcl::TfQuadraticXYZComparison::evaluate] unrecognized op_!\n");
512  return (false);
513  }
514 }
515 
516 //////////////////////////////////////////////////////////////////////////
517 //////////////////////////////////////////////////////////////////////////
518 //////////////////////////////////////////////////////////////////////////
519 template <typename PointT> int
520 pcl::PointDataAtOffset<PointT>::compare (const PointT& p, const double& val)
521 {
522  // if p(data) > val return 1
523  // if p(data) == val return 0
524  // if p(data) < val return -1
525 
526  const std::uint8_t* pt_data = reinterpret_cast<const std::uint8_t*> (&p);
527 
528  switch (datatype_)
529  {
531  {
532  std::int8_t pt_val;
533  memcpy (&pt_val, pt_data + this->offset_, sizeof (std::int8_t));
534  return (pt_val > static_cast<std::int8_t>(val)) - (pt_val < static_cast<std::int8_t> (val));
535  }
537  {
538  std::uint8_t pt_val;
539  memcpy (&pt_val, pt_data + this->offset_, sizeof (std::uint8_t));
540  return (pt_val > static_cast<std::uint8_t>(val)) - (pt_val < static_cast<std::uint8_t> (val));
541  }
543  {
544  std::int16_t pt_val;
545  memcpy (&pt_val, pt_data + this->offset_, sizeof (std::int16_t));
546  return (pt_val > static_cast<std::int16_t>(val)) - (pt_val < static_cast<std::int16_t> (val));
547  }
549  {
550  std::uint16_t pt_val;
551  memcpy (&pt_val, pt_data + this->offset_, sizeof (std::uint16_t));
552  return (pt_val > static_cast<std::uint16_t> (val)) - (pt_val < static_cast<std::uint16_t> (val));
553  }
555  {
556  std::int32_t pt_val;
557  memcpy (&pt_val, pt_data + this->offset_, sizeof (std::int32_t));
558  return (pt_val > static_cast<std::int32_t> (val)) - (pt_val < static_cast<std::int32_t> (val));
559  }
561  {
562  std::uint32_t pt_val;
563  memcpy (&pt_val, pt_data + this->offset_, sizeof (std::uint32_t));
564  return (pt_val > static_cast<std::uint32_t> (val)) - (pt_val < static_cast<std::uint32_t> (val));
565  }
567  {
568  float pt_val;
569  memcpy (&pt_val, pt_data + this->offset_, sizeof (float));
570  return (pt_val > static_cast<float> (val)) - (pt_val < static_cast<float> (val));
571  }
573  {
574  double pt_val;
575  memcpy (&pt_val, pt_data + this->offset_, sizeof (double));
576  return (pt_val > val) - (pt_val < val);
577  }
578  default :
579  PCL_WARN ("[pcl::PointDataAtOffset::compare] unknown data_type!\n");
580  return (0);
581  }
582 }
583 
584 //////////////////////////////////////////////////////////////////////////
585 //////////////////////////////////////////////////////////////////////////
586 //////////////////////////////////////////////////////////////////////////
587 template <typename PointT> void
589 {
590  if (!comparison->isCapable ())
591  capable_ = false;
592  comparisons_.push_back (comparison);
593 }
594 
595 //////////////////////////////////////////////////////////////////////////
596 template <typename PointT> void
598 {
599  if (!condition->isCapable ())
600  capable_ = false;
601  conditions_.push_back (condition);
602 }
603 
604 //////////////////////////////////////////////////////////////////////////
605 //////////////////////////////////////////////////////////////////////////
606 //////////////////////////////////////////////////////////////////////////
607 template <typename PointT> bool
609 {
610  for (std::size_t i = 0; i < comparisons_.size (); ++i)
611  if (!comparisons_[i]->evaluate (point))
612  return (false);
613 
614  for (std::size_t i = 0; i < conditions_.size (); ++i)
615  if (!conditions_[i]->evaluate (point))
616  return (false);
617 
618  return (true);
619 }
620 
621 //////////////////////////////////////////////////////////////////////////
622 //////////////////////////////////////////////////////////////////////////
623 //////////////////////////////////////////////////////////////////////////
624 template <typename PointT> bool
626 {
627  if (comparisons_.empty () && conditions_.empty ())
628  return (true);
629  for (std::size_t i = 0; i < comparisons_.size (); ++i)
630  if (comparisons_[i]->evaluate(point))
631  return (true);
632 
633  for (std::size_t i = 0; i < conditions_.size (); ++i)
634  if (conditions_[i]->evaluate (point))
635  return (true);
636 
637  return (false);
638 }
639 
640 //////////////////////////////////////////////////////////////////////////
641 //////////////////////////////////////////////////////////////////////////
642 //////////////////////////////////////////////////////////////////////////
643 template <typename PointT> void
645 {
646  condition_ = condition;
647  capable_ = condition_->isCapable ();
648 }
649 
650 //////////////////////////////////////////////////////////////////////////
651 template <typename PointT> void
653 {
654  if (capable_ == false)
655  {
656  PCL_WARN ("[pcl::%s::applyFilter] not capable!\n", getClassName ().c_str ());
657  return;
658  }
659  // Has the input dataset been set already?
660  if (!input_)
661  {
662  PCL_WARN ("[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ());
663  return;
664  }
665 
666  if (condition_.get () == nullptr)
667  {
668  PCL_WARN ("[pcl::%s::applyFilter] No filtering condition given!\n", getClassName ().c_str ());
669  return;
670  }
671 
672  // Copy the header (and thus the frame_id) + allocate enough space for points
673  output.header = input_->header;
674  if (!keep_organized_)
675  {
676  output.height = 1; // filtering breaks the organized structure
677  output.is_dense = true;
678  }
679  else
680  {
681  output.height = this->input_->height;
682  output.width = this->input_->width;
683  output.is_dense = this->input_->is_dense;
684  }
685  output.resize (input_->size ());
686  removed_indices_->resize (input_->size ());
687 
688  int nr_removed_p = 0;
689 
690  if (!keep_organized_)
691  {
692  int nr_p = 0;
693  for (std::size_t index: (*Filter<PointT>::indices_))
694  {
695 
696  const PointT& point = (*input_)[index];
697  // Check if the point is invalid
698  if (!std::isfinite (point.x)
699  || !std::isfinite (point.y)
700  || !std::isfinite (point.z))
701  {
702  if (extract_removed_indices_)
703  {
704  (*removed_indices_)[nr_removed_p] = index;
705  nr_removed_p++;
706  }
707  continue;
708  }
709 
710  if (condition_->evaluate (point))
711  {
712  copyPoint (point, output[nr_p]);
713  nr_p++;
714  }
715  else
716  {
717  if (extract_removed_indices_)
718  {
719  (*removed_indices_)[nr_removed_p] = index;
720  nr_removed_p++;
721  }
722  }
723  }
724 
725  output.width = nr_p;
726  output.resize (nr_p);
727  }
728  else
729  {
731  std::sort (indices.begin (), indices.end ()); //TODO: is this necessary or can we assume the indices to be sorted?
732  bool removed_p = false;
733  std::size_t ci = 0;
734  for (std::size_t cp = 0; cp < input_->size (); ++cp)
735  {
736  if (cp == static_cast<std::size_t> (indices[ci]))
737  {
738  if (ci < indices.size () - 1)
739  {
740  ci++;
741  if (cp == static_cast<std::size_t> (indices[ci])) //check whether the next index will have the same value. TODO: necessary?
742  continue;
743  }
744 
745  // copy all the fields
746  copyPoint ((*input_)[cp], output[cp]);
747 
748  if (!condition_->evaluate ((*input_)[cp]))
749  {
750  output[cp].getVector4fMap ().setConstant (user_filter_value_);
751  removed_p = true;
752 
753  if (extract_removed_indices_)
754  {
755  (*removed_indices_)[nr_removed_p] = static_cast<int> (cp);
756  nr_removed_p++;
757  }
758  }
759  }
760  else
761  {
762  output[cp].getVector4fMap ().setConstant (user_filter_value_);
763  removed_p = true;
764  //as for !keep_organized_: removed points due to setIndices are not considered as removed_indices_
765  }
766  }
767 
768  if (removed_p && !std::isfinite (user_filter_value_))
769  output.is_dense = false;
770  }
771  removed_indices_->resize (nr_removed_p);
772 }
773 
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>;
784 
785 #endif
pcl::ConditionBase::Ptr
shared_ptr< ConditionBase< PointT > > Ptr
Definition: conditional_removal.h:453
pcl::PointDataAtOffset
A datatype that enables type-correct comparisons.
Definition: conditional_removal.h:61
pcl::PackedHSIComparison::H
@ H
Definition: conditional_removal.h:269
pcl::PointCloud::height
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:394
pcl::PackedHSIComparison::S
@ S
Definition: conditional_removal.h:270
pcl::PackedHSIComparison
A packed HSI specialization of the comparison object.
Definition: conditional_removal.h:241
pcl::ConditionalRemoval::ConditionBasePtr
typename ConditionBase::Ptr ConditionBasePtr
Definition: conditional_removal.h:605
pcl::ComparisonOps::LE
@ LE
Definition: conditional_removal.h:54
pcl::ComparisonBase
The (abstract) base class for the comparison object.
Definition: conditional_removal.h:89
pcl::PackedHSIComparison::I
@ I
Definition: conditional_removal.h:271
pcl::ComparisonOps::GE
@ GE
Definition: conditional_removal.h:54
pcl::ComparisonBase::capable_
bool capable_
True if capable.
Definition: conditional_removal.h:114
pcl::PCLPointField::INT8
@ INT8
Definition: PCLPointField.h:20
pcl::ComparisonOps::LT
@ LT
Definition: conditional_removal.h:54
pcl::ComparisonBase::op_
ComparisonOps::CompareOp op_
The comparison operator type.
Definition: conditional_removal.h:123
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: distances.h:55
pcl::PCLPointField::INT32
@ INT32
Definition: PCLPointField.h:24
pcl::TfQuadraticXYZComparison::setComparisonVector
void setComparisonVector(const Eigen::Vector3f &vector)
set the vector "v" of the comparison "p'Ap + 2v'p + c [OP] 0".
Definition: conditional_removal.h:368
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:628
pcl::PCLPointField::FLOAT32
@ FLOAT32
Definition: PCLPointField.h:26
pcl::TfQuadraticXYZComparison::evaluate
bool evaluate(const PointT &point) const override
Determine the result of this comparison.
Definition: conditional_removal.hpp:490
pcl::PCLPointField::UINT32
@ UINT32
Definition: PCLPointField.h:25
pcl::ComparisonOps::CompareOp
CompareOp
The kind of comparison operations that are possible within a comparison object.
Definition: conditional_removal.h:52
pcl::PointDataAtOffset::compare
int compare(const PointT &p, const double &val)
Compare function.
Definition: conditional_removal.hpp:520
pcl::PCLPointField::UINT8
@ UINT8
Definition: PCLPointField.h:21
pcl::ConditionBase::addCondition
void addCondition(Ptr condition)
Add a nested condition to this condition.
Definition: conditional_removal.hpp:597
pcl::PointCloud::width
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:392
pcl::PackedRGBComparison
A packed rgb specialization of the comparison object.
Definition: conditional_removal.h:194
pcl::ConditionBase::addComparison
void addComparison(ComparisonBaseConstPtr comparison)
Add a new comparison.
Definition: conditional_removal.hpp:588
pcl::copyPoint
void copyPoint(const PointInT &point_in, PointOutT &point_out)
Copy the fields of a source point into a target point.
Definition: copy_point.hpp:137
pcl::ConditionalRemoval::setCondition
void setCondition(ConditionBasePtr condition)
Set the condition that the filter will use.
Definition: conditional_removal.hpp:644
pcl::PCLPointField::INT16
@ INT16
Definition: PCLPointField.h:22
pcl::ComparisonOps::EQ
@ EQ
Definition: conditional_removal.h:54
pcl::ConditionAnd::evaluate
bool evaluate(const PointT &point) const override
Determine if a point meets this condition.
Definition: conditional_removal.hpp:608
pcl::FieldComparison::evaluate
bool evaluate(const PointT &point) const override
Determine the result of this comparison.
Definition: conditional_removal.hpp:98
pcl::PCLPointField::UINT16
@ UINT16
Definition: PCLPointField.h:23
M_PI
#define M_PI
Definition: pcl_macros.h:201
pcl::TfQuadraticXYZComparison::transformComparison
void transformComparison(const Eigen::Matrix4f &transform)
transform the coordinate system of the comparison.
Definition: conditional_removal.h:401
pcl::PackedRGBComparison::component_offset_
std::uint32_t component_offset_
The offset of the component.
Definition: conditional_removal.h:225
pcl::ComparisonBase::field_name_
std::string field_name_
Field name to compare data on.
Definition: conditional_removal.h:117
pcl::PCLPointField::FLOAT64
@ FLOAT64
Definition: PCLPointField.h:27
pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison
TfQuadraticXYZComparison()
Constructor.
Definition: conditional_removal.hpp:369
pcl::FieldComparison::~FieldComparison
~FieldComparison()
Destructor.
Definition: conditional_removal.hpp:91
pcl::PointCloud::is_dense
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
Definition: point_cloud.h:397
pcl::PointCloud::resize
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition: point_cloud.h:456
pcl::Filter
Filter represents the base filter class.
Definition: filter.h:80
pcl::PointCloud::header
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:386
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:131
pcl::TfQuadraticXYZComparison::comp_vect_
Eigen::Vector4f comp_vect_
Definition: conditional_removal.h:434
pcl::FieldComparison::point_data_
PointDataAtOffset< PointT > * point_data_
The point data to compare.
Definition: conditional_removal.h:182
pcl::ConditionalRemoval::applyFilter
void applyFilter(PointCloud &output) override
Filter a Point Cloud.
Definition: conditional_removal.hpp:652
pcl::PackedRGBComparison::evaluate
bool evaluate(const PointT &point) const override
Determine the result of this comparison.
Definition: conditional_removal.hpp:193
pcl::TfQuadraticXYZComparison::comp_matr_
Eigen::Matrix4f comp_matr_
Definition: conditional_removal.h:433
pcl::PackedHSIComparison::evaluate
bool evaluate(const PointT &point) const override
Determine the result of this comparison.
Definition: conditional_removal.hpp:289
pcl::PackedHSIComparison::rgb_offset_
std::uint32_t rgb_offset_
The offset of the component.
Definition: conditional_removal.h:285
pcl::ComparisonOps::GT
@ GT
Definition: conditional_removal.h:54
pcl::ConditionOr::evaluate
bool evaluate(const PointT &point) const override
Determine if a point meets this condition.
Definition: conditional_removal.hpp:625
pcl::PackedHSIComparison::component_id_
ComponentId component_id_
The ID of the component.
Definition: conditional_removal.h:279
pcl::TfQuadraticXYZComparison::setComparisonMatrix
void setComparisonMatrix(const Eigen::Matrix3f &matrix)
set the matrix "A" of the comparison "p'Ap + 2v'p + c [OP] 0".
Definition: conditional_removal.h:347
pcl::ConditionBase::ComparisonBaseConstPtr
typename ComparisonBase::ConstPtr ComparisonBaseConstPtr
Definition: conditional_removal.h:451
pcl::FieldComparison
The field-based specialization of the comparison object.
Definition: conditional_removal.h:129