+
+
3 #ifndef __YARP_CLOUD_UTILS_PCL_IMPL_HPP__
+
4 #define __YARP_CLOUD_UTILS_PCL_IMPL_HPP__
+
+
6 #define _USE_MATH_DEFINES
+
+
+
+
+
+
+
+
+
15 #include <pcl/pcl_config.h>
+
16 #include <pcl/point_cloud.h>
+
17 #include <pcl/point_types.h>
+
18 #include <pcl/PolygonMesh.h>
+
19 #include <pcl/common/transforms.h>
+
20 #include <pcl/features/normal_3d.h>
+
21 #include <pcl/features/normal_3d_omp.h>
+
22 #include <pcl/filters/approximate_voxel_grid.h>
+
23 #include <pcl/filters/bilateral.h>
+
24 #include <pcl/filters/crop_box.h>
+
25 #include <pcl/filters/fast_bilateral.h>
+
26 #include <pcl/filters/fast_bilateral_omp.h>
+
27 #include <pcl/filters/grid_minimum.h>
+
28 #include <pcl/filters/local_maximum.h>
+
29 #include <pcl/filters/median_filter.h>
+
30 #include <pcl/filters/passthrough.h>
+
31 #include <pcl/filters/radius_outlier_removal.h>
+
32 #include <pcl/filters/random_sample.h>
+
33 #include <pcl/filters/sampling_surface_normal.h>
+
34 #include <pcl/filters/shadowpoints.h>
+
35 #include <pcl/filters/statistical_outlier_removal.h>
+
36 #include <pcl/filters/uniform_sampling.h>
+
37 #include <pcl/filters/voxel_grid.h>
+
38 #include <pcl/search/kdtree.h>
+
39 #include <pcl/surface/bilateral_upsampling.h>
+
40 #include <pcl/surface/concave_hull.h>
+
41 #include <pcl/surface/convex_hull.h>
+
42 #include <pcl/surface/gp3.h>
+
43 #include <pcl/surface/grid_projection.h>
+
44 #include <pcl/surface/marching_cubes_hoppe.h>
+
45 #include <pcl/surface/marching_cubes_rbf.h>
+
46 #if PCL_VERSION_COMPARE(>=, 1, 9, 0)
+
47 #include <pcl/surface/mls.h>
+
+
49 #include <pcl/surface/organized_fast_mesh.h>
+
50 #include <pcl/surface/poisson.h>
+
51 #include <pcl/surface/simplification_remove_unused_vertices.h>
+
52 #include <pcl/surface/vtk_smoothing/vtk_mesh_quadric_decimation.h>
+
53 #include <pcl/surface/vtk_smoothing/vtk_mesh_smoothing_laplacian.h>
+
54 #include <pcl/surface/vtk_smoothing/vtk_mesh_smoothing_windowed_sinc.h>
+
55 #include <pcl/surface/vtk_smoothing/vtk_mesh_subdivision.h>
+
+
+
+
+
60 auto getTransformation(
const yarp::os::Searchable & options)
+
+
62 auto transformation = Eigen::Transform<double, 3, Eigen::Affine>::Identity();
+
+
64 if (
const auto & translation = options.find(
"translation"); !translation.isNull())
+
+
66 if (!translation.isList() || translation.asList()->size() != 3)
+
+
68 throw std::runtime_error(
"translation is not a list or size not equal to 3");
+
+
+
71 const auto * b = translation.asList();
+
72 Eigen::Vector3d vector(b->get(0).asFloat64(), b->get(1).asFloat64(), b->get(2).asFloat64());
+
73 transformation.translate(vector);
+
+
+
76 if (
const auto & rotation = options.find(
"rotation"); !rotation.isNull())
+
+
78 if (!rotation.isList() || rotation.asList()->size() != 3)
+
+
80 throw std::runtime_error(
"rotation is not a list or size not equal to 3");
+
+
+
83 const auto * b = rotation.asList();
+
84 Eigen::Vector3d axis(b->get(0).asFloat64(), b->get(1).asFloat64(), b->get(2).asFloat64());
+
85 Eigen::AngleAxisd rot(axis.norm(), axis.normalized());
+
86 transformation.rotate(rot);
+
+
+
89 return transformation;
+
+
+
+
93 void checkOutput(
const typename pcl::PointCloud<T>::ConstPtr & cloud,
const std::string & caller)
+
+
+
+
97 throw std::runtime_error(caller +
" returned an empty cloud");
+
+
+
+
101 inline void checkOutput(
const pcl::PolygonMesh::ConstPtr & mesh,
const std::string & caller)
+
+
103 if (mesh->cloud.data.empty() || mesh->polygons.empty())
+
+
105 throw std::runtime_error(caller +
" returned an empty or incomplete mesh");
+
+
+
+
109 template <
typename T>
+
110 void doTransformPointCloud(
const typename pcl::PointCloud<T>::ConstPtr & in,
const typename pcl::PointCloud<T>::Ptr & out,
const yarp::os::Searchable & options)
+
+
112 auto transformation = getTransformation(options);
+
113 pcl::transformPointCloud(*in, *out, transformation);
+
114 checkOutput<T>(out,
"transformPointCloud");
+
+
+
117 template <
typename T>
+
118 void doTransformPointCloudWithNormals(
const typename pcl::PointCloud<T>::ConstPtr & in,
const typename pcl::PointCloud<T>::Ptr & out,
const yarp::os::Searchable & options)
+
+
120 auto transformation = getTransformation(options);
+
121 pcl::transformPointCloudWithNormals(*in, *out, transformation);
+
122 checkOutput<T>(out,
"transformPointCloudWithNormals");
+
+
+
125 template <
typename T>
+
126 void doApproximateVoxelGrid(
const typename pcl::PointCloud<T>::ConstPtr & in,
const typename pcl::PointCloud<T>::Ptr & out,
const yarp::os::Searchable & options)
+
+
128 auto downsampleAllData = options.check(
"downsampleAllData", yarp::os::Value(
true)).asBool();
+
129 auto leafSize = options.check(
"leafSize", yarp::os::Value(0.0f)).asFloat32();
+
130 auto leafSizeX = options.check(
"leafSizeX", yarp::os::Value(leafSize)).asFloat32();
+
131 auto leafSizeY = options.check(
"leafSizeY", yarp::os::Value(leafSize)).asFloat32();
+
132 auto leafSizeZ = options.check(
"leafSizeZ", yarp::os::Value(leafSize)).asFloat32();
+
+
134 pcl::ApproximateVoxelGrid<T> grid;
+
135 grid.setDownsampleAllData(downsampleAllData);
+
136 grid.setInputCloud(in);
+
137 grid.setLeafSize(leafSizeX, leafSizeY, leafSizeZ);
+
+
+
140 checkOutput<T>(out,
"ApproximateVoxelGrid");
+
+
+
143 template <
typename T>
+
144 void doBilateralFilter(
const typename pcl::PointCloud<T>::ConstPtr & in,
const typename pcl::PointCloud<T>::Ptr & out,
const yarp::os::Searchable & options)
+
+
146 auto halfSize = options.check(
"halfSize", yarp::os::Value(0.0)).asFloat64();
+
147 auto stdDev = options.check(
"stdDev", yarp::os::Value(std::numeric_limits<double>::max())).asFloat64();
+
+
149 typename pcl::search::KdTree<T>::Ptr tree(
new pcl::search::KdTree<T>());
+
150 tree->setInputCloud(in);
+
+
152 pcl::BilateralFilter<T> filter;
+
153 filter.setHalfSize(halfSize);
+
154 filter.setInputCloud(in);
+
155 filter.setSearchMethod(tree);
+
156 filter.setStdDev(stdDev);
+
+
+
159 checkOutput<T>(out,
"BilateralFilter");
+
+
+
162 template <
typename T>
+
163 void doBilateralUpsampling(
const typename pcl::PointCloud<T>::ConstPtr & in,
const typename pcl::PointCloud<T>::Ptr & out,
const yarp::os::Searchable & options)
+
+
165 auto sigmaColor = options.check(
"sigmaColor", yarp::os::Value(15.0f)).asFloat32();
+
166 auto sigmaDepth = options.check(
"sigmaDepth", yarp::os::Value(0.5f)).asFloat32();
+
167 auto windowSize = options.check(
"windowSize", yarp::os::Value(5)).asInt32();
+
+
169 pcl::BilateralUpsampling<T, T> upsampler;
+
170 upsampler.setInputCloud(in);
+
171 upsampler.setSigmaColor(sigmaColor);
+
172 upsampler.setSigmaDepth(sigmaDepth);
+
173 upsampler.setWindowSize(windowSize);
+
174 upsampler.process(*out);
+
+
176 checkOutput<T>(out,
"BilateralUpsampling");
+
+
+
179 template <
typename T>
+
180 void doConcaveHull(
const typename pcl::PointCloud<T>::ConstPtr & in,
const pcl::PolygonMesh::Ptr & out,
const yarp::os::Searchable & options)
+
+
182 auto alpha = options.check(
"alpha", yarp::os::Value(0.0)).asFloat64();
+
+
184 pcl::ConcaveHull<T> concave;
+
185 concave.setAlpha(alpha);
+
186 concave.setDimension(3);
+
187 concave.setInputCloud(in);
+
188 concave.setKeepInformation(
true);
+
189 concave.reconstruct(*out);
+
+
191 checkOutput(out,
"ConcaveHull");
+
+
+
194 template <
typename T>
+
195 void doConvexHull(
const typename pcl::PointCloud<T>::ConstPtr & in,
const pcl::PolygonMesh::Ptr & out,
const yarp::os::Searchable & options)
+
+
197 pcl::ConvexHull<T> convex;
+
198 convex.setDimension(3);
+
199 convex.setInputCloud(in);
+
200 convex.reconstruct(*out);
+
+
202 checkOutput(out,
"ConvexHull");
+
+
+
205 template <
typename T>
+
206 void doCropBox(
const typename pcl::PointCloud<T>::ConstPtr & in,
const typename pcl::PointCloud<T>::Ptr & out,
const yarp::os::Searchable & options)
+
+
208 auto keepOrganized = options.check(
"keepOrganized", yarp::os::Value(
false)).asBool();
+
209 auto maxX = options.check(
"maxX", yarp::os::Value(1.0f)).asFloat32();
+
210 auto maxY = options.check(
"maxY", yarp::os::Value(1.0f)).asFloat32();
+
211 auto maxZ = options.check(
"maxZ", yarp::os::Value(1.0f)).asFloat32();
+
212 auto minX = options.check(
"minX", yarp::os::Value(-1.0f)).asFloat32();
+
213 auto minY = options.check(
"minY", yarp::os::Value(-1.0f)).asFloat32();
+
214 auto minZ = options.check(
"minZ", yarp::os::Value(-1.0f)).asFloat32();
+
215 auto negative = options.check(
"negative", yarp::os::Value(
false)).asBool();
+
216 auto rotationX = options.check(
"rotationX", yarp::os::Value(0.0f)).asFloat32();
+
217 auto rotationY = options.check(
"rotationY", yarp::os::Value(0.0f)).asFloat32();
+
218 auto rotationZ = options.check(
"rotationZ", yarp::os::Value(0.0f)).asFloat32();
+
219 auto translationX = options.check(
"translationX", yarp::os::Value(0.0f)).asFloat32();
+
220 auto translationY = options.check(
"translationY", yarp::os::Value(0.0f)).asFloat32();
+
221 auto translationZ = options.check(
"translationZ", yarp::os::Value(0.0f)).asFloat32();
+
+
223 pcl::CropBox<T> cropper;
+
224 cropper.setInputCloud(in);
+
225 cropper.setKeepOrganized(keepOrganized);
+
226 cropper.setMax({maxX, maxY, maxZ, 1.0f});
+
227 cropper.setMin({minX, minY, minZ, 1.0f});
+
228 cropper.setNegative(negative);
+
229 cropper.setRotation({rotationX, rotationY, rotationZ});
+
230 cropper.setTranslation({translationX, translationY, translationZ});
+
231 cropper.filter(*out);
+
+
233 checkOutput<T>(out,
"CropBox");
+
+
+
236 template <
typename T>
+
237 void doFastBilateralFilter(
const typename pcl::PointCloud<T>::ConstPtr & in,
const typename pcl::PointCloud<T>::Ptr & out,
const yarp::os::Searchable & options)
+
+
239 auto sigmaR = options.check(
"sigmaR", yarp::os::Value(0.05f)).asFloat32();
+
240 auto sigmaS = options.check(
"sigmaS", yarp::os::Value(15.0f)).asFloat32();
+
+
242 pcl::FastBilateralFilter<T> fast;
+
243 fast.setInputCloud(in);
+
244 fast.setSigmaR(sigmaR);
+
245 fast.setSigmaS(sigmaS);
+
+
+
248 checkOutput<T>(out,
"FastBilateralFilter");
+
+
+
251 template <
typename T>
+
252 void doFastBilateralFilterOMP(
const typename pcl::PointCloud<T>::ConstPtr & in,
const typename pcl::PointCloud<T>::Ptr & out,
const yarp::os::Searchable & options)
+
+
254 auto numberOfThreads = options.check(
"numberOfThreads", yarp::os::Value(0)).asInt32();
+
255 auto sigmaR = options.check(
"sigmaR", yarp::os::Value(0.05f)).asFloat32();
+
256 auto sigmaS = options.check(
"sigmaS", yarp::os::Value(15.0f)).asFloat32();
+
+
258 pcl::FastBilateralFilterOMP<T> fast;
+
259 fast.setInputCloud(in);
+
260 fast.setNumberOfThreads(numberOfThreads);
+
261 fast.setSigmaR(sigmaR);
+
262 fast.setSigmaS(sigmaS);
+
+
+
265 checkOutput<T>(out,
"FastBilateralFilterOMP");
+
+
+
268 template <
typename T>
+
269 void doGreedyProjectionTriangulation(
const typename pcl::PointCloud<T>::ConstPtr & in,
const pcl::PolygonMesh::Ptr & out,
const yarp::os::Searchable & options)
+
+
271 auto consistentVertexOrdering = options.check(
"consistentVertexOrdering", yarp::os::Value(
false)).asBool();
+
272 auto maximumAngle = options.check(
"maximumAngle", yarp::os::Value(2 * M_PI / 3)).asFloat64();
+
273 auto maximumNearestNeighbors = options.check(
"maximumNearestNeighbors", yarp::os::Value(100)).asInt32();
+
274 auto maximumSurfaceAngle = options.check(
"maximumSurfaceAngle", yarp::os::Value(M_PI / 4)).asFloat64();
+
275 auto minimumAngle = options.check(
"minimumAngle", yarp::os::Value(M_PI / 18)).asFloat64();
+
276 auto mu = options.check(
"mu", yarp::os::Value(0.0)).asFloat64();
+
277 auto normalConsistency = options.check(
"normalConsistency", yarp::os::Value(
false)).asBool();
+
278 auto searchRadius = options.check(
"searchRadius", yarp::os::Value(0.0)).asFloat64();
+
+
280 typename pcl::search::KdTree<T>::Ptr tree(
new pcl::search::KdTree<T>());
+
281 tree->setInputCloud(in);
+
+
283 pcl::GreedyProjectionTriangulation<T> gp3;
+
284 gp3.setConsistentVertexOrdering(consistentVertexOrdering);
+
285 gp3.setInputCloud(in);
+
286 gp3.setMaximumAngle(maximumAngle);
+
287 gp3.setMaximumSurfaceAngle(maximumSurfaceAngle);
+
288 gp3.setMaximumNearestNeighbors(maximumNearestNeighbors);
+
289 gp3.setMinimumAngle(minimumAngle);
+
+
291 gp3.setNormalConsistency(normalConsistency);
+
292 gp3.setSearchMethod(tree);
+
293 gp3.setSearchRadius(searchRadius);
+
294 gp3.reconstruct(*out);
+
+
296 checkOutput(out,
"GreedyProjectionTriangulation");
+
+
+
299 template <
typename T>
+
300 void doGridMinimum(
const typename pcl::PointCloud<T>::ConstPtr & in,
const typename pcl::PointCloud<T>::Ptr & out,
const yarp::os::Searchable & options)
+
+
302 auto resolution = options.check(
"resolution", yarp::os::Value(0.0f)).asFloat32();
+
303 pcl::GridMinimum<T> grid(resolution);
+
304 grid.setInputCloud(in);
+
+
306 checkOutput<T>(out,
"GridMinimum");
+
+
+
309 template <
typename T>
+
310 void doGridProjection(
const typename pcl::PointCloud<T>::ConstPtr & in,
const pcl::PolygonMesh::Ptr & out,
const yarp::os::Searchable & options)
+
+
312 auto maxBinarySearchLevel = options.check(
"maxBinarySearchLevel", yarp::os::Value(10)).asInt32();
+
313 auto nearestNeighborNum = options.check(
"nearestNeighborNum", yarp::os::Value(50)).asInt32();
+
314 auto paddingSize = options.check(
"paddingSize", yarp::os::Value(3)).asInt32();
+
315 auto resolution = options.check(
"resolution", yarp::os::Value(0.001)).asFloat64();
+
+
317 typename pcl::search::KdTree<T>::Ptr tree(
new pcl::search::KdTree<T>());
+
318 tree->setInputCloud(in);
+
+
320 pcl::GridProjection<T> gp;
+
321 gp.setInputCloud(in);
+
322 gp.setMaxBinarySearchLevel(maxBinarySearchLevel);
+
323 gp.setNearestNeighborNum(nearestNeighborNum);
+
324 gp.setPaddingSize(paddingSize);
+
325 gp.setResolution(resolution);
+
326 gp.setSearchMethod(tree);
+
327 gp.reconstruct(*out);
+
+
329 checkOutput(out,
"GridProjection");
+
+
+
332 template <
typename T>
+
333 void doLocalMaximum(
const typename pcl::PointCloud<T>::ConstPtr & in,
const typename pcl::PointCloud<T>::Ptr & out,
const yarp::os::Searchable & options)
+
+
335 auto negative = options.check(
"negative", yarp::os::Value(
false)).asBool();
+
336 auto radius = options.check(
"radius", yarp::os::Value(1.0f)).asFloat32();
+
+
338 pcl::LocalMaximum<T> local;
+
339 local.setInputCloud(in);
+
340 local.setNegative(negative);
+
341 local.setRadius(radius);
+
+
+
344 checkOutput<T>(out,
"LocalMaximum");
+
+
+
347 template <
typename T>
+
348 void doMarchingCubesHoppe(
const typename pcl::PointCloud<T>::ConstPtr & in,
const pcl::PolygonMesh::Ptr & out,
const yarp::os::Searchable & options)
+
+
350 #if PCL_VERSION_COMPARE(>=, 1, 9, 0)
+
351 auto distanceIgnore = options.check(
"distanceIgnore", yarp::os::Value(-1.0f)).asFloat32();
+
+
353 auto gridResolution = options.check(
"gridResolution", yarp::os::Value(32)).asInt32();
+
354 auto gridResolutionX = options.check(
"gridResolutionX", yarp::os::Value(gridResolution)).asInt32();
+
355 auto gridResolutionY = options.check(
"gridResolutionY", yarp::os::Value(gridResolution)).asInt32();
+
356 auto gridResolutionZ = options.check(
"gridResolutionZ", yarp::os::Value(gridResolution)).asInt32();
+
357 auto isoLevel = options.check(
"isoLevel", yarp::os::Value(0.0f)).asFloat32();
+
358 auto percentageExtendGrid = options.check(
"percentageExtendGrid", yarp::os::Value(0.0f)).asFloat32();
+
+
360 typename pcl::search::KdTree<T>::Ptr tree(
new pcl::search::KdTree<T>());
+
361 tree->setInputCloud(in);
+
+
363 pcl::MarchingCubesHoppe<T> hoppe;
+
364 #if PCL_VERSION_COMPARE(>=, 1, 9, 0)
+
365 hoppe.setDistanceIgnore(distanceIgnore);
+
+
367 hoppe.setGridResolution(gridResolutionX, gridResolutionY, gridResolutionZ);
+
368 hoppe.setInputCloud(in);
+
369 hoppe.setIsoLevel(isoLevel);
+
370 hoppe.setPercentageExtendGrid(percentageExtendGrid);
+
371 hoppe.setSearchMethod(tree);
+
372 hoppe.reconstruct(*out);
+
+
374 checkOutput(out,
"MarchingCubesHoppe");
+
+
+
377 template <
typename T>
+
378 void doMarchingCubesRBF(
const typename pcl::PointCloud<T>::ConstPtr & in,
const pcl::PolygonMesh::Ptr & out,
const yarp::os::Searchable & options)
+
+
380 auto gridResolution = options.check(
"gridResolution", yarp::os::Value(32)).asInt32();
+
381 auto gridResolutionX = options.check(
"gridResolutionX", yarp::os::Value(gridResolution)).asInt32();
+
382 auto gridResolutionY = options.check(
"gridResolutionY", yarp::os::Value(gridResolution)).asInt32();
+
383 auto gridResolutionZ = options.check(
"gridResolutionZ", yarp::os::Value(gridResolution)).asInt32();
+
384 auto isoLevel = options.check(
"isoLevel", yarp::os::Value(0.0f)).asFloat32();
+
385 auto offSurfaceDisplacement = options.check(
"offSurfaceDisplacement", yarp::os::Value(0.1f)).asFloat32();
+
386 auto percentageExtendGrid = options.check(
"percentageExtendGrid", yarp::os::Value(0.0f)).asFloat32();
+
+
388 typename pcl::search::KdTree<T>::Ptr tree(
new pcl::search::KdTree<T>());
+
389 tree->setInputCloud(in);
+
+
391 pcl::MarchingCubesRBF<T> rbf;
+
392 rbf.setGridResolution(gridResolutionX, gridResolutionY, gridResolutionZ);
+
393 rbf.setInputCloud(in);
+
394 rbf.setIsoLevel(isoLevel);
+
395 rbf.setOffSurfaceDisplacement(offSurfaceDisplacement);
+
396 rbf.setPercentageExtendGrid(percentageExtendGrid);
+
397 rbf.setSearchMethod(tree);
+
398 rbf.reconstruct(*out);
+
+
400 checkOutput(out,
"MarchingCubesRBF");
+
+
+
403 template <
typename T>
+
404 void doMedianFilter(
const typename pcl::PointCloud<T>::ConstPtr & in,
const typename pcl::PointCloud<T>::Ptr & out,
const yarp::os::Searchable & options)
+
+
406 auto maxAllowedMovement = options.check(
"maxAllowedMovement", yarp::os::Value(std::numeric_limits<float>::max())).asFloat32();
+
407 auto windowSize = options.check(
"windowSize", yarp::os::Value(5)).asInt32();
+
+
409 pcl::MedianFilter<T> median;
+
410 median.setInputCloud(in);
+
411 median.setMaxAllowedMovement(maxAllowedMovement);
+
412 median.setWindowSize(windowSize);
+
+
+
415 checkOutput<T>(out,
"MedianFilter");
+
+
+
418 void doMeshQuadricDecimationVTK(
const pcl::PolygonMesh::ConstPtr & in,
const pcl::PolygonMesh::Ptr & out,
const yarp::os::Searchable & options)
+
+
420 auto targetReductionFactor = options.check(
"targetReductionFactor", yarp::os::Value(0.5f)).asFloat32();
+
+
422 pcl::MeshQuadricDecimationVTK quadric;
+
423 quadric.setInputMesh(in);
+
424 quadric.setTargetReductionFactor(targetReductionFactor);
+
425 quadric.process(*out);
+
+
427 checkOutput(out,
"MeshQuadricDecimationVTK");
+
+
+
430 void doMeshSmoothingLaplacianVTK(
const pcl::PolygonMesh::ConstPtr & in,
const pcl::PolygonMesh::Ptr & out,
const yarp::os::Searchable & options)
+
+
432 auto boundarySmoothing = options.check(
"boundarySmoothing", yarp::os::Value(
true)).asBool();
+
433 auto convergence = options.check(
"convergence", yarp::os::Value(0.0f)).asFloat32();
+
434 auto edgeAngle = options.check(
"edgeAngle", yarp::os::Value(15.0f)).asFloat32();
+
435 auto featureAngle = options.check(
"featureAngle", yarp::os::Value(45.0f)).asFloat32();
+
436 auto featureEdgeSmoothing = options.check(
"featureEdgeSmoothing", yarp::os::Value(
false)).asBool();
+
437 auto numIter = options.check(
"numIter", yarp::os::Value(20)).asInt32();
+
438 auto relaxationFactor = options.check(
"relaxationFactor", yarp::os::Value(0.01f)).asFloat32();
+
+
440 pcl::MeshSmoothingLaplacianVTK laplacian;
+
441 laplacian.setBoundarySmoothing(boundarySmoothing);
+
442 laplacian.setConvergence(convergence);
+
443 laplacian.setEdgeAngle(edgeAngle);
+
444 laplacian.setFeatureAngle(featureAngle);
+
445 laplacian.setFeatureEdgeSmoothing(featureEdgeSmoothing);
+
446 laplacian.setInputMesh(in);
+
447 laplacian.setNumIter(numIter);
+
448 laplacian.setRelaxationFactor(relaxationFactor);
+
449 laplacian.process(*out);
+
+
451 checkOutput(out,
"MeshSmoothingLaplacianVTK");
+
+
+
454 void doMeshSmoothingWindowedSincVTK(
const pcl::PolygonMesh::ConstPtr & in,
const pcl::PolygonMesh::Ptr & out,
const yarp::os::Searchable & options)
+
+
456 auto boundarySmoothing = options.check(
"boundarySmoothing", yarp::os::Value(
true)).asBool();
+
457 auto edgeAngle = options.check(
"edgeAngle", yarp::os::Value(15.0f)).asFloat32();
+
458 auto featureAngle = options.check(
"featureAngle", yarp::os::Value(45.0f)).asFloat32();
+
459 auto featureEdgeSmoothing = options.check(
"featureEdgeSmoothing", yarp::os::Value(
false)).asBool();
+
460 auto normalizeCoordinates = options.check(
"normalizeCoordinates", yarp::os::Value(
false)).asBool();
+
461 auto numIter = options.check(
"numIter", yarp::os::Value(20)).asInt32();
+
462 auto passBand = options.check(
"passBand", yarp::os::Value(0.1f)).asFloat32();
+
+
464 pcl::MeshSmoothingWindowedSincVTK windowed;
+
465 windowed.setBoundarySmoothing(boundarySmoothing);
+
466 windowed.setEdgeAngle(edgeAngle);
+
467 windowed.setFeatureAngle(featureAngle);
+
468 windowed.setFeatureEdgeSmoothing(featureEdgeSmoothing);
+
469 windowed.setInputMesh(in);
+
470 windowed.setNormalizeCoordinates(normalizeCoordinates);
+
471 windowed.setNumIter(numIter);
+
472 windowed.setPassBand(passBand);
+
473 windowed.process(*out);
+
+
475 checkOutput(out,
"MeshSmoothingWindowedSincVTK");
+
+
+
478 void doMeshSubdivisionVTK(
const pcl::PolygonMesh::ConstPtr & in,
const pcl::PolygonMesh::Ptr & out,
const yarp::os::Searchable & options)
+
+
480 auto filterTypeStr = options.check(
"filterType", yarp::os::Value(
"linear")).asString();
+
+
482 pcl::MeshSubdivisionVTK::MeshSubdivisionVTKFilterType filterType;
+
+
484 if (filterTypeStr ==
"butterfly")
+
+
486 filterType = pcl::MeshSubdivisionVTK::MeshSubdivisionVTKFilterType::BUTTERFLY;
+
+
488 else if (filterTypeStr ==
"linear")
+
+
490 filterType = pcl::MeshSubdivisionVTK::MeshSubdivisionVTKFilterType::LINEAR;
+
+
492 else if (filterTypeStr ==
"loop")
+
+
494 filterType = pcl::MeshSubdivisionVTK::MeshSubdivisionVTKFilterType::LOOP;
+
+
+
+
498 throw std::invalid_argument(
"unknown filter type: " + filterTypeStr);
+
+
+
501 pcl::MeshSubdivisionVTK subdivision;
+
502 subdivision.setFilterType(filterType);
+
503 subdivision.setInputMesh(in);
+
504 subdivision.process(*out);
+
+
506 checkOutput(out,
"MeshSubdivisionVTK");
+
+
+
509 #if PCL_VERSION_COMPARE(>=, 1, 9, 0)
+
510 template <
typename T1,
typename T2 = T1>
+
511 void doMovingLeastSquares(
const typename pcl::PointCloud<T1>::ConstPtr & in,
const typename pcl::PointCloud<T2>::Ptr & out,
const yarp::os::Searchable & options)
+
+
513 auto cacheMlsResults = options.check(
"cacheMlsResults", yarp::os::Value(
true)).asBool();
+
514 auto computeNormals = options.check(
"computeNormals", yarp::os::Value(
false)).asBool();
+
515 auto dilationIterations = options.check(
"dilationIterations", yarp::os::Value(0)).asInt32();
+
516 auto dilationVoxelSize = options.check(
"dilationVoxelSize", yarp::os::Value(1.0f)).asFloat32();
+
517 auto numberOfThreads = options.check(
"numberOfThreads", yarp::os::Value(1)).asInt32();
+
518 auto pointDensity = options.check(
"pointDensity", yarp::os::Value(0)).asInt32();
+
519 auto polynomialOrder = options.check(
"polynomialOrder", yarp::os::Value(2)).asInt32();
+
520 auto projectionMethodStr = options.check(
"projectionMethod", yarp::os::Value(
"simple")).asString();
+
521 auto searchRadius = options.check(
"searchRadius", yarp::os::Value(0.0)).asFloat64();
+
522 auto sqrGaussParam = options.check(
"sqrGaussParam", yarp::os::Value(0.0)).asFloat64();
+
523 auto upsamplingMethodStr = options.check(
"upsamplingMethod", yarp::os::Value(
"none")).asString();
+
524 auto upsamplingRadius = options.check(
"upsamplingRadius", yarp::os::Value(0.0)).asFloat64();
+
525 auto upsamplingStepSize = options.check(
"upsamplingStepSize", yarp::os::Value(0.0)).asFloat64();
+
+
527 pcl::MLSResult::ProjectionMethod projectionMethod;
+
+
529 if (projectionMethodStr ==
"none")
+
+
531 projectionMethod = pcl::MLSResult::ProjectionMethod::NONE;
+
+
533 else if (projectionMethodStr ==
"orthogonal")
+
+
535 projectionMethod = pcl::MLSResult::ProjectionMethod::ORTHOGONAL;
+
+
537 else if (projectionMethodStr ==
"simple")
+
+
539 projectionMethod = pcl::MLSResult::ProjectionMethod::SIMPLE;
+
+
+
+
543 throw std::invalid_argument(
"unknown projection method: " + projectionMethodStr);
+
+
+
546 typename pcl::MovingLeastSquares<T1, T2>::UpsamplingMethod upsamplingMethod;
+
+
548 if (upsamplingMethodStr ==
"distinctCloud")
+
+
550 upsamplingMethod = decltype(upsamplingMethod)::DISTINCT_CLOUD;
+
+
552 else if (upsamplingMethodStr ==
"none")
+
+
554 upsamplingMethod = decltype(upsamplingMethod)::NONE;
+
+
556 else if (upsamplingMethodStr ==
"randomUniformDensity")
+
+
558 upsamplingMethod = decltype(upsamplingMethod)::RANDOM_UNIFORM_DENSITY;
+
+
560 else if (upsamplingMethodStr ==
"sampleLocalPlane")
+
+
562 upsamplingMethod = decltype(upsamplingMethod)::SAMPLE_LOCAL_PLANE;
+
+
564 else if (upsamplingMethodStr ==
"voxelGridDilation")
+
+
566 upsamplingMethod = decltype(upsamplingMethod)::VOXEL_GRID_DILATION;
+
+
+
+
570 throw std::invalid_argument(
"unknown upsampling method: " + upsamplingMethodStr);
+
+
+
573 typename pcl::search::KdTree<T1>::Ptr tree(
new pcl::search::KdTree<T1>());
+
574 tree->setInputCloud(in);
+
+
576 pcl::MovingLeastSquares<T1, T2> mls;
+
577 mls.setCacheMLSResults(cacheMlsResults);
+
578 mls.setComputeNormals(computeNormals);
+
579 mls.setDilationIterations(dilationIterations);
+
580 mls.setDilationVoxelSize(dilationVoxelSize);
+
581 mls.setInputCloud(in);
+
582 mls.setNumberOfThreads(numberOfThreads);
+
583 mls.setPointDensity(pointDensity);
+
584 mls.setPolynomialOrder(polynomialOrder);
+
585 mls.setProjectionMethod(projectionMethod);
+
586 mls.setSearchMethod(tree);
+
587 mls.setSearchRadius(searchRadius);
+
588 mls.setSqrGaussParam(sqrGaussParam);
+
589 mls.setUpsamplingMethod(upsamplingMethod);
+
590 mls.setUpsamplingRadius(upsamplingRadius);
+
591 mls.setUpsamplingStepSize(upsamplingStepSize);
+
+
+
594 checkOutput<T2>(out,
"MovingLeastSquares");
+
+
+
+
598 template <
typename T1,
typename T2>
+
599 void doNormalEstimation(
const typename pcl::PointCloud<T1>::ConstPtr & in,
const typename pcl::PointCloud<T2>::Ptr & out,
const yarp::os::Searchable & options)
+
+
601 auto kSearch = options.check(
"kSearch", yarp::os::Value(0)).asInt32();
+
602 auto radiusSearch = options.check(
"radiusSearch", yarp::os::Value(0.0)).asFloat64();
+
+
604 typename pcl::search::KdTree<T1>::Ptr tree(
new pcl::search::KdTree<T1>());
+
605 tree->setInputCloud(in);
+
+
607 pcl::NormalEstimation<T1, T2> estimator;
+
608 estimator.setInputCloud(in);
+
609 estimator.setKSearch(kSearch);
+
610 estimator.setRadiusSearch(radiusSearch);
+
611 estimator.setSearchMethod(tree);
+
612 estimator.compute(*out);
+
+
614 checkOutput<T2>(out,
"NormalEstimation");
+
+
+
617 template <
typename T1,
typename T2>
+
618 void doNormalEstimationOMP(
const typename pcl::PointCloud<T1>::ConstPtr & in,
const typename pcl::PointCloud<T2>::Ptr & out,
const yarp::os::Searchable & options)
+
+
620 auto kSearch = options.check(
"kSearch", yarp::os::Value(0)).asInt32();
+
621 auto numberOfThreads = options.check(
"numberOfThreads", yarp::os::Value(0)).asInt32();
+
622 auto radiusSearch = options.check(
"radiusSearch", yarp::os::Value(0.0)).asFloat64();
+
+
624 typename pcl::search::KdTree<T1>::Ptr tree(
new pcl::search::KdTree<T1>());
+
625 tree->setInputCloud(in);
+
+
627 pcl::NormalEstimationOMP<T1, T2> estimator;
+
628 estimator.setInputCloud(in);
+
629 estimator.setKSearch(kSearch);
+
630 estimator.setNumberOfThreads(numberOfThreads);
+
631 estimator.setRadiusSearch(radiusSearch);
+
632 estimator.setSearchMethod(tree);
+
633 estimator.compute(*out);
+
+
635 checkOutput<T2>(out,
"NormalEstimationOMP");
+
+
+
638 template <
typename T>
+
639 void doOrganizedFastMesh(
const typename pcl::PointCloud<T>::ConstPtr & in,
const pcl::PolygonMesh::Ptr & out,
const yarp::os::Searchable & options)
+
+
641 if (!in->isOrganized())
+
+
+
644 throw std::invalid_argument(
"input cloud must be organized (height > 1) for OrganizedFastMesh");
+
+
+
647 auto angleTolerance = options.check(
"angleTolerance", yarp::os::Value(12.5 * M_PI / 180)).asFloat32();
+
648 auto depthDependent = options.check(
"depthDependent", yarp::os::Value(
false)).asBool();
+
649 auto distanceTolerance = options.check(
"distanceTolerance", yarp::os::Value(-1.0f)).asFloat32();
+
650 auto maxEdgeLengthA = options.check(
"maxEdgeLengthA", yarp::os::Value(0.0f)).asFloat32();
+
651 auto maxEdgeLengthB = options.check(
"maxEdgeLengthB", yarp::os::Value(0.0f)).asFloat32();
+
652 auto maxEdgeLengthC = options.check(
"maxEdgeLengthC", yarp::os::Value(0.0f)).asFloat32();
+
653 auto storeShadowedFaces = options.check(
"storeShadowedFaces", yarp::os::Value(
false)).asBool();
+
654 auto trianglePixelSize = options.check(
"trianglePixelSize", yarp::os::Value(1)).asInt32();
+
655 auto trianglePixelSizeColumns = options.check(
"trianglePixelSizeColumns", yarp::os::Value(trianglePixelSize)).asInt32();
+
656 auto trianglePixelSizeRows = options.check(
"trianglePixelSizeRows", yarp::os::Value(trianglePixelSize)).asInt32();
+
657 auto triangulationTypeStr = options.check(
"triangulationType", yarp::os::Value(
"quadMesh")).asString();
+
658 auto useDepthAsDistance = options.check(
"useDepthAsDistance", yarp::os::Value(
false)).asBool();
+
+
660 typename pcl::OrganizedFastMesh<T>::TriangulationType triangulationType;
+
+
662 if (triangulationTypeStr ==
"quadMesh")
+
+
664 triangulationType = decltype(triangulationType)::QUAD_MESH;
+
+
666 else if (triangulationTypeStr ==
"triangleAdaptiveCut")
+
+
668 triangulationType = decltype(triangulationType)::TRIANGLE_ADAPTIVE_CUT;
+
+
670 else if (triangulationTypeStr ==
"triangleLeftCut")
+
+
672 triangulationType = decltype(triangulationType)::TRIANGLE_LEFT_CUT;
+
+
674 else if (triangulationTypeStr ==
"triangleRightCut")
+
+
676 triangulationType = decltype(triangulationType)::TRIANGLE_RIGHT_CUT;
+
+
+
+
680 throw std::invalid_argument(
"unknown triangulation type: " + triangulationTypeStr);
+
+
+
683 pcl::OrganizedFastMesh<T> organized;
+
684 organized.setAngleTolerance(angleTolerance);
+
685 organized.setDistanceTolerance(distanceTolerance, depthDependent);
+
686 organized.setInputCloud(in);
+
687 organized.setMaxEdgeLength(maxEdgeLengthA, maxEdgeLengthB, maxEdgeLengthC);
+
688 organized.setTrianglePixelSize(trianglePixelSize);
+
689 organized.setTrianglePixelSizeColumns(trianglePixelSizeColumns);
+
690 organized.setTrianglePixelSizeRows(trianglePixelSizeRows);
+
691 organized.setTriangulationType(triangulationType);
+
692 organized.storeShadowedFaces(storeShadowedFaces);
+
693 organized.useDepthAsDistance(useDepthAsDistance);
+
694 organized.reconstruct(*out);
+
+
696 checkOutput(out,
"OrganizedFastMesh");
+
+
+
699 template <
typename T>
+
700 void doPassThrough(
const typename pcl::PointCloud<T>::ConstPtr & in,
const typename pcl::PointCloud<T>::Ptr & out,
const yarp::os::Searchable & options)
+
+
702 auto filterFieldName = options.check(
"filterFieldName", yarp::os::Value(
"")).asString();
+
703 auto filterLimitMax = options.check(
"filterLimitMax", yarp::os::Value(std::numeric_limits<float>::max())).asFloat32();
+
704 auto filterLimitMin = options.check(
"filterLimitMin", yarp::os::Value(std::numeric_limits<float>::min())).asFloat32();
+
705 auto negative = options.check(
"negative", yarp::os::Value(
false)).asBool();
+
+
707 pcl::PassThrough<T> pass;
+
708 pass.setFilterFieldName(filterFieldName);
+
709 pass.setFilterLimits(filterLimitMin, filterLimitMax);
+
710 pass.setInputCloud(in);
+
711 pass.setNegative(negative);
+
+
+
714 checkOutput<T>(out,
"PassThrough");
+
+
+
717 template <
typename T>
+
718 void doPoisson(
const typename pcl::PointCloud<T>::ConstPtr & in,
const pcl::PolygonMesh::Ptr & out,
const yarp::os::Searchable & options)
+
+
720 auto confidence = options.check(
"confidence", yarp::os::Value(
false)).asBool();
+
721 auto degree = options.check(
"degree", yarp::os::Value(2)).asInt32();
+
722 auto depth = options.check(
"depth", yarp::os::Value(8)).asInt32();
+
723 auto isoDivide = options.check(
"isoDivide", yarp::os::Value(8)).asInt32();
+
724 auto manifold = options.check(
"manifold", yarp::os::Value(
true)).asBool();
+
725 auto minDepth = options.check(
"minDepth", yarp::os::Value(5)).asInt32();
+
726 auto outputPolygons = options.check(
"outputPolygons", yarp::os::Value(
false)).asBool();
+
727 auto pointWeight = options.check(
"pointWeight", yarp::os::Value(4.0f)).asFloat32();
+
728 auto samplesPerNode = options.check(
"samplesPerNode", yarp::os::Value(1.0f)).asFloat32();
+
729 auto scale = options.check(
"scale", yarp::os::Value(1.1f)).asFloat32();
+
730 auto solverDivide = options.check(
"solverDivide", yarp::os::Value(8)).asInt32();
+
731 #if PCL_VERSION_COMPARE(>=, 1, 12, 0)
+
732 auto threads = options.check(
"threads", yarp::os::Value(1)).asInt32();
+
+
+
735 typename pcl::search::KdTree<T>::Ptr tree(
new pcl::search::KdTree<T>());
+
736 tree->setInputCloud(in);
+
+
738 pcl::Poisson<T> poisson;
+
739 poisson.setConfidence(confidence);
+
740 poisson.setDegree(degree);
+
741 poisson.setDepth(depth);
+
742 poisson.setInputCloud(in);
+
743 poisson.setIsoDivide(isoDivide);
+
744 poisson.setManifold(manifold);
+
745 poisson.setMinDepth(minDepth);
+
746 poisson.setOutputPolygons(outputPolygons);
+
747 poisson.setPointWeight(pointWeight);
+
748 poisson.setSamplesPerNode(samplesPerNode);
+
749 poisson.setScale(scale);
+
750 poisson.setSearchMethod(tree);
+
751 poisson.setSolverDivide(solverDivide);
+
752 #if PCL_VERSION_COMPARE(>=, 1, 12, 0)
+
753 poisson.setThreads(threads);
+
+
755 poisson.reconstruct(*out);
+
+
757 checkOutput(out,
"Poisson");
+
+
+
760 template <
typename T>
+
761 void doRadiusOutlierRemoval(
const typename pcl::PointCloud<T>::ConstPtr & in,
const typename pcl::PointCloud<T>::Ptr & out,
const yarp::os::Searchable & options)
+
+
763 auto minNeighborsInRadius = options.check(
"minNeighborsInRadius", yarp::os::Value(1)).asInt32();
+
764 auto negative = options.check(
"negative", yarp::os::Value(
false)).asBool();
+
765 auto radiusSearch = options.check(
"radiusSearch", yarp::os::Value(0.0)).asFloat64();
+
+
767 pcl::RadiusOutlierRemoval<T> remover;
+
768 remover.setInputCloud(in);
+
769 remover.setMinNeighborsInRadius(minNeighborsInRadius);
+
770 remover.setNegative(negative);
+
771 remover.setRadiusSearch(radiusSearch);
+
772 remover.filter(*out);
+
+
774 checkOutput<T>(out,
"RadiusOutlierRemoval");
+
+
+
777 template <
typename T>
+
778 void doRandomSample(
const typename pcl::PointCloud<T>::ConstPtr & in,
const typename pcl::PointCloud<T>::Ptr & out,
const yarp::os::Searchable & options)
+
+
780 auto negative = options.check(
"negative", yarp::os::Value(
false)).asBool();
+
781 auto sample = options.check(
"sample", yarp::os::Value(std::numeric_limits<int>::max())).asInt64();
+
782 auto seed = options.check(
"seed", yarp::os::Value(
static_cast<int>(std::time(
nullptr)))).asInt64();
+
+
784 pcl::RandomSample<T> random;
+
785 random.setInputCloud(in);
+
786 random.setNegative(negative);
+
787 random.setSample(sample);
+
788 random.setSeed(seed);
+
+
+
791 checkOutput<T>(out,
"RandomSample");
+
+
+
794 template <
typename T>
+
795 void doSamplingSurfaceNormal(
const typename pcl::PointCloud<T>::ConstPtr & in,
const typename pcl::PointCloud<T>::Ptr & out,
const yarp::os::Searchable & options)
+
+
797 auto ratio = options.check(
"ratio", yarp::os::Value(0.0f)).asFloat32();
+
798 auto sample = options.check(
"sample", yarp::os::Value(10)).asInt32();
+
799 auto seed = options.check(
"seed", yarp::os::Value(
static_cast<int>(std::time(
nullptr)))).asInt64();
+
+
801 pcl::SamplingSurfaceNormal<T> sampler;
+
802 sampler.setInputCloud(in);
+
803 sampler.setRatio(ratio);
+
804 sampler.setSample(sample);
+
805 sampler.setSeed(seed);
+
806 sampler.filter(*out);
+
+
808 checkOutput<T>(out,
"SamplingSurfaceNormal");
+
+
+
811 template <
typename T>
+
812 void doShadowPoints(
const typename pcl::PointCloud<T>::ConstPtr & in,
const typename pcl::PointCloud<T>::Ptr & out,
const yarp::os::Searchable & options)
+
+
814 auto keepOrganized = options.check(
"keepOrganized", yarp::os::Value(
false)).asBool();
+
815 auto negative = options.check(
"negative", yarp::os::Value(
false)).asBool();
+
816 auto threshold = options.check(
"threshold", yarp::os::Value(0.1f)).asFloat32();
+
+
818 #if PCL_VERSION_COMPARE(>=, 1, 11, 0)
+
819 typename pcl::PointCloud<T>::Ptr temp = std::const_pointer_cast<pcl::PointCloud<T>>(in);
+
+
821 typename pcl::PointCloud<T>::Ptr temp = boost::const_pointer_cast<pcl::PointCloud<T>>(in);
+
+
+
824 pcl::ShadowPoints<T, T> shadow;
+
825 shadow.setInputCloud(in);
+
826 shadow.setKeepOrganized(keepOrganized);
+
827 shadow.setNegative(negative);
+
828 shadow.setNormals(temp);
+
829 shadow.setThreshold(threshold);
+
+
+
832 checkOutput<T>(out,
"ShadowPoints");
+
+
+
835 void doSimplificationRemoveUnusedVertices(
const pcl::PolygonMesh::ConstPtr & in,
const pcl::PolygonMesh::Ptr & out,
const yarp::os::Searchable & options)
+
+
837 pcl::surface::SimplificationRemoveUnusedVertices cleaner;
+
838 cleaner.simplify(*in, *out);
+
839 checkOutput(out,
"doSimplificationRemoveUnusedVertices");
+
+
+
842 template <
typename T>
+
843 void doStatisticalOutlierRemoval(
const typename pcl::PointCloud<T>::ConstPtr & in,
const typename pcl::PointCloud<T>::Ptr & out,
const yarp::os::Searchable & options)
+
+
845 auto meanK = options.check(
"meanK", yarp::os::Value(1)).asInt32();
+
846 auto negative = options.check(
"negative", yarp::os::Value(
false)).asBool();
+
847 auto stddevMulThresh = options.check(
"stddevMulThresh", yarp::os::Value(0.0)).asFloat64();
+
+
849 pcl::StatisticalOutlierRemoval<T> remover;
+
850 remover.setInputCloud(in);
+
851 remover.setMeanK(meanK);
+
852 remover.setNegative(negative);
+
853 remover.setStddevMulThresh(stddevMulThresh);
+
854 remover.filter(*out);
+
+
856 checkOutput<T>(out,
"StatisticalOutlierRemoval");
+
+
+
859 template <
typename T>
+
860 void doUniformSampling(
const typename pcl::PointCloud<T>::ConstPtr & in,
const typename pcl::PointCloud<T>::Ptr & out,
const yarp::os::Searchable & options)
+
+
862 auto radiusSearch = options.check(
"radiusSearch", yarp::os::Value(0.0)).asFloat64();
+
+
864 pcl::UniformSampling<T> uniform;
+
865 uniform.setInputCloud(in);
+
866 uniform.setRadiusSearch(radiusSearch);
+
867 uniform.filter(*out);
+
+
869 checkOutput<T>(out,
"UniformSampling");
+
+
+
872 template <
typename T>
+
873 void doVoxelGrid(
const typename pcl::PointCloud<T>::ConstPtr & in,
const typename pcl::PointCloud<T>::Ptr & out,
const yarp::os::Searchable & options)
+
+
875 auto downsampleAllData = options.check(
"downsampleAllData", yarp::os::Value(
true)).asBool();
+
876 auto leafSize = options.check(
"leafSize", yarp::os::Value(0.0f)).asFloat32();
+
877 auto leafSizeX = options.check(
"leafSizeX", yarp::os::Value(leafSize)).asFloat32();
+
878 auto leafSizeY = options.check(
"leafSizeY", yarp::os::Value(leafSize)).asFloat32();
+
879 auto leafSizeZ = options.check(
"leafSizeZ", yarp::os::Value(leafSize)).asFloat32();
+
880 auto limitMax = options.check(
"limitMax", yarp::os::Value(std::numeric_limits<float>::max())).asFloat64();
+
881 auto limitMin = options.check(
"limitMin", yarp::os::Value(-std::numeric_limits<float>::max())).asFloat64();
+
882 auto limitsNegative = options.check(
"limitsNegative", yarp::os::Value(
false)).asBool();
+
883 auto minimumPointsNumberPerVoxel = options.check(
"minimumPointsNumberPerVoxel", yarp::os::Value(0)).asInt32();
+
+
885 pcl::VoxelGrid<T> grid;
+
886 grid.setDownsampleAllData(downsampleAllData);
+
887 grid.setFilterLimits(limitMin, limitMax);
+
888 grid.setFilterLimitsNegative(limitsNegative);
+
889 grid.setInputCloud(in);
+
890 grid.setLeafSize(leafSizeX, leafSizeY, leafSizeZ);
+
891 grid.setMinimumPointsNumberPerVoxel(minimumPointsNumberPerVoxel);
+
892 grid.setSaveLeafLayout(
false);
+
+
+
895 checkOutput<T>(out,
"VoxelGrid");
+
+
+
+
+
+