| 82 | } |
| 83 | |
| 84 | void |
| 85 | compute (const pcl::PCLPointCloud2::ConstPtr &input, |
| 86 | pcl::PCLPointCloud2 &output, |
| 87 | double search_radius, |
| 88 | bool sqr_gauss_param_set, |
| 89 | double sqr_gauss_param, |
| 90 | int polynomial_order) |
| 91 | { |
| 92 | |
| 93 | PointCloud<PointXYZ>::Ptr xyz_cloud_pre (new pcl::PointCloud<PointXYZ> ()), |
| 94 | xyz_cloud (new pcl::PointCloud<PointXYZ> ()); |
| 95 | fromPCLPointCloud2 (*input, *xyz_cloud_pre); |
| 96 | |
| 97 | // Filter the NaNs from the cloud |
| 98 | for (std::size_t i = 0; i < xyz_cloud_pre->size (); ++i) |
| 99 | if (std::isfinite ((*xyz_cloud_pre)[i].x)) |
| 100 | xyz_cloud->push_back ((*xyz_cloud_pre)[i]); |
| 101 | xyz_cloud->header = xyz_cloud_pre->header; |
| 102 | xyz_cloud->height = 1; |
| 103 | xyz_cloud->width = xyz_cloud->size (); |
| 104 | xyz_cloud->is_dense = false; |
| 105 | |
| 106 | |
| 107 | |
| 108 | PointCloud<PointNormal>::Ptr xyz_cloud_smoothed (new PointCloud<PointNormal> ()); |
| 109 | |
| 110 | MovingLeastSquares<PointXYZ, PointNormal> mls; |
| 111 | mls.setInputCloud (xyz_cloud); |
| 112 | mls.setSearchRadius (search_radius); |
| 113 | if (sqr_gauss_param_set) mls.setSqrGaussParam (sqr_gauss_param); |
| 114 | mls.setPolynomialOrder (polynomial_order); |
| 115 | |
| 116 | // mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, PointNormal>::SAMPLE_LOCAL_PLANE); |
| 117 | // mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, PointNormal>::RANDOM_UNIFORM_DENSITY); |
| 118 | // mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, PointNormal>::VOXEL_GRID_DILATION); |
| 119 | mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, PointNormal>::NONE); |
| 120 | mls.setPointDensity (60000 * static_cast<int>(search_radius)); // 300 points in a 5 cm radius |
| 121 | mls.setUpsamplingRadius (0.025); |
| 122 | mls.setUpsamplingStepSize (0.015); |
| 123 | mls.setDilationIterations (2); |
| 124 | mls.setDilationVoxelSize (0.01f); |
| 125 | |
| 126 | search::KdTree<PointXYZ>::Ptr tree (new search::KdTree<PointXYZ> ()); |
| 127 | mls.setSearchMethod (tree); |
| 128 | mls.setComputeNormals (true); |
| 129 | |
| 130 | PCL_INFO ("Computing smoothed surface and normals with search_radius %f , sqr_gaussian_param %f, polynomial order %d\n", |
| 131 | mls.getSearchRadius(), mls.getSqrGaussParam(), mls.getPolynomialOrder()); |
| 132 | TicToc tt; |
| 133 | tt.tic (); |
| 134 | mls.process (*xyz_cloud_smoothed); |
| 135 | print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", xyz_cloud_smoothed->width * xyz_cloud_smoothed->height); print_info (" points]\n"); |
| 136 | |
| 137 | toPCLPointCloud2 (*xyz_cloud_smoothed, output); |
| 138 | } |
| 139 | |
| 140 | void |
| 141 | saveCloud (const std::string &filename, const pcl::PCLPointCloud2 &output) |
no test coverage detected