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