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  float groundNormalsUp)
83 {
84  LaserScan scan = scanIn;
85  UDEBUG("scan size=%d format=%d, step=%d, rangeMin=%f, rangeMax=%f, voxel=%f, normalK=%d, normalRadius=%f, groundNormalsUp=%f",
86  scan.size(), (int)scan.format(), downsamplingStep, rangeMin, rangeMax, voxelSize, normalK, normalRadius, groundNormalsUp);
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  if(scan.angleIncrement() > 0.0f)
134  {
135  scan = LaserScan(
136  cv::Mat(tmp, cv::Range::all(), cv::Range(0, oi)),
137  scan.format(),
138  rangeMin>0.0f&&rangeMin>scan.rangeMin()?rangeMin:scan.rangeMin(),
139  rangeMax>0.0f&&rangeMax<scan.rangeMax()?rangeMax:scan.rangeMax(),
140  scan.angleMin(),
141  scan.angleMax(),
142  scan.angleIncrement() * (float)downsamplingStep,
143  scan.localTransform());
144  }
145  else
146  {
147  scan = LaserScan(
148  cv::Mat(tmp, cv::Range::all(), cv::Range(0, oi)),
149  scanMaxPtsTmp/downsamplingStep,
150  rangeMax>0.0f&&rangeMax<scan.rangeMax()?rangeMax:scan.rangeMax(),
151  scan.format(),
152  scan.localTransform());
153  }
154  UDEBUG("Downsampling scan (step=%d): %d -> %d (scanMaxPts=%d->%d)", downsamplingStep, previousSize, scan.size(), scanMaxPtsTmp, scan.maxPoints());
155  }
156 
157  if(scan.size() && (voxelSize > 0.0f || ((normalK > 0 || normalRadius>0.0f) && !scan.hasNormals())))
158  {
159  // convert to compatible PCL format and filter it
160  if(scan.hasRGB())
161  {
162  UASSERT(!scan.is2d());
163  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = laserScanToPointCloudRGB(scan);
164  if(cloud->size())
165  {
166  int scanMaxPts = scan.maxPoints();
167  if(voxelSize > 0.0f)
168  {
169  cloud = voxelize(cloud, voxelSize);
170  float ratio = float(cloud->size()) / scan.size();
171  scanMaxPts = int(float(scanMaxPts) * ratio);
172  UDEBUG("Voxel filtering scan (voxel=%f m): %d -> %d (scanMaxPts=%d->%d)", voxelSize, scan.size(), cloud->size(), scan.maxPoints(), scanMaxPts);
173  }
174  if(cloud->size() && (normalK > 0 || normalRadius>0.0f))
175  {
176  pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, normalK, normalRadius);
177  scan = LaserScan(laserScanFromPointCloud(*cloud, *normals), scanMaxPts, scan.rangeMax(), LaserScan::kXYZRGBNormal, scan.localTransform());
178  UDEBUG("Normals computed (k=%d radius=%f)", normalK, normalRadius);
179  }
180  else
181  {
182  if(scan.hasNormals())
183  {
184  UWARN("Voxel filter is applied, but normal parameters are not set and input scan has normals. The returned scan has no normals.");
185  }
186  scan = LaserScan(laserScanFromPointCloud(*cloud), scanMaxPts, scan.rangeMax(), LaserScan::kXYZRGB, scan.localTransform());
187  }
188  }
189  }
190  else if(scan.hasIntensity())
191  {
192  pcl::PointCloud<pcl::PointXYZI>::Ptr cloud = laserScanToPointCloudI(scan);
193  if(cloud->size())
194  {
195  int scanMaxPts = scan.maxPoints();
196  if(voxelSize > 0.0f)
197  {
198  cloud = voxelize(cloud, voxelSize);
199  float ratio = float(cloud->size()) / scan.size();
200  scanMaxPts = int(float(scanMaxPts) * ratio);
201  UDEBUG("Voxel filtering scan (voxel=%f m): %d -> %d (scanMaxPts=%d->%d)", voxelSize, scan.size(), cloud->size(), scan.maxPoints(), scanMaxPts);
202  }
203  if(cloud->size() && (normalK > 0 || normalRadius>0.0f))
204  {
205  pcl::PointCloud<pcl::Normal>::Ptr normals;
206  if(scan.is2d())
207  {
208  normals = util3d::computeNormals2D(cloud, normalK, normalRadius);
209  if(voxelSize == 0.0f && scan.angleIncrement() > 0.0f)
210  {
211  scan = LaserScan(laserScan2dFromPointCloud(*cloud, *normals), LaserScan::kXYINormal, scan.rangeMin(), scan.rangeMax(), scan.angleMin(), scan.angleMax(), scan.angleIncrement(), scan.localTransform());
212  }
213  else
214  {
215  scan = LaserScan(laserScan2dFromPointCloud(*cloud, *normals), scanMaxPts, scan.rangeMax(), LaserScan::kXYINormal, scan.localTransform());
216  }
217  }
218  else
219  {
220  normals = util3d::computeNormals(cloud, normalK, normalRadius);
221  scan = LaserScan(laserScanFromPointCloud(*cloud, *normals), scanMaxPts, scan.rangeMax(), LaserScan::kXYZINormal, scan.localTransform());
222  }
223  UDEBUG("Normals computed (k=%d radius=%f)", normalK, normalRadius);
224  }
225  else
226  {
227  if(scan.hasNormals())
228  {
229  UWARN("Voxel filter i applied, but normal parameters are not set and input scan has normals. The returned scan has no normals.");
230  }
231  if(scan.is2d())
232  {
233  scan = LaserScan(laserScan2dFromPointCloud(*cloud), scanMaxPts, scan.rangeMax(), LaserScan::kXYI, scan.localTransform());
234  }
235  else
236  {
237  scan = LaserScan(laserScanFromPointCloud(*cloud), scanMaxPts, scan.rangeMax(), LaserScan::kXYZI, scan.localTransform());
238  }
239  }
240  }
241  }
242  else
243  {
244  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = laserScanToPointCloud(scan);
245  if(cloud->size())
246  {
247  int scanMaxPts = scan.maxPoints();
248  if(voxelSize > 0.0f)
249  {
250  cloud = voxelize(cloud, voxelSize);
251  float ratio = float(cloud->size()) / scan.size();
252  scanMaxPts = int(float(scanMaxPts) * ratio);
253  UDEBUG("Voxel filtering scan (voxel=%f m): %d -> %d (scanMaxPts=%d->%d)", voxelSize, scan.size(), cloud->size(), scan.maxPoints(), scanMaxPts);
254  }
255  if(cloud->size() && (normalK > 0 || normalRadius>0.0f))
256  {
257  pcl::PointCloud<pcl::Normal>::Ptr normals;
258  if(scan.is2d())
259  {
260  normals = util3d::computeNormals2D(cloud, normalK, normalRadius);
261  if(voxelSize == 0.0f && scan.angleIncrement() > 0.0f)
262  {
263  scan = LaserScan(laserScan2dFromPointCloud(*cloud, *normals), LaserScan::kXYNormal, scan.rangeMin(), scan.rangeMax(), scan.angleMin(), scan.angleMax(), scan.angleIncrement(), scan.localTransform());
264  }
265  else
266  {
267  scan = LaserScan(laserScan2dFromPointCloud(*cloud, *normals), scanMaxPts, scan.rangeMax(), LaserScan::kXYNormal, scan.localTransform());
268  }
269  }
270  else
271  {
272  normals = util3d::computeNormals(cloud, normalK, normalRadius);
273  scan = LaserScan(laserScanFromPointCloud(*cloud, *normals), scanMaxPts, scan.rangeMax(), LaserScan::kXYZNormal, scan.localTransform());
274  }
275  UDEBUG("Normals computed (k=%d radius=%f)", normalK, normalRadius);
276  }
277  else
278  {
279  if(scan.hasNormals())
280  {
281  UWARN("Voxel filter i applied, but normal parameters are not set and input scan has normals. The returned scan has no normals.");
282  }
283  if(scan.is2d())
284  {
285  scan = LaserScan(laserScan2dFromPointCloud(*cloud), scanMaxPts, scan.rangeMax(), LaserScan::kXY, scan.localTransform());
286  }
287  else
288  {
289  scan = LaserScan(laserScanFromPointCloud(*cloud), scanMaxPts, scan.rangeMax(), LaserScan::kXYZ, scan.localTransform());
290  }
291  }
292  }
293  }
294  }
295 
296  if(scan.size() && !scan.is2d() && scan.hasNormals() && groundNormalsUp>0.0f)
297  {
298  scan = util3d::adjustNormalsToViewPoint(scan, Eigen::Vector3f(0,0,10), groundNormalsUp);
299  }
300  }
301  return scan;
302 }
303 
304 LaserScan commonFiltering(
305  const LaserScan & scanIn,
306  int downsamplingStep,
307  float rangeMin,
308  float rangeMax,
309  float voxelSize,
310  int normalK,
311  float normalRadius,
312  bool forceGroundNormalsUp)
313 {
314  return commonFiltering(scanIn, downsamplingStep, rangeMin, rangeMax, voxelSize, normalK, normalRadius, forceGroundNormalsUp?0.8f:0.0f);
315 }
316 
317 LaserScan rangeFiltering(
318  const LaserScan & scan,
319  float rangeMin,
320  float rangeMax)
321 {
322  UASSERT(rangeMin >=0.0f && rangeMax>=0.0f);
323  if(!scan.isEmpty())
324  {
325  if(rangeMin > 0.0f || rangeMax > 0.0f)
326  {
327  cv::Mat output = cv::Mat(1, scan.size(), scan.dataType());
328  bool is2d = scan.is2d();
329  int oi = 0;
330  float rangeMinSqrd = rangeMin * rangeMin;
331  float rangeMaxSqrd = rangeMax * rangeMax;
332  for(int i=0; i<scan.size(); ++i)
333  {
334  const float * ptr = scan.data().ptr<float>(0, i);
335  float r;
336  if(is2d)
337  {
338  r = ptr[0]*ptr[0] + ptr[1]*ptr[1];
339  }
340  else
341  {
342  r = ptr[0]*ptr[0] + ptr[1]*ptr[1] + ptr[2]*ptr[2];
343  }
344 
345  if(rangeMin > 0.0f && r < rangeMinSqrd)
346  {
347  continue;
348  }
349  if(rangeMax > 0.0f && r > rangeMaxSqrd)
350  {
351  continue;
352  }
353 
354  cv::Mat(scan.data(), cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(output, cv::Range::all(), cv::Range(oi,oi+1)));
355  ++oi;
356  }
357  if(scan.angleIncrement() > 0.0f)
358  {
359  return LaserScan(cv::Mat(output, cv::Range::all(), cv::Range(0, oi)), scan.format(), scan.rangeMin(), scan.rangeMax(), scan.angleMin(), scan.angleMax(), scan.angleIncrement(), scan.localTransform());
360  }
361  return LaserScan(cv::Mat(output, cv::Range::all(), cv::Range(0, oi)), scan.maxPoints(), scan.rangeMax(), scan.format(), scan.localTransform());
362  }
363  }
364 
365  return scan;
366 }
367 
368 LaserScan downsample(
369  const LaserScan & scan,
370  int step)
371 {
372  UASSERT(step > 0);
373  if(step <= 1 || scan.size() <= step)
374  {
375  // no sampling
376  return scan;
377  }
378  else
379  {
380  int finalSize = scan.size()/step;
381  cv::Mat output = cv::Mat(1, finalSize, scan.dataType());
382  int oi = 0;
383  for(int i=0; i<scan.size()-step+1; i+=step)
384  {
385  cv::Mat(scan.data(), cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(output, cv::Range::all(), cv::Range(oi,oi+1)));
386  ++oi;
387  }
388  if(scan.angleIncrement() > 0.0f)
389  {
390  return LaserScan(output, scan.format(), scan.rangeMin(), scan.rangeMax(), scan.angleMin(), scan.angleMax(), scan.angleIncrement()*step, scan.localTransform());
391  }
392  return LaserScan(output, scan.maxPoints()/step, scan.rangeMax(), scan.format(), scan.localTransform());
393  }
394 }
395 template<typename PointT>
396 typename pcl::PointCloud<PointT>::Ptr downsampleImpl(
397  const typename pcl::PointCloud<PointT>::Ptr & cloud,
398  int step)
399 {
400  UASSERT(step > 0);
401  typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
402  if(step <= 1 || (int)cloud->size() <= step)
403  {
404  // no sampling
405  *output = *cloud;
406  }
407  else
408  {
409  if(cloud->height > 1 && cloud->height < cloud->width/4)
410  {
411  // Assuming ouster point cloud (e.g, 2048x64),
412  // for which the lower dimension is the number of rings.
413  // Downsample each ring by the step.
414  // Example data packed:
415  // <ringA-1, ringB-1, ringC-1, ringD-1;
416  // ringA-2, ringB-2, ringC-2, ringD-2;
417  // ringA-3, ringB-3, ringC-3, ringD-3;
418  // ringA-4, ringB-4, ringC-4, ringD-4>
419  unsigned int rings = cloud->height<cloud->width?cloud->height:cloud->width;
420  unsigned int pts = cloud->height>cloud->width?cloud->height:cloud->width;
421  unsigned int finalSize = rings * pts/step;
422  output->resize(finalSize);
423  output->width = rings;
424  output->height = pts/step;
425 
426  for(unsigned int j=0; j<rings; ++j)
427  {
428  for(unsigned int i=0; i<output->height; ++i)
429  {
430  (*output)[i*rings + j] = cloud->at(i*step*rings + j);
431  }
432  }
433 
434  }
435  else if(cloud->height > 1)
436  {
437  // assume depth image (e.g., 640x480), downsample like an image
438  UASSERT_MSG(cloud->height % step == 0 && cloud->width % step == 0,
439  uFormat("Decimation of depth images should be exact! (decimation=%d, size=%dx%d)",
440  step, cloud->width, cloud->height).c_str());
441 
442  int finalSize = cloud->height/step * cloud->width/step;
443  output->resize(finalSize);
444  output->width = cloud->width/step;
445  output->height = cloud->height/step;
446 
447  for(unsigned int j=0; j<output->height; ++j)
448  {
449  for(unsigned int i=0; i<output->width; ++i)
450  {
451  output->at(j*output->width + i) = cloud->at(j*output->width*step + i*step);
452  }
453  }
454  }
455  else
456  {
457  int finalSize = int(cloud->size())/step;
458  output->resize(finalSize);
459  int oi = 0;
460  for(unsigned int i=0; i<cloud->size()-step+1; i+=step)
461  {
462  (*output)[oi++] = cloud->at(i);
463  }
464  }
465  }
466  return output;
467 }
468 pcl::PointCloud<pcl::PointXYZ>::Ptr downsample(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, int step)
469 {
470  return downsampleImpl<pcl::PointXYZ>(cloud, step);
471 }
472 pcl::PointCloud<pcl::PointXYZRGB>::Ptr downsample(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, int step)
473 {
474  return downsampleImpl<pcl::PointXYZRGB>(cloud, step);
475 }
476 pcl::PointCloud<pcl::PointXYZI>::Ptr downsample(const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud, int step)
477 {
478  return downsampleImpl<pcl::PointXYZI>(cloud, step);
479 }
480 pcl::PointCloud<pcl::PointNormal>::Ptr downsample(const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, int step)
481 {
482  return downsampleImpl<pcl::PointNormal>(cloud, step);
483 }
484 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr downsample(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, int step)
485 {
486  return downsampleImpl<pcl::PointXYZRGBNormal>(cloud, step);
487 }
488 pcl::PointCloud<pcl::PointXYZINormal>::Ptr downsample(const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud, int step)
489 {
490  return downsampleImpl<pcl::PointXYZINormal>(cloud, step);
491 }
492 
493 template<typename PointT>
494 typename pcl::PointCloud<PointT>::Ptr voxelizeImpl(
495  const typename pcl::PointCloud<PointT>::Ptr & cloud,
496  const pcl::IndicesPtr & indices,
497  float voxelSize)
498 {
499  UASSERT(voxelSize > 0.0f);
500  typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
501  if((cloud->is_dense && cloud->size()) || (!cloud->is_dense && indices->size()))
502  {
503  pcl::VoxelGrid<PointT> filter;
504  filter.setLeafSize(voxelSize, voxelSize, voxelSize);
505  filter.setInputCloud(cloud);
506  if(indices->size())
507  {
508  filter.setIndices(indices);
509  }
510  filter.filter(*output);
511  }
512  else if(cloud->size() && !cloud->is_dense && indices->size() == 0)
513  {
514  UWARN("Cannot voxelize a not dense (organized) cloud with empty indices! (input=%d pts). Returning empty cloud!", (int)cloud->size());
515  }
516  return output;
517 }
518 
519 pcl::PointCloud<pcl::PointXYZ>::Ptr voxelize(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, const pcl::IndicesPtr & indices, float voxelSize)
520 {
521  return voxelizeImpl<pcl::PointXYZ>(cloud, indices, voxelSize);
522 }
523 pcl::PointCloud<pcl::PointNormal>::Ptr voxelize(const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, const pcl::IndicesPtr & indices, float voxelSize)
524 {
525  return voxelizeImpl<pcl::PointNormal>(cloud, indices, voxelSize);
526 }
527 pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelize(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices, float voxelSize)
528 {
529  return voxelizeImpl<pcl::PointXYZRGB>(cloud, indices, voxelSize);
530 }
531 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr voxelize(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, const pcl::IndicesPtr & indices, float voxelSize)
532 {
533  return voxelizeImpl<pcl::PointXYZRGBNormal>(cloud, indices, voxelSize);
534 }
535 pcl::PointCloud<pcl::PointXYZI>::Ptr voxelize(const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud, const pcl::IndicesPtr & indices, float voxelSize)
536 {
537  return voxelizeImpl<pcl::PointXYZI>(cloud, indices, voxelSize);
538 }
539 pcl::PointCloud<pcl::PointXYZINormal>::Ptr voxelize(const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud, const pcl::IndicesPtr & indices, float voxelSize)
540 {
541  return voxelizeImpl<pcl::PointXYZINormal>(cloud, indices, voxelSize);
542 }
543 
544 pcl::PointCloud<pcl::PointXYZ>::Ptr voxelize(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, float voxelSize)
545 {
546  pcl::IndicesPtr indices(new std::vector<int>);
547  return voxelize(cloud, indices, voxelSize);
548 }
549 pcl::PointCloud<pcl::PointNormal>::Ptr voxelize(const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, float voxelSize)
550 {
551  pcl::IndicesPtr indices(new std::vector<int>);
552  return voxelize(cloud, indices, voxelSize);
553 }
554 pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelize(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, float voxelSize)
555 {
556  pcl::IndicesPtr indices(new std::vector<int>);
557  return voxelize(cloud, indices, voxelSize);
558 }
559 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr voxelize(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, float voxelSize)
560 {
561  pcl::IndicesPtr indices(new std::vector<int>);
562  return voxelize(cloud, indices, voxelSize);
563 }
564 pcl::PointCloud<pcl::PointXYZI>::Ptr voxelize(const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud, float voxelSize)
565 {
566  pcl::IndicesPtr indices(new std::vector<int>);
567  return voxelize(cloud, indices, voxelSize);
568 }
569 pcl::PointCloud<pcl::PointXYZINormal>::Ptr voxelize(const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud, float voxelSize)
570 {
571  pcl::IndicesPtr indices(new std::vector<int>);
572  return voxelize(cloud, indices, voxelSize);
573 }
574 
575 template<typename PointT>
576 typename pcl::PointCloud<PointT>::Ptr randomSamplingImpl(
577  const typename pcl::PointCloud<PointT>::Ptr & cloud, int samples)
578 {
579  UASSERT(samples > 0);
580  typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
581  pcl::RandomSample<PointT> filter;
582  filter.setSample(samples);
583  filter.setInputCloud(cloud);
584  filter.filter(*output);
585  return output;
586 }
587 pcl::PointCloud<pcl::PointXYZ>::Ptr randomSampling(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, int samples)
588 {
589  return randomSamplingImpl<pcl::PointXYZ>(cloud, samples);
590 }
591 pcl::PointCloud<pcl::PointXYZRGB>::Ptr randomSampling(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, int samples)
592 {
593  return randomSamplingImpl<pcl::PointXYZRGB>(cloud, samples);
594 }
595 
596 template<typename PointT>
597 pcl::IndicesPtr passThroughImpl(
598  const typename pcl::PointCloud<PointT>::Ptr & cloud,
599  const pcl::IndicesPtr & indices,
600  const std::string & axis,
601  float min,
602  float max,
603  bool negative)
604 {
605  UASSERT_MSG(max > min, uFormat("cloud=%d, max=%f min=%f axis=%s", (int)cloud->size(), max, min, axis.c_str()).c_str());
606  UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0);
607 
608  pcl::IndicesPtr output(new std::vector<int>);
609  pcl::PassThrough<PointT> filter;
610  filter.setNegative(negative);
611  filter.setFilterFieldName(axis);
612  filter.setFilterLimits(min, max);
613  filter.setInputCloud(cloud);
614  filter.setIndices(indices);
615  filter.filter(*output);
616  return output;
617 }
618 
619 pcl::IndicesPtr passThrough(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, const pcl::IndicesPtr & indices, const std::string & axis, float min, float max, bool negative)
620 {
621  return passThroughImpl<pcl::PointXYZ>(cloud, indices, axis, min, max, negative);
622 }
623 pcl::IndicesPtr passThrough(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices, const std::string & axis, float min, float max, bool negative)
624 {
625  return passThroughImpl<pcl::PointXYZRGB>(cloud, indices, axis, min, max, negative);
626 }
627 pcl::IndicesPtr passThrough(const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud, const pcl::IndicesPtr & indices, const std::string & axis, float min, float max, bool negative)
628 {
629  return passThroughImpl<pcl::PointXYZI>(cloud, indices, axis, min, max, negative);
630 }
631 pcl::IndicesPtr passThrough(const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, const pcl::IndicesPtr & indices, const std::string & axis, float min, float max, bool negative)
632 {
633  return passThroughImpl<pcl::PointNormal>(cloud, indices, axis, min, max, negative);
634 }
635 pcl::IndicesPtr passThrough(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, const pcl::IndicesPtr & indices, const std::string & axis, float min, float max, bool negative)
636 {
637  return passThroughImpl<pcl::PointXYZRGBNormal>(cloud, indices, axis, min, max, negative);
638 }
639 pcl::IndicesPtr passThrough(const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud, const pcl::IndicesPtr & indices, const std::string & axis, float min, float max, bool negative)
640 {
641  return passThroughImpl<pcl::PointXYZINormal>(cloud, indices, axis, min, max, negative);
642 }
643 
644 template<typename PointT>
645 typename pcl::PointCloud<PointT>::Ptr passThroughImpl(
646  const typename pcl::PointCloud<PointT>::Ptr & cloud,
647  const std::string & axis,
648  float min,
649  float max,
650  bool negative)
651 {
652  UASSERT_MSG(max > min, uFormat("cloud=%d, max=%f min=%f axis=%s", (int)cloud->size(), max, min, axis.c_str()).c_str());
653  UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0);
654 
655  typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
656  pcl::PassThrough<PointT> filter;
657  filter.setNegative(negative);
658  filter.setFilterFieldName(axis);
659  filter.setFilterLimits(min, max);
660  filter.setInputCloud(cloud);
661  filter.filter(*output);
662  return output;
663 }
664 pcl::PointCloud<pcl::PointXYZ>::Ptr passThrough(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, const std::string & axis, float min, float max, bool negative)
665 {
666  return passThroughImpl<pcl::PointXYZ>(cloud, axis, min ,max, negative);
667 }
668 pcl::PointCloud<pcl::PointXYZRGB>::Ptr passThrough(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const std::string & axis, float min, float max, bool negative)
669 {
670  return passThroughImpl<pcl::PointXYZRGB>(cloud, axis, min ,max, negative);
671 }
672 pcl::PointCloud<pcl::PointXYZI>::Ptr passThrough(const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud, const std::string & axis, float min, float max, bool negative)
673 {
674  return passThroughImpl<pcl::PointXYZI>(cloud, axis, min ,max, negative);
675 }
676 pcl::PointCloud<pcl::PointNormal>::Ptr passThrough(const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, const std::string & axis, float min, float max, bool negative)
677 {
678  return passThroughImpl<pcl::PointNormal>(cloud, axis, min ,max, negative);
679 }
680 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr passThrough(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, const std::string & axis, float min, float max, bool negative)
681 {
682  return passThroughImpl<pcl::PointXYZRGBNormal>(cloud, axis, min ,max, negative);
683 }
684 pcl::PointCloud<pcl::PointXYZINormal>::Ptr passThrough(const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud, const std::string & axis, float min, float max, bool negative)
685 {
686  return passThroughImpl<pcl::PointXYZINormal>(cloud, axis, min ,max, negative);
687 }
688 
689 template<typename PointT>
690 pcl::IndicesPtr cropBoxImpl(
691  const typename pcl::PointCloud<PointT>::Ptr & cloud,
692  const pcl::IndicesPtr & indices,
693  const Eigen::Vector4f & min,
694  const Eigen::Vector4f & max,
695  const Transform & transform,
696  bool negative)
697 {
698  UASSERT(min[0] < max[0] && min[1] < max[1] && min[2] < max[2]);
699 
700  pcl::IndicesPtr output(new std::vector<int>);
701  pcl::CropBox<PointT> filter;
702  filter.setNegative(negative);
703  filter.setMin(min);
704  filter.setMax(max);
705  if(!transform.isNull() && !transform.isIdentity())
706  {
707  filter.setTransform(transform.toEigen3f());
708  }
709  filter.setInputCloud(cloud);
710  filter.setIndices(indices);
711  filter.filter(*output);
712  return output;
713 }
714 
715 pcl::IndicesPtr cropBox(const pcl::PCLPointCloud2::Ptr & cloud, const pcl::IndicesPtr & indices, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform, bool negative)
716 {
717  UASSERT(min[0] < max[0] && min[1] < max[1] && min[2] < max[2]);
718 
719  pcl::IndicesPtr output(new std::vector<int>);
720  pcl::CropBox<pcl::PCLPointCloud2> filter;
721  filter.setNegative(negative);
722  filter.setMin(min);
723  filter.setMax(max);
724  if(!transform.isNull() && !transform.isIdentity())
725  {
726  filter.setTransform(transform.toEigen3f());
727  }
728  filter.setInputCloud(cloud);
729  filter.setIndices(indices);
730  filter.filter(*output);
731  return output;
732 }
733 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)
734 {
735  return cropBoxImpl<pcl::PointXYZ>(cloud, indices, min, max, transform, negative);
736 }
737 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)
738 {
739  return cropBoxImpl<pcl::PointNormal>(cloud, indices, min, max, transform, negative);
740 }
741 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)
742 {
743  return cropBoxImpl<pcl::PointXYZRGB>(cloud, indices, min, max, transform, negative);
744 }
745 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)
746 {
747  return cropBoxImpl<pcl::PointXYZRGBNormal>(cloud, indices, min, max, transform, negative);
748 }
749 
750 template<typename PointT>
751 typename pcl::PointCloud<PointT>::Ptr cropBoxImpl(
752  const typename pcl::PointCloud<PointT>::Ptr & cloud,
753  const Eigen::Vector4f & min,
754  const Eigen::Vector4f & max,
755  const Transform & transform,
756  bool negative)
757 {
758  UASSERT(min[0] < max[0] && min[1] < max[1] && min[2] < max[2]);
759 
760  typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
761  pcl::CropBox<PointT> filter;
762  filter.setNegative(negative);
763  filter.setMin(min);
764  filter.setMax(max);
765  if(!transform.isNull() && !transform.isIdentity())
766  {
767  filter.setTransform(transform.toEigen3f());
768  }
769  filter.setInputCloud(cloud);
770  filter.filter(*output);
771  return output;
772 }
773 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)
774 {
775  return cropBoxImpl<pcl::PointXYZ>(cloud, min, max, transform, negative);
776 }
777 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)
778 {
779  return cropBoxImpl<pcl::PointNormal>(cloud, min, max, transform, negative);
780 }
781 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)
782 {
783  return cropBoxImpl<pcl::PointXYZRGB>(cloud, min, max, transform, negative);
784 }
785 pcl::PointCloud<pcl::PointXYZINormal>::Ptr cropBox(const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform, bool negative)
786 {
787  return cropBoxImpl<pcl::PointXYZINormal>(cloud, min, max, transform, negative);
788 }
789 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)
790 {
791  return cropBoxImpl<pcl::PointXYZRGBNormal>(cloud, min, max, transform, negative);
792 }
793 
794 template<typename PointT>
795 pcl::IndicesPtr frustumFilteringImpl(
796  const typename pcl::PointCloud<PointT>::Ptr & cloud,
797  const pcl::IndicesPtr & indices,
798  const Transform & cameraPose,
799  float horizontalFOV, // in degrees
800  float verticalFOV, // in degrees
801  float nearClipPlaneDistance,
802  float farClipPlaneDistance,
803  bool negative)
804 {
805  UASSERT(horizontalFOV > 0.0f && verticalFOV > 0.0f);
806  UASSERT(farClipPlaneDistance > nearClipPlaneDistance);
807  UASSERT(!cameraPose.isNull());
808 
809  pcl::IndicesPtr output(new std::vector<int>);
810  pcl::FrustumCulling<PointT> fc;
811  fc.setNegative(negative);
812  fc.setInputCloud (cloud);
813  if(indices.get() && indices->size())
814  {
815  fc.setIndices(indices);
816  }
817  fc.setVerticalFOV (verticalFOV);
818  fc.setHorizontalFOV (horizontalFOV);
819  fc.setNearPlaneDistance (nearClipPlaneDistance);
820  fc.setFarPlaneDistance (farClipPlaneDistance);
821 
822  fc.setCameraPose (cameraPose.toEigen4f());
823  fc.filter (*output);
824 
825  return output;
826 }
827 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)
828 {
829  return frustumFilteringImpl<pcl::PointXYZ>(cloud, indices, cameraPose, horizontalFOV, verticalFOV, nearClipPlaneDistance, farClipPlaneDistance, negative);
830 }
831 
832 template<typename PointT>
833 typename pcl::PointCloud<PointT>::Ptr frustumFilteringImpl(
834  const typename pcl::PointCloud<PointT>::Ptr & cloud,
835  const Transform & cameraPose,
836  float horizontalFOV, // in degrees
837  float verticalFOV, // in degrees
838  float nearClipPlaneDistance,
839  float farClipPlaneDistance,
840  bool negative)
841 {
842  UASSERT(horizontalFOV > 0.0f && verticalFOV > 0.0f);
843  UASSERT(farClipPlaneDistance > nearClipPlaneDistance);
844  UASSERT(!cameraPose.isNull());
845 
846  typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
847  pcl::FrustumCulling<PointT> fc;
848  fc.setNegative(negative);
849  fc.setInputCloud (cloud);
850  fc.setVerticalFOV (verticalFOV);
851  fc.setHorizontalFOV (horizontalFOV);
852  fc.setNearPlaneDistance (nearClipPlaneDistance);
853  fc.setFarPlaneDistance (farClipPlaneDistance);
854 
855  fc.setCameraPose (cameraPose.toEigen4f());
856  fc.filter (*output);
857 
858  return output;
859 }
860 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)
861 {
862  return frustumFilteringImpl<pcl::PointXYZ>(cloud, cameraPose, horizontalFOV, verticalFOV, nearClipPlaneDistance, farClipPlaneDistance, negative);
863 }
864 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)
865 {
866  return frustumFilteringImpl<pcl::PointXYZRGB>(cloud, cameraPose, horizontalFOV, verticalFOV, nearClipPlaneDistance, farClipPlaneDistance, negative);
867 }
868 
869 
870 template<typename PointT>
871 typename pcl::PointCloud<PointT>::Ptr removeNaNFromPointCloudImpl(
872  const typename pcl::PointCloud<PointT>::Ptr & cloud)
873 {
874  typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
875  std::vector<int> indices;
876  pcl::removeNaNFromPointCloud(*cloud, *output, indices);
877  return output;
878 }
879 pcl::PointCloud<pcl::PointXYZ>::Ptr removeNaNFromPointCloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud)
880 {
881  return removeNaNFromPointCloudImpl<pcl::PointXYZ>(cloud);
882 }
883 pcl::PointCloud<pcl::PointXYZRGB>::Ptr removeNaNFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud)
884 {
885  return removeNaNFromPointCloudImpl<pcl::PointXYZRGB>(cloud);
886 }
887 pcl::PointCloud<pcl::PointXYZI>::Ptr removeNaNFromPointCloud(const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud)
888 {
889  return removeNaNFromPointCloudImpl<pcl::PointXYZI>(cloud);
890 }
891 
892 template<typename PointT>
893 typename pcl::PointCloud<PointT>::Ptr removeNaNNormalsFromPointCloudImpl(
894  const typename pcl::PointCloud<PointT>::Ptr & cloud)
895 {
896  typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
897  std::vector<int> indices;
898  pcl::removeNaNNormalsFromPointCloud(*cloud, *output, indices);
899  return output;
900 }
901 pcl::PointCloud<pcl::PointNormal>::Ptr removeNaNNormalsFromPointCloud(
902  const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud)
903 {
904  return removeNaNNormalsFromPointCloudImpl<pcl::PointNormal>(cloud);
905 }
906 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr removeNaNNormalsFromPointCloud(
907  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud)
908 {
909  return removeNaNNormalsFromPointCloudImpl<pcl::PointXYZRGBNormal>(cloud);
910 }
911 pcl::PointCloud<pcl::PointXYZINormal>::Ptr removeNaNNormalsFromPointCloud(
912  const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud)
913 {
914  return removeNaNNormalsFromPointCloudImpl<pcl::PointXYZINormal>(cloud);
915 }
916 
917 
918 pcl::IndicesPtr radiusFiltering(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, float radiusSearch, int minNeighborsInRadius)
919 {
920  pcl::IndicesPtr indices(new std::vector<int>);
921  return radiusFiltering(cloud, indices, radiusSearch, minNeighborsInRadius);
922 }
923 pcl::IndicesPtr radiusFiltering(const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, float radiusSearch, int minNeighborsInRadius)
924 {
925  pcl::IndicesPtr indices(new std::vector<int>);
926  return radiusFiltering(cloud, indices, radiusSearch, minNeighborsInRadius);
927 }
928 pcl::IndicesPtr radiusFiltering(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, float radiusSearch, int minNeighborsInRadius)
929 {
930  pcl::IndicesPtr indices(new std::vector<int>);
931  return radiusFiltering(cloud, indices, radiusSearch, minNeighborsInRadius);
932 }
933 pcl::IndicesPtr radiusFiltering(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, float radiusSearch, int minNeighborsInRadius)
934 {
935  pcl::IndicesPtr indices(new std::vector<int>);
936  return radiusFiltering(cloud, indices, radiusSearch, minNeighborsInRadius);
937 }
938 
939 template<typename PointT>
940 pcl::IndicesPtr radiusFilteringImpl(
941  const typename pcl::PointCloud<PointT>::Ptr & cloud,
942  const pcl::IndicesPtr & indices,
943  float radiusSearch,
944  int minNeighborsInRadius)
945 {
946  typename pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>(false));
947 
948  if(indices->size())
949  {
950  pcl::IndicesPtr output(new std::vector<int>(indices->size()));
951  int oi = 0; // output iterator
952  tree->setInputCloud(cloud, indices);
953  for(unsigned int i=0; i<indices->size(); ++i)
954  {
955  std::vector<int> kIndices;
956  std::vector<float> kDistances;
957  int k = tree->radiusSearch(cloud->at(indices->at(i)), radiusSearch, kIndices, kDistances);
958  if(k > minNeighborsInRadius)
959  {
960  output->at(oi++) = indices->at(i);
961  }
962  }
963  output->resize(oi);
964  return output;
965  }
966  else
967  {
968  pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
969  int oi = 0; // output iterator
970  tree->setInputCloud(cloud);
971  for(unsigned int i=0; i<cloud->size(); ++i)
972  {
973  std::vector<int> kIndices;
974  std::vector<float> kDistances;
975  int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances);
976  if(k > minNeighborsInRadius)
977  {
978  output->at(oi++) = i;
979  }
980  }
981  output->resize(oi);
982  return output;
983  }
984 }
985 
986 pcl::IndicesPtr radiusFiltering(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, const pcl::IndicesPtr & indices, float radiusSearch, int minNeighborsInRadius)
987 {
988  return radiusFilteringImpl<pcl::PointXYZ>(cloud, indices, radiusSearch, minNeighborsInRadius);
989 }
990 pcl::IndicesPtr radiusFiltering(const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, const pcl::IndicesPtr & indices, float radiusSearch, int minNeighborsInRadius)
991 {
992  return radiusFilteringImpl<pcl::PointNormal>(cloud, indices, radiusSearch, minNeighborsInRadius);
993 }
994 pcl::IndicesPtr radiusFiltering(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices, float radiusSearch, int minNeighborsInRadius)
995 {
996  return radiusFilteringImpl<pcl::PointXYZRGB>(cloud, indices, radiusSearch, minNeighborsInRadius);
997 }
998 pcl::IndicesPtr radiusFiltering(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, const pcl::IndicesPtr & indices, float radiusSearch, int minNeighborsInRadius)
999 {
1000  return radiusFilteringImpl<pcl::PointXYZRGBNormal>(cloud, indices, radiusSearch, minNeighborsInRadius);
1001 }
1002 
1003 pcl::PointCloud<pcl::PointXYZRGB>::Ptr subtractFiltering(
1004  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
1005  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & substractCloud,
1006  float radiusSearch,
1007  int minNeighborsInRadius)
1008 {
1009  pcl::IndicesPtr indices(new std::vector<int>);
1010  pcl::IndicesPtr indicesOut = subtractFiltering(cloud, indices, substractCloud, indices, radiusSearch, minNeighborsInRadius);
1011  pcl::PointCloud<pcl::PointXYZRGB>::Ptr out(new pcl::PointCloud<pcl::PointXYZRGB>);
1012  pcl::copyPointCloud(*cloud, *indicesOut, *out);
1013  return out;
1014 }
1015 
1016 template<typename PointT>
1017 pcl::IndicesPtr subtractFilteringImpl(
1018  const typename pcl::PointCloud<PointT>::Ptr & cloud,
1019  const pcl::IndicesPtr & indices,
1020  const typename pcl::PointCloud<PointT>::Ptr & substractCloud,
1021  const pcl::IndicesPtr & substractIndices,
1022  float radiusSearch,
1023  int minNeighborsInRadius)
1024 {
1025  UASSERT(minNeighborsInRadius > 0);
1026  typename pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>(false));
1027 
1028  if(indices->size())
1029  {
1030  pcl::IndicesPtr output(new std::vector<int>(indices->size()));
1031  int oi = 0; // output iterator
1032  if(substractIndices->size())
1033  {
1034  tree->setInputCloud(substractCloud, substractIndices);
1035  }
1036  else
1037  {
1038  tree->setInputCloud(substractCloud);
1039  }
1040  for(unsigned int i=0; i<indices->size(); ++i)
1041  {
1042  std::vector<int> kIndices;
1043  std::vector<float> kDistances;
1044  int k = tree->radiusSearch(cloud->at(indices->at(i)), radiusSearch, kIndices, kDistances);
1045  if(k < minNeighborsInRadius)
1046  {
1047  output->at(oi++) = indices->at(i);
1048  }
1049  }
1050  output->resize(oi);
1051  return output;
1052  }
1053  else
1054  {
1055  pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
1056  int oi = 0; // output iterator
1057  if(substractIndices->size())
1058  {
1059  tree->setInputCloud(substractCloud, substractIndices);
1060  }
1061  else
1062  {
1063  tree->setInputCloud(substractCloud);
1064  }
1065  for(unsigned int i=0; i<cloud->size(); ++i)
1066  {
1067  std::vector<int> kIndices;
1068  std::vector<float> kDistances;
1069  int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances);
1070  if(k < minNeighborsInRadius)
1071  {
1072  output->at(oi++) = i;
1073  }
1074  }
1075  output->resize(oi);
1076  return output;
1077  }
1078 }
1079 pcl::IndicesPtr subtractFiltering(
1080  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
1081  const pcl::IndicesPtr & indices,
1082  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & substractCloud,
1083  const pcl::IndicesPtr & substractIndices,
1084  float radiusSearch,
1085  int minNeighborsInRadius)
1086 {
1087  return subtractFilteringImpl<pcl::PointXYZRGB>(cloud, indices, substractCloud, substractIndices, radiusSearch, minNeighborsInRadius);
1088 }
1089 
1090 pcl::PointCloud<pcl::PointNormal>::Ptr subtractFiltering(
1091  const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
1092  const pcl::PointCloud<pcl::PointNormal>::Ptr & substractCloud,
1093  float radiusSearch,
1094  float maxAngle,
1095  int minNeighborsInRadius)
1096 {
1097  pcl::IndicesPtr indices(new std::vector<int>);
1098  pcl::IndicesPtr indicesOut = subtractFiltering(cloud, indices, substractCloud, indices, radiusSearch, maxAngle, minNeighborsInRadius);
1099  pcl::PointCloud<pcl::PointNormal>::Ptr out(new pcl::PointCloud<pcl::PointNormal>);
1100  pcl::copyPointCloud(*cloud, *indicesOut, *out);
1101  return out;
1102 }
1103 pcl::PointCloud<pcl::PointXYZINormal>::Ptr subtractFiltering(
1104  const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
1105  const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & substractCloud,
1106  float radiusSearch,
1107  float maxAngle,
1108  int minNeighborsInRadius)
1109 {
1110  pcl::IndicesPtr indices(new std::vector<int>);
1111  pcl::IndicesPtr indicesOut = subtractFiltering(cloud, indices, substractCloud, indices, radiusSearch, maxAngle, minNeighborsInRadius);
1112  pcl::PointCloud<pcl::PointXYZINormal>::Ptr out(new pcl::PointCloud<pcl::PointXYZINormal>);
1113  pcl::copyPointCloud(*cloud, *indicesOut, *out);
1114  return out;
1115 }
1116 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr subtractFiltering(
1117  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
1118  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
1119  float radiusSearch,
1120  float maxAngle,
1121  int minNeighborsInRadius)
1122 {
1123  pcl::IndicesPtr indices(new std::vector<int>);
1124  pcl::IndicesPtr indicesOut = subtractFiltering(cloud, indices, substractCloud, indices, radiusSearch, maxAngle, minNeighborsInRadius);
1125  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr out(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
1126  pcl::copyPointCloud(*cloud, *indicesOut, *out);
1127  return out;
1128 }
1129 
1130 template<typename PointT>
1131 pcl::IndicesPtr subtractFilteringImpl(
1132  const typename pcl::PointCloud<PointT>::Ptr & cloud,
1133  const pcl::IndicesPtr & indices,
1134  const typename pcl::PointCloud<PointT>::Ptr & substractCloud,
1135  const pcl::IndicesPtr & substractIndices,
1136  float radiusSearch,
1137  float maxAngle,
1138  int minNeighborsInRadius)
1139 {
1140  UASSERT(minNeighborsInRadius > 0);
1141  typename pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>(false));
1142 
1143  if(indices->size())
1144  {
1145  pcl::IndicesPtr output(new std::vector<int>(indices->size()));
1146  int oi = 0; // output iterator
1147  if(substractIndices->size())
1148  {
1149  tree->setInputCloud(substractCloud, substractIndices);
1150  }
1151  else
1152  {
1153  tree->setInputCloud(substractCloud);
1154  }
1155  for(unsigned int i=0; i<indices->size(); ++i)
1156  {
1157  std::vector<int> kIndices;
1158  std::vector<float> kDistances;
1159  int k = tree->radiusSearch(cloud->at(indices->at(i)), radiusSearch, kIndices, kDistances);
1160  if(k>=minNeighborsInRadius && maxAngle > 0.0f)
1161  {
1162  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);
1163  if (uIsFinite(normal[0]) &&
1164  uIsFinite(normal[1]) &&
1165  uIsFinite(normal[2]))
1166  {
1167  int count = k;
1168  for(int j=0; j<count && k >= minNeighborsInRadius; ++j)
1169  {
1170  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);
1171  if(uIsFinite(v[0]) &&
1172  uIsFinite(v[1]) &&
1173  uIsFinite(v[2]))
1174  {
1175  float angle = pcl::getAngle3D(normal, v);
1176  if(angle > maxAngle)
1177  {
1178  k-=1;
1179  }
1180  }
1181  else
1182  {
1183  k-=1;
1184  }
1185  }
1186  }
1187  else
1188  {
1189  k=0;
1190  }
1191  }
1192  if(k < minNeighborsInRadius)
1193  {
1194  output->at(oi++) = indices->at(i);
1195  }
1196  }
1197  output->resize(oi);
1198  return output;
1199  }
1200  else
1201  {
1202  pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
1203  int oi = 0; // output iterator
1204  if(substractIndices->size())
1205  {
1206  tree->setInputCloud(substractCloud, substractIndices);
1207  }
1208  else
1209  {
1210  tree->setInputCloud(substractCloud);
1211  }
1212  for(unsigned int i=0; i<cloud->size(); ++i)
1213  {
1214  std::vector<int> kIndices;
1215  std::vector<float> kDistances;
1216  int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances);
1217  if(k>=minNeighborsInRadius && maxAngle > 0.0f)
1218  {
1219  Eigen::Vector4f normal(cloud->at(i).normal_x, cloud->at(i).normal_y, cloud->at(i).normal_z, 0.0f);
1220  if (uIsFinite(normal[0]) &&
1221  uIsFinite(normal[1]) &&
1222  uIsFinite(normal[2]))
1223  {
1224  int count = k;
1225  for(int j=0; j<count && k >= minNeighborsInRadius; ++j)
1226  {
1227  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);
1228  if(uIsFinite(v[0]) &&
1229  uIsFinite(v[1]) &&
1230  uIsFinite(v[2]))
1231  {
1232  float angle = pcl::getAngle3D(normal, v);
1233  if(angle > maxAngle)
1234  {
1235  k-=1;
1236  }
1237  }
1238  else
1239  {
1240  k-=1;
1241  }
1242  }
1243  }
1244  else
1245  {
1246  k=0;
1247  }
1248  }
1249  if(k < minNeighborsInRadius)
1250  {
1251  output->at(oi++) = i;
1252  }
1253  }
1254  output->resize(oi);
1255  return output;
1256  }
1257 }
1258 
1259 pcl::IndicesPtr subtractFiltering(
1260  const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
1261  const pcl::IndicesPtr & indices,
1262  const pcl::PointCloud<pcl::PointNormal>::Ptr & substractCloud,
1263  const pcl::IndicesPtr & substractIndices,
1264  float radiusSearch,
1265  float maxAngle,
1266  int minNeighborsInRadius)
1267 {
1268  return subtractFilteringImpl<pcl::PointNormal>(cloud, indices, substractCloud, substractIndices, radiusSearch, maxAngle, minNeighborsInRadius);
1269 }
1270 pcl::IndicesPtr subtractFiltering(
1271  const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
1272  const pcl::IndicesPtr & indices,
1273  const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & substractCloud,
1274  const pcl::IndicesPtr & substractIndices,
1275  float radiusSearch,
1276  float maxAngle,
1277  int minNeighborsInRadius)
1278 {
1279  return subtractFilteringImpl<pcl::PointXYZINormal>(cloud, indices, substractCloud, substractIndices, radiusSearch, maxAngle, minNeighborsInRadius);
1280 }
1281 pcl::IndicesPtr subtractFiltering(
1282  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
1283  const pcl::IndicesPtr & indices,
1284  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
1285  const pcl::IndicesPtr & substractIndices,
1286  float radiusSearch,
1287  float maxAngle,
1288  int minNeighborsInRadius)
1289 {
1290  return subtractFilteringImpl<pcl::PointXYZRGBNormal>(cloud, indices, substractCloud, substractIndices, radiusSearch, maxAngle, minNeighborsInRadius);
1291 }
1292 
1293 pcl::IndicesPtr subtractAdaptiveFiltering(
1294  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
1295  const pcl::IndicesPtr & indices,
1296  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & substractCloud,
1297  const pcl::IndicesPtr & substractIndices,
1298  float radiusSearchRatio,
1299  int minNeighborsInRadius,
1300  const Eigen::Vector3f & viewpoint)
1301 {
1302  UWARN("Add angle to avoid subtraction of points with opposite normals");
1303  UASSERT(minNeighborsInRadius > 0);
1304  UASSERT(radiusSearchRatio > 0.0f);
1305  pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>(false));
1306 
1307  if(indices->size())
1308  {
1309  pcl::IndicesPtr output(new std::vector<int>(indices->size()));
1310  int oi = 0; // output iterator
1311  if(substractIndices->size())
1312  {
1313  tree->setInputCloud(substractCloud, substractIndices);
1314  }
1315  else
1316  {
1317  tree->setInputCloud(substractCloud);
1318  }
1319  for(unsigned int i=0; i<indices->size(); ++i)
1320  {
1321  if(pcl::isFinite(cloud->at(indices->at(i))))
1322  {
1323  std::vector<int> kIndices;
1324  std::vector<float> kSqrdDistances;
1325  float radius = radiusSearchRatio*uNorm(
1326  cloud->at(indices->at(i)).x-viewpoint[0],
1327  cloud->at(indices->at(i)).y-viewpoint[1],
1328  cloud->at(indices->at(i)).z-viewpoint[2]);
1329  int k = tree->radiusSearch(cloud->at(indices->at(i)), radius, kIndices, kSqrdDistances);
1330  if(k < minNeighborsInRadius)
1331  {
1332  output->at(oi++) = indices->at(i);
1333  }
1334  }
1335  }
1336  output->resize(oi);
1337  return output;
1338  }
1339  else
1340  {
1341  pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
1342  int oi = 0; // output iterator
1343  if(substractIndices->size())
1344  {
1345  tree->setInputCloud(substractCloud, substractIndices);
1346  }
1347  else
1348  {
1349  tree->setInputCloud(substractCloud);
1350  }
1351  for(unsigned int i=0; i<cloud->size(); ++i)
1352  {
1353  if(pcl::isFinite(cloud->at(i)))
1354  {
1355  std::vector<int> kIndices;
1356  std::vector<float> kSqrdDistances;
1357  float radius = radiusSearchRatio*uNorm(
1358  cloud->at(i).x-viewpoint[0],
1359  cloud->at(i).y-viewpoint[1],
1360  cloud->at(i).z-viewpoint[2]);
1361  int k = tree->radiusSearch(cloud->at(i), radius, kIndices, kSqrdDistances);
1362  if(k < minNeighborsInRadius)
1363  {
1364  output->at(oi++) = i;
1365  }
1366  }
1367  }
1368  output->resize(oi);
1369  return output;
1370  }
1371 }
1372 pcl::IndicesPtr subtractAdaptiveFiltering(
1373  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
1374  const pcl::IndicesPtr & indices,
1375  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
1376  const pcl::IndicesPtr & substractIndices,
1377  float radiusSearchRatio,
1378  float maxAngle,
1379  int minNeighborsInRadius,
1380  const Eigen::Vector3f & viewpoint)
1381 {
1382  UASSERT(minNeighborsInRadius > 0);
1383  UASSERT(radiusSearchRatio > 0.0f);
1384  pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBNormal>(false));
1385 
1386  if(indices->size())
1387  {
1388  pcl::IndicesPtr output(new std::vector<int>(indices->size()));
1389  int oi = 0; // output iterator
1390  if(substractIndices->size())
1391  {
1392  tree->setInputCloud(substractCloud, substractIndices);
1393  }
1394  else
1395  {
1396  tree->setInputCloud(substractCloud);
1397  }
1398  for(unsigned int i=0; i<indices->size(); ++i)
1399  {
1400  if(pcl::isFinite(cloud->at(indices->at(i))))
1401  {
1402  std::vector<int> kIndices;
1403  std::vector<float> kSqrdDistances;
1404  float radius = radiusSearchRatio*uNorm(
1405  cloud->at(indices->at(i)).x-viewpoint[0],
1406  cloud->at(indices->at(i)).y-viewpoint[1],
1407  cloud->at(indices->at(i)).z-viewpoint[2]);
1408  int k = tree->radiusSearch(cloud->at(indices->at(i)), radius, kIndices, kSqrdDistances);
1409  if(k>=minNeighborsInRadius && maxAngle > 0.0f)
1410  {
1411  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);
1412  if (uIsFinite(normal[0]) &&
1413  uIsFinite(normal[1]) &&
1414  uIsFinite(normal[2]))
1415  {
1416  int count = k;
1417  for(int j=0; j<count && k >= minNeighborsInRadius; ++j)
1418  {
1419  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);
1420  if(uIsFinite(v[0]) &&
1421  uIsFinite(v[1]) &&
1422  uIsFinite(v[2]))
1423  {
1424  float angle = pcl::getAngle3D(normal, v);
1425  if(angle > maxAngle)
1426  {
1427  k-=1;
1428  }
1429  }
1430  else
1431  {
1432  k-=1;
1433  }
1434  }
1435  }
1436  else
1437  {
1438  k=0;
1439  }
1440  }
1441 
1442  if(k < minNeighborsInRadius)
1443  {
1444  output->at(oi++) = indices->at(i);
1445  }
1446  }
1447  }
1448  output->resize(oi);
1449  return output;
1450  }
1451  else
1452  {
1453  pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
1454  int oi = 0; // output iterator
1455  if(substractIndices->size())
1456  {
1457  tree->setInputCloud(substractCloud, substractIndices);
1458  }
1459  else
1460  {
1461  tree->setInputCloud(substractCloud);
1462  }
1463  for(unsigned int i=0; i<cloud->size(); ++i)
1464  {
1465  if(pcl::isFinite(cloud->at(i)))
1466  {
1467  std::vector<int> kIndices;
1468  std::vector<float> kSqrdDistances;
1469  float radius = radiusSearchRatio*uNorm(
1470  cloud->at(i).x-viewpoint[0],
1471  cloud->at(i).y-viewpoint[1],
1472  cloud->at(i).z-viewpoint[2]);
1473  int k = tree->radiusSearch(cloud->at(i), radius, kIndices, kSqrdDistances);
1474  if(k>=minNeighborsInRadius && maxAngle > 0.0f)
1475  {
1476  Eigen::Vector4f normal(cloud->at(i).normal_x, cloud->at(i).normal_y, cloud->at(i).normal_z, 0.0f);
1477  if (uIsFinite(normal[0]) &&
1478  uIsFinite(normal[1]) &&
1479  uIsFinite(normal[2]))
1480  {
1481  int count = k;
1482  for(int j=0; j<count && k >= minNeighborsInRadius; ++j)
1483  {
1484  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);
1485  if(uIsFinite(v[0]) &&
1486  uIsFinite(v[1]) &&
1487  uIsFinite(v[2]))
1488  {
1489  float angle = pcl::getAngle3D(normal, v);
1490  if(angle > maxAngle)
1491  {
1492  k-=1;
1493  }
1494  }
1495  else
1496  {
1497  k-=1;
1498  }
1499  }
1500  }
1501  else
1502  {
1503  k=0;
1504  }
1505  }
1506  if(k < minNeighborsInRadius)
1507  {
1508  output->at(oi++) = i;
1509  }
1510  }
1511  }
1512  output->resize(oi);
1513  return output;
1514  }
1515 }
1516 
1517 
1518 pcl::IndicesPtr normalFiltering(
1519  const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
1520  float angleMax,
1521  const Eigen::Vector4f & normal,
1522  int normalKSearch,
1523  const Eigen::Vector4f & viewpoint)
1524 {
1525  pcl::IndicesPtr indices(new std::vector<int>);
1526  return normalFiltering(cloud, indices, angleMax, normal, normalKSearch, viewpoint);
1527 }
1528 pcl::IndicesPtr normalFiltering(
1529  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
1530  float angleMax,
1531  const Eigen::Vector4f & normal,
1532  int normalKSearch,
1533  const Eigen::Vector4f & viewpoint)
1534 {
1535  pcl::IndicesPtr indices(new std::vector<int>);
1536  return normalFiltering(cloud, indices, angleMax, normal, normalKSearch, viewpoint);
1537 }
1538 
1539 
1540 template<typename PointT>
1541 pcl::IndicesPtr normalFilteringImpl(
1542  const typename pcl::PointCloud<PointT>::Ptr & cloud,
1543  const pcl::IndicesPtr & indices,
1544  float angleMax,
1545  const Eigen::Vector4f & normal,
1546  int normalKSearch,
1547  const Eigen::Vector4f & viewpoint)
1548 {
1549  pcl::IndicesPtr output(new std::vector<int>());
1550 
1551  if(cloud->size())
1552  {
1553  typename pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>(false));
1554 
1555  pcl::NormalEstimationOMP<PointT, pcl::Normal> ne;
1556  ne.setInputCloud (cloud);
1557  if(indices->size())
1558  {
1559  ne.setIndices(indices);
1560  }
1561 
1562  if(indices->size())
1563  {
1564  tree->setInputCloud(cloud, indices);
1565  }
1566  else
1567  {
1568  tree->setInputCloud(cloud);
1569  }
1570  ne.setSearchMethod (tree);
1571 
1572  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
1573 
1574  ne.setKSearch(normalKSearch);
1575  if(viewpoint[0] != 0 || viewpoint[1] != 0 || viewpoint[2] != 0)
1576  {
1577  ne.setViewPoint(viewpoint[0], viewpoint[1], viewpoint[2]);
1578  }
1579 
1580  ne.compute (*cloud_normals);
1581 
1582  output->resize(cloud_normals->size());
1583  int oi = 0; // output iterator
1584  for(unsigned int i=0; i<cloud_normals->size(); ++i)
1585  {
1586  Eigen::Vector4f v(cloud_normals->at(i).normal_x, cloud_normals->at(i).normal_y, cloud_normals->at(i).normal_z, 0.0f);
1587  float angle = pcl::getAngle3D(normal, v);
1588  if(angle < angleMax)
1589  {
1590  output->at(oi++) = indices->size()!=0?indices->at(i):i;
1591  }
1592  }
1593  output->resize(oi);
1594  }
1595 
1596  return output;
1597 }
1598 pcl::IndicesPtr normalFiltering(
1599  const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
1600  const pcl::IndicesPtr & indices,
1601  float angleMax,
1602  const Eigen::Vector4f & normal,
1603  int normalKSearch,
1604  const Eigen::Vector4f & viewpoint)
1605 
1606 {
1607  return normalFilteringImpl<pcl::PointXYZ>(cloud, indices, angleMax, normal, normalKSearch, viewpoint);
1608 }
1609 pcl::IndicesPtr normalFiltering(
1610  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
1611  const pcl::IndicesPtr & indices,
1612  float angleMax,
1613  const Eigen::Vector4f & normal,
1614  int normalKSearch,
1615  const Eigen::Vector4f & viewpoint)
1616 {
1617  return normalFilteringImpl<pcl::PointXYZRGB>(cloud, indices, angleMax, normal, normalKSearch, viewpoint);
1618 }
1619 
1620 template<typename PointT>
1621 pcl::IndicesPtr normalFilteringImpl(
1622  const typename pcl::PointCloud<PointT>::Ptr & cloud,
1623  const pcl::IndicesPtr & indices,
1624  float angleMax,
1625  const Eigen::Vector4f & normal)
1626 {
1627  pcl::IndicesPtr output(new std::vector<int>());
1628 
1629  if(cloud->size())
1630  {
1631  int oi = 0; // output iterator
1632  if(indices->size())
1633  {
1634  output->resize(indices->size());
1635  for(unsigned int i=0; i<indices->size(); ++i)
1636  {
1637  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);
1638  float angle = pcl::getAngle3D(normal, v);
1639  if(angle < angleMax)
1640  {
1641  output->at(oi++) = indices->size()!=0?indices->at(i):i;
1642  }
1643  }
1644  }
1645  else
1646  {
1647  output->resize(cloud->size());
1648  for(unsigned int i=0; i<cloud->size(); ++i)
1649  {
1650  Eigen::Vector4f v(cloud->at(i).normal_x, cloud->at(i).normal_y, cloud->at(i).normal_z, 0.0f);
1651  float angle = pcl::getAngle3D(normal, v);
1652  if(angle < angleMax)
1653  {
1654  output->at(oi++) = indices->size()!=0?indices->at(i):i;
1655  }
1656  }
1657  }
1658 
1659  output->resize(oi);
1660  }
1661 
1662  return output;
1663 }
1664 pcl::IndicesPtr normalFiltering(
1665  const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
1666  const pcl::IndicesPtr & indices,
1667  float angleMax,
1668  const Eigen::Vector4f & normal,
1669  int normalKSearch,
1670  const Eigen::Vector4f & viewpoint)
1671 {
1672  return normalFilteringImpl<pcl::PointNormal>(cloud, indices, angleMax, normal);
1673 }
1674 pcl::IndicesPtr normalFiltering(
1675  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
1676  const pcl::IndicesPtr & indices,
1677  float angleMax,
1678  const Eigen::Vector4f & normal,
1679  int normalKSearch,
1680  const Eigen::Vector4f & viewpoint)
1681 {
1682  return normalFilteringImpl<pcl::PointXYZRGBNormal>(cloud, indices, angleMax, normal);
1683 }
1684 
1685 std::vector<pcl::IndicesPtr> extractClusters(
1686  const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
1687  float clusterTolerance,
1688  int minClusterSize,
1689  int maxClusterSize,
1690  int * biggestClusterIndex)
1691 {
1692  pcl::IndicesPtr indices(new std::vector<int>);
1693  return extractClusters(cloud, indices, clusterTolerance, minClusterSize, maxClusterSize, biggestClusterIndex);
1694 }
1695 std::vector<pcl::IndicesPtr> extractClusters(
1696  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
1697  float clusterTolerance,
1698  int minClusterSize,
1699  int maxClusterSize,
1700  int * biggestClusterIndex)
1701 {
1702  pcl::IndicesPtr indices(new std::vector<int>);
1703  return extractClusters(cloud, indices, clusterTolerance, minClusterSize, maxClusterSize, biggestClusterIndex);
1704 }
1705 
1706 template<typename PointT>
1707 std::vector<pcl::IndicesPtr> extractClustersImpl(
1708  const typename pcl::PointCloud<PointT>::Ptr & cloud,
1709  const pcl::IndicesPtr & indices,
1710  float clusterTolerance,
1711  int minClusterSize,
1712  int maxClusterSize,
1713  int * biggestClusterIndex)
1714 {
1715  typename pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>(true));
1716  pcl::EuclideanClusterExtraction<PointT> ec;
1717  ec.setClusterTolerance (clusterTolerance);
1718  ec.setMinClusterSize (minClusterSize);
1719  ec.setMaxClusterSize (maxClusterSize);
1720  ec.setInputCloud (cloud);
1721 
1722  if(indices->size())
1723  {
1724  ec.setIndices(indices);
1725  tree->setInputCloud(cloud, indices);
1726  }
1727  else
1728  {
1729  tree->setInputCloud(cloud);
1730  }
1731  ec.setSearchMethod (tree);
1732 
1733  std::vector<pcl::PointIndices> cluster_indices;
1734  ec.extract (cluster_indices);
1735 
1736  int maxIndex=-1;
1737  unsigned int maxSize = 0;
1738  std::vector<pcl::IndicesPtr> output(cluster_indices.size());
1739  for(unsigned int i=0; i<cluster_indices.size(); ++i)
1740  {
1741  output[i] = pcl::IndicesPtr(new std::vector<int>(cluster_indices[i].indices));
1742 
1743  if(maxSize < cluster_indices[i].indices.size())
1744  {
1745  maxSize = (unsigned int)cluster_indices[i].indices.size();
1746  maxIndex = i;
1747  }
1748  }
1749  if(biggestClusterIndex)
1750  {
1751  *biggestClusterIndex = maxIndex;
1752  }
1753 
1754  return output;
1755 }
1756 
1757 std::vector<pcl::IndicesPtr> extractClusters(
1758  const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
1759  const pcl::IndicesPtr & indices,
1760  float clusterTolerance,
1761  int minClusterSize,
1762  int maxClusterSize,
1763  int * biggestClusterIndex)
1764 {
1765  return extractClustersImpl<pcl::PointXYZ>(cloud, indices, clusterTolerance, minClusterSize, maxClusterSize, biggestClusterIndex);
1766 }
1767 std::vector<pcl::IndicesPtr> extractClusters(
1768  const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
1769  const pcl::IndicesPtr & indices,
1770  float clusterTolerance,
1771  int minClusterSize,
1772  int maxClusterSize,
1773  int * biggestClusterIndex)
1774 {
1775  return extractClustersImpl<pcl::PointNormal>(cloud, indices, clusterTolerance, minClusterSize, maxClusterSize, biggestClusterIndex);
1776 }
1777 std::vector<pcl::IndicesPtr> extractClusters(
1778  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
1779  const pcl::IndicesPtr & indices,
1780  float clusterTolerance,
1781  int minClusterSize,
1782  int maxClusterSize,
1783  int * biggestClusterIndex)
1784 {
1785  return extractClustersImpl<pcl::PointXYZRGB>(cloud, indices, clusterTolerance, minClusterSize, maxClusterSize, biggestClusterIndex);
1786 }
1787 std::vector<pcl::IndicesPtr> extractClusters(
1788  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
1789  const pcl::IndicesPtr & indices,
1790  float clusterTolerance,
1791  int minClusterSize,
1792  int maxClusterSize,
1793  int * biggestClusterIndex)
1794 {
1795  return extractClustersImpl<pcl::PointXYZRGBNormal>(cloud, indices, clusterTolerance, minClusterSize, maxClusterSize, biggestClusterIndex);
1796 }
1797 
1798 template<typename PointT>
1799 pcl::IndicesPtr extractIndicesImpl(
1800  const typename pcl::PointCloud<PointT>::Ptr & cloud,
1801  const pcl::IndicesPtr & indices,
1802  bool negative)
1803 {
1804  pcl::IndicesPtr output(new std::vector<int>);
1805  pcl::ExtractIndices<PointT> extract;
1806  extract.setInputCloud (cloud);
1807  extract.setIndices(indices);
1808  extract.setNegative(negative);
1809  extract.filter(*output);
1810  return output;
1811 }
1812 
1813 pcl::IndicesPtr extractIndices(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative)
1814 {
1815  return extractIndicesImpl<pcl::PointXYZ>(cloud, indices, negative);
1816 }
1817 pcl::IndicesPtr extractIndices(const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative)
1818 {
1819  return extractIndicesImpl<pcl::PointNormal>(cloud, indices, negative);
1820 }
1821 pcl::IndicesPtr extractIndices(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative)
1822 {
1823  return extractIndicesImpl<pcl::PointXYZRGB>(cloud, indices, negative);
1824 }
1825 pcl::IndicesPtr extractIndices(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative)
1826 {
1827  return extractIndicesImpl<pcl::PointXYZRGBNormal>(cloud, indices, negative);
1828 }
1829 
1830 template<typename PointT>
1831 typename pcl::PointCloud<PointT>::Ptr extractIndicesImpl(
1832  const typename pcl::PointCloud<PointT>::Ptr & cloud,
1833  const pcl::IndicesPtr & indices,
1834  bool negative,
1835  bool keepOrganized)
1836 {
1837  typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
1838  pcl::ExtractIndices<PointT> extract;
1839  extract.setInputCloud (cloud);
1840  extract.setIndices(indices);
1841  extract.setNegative(negative);
1842  extract.setKeepOrganized(keepOrganized);
1843  extract.filter(*output);
1844  return output;
1845 }
1846 pcl::PointCloud<pcl::PointXYZ>::Ptr extractIndices(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative, bool keepOrganized)
1847 {
1848  return extractIndicesImpl<pcl::PointXYZ>(cloud, indices, negative, keepOrganized);
1849 }
1850 pcl::PointCloud<pcl::PointXYZRGB>::Ptr extractIndices(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative, bool keepOrganized)
1851 {
1852  return extractIndicesImpl<pcl::PointXYZRGB>(cloud, indices, negative, keepOrganized);
1853 }
1854 // PCL default lacks of pcl::PointNormal type support
1855 //pcl::PointCloud<pcl::PointNormal>::Ptr extractIndices(const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative, bool keepOrganized)
1856 //{
1857 // return extractIndicesImpl<pcl::PointNormal>(cloud, indices, negative, keepOrganized);
1858 //}
1859 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr extractIndices(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative, bool keepOrganized)
1860 {
1861  return extractIndicesImpl<pcl::PointXYZRGBNormal>(cloud, indices, negative, keepOrganized);
1862 }
1863 
1864 pcl::IndicesPtr extractPlane(
1865  const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
1866  float distanceThreshold,
1867  int maxIterations,
1868  pcl::ModelCoefficients * coefficientsOut)
1869 {
1870  pcl::IndicesPtr indices(new std::vector<int>);
1871  return extractPlane(cloud, indices, distanceThreshold, maxIterations, coefficientsOut);
1872 }
1873 
1874 pcl::IndicesPtr extractPlane(
1875  const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
1876  const pcl::IndicesPtr & indices,
1877  float distanceThreshold,
1878  int maxIterations,
1879  pcl::ModelCoefficients * coefficientsOut)
1880 {
1881  // Extract plane
1882  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
1883  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
1884  // Create the segmentation object
1885  pcl::SACSegmentation<pcl::PointXYZ> seg;
1886  // Optional
1887  seg.setOptimizeCoefficients (true);
1888  seg.setMaxIterations (maxIterations);
1889  // Mandatory
1890  seg.setModelType (pcl::SACMODEL_PLANE);
1891  seg.setMethodType (pcl::SAC_RANSAC);
1892  seg.setDistanceThreshold (distanceThreshold);
1893 
1894  seg.setInputCloud (cloud);
1895  if(indices->size())
1896  {
1897  seg.setIndices(indices);
1898  }
1899  seg.segment (*inliers, *coefficients);
1900 
1901  if(coefficientsOut)
1902  {
1903  *coefficientsOut = *coefficients;
1904  }
1905 
1906  return pcl::IndicesPtr(new std::vector<int>(inliers->indices));
1907 }
1908 
1909 }
1910 
1911 }
rtabmap::util3d::isFinite
bool RTABMAP_EXP isFinite(const cv::Point3f &pt)
Definition: util3d.cpp:3108
rtabmap::LaserScan::rangeMin
float rangeMin() const
Definition: LaserScan.h:93
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:1003
rtabmap::LaserScan::dataType
int dataType() const
Definition: LaserScan.h:103
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:1685
glm::min
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
rtabmap::LaserScan::data
const cv::Mat & data() const
Definition: LaserScan.h:88
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:2777
rtabmap::LaserScan::format
Format format() const
Definition: LaserScan.h:89
rtabmap::LaserScan::hasNormals
bool hasNormals() const
Definition: LaserScan.h:105
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:619
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:597
f
f
rtabmap::util3d::removeNaNNormalsFromPointCloudImpl
pcl::PointCloud< PointT >::Ptr removeNaNNormalsFromPointCloudImpl(const typename pcl::PointCloud< PointT >::Ptr &cloud)
Definition: util3d_filtering.cpp:893
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:1541
glm::axis
GLM_FUNC_DECL detail::tvec3< T, P > axis(detail::tquat< T, P > const &x)
rtabmap::LaserScan::rangeMax
float rangeMax() const
Definition: LaserScan.h:94
rtabmap::util3d::removeNaNFromPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP removeNaNFromPointCloud(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud)
Definition: util3d_filtering.cpp:879
rtabmap::util3d::randomSamplingImpl
pcl::PointCloud< PointT >::Ptr randomSamplingImpl(const typename pcl::PointCloud< PointT >::Ptr &cloud, int samples)
Definition: util3d_filtering.cpp:576
rtabmap::LaserScan::isEmpty
bool isEmpty() const
Definition: LaserScan.h:101
rtabmap::LaserScan::hasRGB
bool hasRGB() const
Definition: LaserScan.h:106
glm::all
GLM_FUNC_DECL bool all(vecType< bool, P > const &v)
rtabmap::LaserScan::maxPoints
int maxPoints() const
Definition: LaserScan.h:92
rtabmap::LaserScan::is2d
bool is2d() const
Definition: LaserScan.h:104
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:1293
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:871
UConversion.h
Some conversion functions.
rtabmap::LaserScan::size
int size() const
Definition: LaserScan.h:102
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:795
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:827
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:1813
rtabmap::util3d::laserScanToPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP laserScanToPointCloud(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2382
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:494
rtabmap::util3d::downsample
LaserScan RTABMAP_EXP downsample(const LaserScan &cloud, int step)
Definition: util3d_filtering.cpp:368
rtabmap::util3d::laserScan2dFromPointCloud
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud< pcl::PointXYZ > &cloud, const Transform &transform=Transform(), bool filterNaNs=true)
Definition: util3d.cpp:2105
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:1017
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:396
rtabmap::util3d::laserScanToPointCloudRGB
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_EXP laserScanToPointCloudRGB(const LaserScan &laserScan, const Transform &transform=Transform(), unsigned char r=100, unsigned char g=100, unsigned char b=100)
Definition: util3d.cpp:2417
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, float groundNormalsUp=0.0f)
Definition: util3d_filtering.cpp:74
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:587
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:1707
util3d_filtering.h
rtabmap::LaserScan::angleMin
float angleMin() const
Definition: LaserScan.h:95
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:690
rtabmap::util3d::computeNormals
cv::Mat RTABMAP_EXP computeNormals(const cv::Mat &laserScan, int searchK, float searchRadius)
rtabmap::LaserScan::angleMax
float angleMax() const
Definition: LaserScan.h:96
tree
tree
rtabmap::LaserScan::localTransform
Transform localTransform() const
Definition: LaserScan.h:98
rtabmap::LaserScan::angleIncrement
float angleIncrement() const
Definition: LaserScan.h:97
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:940
rtabmap::util3d::radiusFiltering
pcl::IndicesPtr RTABMAP_EXP radiusFiltering(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, float radiusSearch, int minNeighborsInRadius)
Definition: util3d_filtering.cpp:918
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:341
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:1864
rtabmap::util3d::cropBox
pcl::IndicesPtr RTABMAP_EXP cropBox(const pcl::PCLPointCloud2::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:715
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:317
rtabmap::util3d::removeNaNNormalsFromPointCloud
pcl::PointCloud< pcl::PointNormal >::Ptr RTABMAP_EXP removeNaNNormalsFromPointCloud(const pcl::PointCloud< pcl::PointNormal >::Ptr &cloud)
Definition: util3d_filtering.cpp:901
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:2435
rtabmap::Transform::toEigen3f
Eigen::Affine3f toEigen3f() const
Definition: Transform.cpp:360
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:519
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:1799
rtabmap::LaserScan::hasIntensity
bool hasIntensity() const
Definition: LaserScan.h:107
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:1518
rtabmap::util3d::adjustNormalsToViewPoint
LaserScan RTABMAP_EXP adjustNormalsToViewPoint(const LaserScan &scan, const Eigen::Vector3f &viewpoint=Eigen::Vector3f(0, 0, 0), float groundNormalsUp=0.0f)
Definition: util3d_surface.cpp:3363
rtabmap::util3d::laserScanFromPointCloud
LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PCLPointCloud2 &cloud, bool filterNaNs=true, bool is2D=false, const Transform &transform=Transform())
Definition: util3d.cpp:1266

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:37:06