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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jul 1 2024 02:42:41