Point Cloud Library (PCL)  1.9.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  std::vector<pcl::PCLPointField> point_fields;
59  // Use a dummy cloud to get the field types in a clever way
60  PointCloud<PointT> dummyCloud;
61  pcl::getFields (dummyCloud, point_fields);
62 
63  // Find field_name
64  if (point_fields.empty ())
65  {
66  PCL_WARN ("[pcl::FieldComparison::FieldComparison] no fields found!\n");
67  capable_ = false;
68  return;
69  }
70 
71  // Get the field index
72  size_t d;
73  for (d = 0; d < point_fields.size (); ++d)
74  {
75  if (point_fields[d].name == field_name)
76  break;
77  }
78 
79  if (d == point_fields.size ())
80  {
81  PCL_WARN ("[pcl::FieldComparison::FieldComparison] field not found!\n");
82  capable_ = false;
83  return;
84  }
85  uint8_t datatype = point_fields[d].datatype;
86  uint32_t offset = point_fields[d].offset;
87 
88  point_data_ = new PointDataAtOffset<PointT>(datatype, offset);
89  capable_ = true;
90 }
91 
92 //////////////////////////////////////////////////////////////////////////
93 template <typename PointT>
95 {
96  if (point_data_ != nullptr)
97  {
98  delete point_data_;
99  point_data_ = nullptr;
100  }
101 }
102 
103 //////////////////////////////////////////////////////////////////////////
104 template <typename PointT> bool
106 {
107  if (!this->capable_)
108  {
109  PCL_WARN ("[pcl::FieldComparison::evaluate] invalid comparison!\n");
110  return (false);
111  }
112 
113  // if p(data) > val then compare_result = 1
114  // if p(data) == val then compare_result = 0
115  // if p(data) < ival then compare_result = -1
116  int compare_result = point_data_->compare (point, compare_val_);
117 
118  switch (this->op_)
119  {
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);
130  default:
131  PCL_WARN ("[pcl::FieldComparison::evaluate] unrecognized op_!\n");
132  return (false);
133  }
134 }
135 
136 //////////////////////////////////////////////////////////////////////////
137 //////////////////////////////////////////////////////////////////////////
138 //////////////////////////////////////////////////////////////////////////
139 template <typename PointT>
141  const std::string &component_name, ComparisonOps::CompareOp op, double compare_val) :
142  component_name_ (component_name), component_offset_ (), compare_val_ (compare_val)
143 {
144  // get all the fields
145  std::vector<pcl::PCLPointField> point_fields;
146  // Use a dummy cloud to get the field types in a clever way
147  PointCloud<PointT> dummyCloud;
148  pcl::getFields (dummyCloud, point_fields);
149 
150  // Locate the "rgb" field
151  size_t d;
152  for (d = 0; d < point_fields.size (); ++d)
153  {
154  if (point_fields[d].name == "rgb" || point_fields[d].name == "rgba")
155  break;
156  }
157  if (d == point_fields.size ())
158  {
159  PCL_WARN ("[pcl::PackedRGBComparison::PackedRGBComparison] rgb field not found!\n");
160  capable_ = false;
161  return;
162  }
163 
164  // Verify the datatype
165  uint8_t datatype = point_fields[d].datatype;
166  if (datatype != pcl::PCLPointField::FLOAT32 &&
167  datatype != pcl::PCLPointField::UINT32 &&
168  datatype != pcl::PCLPointField::INT32)
169  {
170  PCL_WARN ("[pcl::PackedRGBComparison::PackedRGBComparison] has unusable type!\n");
171  capable_ = false;
172  return;
173  }
174 
175  // Verify the component name
176  if (component_name == "r")
177  {
178  component_offset_ = point_fields[d].offset + 2;
179  }
180  else if (component_name == "g")
181  {
182  component_offset_ = point_fields[d].offset + 1;
183  }
184  else if (component_name == "b")
185  {
186  component_offset_ = point_fields[d].offset;
187  }
188  else
189  {
190  PCL_WARN ("[pcl::PackedRGBComparison::PackedRGBComparison] unrecognized component name!\n");
191  capable_ = false;
192  return;
193  }
194 
195  // save the rest of the context
196  capable_ = true;
197  op_ = op;
198 }
199 
200 
201 //////////////////////////////////////////////////////////////////////////
202 template <typename PointT> bool
204 {
205  // extract the component value
206  const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&point);
207  uint8_t my_val = *(pt_data + component_offset_);
208 
209  // now do the comparison
210  switch (this->op_)
211  {
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_);
222  default:
223  PCL_WARN ("[pcl::PackedRGBComparison::evaluate] unrecognized op_!\n");
224  return (false);
225  }
226 }
227 
228 //////////////////////////////////////////////////////////////////////////
229 //////////////////////////////////////////////////////////////////////////
230 //////////////////////////////////////////////////////////////////////////
231 template <typename PointT>
233  const std::string &component_name, ComparisonOps::CompareOp op, double compare_val) :
234  component_name_ (component_name), component_id_ (), compare_val_ (compare_val), rgb_offset_ ()
235 {
236  // Get all the fields
237  std::vector<pcl::PCLPointField> point_fields;
238  // Use a dummy cloud to get the field types in a clever way
239  PointCloud<PointT> dummyCloud;
240  pcl::getFields (dummyCloud, point_fields);
241 
242  // Locate the "rgb" field
243  size_t d;
244  for (d = 0; d < point_fields.size (); ++d)
245  if (point_fields[d].name == "rgb" || point_fields[d].name == "rgba")
246  break;
247  if (d == point_fields.size ())
248  {
249  PCL_WARN ("[pcl::PackedHSIComparison::PackedHSIComparison] rgb field not found!\n");
250  capable_ = false;
251  return;
252  }
253 
254  // Verify the datatype
255  uint8_t datatype = point_fields[d].datatype;
256  if (datatype != pcl::PCLPointField::FLOAT32 &&
257  datatype != pcl::PCLPointField::UINT32 &&
258  datatype != pcl::PCLPointField::INT32)
259  {
260  PCL_WARN ("[pcl::PackedHSIComparison::PackedHSIComparison] has unusable type!\n");
261  capable_ = false;
262  return;
263  }
264 
265  // verify the offset
266  uint32_t offset = point_fields[d].offset;
267  if (offset % 4 != 0)
268  {
269  PCL_WARN ("[pcl::PackedHSIComparison::PackedHSIComparison] rgb field is not 32 bit aligned!\n");
270  capable_ = false;
271  return;
272  }
273  rgb_offset_ = point_fields[d].offset;
274 
275  // verify the component name
276  if (component_name == "h" )
277  {
278  component_id_ = H;
279  }
280  else if (component_name == "s")
281  {
282  component_id_ = S;
283  }
284  else if (component_name == "i")
285  {
286  component_id_ = I;
287  }
288  else
289  {
290  PCL_WARN ("[pcl::PackedHSIComparison::PackedHSIComparison] unrecognized component name!\n");
291  capable_ = false;
292  return;
293  }
294 
295  // Save the context
296  capable_ = true;
297  op_ = op;
298 }
299 
300 //////////////////////////////////////////////////////////////////////////
301 template <typename PointT> bool
303 {
304  // Since this is a const function, we can't make these data members because we change them here
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;
312 
313  // We know that rgb data is 32 bit aligned (verified in the ctor) so...
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;
317 
318  if (rgb_val_ != new_rgb_val)
319  { // avoid having to redo this calc, if possible
320  rgb_val_ = new_rgb_val;
321  // extract r,g,b
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_);
325 
326  // definitions taken from http://en.wikipedia.org/wiki/HSL_and_HSI
327  float hx = (2.0f * r_ - g_ - b_) / 4.0f; // hue x component -127 to 127
328  float hy = static_cast<float> (g_ - b_) * 111.0f / 255.0f; // hue y component -111 to 111
329  h_ = static_cast<int8_t> (atan2(hy, hx) * 128.0f / M_PI);
330 
331  int32_t i = (r_+g_+b_)/3; // 0 to 255
332  i_ = static_cast<uint8_t> (i);
333 
334  int32_t m; // min(r,g,b)
335  m = (r_ < g_) ? r_ : g_;
336  m = (m < b_) ? m : b_;
337 
338  s_ = static_cast<uint8_t> ((i == 0) ? 0 : 255 - (m * 255) / i); // saturation 0 to 255
339  }
340 
341  float my_val = 0;
342 
343  switch (component_id_)
344  {
345  case H:
346  my_val = static_cast <float> (h_);
347  break;
348  case S:
349  my_val = static_cast <float> (s_);
350  break;
351  case I:
352  my_val = static_cast <float> (i_);
353  break;
354  default:
355  assert (false);
356  }
357 
358  // now do the comparison
359  switch (this->op_)
360  {
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_);
371  default:
372  PCL_WARN ("[pcl::PackedHSIComparison::evaluate] unrecognized op_!\n");
373  return (false);
374  }
375 }
376 
377 
378 //////////////////////////////////////////////////////////////////////////
379 //////////////////////////////////////////////////////////////////////////
380 //////////////////////////////////////////////////////////////////////////
381 template<typename PointT>
383  comp_scalar_ (0.0)
384 {
385  // get all the fields
386  std::vector<pcl::PCLPointField> point_fields;
387  // Use a dummy cloud to get the field types in a clever way
388  PointCloud<PointT> dummyCloud;
389  pcl::getFields (dummyCloud, point_fields);
390 
391  // Locate the "x" field
392  size_t dX;
393  for (dX = 0; dX < point_fields.size (); ++dX)
394  {
395  if (point_fields[dX].name == "x")
396  break;
397  }
398  if (dX == point_fields.size ())
399  {
400  PCL_WARN ("[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] x field not found!\n");
401  capable_ = false;
402  return;
403  }
404 
405  // Locate the "y" field
406  size_t dY;
407  for (dY = 0; dY < point_fields.size (); ++dY)
408  {
409  if (point_fields[dY].name == "y")
410  break;
411  }
412  if (dY == point_fields.size ())
413  {
414  PCL_WARN ("[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] y field not found!\n");
415  capable_ = false;
416  return;
417  }
418 
419  // Locate the "z" field
420  size_t dZ;
421  for (dZ = 0; dZ < point_fields.size (); ++dZ)
422  {
423  if (point_fields[dZ].name == "z")
424  break;
425  }
426  if (dZ == point_fields.size ())
427  {
428  PCL_WARN ("[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] z field not found!\n");
429  capable_ = false;
430  return;
431  }
432 
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;
434  comp_vect_ << 0.0, 0.0, 0.0, 1.0;
435  tf_comp_matr_ = comp_matr_;
436  tf_comp_vect_ = comp_vect_;
438  capable_ = true;
439 }
440 
441 //////////////////////////////////////////////////////////////////////////
442 template<typename PointT>
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_scalar_ (comparison_scalar)
449 {
450  // get all the fields
451  std::vector<pcl::PCLPointField> point_fields;
452  // Use a dummy cloud to get the field types in a clever way
453  PointCloud<PointT> dummyCloud;
454  pcl::getFields (dummyCloud, point_fields);
455 
456  // Locate the "x" field
457  size_t dX;
458  for (dX = 0; dX < point_fields.size (); ++dX)
459  {
460  if (point_fields[dX].name == "x")
461  break;
462  }
463  if (dX == point_fields.size ())
464  {
465  PCL_WARN ("[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] x field not found!\n");
466  capable_ = false;
467  return;
468  }
469 
470  // Locate the "y" field
471  size_t dY;
472  for (dY = 0; dY < point_fields.size (); ++dY)
473  {
474  if (point_fields[dY].name == "y")
475  break;
476  }
477  if (dY == point_fields.size ())
478  {
479  PCL_WARN ("[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] y field not found!\n");
480  capable_ = false;
481  return;
482  }
483 
484  // Locate the "z" field
485  size_t dZ;
486  for (dZ = 0; dZ < point_fields.size (); ++dZ)
487  {
488  if (point_fields[dZ].name == "z")
489  break;
490  }
491  if (dZ == point_fields.size ())
492  {
493  PCL_WARN ("[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] z field not found!\n");
494  capable_ = false;
495  return;
496  }
497 
498  capable_ = true;
499  op_ = op;
500  setComparisonMatrix (comparison_matrix);
501  setComparisonVector (comparison_vector);
502  if (!comparison_transform.matrix ().isIdentity ())
503  transformComparison (comparison_transform);
504 }
505 
506 //////////////////////////////////////////////////////////////////////////
507 template<typename PointT>
508 bool
510 {
511  Eigen::Vector4f pointAffine;
512  pointAffine << point.x, point.y, point.z, 1;
513 
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;
515 
516  // now do the comparison
517  switch (this->op_)
518  {
520  return (myVal > 0);
522  return (myVal >= 0);
524  return (myVal < 0);
526  return (myVal <= 0);
528  return (myVal == 0);
529  default:
530  PCL_WARN ("[pcl::TfQuadraticXYZComparison::evaluate] unrecognized op_!\n");
531  return (false);
532  }
533 }
534 
535 //////////////////////////////////////////////////////////////////////////
536 //////////////////////////////////////////////////////////////////////////
537 //////////////////////////////////////////////////////////////////////////
538 template <typename PointT> int
539 pcl::PointDataAtOffset<PointT>::compare (const PointT& p, const double& val)
540 {
541  // if p(data) > val return 1
542  // if p(data) == val return 0
543  // if p(data) < val return -1
544 
545  const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&p);
546 
547  switch (datatype_)
548  {
550  {
551  int8_t pt_val;
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));
554  }
556  {
557  uint8_t pt_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));
560  }
562  {
563  int16_t pt_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));
566  }
568  {
569  uint16_t pt_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));
572  }
574  {
575  int32_t pt_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));
578  }
580  {
581  uint32_t pt_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));
584  }
586  {
587  float pt_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));
590  }
592  {
593  double pt_val;
594  memcpy (&pt_val, pt_data + this->offset_, sizeof (double));
595  return (pt_val > val) - (pt_val < val);
596  }
597  default :
598  PCL_WARN ("[pcl::PointDataAtOffset::compare] unknown data_type!\n");
599  return (0);
600  }
601 }
602 
603 //////////////////////////////////////////////////////////////////////////
604 //////////////////////////////////////////////////////////////////////////
605 //////////////////////////////////////////////////////////////////////////
606 template <typename PointT> void
608 {
609  if (!comparison->isCapable ())
610  capable_ = false;
611  comparisons_.push_back (comparison);
612 }
613 
614 //////////////////////////////////////////////////////////////////////////
615 template <typename PointT> void
617 {
618  if (!condition->isCapable ())
619  capable_ = false;
620  conditions_.push_back (condition);
621 }
622 
623 //////////////////////////////////////////////////////////////////////////
624 //////////////////////////////////////////////////////////////////////////
625 //////////////////////////////////////////////////////////////////////////
626 template <typename PointT> bool
628 {
629  for (size_t i = 0; i < comparisons_.size (); ++i)
630  if (!comparisons_[i]->evaluate (point))
631  return (false);
632 
633  for (size_t i = 0; i < conditions_.size (); ++i)
634  if (!conditions_[i]->evaluate (point))
635  return (false);
636 
637  return (true);
638 }
639 
640 //////////////////////////////////////////////////////////////////////////
641 //////////////////////////////////////////////////////////////////////////
642 //////////////////////////////////////////////////////////////////////////
643 template <typename PointT> bool
645 {
646  if (comparisons_.empty () && conditions_.empty ())
647  return (true);
648  for (size_t i = 0; i < comparisons_.size (); ++i)
649  if (comparisons_[i]->evaluate(point))
650  return (true);
651 
652  for (size_t i = 0; i < conditions_.size (); ++i)
653  if (conditions_[i]->evaluate (point))
654  return (true);
655 
656  return (false);
657 }
658 
659 //////////////////////////////////////////////////////////////////////////
660 //////////////////////////////////////////////////////////////////////////
661 //////////////////////////////////////////////////////////////////////////
662 template <typename PointT> void
664 {
665  condition_ = condition;
666  capable_ = condition_->isCapable ();
667 }
668 
669 //////////////////////////////////////////////////////////////////////////
670 template <typename PointT> void
672 {
673  if (capable_ == false)
674  {
675  PCL_WARN ("[pcl::%s::applyFilter] not capable!\n", getClassName ().c_str ());
676  return;
677  }
678  // Has the input dataset been set already?
679  if (!input_)
680  {
681  PCL_WARN ("[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ());
682  return;
683  }
684 
685  if (condition_.get () == nullptr)
686  {
687  PCL_WARN ("[pcl::%s::applyFilter] No filtering condition given!\n", getClassName ().c_str ());
688  return;
689  }
690 
691  // Copy the header (and thus the frame_id) + allocate enough space for points
692  output.header = input_->header;
693  if (!keep_organized_)
694  {
695  output.height = 1; // filtering breaks the organized structure
696  output.is_dense = true;
697  }
698  else
699  {
700  output.height = this->input_->height;
701  output.width = this->input_->width;
702  output.is_dense = this->input_->is_dense;
703  }
704  output.points.resize (input_->points.size ());
705  removed_indices_->resize (input_->points.size ());
706 
707  int nr_p = 0;
708  int nr_removed_p = 0;
709 
710  if (!keep_organized_)
711  {
712  for (size_t cp = 0; cp < Filter<PointT>::indices_->size (); ++cp)
713  {
714  // Check if the point is invalid
715  if (!std::isfinite (input_->points[(*Filter < PointT > ::indices_)[cp]].x)
716  || !std::isfinite (input_->points[(*Filter < PointT > ::indices_)[cp]].y)
717  || !std::isfinite (input_->points[(*Filter < PointT > ::indices_)[cp]].z))
718  {
719  if (extract_removed_indices_)
720  {
721  (*removed_indices_)[nr_removed_p] = (*Filter<PointT>::indices_)[cp];
722  nr_removed_p++;
723  }
724  continue;
725  }
726 
727  if (condition_->evaluate (input_->points[(*Filter < PointT > ::indices_)[cp]]))
728  {
729  copyPoint (input_->points[(*Filter < PointT > ::indices_)[cp]], output.points[nr_p]);
730  nr_p++;
731  }
732  else
733  {
734  if (extract_removed_indices_)
735  {
736  (*removed_indices_)[nr_removed_p] = (*Filter<PointT>::indices_)[cp];
737  nr_removed_p++;
738  }
739  }
740  }
741 
742  output.width = nr_p;
743  output.points.resize (nr_p);
744  }
745  else
746  {
747  std::vector<int> indices = *Filter<PointT>::indices_;
748  std::sort (indices.begin (), indices.end ()); //TODO: is this necessary or can we assume the indices to be sorted?
749  bool removed_p = false;
750  size_t ci = 0;
751  for (size_t cp = 0; cp < input_->points.size (); ++cp)
752  {
753  if (cp == static_cast<size_t> (indices[ci]))
754  {
755  if (ci < indices.size () - 1)
756  {
757  ci++;
758  if (cp == static_cast<size_t> (indices[ci])) //check whether the next index will have the same value. TODO: necessary?
759  continue;
760  }
761 
762  // copy all the fields
763  copyPoint (input_->points[cp], output.points[cp]);
764 
765  if (!condition_->evaluate (input_->points[cp]))
766  {
767  output.points[cp].getVector4fMap ().setConstant (user_filter_value_);
768  removed_p = true;
769 
770  if (extract_removed_indices_)
771  {
772  (*removed_indices_)[nr_removed_p] = static_cast<int> (cp);
773  nr_removed_p++;
774  }
775  }
776  }
777  else
778  {
779  output.points[cp].getVector4fMap ().setConstant (user_filter_value_);
780  removed_p = true;
781  //as for !keep_organized_: removed points due to setIndices are not considered as removed_indices_
782  }
783  }
784 
785  if (removed_p && !std::isfinite (user_filter_value_))
786  output.is_dense = false;
787  }
788  removed_indices_->resize (nr_removed_p);
789 }
790 
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>;
801 
802 #endif
void setComparisonMatrix(const Eigen::Matrix3f &matrix)
set the matrix "A" of the comparison "p&#39;Ap + 2v&#39;p + c [OP] 0".
double compare_val_
All types (that we care about) can be represented as a double.
ComparisonBase::ConstPtr ComparisonBaseConstPtr
double compare_val_
All types (that we care about) can be represented as a double.
uint32_t offset_
The data offset.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:409
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.
std::string component_name_
The name of the component.
bool capable_
True if capable.
bool evaluate(const PointT &point) const override
Determine the result of this comparison.
A datatype that enables type-correct comparisons.
void setCondition(ConditionBasePtr condition)
Set the condition that the filter will use.
The (abstract) base class for the comparison object.
bool evaluate(const PointT &point) const override
Determine if a point meets this condition.
void setComparisonVector(const Eigen::Vector3f &vector)
set the vector "v" of the comparison "p&#39;Ap + 2v&#39;p + c [OP] 0".
bool evaluate(const PointT &point) const override
Determine if a point meets this condition.
ComparisonOps::CompareOp op_
The comparison operator type.
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:414
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:412
Filter represents the base filter class.
Definition: filter.h:83
void applyFilter(PointCloud &output) override
Filter a Point Cloud.
int compare(const PointT &p, const double &val)
Compare function.
The field-based specialization of the comparison object.
A packed rgb specialization of the comparison object.
bool evaluate(const PointT &point) const override
Determine the result of this comparison.
uint32_t component_offset_
The offset of the component.
bool evaluate(const PointT &point) const override
Determine the result of this comparison.
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.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:406
void addCondition(Ptr condition)
Add a nested condition to this condition.
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)...
Definition: point_cloud.h:417
bool evaluate(const PointT &point) const override
Determine the result of this comparison.
double compare_val_
All types (that we care about) can be represented as a double.
ConditionBase::Ptr ConditionBasePtr
A packed HSI specialization of the comparison object.
A point structure representing Euclidean xyz coordinates, and the RGB color.
void copyPoint(const PointInT &point_in, PointOutT &point_out)
Copy the fields of a source point into a target point.
Definition: copy_point.hpp:138
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)
Definition: io.hpp:79