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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Sun Dec 1 2024 03:42:59