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  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  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  uint8_t datatype = point_fields[d].datatype;
83  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  if (point_data_ != nullptr)
94  {
95  delete point_data_;
96  point_data_ = nullptr;
97  }
98 }
99 
100 //////////////////////////////////////////////////////////////////////////
101 template <typename PointT> bool
103 {
104  if (!this->capable_)
105  {
106  PCL_WARN ("[pcl::FieldComparison::evaluate] invalid comparison!\n");
107  return (false);
108  }
109 
110  // if p(data) > val then compare_result = 1
111  // if p(data) == val then compare_result = 0
112  // if p(data) < ival then compare_result = -1
113  int compare_result = point_data_->compare (point, compare_val_);
114 
115  switch (this->op_)
116  {
118  return (compare_result > 0);
120  return (compare_result >= 0);
122  return (compare_result < 0);
124  return (compare_result <= 0);
126  return (compare_result == 0);
127  default:
128  PCL_WARN ("[pcl::FieldComparison::evaluate] unrecognized op_!\n");
129  return (false);
130  }
131 }
132 
133 //////////////////////////////////////////////////////////////////////////
134 //////////////////////////////////////////////////////////////////////////
135 //////////////////////////////////////////////////////////////////////////
136 template <typename PointT>
138  const std::string &component_name, ComparisonOps::CompareOp op, double compare_val) :
139  component_name_ (component_name), component_offset_ (), compare_val_ (compare_val)
140 {
141  // get all the fields
142  const auto point_fields = pcl::getFields<PointT> ();
143 
144  // Locate the "rgb" field
145  size_t d;
146  for (d = 0; d < point_fields.size (); ++d)
147  {
148  if (point_fields[d].name == "rgb" || point_fields[d].name == "rgba")
149  break;
150  }
151  if (d == point_fields.size ())
152  {
153  PCL_WARN ("[pcl::PackedRGBComparison::PackedRGBComparison] rgb field not found!\n");
154  capable_ = false;
155  return;
156  }
157 
158  // Verify the datatype
159  uint8_t datatype = point_fields[d].datatype;
160  if (datatype != pcl::PCLPointField::FLOAT32 &&
161  datatype != pcl::PCLPointField::UINT32 &&
162  datatype != pcl::PCLPointField::INT32)
163  {
164  PCL_WARN ("[pcl::PackedRGBComparison::PackedRGBComparison] has unusable type!\n");
165  capable_ = false;
166  return;
167  }
168 
169  // Verify the component name
170  if (component_name == "r")
171  {
172  component_offset_ = point_fields[d].offset + 2;
173  }
174  else if (component_name == "g")
175  {
176  component_offset_ = point_fields[d].offset + 1;
177  }
178  else if (component_name == "b")
179  {
180  component_offset_ = point_fields[d].offset;
181  }
182  else
183  {
184  PCL_WARN ("[pcl::PackedRGBComparison::PackedRGBComparison] unrecognized component name!\n");
185  capable_ = false;
186  return;
187  }
188 
189  // save the rest of the context
190  capable_ = true;
191  op_ = op;
192 }
193 
194 
195 //////////////////////////////////////////////////////////////////////////
196 template <typename PointT> bool
198 {
199  // extract the component value
200  const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&point);
201  uint8_t my_val = *(pt_data + component_offset_);
202 
203  // now do the comparison
204  switch (this->op_)
205  {
207  return (my_val > this->compare_val_);
209  return (my_val >= this->compare_val_);
211  return (my_val < this->compare_val_);
213  return (my_val <= this->compare_val_);
215  return (my_val == this->compare_val_);
216  default:
217  PCL_WARN ("[pcl::PackedRGBComparison::evaluate] unrecognized op_!\n");
218  return (false);
219  }
220 }
221 
222 //////////////////////////////////////////////////////////////////////////
223 //////////////////////////////////////////////////////////////////////////
224 //////////////////////////////////////////////////////////////////////////
225 template <typename PointT>
227  const std::string &component_name, ComparisonOps::CompareOp op, double compare_val) :
228  component_name_ (component_name), component_id_ (), compare_val_ (compare_val), rgb_offset_ ()
229 {
230  // Get all the fields
231  const auto point_fields = pcl::getFields<PointT> ();
232 
233  // Locate the "rgb" field
234  size_t d;
235  for (d = 0; d < point_fields.size (); ++d)
236  if (point_fields[d].name == "rgb" || point_fields[d].name == "rgba")
237  break;
238  if (d == point_fields.size ())
239  {
240  PCL_WARN ("[pcl::PackedHSIComparison::PackedHSIComparison] rgb field not found!\n");
241  capable_ = false;
242  return;
243  }
244 
245  // Verify the datatype
246  uint8_t datatype = point_fields[d].datatype;
247  if (datatype != pcl::PCLPointField::FLOAT32 &&
248  datatype != pcl::PCLPointField::UINT32 &&
249  datatype != pcl::PCLPointField::INT32)
250  {
251  PCL_WARN ("[pcl::PackedHSIComparison::PackedHSIComparison] has unusable type!\n");
252  capable_ = false;
253  return;
254  }
255 
256  // verify the offset
257  uint32_t offset = point_fields[d].offset;
258  if (offset % 4 != 0)
259  {
260  PCL_WARN ("[pcl::PackedHSIComparison::PackedHSIComparison] rgb field is not 32 bit aligned!\n");
261  capable_ = false;
262  return;
263  }
264  rgb_offset_ = point_fields[d].offset;
265 
266  // verify the component name
267  if (component_name == "h" )
268  {
269  component_id_ = H;
270  }
271  else if (component_name == "s")
272  {
273  component_id_ = S;
274  }
275  else if (component_name == "i")
276  {
277  component_id_ = I;
278  }
279  else
280  {
281  PCL_WARN ("[pcl::PackedHSIComparison::PackedHSIComparison] unrecognized component name!\n");
282  capable_ = false;
283  return;
284  }
285 
286  // Save the context
287  capable_ = true;
288  op_ = op;
289 }
290 
291 //////////////////////////////////////////////////////////////////////////
292 template <typename PointT> bool
294 {
295  // Since this is a const function, we can't make these data members because we change them here
296  static uint32_t rgb_val_ = 0;
297  static uint8_t r_ = 0;
298  static uint8_t g_ = 0;
299  static uint8_t b_ = 0;
300  static int8_t h_ = 0;
301  static uint8_t s_ = 0;
302  static uint8_t i_ = 0;
303 
304  // We know that rgb data is 32 bit aligned (verified in the ctor) so...
305  const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&point);
306  const uint32_t* rgb_data = reinterpret_cast<const uint32_t*> (pt_data + rgb_offset_);
307  uint32_t new_rgb_val = *rgb_data;
308 
309  if (rgb_val_ != new_rgb_val)
310  { // avoid having to redo this calc, if possible
311  rgb_val_ = new_rgb_val;
312  // extract r,g,b
313  r_ = static_cast <uint8_t> (rgb_val_ >> 16);
314  g_ = static_cast <uint8_t> (rgb_val_ >> 8);
315  b_ = static_cast <uint8_t> (rgb_val_);
316 
317  // definitions taken from http://en.wikipedia.org/wiki/HSL_and_HSI
318  float hx = (2.0f * r_ - g_ - b_) / 4.0f; // hue x component -127 to 127
319  float hy = static_cast<float> (g_ - b_) * 111.0f / 255.0f; // hue y component -111 to 111
320  h_ = static_cast<int8_t> (std::atan2(hy, hx) * 128.0f / M_PI);
321 
322  int32_t i = (r_+g_+b_)/3; // 0 to 255
323  i_ = static_cast<uint8_t> (i);
324 
325  int32_t m; // min(r,g,b)
326  m = (r_ < g_) ? r_ : g_;
327  m = (m < b_) ? m : b_;
328 
329  s_ = static_cast<uint8_t> ((i == 0) ? 0 : 255 - (m * 255) / i); // saturation 0 to 255
330  }
331 
332  float my_val = 0;
333 
334  switch (component_id_)
335  {
336  case H:
337  my_val = static_cast <float> (h_);
338  break;
339  case S:
340  my_val = static_cast <float> (s_);
341  break;
342  case I:
343  my_val = static_cast <float> (i_);
344  break;
345  default:
346  assert (false);
347  }
348 
349  // now do the comparison
350  switch (this->op_)
351  {
353  return (my_val > this->compare_val_);
355  return (my_val >= this->compare_val_);
357  return (my_val < this->compare_val_);
359  return (my_val <= this->compare_val_);
361  return (my_val == this->compare_val_);
362  default:
363  PCL_WARN ("[pcl::PackedHSIComparison::evaluate] unrecognized op_!\n");
364  return (false);
365  }
366 }
367 
368 
369 //////////////////////////////////////////////////////////////////////////
370 //////////////////////////////////////////////////////////////////////////
371 //////////////////////////////////////////////////////////////////////////
372 template<typename PointT>
374  comp_scalar_ (0.0)
375 {
376  // get all the fields
377  const auto point_fields = pcl::getFields<PointT> ();
378 
379  // Locate the "x" field
380  size_t dX;
381  for (dX = 0; dX < point_fields.size (); ++dX)
382  {
383  if (point_fields[dX].name == "x")
384  break;
385  }
386  if (dX == point_fields.size ())
387  {
388  PCL_WARN ("[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] x field not found!\n");
389  capable_ = false;
390  return;
391  }
392 
393  // Locate the "y" field
394  size_t dY;
395  for (dY = 0; dY < point_fields.size (); ++dY)
396  {
397  if (point_fields[dY].name == "y")
398  break;
399  }
400  if (dY == point_fields.size ())
401  {
402  PCL_WARN ("[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] y field not found!\n");
403  capable_ = false;
404  return;
405  }
406 
407  // Locate the "z" field
408  size_t dZ;
409  for (dZ = 0; dZ < point_fields.size (); ++dZ)
410  {
411  if (point_fields[dZ].name == "z")
412  break;
413  }
414  if (dZ == point_fields.size ())
415  {
416  PCL_WARN ("[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] z field not found!\n");
417  capable_ = false;
418  return;
419  }
420 
421  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;
422  comp_vect_ << 0.0, 0.0, 0.0, 1.0;
423  tf_comp_matr_ = comp_matr_;
424  tf_comp_vect_ = comp_vect_;
426  capable_ = true;
427 }
428 
429 //////////////////////////////////////////////////////////////////////////
430 template<typename PointT>
432  const Eigen::Matrix3f &comparison_matrix,
433  const Eigen::Vector3f &comparison_vector,
434  const float &comparison_scalar,
435  const Eigen::Affine3f &comparison_transform) :
436  comp_scalar_ (comparison_scalar)
437 {
438  // get all the fields
439  const auto point_fields = pcl::getFields<PointT> ();
440 
441  // Locate the "x" field
442  size_t dX;
443  for (dX = 0; dX < point_fields.size (); ++dX)
444  {
445  if (point_fields[dX].name == "x")
446  break;
447  }
448  if (dX == point_fields.size ())
449  {
450  PCL_WARN ("[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] x field not found!\n");
451  capable_ = false;
452  return;
453  }
454 
455  // Locate the "y" field
456  size_t dY;
457  for (dY = 0; dY < point_fields.size (); ++dY)
458  {
459  if (point_fields[dY].name == "y")
460  break;
461  }
462  if (dY == point_fields.size ())
463  {
464  PCL_WARN ("[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] y field not found!\n");
465  capable_ = false;
466  return;
467  }
468 
469  // Locate the "z" field
470  size_t dZ;
471  for (dZ = 0; dZ < point_fields.size (); ++dZ)
472  {
473  if (point_fields[dZ].name == "z")
474  break;
475  }
476  if (dZ == point_fields.size ())
477  {
478  PCL_WARN ("[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] z field not found!\n");
479  capable_ = false;
480  return;
481  }
482 
483  capable_ = true;
484  op_ = op;
485  setComparisonMatrix (comparison_matrix);
486  setComparisonVector (comparison_vector);
487  if (!comparison_transform.matrix ().isIdentity ())
488  transformComparison (comparison_transform);
489 }
490 
491 //////////////////////////////////////////////////////////////////////////
492 template<typename PointT>
493 bool
495 {
496  Eigen::Vector4f pointAffine;
497  pointAffine << point.x, point.y, point.z, 1;
498 
499  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;
500 
501  // now do the comparison
502  switch (this->op_)
503  {
505  return (myVal > 0);
507  return (myVal >= 0);
509  return (myVal < 0);
511  return (myVal <= 0);
513  return (myVal == 0);
514  default:
515  PCL_WARN ("[pcl::TfQuadraticXYZComparison::evaluate] unrecognized op_!\n");
516  return (false);
517  }
518 }
519 
520 //////////////////////////////////////////////////////////////////////////
521 //////////////////////////////////////////////////////////////////////////
522 //////////////////////////////////////////////////////////////////////////
523 template <typename PointT> int
524 pcl::PointDataAtOffset<PointT>::compare (const PointT& p, const double& val)
525 {
526  // if p(data) > val return 1
527  // if p(data) == val return 0
528  // if p(data) < val return -1
529 
530  const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&p);
531 
532  switch (datatype_)
533  {
535  {
536  int8_t pt_val;
537  memcpy (&pt_val, pt_data + this->offset_, sizeof (int8_t));
538  return (pt_val > static_cast<int8_t>(val)) - (pt_val < static_cast<int8_t> (val));
539  }
541  {
542  uint8_t pt_val;
543  memcpy (&pt_val, pt_data + this->offset_, sizeof (uint8_t));
544  return (pt_val > static_cast<uint8_t>(val)) - (pt_val < static_cast<uint8_t> (val));
545  }
547  {
548  int16_t pt_val;
549  memcpy (&pt_val, pt_data + this->offset_, sizeof (int16_t));
550  return (pt_val > static_cast<int16_t>(val)) - (pt_val < static_cast<int16_t> (val));
551  }
553  {
554  uint16_t pt_val;
555  memcpy (&pt_val, pt_data + this->offset_, sizeof (uint16_t));
556  return (pt_val > static_cast<uint16_t> (val)) - (pt_val < static_cast<uint16_t> (val));
557  }
559  {
560  int32_t pt_val;
561  memcpy (&pt_val, pt_data + this->offset_, sizeof (int32_t));
562  return (pt_val > static_cast<int32_t> (val)) - (pt_val < static_cast<int32_t> (val));
563  }
565  {
566  uint32_t pt_val;
567  memcpy (&pt_val, pt_data + this->offset_, sizeof (uint32_t));
568  return (pt_val > static_cast<uint32_t> (val)) - (pt_val < static_cast<uint32_t> (val));
569  }
571  {
572  float pt_val;
573  memcpy (&pt_val, pt_data + this->offset_, sizeof (float));
574  return (pt_val > static_cast<float> (val)) - (pt_val < static_cast<float> (val));
575  }
577  {
578  double pt_val;
579  memcpy (&pt_val, pt_data + this->offset_, sizeof (double));
580  return (pt_val > val) - (pt_val < val);
581  }
582  default :
583  PCL_WARN ("[pcl::PointDataAtOffset::compare] unknown data_type!\n");
584  return (0);
585  }
586 }
587 
588 //////////////////////////////////////////////////////////////////////////
589 //////////////////////////////////////////////////////////////////////////
590 //////////////////////////////////////////////////////////////////////////
591 template <typename PointT> void
593 {
594  if (!comparison->isCapable ())
595  capable_ = false;
596  comparisons_.push_back (comparison);
597 }
598 
599 //////////////////////////////////////////////////////////////////////////
600 template <typename PointT> void
602 {
603  if (!condition->isCapable ())
604  capable_ = false;
605  conditions_.push_back (condition);
606 }
607 
608 //////////////////////////////////////////////////////////////////////////
609 //////////////////////////////////////////////////////////////////////////
610 //////////////////////////////////////////////////////////////////////////
611 template <typename PointT> bool
613 {
614  for (size_t i = 0; i < comparisons_.size (); ++i)
615  if (!comparisons_[i]->evaluate (point))
616  return (false);
617 
618  for (size_t i = 0; i < conditions_.size (); ++i)
619  if (!conditions_[i]->evaluate (point))
620  return (false);
621 
622  return (true);
623 }
624 
625 //////////////////////////////////////////////////////////////////////////
626 //////////////////////////////////////////////////////////////////////////
627 //////////////////////////////////////////////////////////////////////////
628 template <typename PointT> bool
630 {
631  if (comparisons_.empty () && conditions_.empty ())
632  return (true);
633  for (size_t i = 0; i < comparisons_.size (); ++i)
634  if (comparisons_[i]->evaluate(point))
635  return (true);
636 
637  for (size_t i = 0; i < conditions_.size (); ++i)
638  if (conditions_[i]->evaluate (point))
639  return (true);
640 
641  return (false);
642 }
643 
644 //////////////////////////////////////////////////////////////////////////
645 //////////////////////////////////////////////////////////////////////////
646 //////////////////////////////////////////////////////////////////////////
647 template <typename PointT> void
649 {
650  condition_ = condition;
651  capable_ = condition_->isCapable ();
652 }
653 
654 //////////////////////////////////////////////////////////////////////////
655 template <typename PointT> void
657 {
658  if (capable_ == false)
659  {
660  PCL_WARN ("[pcl::%s::applyFilter] not capable!\n", getClassName ().c_str ());
661  return;
662  }
663  // Has the input dataset been set already?
664  if (!input_)
665  {
666  PCL_WARN ("[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ());
667  return;
668  }
669 
670  if (condition_.get () == nullptr)
671  {
672  PCL_WARN ("[pcl::%s::applyFilter] No filtering condition given!\n", getClassName ().c_str ());
673  return;
674  }
675 
676  // Copy the header (and thus the frame_id) + allocate enough space for points
677  output.header = input_->header;
678  if (!keep_organized_)
679  {
680  output.height = 1; // filtering breaks the organized structure
681  output.is_dense = true;
682  }
683  else
684  {
685  output.height = this->input_->height;
686  output.width = this->input_->width;
687  output.is_dense = this->input_->is_dense;
688  }
689  output.points.resize (input_->points.size ());
690  removed_indices_->resize (input_->points.size ());
691 
692  int nr_p = 0;
693  int nr_removed_p = 0;
694 
695  if (!keep_organized_)
696  {
697  for (size_t cp = 0; cp < Filter<PointT>::indices_->size (); ++cp)
698  {
699  // Check if the point is invalid
700  if (!std::isfinite (input_->points[(*Filter < PointT > ::indices_)[cp]].x)
701  || !std::isfinite (input_->points[(*Filter < PointT > ::indices_)[cp]].y)
702  || !std::isfinite (input_->points[(*Filter < PointT > ::indices_)[cp]].z))
703  {
704  if (extract_removed_indices_)
705  {
706  (*removed_indices_)[nr_removed_p] = (*Filter<PointT>::indices_)[cp];
707  nr_removed_p++;
708  }
709  continue;
710  }
711 
712  if (condition_->evaluate (input_->points[(*Filter < PointT > ::indices_)[cp]]))
713  {
714  copyPoint (input_->points[(*Filter < PointT > ::indices_)[cp]], output.points[nr_p]);
715  nr_p++;
716  }
717  else
718  {
719  if (extract_removed_indices_)
720  {
721  (*removed_indices_)[nr_removed_p] = (*Filter<PointT>::indices_)[cp];
722  nr_removed_p++;
723  }
724  }
725  }
726 
727  output.width = nr_p;
728  output.points.resize (nr_p);
729  }
730  else
731  {
732  std::vector<int> indices = *Filter<PointT>::indices_;
733  std::sort (indices.begin (), indices.end ()); //TODO: is this necessary or can we assume the indices to be sorted?
734  bool removed_p = false;
735  size_t ci = 0;
736  for (size_t cp = 0; cp < input_->points.size (); ++cp)
737  {
738  if (cp == static_cast<size_t> (indices[ci]))
739  {
740  if (ci < indices.size () - 1)
741  {
742  ci++;
743  if (cp == static_cast<size_t> (indices[ci])) //check whether the next index will have the same value. TODO: necessary?
744  continue;
745  }
746 
747  // copy all the fields
748  copyPoint (input_->points[cp], output.points[cp]);
749 
750  if (!condition_->evaluate (input_->points[cp]))
751  {
752  output.points[cp].getVector4fMap ().setConstant (user_filter_value_);
753  removed_p = true;
754 
755  if (extract_removed_indices_)
756  {
757  (*removed_indices_)[nr_removed_p] = static_cast<int> (cp);
758  nr_removed_p++;
759  }
760  }
761  }
762  else
763  {
764  output.points[cp].getVector4fMap ().setConstant (user_filter_value_);
765  removed_p = true;
766  //as for !keep_organized_: removed points due to setIndices are not considered as removed_indices_
767  }
768  }
769 
770  if (removed_p && !std::isfinite (user_filter_value_))
771  output.is_dense = false;
772  }
773  removed_indices_->resize (nr_removed_p);
774 }
775 
776 #define PCL_INSTANTIATE_PointDataAtOffset(T) template class PCL_EXPORTS pcl::PointDataAtOffset<T>;
777 #define PCL_INSTANTIATE_ComparisonBase(T) template class PCL_EXPORTS pcl::ComparisonBase<T>;
778 #define PCL_INSTANTIATE_FieldComparison(T) template class PCL_EXPORTS pcl::FieldComparison<T>;
779 #define PCL_INSTANTIATE_PackedRGBComparison(T) template class PCL_EXPORTS pcl::PackedRGBComparison<T>;
780 #define PCL_INSTANTIATE_PackedHSIComparison(T) template class PCL_EXPORTS pcl::PackedHSIComparison<T>;
781 #define PCL_INSTANTIATE_TfQuadraticXYZComparison(T) template class PCL_EXPORTS pcl::TfQuadraticXYZComparison<T>;
782 #define PCL_INSTANTIATE_ConditionBase(T) template class PCL_EXPORTS pcl::ConditionBase<T>;
783 #define PCL_INSTANTIATE_ConditionAnd(T) template class PCL_EXPORTS pcl::ConditionAnd<T>;
784 #define PCL_INSTANTIATE_ConditionOr(T) template class PCL_EXPORTS pcl::ConditionOr<T>;
785 #define PCL_INSTANTIATE_ConditionalRemoval(T) template class PCL_EXPORTS pcl::ConditionalRemoval<T>;
786 
787 #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.
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:426
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.
typename ConditionBase::Ptr ConditionBasePtr
ComparisonOps::CompareOp op_
The comparison operator type.
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:431
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:429
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:423
void addCondition(Ptr condition)
Add a nested condition to this condition.
typename ComparisonBase::ConstPtr ComparisonBaseConstPtr
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:434
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.
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