util3d_filtering.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include "rtabmap/core/util3d_filtering.h"
29 
30 #include <pcl/filters/extract_indices.h>
31 #include <pcl/filters/voxel_grid.h>
32 #include <pcl/filters/frustum_culling.h>
33 #include <pcl/filters/random_sample.h>
34 #include <pcl/filters/passthrough.h>
35 #include <pcl/filters/crop_box.h>
36 
37 #include <pcl/features/normal_3d_omp.h>
38 
39 #include <pcl/search/kdtree.h>
40 
41 #include <pcl/common/common.h>
42 
43 #include <pcl/segmentation/extract_clusters.h>
44 #include <pcl/segmentation/sac_segmentation.h>
45 
46 #include <rtabmap/core/util3d.h>
47 #include <rtabmap/core/util3d_surface.h>
48 
49 #include <rtabmap/utilite/ULogger.h>
50 #include <rtabmap/utilite/UMath.h>
51 #include <rtabmap/utilite/UConversion.h>
52 
53 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
54 #include <pcl/impl/instantiate.hpp>
55 #include <pcl/point_types.h>
56 #include <pcl/segmentation/impl/extract_clusters.hpp>
57 #include <pcl/segmentation/extract_labeled_clusters.h>
58 #include <pcl/segmentation/impl/extract_labeled_clusters.hpp>
59 #include <pcl/filters/impl/extract_indices.hpp>
60 
61 PCL_INSTANTIATE(EuclideanClusterExtraction, (pcl::PointXYZRGBNormal))
62 PCL_INSTANTIATE(extractEuclideanClusters, (pcl::PointXYZRGBNormal))
63 PCL_INSTANTIATE(extractEuclideanClusters_indices, (pcl::PointXYZRGBNormal))
64 PCL_INSTANTIATE(ExtractIndices, (pcl::PointNormal))
65 
66 #endif
67 
68 namespace rtabmap
69 {
70 
71 namespace util3d
72 {
73 
74 LaserScan commonFiltering(
75  const LaserScan & scanIn,
76  int downsamplingStep,
77  float rangeMin,
78  float rangeMax,
79  float voxelSize,
80  int normalK,
81  float normalRadius,
82  bool forceGroundNormalsUp)
83 {
84  LaserScan scan = scanIn;
85  UDEBUG("scan size=%d format=%d, step=%d, rangeMin=%f, rangeMax=%f, voxel=%f, normalK=%d, normalRadius=%f",
86  scan.size(), (int)scan.format(), downsamplingStep, rangeMin, rangeMax, voxelSize, normalK, normalRadius);
87  if(!scan.isEmpty())
88  {
89  // combined downsampling and range filtering step
90  if(downsamplingStep<=1 || scan.size() <= downsamplingStep)
91  {
92  downsamplingStep = 1;
93  }
94 
95  if(downsamplingStep > 1 || rangeMin > 0.0f || rangeMax > 0.0f)
96  {
97  cv::Mat tmp = cv::Mat(1, scan.size()/downsamplingStep, scan.dataType());
98  bool is2d = scan.is2d();
99  int oi = 0;
100  float rangeMinSqrd = rangeMin * rangeMin;
101  float rangeMaxSqrd = rangeMax * rangeMax;
102  for(int i=0; i<scan.size()-downsamplingStep+1; i+=downsamplingStep)
103  {
104  const float * ptr = scan.data().ptr<float>(0, i);
105 
106  if(rangeMin>0.0f || rangeMax>0.0f)
107  {
108  float r;
109  if(is2d)
110  {
111  r = ptr[0]*ptr[0] + ptr[1]*ptr[1];
112  }
113  else
114  {
115  r = ptr[0]*ptr[0] + ptr[1]*ptr[1] + ptr[2]*ptr[2];
116  }
117 
118  if(rangeMin > 0.0f && r < rangeMinSqrd)
119  {
120  continue;
121  }
122  if(rangeMax > 0.0f && r > rangeMaxSqrd)
123  {
124  continue;
125  }
126  }
127 
128  cv::Mat(scan.data(), cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(tmp, cv::Range::all(), cv::Range(oi,oi+1)));
129  ++oi;
130  }
131  int previousSize = scan.size();
132  int scanMaxPtsTmp = scan.maxPoints();
133  scan = LaserScan(cv::Mat(tmp, cv::Range::all(), cv::Range(0, oi)), scanMaxPtsTmp/downsamplingStep, rangeMax>0.0f&&rangeMax<scan.maxRange()?rangeMax:scan.maxRange(), scan.format(), scan.localTransform());
134  UDEBUG("Downsampling scan (step=%d): %d -> %d (scanMaxPts=%d->%d)", downsamplingStep, previousSize, scan.size(), scanMaxPtsTmp, scan.maxPoints());
135  }
136 
137  if(scan.size() && (voxelSize > 0.0f || ((normalK > 0 || normalRadius>0.0f) && !scan.hasNormals())))
138  {
139  // convert to compatible PCL format and filter it
140  if(scan.hasRGB())
141  {
142  UASSERT(!scan.is2d());
143  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = laserScanToPointCloudRGB(scan);
144  if(cloud->size())
145  {
146  int scanMaxPts = scan.maxPoints();
147  if(voxelSize > 0.0f)
148  {
149  cloud = voxelize(cloud, voxelSize);
150  float ratio = float(cloud->size()) / scan.size();
151  scanMaxPts = int(float(scanMaxPts) * ratio);
152  UDEBUG("Voxel filtering scan (voxel=%f m): %d -> %d (scanMaxPts=%d->%d)", voxelSize, scan.size(), cloud->size(), scan.maxPoints(), scanMaxPts);
153  }
154  if(cloud->size() && (normalK > 0 || normalRadius>0.0f))
155  {
156  pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, normalK, normalRadius);
157  scan = LaserScan(laserScanFromPointCloud(*cloud, *normals), scanMaxPts, scan.maxRange(), LaserScan::kXYZRGBNormal, scan.localTransform());
158  UDEBUG("Normals computed (k=%d radius=%f)", normalK, normalRadius);
159  }
160  else
161  {
162  if(scan.hasNormals())
163  {
164  UWARN("Voxel filter i applied, but normal parameters are not set and input scan has normals. The returned scan has no normals.");
165  }
166  scan = LaserScan(laserScanFromPointCloud(*cloud), scanMaxPts, scan.maxRange(), LaserScan::kXYZRGB, scan.localTransform());
167  }
168  }
169  }
170  else if(scan.hasIntensity())
171  {
172  pcl::PointCloud<pcl::PointXYZI>::Ptr cloud = laserScanToPointCloudI(scan);
173  if(cloud->size())
174  {
175  int scanMaxPts = scan.maxPoints();
176  if(voxelSize > 0.0f)
177  {
178  cloud = voxelize(cloud, voxelSize);
179  float ratio = float(cloud->size()) / scan.size();
180  scanMaxPts = int(float(scanMaxPts) * ratio);
181  UDEBUG("Voxel filtering scan (voxel=%f m): %d -> %d (scanMaxPts=%d->%d)", voxelSize, scan.size(), cloud->size(), scan.maxPoints(), scanMaxPts);
182  }
183  if(cloud->size() && (normalK > 0 || normalRadius>0.0f))
184  {
185  pcl::PointCloud<pcl::Normal>::Ptr normals;
186  if(scan.is2d())
187  {
188  normals = util3d::computeNormals2D(cloud, normalK, normalRadius);
189  scan = LaserScan(laserScan2dFromPointCloud(*cloud, *normals), scanMaxPts, scan.maxRange(), LaserScan::kXYINormal, scan.localTransform());
190  }
191  else
192  {
193  normals = util3d::computeNormals(cloud, normalK, normalRadius);
194  scan = LaserScan(laserScanFromPointCloud(*cloud, *normals), scanMaxPts, scan.maxRange(), LaserScan::kXYZINormal, scan.localTransform());
195  }
196  UDEBUG("Normals computed (k=%d radius=%f)", normalK, normalRadius);
197  }
198  else
199  {
200  if(scan.hasNormals())
201  {
202  UWARN("Voxel filter i applied, but normal parameters are not set and input scan has normals. The returned scan has no normals.");
203  }
204  if(scan.is2d())
205  {
206  scan = LaserScan(laserScan2dFromPointCloud(*cloud), scanMaxPts, scan.maxRange(), LaserScan::kXYI, scan.localTransform());
207  }
208  else
209  {
210  scan = LaserScan(laserScanFromPointCloud(*cloud), scanMaxPts, scan.maxRange(), LaserScan::kXYZI, scan.localTransform());
211  }
212  }
213  }
214  }
215  else
216  {
217  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = laserScanToPointCloud(scan);
218  if(cloud->size())
219  {
220  int scanMaxPts = scan.maxPoints();
221  if(voxelSize > 0.0f)
222  {
223  cloud = voxelize(cloud, voxelSize);
224  float ratio = float(cloud->size()) / scan.size();
225  scanMaxPts = int(float(scanMaxPts) * ratio);
226  UDEBUG("Voxel filtering scan (voxel=%f m): %d -> %d (scanMaxPts=%d->%d)", voxelSize, scan.size(), cloud->size(), scan.maxPoints(), scanMaxPts);
227  }
228  if(cloud->size() && (normalK > 0 || normalRadius>0.0f))
229  {
230  pcl::PointCloud<pcl::Normal>::Ptr normals;
231  if(scan.is2d())
232  {
233  normals = util3d::computeNormals2D(cloud, normalK, normalRadius);
234  scan = LaserScan(laserScan2dFromPointCloud(*cloud, *normals), scanMaxPts, scan.maxRange(), LaserScan::kXYNormal, scan.localTransform());
235  }
236  else
237  {
238  normals = util3d::computeNormals(cloud, normalK, normalRadius);
239  scan = LaserScan(laserScanFromPointCloud(*cloud, *normals), scanMaxPts, scan.maxRange(), LaserScan::kXYZNormal, scan.localTransform());
240  }
241  UDEBUG("Normals computed (k=%d radius=%f)", normalK, normalRadius);
242  }
243  else
244  {
245  if(scan.hasNormals())
246  {
247  UWARN("Voxel filter i applied, but normal parameters are not set and input scan has normals. The returned scan has no normals.");
248  }
249  if(scan.is2d())
250  {
251  scan = LaserScan(laserScan2dFromPointCloud(*cloud), scanMaxPts, scan.maxRange(), LaserScan::kXY, scan.localTransform());
252  }
253  else
254  {
255  scan = LaserScan(laserScanFromPointCloud(*cloud), scanMaxPts, scan.maxRange(), LaserScan::kXYZ, scan.localTransform());
256  }
257  }
258  }
259  }
260  }
261 
262  if(scan.size() && !scan.is2d() && scan.hasNormals() && forceGroundNormalsUp)
263  {
264  scan = util3d::adjustNormalsToViewPoint(scan, Eigen::Vector3f(0,0,0), forceGroundNormalsUp);
265  }
266  }
267  return scan;
268 }
269 
270 LaserScan rangeFiltering(
271  const LaserScan & scan,
272  float rangeMin,
273  float rangeMax)
274 {
275  UASSERT(rangeMin >=0.0f && rangeMax>=0.0f);
276  if(!scan.isEmpty())
277  {
278  if(rangeMin > 0.0f || rangeMax > 0.0f)
279  {
280  cv::Mat output = cv::Mat(1, scan.size(), scan.dataType());
281  bool is2d = scan.is2d();
282  int oi = 0;
283  float rangeMinSqrd = rangeMin * rangeMin;
284  float rangeMaxSqrd = rangeMax * rangeMax;
285  for(int i=0; i<scan.size(); ++i)
286  {
287  const float * ptr = scan.data().ptr<float>(0, i);
288  float r;
289  if(is2d)
290  {
291  r = ptr[0]*ptr[0] + ptr[1]*ptr[1];
292  }
293  else
294  {
295  r = ptr[0]*ptr[0] + ptr[1]*ptr[1] + ptr[2]*ptr[2];
296  }
297 
298  if(rangeMin > 0.0f && r < rangeMinSqrd)
299  {
300  continue;
301  }
302  if(rangeMax > 0.0f && r > rangeMaxSqrd)
303  {
304  continue;
305  }
306 
307  cv::Mat(scan.data(), cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(output, cv::Range::all(), cv::Range(oi,oi+1)));
308  ++oi;
309  }
310  return LaserScan(cv::Mat(output, cv::Range::all(), cv::Range(0, oi)), scan.maxPoints(), scan.maxRange(), scan.format(), scan.localTransform());
311  }
312  }
313 
314  return scan;
315 }
316 
317 LaserScan downsample(
318  const LaserScan & scan,
319  int step)
320 {
321  UASSERT(step > 0);
322  if(step <= 1 || scan.size() <= step)
323  {
324  // no sampling
325  return scan;
326  }
327  else
328  {
329  int finalSize = scan.size()/step;
330  cv::Mat output = cv::Mat(1, finalSize, scan.dataType());
331  int oi = 0;
332  for(int i=0; i<scan.size()-step+1; i+=step)
333  {
334  cv::Mat(scan.data(), cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(output, cv::Range::all(), cv::Range(oi,oi+1)));
335  ++oi;
336  }
337  return LaserScan(output, scan.maxPoints()/step, scan.maxRange(), scan.format(), scan.localTransform());
338  }
339 }
340 template<typename PointT>
341 typename pcl::PointCloud<PointT>::Ptr downsampleImpl(
342  const typename pcl::PointCloud<PointT>::Ptr & cloud,
343  int step)
344 {
345  UASSERT(step > 0);
346  typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
347  if(step <= 1 || (int)cloud->size() <= step)
348  {
349  // no sampling
350  *output = *cloud;
351  }
352  else
353  {
354  int finalSize = int(cloud->size())/step;
355  output->resize(finalSize);
356  int oi = 0;
357  for(unsigned int i=0; i<cloud->size()-step+1; i+=step)
358  {
359  (*output)[oi++] = cloud->at(i);
360  }
361  }
362  return output;
363 }
364 pcl::PointCloud<pcl::PointXYZ>::Ptr downsample(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, int step)
365 {
366  return downsampleImpl<pcl::PointXYZ>(cloud, step);
367 }
368 pcl::PointCloud<pcl::PointXYZRGB>::Ptr downsample(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, int step)
369 {
370  return downsampleImpl<pcl::PointXYZRGB>(cloud, step);
371 }
372 pcl::PointCloud<pcl::PointNormal>::Ptr downsample(const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, int step)
373 {
374  return downsampleImpl<pcl::PointNormal>(cloud, step);
375 }
376 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr downsample(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, int step)
377 {
378  return downsampleImpl<pcl::PointXYZRGBNormal>(cloud, step);
379 }
380 
381 template<typename PointT>
382 typename pcl::PointCloud<PointT>::Ptr voxelizeImpl(
383  const typename pcl::PointCloud<PointT>::Ptr & cloud,
384  const pcl::IndicesPtr & indices,
385  float voxelSize)
386 {
387  UASSERT(voxelSize > 0.0f);
388  typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
389  if((cloud->is_dense && cloud->size()) || (!cloud->is_dense && indices->size()))
390  {
391  pcl::VoxelGrid<PointT> filter;
392  filter.setLeafSize(voxelSize, voxelSize, voxelSize);
393  filter.setInputCloud(cloud);
394  if(indices->size())
395  {
396  filter.setIndices(indices);
397  }
398  filter.filter(*output);
399  }
400  else if(cloud->size() && !cloud->is_dense && indices->size() == 0)
401  {
402  UWARN("Cannot voxelize a not dense (organized) cloud with empty indices! (input=%d pts). Returning empty cloud!", (int)cloud->size());
403  }
404  return output;
405 }
406 
407 pcl::PointCloud<pcl::PointXYZ>::Ptr voxelize(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, const pcl::IndicesPtr & indices, float voxelSize)
408 {
409  return voxelizeImpl<pcl::PointXYZ>(cloud, indices, voxelSize);
410 }
411 pcl::PointCloud<pcl::PointNormal>::Ptr voxelize(const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, const pcl::IndicesPtr & indices, float voxelSize)
412 {
413  return voxelizeImpl<pcl::PointNormal>(cloud, indices, voxelSize);
414 }
415 pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelize(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices, float voxelSize)
416 {
417  return voxelizeImpl<pcl::PointXYZRGB>(cloud, indices, voxelSize);
418 }
419 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr voxelize(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, const pcl::IndicesPtr & indices, float voxelSize)
420 {
421  return voxelizeImpl<pcl::PointXYZRGBNormal>(cloud, indices, voxelSize);
422 }
423 pcl::PointCloud<pcl::PointXYZI>::Ptr voxelize(const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud, const pcl::IndicesPtr & indices, float voxelSize)
424 {
425  return voxelizeImpl<pcl::PointXYZI>(cloud, indices, voxelSize);
426 }
427 pcl::PointCloud<pcl::PointXYZINormal>::Ptr voxelize(const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud, const pcl::IndicesPtr & indices, float voxelSize)
428 {
429  return voxelizeImpl<pcl::PointXYZINormal>(cloud, indices, voxelSize);
430 }
431 
432 pcl::PointCloud<pcl::PointXYZ>::Ptr voxelize(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, float voxelSize)
433 {
434  pcl::IndicesPtr indices(new std::vector<int>);
435  return voxelize(cloud, indices, voxelSize);
436 }
437 pcl::PointCloud<pcl::PointNormal>::Ptr voxelize(const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, float voxelSize)
438 {
439  pcl::IndicesPtr indices(new std::vector<int>);
440  return voxelize(cloud, indices, voxelSize);
441 }
442 pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelize(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, float voxelSize)
443 {
444  pcl::IndicesPtr indices(new std::vector<int>);
445  return voxelize(cloud, indices, voxelSize);
446 }
447 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr voxelize(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, float voxelSize)
448 {
449  pcl::IndicesPtr indices(new std::vector<int>);
450  return voxelize(cloud, indices, voxelSize);
451 }
452 pcl::PointCloud<pcl::PointXYZI>::Ptr voxelize(const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud, float voxelSize)
453 {
454  pcl::IndicesPtr indices(new std::vector<int>);
455  return voxelize(cloud, indices, voxelSize);
456 }
457 pcl::PointCloud<pcl::PointXYZINormal>::Ptr voxelize(const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud, float voxelSize)
458 {
459  pcl::IndicesPtr indices(new std::vector<int>);
460  return voxelize(cloud, indices, voxelSize);
461 }
462 
463 template<typename PointT>
464 typename pcl::PointCloud<PointT>::Ptr randomSamplingImpl(
465  const typename pcl::PointCloud<PointT>::Ptr & cloud, int samples)
466 {
467  UASSERT(samples > 0);
468  typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
469  pcl::RandomSample<PointT> filter;
470  filter.setSample(samples);
471  filter.setInputCloud(cloud);
472  filter.filter(*output);
473  return output;
474 }
475 pcl::PointCloud<pcl::PointXYZ>::Ptr randomSampling(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, int samples)
476 {
477  return randomSamplingImpl<pcl::PointXYZ>(cloud, samples);
478 }
479 pcl::PointCloud<pcl::PointXYZRGB>::Ptr randomSampling(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, int samples)
480 {
481  return randomSamplingImpl<pcl::PointXYZRGB>(cloud, samples);
482 }
483 
484 template<typename PointT>
485 pcl::IndicesPtr passThroughImpl(
486  const typename pcl::PointCloud<PointT>::Ptr & cloud,
487  const pcl::IndicesPtr & indices,
488  const std::string & axis,
489  float min,
490  float max,
491  bool negative)
492 {
493  UASSERT_MSG(max > min, uFormat("cloud=%d, max=%f min=%f axis=%s", (int)cloud->size(), max, min, axis.c_str()).c_str());
494  UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0);
495 
496  pcl::IndicesPtr output(new std::vector<int>);
497  pcl::PassThrough<PointT> filter;
498  filter.setNegative(negative);
499  filter.setFilterFieldName(axis);
500  filter.setFilterLimits(min, max);
501  filter.setInputCloud(cloud);
502  filter.setIndices(indices);
503  filter.filter(*output);
504  return output;
505 }
506 
507 pcl::IndicesPtr passThrough(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, const pcl::IndicesPtr & indices, const std::string & axis, float min, float max, bool negative)
508 {
509  return passThroughImpl<pcl::PointXYZ>(cloud, indices, axis, min, max, negative);
510 }
511 pcl::IndicesPtr passThrough(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices, const std::string & axis, float min, float max, bool negative)
512 {
513  return passThroughImpl<pcl::PointXYZRGB>(cloud, indices, axis, min, max, negative);
514 }
515 pcl::IndicesPtr passThrough(const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud, const pcl::IndicesPtr & indices, const std::string & axis, float min, float max, bool negative)
516 {
517  return passThroughImpl<pcl::PointXYZI>(cloud, indices, axis, min, max, negative);
518 }
519 pcl::IndicesPtr passThrough(const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, const pcl::IndicesPtr & indices, const std::string & axis, float min, float max, bool negative)
520 {
521  return passThroughImpl<pcl::PointNormal>(cloud, indices, axis, min, max, negative);
522 }
523 pcl::IndicesPtr passThrough(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, const pcl::IndicesPtr & indices, const std::string & axis, float min, float max, bool negative)
524 {
525  return passThroughImpl<pcl::PointXYZRGBNormal>(cloud, indices, axis, min, max, negative);
526 }
527 pcl::IndicesPtr passThrough(const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud, const pcl::IndicesPtr & indices, const std::string & axis, float min, float max, bool negative)
528 {
529  return passThroughImpl<pcl::PointXYZINormal>(cloud, indices, axis, min, max, negative);
530 }
531 
532 template<typename PointT>
533 typename pcl::PointCloud<PointT>::Ptr passThroughImpl(
534  const typename pcl::PointCloud<PointT>::Ptr & cloud,
535  const std::string & axis,
536  float min,
537  float max,
538  bool negative)
539 {
540  UASSERT_MSG(max > min, uFormat("cloud=%d, max=%f min=%f axis=%s", (int)cloud->size(), max, min, axis.c_str()).c_str());
541  UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0);
542 
543  typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
544  pcl::PassThrough<PointT> filter;
545  filter.setNegative(negative);
546  filter.setFilterFieldName(axis);
547  filter.setFilterLimits(min, max);
548  filter.setInputCloud(cloud);
549  filter.filter(*output);
550  return output;
551 }
552 pcl::PointCloud<pcl::PointXYZ>::Ptr passThrough(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, const std::string & axis, float min, float max, bool negative)
553 {
554  return passThroughImpl<pcl::PointXYZ>(cloud, axis, min ,max, negative);
555 }
556 pcl::PointCloud<pcl::PointXYZRGB>::Ptr passThrough(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const std::string & axis, float min, float max, bool negative)
557 {
558  return passThroughImpl<pcl::PointXYZRGB>(cloud, axis, min ,max, negative);
559 }
560 pcl::PointCloud<pcl::PointXYZI>::Ptr passThrough(const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud, const std::string & axis, float min, float max, bool negative)
561 {
562  return passThroughImpl<pcl::PointXYZI>(cloud, axis, min ,max, negative);
563 }
564 pcl::PointCloud<pcl::PointNormal>::Ptr passThrough(const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, const std::string & axis, float min, float max, bool negative)
565 {
566  return passThroughImpl<pcl::PointNormal>(cloud, axis, min ,max, negative);
567 }
568 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr passThrough(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, const std::string & axis, float min, float max, bool negative)
569 {
570  return passThroughImpl<pcl::PointXYZRGBNormal>(cloud, axis, min ,max, negative);
571 }
572 pcl::PointCloud<pcl::PointXYZINormal>::Ptr passThrough(const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud, const std::string & axis, float min, float max, bool negative)
573 {
574  return passThroughImpl<pcl::PointXYZINormal>(cloud, axis, min ,max, negative);
575 }
576 
577 template<typename PointT>
578 pcl::IndicesPtr cropBoxImpl(
579  const typename pcl::PointCloud<PointT>::Ptr & cloud,
580  const pcl::IndicesPtr & indices,
581  const Eigen::Vector4f & min,
582  const Eigen::Vector4f & max,
583  const Transform & transform,
584  bool negative)
585 {
586  UASSERT(min[0] < max[0] && min[1] < max[1] && min[2] < max[2]);
587 
588  pcl::IndicesPtr output(new std::vector<int>);
589  pcl::CropBox<PointT> filter;
590  filter.setNegative(negative);
591  filter.setMin(min);
592  filter.setMax(max);
593  if(!transform.isNull() && !transform.isIdentity())
594  {
595  filter.setTransform(transform.toEigen3f());
596  }
597  filter.setInputCloud(cloud);
598  filter.setIndices(indices);
599  filter.filter(*output);
600  return output;
601 }
602 
603 pcl::IndicesPtr cropBox(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, const pcl::IndicesPtr & indices, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform, bool negative)
604 {
605  return cropBoxImpl<pcl::PointXYZ>(cloud, indices, min, max, transform, negative);
606 }
607 pcl::IndicesPtr cropBox(const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, const pcl::IndicesPtr & indices, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform, bool negative)
608 {
609  return cropBoxImpl<pcl::PointNormal>(cloud, indices, min, max, transform, negative);
610 }
611 pcl::IndicesPtr cropBox(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform, bool negative)
612 {
613  return cropBoxImpl<pcl::PointXYZRGB>(cloud, indices, min, max, transform, negative);
614 }
615 pcl::IndicesPtr cropBox(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, const pcl::IndicesPtr & indices, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform, bool negative)
616 {
617  return cropBoxImpl<pcl::PointXYZRGBNormal>(cloud, indices, min, max, transform, negative);
618 }
619 
620 template<typename PointT>
621 typename pcl::PointCloud<PointT>::Ptr cropBoxImpl(
622  const typename pcl::PointCloud<PointT>::Ptr & cloud,
623  const Eigen::Vector4f & min,
624  const Eigen::Vector4f & max,
625  const Transform & transform,
626  bool negative)
627 {
628  UASSERT(min[0] < max[0] && min[1] < max[1] && min[2] < max[2]);
629 
630  typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
631  pcl::CropBox<PointT> filter;
632  filter.setNegative(negative);
633  filter.setMin(min);
634  filter.setMax(max);
635  if(!transform.isNull() && !transform.isIdentity())
636  {
637  filter.setTransform(transform.toEigen3f());
638  }
639  filter.setInputCloud(cloud);
640  filter.filter(*output);
641  return output;
642 }
643 pcl::PointCloud<pcl::PointXYZ>::Ptr cropBox(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform, bool negative)
644 {
645  return cropBoxImpl<pcl::PointXYZ>(cloud, min, max, transform, negative);
646 }
647 pcl::PointCloud<pcl::PointNormal>::Ptr cropBox(const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform, bool negative)
648 {
649  return cropBoxImpl<pcl::PointNormal>(cloud, min, max, transform, negative);
650 }
651 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cropBox(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform, bool negative)
652 {
653  return cropBoxImpl<pcl::PointXYZRGB>(cloud, min, max, transform, negative);
654 }
655 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cropBox(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform, bool negative)
656 {
657  return cropBoxImpl<pcl::PointXYZRGBNormal>(cloud, min, max, transform, negative);
658 }
659 
660 template<typename PointT>
661 pcl::IndicesPtr frustumFilteringImpl(
662  const typename pcl::PointCloud<PointT>::Ptr & cloud,
663  const pcl::IndicesPtr & indices,
664  const Transform & cameraPose,
665  float horizontalFOV, // in degrees
666  float verticalFOV, // in degrees
667  float nearClipPlaneDistance,
668  float farClipPlaneDistance,
669  bool negative)
670 {
671  UASSERT(horizontalFOV > 0.0f && verticalFOV > 0.0f);
672  UASSERT(farClipPlaneDistance > nearClipPlaneDistance);
673  UASSERT(!cameraPose.isNull());
674 
675  pcl::IndicesPtr output(new std::vector<int>);
676  pcl::FrustumCulling<PointT> fc;
677  fc.setNegative(negative);
678  fc.setInputCloud (cloud);
679  if(indices.get() && indices->size())
680  {
681  fc.setIndices(indices);
682  }
683  fc.setVerticalFOV (verticalFOV);
684  fc.setHorizontalFOV (horizontalFOV);
685  fc.setNearPlaneDistance (nearClipPlaneDistance);
686  fc.setFarPlaneDistance (farClipPlaneDistance);
687 
688  fc.setCameraPose (cameraPose.toEigen4f());
689  fc.filter (*output);
690 
691  return output;
692 }
693 pcl::IndicesPtr frustumFiltering(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, const pcl::IndicesPtr & indices, const Transform & cameraPose, float horizontalFOV, float verticalFOV, float nearClipPlaneDistance, float farClipPlaneDistance, bool negative)
694 {
695  return frustumFilteringImpl<pcl::PointXYZ>(cloud, indices, cameraPose, horizontalFOV, verticalFOV, nearClipPlaneDistance, farClipPlaneDistance, negative);
696 }
697 
698 template<typename PointT>
699 typename pcl::PointCloud<PointT>::Ptr frustumFilteringImpl(
700  const typename pcl::PointCloud<PointT>::Ptr & cloud,
701  const Transform & cameraPose,
702  float horizontalFOV, // in degrees
703  float verticalFOV, // in degrees
704  float nearClipPlaneDistance,
705  float farClipPlaneDistance,
706  bool negative)
707 {
708  UASSERT(horizontalFOV > 0.0f && verticalFOV > 0.0f);
709  UASSERT(farClipPlaneDistance > nearClipPlaneDistance);
710  UASSERT(!cameraPose.isNull());
711 
712  typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
713  pcl::FrustumCulling<PointT> fc;
714  fc.setNegative(negative);
715  fc.setInputCloud (cloud);
716  fc.setVerticalFOV (verticalFOV);
717  fc.setHorizontalFOV (horizontalFOV);
718  fc.setNearPlaneDistance (nearClipPlaneDistance);
719  fc.setFarPlaneDistance (farClipPlaneDistance);
720 
721  fc.setCameraPose (cameraPose.toEigen4f());
722  fc.filter (*output);
723 
724  return output;
725 }
726 pcl::PointCloud<pcl::PointXYZ>::Ptr frustumFiltering(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, const Transform & cameraPose, float horizontalFOV, float verticalFOV, float nearClipPlaneDistance, float farClipPlaneDistance, bool negative)
727 {
728  return frustumFilteringImpl<pcl::PointXYZ>(cloud, cameraPose, horizontalFOV, verticalFOV, nearClipPlaneDistance, farClipPlaneDistance, negative);
729 }
730 pcl::PointCloud<pcl::PointXYZRGB>::Ptr frustumFiltering(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const Transform & cameraPose, float horizontalFOV, float verticalFOV, float nearClipPlaneDistance, float farClipPlaneDistance, bool negative)
731 {
732  return frustumFilteringImpl<pcl::PointXYZRGB>(cloud, cameraPose, horizontalFOV, verticalFOV, nearClipPlaneDistance, farClipPlaneDistance, negative);
733 }
734 
735 
736 template<typename PointT>
737 typename pcl::PointCloud<PointT>::Ptr removeNaNFromPointCloudImpl(
738  const typename pcl::PointCloud<PointT>::Ptr & cloud)
739 {
740  typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
741  std::vector<int> indices;
742  pcl::removeNaNFromPointCloud(*cloud, *output, indices);
743  return output;
744 }
745 pcl::PointCloud<pcl::PointXYZ>::Ptr removeNaNFromPointCloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud)
746 {
747  return removeNaNFromPointCloudImpl<pcl::PointXYZ>(cloud);
748 }
749 pcl::PointCloud<pcl::PointXYZRGB>::Ptr removeNaNFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud)
750 {
751  return removeNaNFromPointCloudImpl<pcl::PointXYZRGB>(cloud);
752 }
753 
754 template<typename PointT>
755 typename pcl::PointCloud<PointT>::Ptr removeNaNNormalsFromPointCloudImpl(
756  const typename pcl::PointCloud<PointT>::Ptr & cloud)
757 {
758  typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
759  std::vector<int> indices;
760  pcl::removeNaNNormalsFromPointCloud(*cloud, *output, indices);
761  return output;
762 }
763 pcl::PointCloud<pcl::PointNormal>::Ptr removeNaNNormalsFromPointCloud(
764  const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud)
765 {
766  return removeNaNNormalsFromPointCloudImpl<pcl::PointNormal>(cloud);
767 }
768 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr removeNaNNormalsFromPointCloud(
769  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud)
770 {
771  return removeNaNNormalsFromPointCloudImpl<pcl::PointXYZRGBNormal>(cloud);
772 }
773 
774 
775 pcl::IndicesPtr radiusFiltering(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, float radiusSearch, int minNeighborsInRadius)
776 {
777  pcl::IndicesPtr indices(new std::vector<int>);
778  return radiusFiltering(cloud, indices, radiusSearch, minNeighborsInRadius);
779 }
780 pcl::IndicesPtr radiusFiltering(const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, float radiusSearch, int minNeighborsInRadius)
781 {
782  pcl::IndicesPtr indices(new std::vector<int>);
783  return radiusFiltering(cloud, indices, radiusSearch, minNeighborsInRadius);
784 }
785 pcl::IndicesPtr radiusFiltering(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, float radiusSearch, int minNeighborsInRadius)
786 {
787  pcl::IndicesPtr indices(new std::vector<int>);
788  return radiusFiltering(cloud, indices, radiusSearch, minNeighborsInRadius);
789 }
790 pcl::IndicesPtr radiusFiltering(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, float radiusSearch, int minNeighborsInRadius)
791 {
792  pcl::IndicesPtr indices(new std::vector<int>);
793  return radiusFiltering(cloud, indices, radiusSearch, minNeighborsInRadius);
794 }
795 
796 template<typename PointT>
797 pcl::IndicesPtr radiusFilteringImpl(
798  const typename pcl::PointCloud<PointT>::Ptr & cloud,
799  const pcl::IndicesPtr & indices,
800  float radiusSearch,
801  int minNeighborsInRadius)
802 {
803  typename pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>(false));
804 
805  if(indices->size())
806  {
807  pcl::IndicesPtr output(new std::vector<int>(indices->size()));
808  int oi = 0; // output iterator
809  tree->setInputCloud(cloud, indices);
810  for(unsigned int i=0; i<indices->size(); ++i)
811  {
812  std::vector<int> kIndices;
813  std::vector<float> kDistances;
814  int k = tree->radiusSearch(cloud->at(indices->at(i)), radiusSearch, kIndices, kDistances);
815  if(k > minNeighborsInRadius)
816  {
817  output->at(oi++) = indices->at(i);
818  }
819  }
820  output->resize(oi);
821  return output;
822  }
823  else
824  {
825  pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
826  int oi = 0; // output iterator
827  tree->setInputCloud(cloud);
828  for(unsigned int i=0; i<cloud->size(); ++i)
829  {
830  std::vector<int> kIndices;
831  std::vector<float> kDistances;
832  int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances);
833  if(k > minNeighborsInRadius)
834  {
835  output->at(oi++) = i;
836  }
837  }
838  output->resize(oi);
839  return output;
840  }
841 }
842 
843 pcl::IndicesPtr radiusFiltering(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, const pcl::IndicesPtr & indices, float radiusSearch, int minNeighborsInRadius)
844 {
845  return radiusFilteringImpl<pcl::PointXYZ>(cloud, indices, radiusSearch, minNeighborsInRadius);
846 }
847 pcl::IndicesPtr radiusFiltering(const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, const pcl::IndicesPtr & indices, float radiusSearch, int minNeighborsInRadius)
848 {
849  return radiusFilteringImpl<pcl::PointNormal>(cloud, indices, radiusSearch, minNeighborsInRadius);
850 }
851 pcl::IndicesPtr radiusFiltering(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices, float radiusSearch, int minNeighborsInRadius)
852 {
853  return radiusFilteringImpl<pcl::PointXYZRGB>(cloud, indices, radiusSearch, minNeighborsInRadius);
854 }
855 pcl::IndicesPtr radiusFiltering(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, const pcl::IndicesPtr & indices, float radiusSearch, int minNeighborsInRadius)
856 {
857  return radiusFilteringImpl<pcl::PointXYZRGBNormal>(cloud, indices, radiusSearch, minNeighborsInRadius);
858 }
859 
860 pcl::PointCloud<pcl::PointXYZRGB>::Ptr subtractFiltering(
861  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
862  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & substractCloud,
863  float radiusSearch,
864  int minNeighborsInRadius)
865 {
866  pcl::IndicesPtr indices(new std::vector<int>);
867  pcl::IndicesPtr indicesOut = subtractFiltering(cloud, indices, substractCloud, indices, radiusSearch, minNeighborsInRadius);
868  pcl::PointCloud<pcl::PointXYZRGB>::Ptr out(new pcl::PointCloud<pcl::PointXYZRGB>);
869  pcl::copyPointCloud(*cloud, *indicesOut, *out);
870  return out;
871 }
872 
873 template<typename PointT>
874 pcl::IndicesPtr subtractFilteringImpl(
875  const typename pcl::PointCloud<PointT>::Ptr & cloud,
876  const pcl::IndicesPtr & indices,
877  const typename pcl::PointCloud<PointT>::Ptr & substractCloud,
878  const pcl::IndicesPtr & substractIndices,
879  float radiusSearch,
880  int minNeighborsInRadius)
881 {
882  UASSERT(minNeighborsInRadius > 0);
883  typename pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>(false));
884 
885  if(indices->size())
886  {
887  pcl::IndicesPtr output(new std::vector<int>(indices->size()));
888  int oi = 0; // output iterator
889  if(substractIndices->size())
890  {
891  tree->setInputCloud(substractCloud, substractIndices);
892  }
893  else
894  {
895  tree->setInputCloud(substractCloud);
896  }
897  for(unsigned int i=0; i<indices->size(); ++i)
898  {
899  std::vector<int> kIndices;
900  std::vector<float> kDistances;
901  int k = tree->radiusSearch(cloud->at(indices->at(i)), radiusSearch, kIndices, kDistances);
902  if(k < minNeighborsInRadius)
903  {
904  output->at(oi++) = indices->at(i);
905  }
906  }
907  output->resize(oi);
908  return output;
909  }
910  else
911  {
912  pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
913  int oi = 0; // output iterator
914  if(substractIndices->size())
915  {
916  tree->setInputCloud(substractCloud, substractIndices);
917  }
918  else
919  {
920  tree->setInputCloud(substractCloud);
921  }
922  for(unsigned int i=0; i<cloud->size(); ++i)
923  {
924  std::vector<int> kIndices;
925  std::vector<float> kDistances;
926  int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances);
927  if(k < minNeighborsInRadius)
928  {
929  output->at(oi++) = i;
930  }
931  }
932  output->resize(oi);
933  return output;
934  }
935 }
936 pcl::IndicesPtr subtractFiltering(
937  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
938  const pcl::IndicesPtr & indices,
939  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & substractCloud,
940  const pcl::IndicesPtr & substractIndices,
941  float radiusSearch,
942  int minNeighborsInRadius)
943 {
944  return subtractFilteringImpl<pcl::PointXYZRGB>(cloud, indices, substractCloud, substractIndices, radiusSearch, minNeighborsInRadius);
945 }
946 
947 pcl::PointCloud<pcl::PointNormal>::Ptr subtractFiltering(
948  const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
949  const pcl::PointCloud<pcl::PointNormal>::Ptr & substractCloud,
950  float radiusSearch,
951  float maxAngle,
952  int minNeighborsInRadius)
953 {
954  pcl::IndicesPtr indices(new std::vector<int>);
955  pcl::IndicesPtr indicesOut = subtractFiltering(cloud, indices, substractCloud, indices, radiusSearch, maxAngle, minNeighborsInRadius);
956  pcl::PointCloud<pcl::PointNormal>::Ptr out(new pcl::PointCloud<pcl::PointNormal>);
957  pcl::copyPointCloud(*cloud, *indicesOut, *out);
958  return out;
959 }
960 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr subtractFiltering(
961  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
962  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
963  float radiusSearch,
964  float maxAngle,
965  int minNeighborsInRadius)
966 {
967  pcl::IndicesPtr indices(new std::vector<int>);
968  pcl::IndicesPtr indicesOut = subtractFiltering(cloud, indices, substractCloud, indices, radiusSearch, maxAngle, minNeighborsInRadius);
969  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr out(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
970  pcl::copyPointCloud(*cloud, *indicesOut, *out);
971  return out;
972 }
973 
974 template<typename PointT>
975 pcl::IndicesPtr subtractFilteringImpl(
976  const typename pcl::PointCloud<PointT>::Ptr & cloud,
977  const pcl::IndicesPtr & indices,
978  const typename pcl::PointCloud<PointT>::Ptr & substractCloud,
979  const pcl::IndicesPtr & substractIndices,
980  float radiusSearch,
981  float maxAngle,
982  int minNeighborsInRadius)
983 {
984  UASSERT(minNeighborsInRadius > 0);
985  typename pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>(false));
986 
987  if(indices->size())
988  {
989  pcl::IndicesPtr output(new std::vector<int>(indices->size()));
990  int oi = 0; // output iterator
991  if(substractIndices->size())
992  {
993  tree->setInputCloud(substractCloud, substractIndices);
994  }
995  else
996  {
997  tree->setInputCloud(substractCloud);
998  }
999  for(unsigned int i=0; i<indices->size(); ++i)
1000  {
1001  std::vector<int> kIndices;
1002  std::vector<float> kDistances;
1003  int k = tree->radiusSearch(cloud->at(indices->at(i)), radiusSearch, kIndices, kDistances);
1004  if(k>=minNeighborsInRadius && maxAngle > 0.0f)
1005  {
1006  Eigen::Vector4f normal(cloud->at(indices->at(i)).normal_x, cloud->at(indices->at(i)).normal_y, cloud->at(indices->at(i)).normal_z, 0.0f);
1007  if (uIsFinite(normal[0]) &&
1008  uIsFinite(normal[1]) &&
1009  uIsFinite(normal[2]))
1010  {
1011  int count = k;
1012  for(int j=0; j<count && k >= minNeighborsInRadius; ++j)
1013  {
1014  Eigen::Vector4f v(substractCloud->at(kIndices.at(j)).normal_x, substractCloud->at(kIndices.at(j)).normal_y, substractCloud->at(kIndices.at(j)).normal_z, 0.0f);
1015  if(uIsFinite(v[0]) &&
1016  uIsFinite(v[1]) &&
1017  uIsFinite(v[2]))
1018  {
1019  float angle = pcl::getAngle3D(normal, v);
1020  if(angle > maxAngle)
1021  {
1022  k-=1;
1023  }
1024  }
1025  else
1026  {
1027  k-=1;
1028  }
1029  }
1030  }
1031  else
1032  {
1033  k=0;
1034  }
1035  }
1036  if(k < minNeighborsInRadius)
1037  {
1038  output->at(oi++) = indices->at(i);
1039  }
1040  }
1041  output->resize(oi);
1042  return output;
1043  }
1044  else
1045  {
1046  pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
1047  int oi = 0; // output iterator
1048  if(substractIndices->size())
1049  {
1050  tree->setInputCloud(substractCloud, substractIndices);
1051  }
1052  else
1053  {
1054  tree->setInputCloud(substractCloud);
1055  }
1056  for(unsigned int i=0; i<cloud->size(); ++i)
1057  {
1058  std::vector<int> kIndices;
1059  std::vector<float> kDistances;
1060  int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances);
1061  if(k>=minNeighborsInRadius && maxAngle > 0.0f)
1062  {
1063  Eigen::Vector4f normal(cloud->at(i).normal_x, cloud->at(i).normal_y, cloud->at(i).normal_z, 0.0f);
1064  if (uIsFinite(normal[0]) &&
1065  uIsFinite(normal[1]) &&
1066  uIsFinite(normal[2]))
1067  {
1068  int count = k;
1069  for(int j=0; j<count && k >= minNeighborsInRadius; ++j)
1070  {
1071  Eigen::Vector4f v(substractCloud->at(kIndices.at(j)).normal_x, substractCloud->at(kIndices.at(j)).normal_y, substractCloud->at(kIndices.at(j)).normal_z, 0.0f);
1072  if(uIsFinite(v[0]) &&
1073  uIsFinite(v[1]) &&
1074  uIsFinite(v[2]))
1075  {
1076  float angle = pcl::getAngle3D(normal, v);
1077  if(angle > maxAngle)
1078  {
1079  k-=1;
1080  }
1081  }
1082  else
1083  {
1084  k-=1;
1085  }
1086  }
1087  }
1088  else
1089  {
1090  k=0;
1091  }
1092  }
1093  if(k < minNeighborsInRadius)
1094  {
1095  output->at(oi++) = i;
1096  }
1097  }
1098  output->resize(oi);
1099  return output;
1100  }
1101 }
1102 
1103 pcl::IndicesPtr subtractFiltering(
1104  const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
1105  const pcl::IndicesPtr & indices,
1106  const pcl::PointCloud<pcl::PointNormal>::Ptr & substractCloud,
1107  const pcl::IndicesPtr & substractIndices,
1108  float radiusSearch,
1109  float maxAngle,
1110  int minNeighborsInRadius)
1111 {
1112  return subtractFilteringImpl<pcl::PointNormal>(cloud, indices, substractCloud, substractIndices, radiusSearch, maxAngle, minNeighborsInRadius);
1113 }
1114 pcl::IndicesPtr subtractFiltering(
1115  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
1116  const pcl::IndicesPtr & indices,
1117  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
1118  const pcl::IndicesPtr & substractIndices,
1119  float radiusSearch,
1120  float maxAngle,
1121  int minNeighborsInRadius)
1122 {
1123  return subtractFilteringImpl<pcl::PointXYZRGBNormal>(cloud, indices, substractCloud, substractIndices, radiusSearch, maxAngle, minNeighborsInRadius);
1124 }
1125 
1126 pcl::IndicesPtr subtractAdaptiveFiltering(
1127  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
1128  const pcl::IndicesPtr & indices,
1129  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & substractCloud,
1130  const pcl::IndicesPtr & substractIndices,
1131  float radiusSearchRatio,
1132  int minNeighborsInRadius,
1133  const Eigen::Vector3f & viewpoint)
1134 {
1135  UWARN("Add angle to avoid subtraction of points with opposite normals");
1136  UASSERT(minNeighborsInRadius > 0);
1137  UASSERT(radiusSearchRatio > 0.0f);
1138  pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>(false));
1139 
1140  if(indices->size())
1141  {
1142  pcl::IndicesPtr output(new std::vector<int>(indices->size()));
1143  int oi = 0; // output iterator
1144  if(substractIndices->size())
1145  {
1146  tree->setInputCloud(substractCloud, substractIndices);
1147  }
1148  else
1149  {
1150  tree->setInputCloud(substractCloud);
1151  }
1152  for(unsigned int i=0; i<indices->size(); ++i)
1153  {
1154  if(pcl::isFinite(cloud->at(indices->at(i))))
1155  {
1156  std::vector<int> kIndices;
1157  std::vector<float> kSqrdDistances;
1158  float radius = radiusSearchRatio*uNorm(
1159  cloud->at(indices->at(i)).x-viewpoint[0],
1160  cloud->at(indices->at(i)).y-viewpoint[1],
1161  cloud->at(indices->at(i)).z-viewpoint[2]);
1162  int k = tree->radiusSearch(cloud->at(indices->at(i)), radius, kIndices, kSqrdDistances);
1163  if(k < minNeighborsInRadius)
1164  {
1165  output->at(oi++) = indices->at(i);
1166  }
1167  }
1168  }
1169  output->resize(oi);
1170  return output;
1171  }
1172  else
1173  {
1174  pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
1175  int oi = 0; // output iterator
1176  if(substractIndices->size())
1177  {
1178  tree->setInputCloud(substractCloud, substractIndices);
1179  }
1180  else
1181  {
1182  tree->setInputCloud(substractCloud);
1183  }
1184  for(unsigned int i=0; i<cloud->size(); ++i)
1185  {
1186  if(pcl::isFinite(cloud->at(i)))
1187  {
1188  std::vector<int> kIndices;
1189  std::vector<float> kSqrdDistances;
1190  float radius = radiusSearchRatio*uNorm(
1191  cloud->at(i).x-viewpoint[0],
1192  cloud->at(i).y-viewpoint[1],
1193  cloud->at(i).z-viewpoint[2]);
1194  int k = tree->radiusSearch(cloud->at(i), radius, kIndices, kSqrdDistances);
1195  if(k < minNeighborsInRadius)
1196  {
1197  output->at(oi++) = i;
1198  }
1199  }
1200  }
1201  output->resize(oi);
1202  return output;
1203  }
1204 }
1205 pcl::IndicesPtr subtractAdaptiveFiltering(
1206  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
1207  const pcl::IndicesPtr & indices,
1208  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
1209  const pcl::IndicesPtr & substractIndices,
1210  float radiusSearchRatio,
1211  float maxAngle,
1212  int minNeighborsInRadius,
1213  const Eigen::Vector3f & viewpoint)
1214 {
1215  UASSERT(minNeighborsInRadius > 0);
1216  UASSERT(radiusSearchRatio > 0.0f);
1217  pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBNormal>(false));
1218 
1219  if(indices->size())
1220  {
1221  pcl::IndicesPtr output(new std::vector<int>(indices->size()));
1222  int oi = 0; // output iterator
1223  if(substractIndices->size())
1224  {
1225  tree->setInputCloud(substractCloud, substractIndices);
1226  }
1227  else
1228  {
1229  tree->setInputCloud(substractCloud);
1230  }
1231  for(unsigned int i=0; i<indices->size(); ++i)
1232  {
1233  if(pcl::isFinite(cloud->at(indices->at(i))))
1234  {
1235  std::vector<int> kIndices;
1236  std::vector<float> kSqrdDistances;
1237  float radius = radiusSearchRatio*uNorm(
1238  cloud->at(indices->at(i)).x-viewpoint[0],
1239  cloud->at(indices->at(i)).y-viewpoint[1],
1240  cloud->at(indices->at(i)).z-viewpoint[2]);
1241  int k = tree->radiusSearch(cloud->at(indices->at(i)), radius, kIndices, kSqrdDistances);
1242  if(k>=minNeighborsInRadius && maxAngle > 0.0f)
1243  {
1244  Eigen::Vector4f normal(cloud->at(indices->at(i)).normal_x, cloud->at(indices->at(i)).normal_y, cloud->at(indices->at(i)).normal_z, 0.0f);
1245  if (uIsFinite(normal[0]) &&
1246  uIsFinite(normal[1]) &&
1247  uIsFinite(normal[2]))
1248  {
1249  int count = k;
1250  for(int j=0; j<count && k >= minNeighborsInRadius; ++j)
1251  {
1252  Eigen::Vector4f v(substractCloud->at(kIndices.at(j)).normal_x, substractCloud->at(kIndices.at(j)).normal_y, substractCloud->at(kIndices.at(j)).normal_z, 0.0f);
1253  if(uIsFinite(v[0]) &&
1254  uIsFinite(v[1]) &&
1255  uIsFinite(v[2]))
1256  {
1257  float angle = pcl::getAngle3D(normal, v);
1258  if(angle > maxAngle)
1259  {
1260  k-=1;
1261  }
1262  }
1263  else
1264  {
1265  k-=1;
1266  }
1267  }
1268  }
1269  else
1270  {
1271  k=0;
1272  }
1273  }
1274 
1275  if(k < minNeighborsInRadius)
1276  {
1277  output->at(oi++) = indices->at(i);
1278  }
1279  }
1280  }
1281  output->resize(oi);
1282  return output;
1283  }
1284  else
1285  {
1286  pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
1287  int oi = 0; // output iterator
1288  if(substractIndices->size())
1289  {
1290  tree->setInputCloud(substractCloud, substractIndices);
1291  }
1292  else
1293  {
1294  tree->setInputCloud(substractCloud);
1295  }
1296  for(unsigned int i=0; i<cloud->size(); ++i)
1297  {
1298  if(pcl::isFinite(cloud->at(i)))
1299  {
1300  std::vector<int> kIndices;
1301  std::vector<float> kSqrdDistances;
1302  float radius = radiusSearchRatio*uNorm(
1303  cloud->at(i).x-viewpoint[0],
1304  cloud->at(i).y-viewpoint[1],
1305  cloud->at(i).z-viewpoint[2]);
1306  int k = tree->radiusSearch(cloud->at(i), radius, kIndices, kSqrdDistances);
1307  if(k>=minNeighborsInRadius && maxAngle > 0.0f)
1308  {
1309  Eigen::Vector4f normal(cloud->at(i).normal_x, cloud->at(i).normal_y, cloud->at(i).normal_z, 0.0f);
1310  if (uIsFinite(normal[0]) &&
1311  uIsFinite(normal[1]) &&
1312  uIsFinite(normal[2]))
1313  {
1314  int count = k;
1315  for(int j=0; j<count && k >= minNeighborsInRadius; ++j)
1316  {
1317  Eigen::Vector4f v(substractCloud->at(kIndices.at(j)).normal_x, substractCloud->at(kIndices.at(j)).normal_y, substractCloud->at(kIndices.at(j)).normal_z, 0.0f);
1318  if(uIsFinite(v[0]) &&
1319  uIsFinite(v[1]) &&
1320  uIsFinite(v[2]))
1321  {
1322  float angle = pcl::getAngle3D(normal, v);
1323  if(angle > maxAngle)
1324  {
1325  k-=1;
1326  }
1327  }
1328  else
1329  {
1330  k-=1;
1331  }
1332  }
1333  }
1334  else
1335  {
1336  k=0;
1337  }
1338  }
1339  if(k < minNeighborsInRadius)
1340  {
1341  output->at(oi++) = i;
1342  }
1343  }
1344  }
1345  output->resize(oi);
1346  return output;
1347  }
1348 }
1349 
1350 
1351 pcl::IndicesPtr normalFiltering(
1352  const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
1353  float angleMax,
1354  const Eigen::Vector4f & normal,
1355  int normalKSearch,
1356  const Eigen::Vector4f & viewpoint)
1357 {
1358  pcl::IndicesPtr indices(new std::vector<int>);
1359  return normalFiltering(cloud, indices, angleMax, normal, normalKSearch, viewpoint);
1360 }
1361 pcl::IndicesPtr normalFiltering(
1362  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
1363  float angleMax,
1364  const Eigen::Vector4f & normal,
1365  int normalKSearch,
1366  const Eigen::Vector4f & viewpoint)
1367 {
1368  pcl::IndicesPtr indices(new std::vector<int>);
1369  return normalFiltering(cloud, indices, angleMax, normal, normalKSearch, viewpoint);
1370 }
1371 
1372 
1373 template<typename PointT>
1374 pcl::IndicesPtr normalFilteringImpl(
1375  const typename pcl::PointCloud<PointT>::Ptr & cloud,
1376  const pcl::IndicesPtr & indices,
1377  float angleMax,
1378  const Eigen::Vector4f & normal,
1379  int normalKSearch,
1380  const Eigen::Vector4f & viewpoint)
1381 {
1382  pcl::IndicesPtr output(new std::vector<int>());
1383 
1384  if(cloud->size())
1385  {
1386  typename pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>(false));
1387 
1388  pcl::NormalEstimationOMP<PointT, pcl::Normal> ne;
1389  ne.setInputCloud (cloud);
1390  if(indices->size())
1391  {
1392  ne.setIndices(indices);
1393  }
1394 
1395  if(indices->size())
1396  {
1397  tree->setInputCloud(cloud, indices);
1398  }
1399  else
1400  {
1401  tree->setInputCloud(cloud);
1402  }
1403  ne.setSearchMethod (tree);
1404 
1405  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
1406 
1407  ne.setKSearch(normalKSearch);
1408  if(viewpoint[0] != 0 || viewpoint[1] != 0 || viewpoint[2] != 0)
1409  {
1410  ne.setViewPoint(viewpoint[0], viewpoint[1], viewpoint[2]);
1411  }
1412 
1413  ne.compute (*cloud_normals);
1414 
1415  output->resize(cloud_normals->size());
1416  int oi = 0; // output iterator
1417  for(unsigned int i=0; i<cloud_normals->size(); ++i)
1418  {
1419  Eigen::Vector4f v(cloud_normals->at(i).normal_x, cloud_normals->at(i).normal_y, cloud_normals->at(i).normal_z, 0.0f);
1420  float angle = pcl::getAngle3D(normal, v);
1421  if(angle < angleMax)
1422  {
1423  output->at(oi++) = indices->size()!=0?indices->at(i):i;
1424  }
1425  }
1426  output->resize(oi);
1427  }
1428 
1429  return output;
1430 }
1431 pcl::IndicesPtr normalFiltering(
1432  const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
1433  const pcl::IndicesPtr & indices,
1434  float angleMax,
1435  const Eigen::Vector4f & normal,
1436  int normalKSearch,
1437  const Eigen::Vector4f & viewpoint)
1438 
1439 {
1440  return normalFilteringImpl<pcl::PointXYZ>(cloud, indices, angleMax, normal, normalKSearch, viewpoint);
1441 }
1442 pcl::IndicesPtr normalFiltering(
1443  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
1444  const pcl::IndicesPtr & indices,
1445  float angleMax,
1446  const Eigen::Vector4f & normal,
1447  int normalKSearch,
1448  const Eigen::Vector4f & viewpoint)
1449 {
1450  return normalFilteringImpl<pcl::PointXYZRGB>(cloud, indices, angleMax, normal, normalKSearch, viewpoint);
1451 }
1452 
1453 template<typename PointT>
1454 pcl::IndicesPtr normalFilteringImpl(
1455  const typename pcl::PointCloud<PointT>::Ptr & cloud,
1456  const pcl::IndicesPtr & indices,
1457  float angleMax,
1458  const Eigen::Vector4f & normal)
1459 {
1460  pcl::IndicesPtr output(new std::vector<int>());
1461 
1462  if(cloud->size())
1463  {
1464  int oi = 0; // output iterator
1465  if(indices->size())
1466  {
1467  output->resize(indices->size());
1468  for(unsigned int i=0; i<indices->size(); ++i)
1469  {
1470  Eigen::Vector4f v(cloud->at(indices->at(i)).normal_x, cloud->at(indices->at(i)).normal_y, cloud->at(indices->at(i)).normal_z, 0.0f);
1471  float angle = pcl::getAngle3D(normal, v);
1472  if(angle < angleMax)
1473  {
1474  output->at(oi++) = indices->size()!=0?indices->at(i):i;
1475  }
1476  }
1477  }
1478  else
1479  {
1480  output->resize(cloud->size());
1481  for(unsigned int i=0; i<cloud->size(); ++i)
1482  {
1483  Eigen::Vector4f v(cloud->at(i).normal_x, cloud->at(i).normal_y, cloud->at(i).normal_z, 0.0f);
1484  float angle = pcl::getAngle3D(normal, v);
1485  if(angle < angleMax)
1486  {
1487  output->at(oi++) = indices->size()!=0?indices->at(i):i;
1488  }
1489  }
1490  }
1491 
1492  output->resize(oi);
1493  }
1494 
1495  return output;
1496 }
1497 pcl::IndicesPtr normalFiltering(
1498  const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
1499  const pcl::IndicesPtr & indices,
1500  float angleMax,
1501  const Eigen::Vector4f & normal,
1502  int normalKSearch,
1503  const Eigen::Vector4f & viewpoint)
1504 {
1505  return normalFilteringImpl<pcl::PointNormal>(cloud, indices, angleMax, normal);
1506 }
1507 pcl::IndicesPtr normalFiltering(
1508  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
1509  const pcl::IndicesPtr & indices,
1510  float angleMax,
1511  const Eigen::Vector4f & normal,
1512  int normalKSearch,
1513  const Eigen::Vector4f & viewpoint)
1514 {
1515  return normalFilteringImpl<pcl::PointXYZRGBNormal>(cloud, indices, angleMax, normal);
1516 }
1517 
1518 std::vector<pcl::IndicesPtr> extractClusters(
1519  const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
1520  float clusterTolerance,
1521  int minClusterSize,
1522  int maxClusterSize,
1523  int * biggestClusterIndex)
1524 {
1525  pcl::IndicesPtr indices(new std::vector<int>);
1526  return extractClusters(cloud, indices, clusterTolerance, minClusterSize, maxClusterSize, biggestClusterIndex);
1527 }
1528 std::vector<pcl::IndicesPtr> extractClusters(
1529  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
1530  float clusterTolerance,
1531  int minClusterSize,
1532  int maxClusterSize,
1533  int * biggestClusterIndex)
1534 {
1535  pcl::IndicesPtr indices(new std::vector<int>);
1536  return extractClusters(cloud, indices, clusterTolerance, minClusterSize, maxClusterSize, biggestClusterIndex);
1537 }
1538 
1539 template<typename PointT>
1540 std::vector<pcl::IndicesPtr> extractClustersImpl(
1541  const typename pcl::PointCloud<PointT>::Ptr & cloud,
1542  const pcl::IndicesPtr & indices,
1543  float clusterTolerance,
1544  int minClusterSize,
1545  int maxClusterSize,
1546  int * biggestClusterIndex)
1547 {
1548  typename pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>(true));
1549  pcl::EuclideanClusterExtraction<PointT> ec;
1550  ec.setClusterTolerance (clusterTolerance);
1551  ec.setMinClusterSize (minClusterSize);
1552  ec.setMaxClusterSize (maxClusterSize);
1553  ec.setInputCloud (cloud);
1554 
1555  if(indices->size())
1556  {
1557  ec.setIndices(indices);
1558  tree->setInputCloud(cloud, indices);
1559  }
1560  else
1561  {
1562  tree->setInputCloud(cloud);
1563  }
1564  ec.setSearchMethod (tree);
1565 
1566  std::vector<pcl::PointIndices> cluster_indices;
1567  ec.extract (cluster_indices);
1568 
1569  int maxIndex=-1;
1570  unsigned int maxSize = 0;
1571  std::vector<pcl::IndicesPtr> output(cluster_indices.size());
1572  for(unsigned int i=0; i<cluster_indices.size(); ++i)
1573  {
1574  output[i] = pcl::IndicesPtr(new std::vector<int>(cluster_indices[i].indices));
1575 
1576  if(maxSize < cluster_indices[i].indices.size())
1577  {
1578  maxSize = (unsigned int)cluster_indices[i].indices.size();
1579  maxIndex = i;
1580  }
1581  }
1582  if(biggestClusterIndex)
1583  {
1584  *biggestClusterIndex = maxIndex;
1585  }
1586 
1587  return output;
1588 }
1589 
1590 std::vector<pcl::IndicesPtr> extractClusters(
1591  const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
1592  const pcl::IndicesPtr & indices,
1593  float clusterTolerance,
1594  int minClusterSize,
1595  int maxClusterSize,
1596  int * biggestClusterIndex)
1597 {
1598  return extractClustersImpl<pcl::PointXYZ>(cloud, indices, clusterTolerance, minClusterSize, maxClusterSize, biggestClusterIndex);
1599 }
1600 std::vector<pcl::IndicesPtr> extractClusters(
1601  const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
1602  const pcl::IndicesPtr & indices,
1603  float clusterTolerance,
1604  int minClusterSize,
1605  int maxClusterSize,
1606  int * biggestClusterIndex)
1607 {
1608  return extractClustersImpl<pcl::PointNormal>(cloud, indices, clusterTolerance, minClusterSize, maxClusterSize, biggestClusterIndex);
1609 }
1610 std::vector<pcl::IndicesPtr> extractClusters(
1611  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
1612  const pcl::IndicesPtr & indices,
1613  float clusterTolerance,
1614  int minClusterSize,
1615  int maxClusterSize,
1616  int * biggestClusterIndex)
1617 {
1618  return extractClustersImpl<pcl::PointXYZRGB>(cloud, indices, clusterTolerance, minClusterSize, maxClusterSize, biggestClusterIndex);
1619 }
1620 std::vector<pcl::IndicesPtr> extractClusters(
1621  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
1622  const pcl::IndicesPtr & indices,
1623  float clusterTolerance,
1624  int minClusterSize,
1625  int maxClusterSize,
1626  int * biggestClusterIndex)
1627 {
1628  return extractClustersImpl<pcl::PointXYZRGBNormal>(cloud, indices, clusterTolerance, minClusterSize, maxClusterSize, biggestClusterIndex);
1629 }
1630 
1631 template<typename PointT>
1632 pcl::IndicesPtr extractIndicesImpl(
1633  const typename pcl::PointCloud<PointT>::Ptr & cloud,
1634  const pcl::IndicesPtr & indices,
1635  bool negative)
1636 {
1637  pcl::IndicesPtr output(new std::vector<int>);
1638  pcl::ExtractIndices<PointT> extract;
1639  extract.setInputCloud (cloud);
1640  extract.setIndices(indices);
1641  extract.setNegative(negative);
1642  extract.filter(*output);
1643  return output;
1644 }
1645 
1646 pcl::IndicesPtr extractIndices(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative)
1647 {
1648  return extractIndicesImpl<pcl::PointXYZ>(cloud, indices, negative);
1649 }
1650 pcl::IndicesPtr extractIndices(const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative)
1651 {
1652  return extractIndicesImpl<pcl::PointNormal>(cloud, indices, negative);
1653 }
1654 pcl::IndicesPtr extractIndices(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative)
1655 {
1656  return extractIndicesImpl<pcl::PointXYZRGB>(cloud, indices, negative);
1657 }
1658 pcl::IndicesPtr extractIndices(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative)
1659 {
1660  return extractIndicesImpl<pcl::PointXYZRGBNormal>(cloud, indices, negative);
1661 }
1662 
1663 template<typename PointT>
1664 typename pcl::PointCloud<PointT>::Ptr extractIndicesImpl(
1665  const typename pcl::PointCloud<PointT>::Ptr & cloud,
1666  const pcl::IndicesPtr & indices,
1667  bool negative,
1668  bool keepOrganized)
1669 {
1670  typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
1671  pcl::ExtractIndices<PointT> extract;
1672  extract.setInputCloud (cloud);
1673  extract.setIndices(indices);
1674  extract.setNegative(negative);
1675  extract.setKeepOrganized(keepOrganized);
1676  extract.filter(*output);
1677  return output;
1678 }
1679 pcl::PointCloud<pcl::PointXYZ>::Ptr extractIndices(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative, bool keepOrganized)
1680 {
1681  return extractIndicesImpl<pcl::PointXYZ>(cloud, indices, negative, keepOrganized);
1682 }
1683 pcl::PointCloud<pcl::PointXYZRGB>::Ptr extractIndices(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative, bool keepOrganized)
1684 {
1685  return extractIndicesImpl<pcl::PointXYZRGB>(cloud, indices, negative, keepOrganized);
1686 }
1687 // PCL default lacks of pcl::PointNormal type support
1688 //pcl::PointCloud<pcl::PointNormal>::Ptr extractIndices(const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative, bool keepOrganized)
1689 //{
1690 // return extractIndicesImpl<pcl::PointNormal>(cloud, indices, negative, keepOrganized);
1691 //}
1692 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr extractIndices(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative, bool keepOrganized)
1693 {
1694  return extractIndicesImpl<pcl::PointXYZRGBNormal>(cloud, indices, negative, keepOrganized);
1695 }
1696 
1697 pcl::IndicesPtr extractPlane(
1698  const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
1699  float distanceThreshold,
1700  int maxIterations,
1701  pcl::ModelCoefficients * coefficientsOut)
1702 {
1703  pcl::IndicesPtr indices(new std::vector<int>);
1704  return extractPlane(cloud, indices, distanceThreshold, maxIterations, coefficientsOut);
1705 }
1706 
1707 pcl::IndicesPtr extractPlane(
1708  const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
1709  const pcl::IndicesPtr & indices,
1710  float distanceThreshold,
1711  int maxIterations,
1712  pcl::ModelCoefficients * coefficientsOut)
1713 {
1714  // Extract plane
1715  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
1716  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
1717  // Create the segmentation object
1718  pcl::SACSegmentation<pcl::PointXYZ> seg;
1719  // Optional
1720  seg.setOptimizeCoefficients (true);
1721  seg.setMaxIterations (maxIterations);
1722  // Mandatory
1723  seg.setModelType (pcl::SACMODEL_PLANE);
1724  seg.setMethodType (pcl::SAC_RANSAC);
1725  seg.setDistanceThreshold (distanceThreshold);
1726 
1727  seg.setInputCloud (cloud);
1728  if(indices->size())
1729  {
1730  seg.setIndices(indices);
1731  }
1732  seg.segment (*inliers, *coefficients);
1733 
1734  if(coefficientsOut)
1735  {
1736  *coefficientsOut = *coefficients;
1737  }
1738 
1739  return pcl::IndicesPtr(new std::vector<int>(inliers->indices));
1740 }
1741 
1742 }
1743 
1744 }
rtabmap::util3d::laserScanFromPointCloud
LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PCLPointCloud2 &cloud, bool filterNaNs=true)
Definition: util3d.cpp:1266
rtabmap::util3d::isFinite
bool RTABMAP_EXP isFinite(const cv::Point3f &pt)
Definition: util3d.cpp:3035
rtabmap::util3d::subtractFiltering
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_EXP subtractFiltering(const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &substractCloud, float radiusSearch, int minNeighborsInRadius=1)
Definition: util3d_filtering.cpp:860
rtabmap::util3d::cropBox
pcl::IndicesPtr RTABMAP_EXP cropBox(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, const Eigen::Vector4f &min, const Eigen::Vector4f &max, const Transform &transform=Transform::getIdentity(), bool negative=false)
Definition: util3d_filtering.cpp:603
rtabmap::LaserScan::dataType
int dataType() const
Definition: LaserScan.h:71
uNorm
T uNorm(const std::vector< T > &v)
Definition: UMath.h:575
rtabmap::util3d::extractClusters
std::vector< pcl::IndicesPtr > RTABMAP_EXP extractClusters(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, float clusterTolerance, int minClusterSize, int maxClusterSize=std::numeric_limits< int >::max(), int *biggestClusterIndex=0)
Definition: util3d_filtering.cpp:1518
glm::min
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
rtabmap::LaserScan::data
const cv::Mat & data() const
Definition: LaserScan.h:63
rtabmap::util3d::computeNormals2D
pcl::PointCloud< pcl::Normal >::Ptr RTABMAP_EXP computeNormals2D(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, int searchK=5, float searchRadius=0.0f, const Eigen::Vector3f &viewPoint=Eigen::Vector3f(0, 0, 0))
Definition: util3d_surface.cpp:2359
rtabmap::LaserScan::format
Format format() const
Definition: LaserScan.h:66
rtabmap::LaserScan::hasNormals
bool hasNormals() const
Definition: LaserScan.h:73
rtabmap::util3d::passThrough
pcl::IndicesPtr RTABMAP_EXP passThrough(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, const std::string &axis, float min, float max, bool negative=false)
Definition: util3d_filtering.cpp:507
rtabmap::util3d::passThroughImpl
pcl::IndicesPtr passThroughImpl(const typename pcl::PointCloud< PointT >::Ptr &cloud, const pcl::IndicesPtr &indices, const std::string &axis, float min, float max, bool negative)
Definition: util3d_filtering.cpp:485
f
f
rtabmap::LaserScan::maxRange
float maxRange() const
Definition: LaserScan.h:65
rtabmap::util3d::removeNaNNormalsFromPointCloudImpl
pcl::PointCloud< PointT >::Ptr removeNaNNormalsFromPointCloudImpl(const typename pcl::PointCloud< PointT >::Ptr &cloud)
Definition: util3d_filtering.cpp:755
pcl
Definition: CameraRGBD.h:59
rtabmap::util3d::normalFilteringImpl
pcl::IndicesPtr normalFilteringImpl(const typename pcl::PointCloud< PointT >::Ptr &cloud, const pcl::IndicesPtr &indices, float angleMax, const Eigen::Vector4f &normal, int normalKSearch, const Eigen::Vector4f &viewpoint)
Definition: util3d_filtering.cpp:1374
glm::axis
GLM_FUNC_DECL detail::tvec3< T, P > axis(detail::tquat< T, P > const &x)
rtabmap::util3d::removeNaNFromPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP removeNaNFromPointCloud(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud)
Definition: util3d_filtering.cpp:745
rtabmap::util3d::randomSamplingImpl
pcl::PointCloud< PointT >::Ptr randomSamplingImpl(const typename pcl::PointCloud< PointT >::Ptr &cloud, int samples)
Definition: util3d_filtering.cpp:464
rtabmap::LaserScan::isEmpty
bool isEmpty() const
Definition: LaserScan.h:69
rtabmap::LaserScan::hasRGB
bool hasRGB() const
Definition: LaserScan.h:74
glm::all
GLM_FUNC_DECL bool all(vecType< bool, P > const &v)
rtabmap::LaserScan::maxPoints
int maxPoints() const
Definition: LaserScan.h:64
rtabmap::LaserScan::is2d
bool is2d() const
Definition: LaserScan.h:72
rtabmap::util3d::subtractAdaptiveFiltering
pcl::IndicesPtr RTABMAP_EXP subtractAdaptiveFiltering(const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, const pcl::IndicesPtr &indices, const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &substractCloud, const pcl::IndicesPtr &substractIndices, float radiusSearchRatio=0.01, int minNeighborsInRadius=1, const Eigen::Vector3f &viewpoint=Eigen::Vector3f(0, 0, 0))
Definition: util3d_filtering.cpp:1126
UMath.h
Basic mathematics functions.
glm::angle
GLM_FUNC_DECL T angle(detail::tquat< T, P > const &x)
rtabmap::util3d::removeNaNFromPointCloudImpl
pcl::PointCloud< PointT >::Ptr removeNaNFromPointCloudImpl(const typename pcl::PointCloud< PointT >::Ptr &cloud)
Definition: util3d_filtering.cpp:737
UConversion.h
Some conversion functions.
rtabmap::LaserScan::size
int size() const
Definition: LaserScan.h:70
rtabmap::util3d::frustumFilteringImpl
pcl::IndicesPtr frustumFilteringImpl(const typename pcl::PointCloud< PointT >::Ptr &cloud, const pcl::IndicesPtr &indices, const Transform &cameraPose, float horizontalFOV, float verticalFOV, float nearClipPlaneDistance, float farClipPlaneDistance, bool negative)
Definition: util3d_filtering.cpp:661
glm::step
GLM_FUNC_DECL genType step(genType const &edge, genType const &x)
rtabmap::util3d::frustumFiltering
pcl::IndicesPtr RTABMAP_EXP frustumFiltering(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, const Transform &cameraPose, float horizontalFOV, float verticalFOV, float nearClipPlaneDistance, float farClipPlaneDistance, bool negative=false)
Definition: util3d_filtering.cpp:693
rtabmap::util3d::extractIndices
pcl::IndicesPtr RTABMAP_EXP extractIndices(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, bool negative)
Definition: util3d_filtering.cpp:1646
rtabmap::util3d::laserScanToPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP laserScanToPointCloud(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2328
rtabmap::Transform::isIdentity
bool isIdentity() const
Definition: Transform.cpp:136
rtabmap::util3d::voxelizeImpl
pcl::PointCloud< PointT >::Ptr voxelizeImpl(const typename pcl::PointCloud< PointT >::Ptr &cloud, const pcl::IndicesPtr &indices, float voxelSize)
Definition: util3d_filtering.cpp:382
rtabmap::util3d::downsample
LaserScan RTABMAP_EXP downsample(const LaserScan &cloud, int step)
Definition: util3d_filtering.cpp:317
rtabmap::util3d::laserScan2dFromPointCloud
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud< pcl::PointXYZ > &cloud, const Transform &transform=Transform(), bool filterNaNs=true)
Definition: util3d.cpp:2051
uIsFinite
bool uIsFinite(const T &value)
Definition: UMath.h:55
rtabmap::util3d::subtractFilteringImpl
pcl::IndicesPtr subtractFilteringImpl(const typename pcl::PointCloud< PointT >::Ptr &cloud, const pcl::IndicesPtr &indices, const typename pcl::PointCloud< PointT >::Ptr &substractCloud, const pcl::IndicesPtr &substractIndices, float radiusSearch, int minNeighborsInRadius)
Definition: util3d_filtering.cpp:874
UASSERT
#define UASSERT(condition)
rtabmap::util3d::downsampleImpl
pcl::PointCloud< PointT >::Ptr downsampleImpl(const typename pcl::PointCloud< PointT >::Ptr &cloud, int step)
Definition: util3d_filtering.cpp:341
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
rtabmap::util3d::randomSampling
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP randomSampling(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, int samples)
Definition: util3d_filtering.cpp:475
rtabmap::util3d::extractClustersImpl
std::vector< pcl::IndicesPtr > extractClustersImpl(const typename pcl::PointCloud< PointT >::Ptr &cloud, const pcl::IndicesPtr &indices, float clusterTolerance, int minClusterSize, int maxClusterSize, int *biggestClusterIndex)
Definition: util3d_filtering.cpp:1540
util3d_filtering.h
rtabmap::util3d::cropBoxImpl
pcl::IndicesPtr cropBoxImpl(const typename pcl::PointCloud< PointT >::Ptr &cloud, const pcl::IndicesPtr &indices, const Eigen::Vector4f &min, const Eigen::Vector4f &max, const Transform &transform, bool negative)
Definition: util3d_filtering.cpp:578
rtabmap::util3d::computeNormals
cv::Mat RTABMAP_EXP computeNormals(const cv::Mat &laserScan, int searchK, float searchRadius)
tree
tree
rtabmap::LaserScan::localTransform
Transform localTransform() const
Definition: LaserScan.h:67
rtabmap::util3d::radiusFilteringImpl
pcl::IndicesPtr radiusFilteringImpl(const typename pcl::PointCloud< PointT >::Ptr &cloud, const pcl::IndicesPtr &indices, float radiusSearch, int minNeighborsInRadius)
Definition: util3d_filtering.cpp:797
rtabmap::util3d::radiusFiltering
pcl::IndicesPtr RTABMAP_EXP radiusFiltering(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, float radiusSearch, int minNeighborsInRadius)
Definition: util3d_filtering.cpp:775
UDEBUG
#define UDEBUG(...)
glm::max
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
rtabmap::Transform::toEigen4f
Eigen::Matrix4f toEigen4f() const
Definition: Transform.cpp:325
rtabmap::util3d::extractPlane
pcl::IndicesPtr extractPlane(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, float distanceThreshold, int maxIterations=100, pcl::ModelCoefficients *coefficientsOut=0)
Definition: util3d_filtering.cpp:1697
rtabmap::util3d::adjustNormalsToViewPoint
LaserScan RTABMAP_EXP adjustNormalsToViewPoint(const LaserScan &scan, const Eigen::Vector3f &viewpoint, bool forceGroundNormalsUp)
Definition: util3d_surface.cpp:2827
ULogger.h
ULogger class and convenient macros.
UWARN
#define UWARN(...)
rtabmap::util3d::rangeFiltering
LaserScan RTABMAP_EXP rangeFiltering(const LaserScan &scan, float rangeMin, float rangeMax)
Definition: util3d_filtering.cpp:270
rtabmap::util3d::removeNaNNormalsFromPointCloud
pcl::PointCloud< pcl::PointNormal >::Ptr RTABMAP_EXP removeNaNNormalsFromPointCloud(const pcl::PointCloud< pcl::PointNormal >::Ptr &cloud)
Definition: util3d_filtering.cpp:763
rtabmap::util3d::commonFiltering
LaserScan RTABMAP_EXP commonFiltering(const LaserScan &scan, int downsamplingStep, float rangeMin=0.0f, float rangeMax=0.0f, float voxelSize=0.0f, int normalK=0, float normalRadius=0.0f, bool forceGroundNormalsUp=false)
Definition: util3d_filtering.cpp:74
tango_gl::kIndices
static const GLushort kIndices[]
Definition: video_overlay.cpp:28
rtabmap::util3d::laserScanToPointCloudI
pcl::PointCloud< pcl::PointXYZI >::Ptr RTABMAP_EXP laserScanToPointCloudI(const LaserScan &laserScan, const Transform &transform=Transform(), float intensity=0.0f)
Definition: util3d.cpp:2381
rtabmap::Transform::toEigen3f
Eigen::Affine3f toEigen3f() const
Definition: Transform.cpp:344
rtabmap::util3d::voxelize
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP voxelize(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, float voxelSize)
Definition: util3d_filtering.cpp:407
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:350
rtabmap::util3d::extractIndicesImpl
pcl::IndicesPtr extractIndicesImpl(const typename pcl::PointCloud< PointT >::Ptr &cloud, const pcl::IndicesPtr &indices, bool negative)
Definition: util3d_filtering.cpp:1632
rtabmap::util3d::laserScanToPointCloudRGB
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_EXP laserScanToPointCloudRGB(const LaserScan &laserScan, const Transform &transform=Transform(), unsigned char r=255, unsigned char g=255, unsigned char b=255)
Definition: util3d.cpp:2363
rtabmap::LaserScan::hasIntensity
bool hasIntensity() const
Definition: LaserScan.h:75
util3d_surface.h
rtabmap::util3d::normalFiltering
pcl::IndicesPtr RTABMAP_EXP normalFiltering(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, float angleMax, const Eigen::Vector4f &normal, int normalKSearch, const Eigen::Vector4f &viewpoint)
Definition: util3d_filtering.cpp:1351

rtabmap
Author(s): Mathieu Labbe
autogenerated on Wed Jun 5 2019 22:43:41