From a0ac04febce90546b1e02fb0aa030b5b7bdd0c3e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Wed, 16 Dec 2020 00:01:35 +0100 Subject: [PATCH] Introduce transformPointCloud(WithNormals) --- libraries/YarpCloudUtils/README.md | 72 ++++++++++--------- .../YarpCloudUtils-pcl-impl.hpp | 52 ++++++++++++++ .../YarpCloudUtils/YarpCloudUtils-pcl.cpp | 6 ++ 3 files changed, 95 insertions(+), 35 deletions(-) diff --git a/libraries/YarpCloudUtils/README.md b/libraries/YarpCloudUtils/README.md index fafbcb3e..45ec97ba 100644 --- a/libraries/YarpCloudUtils/README.md +++ b/libraries/YarpCloudUtils/README.md @@ -28,41 +28,43 @@ Both `roboticslab::YarpCloudUtils::meshFromCloud` and `roboticslab::YarpCloudUti implement a set of [Point Cloud Library (PCL)](https://pointclouds.org/) algorithms. A variety of PCL classes are exposed in a highly configurable pipeline: -| PCL class | usage | point types | notes | -|-------------------------------------------------------------------------------------------------------------------------------------------------------|------------------------|---------------------------|------------------------------| -| [`pcl::ApproximateVoxelGrid`](https://pointclouds.org/documentation/classpcl_1_1_approximate_voxel_grid.html) | cloud resampling | any | | -| [`pcl::BilateralFilter`](https://pointclouds.org/documentation/classpcl_1_1_bilateral_filter.html) | cloud filtering | XYZI(Normal) | | -| [`pcl::BilateralUpsampling`](https://pointclouds.org/documentation/classpcl_1_1_bilateral_upsampling.html) | cloud processing | XYZRGBA | organized clouds only | -| [`pcl::ConcaveHull`](https://pointclouds.org/documentation/classpcl_1_1_concave_hull.html) | mesh construction | any | | -| [`pcl::ConvexHull`](https://pointclouds.org/documentation/classpcl_1_1_convex_hull.html) | mesh construction | any | | -| [`pcl::CropBox`](https://pointclouds.org/documentation/classpcl_1_1_crop_box.html) | cloud filtering | any | | -| [`pcl::FastBilateralFilter`](https://pointclouds.org/documentation/classpcl_1_1_fast_bilateral_filter.html) | cloud filtering | XYZ(RGBA) | organized clouds only | -| [`pcl::FastBilateralFilterOMP`](https://pointclouds.org/documentation/classpcl_1_1_fast_bilateral_filter_o_m_p.html) | cloud filtering | XYZ(RGBA) | organized clouds only | -| [`pcl::GreedyProjectionTriangulation`](https://pointclouds.org/documentation/classpcl_1_1_greedy_projection_triangulation.html) | mesh construction | XYZ/XYZI/XYZRGBA + Normal | | -| [`pcl::GridMinimum`](https://pointclouds.org/documentation/classpcl_1_1_grid_minimum.html) | cloud resampling | any | | -| [ `pcl::GridProjection` ](https://pointclouds.org/documentation/classpcl_1_1_grid_projection.html) | surface reconstruction | XYZ/XYZI/XYZRGBA + Normal | | -| [`pcl::LocalMaximum`](https://pointclouds.org/documentation/classpcl_1_1_local_maximum.html) | cloud resampling | any | | -| [`pcl::MarchingCubesHoppe`](https://pointclouds.org/documentation/classpcl_1_1_marching_cubes_hoppe.html) | surface reconstruction | XYZ/XYZI/XYZRGBA + Normal | | -| [`pcl::MarchingCubesRBF`](https://pointclouds.org/documentation/classpcl_1_1_marching_cubes_r_b_f.html) | surface reconstruction | XYZ/XYZI/XYZRGBA + Normal | | -| [`pcl::MedianFilter`](https://pointclouds.org/documentation/classpcl_1_1_median_filter.html) | cloud filtering | any | organized clouds only | -| [`pcl::MeshQuadricDecimationVTK`](https://pointclouds.org/documentation/classpcl_1_1_mesh_quadric_decimation_v_t_k.html) | mesh processing | (mesh) | | -| [`pcl::MeshSmoothingLaplacianVTK`](https://pointclouds.org/documentation/classpcl_1_1_mesh_smoothing_laplacian_v_t_k.html) | mesh processing | (mesh) | | -| [`pcl::MeshSmoothingWindowedSincVTK`](https://pointclouds.org/documentation/classpcl_1_1_mesh_smoothing_windowed_sinc_v_t_k.html) | mesh processing | (mesh) | | -| [`pcl::MeshSubdivisionVTK`](https://pointclouds.org/documentation/classpcl_1_1_mesh_subdivision_v_t_k.html) | mesh processing | (mesh) | | -| [`pcl::MovingLeastSquares`](https://pointclouds.org/documentation/classpcl_1_1_moving_least_squares.html) | cloud processing | any | requires PCL 1.9+ | -| [`pcl::NormalEstimation`](https://pointclouds.org/documentation/classpcl_1_1_normal_estimation.html) | normal estimation | any | | -| [`pcl::NormalEstimationOMP`](https://pointclouds.org/documentation/classpcl_1_1_normal_estimation_o_m_p.html) | normal estimation | any | | -| [`pcl::OrganizedFastMesh`](https://pointclouds.org/documentation/classpcl_1_1_organized_fast_mesh.html) | mesh construction | XYZ(RGBA) | organized clouds only | -| [`pcl::PassThrough`](https://pointclouds.org/documentation/classpcl_1_1_pass_through.html) | cloud filtering | any | | -| [`pcl::Poisson`](https://pointclouds.org/documentation/classpcl_1_1_poisson.html) | surface reconstruction | XYZ/XYZI/XYZRGBA + Normal | `threads` requires PCL 1.12+ | -| [`pcl::RadiusOutlierRemoval`](https://pointclouds.org/documentation/classpcl_1_1_radius_outlier_removal.html) | cloud filtering | any | | -| [`pcl::RandomSample`](https://pointclouds.org/documentation/classpcl_1_1_random_sample.html) | cloud resampling | any | | -| [`pcl::SamplingSurfaceNormal`](https://pointclouds.org/documentation/classpcl_1_1_sampling_surface_normal.html) | cloud resampling | XYZ/XYZI/XYZRGBA + Normal | | -| [`pcl::ShadowPoints`](https://pointclouds.org/documentation/classpcl_1_1_shadow_points.html) | cloud filtering | any normal type | only normal types | -| [`pcl::SimplificationRemoveUnusedVertices`](https://pointclouds.org/documentation/classpcl_1_1surface_1_1_simplification_remove_unused_vertices.html) | mesh simplification | (mesh) | | -| [`pcl::StatisticalOutlierRemoval`](https://pointclouds.org/documentation/classpcl_1_1_statistical_outlier_removal.html) | cloud filtering | any | | -| [`pcl::UniformSampling`](https://pointclouds.org/documentation/classpcl_1_1_uniform_sampling.html) | cloud resampling | any | | -| [`pcl::VoxelGrid`](https://pointclouds.org/documentation/classpcl_1_1_voxel_grid.html) | cloud resampling | any | | +| PCL class | usage | point types | notes | +|-------------------------------------------------------------------------------------------------------------------------------------------------------|------------------------|---------------------------|--------------------------------------------------------------------| +| [`pcl::transformPointCloud`](https://pointclouds.org/documentation/group__common.html#gac841d05d13c925f3a3a8090d9d7ff24d) | affine transformation | any | `translation` (meters) and `rotation` (scaled axis-angle, radians) | +| [`pcl::transformPointCloudWithNormals`](https://pointclouds.org/documentation/group__common.html#ga01dcf9e24dec3109a0c8a8b8f2e24bcc) | affine transformation | any normal type | see above | +| [`pcl::ApproximateVoxelGrid`](https://pointclouds.org/documentation/classpcl_1_1_approximate_voxel_grid.html) | cloud resampling | any | | +| [`pcl::BilateralFilter`](https://pointclouds.org/documentation/classpcl_1_1_bilateral_filter.html) | cloud filtering | XYZI(Normal) | | +| [`pcl::BilateralUpsampling`](https://pointclouds.org/documentation/classpcl_1_1_bilateral_upsampling.html) | cloud processing | XYZRGBA | organized clouds only | +| [`pcl::ConcaveHull`](https://pointclouds.org/documentation/classpcl_1_1_concave_hull.html) | mesh construction | any | | +| [`pcl::ConvexHull`](https://pointclouds.org/documentation/classpcl_1_1_convex_hull.html) | mesh construction | any | | +| [`pcl::CropBox`](https://pointclouds.org/documentation/classpcl_1_1_crop_box.html) | cloud filtering | any | | +| [`pcl::FastBilateralFilter`](https://pointclouds.org/documentation/classpcl_1_1_fast_bilateral_filter.html) | cloud filtering | XYZ(RGBA) | organized clouds only | +| [`pcl::FastBilateralFilterOMP`](https://pointclouds.org/documentation/classpcl_1_1_fast_bilateral_filter_o_m_p.html) | cloud filtering | XYZ(RGBA) | organized clouds only | +| [`pcl::GreedyProjectionTriangulation`](https://pointclouds.org/documentation/classpcl_1_1_greedy_projection_triangulation.html) | mesh construction | XYZ/XYZI/XYZRGBA + Normal | | +| [`pcl::GridMinimum`](https://pointclouds.org/documentation/classpcl_1_1_grid_minimum.html) | cloud resampling | any | | +| [ `pcl::GridProjection` ](https://pointclouds.org/documentation/classpcl_1_1_grid_projection.html) | surface reconstruction | XYZ/XYZI/XYZRGBA + Normal | | +| [`pcl::LocalMaximum`](https://pointclouds.org/documentation/classpcl_1_1_local_maximum.html) | cloud resampling | any | | +| [`pcl::MarchingCubesHoppe`](https://pointclouds.org/documentation/classpcl_1_1_marching_cubes_hoppe.html) | surface reconstruction | XYZ/XYZI/XYZRGBA + Normal | | +| [`pcl::MarchingCubesRBF`](https://pointclouds.org/documentation/classpcl_1_1_marching_cubes_r_b_f.html) | surface reconstruction | XYZ/XYZI/XYZRGBA + Normal | | +| [`pcl::MedianFilter`](https://pointclouds.org/documentation/classpcl_1_1_median_filter.html) | cloud filtering | any | organized clouds only | +| [`pcl::MeshQuadricDecimationVTK`](https://pointclouds.org/documentation/classpcl_1_1_mesh_quadric_decimation_v_t_k.html) | mesh processing | (mesh) | | +| [`pcl::MeshSmoothingLaplacianVTK`](https://pointclouds.org/documentation/classpcl_1_1_mesh_smoothing_laplacian_v_t_k.html) | mesh processing | (mesh) | | +| [`pcl::MeshSmoothingWindowedSincVTK`](https://pointclouds.org/documentation/classpcl_1_1_mesh_smoothing_windowed_sinc_v_t_k.html) | mesh processing | (mesh) | | +| [`pcl::MeshSubdivisionVTK`](https://pointclouds.org/documentation/classpcl_1_1_mesh_subdivision_v_t_k.html) | mesh processing | (mesh) | | +| [`pcl::MovingLeastSquares`](https://pointclouds.org/documentation/classpcl_1_1_moving_least_squares.html) | cloud processing | any | requires PCL 1.9+ | +| [`pcl::NormalEstimation`](https://pointclouds.org/documentation/classpcl_1_1_normal_estimation.html) | normal estimation | any | | +| [`pcl::NormalEstimationOMP`](https://pointclouds.org/documentation/classpcl_1_1_normal_estimation_o_m_p.html) | normal estimation | any | | +| [`pcl::OrganizedFastMesh`](https://pointclouds.org/documentation/classpcl_1_1_organized_fast_mesh.html) | mesh construction | XYZ(RGBA) | organized clouds only | +| [`pcl::PassThrough`](https://pointclouds.org/documentation/classpcl_1_1_pass_through.html) | cloud filtering | any | | +| [`pcl::Poisson`](https://pointclouds.org/documentation/classpcl_1_1_poisson.html) | surface reconstruction | XYZ/XYZI/XYZRGBA + Normal | `threads` requires PCL 1.12+ | +| [`pcl::RadiusOutlierRemoval`](https://pointclouds.org/documentation/classpcl_1_1_radius_outlier_removal.html) | cloud filtering | any | | +| [`pcl::RandomSample`](https://pointclouds.org/documentation/classpcl_1_1_random_sample.html) | cloud resampling | any | | +| [`pcl::SamplingSurfaceNormal`](https://pointclouds.org/documentation/classpcl_1_1_sampling_surface_normal.html) | cloud resampling | XYZ/XYZI/XYZRGBA + Normal | | +| [`pcl::ShadowPoints`](https://pointclouds.org/documentation/classpcl_1_1_shadow_points.html) | cloud filtering | any normal type | only normal types | +| [`pcl::SimplificationRemoveUnusedVertices`](https://pointclouds.org/documentation/classpcl_1_1surface_1_1_simplification_remove_unused_vertices.html) | mesh simplification | (mesh) | | +| [`pcl::StatisticalOutlierRemoval`](https://pointclouds.org/documentation/classpcl_1_1_statistical_outlier_removal.html) | cloud filtering | any | | +| [`pcl::UniformSampling`](https://pointclouds.org/documentation/classpcl_1_1_uniform_sampling.html) | cloud resampling | any | | +| [`pcl::VoxelGrid`](https://pointclouds.org/documentation/classpcl_1_1_voxel_grid.html) | cloud resampling | any | | Mesh construction methods preserve the original point cloud as the surface vertices and simply construct the mesh on top, while surface reconstruction changes the topology diff --git a/libraries/YarpCloudUtils/YarpCloudUtils-pcl-impl.hpp b/libraries/YarpCloudUtils/YarpCloudUtils-pcl-impl.hpp index 7ade92f9..926dd7e4 100644 --- a/libraries/YarpCloudUtils/YarpCloudUtils-pcl-impl.hpp +++ b/libraries/YarpCloudUtils/YarpCloudUtils-pcl-impl.hpp @@ -56,6 +56,42 @@ namespace { +auto getTransformation(const yarp::os::Searchable & options) +{ + auto transformation = Eigen::Transform::Identity(); + + const auto & translation = options.find("translation"); + + if (!translation.isNull()) + { + if (!translation.isList() || translation.asList()->size() != 3) + { + throw std::runtime_error("translation is not a list or size not equal to 3"); + } + + const auto * b = translation.asList(); + Eigen::Vector3d vector(b->get(0).asFloat64(), b->get(1).asFloat64(), b->get(2).asFloat64()); + transformation.translate(vector); + } + + const auto & rotation = options.find("rotation"); + + if (!rotation.isNull()) + { + if (!rotation.isList() || rotation.asList()->size() != 3) + { + throw std::runtime_error("rotation is not a list or size not equal to 3"); + } + + const auto * b = rotation.asList(); + Eigen::Vector3d axis(b->get(0).asFloat64(), b->get(1).asFloat64(), b->get(2).asFloat64()); + Eigen::AngleAxisd rot(axis.norm(), axis.normalized()); + transformation.rotate(rot); + } + + return transformation; +} + template void checkOutput(const typename pcl::PointCloud::ConstPtr & cloud, const std::string & caller) { @@ -73,6 +109,22 @@ inline void checkOutput(const pcl::PolygonMesh::ConstPtr & mesh, const std::stri } } +template +void doTransformPointCloud(const typename pcl::PointCloud::ConstPtr & in, const typename pcl::PointCloud::Ptr & out, const yarp::os::Searchable & options) +{ + auto transformation = getTransformation(options); + pcl::transformPointCloud(*in, *out, transformation); + checkOutput(out, "transformPointCloud"); +} + +template +void doTransformPointCloudWithNormals(const typename pcl::PointCloud::ConstPtr & in, const typename pcl::PointCloud::Ptr & out, const yarp::os::Searchable & options) +{ + auto transformation = getTransformation(options); + pcl::transformPointCloudWithNormals(*in, *out, transformation); + checkOutput(out, "transformPointCloudWithNormals"); +} + template void doApproximateVoxelGrid(const typename pcl::PointCloud::ConstPtr & in, const typename pcl::PointCloud::Ptr & out, const yarp::os::Searchable & options) { diff --git a/libraries/YarpCloudUtils/YarpCloudUtils-pcl.cpp b/libraries/YarpCloudUtils/YarpCloudUtils-pcl.cpp index e1dcee92..ef1de969 100644 --- a/libraries/YarpCloudUtils/YarpCloudUtils-pcl.cpp +++ b/libraries/YarpCloudUtils/YarpCloudUtils-pcl.cpp @@ -76,6 +76,12 @@ namespace switch (makeHash(algorithm)) { + case "transformPointCloud"_hash: + doTransformPointCloud(prev.getCloud(), curr.setCloud(), options); + break; + case "transformPointCloudWithNormals"_hash: + doTransformPointCloudWithNormals(prev.getCloud(), curr.setCloud(), options); + break; case "ApproximateVoxelGrid"_hash: doApproximateVoxelGrid(prev.getCloud(), curr.setCloud(), options); break;