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(), scan.localTransform());
178  UDEBUG("Normals computed (k=%d radius=%f)", normalK, normalRadius);
179  if(scan.empty())
180  {
181  UWARN("Only NaNs returned after normals estimation! The returned point cloud is empty. Normal k (%d) and/or radius (%f) may be too small.", normalK, normalRadius);
182  }
183  }
184  else
185  {
186  if(scan.hasNormals())
187  {
188  UWARN("Voxel filter is applied, but normal parameters are not set and input scan has normals. The returned scan has no normals.");
189  }
190  scan = LaserScan(laserScanFromPointCloud(*cloud), scanMaxPts, scan.rangeMax(), scan.localTransform());
191  }
192  }
193  }
194  else if(scan.hasIntensity())
195  {
196  pcl::PointCloud<pcl::PointXYZI>::Ptr cloud = laserScanToPointCloudI(scan);
197  if(cloud->size())
198  {
199  int scanMaxPts = scan.maxPoints();
200  if(voxelSize > 0.0f)
201  {
202  cloud = voxelize(cloud, voxelSize);
203  float ratio = float(cloud->size()) / scan.size();
204  scanMaxPts = int(float(scanMaxPts) * ratio);
205  UDEBUG("Voxel filtering scan (voxel=%f m): %d -> %d (scanMaxPts=%d->%d)", voxelSize, scan.size(), cloud->size(), scan.maxPoints(), scanMaxPts);
206  }
207  if(cloud->size() && (normalK > 0 || normalRadius>0.0f))
208  {
209  pcl::PointCloud<pcl::Normal>::Ptr normals;
210  if(scan.is2d())
211  {
212  normals = util3d::computeNormals2D(cloud, normalK, normalRadius);
213  if(voxelSize == 0.0f && scan.angleIncrement() > 0.0f)
214  {
215  scan = LaserScan(laserScan2dFromPointCloud(*cloud, *normals), scan.rangeMin(), scan.rangeMax(), scan.angleMin(), scan.angleMax(), scan.angleIncrement(), scan.localTransform());
216  }
217  else
218  {
219  scan = LaserScan(laserScan2dFromPointCloud(*cloud, *normals), scanMaxPts, scan.rangeMax(), scan.localTransform());
220  }
221  if(scan.empty())
222  {
223  UWARN("Only NaNs returned after normals estimation! The returned point cloud is empty. Normal k (%d) and/or radius (%f) may be too small.", normalK, normalRadius);
224  }
225  }
226  else
227  {
228  normals = util3d::computeNormals(cloud, normalK, normalRadius);
229  scan = LaserScan(laserScanFromPointCloud(*cloud, *normals), scanMaxPts, scan.rangeMax(), scan.localTransform());
230  if(scan.empty())
231  {
232  UWARN("Only NaNs returned after normals estimation! The returned point cloud is empty. Normal k (%d) and/or radius (%f) may be too small.", normalK, normalRadius);
233  }
234  }
235  UDEBUG("Normals computed (k=%d radius=%f)", normalK, normalRadius);
236  }
237  else
238  {
239  if(scan.hasNormals())
240  {
241  UWARN("Voxel filter is applied, but normal parameters are not set and input scan has normals. The returned scan has no normals.");
242  }
243  if(scan.is2d())
244  {
245  scan = LaserScan(laserScan2dFromPointCloud(*cloud), scanMaxPts, scan.rangeMax(), scan.localTransform());
246  }
247  else
248  {
249  scan = LaserScan(laserScanFromPointCloud(*cloud), scanMaxPts, scan.rangeMax(), scan.localTransform());
250  }
251  }
252  }
253  }
254  else
255  {
256  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = laserScanToPointCloud(scan);
257  if(cloud->size())
258  {
259  int scanMaxPts = scan.maxPoints();
260  if(voxelSize > 0.0f)
261  {
262  cloud = voxelize(cloud, voxelSize);
263  float ratio = float(cloud->size()) / scan.size();
264  scanMaxPts = int(float(scanMaxPts) * ratio);
265  UDEBUG("Voxel filtering scan (voxel=%f m): %d -> %d (scanMaxPts=%d->%d)", voxelSize, scan.size(), cloud->size(), scan.maxPoints(), scanMaxPts);
266  }
267  if(cloud->size() && (normalK > 0 || normalRadius>0.0f))
268  {
269  pcl::PointCloud<pcl::Normal>::Ptr normals;
270  if(scan.is2d())
271  {
272  normals = util3d::computeNormals2D(cloud, normalK, normalRadius);
273  if(voxelSize == 0.0f && scan.angleIncrement() > 0.0f)
274  {
275  scan = LaserScan(laserScan2dFromPointCloud(*cloud, *normals), scan.rangeMin(), scan.rangeMax(), scan.angleMin(), scan.angleMax(), scan.angleIncrement(), scan.localTransform());
276  }
277  else
278  {
279  scan = LaserScan(laserScan2dFromPointCloud(*cloud, *normals), scanMaxPts, scan.rangeMax(), scan.localTransform());
280  }
281  if(scan.empty())
282  {
283  UWARN("Only NaNs returned after normals estimation! The returned point cloud is empty. Normal k (%d) and/or radius (%f) may be too small.", normalK, normalRadius);
284  }
285  }
286  else
287  {
288  normals = util3d::computeNormals(cloud, normalK, normalRadius);
289  scan = LaserScan(laserScanFromPointCloud(*cloud, *normals), scanMaxPts, scan.rangeMax(), scan.localTransform());
290  if(scan.empty())
291  {
292  UWARN("Only NaNs returned after normals estimation! The returned point cloud is empty. Normal k (%d) and/or radius (%f) may be too small.", normalK, normalRadius);
293  }
294  }
295  UDEBUG("Normals computed (k=%d radius=%f)", normalK, normalRadius);
296  }
297  else
298  {
299  if(scan.hasNormals())
300  {
301  UWARN("Voxel filter i applied, but normal parameters are not set and input scan has normals. The returned scan has no normals.");
302  }
303  if(scan.is2d())
304  {
305  scan = LaserScan(laserScan2dFromPointCloud(*cloud), scanMaxPts, scan.rangeMax(), scan.localTransform());
306  }
307  else
308  {
309  scan = LaserScan(laserScanFromPointCloud(*cloud), scanMaxPts, scan.rangeMax(), scan.localTransform());
310  }
311  }
312  }
313  }
314  }
315 
316  if(scan.size() && !scan.is2d() && scan.hasNormals() && groundNormalsUp>0.0f)
317  {
318  scan = util3d::adjustNormalsToViewPoint(scan, Eigen::Vector3f(0,0,10), groundNormalsUp);
319  }
320  }
321  return scan;
322 }
323 
324 LaserScan commonFiltering(
325  const LaserScan & scanIn,
326  int downsamplingStep,
327  float rangeMin,
328  float rangeMax,
329  float voxelSize,
330  int normalK,
331  float normalRadius,
332  bool forceGroundNormalsUp)
333 {
334  return commonFiltering(scanIn, downsamplingStep, rangeMin, rangeMax, voxelSize, normalK, normalRadius, forceGroundNormalsUp?0.8f:0.0f);
335 }
336 
337 LaserScan rangeFiltering(
338  const LaserScan & scan,
339  float rangeMin,
340  float rangeMax)
341 {
342  UASSERT(rangeMin >=0.0f && rangeMax>=0.0f);
343  if(!scan.isEmpty())
344  {
345  if(rangeMin > 0.0f || rangeMax > 0.0f)
346  {
347  cv::Mat output = cv::Mat(1, scan.size(), scan.dataType());
348  bool is2d = scan.is2d();
349  int oi = 0;
350  float rangeMinSqrd = rangeMin * rangeMin;
351  float rangeMaxSqrd = rangeMax * rangeMax;
352  for(int i=0; i<scan.size(); ++i)
353  {
354  const float * ptr = scan.data().ptr<float>(0, i);
355  float r;
356  if(is2d)
357  {
358  r = ptr[0]*ptr[0] + ptr[1]*ptr[1];
359  }
360  else
361  {
362  r = ptr[0]*ptr[0] + ptr[1]*ptr[1] + ptr[2]*ptr[2];
363  }
364 
365  if(rangeMin > 0.0f && r < rangeMinSqrd)
366  {
367  continue;
368  }
369  if(rangeMax > 0.0f && r > rangeMaxSqrd)
370  {
371  continue;
372  }
373 
374  cv::Mat(scan.data(), cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(output, cv::Range::all(), cv::Range(oi,oi+1)));
375  ++oi;
376  }
377  if(scan.angleIncrement() > 0.0f)
378  {
379  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());
380  }
381  return LaserScan(cv::Mat(output, cv::Range::all(), cv::Range(0, oi)), scan.maxPoints(), scan.rangeMax(), scan.format(), scan.localTransform());
382  }
383  }
384 
385  return scan;
386 }
387 
388 LaserScan downsample(
389  const LaserScan & scan,
390  int step)
391 {
392  UASSERT(step > 0);
393  if(step <= 1 || scan.size() <= step)
394  {
395  // no sampling
396  return scan;
397  }
398  else
399  {
400  int finalSize = scan.size()/step;
401  cv::Mat output = cv::Mat(1, finalSize, scan.dataType());
402  int oi = 0;
403  for(int i=0; i<scan.size()-step+1; i+=step)
404  {
405  cv::Mat(scan.data(), cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(output, cv::Range::all(), cv::Range(oi,oi+1)));
406  ++oi;
407  }
408  if(scan.angleIncrement() > 0.0f)
409  {
410  return LaserScan(output, scan.format(), scan.rangeMin(), scan.rangeMax(), scan.angleMin(), scan.angleMax(), scan.angleIncrement()*step, scan.localTransform());
411  }
412  return LaserScan(output, scan.maxPoints()/step, scan.rangeMax(), scan.format(), scan.localTransform());
413  }
414 }
415 template<typename PointT>
416 typename pcl::PointCloud<PointT>::Ptr downsampleImpl(
417  const typename pcl::PointCloud<PointT>::Ptr & cloud,
418  int step)
419 {
420  UASSERT(step > 0);
421  typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
422  if(step <= 1 || (int)cloud->size() <= step)
423  {
424  // no sampling
425  *output = *cloud;
426  }
427  else
428  {
429  if(cloud->height > 1 && cloud->height < cloud->width/4)
430  {
431  // Assuming ouster point cloud (e.g, 2048x64),
432  // for which the lower dimension is the number of rings.
433  // Downsample each ring by the step.
434  // Example data packed:
435  // <ringA-1, ringB-1, ringC-1, ringD-1;
436  // ringA-2, ringB-2, ringC-2, ringD-2;
437  // ringA-3, ringB-3, ringC-3, ringD-3;
438  // ringA-4, ringB-4, ringC-4, ringD-4>
439  unsigned int rings = cloud->height<cloud->width?cloud->height:cloud->width;
440  unsigned int pts = cloud->height>cloud->width?cloud->height:cloud->width;
441  unsigned int finalSize = rings * pts/step;
442  output->resize(finalSize);
443  output->width = rings;
444  output->height = pts/step;
445 
446  for(unsigned int j=0; j<rings; ++j)
447  {
448  for(unsigned int i=0; i<output->height; ++i)
449  {
450  (*output)[i*rings + j] = cloud->at(i*step*rings + j);
451  }
452  }
453 
454  }
455  else if(cloud->height > 1)
456  {
457  // assume depth image (e.g., 640x480), downsample like an image
458  UASSERT_MSG(cloud->height % step == 0 && cloud->width % step == 0,
459  uFormat("Decimation of depth images should be exact! (decimation=%d, size=%dx%d)",
460  step, cloud->width, cloud->height).c_str());
461 
462  int finalSize = cloud->height/step * cloud->width/step;
463  output->resize(finalSize);
464  output->width = cloud->width/step;
465  output->height = cloud->height/step;
466 
467  for(unsigned int j=0; j<output->height; ++j)
468  {
469  for(unsigned int i=0; i<output->width; ++i)
470  {
471  output->at(j*output->width + i) = cloud->at(j*output->width*step + i*step);
472  }
473  }
474  }
475  else
476  {
477  int finalSize = int(cloud->size())/step;
478  output->resize(finalSize);
479  int oi = 0;
480  for(unsigned int i=0; i<cloud->size()-step+1; i+=step)
481  {
482  (*output)[oi++] = cloud->at(i);
483  }
484  }
485  }
486  return output;
487 }
488 pcl::PointCloud<pcl::PointXYZ>::Ptr downsample(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, int step)
489 {
490  return downsampleImpl<pcl::PointXYZ>(cloud, step);
491 }
492 pcl::PointCloud<pcl::PointXYZRGB>::Ptr downsample(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, int step)
493 {
494  return downsampleImpl<pcl::PointXYZRGB>(cloud, step);
495 }
496 pcl::PointCloud<pcl::PointXYZI>::Ptr downsample(const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud, int step)
497 {
498  return downsampleImpl<pcl::PointXYZI>(cloud, step);
499 }
500 pcl::PointCloud<pcl::PointNormal>::Ptr downsample(const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, int step)
501 {
502  return downsampleImpl<pcl::PointNormal>(cloud, step);
503 }
504 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr downsample(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, int step)
505 {
506  return downsampleImpl<pcl::PointXYZRGBNormal>(cloud, step);
507 }
508 pcl::PointCloud<pcl::PointXYZINormal>::Ptr downsample(const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud, int step)
509 {
510  return downsampleImpl<pcl::PointXYZINormal>(cloud, step);
511 }
512 
513 template<typename PointT>
514 typename pcl::PointCloud<PointT>::Ptr voxelizeImpl(
515  const typename pcl::PointCloud<PointT>::Ptr & cloud,
516  const pcl::IndicesPtr & indices,
517  float voxelSize,
518  int level = 0)
519 {
520  UASSERT(voxelSize > 0.0f);
521  typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
522  if((cloud->is_dense && cloud->size()) || (!cloud->is_dense && !indices->empty()))
523  {
524  Eigen::Vector4f min_p, max_p;
525  // Get the minimum and maximum dimensions
526  if(indices->empty())
527  pcl::getMinMax3D<PointT>(*cloud, min_p, max_p);
528  else
529  pcl::getMinMax3D<PointT>(*cloud, *indices, min_p, max_p);
530 
531  // Check that the leaf size is not too small, given the size of the data
532  float inverseVoxelSize = 1.0f/voxelSize;
533  std::int64_t dx = static_cast<std::int64_t>((max_p[0] - min_p[0]) * inverseVoxelSize)+1;
534  std::int64_t dy = static_cast<std::int64_t>((max_p[1] - min_p[1]) * inverseVoxelSize)+1;
535  std::int64_t dz = static_cast<std::int64_t>((max_p[2] - min_p[2]) * inverseVoxelSize)+1;
536 
537  if ((dx*dy*dz) > static_cast<std::int64_t>(std::numeric_limits<std::int32_t>::max()))
538  {
539  UWARN("Leaf size is too small for the input dataset. Integer indices would overflow. "
540  "We will split space to be able to voxelize (lvl=%d cloud=%d min=[%f %f %f] max=[%f %f %f] voxel=%f).",
541  level,
542  (int)(indices->empty()?cloud->size():indices->size()),
543  min_p[0], min_p[1], min_p[2],
544  max_p[0], max_p[1], max_p[2],
545  voxelSize);
546  pcl::IndicesPtr denseIndices;
547  if(indices->empty())
548  {
549  denseIndices.reset(new std::vector<int>(cloud->size()));
550  for(size_t i=0; i<cloud->size(); ++i)
551  {
552  denseIndices->at(i) = i;
553  }
554  }
555 
556  Eigen::Vector4f mid = (max_p-min_p)/2.0f;
557  int zMax = max_p[2]-min_p[2] < 10?1:2; // do quad tree for 2D maps
558  for(int x=0; x<2; ++x)
559  {
560  for(int y=0; y<2; ++y)
561  {
562  for(int z=0; z<zMax; ++z)
563  {
564  Eigen::Vector4f m = min_p+Eigen::Vector4f(mid[0]*x,mid[1]*y,mid[2]*z,0);
565  Eigen::Vector4f mx = m+mid;
566  if(zMax==1)
567  {
568  mx[2] = max_p[2];
569  }
570  pcl::IndicesPtr ind = util3d::cropBox(cloud, denseIndices.get()?denseIndices:indices, m, mx);
571  if(!ind->empty())
572  {
573  // extract indices to avoid high memory usage
574  *output+=*voxelizeImpl<PointT>(cloud, ind, voxelSize, level+1);
575  }
576  }
577  }
578  }
579  }
580  else
581  {
582  pcl::VoxelGrid<PointT> filter;
583  filter.setLeafSize(voxelSize, voxelSize, voxelSize);
584  filter.setInputCloud(cloud);
585 #ifdef WIN32
586  // Pre-allocating the cloud helps to avoid crash when freeing memory allocated inside pcl library
587  output->resize(cloud->size());
588 #endif
589  if(!indices->empty())
590  {
591  filter.setIndices(indices);
592  }
593  filter.filter(*output);
594  }
595  }
596  else if(cloud->size() && !cloud->is_dense && indices->size() == 0)
597  {
598  UWARN("Cannot voxelize a not dense (organized) cloud with empty indices! (input=%d pts). Returning empty cloud!", (int)cloud->size());
599  }
600  return output;
601 }
602 
603 pcl::PointCloud<pcl::PointXYZ>::Ptr voxelize(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, const pcl::IndicesPtr & indices, float voxelSize)
604 {
605  return voxelizeImpl<pcl::PointXYZ>(cloud, indices, voxelSize);
606 }
607 pcl::PointCloud<pcl::PointNormal>::Ptr voxelize(const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, const pcl::IndicesPtr & indices, float voxelSize)
608 {
609  return voxelizeImpl<pcl::PointNormal>(cloud, indices, voxelSize);
610 }
611 pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelize(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices, float voxelSize)
612 {
613  return voxelizeImpl<pcl::PointXYZRGB>(cloud, indices, voxelSize);
614 }
615 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr voxelize(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, const pcl::IndicesPtr & indices, float voxelSize)
616 {
617  return voxelizeImpl<pcl::PointXYZRGBNormal>(cloud, indices, voxelSize);
618 }
619 pcl::PointCloud<pcl::PointXYZI>::Ptr voxelize(const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud, const pcl::IndicesPtr & indices, float voxelSize)
620 {
621  return voxelizeImpl<pcl::PointXYZI>(cloud, indices, voxelSize);
622 }
623 pcl::PointCloud<pcl::PointXYZINormal>::Ptr voxelize(const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud, const pcl::IndicesPtr & indices, float voxelSize)
624 {
625  return voxelizeImpl<pcl::PointXYZINormal>(cloud, indices, voxelSize);
626 }
627 
628 pcl::PointCloud<pcl::PointXYZ>::Ptr voxelize(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, float voxelSize)
629 {
630  pcl::IndicesPtr indices(new std::vector<int>);
631  return voxelize(cloud, indices, voxelSize);
632 }
633 pcl::PointCloud<pcl::PointNormal>::Ptr voxelize(const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, float voxelSize)
634 {
635  pcl::IndicesPtr indices(new std::vector<int>);
636  return voxelize(cloud, indices, voxelSize);
637 }
638 pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelize(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, float voxelSize)
639 {
640  pcl::IndicesPtr indices(new std::vector<int>);
641  return voxelize(cloud, indices, voxelSize);
642 }
643 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr voxelize(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, float voxelSize)
644 {
645  pcl::IndicesPtr indices(new std::vector<int>);
646  return voxelize(cloud, indices, voxelSize);
647 }
648 pcl::PointCloud<pcl::PointXYZI>::Ptr voxelize(const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud, float voxelSize)
649 {
650  pcl::IndicesPtr indices(new std::vector<int>);
651  return voxelize(cloud, indices, voxelSize);
652 }
653 pcl::PointCloud<pcl::PointXYZINormal>::Ptr voxelize(const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud, float voxelSize)
654 {
655  pcl::IndicesPtr indices(new std::vector<int>);
656  return voxelize(cloud, indices, voxelSize);
657 }
658 
659 template<typename PointT>
660 typename pcl::PointCloud<PointT>::Ptr randomSamplingImpl(
661  const typename pcl::PointCloud<PointT>::Ptr & cloud, int samples)
662 {
663  UASSERT(samples > 0);
664  typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
665  pcl::RandomSample<PointT> filter;
666  filter.setSample(samples);
667  filter.setSeed (std::rand ());
668  filter.setInputCloud(cloud);
669  filter.filter(*output);
670  return output;
671 }
672 pcl::PointCloud<pcl::PointXYZ>::Ptr randomSampling(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, int samples)
673 {
674  return randomSamplingImpl<pcl::PointXYZ>(cloud, samples);
675 }
676 pcl::PointCloud<pcl::PointNormal>::Ptr randomSampling(const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, int samples)
677 {
678  return randomSamplingImpl<pcl::PointNormal>(cloud, samples);
679 }
680 pcl::PointCloud<pcl::PointXYZRGB>::Ptr randomSampling(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, int samples)
681 {
682  return randomSamplingImpl<pcl::PointXYZRGB>(cloud, samples);
683 }
684 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr randomSampling(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, int samples)
685 {
686  return randomSamplingImpl<pcl::PointXYZRGBNormal>(cloud, samples);
687 }
688 pcl::PointCloud<pcl::PointXYZI>::Ptr randomSampling(const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud, int samples)
689 {
690  return randomSamplingImpl<pcl::PointXYZI>(cloud, samples);
691 }
692 pcl::PointCloud<pcl::PointXYZINormal>::Ptr randomSampling(const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud, int samples)
693 {
694  return randomSamplingImpl<pcl::PointXYZINormal>(cloud, samples);
695 }
696 
697 template<typename PointT>
698 pcl::IndicesPtr passThroughImpl(
699  const typename pcl::PointCloud<PointT>::Ptr & cloud,
700  const pcl::IndicesPtr & indices,
701  const std::string & axis,
702  float min,
703  float max,
704  bool negative)
705 {
706  UASSERT_MSG(max > min, uFormat("cloud=%d, max=%f min=%f axis=%s", (int)cloud->size(), max, min, axis.c_str()).c_str());
707  UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0);
708 
709  pcl::IndicesPtr output(new std::vector<int>);
710  pcl::PassThrough<PointT> filter;
711  filter.setNegative(negative);
712  filter.setFilterFieldName(axis);
713  filter.setFilterLimits(min, max);
714  filter.setInputCloud(cloud);
715  filter.setIndices(indices);
716  filter.filter(*output);
717  return output;
718 }
719 
720 pcl::IndicesPtr passThrough(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, const pcl::IndicesPtr & indices, const std::string & axis, float min, float max, bool negative)
721 {
722  return passThroughImpl<pcl::PointXYZ>(cloud, indices, axis, min, max, negative);
723 }
724 pcl::IndicesPtr passThrough(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices, const std::string & axis, float min, float max, bool negative)
725 {
726  return passThroughImpl<pcl::PointXYZRGB>(cloud, indices, axis, min, max, negative);
727 }
728 pcl::IndicesPtr passThrough(const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud, const pcl::IndicesPtr & indices, const std::string & axis, float min, float max, bool negative)
729 {
730  return passThroughImpl<pcl::PointXYZI>(cloud, indices, axis, min, max, negative);
731 }
732 pcl::IndicesPtr passThrough(const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, const pcl::IndicesPtr & indices, const std::string & axis, float min, float max, bool negative)
733 {
734  return passThroughImpl<pcl::PointNormal>(cloud, indices, axis, min, max, negative);
735 }
736 pcl::IndicesPtr passThrough(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, const pcl::IndicesPtr & indices, const std::string & axis, float min, float max, bool negative)
737 {
738  return passThroughImpl<pcl::PointXYZRGBNormal>(cloud, indices, axis, min, max, negative);
739 }
740 pcl::IndicesPtr passThrough(const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud, const pcl::IndicesPtr & indices, const std::string & axis, float min, float max, bool negative)
741 {
742  return passThroughImpl<pcl::PointXYZINormal>(cloud, indices, axis, min, max, negative);
743 }
744 
745 template<typename PointT>
746 typename pcl::PointCloud<PointT>::Ptr passThroughImpl(
747  const typename pcl::PointCloud<PointT>::Ptr & cloud,
748  const std::string & axis,
749  float min,
750  float max,
751  bool negative)
752 {
753  UASSERT_MSG(max > min, uFormat("cloud=%d, max=%f min=%f axis=%s", (int)cloud->size(), max, min, axis.c_str()).c_str());
754  UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0);
755 
756  typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
757  pcl::PassThrough<PointT> filter;
758  filter.setNegative(negative);
759  filter.setFilterFieldName(axis);
760  filter.setFilterLimits(min, max);
761  filter.setInputCloud(cloud);
762  filter.filter(*output);
763  return output;
764 }
765 pcl::PointCloud<pcl::PointXYZ>::Ptr passThrough(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, const std::string & axis, float min, float max, bool negative)
766 {
767  return passThroughImpl<pcl::PointXYZ>(cloud, axis, min ,max, negative);
768 }
769 pcl::PointCloud<pcl::PointXYZRGB>::Ptr passThrough(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const std::string & axis, float min, float max, bool negative)
770 {
771  return passThroughImpl<pcl::PointXYZRGB>(cloud, axis, min ,max, negative);
772 }
773 pcl::PointCloud<pcl::PointXYZI>::Ptr passThrough(const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud, const std::string & axis, float min, float max, bool negative)
774 {
775  return passThroughImpl<pcl::PointXYZI>(cloud, axis, min ,max, negative);
776 }
777 pcl::PointCloud<pcl::PointNormal>::Ptr passThrough(const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, const std::string & axis, float min, float max, bool negative)
778 {
779  return passThroughImpl<pcl::PointNormal>(cloud, axis, min ,max, negative);
780 }
781 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr passThrough(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, const std::string & axis, float min, float max, bool negative)
782 {
783  return passThroughImpl<pcl::PointXYZRGBNormal>(cloud, axis, min ,max, negative);
784 }
785 pcl::PointCloud<pcl::PointXYZINormal>::Ptr passThrough(const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud, const std::string & axis, float min, float max, bool negative)
786 {
787  return passThroughImpl<pcl::PointXYZINormal>(cloud, axis, min ,max, negative);
788 }
789 
790 template<typename PointT>
791 pcl::IndicesPtr cropBoxImpl(
792  const typename pcl::PointCloud<PointT>::Ptr & cloud,
793  const pcl::IndicesPtr & indices,
794  const Eigen::Vector4f & min,
795  const Eigen::Vector4f & max,
796  const Transform & transform,
797  bool negative)
798 {
799  UASSERT(min[0] < max[0] && min[1] < max[1] && min[2] < max[2]);
800 
801  pcl::IndicesPtr output(new std::vector<int>);
802  pcl::CropBox<PointT> filter;
803  filter.setNegative(negative);
804  filter.setMin(min);
805  filter.setMax(max);
806  if(!transform.isNull() && !transform.isIdentity())
807  {
808  filter.setTransform(transform.toEigen3f());
809  }
810  filter.setInputCloud(cloud);
811  filter.setIndices(indices);
812  filter.filter(*output);
813  return output;
814 }
815 
816 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)
817 {
818  UASSERT(min[0] < max[0] && min[1] < max[1] && min[2] < max[2]);
819 
820  pcl::IndicesPtr output(new std::vector<int>);
821  pcl::CropBox<pcl::PCLPointCloud2> filter;
822  filter.setNegative(negative);
823  filter.setMin(min);
824  filter.setMax(max);
825  if(!transform.isNull() && !transform.isIdentity())
826  {
827  filter.setTransform(transform.toEigen3f());
828  }
829  filter.setInputCloud(cloud);
830  filter.setIndices(indices);
831  filter.filter(*output);
832  return output;
833 }
834 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)
835 {
836  return cropBoxImpl<pcl::PointXYZ>(cloud, indices, min, max, transform, negative);
837 }
838 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)
839 {
840  return cropBoxImpl<pcl::PointNormal>(cloud, indices, min, max, transform, negative);
841 }
842 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)
843 {
844  return cropBoxImpl<pcl::PointXYZRGB>(cloud, indices, min, max, transform, negative);
845 }
846 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)
847 {
848  return cropBoxImpl<pcl::PointXYZRGBNormal>(cloud, indices, min, max, transform, negative);
849 }
850 pcl::IndicesPtr cropBox(const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud, const pcl::IndicesPtr & indices, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform, bool negative)
851 {
852  return cropBoxImpl<pcl::PointXYZI>(cloud, indices, min, max, transform, negative);
853 }
854 pcl::IndicesPtr cropBox(const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud, const pcl::IndicesPtr & indices, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform, bool negative)
855 {
856  return cropBoxImpl<pcl::PointXYZINormal>(cloud, indices, min, max, transform, negative);
857 }
858 
859 template<typename PointT>
860 typename pcl::PointCloud<PointT>::Ptr cropBoxImpl(
861  const typename pcl::PointCloud<PointT>::Ptr & cloud,
862  const Eigen::Vector4f & min,
863  const Eigen::Vector4f & max,
864  const Transform & transform,
865  bool negative)
866 {
867  UASSERT(min[0] < max[0] && min[1] < max[1] && min[2] < max[2]);
868 
869  typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
870  pcl::CropBox<PointT> filter;
871  filter.setNegative(negative);
872  filter.setMin(min);
873  filter.setMax(max);
874  if(!transform.isNull() && !transform.isIdentity())
875  {
876  filter.setTransform(transform.toEigen3f());
877  }
878  filter.setInputCloud(cloud);
879  filter.filter(*output);
880  return output;
881 }
882 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)
883 {
884  return cropBoxImpl<pcl::PointXYZ>(cloud, min, max, transform, negative);
885 }
886 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)
887 {
888  return cropBoxImpl<pcl::PointNormal>(cloud, min, max, transform, negative);
889 }
890 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)
891 {
892  return cropBoxImpl<pcl::PointXYZRGB>(cloud, min, max, transform, negative);
893 }
894 pcl::PointCloud<pcl::PointXYZI>::Ptr cropBox(const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform, bool negative)
895 {
896  return cropBoxImpl<pcl::PointXYZI>(cloud, min, max, transform, negative);
897 }
898 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)
899 {
900  return cropBoxImpl<pcl::PointXYZINormal>(cloud, min, max, transform, negative);
901 }
902 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)
903 {
904  return cropBoxImpl<pcl::PointXYZRGBNormal>(cloud, min, max, transform, negative);
905 }
906 
907 template<typename PointT>
908 pcl::IndicesPtr frustumFilteringImpl(
909  const typename pcl::PointCloud<PointT>::Ptr & cloud,
910  const pcl::IndicesPtr & indices,
911  const Transform & cameraPose,
912  float horizontalFOV, // in degrees
913  float verticalFOV, // in degrees
914  float nearClipPlaneDistance,
915  float farClipPlaneDistance,
916  bool negative)
917 {
918  UASSERT(horizontalFOV > 0.0f && verticalFOV > 0.0f);
919  UASSERT(farClipPlaneDistance > nearClipPlaneDistance);
920  UASSERT(!cameraPose.isNull());
921 
922  pcl::IndicesPtr output(new std::vector<int>);
923  pcl::FrustumCulling<PointT> fc;
924  fc.setNegative(negative);
925  fc.setInputCloud (cloud);
926  if(indices.get() && indices->size())
927  {
928  fc.setIndices(indices);
929  }
930  fc.setVerticalFOV (verticalFOV);
931  fc.setHorizontalFOV (horizontalFOV);
932  fc.setNearPlaneDistance (nearClipPlaneDistance);
933  fc.setFarPlaneDistance (farClipPlaneDistance);
934 
935  fc.setCameraPose (cameraPose.toEigen4f());
936  fc.filter (*output);
937 
938  return output;
939 }
940 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)
941 {
942  return frustumFilteringImpl<pcl::PointXYZ>(cloud, indices, cameraPose, horizontalFOV, verticalFOV, nearClipPlaneDistance, farClipPlaneDistance, negative);
943 }
944 
945 template<typename PointT>
946 typename pcl::PointCloud<PointT>::Ptr frustumFilteringImpl(
947  const typename pcl::PointCloud<PointT>::Ptr & cloud,
948  const Transform & cameraPose,
949  float horizontalFOV, // in degrees
950  float verticalFOV, // in degrees
951  float nearClipPlaneDistance,
952  float farClipPlaneDistance,
953  bool negative)
954 {
955  UASSERT(horizontalFOV > 0.0f && verticalFOV > 0.0f);
956  UASSERT(farClipPlaneDistance > nearClipPlaneDistance);
957  UASSERT(!cameraPose.isNull());
958 
959  typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
960  pcl::FrustumCulling<PointT> fc;
961  fc.setNegative(negative);
962  fc.setInputCloud (cloud);
963  fc.setVerticalFOV (verticalFOV);
964  fc.setHorizontalFOV (horizontalFOV);
965  fc.setNearPlaneDistance (nearClipPlaneDistance);
966  fc.setFarPlaneDistance (farClipPlaneDistance);
967 
968  fc.setCameraPose (cameraPose.toEigen4f());
969  fc.filter (*output);
970 
971  return output;
972 }
973 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)
974 {
975  return frustumFilteringImpl<pcl::PointXYZ>(cloud, cameraPose, horizontalFOV, verticalFOV, nearClipPlaneDistance, farClipPlaneDistance, negative);
976 }
977 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)
978 {
979  return frustumFilteringImpl<pcl::PointXYZRGB>(cloud, cameraPose, horizontalFOV, verticalFOV, nearClipPlaneDistance, farClipPlaneDistance, negative);
980 }
981 
982 
983 template<typename PointT>
984 typename pcl::PointCloud<PointT>::Ptr removeNaNFromPointCloudImpl(
985  const typename pcl::PointCloud<PointT>::Ptr & cloud)
986 {
987  typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
988  std::vector<int> indices;
989  pcl::removeNaNFromPointCloud(*cloud, *output, indices);
990  return output;
991 }
992 pcl::PointCloud<pcl::PointXYZ>::Ptr removeNaNFromPointCloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud)
993 {
994  return removeNaNFromPointCloudImpl<pcl::PointXYZ>(cloud);
995 }
996 pcl::PointCloud<pcl::PointXYZRGB>::Ptr removeNaNFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud)
997 {
998  return removeNaNFromPointCloudImpl<pcl::PointXYZRGB>(cloud);
999 }
1000 pcl::PointCloud<pcl::PointXYZI>::Ptr removeNaNFromPointCloud(const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud)
1001 {
1002  return removeNaNFromPointCloudImpl<pcl::PointXYZI>(cloud);
1003 }
1004 
1005 pcl::PCLPointCloud2::Ptr RTABMAP_EXP removeNaNFromPointCloud(const pcl::PCLPointCloud2::Ptr & cloud)
1006 {
1007  pcl::PCLPointCloud2::Ptr output(new pcl::PCLPointCloud2);
1008  output->fields = cloud->fields;
1009  output->header = cloud->header;
1010  output->height = 1;
1011  output->point_step = cloud->point_step;
1012  output->is_dense = true;
1013  output->data.resize(cloud->row_step*cloud->height);
1014 
1015  bool is3D = false;
1016  for(size_t i=0; i<cloud->fields.size(); ++i)
1017  {
1018  if(cloud->fields[i].name.compare("z") == 0)
1019  {
1020  is3D = true;
1021  break;
1022  }
1023  }
1024 
1025  std::uint8_t* output_data = reinterpret_cast<std::uint8_t*>(&output->data[0]);
1026 
1027  size_t oi = 0;
1028  for (size_t row = 0; row < cloud->height; ++row)
1029  {
1030  const std::uint8_t* row_data = &cloud->data[row * cloud->row_step];
1031  for (size_t col = 0; col < cloud->width; ++col)
1032  {
1033  const std::uint8_t* msg_data = row_data + col * cloud->point_step;
1034  const float * x = (const float*)&msg_data[0];
1035  const float * y = (const float*)&msg_data[4];
1036  const float * z = (const float*)&msg_data[is3D?8:4];
1037  if(uIsFinite(*x) && uIsFinite(*y) && uIsFinite(*z))
1038  {
1039  memcpy (output_data, msg_data, cloud->point_step);
1040  output_data += cloud->point_step;
1041  ++oi;
1042  }
1043  }
1044  }
1045  output->width = oi;
1046  output->row_step = output->width*output->point_step;
1047  output->data.resize(output->row_step);
1048  return output;
1049 }
1050 
1051 template<typename PointT>
1052 typename pcl::PointCloud<PointT>::Ptr removeNaNNormalsFromPointCloudImpl(
1053  const typename pcl::PointCloud<PointT>::Ptr & cloud)
1054 {
1055  typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
1056  std::vector<int> indices;
1057  pcl::removeNaNNormalsFromPointCloud(*cloud, *output, indices);
1058  return output;
1059 }
1060 pcl::PointCloud<pcl::PointNormal>::Ptr removeNaNNormalsFromPointCloud(
1061  const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud)
1062 {
1063  return removeNaNNormalsFromPointCloudImpl<pcl::PointNormal>(cloud);
1064 }
1065 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr removeNaNNormalsFromPointCloud(
1066  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud)
1067 {
1068  return removeNaNNormalsFromPointCloudImpl<pcl::PointXYZRGBNormal>(cloud);
1069 }
1070 pcl::PointCloud<pcl::PointXYZINormal>::Ptr removeNaNNormalsFromPointCloud(
1071  const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud)
1072 {
1073  return removeNaNNormalsFromPointCloudImpl<pcl::PointXYZINormal>(cloud);
1074 }
1075 
1076 
1077 pcl::IndicesPtr radiusFiltering(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, float radiusSearch, int minNeighborsInRadius)
1078 {
1079  pcl::IndicesPtr indices(new std::vector<int>);
1080  return radiusFiltering(cloud, indices, radiusSearch, minNeighborsInRadius);
1081 }
1082 pcl::IndicesPtr radiusFiltering(const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, float radiusSearch, int minNeighborsInRadius)
1083 {
1084  pcl::IndicesPtr indices(new std::vector<int>);
1085  return radiusFiltering(cloud, indices, radiusSearch, minNeighborsInRadius);
1086 }
1087 pcl::IndicesPtr radiusFiltering(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, float radiusSearch, int minNeighborsInRadius)
1088 {
1089  pcl::IndicesPtr indices(new std::vector<int>);
1090  return radiusFiltering(cloud, indices, radiusSearch, minNeighborsInRadius);
1091 }
1092 pcl::IndicesPtr radiusFiltering(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, float radiusSearch, int minNeighborsInRadius)
1093 {
1094  pcl::IndicesPtr indices(new std::vector<int>);
1095  return radiusFiltering(cloud, indices, radiusSearch, minNeighborsInRadius);
1096 }
1097 pcl::IndicesPtr radiusFiltering(const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud, float radiusSearch, int minNeighborsInRadius)
1098 {
1099  pcl::IndicesPtr indices(new std::vector<int>);
1100  return radiusFiltering(cloud, indices, radiusSearch, minNeighborsInRadius);
1101 }
1102 pcl::IndicesPtr radiusFiltering(const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud, float radiusSearch, int minNeighborsInRadius)
1103 {
1104  pcl::IndicesPtr indices(new std::vector<int>);
1105  return radiusFiltering(cloud, indices, radiusSearch, minNeighborsInRadius);
1106 }
1107 
1108 template<typename PointT>
1109 pcl::IndicesPtr radiusFilteringImpl(
1110  const typename pcl::PointCloud<PointT>::Ptr & cloud,
1111  const pcl::IndicesPtr & indices,
1112  float radiusSearch,
1113  int minNeighborsInRadius)
1114 {
1115  typename pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>(false));
1116 
1117  if(indices->size())
1118  {
1119  pcl::IndicesPtr output(new std::vector<int>(indices->size()));
1120  int oi = 0; // output iterator
1121  tree->setInputCloud(cloud, indices);
1122  for(unsigned int i=0; i<indices->size(); ++i)
1123  {
1124  std::vector<int> kIndices;
1125  std::vector<float> kDistances;
1126  int k = tree->radiusSearch(cloud->at(indices->at(i)), radiusSearch, kIndices, kDistances, minNeighborsInRadius+1);
1127  if(k > minNeighborsInRadius)
1128  {
1129  output->at(oi++) = indices->at(i);
1130  }
1131  }
1132  output->resize(oi);
1133  return output;
1134  }
1135  else
1136  {
1137  pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
1138  int oi = 0; // output iterator
1139  tree->setInputCloud(cloud);
1140  for(unsigned int i=0; i<cloud->size(); ++i)
1141  {
1142  std::vector<int> kIndices;
1143  std::vector<float> kDistances;
1144  int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances, minNeighborsInRadius+1);
1145  if(k > minNeighborsInRadius)
1146  {
1147  output->at(oi++) = i;
1148  }
1149  }
1150  output->resize(oi);
1151  return output;
1152  }
1153 }
1154 
1155 pcl::IndicesPtr radiusFiltering(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, const pcl::IndicesPtr & indices, float radiusSearch, int minNeighborsInRadius)
1156 {
1157  return radiusFilteringImpl<pcl::PointXYZ>(cloud, indices, radiusSearch, minNeighborsInRadius);
1158 }
1159 pcl::IndicesPtr radiusFiltering(const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, const pcl::IndicesPtr & indices, float radiusSearch, int minNeighborsInRadius)
1160 {
1161  return radiusFilteringImpl<pcl::PointNormal>(cloud, indices, radiusSearch, minNeighborsInRadius);
1162 }
1163 pcl::IndicesPtr radiusFiltering(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices, float radiusSearch, int minNeighborsInRadius)
1164 {
1165  return radiusFilteringImpl<pcl::PointXYZRGB>(cloud, indices, radiusSearch, minNeighborsInRadius);
1166 }
1167 pcl::IndicesPtr radiusFiltering(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, const pcl::IndicesPtr & indices, float radiusSearch, int minNeighborsInRadius)
1168 {
1169  return radiusFilteringImpl<pcl::PointXYZRGBNormal>(cloud, indices, radiusSearch, minNeighborsInRadius);
1170 }
1171 pcl::IndicesPtr radiusFiltering(const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud, const pcl::IndicesPtr & indices, float radiusSearch, int minNeighborsInRadius)
1172 {
1173  return radiusFilteringImpl<pcl::PointXYZI>(cloud, indices, radiusSearch, minNeighborsInRadius);
1174 }
1175 pcl::IndicesPtr radiusFiltering(const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud, const pcl::IndicesPtr & indices, float radiusSearch, int minNeighborsInRadius)
1176 {
1177  return radiusFilteringImpl<pcl::PointXYZINormal>(cloud, indices, radiusSearch, minNeighborsInRadius);
1178 }
1179 
1180 pcl::IndicesPtr proportionalRadiusFiltering(
1181  const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
1182  const std::vector<int> & viewpointIndices,
1183  const std::map<int, Transform> & viewpoints,
1184  float factor,
1185  float neighborScale)
1186 {
1187  pcl::IndicesPtr indices(new std::vector<int>);
1188  return proportionalRadiusFiltering(cloud, indices, viewpointIndices, viewpoints, factor, neighborScale);
1189 }
1190 pcl::IndicesPtr proportionalRadiusFiltering(
1191  const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
1192  const std::vector<int> & viewpointIndices,
1193  const std::map<int, Transform> & viewpoints,
1194  float factor,
1195  float neighborScale)
1196 {
1197  pcl::IndicesPtr indices(new std::vector<int>);
1198  return proportionalRadiusFiltering(cloud, indices, viewpointIndices, viewpoints, factor, neighborScale);
1199 }
1200 pcl::IndicesPtr proportionalRadiusFiltering(
1201  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
1202  const std::vector<int> & viewpointIndices,
1203  const std::map<int, Transform> & viewpoints,
1204  float factor,
1205  float neighborScale)
1206 {
1207  pcl::IndicesPtr indices(new std::vector<int>);
1208  return proportionalRadiusFiltering(cloud, indices, viewpointIndices, viewpoints, factor, neighborScale);
1209 }
1210 pcl::IndicesPtr proportionalRadiusFiltering(
1211  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
1212  const std::vector<int> & viewpointIndices,
1213  const std::map<int, Transform> & viewpoints,
1214  float factor,
1215  float neighborScale)
1216 {
1217  pcl::IndicesPtr indices(new std::vector<int>);
1218  return proportionalRadiusFiltering(cloud, indices, viewpointIndices, viewpoints, factor, neighborScale);
1219 }
1220 pcl::IndicesPtr proportionalRadiusFiltering(
1221  const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
1222  const std::vector<int> & viewpointIndices,
1223  const std::map<int, Transform> & viewpoints,
1224  float factor,
1225  float neighborScale)
1226 {
1227  pcl::IndicesPtr indices(new std::vector<int>);
1228  return proportionalRadiusFiltering(cloud, indices, viewpointIndices, viewpoints, factor, neighborScale);
1229 }
1230 pcl::IndicesPtr proportionalRadiusFiltering(
1231  const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
1232  const std::vector<int> & viewpointIndices,
1233  const std::map<int, Transform> & viewpoints,
1234  float factor,
1235  float neighborScale)
1236 {
1237  pcl::IndicesPtr indices(new std::vector<int>);
1238  return proportionalRadiusFiltering(cloud, indices, viewpointIndices, viewpoints, factor, neighborScale);
1239 }
1240 
1241 template<typename PointT>
1242 pcl::IndicesPtr proportionalRadiusFilteringImpl(
1243  const typename pcl::PointCloud<PointT>::Ptr & cloud,
1244  const pcl::IndicesPtr & indices,
1245  const std::vector<int> & viewpointIndices,
1246  const std::map<int, Transform> & viewpoints,
1247  float factor,
1248  float neighborScale)
1249 {
1250  typename pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>(false));
1251 
1252  UASSERT(cloud->size() == viewpointIndices.size());
1253  UASSERT(factor>0.0f);
1254  UASSERT(neighborScale>=1.0f);
1255 
1256  if(!indices->empty())
1257  {
1258  std::vector<bool> kept(indices->size());
1259  tree->setInputCloud(cloud, indices);
1260  for(size_t i=0; i<indices->size(); ++i)
1261  {
1262  int index = indices->at(i);
1263  std::vector<int> kIndices;
1264  std::vector<float> kDistances;
1265  std::map<int, Transform>::const_iterator viewpointIter = viewpoints.find(viewpointIndices[index]);
1266  UASSERT(viewpointIter != viewpoints.end());
1267  cv::Point3f viewpoint(viewpointIter->second.x(), viewpointIter->second.y(), viewpointIter->second.z());
1268  cv::Point3f point = cv::Point3f(cloud->at(index).x,cloud->at(index).y, cloud->at(index).z);
1269  float radiusSearch = factor * cv::norm(viewpoint-point);
1270  int k = tree->radiusSearch(cloud->at(index), radiusSearch, kIndices, kDistances);
1271  bool keep = k>0;
1272  for(int j=0; j<k && keep; ++j)
1273  {
1274  if(kIndices[j] != index)
1275  {
1276  cv::Point3f pointTmp(cloud->at(kIndices[j]).x,cloud->at(kIndices[j]).y, cloud->at(kIndices[j]).z);
1277  cv::Point3f tmp = pointTmp - point;
1278  float distPtSqr = tmp.dot(tmp); // L2sqr
1279  viewpointIter = viewpoints.find(viewpointIndices[kIndices[j]]);
1280  UASSERT(viewpointIter != viewpoints.end());
1281  viewpoint = cv::Point3f(viewpointIter->second.x(), viewpointIter->second.y(), viewpointIter->second.z());
1282  float radiusSearchTmp = factor * cv::norm(viewpoint-pointTmp) * neighborScale;
1283  if(distPtSqr > radiusSearchTmp*radiusSearchTmp)
1284  {
1285  keep = false;
1286  }
1287  }
1288  }
1289  kept[i] = keep;
1290  }
1291  pcl::IndicesPtr output(new std::vector<int>(indices->size()));
1292  int oi = 0;
1293  for(size_t i=0; i<indices->size(); ++i)
1294  {
1295  if(kept[i])
1296  {
1297  output->at(oi++) = indices->at(i);
1298  }
1299  }
1300  output->resize(oi);
1301  return output;
1302  }
1303  else
1304  {
1305  std::vector<bool> kept(cloud->size());
1306  tree->setInputCloud(cloud);
1307  #pragma omp parallel for
1308  for(int i=0; i<(int)cloud->size(); ++i)
1309  {
1310  std::vector<int> kIndices;
1311  std::vector<float> kDistances;
1312  std::map<int, Transform>::const_iterator viewpointIter = viewpoints.find(viewpointIndices[i]);
1313  UASSERT(viewpointIter != viewpoints.end());
1314  cv::Point3f viewpoint(viewpointIter->second.x(), viewpointIter->second.y(), viewpointIter->second.z());
1315  cv::Point3f point = cv::Point3f(cloud->at(i).x,cloud->at(i).y, cloud->at(i).z);
1316  float radiusSearch = factor * cv::norm(viewpoint-point);
1317  int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances);
1318  bool keep = k>0;
1319  for(int j=0; j<k && keep; ++j)
1320  {
1321  if(kIndices[j] != (int)i)
1322  {
1323  cv::Point3f pointTmp(cloud->at(kIndices[j]).x,cloud->at(kIndices[j]).y, cloud->at(kIndices[j]).z);
1324  cv::Point3f tmp = pointTmp - point;
1325  float distPtSqr = tmp.dot(tmp); // L2sqr
1326  viewpointIter = viewpoints.find(viewpointIndices[kIndices[j]]);
1327  UASSERT(viewpointIter != viewpoints.end());
1328  viewpoint = cv::Point3f(viewpointIter->second.x(), viewpointIter->second.y(), viewpointIter->second.z());
1329  float radiusSearchTmp = factor * cv::norm(viewpoint-pointTmp) * neighborScale;
1330  if(distPtSqr > radiusSearchTmp*radiusSearchTmp)
1331  {
1332  keep = false;
1333  }
1334  }
1335  }
1336  kept[i] = keep;
1337  }
1338  pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
1339  int oi = 0;
1340  for(size_t i=0; i<cloud->size(); ++i)
1341  {
1342  if(kept[i])
1343  {
1344  output->at(oi++) = i;
1345  }
1346  }
1347  output->resize(oi);
1348  return output;
1349  }
1350 }
1351 
1352 pcl::IndicesPtr proportionalRadiusFiltering(
1353  const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
1354  const pcl::IndicesPtr & indices,
1355  const std::vector<int> & viewpointIndices,
1356  const std::map<int, Transform> & viewpoints,
1357  float factor,
1358  float neighborScale)
1359 {
1360  return proportionalRadiusFilteringImpl<pcl::PointXYZ>(cloud, indices, viewpointIndices, viewpoints, factor, neighborScale);
1361 }
1362 pcl::IndicesPtr proportionalRadiusFiltering(
1363  const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
1364  const pcl::IndicesPtr & indices,
1365  const std::vector<int> & viewpointIndices,
1366  const std::map<int, Transform> & viewpoints,
1367  float factor,
1368  float neighborScale)
1369 {
1370  return proportionalRadiusFilteringImpl<pcl::PointNormal>(cloud, indices, viewpointIndices, viewpoints, factor, neighborScale);
1371 }
1372 pcl::IndicesPtr proportionalRadiusFiltering(
1373  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
1374  const pcl::IndicesPtr & indices,
1375  const std::vector<int> & viewpointIndices,
1376  const std::map<int, Transform> & viewpoints,
1377  float factor,
1378  float neighborScale)
1379 {
1380  return proportionalRadiusFilteringImpl<pcl::PointXYZRGB>(cloud, indices, viewpointIndices, viewpoints, factor, neighborScale);
1381 }
1382 pcl::IndicesPtr proportionalRadiusFiltering(
1383  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
1384  const pcl::IndicesPtr & indices,
1385  const std::vector<int> & viewpointIndices,
1386  const std::map<int, Transform> & viewpoints,
1387  float factor,
1388  float neighborScale)
1389 {
1390  return proportionalRadiusFilteringImpl<pcl::PointXYZRGBNormal>(cloud, indices, viewpointIndices, viewpoints, factor, neighborScale);
1391 }
1392 pcl::IndicesPtr proportionalRadiusFiltering(
1393  const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
1394  const pcl::IndicesPtr & indices,
1395  const std::vector<int> & viewpointIndices,
1396  const std::map<int, Transform> & viewpoints,
1397  float factor,
1398  float neighborScale)
1399 {
1400  return proportionalRadiusFilteringImpl<pcl::PointXYZI>(cloud, indices, viewpointIndices, viewpoints, factor, neighborScale);
1401 }
1402 pcl::IndicesPtr proportionalRadiusFiltering(
1403  const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
1404  const pcl::IndicesPtr & indices,
1405  const std::vector<int> & viewpointIndices,
1406  const std::map<int, Transform> & viewpoints,
1407  float factor,
1408  float neighborScale)
1409 {
1410  return proportionalRadiusFilteringImpl<pcl::PointXYZINormal>(cloud, indices, viewpointIndices, viewpoints, factor, neighborScale);
1411 }
1412 
1413 pcl::PointCloud<pcl::PointXYZRGB>::Ptr subtractFiltering(
1414  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
1415  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & substractCloud,
1416  float radiusSearch,
1417  int minNeighborsInRadius)
1418 {
1419  pcl::IndicesPtr indices(new std::vector<int>);
1420  pcl::IndicesPtr indicesOut = subtractFiltering(cloud, indices, substractCloud, indices, radiusSearch, minNeighborsInRadius);
1421  pcl::PointCloud<pcl::PointXYZRGB>::Ptr out(new pcl::PointCloud<pcl::PointXYZRGB>);
1422  pcl::copyPointCloud(*cloud, *indicesOut, *out);
1423  return out;
1424 }
1425 
1426 template<typename PointT>
1427 pcl::IndicesPtr subtractFilteringImpl(
1428  const typename pcl::PointCloud<PointT>::Ptr & cloud,
1429  const pcl::IndicesPtr & indices,
1430  const typename pcl::PointCloud<PointT>::Ptr & substractCloud,
1431  const pcl::IndicesPtr & substractIndices,
1432  float radiusSearch,
1433  int minNeighborsInRadius)
1434 {
1435  UASSERT(minNeighborsInRadius > 0);
1436  typename pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>(false));
1437 
1438  if(indices->size())
1439  {
1440  pcl::IndicesPtr output(new std::vector<int>(indices->size()));
1441  int oi = 0; // output iterator
1442  if(substractIndices->size())
1443  {
1444  tree->setInputCloud(substractCloud, substractIndices);
1445  }
1446  else
1447  {
1448  tree->setInputCloud(substractCloud);
1449  }
1450  for(unsigned int i=0; i<indices->size(); ++i)
1451  {
1452  std::vector<int> kIndices;
1453  std::vector<float> kDistances;
1454  int k = tree->radiusSearch(cloud->at(indices->at(i)), radiusSearch, kIndices, kDistances);
1455  if(k < minNeighborsInRadius)
1456  {
1457  output->at(oi++) = indices->at(i);
1458  }
1459  }
1460  output->resize(oi);
1461  return output;
1462  }
1463  else
1464  {
1465  pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
1466  int oi = 0; // output iterator
1467  if(substractIndices->size())
1468  {
1469  tree->setInputCloud(substractCloud, substractIndices);
1470  }
1471  else
1472  {
1473  tree->setInputCloud(substractCloud);
1474  }
1475  for(unsigned int i=0; i<cloud->size(); ++i)
1476  {
1477  std::vector<int> kIndices;
1478  std::vector<float> kDistances;
1479  int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances);
1480  if(k < minNeighborsInRadius)
1481  {
1482  output->at(oi++) = i;
1483  }
1484  }
1485  output->resize(oi);
1486  return output;
1487  }
1488 }
1489 pcl::IndicesPtr subtractFiltering(
1490  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
1491  const pcl::IndicesPtr & indices,
1492  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & substractCloud,
1493  const pcl::IndicesPtr & substractIndices,
1494  float radiusSearch,
1495  int minNeighborsInRadius)
1496 {
1497  return subtractFilteringImpl<pcl::PointXYZRGB>(cloud, indices, substractCloud, substractIndices, radiusSearch, minNeighborsInRadius);
1498 }
1499 
1500 pcl::PointCloud<pcl::PointNormal>::Ptr subtractFiltering(
1501  const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
1502  const pcl::PointCloud<pcl::PointNormal>::Ptr & substractCloud,
1503  float radiusSearch,
1504  float maxAngle,
1505  int minNeighborsInRadius)
1506 {
1507  pcl::IndicesPtr indices(new std::vector<int>);
1508  pcl::IndicesPtr indicesOut = subtractFiltering(cloud, indices, substractCloud, indices, radiusSearch, maxAngle, minNeighborsInRadius);
1509  pcl::PointCloud<pcl::PointNormal>::Ptr out(new pcl::PointCloud<pcl::PointNormal>);
1510  pcl::copyPointCloud(*cloud, *indicesOut, *out);
1511  return out;
1512 }
1513 pcl::PointCloud<pcl::PointXYZINormal>::Ptr subtractFiltering(
1514  const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
1515  const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & substractCloud,
1516  float radiusSearch,
1517  float maxAngle,
1518  int minNeighborsInRadius)
1519 {
1520  pcl::IndicesPtr indices(new std::vector<int>);
1521  pcl::IndicesPtr indicesOut = subtractFiltering(cloud, indices, substractCloud, indices, radiusSearch, maxAngle, minNeighborsInRadius);
1522  pcl::PointCloud<pcl::PointXYZINormal>::Ptr out(new pcl::PointCloud<pcl::PointXYZINormal>);
1523  pcl::copyPointCloud(*cloud, *indicesOut, *out);
1524  return out;
1525 }
1526 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr subtractFiltering(
1527  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
1528  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
1529  float radiusSearch,
1530  float maxAngle,
1531  int minNeighborsInRadius)
1532 {
1533  pcl::IndicesPtr indices(new std::vector<int>);
1534  pcl::IndicesPtr indicesOut = subtractFiltering(cloud, indices, substractCloud, indices, radiusSearch, maxAngle, minNeighborsInRadius);
1535  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr out(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
1536  pcl::copyPointCloud(*cloud, *indicesOut, *out);
1537  return out;
1538 }
1539 
1540 template<typename PointT>
1541 pcl::IndicesPtr subtractFilteringImpl(
1542  const typename pcl::PointCloud<PointT>::Ptr & cloud,
1543  const pcl::IndicesPtr & indices,
1544  const typename pcl::PointCloud<PointT>::Ptr & substractCloud,
1545  const pcl::IndicesPtr & substractIndices,
1546  float radiusSearch,
1547  float maxAngle,
1548  int minNeighborsInRadius)
1549 {
1550  UASSERT(minNeighborsInRadius > 0);
1551  typename pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>(false));
1552 
1553  if(indices->size())
1554  {
1555  pcl::IndicesPtr output(new std::vector<int>(indices->size()));
1556  int oi = 0; // output iterator
1557  if(substractIndices->size())
1558  {
1559  tree->setInputCloud(substractCloud, substractIndices);
1560  }
1561  else
1562  {
1563  tree->setInputCloud(substractCloud);
1564  }
1565  for(unsigned int i=0; i<indices->size(); ++i)
1566  {
1567  std::vector<int> kIndices;
1568  std::vector<float> kDistances;
1569  int k = tree->radiusSearch(cloud->at(indices->at(i)), radiusSearch, kIndices, kDistances);
1570  if(k>=minNeighborsInRadius && maxAngle > 0.0f)
1571  {
1572  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);
1573  if (uIsFinite(normal[0]) &&
1574  uIsFinite(normal[1]) &&
1575  uIsFinite(normal[2]))
1576  {
1577  int count = k;
1578  for(int j=0; j<count && k >= minNeighborsInRadius; ++j)
1579  {
1580  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);
1581  if(uIsFinite(v[0]) &&
1582  uIsFinite(v[1]) &&
1583  uIsFinite(v[2]))
1584  {
1585  float angle = pcl::getAngle3D(normal, v);
1586  if(angle > maxAngle)
1587  {
1588  k-=1;
1589  }
1590  }
1591  else
1592  {
1593  k-=1;
1594  }
1595  }
1596  }
1597  else
1598  {
1599  k=0;
1600  }
1601  }
1602  if(k < minNeighborsInRadius)
1603  {
1604  output->at(oi++) = indices->at(i);
1605  }
1606  }
1607  output->resize(oi);
1608  return output;
1609  }
1610  else
1611  {
1612  pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
1613  int oi = 0; // output iterator
1614  if(substractIndices->size())
1615  {
1616  tree->setInputCloud(substractCloud, substractIndices);
1617  }
1618  else
1619  {
1620  tree->setInputCloud(substractCloud);
1621  }
1622  for(unsigned int i=0; i<cloud->size(); ++i)
1623  {
1624  std::vector<int> kIndices;
1625  std::vector<float> kDistances;
1626  int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances);
1627  if(k>=minNeighborsInRadius && maxAngle > 0.0f)
1628  {
1629  Eigen::Vector4f normal(cloud->at(i).normal_x, cloud->at(i).normal_y, cloud->at(i).normal_z, 0.0f);
1630  if (uIsFinite(normal[0]) &&
1631  uIsFinite(normal[1]) &&
1632  uIsFinite(normal[2]))
1633  {
1634  int count = k;
1635  for(int j=0; j<count && k >= minNeighborsInRadius; ++j)
1636  {
1637  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);
1638  if(uIsFinite(v[0]) &&
1639  uIsFinite(v[1]) &&
1640  uIsFinite(v[2]))
1641  {
1642  float angle = pcl::getAngle3D(normal, v);
1643  if(angle > maxAngle)
1644  {
1645  k-=1;
1646  }
1647  }
1648  else
1649  {
1650  k-=1;
1651  }
1652  }
1653  }
1654  else
1655  {
1656  k=0;
1657  }
1658  }
1659  if(k < minNeighborsInRadius)
1660  {
1661  output->at(oi++) = i;
1662  }
1663  }
1664  output->resize(oi);
1665  return output;
1666  }
1667 }
1668 
1669 pcl::IndicesPtr subtractFiltering(
1670  const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
1671  const pcl::IndicesPtr & indices,
1672  const pcl::PointCloud<pcl::PointNormal>::Ptr & substractCloud,
1673  const pcl::IndicesPtr & substractIndices,
1674  float radiusSearch,
1675  float maxAngle,
1676  int minNeighborsInRadius)
1677 {
1678  return subtractFilteringImpl<pcl::PointNormal>(cloud, indices, substractCloud, substractIndices, radiusSearch, maxAngle, minNeighborsInRadius);
1679 }
1680 pcl::IndicesPtr subtractFiltering(
1681  const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
1682  const pcl::IndicesPtr & indices,
1683  const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & substractCloud,
1684  const pcl::IndicesPtr & substractIndices,
1685  float radiusSearch,
1686  float maxAngle,
1687  int minNeighborsInRadius)
1688 {
1689  return subtractFilteringImpl<pcl::PointXYZINormal>(cloud, indices, substractCloud, substractIndices, radiusSearch, maxAngle, minNeighborsInRadius);
1690 }
1691 pcl::IndicesPtr subtractFiltering(
1692  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
1693  const pcl::IndicesPtr & indices,
1694  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
1695  const pcl::IndicesPtr & substractIndices,
1696  float radiusSearch,
1697  float maxAngle,
1698  int minNeighborsInRadius)
1699 {
1700  return subtractFilteringImpl<pcl::PointXYZRGBNormal>(cloud, indices, substractCloud, substractIndices, radiusSearch, maxAngle, minNeighborsInRadius);
1701 }
1702 
1703 pcl::IndicesPtr subtractAdaptiveFiltering(
1704  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
1705  const pcl::IndicesPtr & indices,
1706  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & substractCloud,
1707  const pcl::IndicesPtr & substractIndices,
1708  float radiusSearchRatio,
1709  int minNeighborsInRadius,
1710  const Eigen::Vector3f & viewpoint)
1711 {
1712  UWARN("Add angle to avoid subtraction of points with opposite normals");
1713  UASSERT(minNeighborsInRadius > 0);
1714  UASSERT(radiusSearchRatio > 0.0f);
1715  pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>(false));
1716 
1717  if(indices->size())
1718  {
1719  pcl::IndicesPtr output(new std::vector<int>(indices->size()));
1720  int oi = 0; // output iterator
1721  if(substractIndices->size())
1722  {
1723  tree->setInputCloud(substractCloud, substractIndices);
1724  }
1725  else
1726  {
1727  tree->setInputCloud(substractCloud);
1728  }
1729  for(unsigned int i=0; i<indices->size(); ++i)
1730  {
1731  if(pcl::isFinite(cloud->at(indices->at(i))))
1732  {
1733  std::vector<int> kIndices;
1734  std::vector<float> kSqrdDistances;
1735  float radius = radiusSearchRatio*uNorm(
1736  cloud->at(indices->at(i)).x-viewpoint[0],
1737  cloud->at(indices->at(i)).y-viewpoint[1],
1738  cloud->at(indices->at(i)).z-viewpoint[2]);
1739  int k = tree->radiusSearch(cloud->at(indices->at(i)), radius, kIndices, kSqrdDistances);
1740  if(k < minNeighborsInRadius)
1741  {
1742  output->at(oi++) = indices->at(i);
1743  }
1744  }
1745  }
1746  output->resize(oi);
1747  return output;
1748  }
1749  else
1750  {
1751  pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
1752  int oi = 0; // output iterator
1753  if(substractIndices->size())
1754  {
1755  tree->setInputCloud(substractCloud, substractIndices);
1756  }
1757  else
1758  {
1759  tree->setInputCloud(substractCloud);
1760  }
1761  for(unsigned int i=0; i<cloud->size(); ++i)
1762  {
1763  if(pcl::isFinite(cloud->at(i)))
1764  {
1765  std::vector<int> kIndices;
1766  std::vector<float> kSqrdDistances;
1767  float radius = radiusSearchRatio*uNorm(
1768  cloud->at(i).x-viewpoint[0],
1769  cloud->at(i).y-viewpoint[1],
1770  cloud->at(i).z-viewpoint[2]);
1771  int k = tree->radiusSearch(cloud->at(i), radius, kIndices, kSqrdDistances);
1772  if(k < minNeighborsInRadius)
1773  {
1774  output->at(oi++) = i;
1775  }
1776  }
1777  }
1778  output->resize(oi);
1779  return output;
1780  }
1781 }
1782 pcl::IndicesPtr subtractAdaptiveFiltering(
1783  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
1784  const pcl::IndicesPtr & indices,
1785  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
1786  const pcl::IndicesPtr & substractIndices,
1787  float radiusSearchRatio,
1788  float maxAngle,
1789  int minNeighborsInRadius,
1790  const Eigen::Vector3f & viewpoint)
1791 {
1792  UASSERT(minNeighborsInRadius > 0);
1793  UASSERT(radiusSearchRatio > 0.0f);
1794  pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBNormal>(false));
1795 
1796  if(indices->size())
1797  {
1798  pcl::IndicesPtr output(new std::vector<int>(indices->size()));
1799  int oi = 0; // output iterator
1800  if(substractIndices->size())
1801  {
1802  tree->setInputCloud(substractCloud, substractIndices);
1803  }
1804  else
1805  {
1806  tree->setInputCloud(substractCloud);
1807  }
1808  for(unsigned int i=0; i<indices->size(); ++i)
1809  {
1810  if(pcl::isFinite(cloud->at(indices->at(i))))
1811  {
1812  std::vector<int> kIndices;
1813  std::vector<float> kSqrdDistances;
1814  float radius = radiusSearchRatio*uNorm(
1815  cloud->at(indices->at(i)).x-viewpoint[0],
1816  cloud->at(indices->at(i)).y-viewpoint[1],
1817  cloud->at(indices->at(i)).z-viewpoint[2]);
1818  int k = tree->radiusSearch(cloud->at(indices->at(i)), radius, kIndices, kSqrdDistances);
1819  if(k>=minNeighborsInRadius && maxAngle > 0.0f)
1820  {
1821  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);
1822  if (uIsFinite(normal[0]) &&
1823  uIsFinite(normal[1]) &&
1824  uIsFinite(normal[2]))
1825  {
1826  int count = k;
1827  for(int j=0; j<count && k >= minNeighborsInRadius; ++j)
1828  {
1829  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);
1830  if(uIsFinite(v[0]) &&
1831  uIsFinite(v[1]) &&
1832  uIsFinite(v[2]))
1833  {
1834  float angle = pcl::getAngle3D(normal, v);
1835  if(angle > maxAngle)
1836  {
1837  k-=1;
1838  }
1839  }
1840  else
1841  {
1842  k-=1;
1843  }
1844  }
1845  }
1846  else
1847  {
1848  k=0;
1849  }
1850  }
1851 
1852  if(k < minNeighborsInRadius)
1853  {
1854  output->at(oi++) = indices->at(i);
1855  }
1856  }
1857  }
1858  output->resize(oi);
1859  return output;
1860  }
1861  else
1862  {
1863  pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
1864  int oi = 0; // output iterator
1865  if(substractIndices->size())
1866  {
1867  tree->setInputCloud(substractCloud, substractIndices);
1868  }
1869  else
1870  {
1871  tree->setInputCloud(substractCloud);
1872  }
1873  for(unsigned int i=0; i<cloud->size(); ++i)
1874  {
1875  if(pcl::isFinite(cloud->at(i)))
1876  {
1877  std::vector<int> kIndices;
1878  std::vector<float> kSqrdDistances;
1879  float radius = radiusSearchRatio*uNorm(
1880  cloud->at(i).x-viewpoint[0],
1881  cloud->at(i).y-viewpoint[1],
1882  cloud->at(i).z-viewpoint[2]);
1883  int k = tree->radiusSearch(cloud->at(i), radius, kIndices, kSqrdDistances);
1884  if(k>=minNeighborsInRadius && maxAngle > 0.0f)
1885  {
1886  Eigen::Vector4f normal(cloud->at(i).normal_x, cloud->at(i).normal_y, cloud->at(i).normal_z, 0.0f);
1887  if (uIsFinite(normal[0]) &&
1888  uIsFinite(normal[1]) &&
1889  uIsFinite(normal[2]))
1890  {
1891  int count = k;
1892  for(int j=0; j<count && k >= minNeighborsInRadius; ++j)
1893  {
1894  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);
1895  if(uIsFinite(v[0]) &&
1896  uIsFinite(v[1]) &&
1897  uIsFinite(v[2]))
1898  {
1899  float angle = pcl::getAngle3D(normal, v);
1900  if(angle > maxAngle)
1901  {
1902  k-=1;
1903  }
1904  }
1905  else
1906  {
1907  k-=1;
1908  }
1909  }
1910  }
1911  else
1912  {
1913  k=0;
1914  }
1915  }
1916  if(k < minNeighborsInRadius)
1917  {
1918  output->at(oi++) = i;
1919  }
1920  }
1921  }
1922  output->resize(oi);
1923  return output;
1924  }
1925 }
1926 
1927 
1928 pcl::IndicesPtr normalFiltering(
1929  const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
1930  float angleMax,
1931  const Eigen::Vector4f & normal,
1932  int normalKSearch,
1933  const Eigen::Vector4f & viewpoint,
1934  float groundNormalsUp)
1935 {
1936  pcl::IndicesPtr indices(new std::vector<int>);
1937  return normalFiltering(cloud, indices, angleMax, normal, normalKSearch, viewpoint, groundNormalsUp);
1938 }
1939 pcl::IndicesPtr normalFiltering(
1940  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
1941  float angleMax,
1942  const Eigen::Vector4f & normal,
1943  int normalKSearch,
1944  const Eigen::Vector4f & viewpoint,
1945  float groundNormalsUp)
1946 {
1947  pcl::IndicesPtr indices(new std::vector<int>);
1948  return normalFiltering(cloud, indices, angleMax, normal, normalKSearch, viewpoint, groundNormalsUp);
1949 }
1950 
1951 
1952 template<typename PointT>
1953 pcl::IndicesPtr normalFilteringImpl(
1954  const typename pcl::PointCloud<PointT>::Ptr & cloud,
1955  const pcl::IndicesPtr & indices,
1956  float angleMax,
1957  const Eigen::Vector4f & normal,
1958  int normalKSearch,
1959  const Eigen::Vector4f & viewpoint,
1960  float groundNormalsUp)
1961 {
1962  pcl::IndicesPtr output(new std::vector<int>());
1963 
1964  if(cloud->size())
1965  {
1966  typename pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>(false));
1967 
1968  pcl::NormalEstimationOMP<PointT, pcl::Normal> ne;
1969  ne.setInputCloud (cloud);
1970  if(indices->size())
1971  {
1972  ne.setIndices(indices);
1973  }
1974 
1975  if(indices->size())
1976  {
1977  tree->setInputCloud(cloud, indices);
1978  }
1979  else
1980  {
1981  tree->setInputCloud(cloud);
1982  }
1983  ne.setSearchMethod (tree);
1984 
1985  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
1986 
1987  ne.setKSearch(normalKSearch);
1988  if(viewpoint[0] != 0 || viewpoint[1] != 0 || viewpoint[2] != 0)
1989  {
1990  ne.setViewPoint(viewpoint[0], viewpoint[1], viewpoint[2]);
1991  }
1992 
1993  ne.compute (*cloud_normals);
1994 
1995  output->resize(cloud_normals->size());
1996  int oi = 0; // output iterator
1997  for(unsigned int i=0; i<cloud_normals->size(); ++i)
1998  {
1999  Eigen::Vector4f v(cloud_normals->at(i).normal_x, cloud_normals->at(i).normal_y, cloud_normals->at(i).normal_z, 0.0f);
2000  if(groundNormalsUp>0.0f && v[2] < -groundNormalsUp && cloud->at(indices->size()!=0?indices->at(i):i).z < viewpoint[3]) // some far velodyne rays on road can have normals toward ground
2001  {
2002  //reverse normal
2003  v *= -1.0f;
2004  }
2005 
2006  float angle = pcl::getAngle3D(normal, v);
2007  if(angle < angleMax)
2008  {
2009  output->at(oi++) = indices->size()!=0?indices->at(i):i;
2010  }
2011  }
2012  output->resize(oi);
2013  }
2014 
2015  return output;
2016 }
2017 pcl::IndicesPtr normalFiltering(
2018  const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
2019  const pcl::IndicesPtr & indices,
2020  float angleMax,
2021  const Eigen::Vector4f & normal,
2022  int normalKSearch,
2023  const Eigen::Vector4f & viewpoint,
2024  float groundNormalsUp)
2025 {
2026  return normalFilteringImpl<pcl::PointXYZ>(cloud, indices, angleMax, normal, normalKSearch, viewpoint, groundNormalsUp);
2027 }
2028 pcl::IndicesPtr normalFiltering(
2029  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
2030  const pcl::IndicesPtr & indices,
2031  float angleMax,
2032  const Eigen::Vector4f & normal,
2033  int normalKSearch,
2034  const Eigen::Vector4f & viewpoint,
2035  float groundNormalsUp)
2036 {
2037  return normalFilteringImpl<pcl::PointXYZRGB>(cloud, indices, angleMax, normal, normalKSearch, viewpoint, groundNormalsUp);
2038 }
2039 pcl::IndicesPtr normalFiltering(
2040  const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
2041  const pcl::IndicesPtr & indices,
2042  float angleMax,
2043  const Eigen::Vector4f & normal,
2044  int normalKSearch,
2045  const Eigen::Vector4f & viewpoint,
2046  float groundNormalsUp)
2047 {
2048  return normalFilteringImpl<pcl::PointXYZI>(cloud, indices, angleMax, normal, normalKSearch, viewpoint, groundNormalsUp);
2049 }
2050 
2051 template<typename PointNormalT>
2052 pcl::IndicesPtr normalFilteringImpl(
2053  const typename pcl::PointCloud<PointNormalT>::Ptr & cloud,
2054  const pcl::IndicesPtr & indices,
2055  float angleMax,
2056  const Eigen::Vector4f & normal,
2057  const Eigen::Vector4f & viewpoint,
2058  float groundNormalsUp)
2059 {
2060  pcl::IndicesPtr output(new std::vector<int>());
2061 
2062  if(cloud->size())
2063  {
2064  int oi = 0; // output iterator
2065  if(indices->size())
2066  {
2067  output->resize(indices->size());
2068  for(unsigned int i=0; i<indices->size(); ++i)
2069  {
2070  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);
2071  if(groundNormalsUp>0.0f && v[2] < -groundNormalsUp && cloud->at(indices->at(i)).z < viewpoint[3]) // some far velodyne rays on road can have normals toward ground
2072  {
2073  //reverse normal
2074  v *= -1.0f;
2075  }
2076  float angle = pcl::getAngle3D(normal, v);
2077  if(angle < angleMax)
2078  {
2079  output->at(oi++) = indices->size()!=0?indices->at(i):i;
2080  }
2081  }
2082  }
2083  else
2084  {
2085  output->resize(cloud->size());
2086  for(unsigned int i=0; i<cloud->size(); ++i)
2087  {
2088  Eigen::Vector4f v(cloud->at(i).normal_x, cloud->at(i).normal_y, cloud->at(i).normal_z, 0.0f);
2089  if(groundNormalsUp>0.0f && v[2] < -groundNormalsUp && cloud->at(i).z < viewpoint[3]) // some far velodyne rays on road can have normals toward ground
2090  {
2091  //reverse normal
2092  v *= -1.0f;
2093  }
2094  float angle = pcl::getAngle3D(normal, v);
2095  if(angle < angleMax)
2096  {
2097  output->at(oi++) = indices->size()!=0?indices->at(i):i;
2098  }
2099  }
2100  }
2101 
2102  output->resize(oi);
2103  }
2104 
2105  return output;
2106 }
2107 pcl::IndicesPtr normalFiltering(
2108  const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
2109  const pcl::IndicesPtr & indices,
2110  float angleMax,
2111  const Eigen::Vector4f & normal,
2112  int,
2113  const Eigen::Vector4f & viewpoint,
2114  float groundNormalsUp)
2115 {
2116  return normalFilteringImpl<pcl::PointNormal>(cloud, indices, angleMax, normal, viewpoint, groundNormalsUp);
2117 }
2118 pcl::IndicesPtr normalFiltering(
2119  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
2120  const pcl::IndicesPtr & indices,
2121  float angleMax,
2122  const Eigen::Vector4f & normal,
2123  int,
2124  const Eigen::Vector4f & viewpoint,
2125  float groundNormalsUp)
2126 {
2127  return normalFilteringImpl<pcl::PointXYZRGBNormal>(cloud, indices, angleMax, normal, viewpoint, groundNormalsUp);
2128 }
2129 pcl::IndicesPtr normalFiltering(
2130  const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
2131  const pcl::IndicesPtr & indices,
2132  float angleMax,
2133  const Eigen::Vector4f & normal,
2134  int,
2135  const Eigen::Vector4f & viewpoint,
2136  float groundNormalsUp)
2137 {
2138  return normalFilteringImpl<pcl::PointXYZINormal>(cloud, indices, angleMax, normal, viewpoint, groundNormalsUp);
2139 }
2140 
2141 std::vector<pcl::IndicesPtr> extractClusters(
2142  const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
2143  float clusterTolerance,
2144  int minClusterSize,
2145  int maxClusterSize,
2146  int * biggestClusterIndex)
2147 {
2148  pcl::IndicesPtr indices(new std::vector<int>);
2149  return extractClusters(cloud, indices, clusterTolerance, minClusterSize, maxClusterSize, biggestClusterIndex);
2150 }
2151 std::vector<pcl::IndicesPtr> extractClusters(
2152  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
2153  float clusterTolerance,
2154  int minClusterSize,
2155  int maxClusterSize,
2156  int * biggestClusterIndex)
2157 {
2158  pcl::IndicesPtr indices(new std::vector<int>);
2159  return extractClusters(cloud, indices, clusterTolerance, minClusterSize, maxClusterSize, biggestClusterIndex);
2160 }
2161 
2162 template<typename PointT>
2163 std::vector<pcl::IndicesPtr> extractClustersImpl(
2164  const typename pcl::PointCloud<PointT>::Ptr & cloud,
2165  const pcl::IndicesPtr & indices,
2166  float clusterTolerance,
2167  int minClusterSize,
2168  int maxClusterSize,
2169  int * biggestClusterIndex)
2170 {
2171  typename pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>(true));
2172  pcl::EuclideanClusterExtraction<PointT> ec;
2173  ec.setClusterTolerance (clusterTolerance);
2174  ec.setMinClusterSize (minClusterSize);
2175  ec.setMaxClusterSize (maxClusterSize);
2176  ec.setInputCloud (cloud);
2177 
2178  if(indices->size())
2179  {
2180  ec.setIndices(indices);
2181  tree->setInputCloud(cloud, indices);
2182  }
2183  else
2184  {
2185  tree->setInputCloud(cloud);
2186  }
2187  ec.setSearchMethod (tree);
2188 
2189  std::vector<pcl::PointIndices> cluster_indices;
2190  ec.extract (cluster_indices);
2191 
2192  int maxIndex=-1;
2193  unsigned int maxSize = 0;
2194  std::vector<pcl::IndicesPtr> output(cluster_indices.size());
2195  for(unsigned int i=0; i<cluster_indices.size(); ++i)
2196  {
2197  output[i] = pcl::IndicesPtr(new std::vector<int>(cluster_indices[i].indices));
2198 
2199  if(maxSize < cluster_indices[i].indices.size())
2200  {
2201  maxSize = (unsigned int)cluster_indices[i].indices.size();
2202  maxIndex = i;
2203  }
2204  }
2205  if(biggestClusterIndex)
2206  {
2207  *biggestClusterIndex = maxIndex;
2208  }
2209 
2210  return output;
2211 }
2212 
2213 std::vector<pcl::IndicesPtr> extractClusters(
2214  const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
2215  const pcl::IndicesPtr & indices,
2216  float clusterTolerance,
2217  int minClusterSize,
2218  int maxClusterSize,
2219  int * biggestClusterIndex)
2220 {
2221  return extractClustersImpl<pcl::PointXYZ>(cloud, indices, clusterTolerance, minClusterSize, maxClusterSize, biggestClusterIndex);
2222 }
2223 std::vector<pcl::IndicesPtr> extractClusters(
2224  const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
2225  const pcl::IndicesPtr & indices,
2226  float clusterTolerance,
2227  int minClusterSize,
2228  int maxClusterSize,
2229  int * biggestClusterIndex)
2230 {
2231  return extractClustersImpl<pcl::PointNormal>(cloud, indices, clusterTolerance, minClusterSize, maxClusterSize, biggestClusterIndex);
2232 }
2233 std::vector<pcl::IndicesPtr> extractClusters(
2234  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
2235  const pcl::IndicesPtr & indices,
2236  float clusterTolerance,
2237  int minClusterSize,
2238  int maxClusterSize,
2239  int * biggestClusterIndex)
2240 {
2241  return extractClustersImpl<pcl::PointXYZRGB>(cloud, indices, clusterTolerance, minClusterSize, maxClusterSize, biggestClusterIndex);
2242 }
2243 std::vector<pcl::IndicesPtr> extractClusters(
2244  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
2245  const pcl::IndicesPtr & indices,
2246  float clusterTolerance,
2247  int minClusterSize,
2248  int maxClusterSize,
2249  int * biggestClusterIndex)
2250 {
2251  return extractClustersImpl<pcl::PointXYZRGBNormal>(cloud, indices, clusterTolerance, minClusterSize, maxClusterSize, biggestClusterIndex);
2252 }
2253 std::vector<pcl::IndicesPtr> extractClusters(
2254  const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
2255  const pcl::IndicesPtr & indices,
2256  float clusterTolerance,
2257  int minClusterSize,
2258  int maxClusterSize,
2259  int * biggestClusterIndex)
2260 {
2261  return extractClustersImpl<pcl::PointXYZI>(cloud, indices, clusterTolerance, minClusterSize, maxClusterSize, biggestClusterIndex);
2262 }
2263 std::vector<pcl::IndicesPtr> extractClusters(
2264  const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
2265  const pcl::IndicesPtr & indices,
2266  float clusterTolerance,
2267  int minClusterSize,
2268  int maxClusterSize,
2269  int * biggestClusterIndex)
2270 {
2271  return extractClustersImpl<pcl::PointXYZINormal>(cloud, indices, clusterTolerance, minClusterSize, maxClusterSize, biggestClusterIndex);
2272 }
2273 
2274 template<typename PointT>
2275 pcl::IndicesPtr extractIndicesImpl(
2276  const typename pcl::PointCloud<PointT>::Ptr & cloud,
2277  const pcl::IndicesPtr & indices,
2278  bool negative)
2279 {
2280  pcl::IndicesPtr output(new std::vector<int>);
2281  pcl::ExtractIndices<PointT> extract;
2282  extract.setInputCloud (cloud);
2283  extract.setIndices(indices);
2284  extract.setNegative(negative);
2285  extract.filter(*output);
2286  return output;
2287 }
2288 
2289 pcl::IndicesPtr extractIndices(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative)
2290 {
2291  return extractIndicesImpl<pcl::PointXYZ>(cloud, indices, negative);
2292 }
2293 pcl::IndicesPtr extractIndices(const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative)
2294 {
2295  return extractIndicesImpl<pcl::PointNormal>(cloud, indices, negative);
2296 }
2297 pcl::IndicesPtr extractIndices(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative)
2298 {
2299  return extractIndicesImpl<pcl::PointXYZRGB>(cloud, indices, negative);
2300 }
2301 pcl::IndicesPtr extractIndices(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative)
2302 {
2303  return extractIndicesImpl<pcl::PointXYZRGBNormal>(cloud, indices, negative);
2304 }
2305 pcl::IndicesPtr extractIndices(const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative)
2306 {
2307  return extractIndicesImpl<pcl::PointXYZI>(cloud, indices, negative);
2308 }
2309 pcl::IndicesPtr extractIndices(const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative)
2310 {
2311  return extractIndicesImpl<pcl::PointXYZINormal>(cloud, indices, negative);
2312 }
2313 
2314 template<typename PointT>
2315 typename pcl::PointCloud<PointT>::Ptr extractIndicesImpl(
2316  const typename pcl::PointCloud<PointT>::Ptr & cloud,
2317  const pcl::IndicesPtr & indices,
2318  bool negative,
2319  bool keepOrganized)
2320 {
2321  typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
2322  pcl::ExtractIndices<PointT> extract;
2323  extract.setInputCloud (cloud);
2324  extract.setIndices(indices);
2325  extract.setNegative(negative);
2326  extract.setKeepOrganized(keepOrganized);
2327  extract.filter(*output);
2328  return output;
2329 }
2330 pcl::PointCloud<pcl::PointXYZ>::Ptr extractIndices(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative, bool keepOrganized)
2331 {
2332  return extractIndicesImpl<pcl::PointXYZ>(cloud, indices, negative, keepOrganized);
2333 }
2334 pcl::PointCloud<pcl::PointXYZRGB>::Ptr extractIndices(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative, bool keepOrganized)
2335 {
2336  return extractIndicesImpl<pcl::PointXYZRGB>(cloud, indices, negative, keepOrganized);
2337 }
2338 // PCL default lacks of pcl::PointNormal type support
2339 //pcl::PointCloud<pcl::PointNormal>::Ptr extractIndices(const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative, bool keepOrganized)
2340 //{
2341 // return extractIndicesImpl<pcl::PointNormal>(cloud, indices, negative, keepOrganized);
2342 //}
2343 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr extractIndices(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative, bool keepOrganized)
2344 {
2345  return extractIndicesImpl<pcl::PointXYZRGBNormal>(cloud, indices, negative, keepOrganized);
2346 }
2347 
2348 pcl::IndicesPtr extractPlane(
2349  const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
2350  float distanceThreshold,
2351  int maxIterations,
2352  pcl::ModelCoefficients * coefficientsOut)
2353 {
2354  pcl::IndicesPtr indices(new std::vector<int>);
2355  return extractPlane(cloud, indices, distanceThreshold, maxIterations, coefficientsOut);
2356 }
2357 
2358 pcl::IndicesPtr extractPlane(
2359  const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
2360  const pcl::IndicesPtr & indices,
2361  float distanceThreshold,
2362  int maxIterations,
2363  pcl::ModelCoefficients * coefficientsOut)
2364 {
2365  // Extract plane
2366  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
2367  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
2368  // Create the segmentation object
2369  pcl::SACSegmentation<pcl::PointXYZ> seg;
2370  // Optional
2371  seg.setOptimizeCoefficients (true);
2372  seg.setMaxIterations (maxIterations);
2373  // Mandatory
2374  seg.setModelType (pcl::SACMODEL_PLANE);
2375  seg.setMethodType (pcl::SAC_RANSAC);
2376  seg.setDistanceThreshold (distanceThreshold);
2377 
2378  seg.setInputCloud (cloud);
2379  if(indices->size())
2380  {
2381  seg.setIndices(indices);
2382  }
2383  seg.segment (*inliers, *coefficients);
2384 
2385  if(coefficientsOut)
2386  {
2387  *coefficientsOut = *coefficients;
2388  }
2389 
2390  return pcl::IndicesPtr(new std::vector<int>(inliers->indices));
2391 }
2392 
2393 }
2394 
2395 }
rtabmap::util3d::isFinite
bool RTABMAP_EXP isFinite(const cv::Point3f &pt)
Definition: util3d.cpp:3194
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:1413
rtabmap::util3d::proportionalRadiusFilteringImpl
pcl::IndicesPtr proportionalRadiusFilteringImpl(const typename pcl::PointCloud< PointT >::Ptr &cloud, const pcl::IndicesPtr &indices, const std::vector< int > &viewpointIndices, const std::map< int, Transform > &viewpoints, float factor, float neighborScale)
Definition: util3d_filtering.cpp:1242
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:2141
glm::min
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
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:2907
rtabmap::LaserScan::maxPoints
int maxPoints() const
Definition: LaserScan.h:116
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, float groundNormalsUp)
Definition: util3d_filtering.cpp:1953
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:720
glm::int64_t
detail::int64 int64_t
Definition: fwd.hpp:311
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:698
f
f
x
x
rtabmap::LaserScan::angleMin
float angleMin() const
Definition: LaserScan.h:119
rtabmap::util3d::removeNaNNormalsFromPointCloudImpl
pcl::PointCloud< PointT >::Ptr removeNaNNormalsFromPointCloudImpl(const typename pcl::PointCloud< PointT >::Ptr &cloud)
Definition: util3d_filtering.cpp:1052
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:992
rtabmap::util3d::randomSamplingImpl
pcl::PointCloud< PointT >::Ptr randomSamplingImpl(const typename pcl::PointCloud< PointT >::Ptr &cloud, int samples)
Definition: util3d_filtering.cpp:660
glm::all
GLM_FUNC_DECL bool all(vecType< bool, P > const &v)
rtabmap::LaserScan::data
const cv::Mat & data() const
Definition: LaserScan.h:112
rtabmap::LaserScan::format
Format format() const
Definition: LaserScan.h:113
glm::row
GLM_FUNC_DECL genType::row_type row(genType const &m, length_t const &index)
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:1703
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:984
UConversion.h
Some conversion functions.
rtabmap::LaserScan::isEmpty
bool isEmpty() const
Definition: LaserScan.h:125
rtabmap::LaserScan::is2d
bool is2d() const
Definition: LaserScan.h:128
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:908
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:940
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:2289
rtabmap::util3d::laserScanToPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP laserScanToPointCloud(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2197
rtabmap::LaserScan::rangeMax
float rangeMax() const
Definition: LaserScan.h:118
rtabmap::LaserScan::hasRGB
bool hasRGB() const
Definition: LaserScan.h:130
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, float groundNormalsUp=0.0f)
Definition: util3d_filtering.cpp:1928
rtabmap::util3d::downsample
LaserScan RTABMAP_EXP downsample(const LaserScan &cloud, int step)
Definition: util3d_filtering.cpp:388
uIsFinite
bool uIsFinite(const T &value)
Definition: UMath.h:55
glm::uint8_t
detail::uint8 uint8_t
Definition: fwd.hpp:908
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:1427
UASSERT
#define UASSERT(condition)
RTABMAP_EXP
#define RTABMAP_EXP
Definition: RtabmapExp.h:38
rtabmap::util3d::downsampleImpl
pcl::PointCloud< PointT >::Ptr downsampleImpl(const typename pcl::PointCloud< PointT >::Ptr &cloud, int step)
Definition: util3d_filtering.cpp:416
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:2232
rtabmap::LaserScan::hasNormals
bool hasNormals() const
Definition: LaserScan.h:129
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
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:672
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:2163
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:791
rtabmap::LaserScan::empty
bool empty() const
Definition: LaserScan.h:124
rtabmap::util3d::computeNormals
cv::Mat RTABMAP_EXP computeNormals(const cv::Mat &laserScan, int searchK, float searchRadius)
tree
tree
rtabmap::util3d::laserScanFromPointCloud
LaserScan laserScanFromPointCloud(const PointCloud2T &cloud, bool filterNaNs, bool is2D, const Transform &transform)
Definition: util3d.hpp:37
rtabmap::LaserScan::rangeMin
float rangeMin() const
Definition: LaserScan.h:117
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:1109
rtabmap::Transform::toEigen3f
Eigen::Affine3f toEigen3f() const
Definition: Transform.cpp:381
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
rtabmap::LaserScan::angleMax
float angleMax() const
Definition: LaserScan.h:120
rtabmap::util3d::radiusFiltering
pcl::IndicesPtr RTABMAP_EXP radiusFiltering(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, float radiusSearch, int minNeighborsInRadius)
Definition: util3d_filtering.cpp:1077
UDEBUG
#define UDEBUG(...)
glm::max
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
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:2348
rtabmap::util3d::proportionalRadiusFiltering
pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const std::vector< int > &viewpointIndices, const std::map< int, Transform > &viewpoints, float factor=0.01f, float neighborScale=2.0f)
Definition: util3d_filtering.cpp:1180
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:816
ULogger.h
ULogger class and convenient macros.
rtabmap::Transform::toEigen4f
Eigen::Matrix4f toEigen4f() const
Definition: Transform.cpp:362
UWARN
#define UWARN(...)
rtabmap::util3d::rangeFiltering
LaserScan RTABMAP_EXP rangeFiltering(const LaserScan &scan, float rangeMin, float rangeMax)
Definition: util3d_filtering.cpp:337
rtabmap::LaserScan::hasIntensity
bool hasIntensity() const
Definition: LaserScan.h:131
rtabmap::util3d::removeNaNNormalsFromPointCloud
pcl::PointCloud< pcl::PointNormal >::Ptr RTABMAP_EXP removeNaNNormalsFromPointCloud(const pcl::PointCloud< pcl::PointNormal >::Ptr &cloud)
Definition: util3d_filtering.cpp:1060
rtabmap::util3d::voxelizeImpl
pcl::PointCloud< PointT >::Ptr voxelizeImpl(const typename pcl::PointCloud< PointT >::Ptr &cloud, const pcl::IndicesPtr &indices, float voxelSize, int level=0)
Definition: util3d_filtering.cpp:514
rtabmap::LaserScan::angleIncrement
float angleIncrement() const
Definition: LaserScan.h:121
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:2250
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:603
rtabmap::LaserScan::dataType
int dataType() const
Definition: LaserScan.h:127
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
rtabmap::util3d::extractIndicesImpl
pcl::IndicesPtr extractIndicesImpl(const typename pcl::PointCloud< PointT >::Ptr &cloud, const pcl::IndicesPtr &indices, bool negative)
Definition: util3d_filtering.cpp:2275
rtabmap::LaserScan::localTransform
Transform localTransform() const
Definition: LaserScan.h:122
rtabmap::util3d::laserScan2dFromPointCloud
LaserScan RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud< pcl::PointXYZ > &cloud, const Transform &transform=Transform(), bool filterNaNs=true)
Definition: util3d.cpp:1920
rtabmap::LaserScan::size
int size() const
Definition: LaserScan.h:126
rtabmap::Transform::isIdentity
bool isIdentity() const
Definition: Transform.cpp:136
util3d_surface.h
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:3493

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:38:58