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> 37 #include <pcl/features/normal_3d_omp.h> 39 #include <pcl/search/kdtree.h> 41 #include <pcl/common/common.h> 43 #include <pcl/segmentation/extract_clusters.h> 44 #include <pcl/segmentation/sac_segmentation.h> 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> 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))
82 bool forceGroundNormalsUp)
85 UDEBUG(
"scan size=%d format=%d, step=%d, rangeMin=%f, rangeMax=%f, voxel=%f, normalK=%d, normalRadius=%f",
86 scan.
size(), (int)scan.
format(), downsamplingStep, rangeMin, rangeMax, voxelSize, normalK, normalRadius);
90 if(downsamplingStep<=1 || scan.
size() <= downsamplingStep)
95 if(downsamplingStep > 1 || rangeMin > 0.0
f || rangeMax > 0.0
f)
97 cv::Mat tmp = cv::Mat(1, scan.
size()/downsamplingStep, scan.
dataType());
98 bool is2d = scan.
is2d();
100 float rangeMinSqrd = rangeMin * rangeMin;
101 float rangeMaxSqrd = rangeMax * rangeMax;
102 for(
int i=0; i<scan.
size()-downsamplingStep+1; i+=downsamplingStep)
104 const float * ptr = scan.
data().ptr<
float>(0, i);
106 if(rangeMin>0.0
f || rangeMax>0.0
f)
111 r = ptr[0]*ptr[0] + ptr[1]*ptr[1];
115 r = ptr[0]*ptr[0] + ptr[1]*ptr[1] + ptr[2]*ptr[2];
118 if(rangeMin > 0.0
f && r < rangeMinSqrd)
122 if(rangeMax > 0.0
f && r > rangeMaxSqrd)
131 int previousSize = scan.
size();
134 UDEBUG(
"Downsampling scan (step=%d): %d -> %d (scanMaxPts=%d->%d)", downsamplingStep, previousSize, scan.
size(), scanMaxPtsTmp, scan.
maxPoints());
137 if(scan.
size() && (voxelSize > 0.0f || ((normalK > 0 || normalRadius>0.0f) && !scan.
hasNormals())))
150 float ratio = float(cloud->size()) / scan.
size();
151 scanMaxPts = int(
float(scanMaxPts) * ratio);
152 UDEBUG(
"Voxel filtering scan (voxel=%f m): %d -> %d (scanMaxPts=%d->%d)", voxelSize, scan.
size(), cloud->size(), scan.
maxPoints(), scanMaxPts);
154 if(cloud->size() && (normalK > 0 || normalRadius>0.0f))
158 UDEBUG(
"Normals computed (k=%d radius=%f)", normalK, normalRadius);
164 UWARN(
"Voxel filter i applied, but normal parameters are not set and input scan has normals. The returned scan has no normals.");
179 float ratio = float(cloud->size()) / scan.
size();
180 scanMaxPts = int(
float(scanMaxPts) * ratio);
181 UDEBUG(
"Voxel filtering scan (voxel=%f m): %d -> %d (scanMaxPts=%d->%d)", voxelSize, scan.
size(), cloud->size(), scan.
maxPoints(), scanMaxPts);
183 if(cloud->size() && (normalK > 0 || normalRadius>0.0f))
185 pcl::PointCloud<pcl::Normal>::Ptr normals;
196 UDEBUG(
"Normals computed (k=%d radius=%f)", normalK, normalRadius);
202 UWARN(
"Voxel filter i applied, but normal parameters are not set and input scan has normals. The returned scan has no normals.");
224 float ratio = float(cloud->size()) / scan.
size();
225 scanMaxPts = int(
float(scanMaxPts) * ratio);
226 UDEBUG(
"Voxel filtering scan (voxel=%f m): %d -> %d (scanMaxPts=%d->%d)", voxelSize, scan.
size(), cloud->size(), scan.
maxPoints(), scanMaxPts);
228 if(cloud->size() && (normalK > 0 || normalRadius>0.0f))
230 pcl::PointCloud<pcl::Normal>::Ptr normals;
241 UDEBUG(
"Normals computed (k=%d radius=%f)", normalK, normalRadius);
247 UWARN(
"Voxel filter i applied, but normal parameters are not set and input scan has normals. The returned scan has no normals.");
275 UASSERT(rangeMin >=0.0
f && rangeMax>=0.0
f);
278 if(rangeMin > 0.0
f || rangeMax > 0.0
f)
280 cv::Mat output = cv::Mat(1, scan.
size(), scan.
dataType());
281 bool is2d = scan.
is2d();
283 float rangeMinSqrd = rangeMin * rangeMin;
284 float rangeMaxSqrd = rangeMax * rangeMax;
285 for(
int i=0; i<scan.
size(); ++i)
287 const float * ptr = scan.
data().ptr<
float>(0, i);
291 r = ptr[0]*ptr[0] + ptr[1]*ptr[1];
295 r = ptr[0]*ptr[0] + ptr[1]*ptr[1] + ptr[2]*ptr[2];
298 if(rangeMin > 0.0
f && r < rangeMinSqrd)
302 if(rangeMax > 0.0
f && r > rangeMaxSqrd)
322 if(step <= 1 || scan.
size() <=
step)
330 cv::Mat output = cv::Mat(1, finalSize, scan.
dataType());
332 for(
int i=0; i<scan.
size()-step+1; i+=
step)
340 template<
typename Po
intT>
342 const typename pcl::PointCloud<PointT>::Ptr & cloud,
346 typename pcl::PointCloud<PointT>::Ptr output(
new pcl::PointCloud<PointT>);
347 if(step <= 1 || (
int)cloud->size() <=
step)
354 int finalSize = int(cloud->size())/step;
355 output->resize(finalSize);
357 for(
unsigned int i=0; i<cloud->size()-step+1; i+=
step)
359 (*output)[oi++] = cloud->at(i);
364 pcl::PointCloud<pcl::PointXYZ>::Ptr
downsample(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
int step)
366 return downsampleImpl<pcl::PointXYZ>(cloud,
step);
368 pcl::PointCloud<pcl::PointXYZRGB>::Ptr
downsample(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
int step)
370 return downsampleImpl<pcl::PointXYZRGB>(cloud,
step);
372 pcl::PointCloud<pcl::PointNormal>::Ptr
downsample(
const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
int step)
374 return downsampleImpl<pcl::PointNormal>(cloud,
step);
376 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr
downsample(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
int step)
378 return downsampleImpl<pcl::PointXYZRGBNormal>(cloud,
step);
381 template<
typename Po
intT>
383 const typename pcl::PointCloud<PointT>::Ptr & cloud,
384 const pcl::IndicesPtr & indices,
388 typename pcl::PointCloud<PointT>::Ptr output(
new pcl::PointCloud<PointT>);
389 if((cloud->is_dense && cloud->size()) || (!cloud->is_dense && indices->size()))
391 pcl::VoxelGrid<PointT> filter;
392 filter.setLeafSize(voxelSize, voxelSize, voxelSize);
393 filter.setInputCloud(cloud);
396 filter.setIndices(indices);
398 filter.filter(*output);
400 else if(cloud->size() && !cloud->is_dense && indices->size() == 0)
402 UWARN(
"Cannot voxelize a not dense (organized) cloud with empty indices! (input=%d pts). Returning empty cloud!", (
int)cloud->size());
407 pcl::PointCloud<pcl::PointXYZ>::Ptr
voxelize(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float voxelSize)
409 return voxelizeImpl<pcl::PointXYZ>(cloud, indices, voxelSize);
411 pcl::PointCloud<pcl::PointNormal>::Ptr
voxelize(
const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float voxelSize)
413 return voxelizeImpl<pcl::PointNormal>(cloud, indices, voxelSize);
415 pcl::PointCloud<pcl::PointXYZRGB>::Ptr
voxelize(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float voxelSize)
417 return voxelizeImpl<pcl::PointXYZRGB>(cloud, indices, voxelSize);
419 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr
voxelize(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float voxelSize)
421 return voxelizeImpl<pcl::PointXYZRGBNormal>(cloud, indices, voxelSize);
423 pcl::PointCloud<pcl::PointXYZI>::Ptr
voxelize(
const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float voxelSize)
425 return voxelizeImpl<pcl::PointXYZI>(cloud, indices, voxelSize);
427 pcl::PointCloud<pcl::PointXYZINormal>::Ptr
voxelize(
const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float voxelSize)
429 return voxelizeImpl<pcl::PointXYZINormal>(cloud, indices, voxelSize);
432 pcl::PointCloud<pcl::PointXYZ>::Ptr
voxelize(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
float voxelSize)
434 pcl::IndicesPtr indices(
new std::vector<int>);
435 return voxelize(cloud, indices, voxelSize);
437 pcl::PointCloud<pcl::PointNormal>::Ptr
voxelize(
const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
float voxelSize)
439 pcl::IndicesPtr indices(
new std::vector<int>);
440 return voxelize(cloud, indices, voxelSize);
442 pcl::PointCloud<pcl::PointXYZRGB>::Ptr
voxelize(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
float voxelSize)
444 pcl::IndicesPtr indices(
new std::vector<int>);
445 return voxelize(cloud, indices, voxelSize);
447 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr
voxelize(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
float voxelSize)
449 pcl::IndicesPtr indices(
new std::vector<int>);
450 return voxelize(cloud, indices, voxelSize);
452 pcl::PointCloud<pcl::PointXYZI>::Ptr
voxelize(
const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
float voxelSize)
454 pcl::IndicesPtr indices(
new std::vector<int>);
455 return voxelize(cloud, indices, voxelSize);
457 pcl::PointCloud<pcl::PointXYZINormal>::Ptr
voxelize(
const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
float voxelSize)
459 pcl::IndicesPtr indices(
new std::vector<int>);
460 return voxelize(cloud, indices, voxelSize);
463 template<
typename Po
intT>
465 const typename pcl::PointCloud<PointT>::Ptr & cloud,
int samples)
468 typename pcl::PointCloud<PointT>::Ptr output(
new pcl::PointCloud<PointT>);
469 pcl::RandomSample<PointT> filter;
470 filter.setSample(samples);
471 filter.setInputCloud(cloud);
472 filter.filter(*output);
475 pcl::PointCloud<pcl::PointXYZ>::Ptr
randomSampling(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
int samples)
477 return randomSamplingImpl<pcl::PointXYZ>(cloud, samples);
479 pcl::PointCloud<pcl::PointXYZRGB>::Ptr
randomSampling(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
int samples)
481 return randomSamplingImpl<pcl::PointXYZRGB>(cloud, samples);
484 template<
typename Po
intT>
486 const typename pcl::PointCloud<PointT>::Ptr & cloud,
487 const pcl::IndicesPtr & indices,
488 const std::string &
axis,
493 UASSERT_MSG(max > min,
uFormat(
"cloud=%d, max=%f min=%f axis=%s", (
int)cloud->size(),
max,
min, axis.c_str()).c_str());
494 UASSERT(axis.compare(
"x") == 0 || axis.compare(
"y") == 0 || axis.compare(
"z") == 0);
496 pcl::IndicesPtr output(
new std::vector<int>);
497 pcl::PassThrough<PointT> filter;
498 filter.setNegative(negative);
499 filter.setFilterFieldName(axis);
500 filter.setFilterLimits(min, max);
501 filter.setInputCloud(cloud);
502 filter.setIndices(indices);
503 filter.filter(*output);
507 pcl::IndicesPtr
passThrough(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const std::string &
axis,
float min,
float max,
bool negative)
509 return passThroughImpl<pcl::PointXYZ>(cloud, indices,
axis,
min,
max, negative);
511 pcl::IndicesPtr
passThrough(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const std::string &
axis,
float min,
float max,
bool negative)
513 return passThroughImpl<pcl::PointXYZRGB>(cloud, indices,
axis,
min,
max, negative);
515 pcl::IndicesPtr
passThrough(
const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const std::string &
axis,
float min,
float max,
bool negative)
517 return passThroughImpl<pcl::PointXYZI>(cloud, indices,
axis,
min,
max, negative);
519 pcl::IndicesPtr
passThrough(
const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const std::string &
axis,
float min,
float max,
bool negative)
521 return passThroughImpl<pcl::PointNormal>(cloud, indices,
axis,
min,
max, negative);
523 pcl::IndicesPtr
passThrough(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const std::string &
axis,
float min,
float max,
bool negative)
525 return passThroughImpl<pcl::PointXYZRGBNormal>(cloud, indices,
axis,
min,
max, negative);
527 pcl::IndicesPtr
passThrough(
const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const std::string &
axis,
float min,
float max,
bool negative)
529 return passThroughImpl<pcl::PointXYZINormal>(cloud, indices,
axis,
min,
max, negative);
532 template<
typename Po
intT>
534 const typename pcl::PointCloud<PointT>::Ptr & cloud,
535 const std::string &
axis,
540 UASSERT_MSG(max > min,
uFormat(
"cloud=%d, max=%f min=%f axis=%s", (
int)cloud->size(),
max,
min, axis.c_str()).c_str());
541 UASSERT(axis.compare(
"x") == 0 || axis.compare(
"y") == 0 || axis.compare(
"z") == 0);
543 typename pcl::PointCloud<PointT>::Ptr output(
new pcl::PointCloud<PointT>);
544 pcl::PassThrough<PointT> filter;
545 filter.setNegative(negative);
546 filter.setFilterFieldName(axis);
547 filter.setFilterLimits(min, max);
548 filter.setInputCloud(cloud);
549 filter.filter(*output);
552 pcl::PointCloud<pcl::PointXYZ>::Ptr
passThrough(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const std::string &
axis,
float min,
float max,
bool negative)
554 return passThroughImpl<pcl::PointXYZ>(cloud,
axis,
min ,
max, negative);
556 pcl::PointCloud<pcl::PointXYZRGB>::Ptr
passThrough(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const std::string &
axis,
float min,
float max,
bool negative)
558 return passThroughImpl<pcl::PointXYZRGB>(cloud,
axis,
min ,
max, negative);
560 pcl::PointCloud<pcl::PointXYZI>::Ptr
passThrough(
const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
const std::string &
axis,
float min,
float max,
bool negative)
562 return passThroughImpl<pcl::PointXYZI>(cloud,
axis,
min ,
max, negative);
564 pcl::PointCloud<pcl::PointNormal>::Ptr
passThrough(
const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
const std::string &
axis,
float min,
float max,
bool negative)
566 return passThroughImpl<pcl::PointNormal>(cloud,
axis,
min ,
max, negative);
568 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr
passThrough(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const std::string &
axis,
float min,
float max,
bool negative)
570 return passThroughImpl<pcl::PointXYZRGBNormal>(cloud,
axis,
min ,
max, negative);
572 pcl::PointCloud<pcl::PointXYZINormal>::Ptr
passThrough(
const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
const std::string &
axis,
float min,
float max,
bool negative)
574 return passThroughImpl<pcl::PointXYZINormal>(cloud,
axis,
min ,
max, negative);
577 template<
typename Po
intT>
579 const typename pcl::PointCloud<PointT>::Ptr & cloud,
580 const pcl::IndicesPtr & indices,
581 const Eigen::Vector4f &
min,
582 const Eigen::Vector4f &
max,
586 UASSERT(min[0] < max[0] && min[1] < max[1] && min[2] < max[2]);
588 pcl::IndicesPtr output(
new std::vector<int>);
589 pcl::CropBox<PointT> filter;
590 filter.setNegative(negative);
595 filter.setTransform(transform.
toEigen3f());
597 filter.setInputCloud(cloud);
598 filter.setIndices(indices);
599 filter.filter(*output);
603 pcl::IndicesPtr
cropBox(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const Eigen::Vector4f &
min,
const Eigen::Vector4f &
max,
const Transform & transform,
bool negative)
605 return cropBoxImpl<pcl::PointXYZ>(cloud, indices,
min,
max, transform, negative);
607 pcl::IndicesPtr
cropBox(
const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const Eigen::Vector4f &
min,
const Eigen::Vector4f &
max,
const Transform & transform,
bool negative)
609 return cropBoxImpl<pcl::PointNormal>(cloud, indices,
min,
max, transform, negative);
611 pcl::IndicesPtr
cropBox(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const Eigen::Vector4f &
min,
const Eigen::Vector4f &
max,
const Transform & transform,
bool negative)
613 return cropBoxImpl<pcl::PointXYZRGB>(cloud, indices,
min,
max, transform, negative);
615 pcl::IndicesPtr
cropBox(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const Eigen::Vector4f &
min,
const Eigen::Vector4f &
max,
const Transform & transform,
bool negative)
617 return cropBoxImpl<pcl::PointXYZRGBNormal>(cloud, indices,
min,
max, transform, negative);
620 template<
typename Po
intT>
622 const typename pcl::PointCloud<PointT>::Ptr & cloud,
623 const Eigen::Vector4f &
min,
624 const Eigen::Vector4f &
max,
628 UASSERT(min[0] < max[0] && min[1] < max[1] && min[2] < max[2]);
630 typename pcl::PointCloud<PointT>::Ptr output(
new pcl::PointCloud<PointT>);
631 pcl::CropBox<PointT> filter;
632 filter.setNegative(negative);
637 filter.setTransform(transform.
toEigen3f());
639 filter.setInputCloud(cloud);
640 filter.filter(*output);
643 pcl::PointCloud<pcl::PointXYZ>::Ptr
cropBox(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const Eigen::Vector4f &
min,
const Eigen::Vector4f &
max,
const Transform & transform,
bool negative)
645 return cropBoxImpl<pcl::PointXYZ>(cloud,
min,
max, transform, negative);
647 pcl::PointCloud<pcl::PointNormal>::Ptr
cropBox(
const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
const Eigen::Vector4f &
min,
const Eigen::Vector4f &
max,
const Transform & transform,
bool negative)
649 return cropBoxImpl<pcl::PointNormal>(cloud,
min,
max, transform, negative);
651 pcl::PointCloud<pcl::PointXYZRGB>::Ptr
cropBox(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const Eigen::Vector4f &
min,
const Eigen::Vector4f &
max,
const Transform & transform,
bool negative)
653 return cropBoxImpl<pcl::PointXYZRGB>(cloud,
min,
max, transform, negative);
655 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr
cropBox(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const Eigen::Vector4f &
min,
const Eigen::Vector4f &
max,
const Transform & transform,
bool negative)
657 return cropBoxImpl<pcl::PointXYZRGBNormal>(cloud,
min,
max, transform, negative);
660 template<
typename Po
intT>
662 const typename pcl::PointCloud<PointT>::Ptr & cloud,
663 const pcl::IndicesPtr & indices,
667 float nearClipPlaneDistance,
668 float farClipPlaneDistance,
671 UASSERT(horizontalFOV > 0.0
f && verticalFOV > 0.0
f);
672 UASSERT(farClipPlaneDistance > nearClipPlaneDistance);
675 pcl::IndicesPtr output(
new std::vector<int>);
676 pcl::FrustumCulling<PointT> fc;
677 fc.setNegative(negative);
678 fc.setInputCloud (cloud);
679 if(indices.get() && indices->size())
681 fc.setIndices(indices);
683 fc.setVerticalFOV (verticalFOV);
684 fc.setHorizontalFOV (horizontalFOV);
685 fc.setNearPlaneDistance (nearClipPlaneDistance);
686 fc.setFarPlaneDistance (farClipPlaneDistance);
688 fc.setCameraPose (cameraPose.
toEigen4f());
693 pcl::IndicesPtr
frustumFiltering(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const Transform & cameraPose,
float horizontalFOV,
float verticalFOV,
float nearClipPlaneDistance,
float farClipPlaneDistance,
bool negative)
695 return frustumFilteringImpl<pcl::PointXYZ>(cloud, indices, cameraPose, horizontalFOV, verticalFOV, nearClipPlaneDistance, farClipPlaneDistance, negative);
698 template<
typename Po
intT>
700 const typename pcl::PointCloud<PointT>::Ptr & cloud,
704 float nearClipPlaneDistance,
705 float farClipPlaneDistance,
708 UASSERT(horizontalFOV > 0.0
f && verticalFOV > 0.0
f);
709 UASSERT(farClipPlaneDistance > nearClipPlaneDistance);
712 typename pcl::PointCloud<PointT>::Ptr output(
new pcl::PointCloud<PointT>);
713 pcl::FrustumCulling<PointT> fc;
714 fc.setNegative(negative);
715 fc.setInputCloud (cloud);
716 fc.setVerticalFOV (verticalFOV);
717 fc.setHorizontalFOV (horizontalFOV);
718 fc.setNearPlaneDistance (nearClipPlaneDistance);
719 fc.setFarPlaneDistance (farClipPlaneDistance);
721 fc.setCameraPose (cameraPose.
toEigen4f());
726 pcl::PointCloud<pcl::PointXYZ>::Ptr
frustumFiltering(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const Transform & cameraPose,
float horizontalFOV,
float verticalFOV,
float nearClipPlaneDistance,
float farClipPlaneDistance,
bool negative)
728 return frustumFilteringImpl<pcl::PointXYZ>(cloud, cameraPose, horizontalFOV, verticalFOV, nearClipPlaneDistance, farClipPlaneDistance, negative);
730 pcl::PointCloud<pcl::PointXYZRGB>::Ptr
frustumFiltering(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const Transform & cameraPose,
float horizontalFOV,
float verticalFOV,
float nearClipPlaneDistance,
float farClipPlaneDistance,
bool negative)
732 return frustumFilteringImpl<pcl::PointXYZRGB>(cloud, cameraPose, horizontalFOV, verticalFOV, nearClipPlaneDistance, farClipPlaneDistance, negative);
736 template<
typename Po
intT>
738 const typename pcl::PointCloud<PointT>::Ptr & cloud)
740 typename pcl::PointCloud<PointT>::Ptr output(
new pcl::PointCloud<PointT>);
741 std::vector<int> indices;
747 return removeNaNFromPointCloudImpl<pcl::PointXYZ>(cloud);
751 return removeNaNFromPointCloudImpl<pcl::PointXYZRGB>(cloud);
754 template<
typename Po
intT>
756 const typename pcl::PointCloud<PointT>::Ptr & cloud)
758 typename pcl::PointCloud<PointT>::Ptr output(
new pcl::PointCloud<PointT>);
759 std::vector<int> indices;
764 const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud)
766 return removeNaNNormalsFromPointCloudImpl<pcl::PointNormal>(cloud);
769 const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud)
771 return removeNaNNormalsFromPointCloudImpl<pcl::PointXYZRGBNormal>(cloud);
775 pcl::IndicesPtr
radiusFiltering(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
float radiusSearch,
int minNeighborsInRadius)
777 pcl::IndicesPtr indices(
new std::vector<int>);
778 return radiusFiltering(cloud, indices, radiusSearch, minNeighborsInRadius);
780 pcl::IndicesPtr
radiusFiltering(
const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
float radiusSearch,
int minNeighborsInRadius)
782 pcl::IndicesPtr indices(
new std::vector<int>);
783 return radiusFiltering(cloud, indices, radiusSearch, minNeighborsInRadius);
785 pcl::IndicesPtr
radiusFiltering(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
float radiusSearch,
int minNeighborsInRadius)
787 pcl::IndicesPtr indices(
new std::vector<int>);
788 return radiusFiltering(cloud, indices, radiusSearch, minNeighborsInRadius);
790 pcl::IndicesPtr
radiusFiltering(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
float radiusSearch,
int minNeighborsInRadius)
792 pcl::IndicesPtr indices(
new std::vector<int>);
793 return radiusFiltering(cloud, indices, radiusSearch, minNeighborsInRadius);
796 template<
typename Po
intT>
798 const typename pcl::PointCloud<PointT>::Ptr & cloud,
799 const pcl::IndicesPtr & indices,
801 int minNeighborsInRadius)
803 typename pcl::search::KdTree<PointT>::Ptr
tree (
new pcl::search::KdTree<PointT>(
false));
807 pcl::IndicesPtr output(
new std::vector<int>(indices->size()));
809 tree->setInputCloud(cloud, indices);
810 for(
unsigned int i=0; i<indices->size(); ++i)
813 std::vector<float> kDistances;
814 int k = tree->radiusSearch(cloud->at(indices->at(i)), radiusSearch, kIndices, kDistances);
815 if(k > minNeighborsInRadius)
817 output->at(oi++) = indices->at(i);
825 pcl::IndicesPtr output(
new std::vector<int>(cloud->size()));
827 tree->setInputCloud(cloud);
828 for(
unsigned int i=0; i<cloud->size(); ++i)
831 std::vector<float> kDistances;
832 int k = tree->radiusSearch(cloud->at(i), radiusSearch,
kIndices, kDistances);
833 if(k > minNeighborsInRadius)
835 output->at(oi++) = i;
843 pcl::IndicesPtr
radiusFiltering(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float radiusSearch,
int minNeighborsInRadius)
845 return radiusFilteringImpl<pcl::PointXYZ>(cloud, indices, radiusSearch, minNeighborsInRadius);
847 pcl::IndicesPtr
radiusFiltering(
const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float radiusSearch,
int minNeighborsInRadius)
849 return radiusFilteringImpl<pcl::PointNormal>(cloud, indices, radiusSearch, minNeighborsInRadius);
851 pcl::IndicesPtr
radiusFiltering(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float radiusSearch,
int minNeighborsInRadius)
853 return radiusFilteringImpl<pcl::PointXYZRGB>(cloud, indices, radiusSearch, minNeighborsInRadius);
855 pcl::IndicesPtr
radiusFiltering(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float radiusSearch,
int minNeighborsInRadius)
857 return radiusFilteringImpl<pcl::PointXYZRGBNormal>(cloud, indices, radiusSearch, minNeighborsInRadius);
861 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
862 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & substractCloud,
864 int minNeighborsInRadius)
866 pcl::IndicesPtr indices(
new std::vector<int>);
867 pcl::IndicesPtr indicesOut =
subtractFiltering(cloud, indices, substractCloud, indices, radiusSearch, minNeighborsInRadius);
868 pcl::PointCloud<pcl::PointXYZRGB>::Ptr out(
new pcl::PointCloud<pcl::PointXYZRGB>);
869 pcl::copyPointCloud(*cloud, *indicesOut, *out);
873 template<
typename Po
intT>
875 const typename pcl::PointCloud<PointT>::Ptr & cloud,
876 const pcl::IndicesPtr & indices,
877 const typename pcl::PointCloud<PointT>::Ptr & substractCloud,
878 const pcl::IndicesPtr & substractIndices,
880 int minNeighborsInRadius)
882 UASSERT(minNeighborsInRadius > 0);
883 typename pcl::search::KdTree<PointT>::Ptr
tree (
new pcl::search::KdTree<PointT>(
false));
887 pcl::IndicesPtr output(
new std::vector<int>(indices->size()));
889 if(substractIndices->size())
891 tree->setInputCloud(substractCloud, substractIndices);
895 tree->setInputCloud(substractCloud);
897 for(
unsigned int i=0; i<indices->size(); ++i)
900 std::vector<float> kDistances;
901 int k = tree->radiusSearch(cloud->at(indices->at(i)), radiusSearch, kIndices, kDistances);
902 if(k < minNeighborsInRadius)
904 output->at(oi++) = indices->at(i);
912 pcl::IndicesPtr output(
new std::vector<int>(cloud->size()));
914 if(substractIndices->size())
916 tree->setInputCloud(substractCloud, substractIndices);
920 tree->setInputCloud(substractCloud);
922 for(
unsigned int i=0; i<cloud->size(); ++i)
925 std::vector<float> kDistances;
926 int k = tree->radiusSearch(cloud->at(i), radiusSearch,
kIndices, kDistances);
927 if(k < minNeighborsInRadius)
929 output->at(oi++) = i;
937 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
938 const pcl::IndicesPtr & indices,
939 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & substractCloud,
940 const pcl::IndicesPtr & substractIndices,
942 int minNeighborsInRadius)
944 return subtractFilteringImpl<pcl::PointXYZRGB>(cloud, indices, substractCloud, substractIndices, radiusSearch, minNeighborsInRadius);
948 const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
949 const pcl::PointCloud<pcl::PointNormal>::Ptr & substractCloud,
952 int minNeighborsInRadius)
954 pcl::IndicesPtr indices(
new std::vector<int>);
955 pcl::IndicesPtr indicesOut =
subtractFiltering(cloud, indices, substractCloud, indices, radiusSearch, maxAngle, minNeighborsInRadius);
956 pcl::PointCloud<pcl::PointNormal>::Ptr out(
new pcl::PointCloud<pcl::PointNormal>);
957 pcl::copyPointCloud(*cloud, *indicesOut, *out);
961 const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
962 const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
965 int minNeighborsInRadius)
967 pcl::IndicesPtr indices(
new std::vector<int>);
968 pcl::IndicesPtr indicesOut =
subtractFiltering(cloud, indices, substractCloud, indices, radiusSearch, maxAngle, minNeighborsInRadius);
969 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr out(
new pcl::PointCloud<pcl::PointXYZRGBNormal>);
970 pcl::copyPointCloud(*cloud, *indicesOut, *out);
974 template<
typename Po
intT>
976 const typename pcl::PointCloud<PointT>::Ptr & cloud,
977 const pcl::IndicesPtr & indices,
978 const typename pcl::PointCloud<PointT>::Ptr & substractCloud,
979 const pcl::IndicesPtr & substractIndices,
982 int minNeighborsInRadius)
984 UASSERT(minNeighborsInRadius > 0);
985 typename pcl::search::KdTree<PointT>::Ptr
tree (
new pcl::search::KdTree<PointT>(
false));
989 pcl::IndicesPtr output(
new std::vector<int>(indices->size()));
991 if(substractIndices->size())
993 tree->setInputCloud(substractCloud, substractIndices);
997 tree->setInputCloud(substractCloud);
999 for(
unsigned int i=0; i<indices->size(); ++i)
1002 std::vector<float> kDistances;
1003 int k = tree->radiusSearch(cloud->at(indices->at(i)), radiusSearch, kIndices, kDistances);
1004 if(k>=minNeighborsInRadius && maxAngle > 0.0
f)
1006 Eigen::Vector4f normal(cloud->at(indices->at(i)).normal_x, cloud->at(indices->at(i)).normal_y, cloud->at(indices->at(i)).normal_z, 0.0
f);
1012 for(
int j=0; j<count && k >= minNeighborsInRadius; ++j)
1014 Eigen::Vector4f v(substractCloud->at(kIndices.at(j)).normal_x, substractCloud->at(kIndices.at(j)).normal_y, substractCloud->at(kIndices.at(j)).normal_z, 0.0
f);
1019 float angle = pcl::getAngle3D(normal, v);
1020 if(angle > maxAngle)
1036 if(k < minNeighborsInRadius)
1038 output->at(oi++) = indices->at(i);
1046 pcl::IndicesPtr output(
new std::vector<int>(cloud->size()));
1048 if(substractIndices->size())
1050 tree->setInputCloud(substractCloud, substractIndices);
1054 tree->setInputCloud(substractCloud);
1056 for(
unsigned int i=0; i<cloud->size(); ++i)
1059 std::vector<float> kDistances;
1060 int k = tree->radiusSearch(cloud->at(i), radiusSearch,
kIndices, kDistances);
1061 if(k>=minNeighborsInRadius && maxAngle > 0.0
f)
1063 Eigen::Vector4f normal(cloud->at(i).normal_x, cloud->at(i).normal_y, cloud->at(i).normal_z, 0.0f);
1069 for(
int j=0; j<count && k >= minNeighborsInRadius; ++j)
1071 Eigen::Vector4f v(substractCloud->at(kIndices.at(j)).normal_x, substractCloud->at(kIndices.at(j)).normal_y, substractCloud->at(kIndices.at(j)).normal_z, 0.0
f);
1076 float angle = pcl::getAngle3D(normal, v);
1077 if(angle > maxAngle)
1093 if(k < minNeighborsInRadius)
1095 output->at(oi++) = i;
1104 const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
1105 const pcl::IndicesPtr & indices,
1106 const pcl::PointCloud<pcl::PointNormal>::Ptr & substractCloud,
1107 const pcl::IndicesPtr & substractIndices,
1110 int minNeighborsInRadius)
1112 return subtractFilteringImpl<pcl::PointNormal>(cloud, indices, substractCloud, substractIndices, radiusSearch, maxAngle, minNeighborsInRadius);
1115 const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
1116 const pcl::IndicesPtr & indices,
1117 const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
1118 const pcl::IndicesPtr & substractIndices,
1121 int minNeighborsInRadius)
1123 return subtractFilteringImpl<pcl::PointXYZRGBNormal>(cloud, indices, substractCloud, substractIndices, radiusSearch, maxAngle, minNeighborsInRadius);
1127 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
1128 const pcl::IndicesPtr & indices,
1129 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & substractCloud,
1130 const pcl::IndicesPtr & substractIndices,
1131 float radiusSearchRatio,
1132 int minNeighborsInRadius,
1133 const Eigen::Vector3f & viewpoint)
1135 UWARN(
"Add angle to avoid subtraction of points with opposite normals");
1136 UASSERT(minNeighborsInRadius > 0);
1138 pcl::search::KdTree<pcl::PointXYZRGB>::Ptr
tree (
new pcl::search::KdTree<pcl::PointXYZRGB>(
false));
1142 pcl::IndicesPtr output(
new std::vector<int>(indices->size()));
1144 if(substractIndices->size())
1146 tree->setInputCloud(substractCloud, substractIndices);
1150 tree->setInputCloud(substractCloud);
1152 for(
unsigned int i=0; i<indices->size(); ++i)
1157 std::vector<float> kSqrdDistances;
1158 float radius = radiusSearchRatio*
uNorm(
1159 cloud->at(indices->at(i)).x-viewpoint[0],
1160 cloud->at(indices->at(i)).y-viewpoint[1],
1161 cloud->at(indices->at(i)).z-viewpoint[2]);
1162 int k = tree->radiusSearch(cloud->at(indices->at(i)), radius, kIndices, kSqrdDistances);
1163 if(k < minNeighborsInRadius)
1165 output->at(oi++) = indices->at(i);
1174 pcl::IndicesPtr output(
new std::vector<int>(cloud->size()));
1176 if(substractIndices->size())
1178 tree->setInputCloud(substractCloud, substractIndices);
1182 tree->setInputCloud(substractCloud);
1184 for(
unsigned int i=0; i<cloud->size(); ++i)
1189 std::vector<float> kSqrdDistances;
1190 float radius = radiusSearchRatio*
uNorm(
1191 cloud->at(i).x-viewpoint[0],
1192 cloud->at(i).y-viewpoint[1],
1193 cloud->at(i).z-viewpoint[2]);
1194 int k = tree->radiusSearch(cloud->at(i), radius,
kIndices, kSqrdDistances);
1195 if(k < minNeighborsInRadius)
1197 output->at(oi++) = i;
1206 const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
1207 const pcl::IndicesPtr & indices,
1208 const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
1209 const pcl::IndicesPtr & substractIndices,
1210 float radiusSearchRatio,
1212 int minNeighborsInRadius,
1213 const Eigen::Vector3f & viewpoint)
1215 UASSERT(minNeighborsInRadius > 0);
1217 pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr
tree (
new pcl::search::KdTree<pcl::PointXYZRGBNormal>(
false));
1221 pcl::IndicesPtr output(
new std::vector<int>(indices->size()));
1223 if(substractIndices->size())
1225 tree->setInputCloud(substractCloud, substractIndices);
1229 tree->setInputCloud(substractCloud);
1231 for(
unsigned int i=0; i<indices->size(); ++i)
1236 std::vector<float> kSqrdDistances;
1237 float radius = radiusSearchRatio*
uNorm(
1238 cloud->at(indices->at(i)).x-viewpoint[0],
1239 cloud->at(indices->at(i)).y-viewpoint[1],
1240 cloud->at(indices->at(i)).z-viewpoint[2]);
1241 int k = tree->radiusSearch(cloud->at(indices->at(i)), radius, kIndices, kSqrdDistances);
1242 if(k>=minNeighborsInRadius && maxAngle > 0.0
f)
1244 Eigen::Vector4f normal(cloud->at(indices->at(i)).normal_x, cloud->at(indices->at(i)).normal_y, cloud->at(indices->at(i)).normal_z, 0.0
f);
1250 for(
int j=0; j<count && k >= minNeighborsInRadius; ++j)
1252 Eigen::Vector4f v(substractCloud->at(kIndices.at(j)).normal_x, substractCloud->at(kIndices.at(j)).normal_y, substractCloud->at(kIndices.at(j)).normal_z, 0.0
f);
1257 float angle = pcl::getAngle3D(normal, v);
1258 if(angle > maxAngle)
1275 if(k < minNeighborsInRadius)
1277 output->at(oi++) = indices->at(i);
1286 pcl::IndicesPtr output(
new std::vector<int>(cloud->size()));
1288 if(substractIndices->size())
1290 tree->setInputCloud(substractCloud, substractIndices);
1294 tree->setInputCloud(substractCloud);
1296 for(
unsigned int i=0; i<cloud->size(); ++i)
1301 std::vector<float> kSqrdDistances;
1302 float radius = radiusSearchRatio*
uNorm(
1303 cloud->at(i).x-viewpoint[0],
1304 cloud->at(i).y-viewpoint[1],
1305 cloud->at(i).z-viewpoint[2]);
1306 int k = tree->radiusSearch(cloud->at(i), radius,
kIndices, kSqrdDistances);
1307 if(k>=minNeighborsInRadius && maxAngle > 0.0
f)
1309 Eigen::Vector4f normal(cloud->at(i).normal_x, cloud->at(i).normal_y, cloud->at(i).normal_z, 0.0f);
1315 for(
int j=0; j<count && k >= minNeighborsInRadius; ++j)
1317 Eigen::Vector4f v(substractCloud->at(kIndices.at(j)).normal_x, substractCloud->at(kIndices.at(j)).normal_y, substractCloud->at(kIndices.at(j)).normal_z, 0.0
f);
1322 float angle = pcl::getAngle3D(normal, v);
1323 if(angle > maxAngle)
1339 if(k < minNeighborsInRadius)
1341 output->at(oi++) = i;
1352 const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
1354 const Eigen::Vector4f & normal,
1356 const Eigen::Vector4f & viewpoint)
1358 pcl::IndicesPtr indices(
new std::vector<int>);
1359 return normalFiltering(cloud, indices, angleMax, normal, normalKSearch, viewpoint);
1362 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
1364 const Eigen::Vector4f & normal,
1366 const Eigen::Vector4f & viewpoint)
1368 pcl::IndicesPtr indices(
new std::vector<int>);
1369 return normalFiltering(cloud, indices, angleMax, normal, normalKSearch, viewpoint);
1373 template<
typename Po
intT>
1375 const typename pcl::PointCloud<PointT>::Ptr & cloud,
1376 const pcl::IndicesPtr & indices,
1378 const Eigen::Vector4f & normal,
1380 const Eigen::Vector4f & viewpoint)
1382 pcl::IndicesPtr output(
new std::vector<int>());
1386 typename pcl::search::KdTree<PointT>::Ptr
tree(
new pcl::search::KdTree<PointT>(
false));
1388 pcl::NormalEstimationOMP<PointT, pcl::Normal> ne;
1389 ne.setInputCloud (cloud);
1392 ne.setIndices(indices);
1397 tree->setInputCloud(cloud, indices);
1401 tree->setInputCloud(cloud);
1403 ne.setSearchMethod (tree);
1405 pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (
new pcl::PointCloud<pcl::Normal>);
1407 ne.setKSearch(normalKSearch);
1408 if(viewpoint[0] != 0 || viewpoint[1] != 0 || viewpoint[2] != 0)
1410 ne.setViewPoint(viewpoint[0], viewpoint[1], viewpoint[2]);
1413 ne.compute (*cloud_normals);
1415 output->resize(cloud_normals->size());
1417 for(
unsigned int i=0; i<cloud_normals->size(); ++i)
1419 Eigen::Vector4f v(cloud_normals->at(i).normal_x, cloud_normals->at(i).normal_y, cloud_normals->at(i).normal_z, 0.0f);
1420 float angle = pcl::getAngle3D(normal, v);
1421 if(angle < angleMax)
1423 output->at(oi++) = indices->size()!=0?indices->at(i):i;
1432 const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
1433 const pcl::IndicesPtr & indices,
1435 const Eigen::Vector4f & normal,
1437 const Eigen::Vector4f & viewpoint)
1440 return normalFilteringImpl<pcl::PointXYZ>(cloud, indices, angleMax, normal, normalKSearch, viewpoint);
1443 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
1444 const pcl::IndicesPtr & indices,
1446 const Eigen::Vector4f & normal,
1448 const Eigen::Vector4f & viewpoint)
1450 return normalFilteringImpl<pcl::PointXYZRGB>(cloud, indices, angleMax, normal, normalKSearch, viewpoint);
1453 template<
typename Po
intT>
1455 const typename pcl::PointCloud<PointT>::Ptr & cloud,
1456 const pcl::IndicesPtr & indices,
1458 const Eigen::Vector4f & normal)
1460 pcl::IndicesPtr output(
new std::vector<int>());
1467 output->resize(indices->size());
1468 for(
unsigned int i=0; i<indices->size(); ++i)
1470 Eigen::Vector4f v(cloud->at(indices->at(i)).normal_x, cloud->at(indices->at(i)).normal_y, cloud->at(indices->at(i)).normal_z, 0.0
f);
1471 float angle = pcl::getAngle3D(normal, v);
1472 if(angle < angleMax)
1474 output->at(oi++) = indices->size()!=0?indices->at(i):i;
1480 output->resize(cloud->size());
1481 for(
unsigned int i=0; i<cloud->size(); ++i)
1483 Eigen::Vector4f v(cloud->at(i).normal_x, cloud->at(i).normal_y, cloud->at(i).normal_z, 0.0f);
1484 float angle = pcl::getAngle3D(normal, v);
1485 if(angle < angleMax)
1487 output->at(oi++) = indices->size()!=0?indices->at(i):i;
1498 const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
1499 const pcl::IndicesPtr & indices,
1501 const Eigen::Vector4f & normal,
1503 const Eigen::Vector4f & viewpoint)
1505 return normalFilteringImpl<pcl::PointNormal>(cloud, indices, angleMax, normal);
1508 const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
1509 const pcl::IndicesPtr & indices,
1511 const Eigen::Vector4f & normal,
1513 const Eigen::Vector4f & viewpoint)
1515 return normalFilteringImpl<pcl::PointXYZRGBNormal>(cloud, indices, angleMax, normal);
1519 const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
1520 float clusterTolerance,
1523 int * biggestClusterIndex)
1525 pcl::IndicesPtr indices(
new std::vector<int>);
1526 return extractClusters(cloud, indices, clusterTolerance, minClusterSize, maxClusterSize, biggestClusterIndex);
1529 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
1530 float clusterTolerance,
1533 int * biggestClusterIndex)
1535 pcl::IndicesPtr indices(
new std::vector<int>);
1536 return extractClusters(cloud, indices, clusterTolerance, minClusterSize, maxClusterSize, biggestClusterIndex);
1539 template<
typename Po
intT>
1541 const typename pcl::PointCloud<PointT>::Ptr & cloud,
1542 const pcl::IndicesPtr & indices,
1543 float clusterTolerance,
1546 int * biggestClusterIndex)
1548 typename pcl::search::KdTree<PointT>::Ptr
tree (
new pcl::search::KdTree<PointT>(
true));
1549 pcl::EuclideanClusterExtraction<PointT> ec;
1550 ec.setClusterTolerance (clusterTolerance);
1551 ec.setMinClusterSize (minClusterSize);
1552 ec.setMaxClusterSize (maxClusterSize);
1553 ec.setInputCloud (cloud);
1557 ec.setIndices(indices);
1558 tree->setInputCloud(cloud, indices);
1562 tree->setInputCloud(cloud);
1564 ec.setSearchMethod (tree);
1566 std::vector<pcl::PointIndices> cluster_indices;
1567 ec.extract (cluster_indices);
1570 unsigned int maxSize = 0;
1571 std::vector<pcl::IndicesPtr> output(cluster_indices.size());
1572 for(
unsigned int i=0; i<cluster_indices.size(); ++i)
1574 output[i] = pcl::IndicesPtr(
new std::vector<int>(cluster_indices[i].indices));
1576 if(maxSize < cluster_indices[i].indices.size())
1578 maxSize = (
unsigned int)cluster_indices[i].indices.size();
1582 if(biggestClusterIndex)
1584 *biggestClusterIndex = maxIndex;
1591 const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
1592 const pcl::IndicesPtr & indices,
1593 float clusterTolerance,
1596 int * biggestClusterIndex)
1598 return extractClustersImpl<pcl::PointXYZ>(cloud, indices, clusterTolerance, minClusterSize, maxClusterSize, biggestClusterIndex);
1601 const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
1602 const pcl::IndicesPtr & indices,
1603 float clusterTolerance,
1606 int * biggestClusterIndex)
1608 return extractClustersImpl<pcl::PointNormal>(cloud, indices, clusterTolerance, minClusterSize, maxClusterSize, biggestClusterIndex);
1611 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
1612 const pcl::IndicesPtr & indices,
1613 float clusterTolerance,
1616 int * biggestClusterIndex)
1618 return extractClustersImpl<pcl::PointXYZRGB>(cloud, indices, clusterTolerance, minClusterSize, maxClusterSize, biggestClusterIndex);
1621 const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
1622 const pcl::IndicesPtr & indices,
1623 float clusterTolerance,
1626 int * biggestClusterIndex)
1628 return extractClustersImpl<pcl::PointXYZRGBNormal>(cloud, indices, clusterTolerance, minClusterSize, maxClusterSize, biggestClusterIndex);
1631 template<
typename Po
intT>
1633 const typename pcl::PointCloud<PointT>::Ptr & cloud,
1634 const pcl::IndicesPtr & indices,
1637 pcl::IndicesPtr output(
new std::vector<int>);
1638 pcl::ExtractIndices<PointT> extract;
1639 extract.setInputCloud (cloud);
1640 extract.setIndices(indices);
1641 extract.setNegative(negative);
1642 extract.filter(*output);
1646 pcl::IndicesPtr
extractIndices(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices,
bool negative)
1648 return extractIndicesImpl<pcl::PointXYZ>(cloud, indices, negative);
1650 pcl::IndicesPtr
extractIndices(
const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
bool negative)
1652 return extractIndicesImpl<pcl::PointNormal>(cloud, indices, negative);
1654 pcl::IndicesPtr
extractIndices(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
bool negative)
1656 return extractIndicesImpl<pcl::PointXYZRGB>(cloud, indices, negative);
1658 pcl::IndicesPtr
extractIndices(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
bool negative)
1660 return extractIndicesImpl<pcl::PointXYZRGBNormal>(cloud, indices, negative);
1663 template<
typename Po
intT>
1665 const typename pcl::PointCloud<PointT>::Ptr & cloud,
1666 const pcl::IndicesPtr & indices,
1670 typename pcl::PointCloud<PointT>::Ptr output(
new pcl::PointCloud<PointT>);
1671 pcl::ExtractIndices<PointT> extract;
1672 extract.setInputCloud (cloud);
1673 extract.setIndices(indices);
1674 extract.setNegative(negative);
1675 extract.setKeepOrganized(keepOrganized);
1676 extract.filter(*output);
1679 pcl::PointCloud<pcl::PointXYZ>::Ptr
extractIndices(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices,
bool negative,
bool keepOrganized)
1681 return extractIndicesImpl<pcl::PointXYZ>(cloud, indices, negative, keepOrganized);
1683 pcl::PointCloud<pcl::PointXYZRGB>::Ptr
extractIndices(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
bool negative,
bool keepOrganized)
1685 return extractIndicesImpl<pcl::PointXYZRGB>(cloud, indices, negative, keepOrganized);
1692 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr
extractIndices(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
bool negative,
bool keepOrganized)
1694 return extractIndicesImpl<pcl::PointXYZRGBNormal>(cloud, indices, negative, keepOrganized);
1698 const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
1699 float distanceThreshold,
1701 pcl::ModelCoefficients * coefficientsOut)
1703 pcl::IndicesPtr indices(
new std::vector<int>);
1704 return extractPlane(cloud, indices, distanceThreshold, maxIterations, coefficientsOut);
1708 const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
1709 const pcl::IndicesPtr & indices,
1710 float distanceThreshold,
1712 pcl::ModelCoefficients * coefficientsOut)
1715 pcl::ModelCoefficients::Ptr coefficients (
new pcl::ModelCoefficients);
1716 pcl::PointIndices::Ptr inliers (
new pcl::PointIndices);
1718 pcl::SACSegmentation<pcl::PointXYZ> seg;
1720 seg.setOptimizeCoefficients (
true);
1721 seg.setMaxIterations (maxIterations);
1723 seg.setModelType (pcl::SACMODEL_PLANE);
1724 seg.setMethodType (pcl::SAC_RANSAC);
1725 seg.setDistanceThreshold (distanceThreshold);
1727 seg.setInputCloud (cloud);
1730 seg.setIndices(indices);
1732 seg.segment (*inliers, *coefficients);
1736 *coefficientsOut = *coefficients;
1739 return pcl::IndicesPtr(
new std::vector<int>(inliers->indices));
LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PCLPointCloud2 &cloud, bool filterNaNs=true)
bool RTABMAP_EXP isFinite(const cv::Point3f &pt)
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_EXP subtractFiltering(const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &substractCloud, float radiusSearch, int minNeighborsInRadius=1)
pcl::IndicesPtr RTABMAP_EXP cropBox(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, const Eigen::Vector4f &min, const Eigen::Vector4f &max, const Transform &transform=Transform::getIdentity(), bool negative=false)
T uNorm(const std::vector< T > &v)
std::vector< pcl::IndicesPtr > RTABMAP_EXP extractClusters(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, float clusterTolerance, int minClusterSize, int maxClusterSize=std::numeric_limits< int >::max(), int *biggestClusterIndex=0)
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
const cv::Mat & data() const
pcl::PointCloud< pcl::Normal >::Ptr RTABMAP_EXP computeNormals2D(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, int searchK=5, float searchRadius=0.0f, const Eigen::Vector3f &viewPoint=Eigen::Vector3f(0, 0, 0))
pcl::IndicesPtr RTABMAP_EXP passThrough(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, const std::string &axis, float min, float max, bool negative=false)
pcl::IndicesPtr passThroughImpl(const typename pcl::PointCloud< PointT >::Ptr &cloud, const pcl::IndicesPtr &indices, const std::string &axis, float min, float max, bool negative)
pcl::PointCloud< PointT >::Ptr removeNaNNormalsFromPointCloudImpl(const typename pcl::PointCloud< PointT >::Ptr &cloud)
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)
GLM_FUNC_DECL detail::tvec3< T, P > axis(detail::tquat< T, P > const &x)
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP removeNaNFromPointCloud(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud)
pcl::PointCloud< PointT >::Ptr randomSamplingImpl(const typename pcl::PointCloud< PointT >::Ptr &cloud, int samples)
GLM_FUNC_DECL bool all(vecType< bool, P > const &v)
pcl::IndicesPtr RTABMAP_EXP subtractAdaptiveFiltering(const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, const pcl::IndicesPtr &indices, const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &substractCloud, const pcl::IndicesPtr &substractIndices, float radiusSearchRatio=0.01, int minNeighborsInRadius=1, const Eigen::Vector3f &viewpoint=Eigen::Vector3f(0, 0, 0))
Basic mathematics functions.
GLM_FUNC_DECL T angle(detail::tquat< T, P > const &x)
pcl::PointCloud< PointT >::Ptr removeNaNFromPointCloudImpl(const typename pcl::PointCloud< PointT >::Ptr &cloud)
Some conversion functions.
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)
GLM_FUNC_DECL genType step(genType const &edge, genType const &x)
pcl::IndicesPtr RTABMAP_EXP frustumFiltering(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, const Transform &cameraPose, float horizontalFOV, float verticalFOV, float nearClipPlaneDistance, float farClipPlaneDistance, bool negative=false)
pcl::IndicesPtr RTABMAP_EXP extractIndices(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, bool negative)
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP laserScanToPointCloud(const LaserScan &laserScan, const Transform &transform=Transform())
pcl::PointCloud< PointT >::Ptr voxelizeImpl(const typename pcl::PointCloud< PointT >::Ptr &cloud, const pcl::IndicesPtr &indices, float voxelSize)
LaserScan RTABMAP_EXP downsample(const LaserScan &cloud, int step)
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud< pcl::PointXYZ > &cloud, const Transform &transform=Transform(), bool filterNaNs=true)
bool uIsFinite(const T &value)
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)
#define UASSERT(condition)
pcl::PointCloud< PointT >::Ptr downsampleImpl(const typename pcl::PointCloud< PointT >::Ptr &cloud, int step)
#define UASSERT_MSG(condition, msg_str)
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP randomSampling(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, int samples)
std::vector< pcl::IndicesPtr > extractClustersImpl(const typename pcl::PointCloud< PointT >::Ptr &cloud, const pcl::IndicesPtr &indices, float clusterTolerance, int minClusterSize, int maxClusterSize, int *biggestClusterIndex)
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)
cv::Mat RTABMAP_EXP computeNormals(const cv::Mat &laserScan, int searchK, float searchRadius)
Transform localTransform() const
pcl::IndicesPtr radiusFilteringImpl(const typename pcl::PointCloud< PointT >::Ptr &cloud, const pcl::IndicesPtr &indices, float radiusSearch, int minNeighborsInRadius)
pcl::IndicesPtr RTABMAP_EXP radiusFiltering(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, float radiusSearch, int minNeighborsInRadius)
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
pcl::IndicesPtr extractPlane(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, float distanceThreshold, int maxIterations=100, pcl::ModelCoefficients *coefficientsOut=0)
LaserScan RTABMAP_EXP adjustNormalsToViewPoint(const LaserScan &scan, const Eigen::Vector3f &viewpoint, bool forceGroundNormalsUp)
ULogger class and convenient macros.
LaserScan RTABMAP_EXP rangeFiltering(const LaserScan &scan, float rangeMin, float rangeMax)
pcl::PointCloud< pcl::PointNormal >::Ptr RTABMAP_EXP removeNaNNormalsFromPointCloud(const pcl::PointCloud< pcl::PointNormal >::Ptr &cloud)
LaserScan RTABMAP_EXP commonFiltering(const LaserScan &scan, int downsamplingStep, float rangeMin=0.0f, float rangeMax=0.0f, float voxelSize=0.0f, int normalK=0, float normalRadius=0.0f, bool forceGroundNormalsUp=false)
static const GLushort kIndices[]
pcl::PointCloud< pcl::PointXYZI >::Ptr RTABMAP_EXP laserScanToPointCloudI(const LaserScan &laserScan, const Transform &transform=Transform(), float intensity=0.0f)
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP voxelize(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, float voxelSize)
std::string UTILITE_EXP uFormat(const char *fmt,...)
pcl::IndicesPtr extractIndicesImpl(const typename pcl::PointCloud< PointT >::Ptr &cloud, const pcl::IndicesPtr &indices, bool negative)
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_EXP laserScanToPointCloudRGB(const LaserScan &laserScan, const Transform &transform=Transform(), unsigned char r=255, unsigned char g=255, unsigned char b=255)
bool hasIntensity() const
pcl::IndicesPtr RTABMAP_EXP normalFiltering(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, float angleMax, const Eigen::Vector4f &normal, int normalKSearch, const Eigen::Vector4f &viewpoint)