void reconstructMesh(const PointCloud<PointXYZ>::ConstPtr &cloud, PointCloud<PointXYZ> &output_cloud, std::vector<Vertices> &triangles) { boost::shared_ptr<std::vector<int> > indices(new std::vector<int>); indices->resize(cloud->points.size ()); for (size_t i = 0; i < indices->size (); ++i) { (*indices)[i] = i; } pcl::search::KdTree<PointXYZ>::Ptr tree(new pcl::search::KdTree<PointXYZ>); tree->setInputCloud(cloud); PointCloud<PointXYZ>::Ptr mls_points(new PointCloud<PointXYZ>); PointCloud<PointNormal>::Ptr mls_normals(new PointCloud<PointNormal>); MovingLeastSquares<PointXYZ, PointNormal> mls; mls.setInputCloud(cloud); mls.setIndices(indices); mls.setPolynomialFit(true); mls.setSearchMethod(tree); mls.setSearchRadius(0.03); mls.process(*mls_normals); ConvexHull<PointXYZ> ch; ch.setInputCloud(mls_points); ch.reconstruct(output_cloud, triangles); }
void compute (const pcl::PCLPointCloud2::ConstPtr &input, pcl::PCLPointCloud2 &output, double search_radius, bool sqr_gauss_param_set, double sqr_gauss_param, int polynomial_order) { PointCloud<PointXYZ>::Ptr xyz_cloud_pre (new pcl::PointCloud<PointXYZ> ()), xyz_cloud (new pcl::PointCloud<PointXYZ> ()); fromPCLPointCloud2 (*input, *xyz_cloud_pre); // Filter the NaNs from the cloud for (size_t i = 0; i < xyz_cloud_pre->size (); ++i) if (pcl_isfinite (xyz_cloud_pre->points[i].x)) xyz_cloud->push_back (xyz_cloud_pre->points[i]); xyz_cloud->header = xyz_cloud_pre->header; xyz_cloud->height = 1; xyz_cloud->width = static_cast<uint32_t> (xyz_cloud->size ()); xyz_cloud->is_dense = false; PointCloud<PointNormal>::Ptr xyz_cloud_smoothed (new PointCloud<PointNormal> ()); MovingLeastSquares<PointXYZ, PointNormal> mls; mls.setInputCloud (xyz_cloud); mls.setSearchRadius (search_radius); if (sqr_gauss_param_set) mls.setSqrGaussParam (sqr_gauss_param); mls.setPolynomialOrder (polynomial_order); // mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, PointNormal>::SAMPLE_LOCAL_PLANE); // mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, PointNormal>::RANDOM_UNIFORM_DENSITY); // mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, PointNormal>::VOXEL_GRID_DILATION); mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, PointNormal>::NONE); mls.setPointDensity (60000 * int (search_radius)); // 300 points in a 5 cm radius mls.setUpsamplingRadius (0.025); mls.setUpsamplingStepSize (0.015); mls.setDilationIterations (2); mls.setDilationVoxelSize (0.01f); search::KdTree<PointXYZ>::Ptr tree (new search::KdTree<PointXYZ> ()); mls.setSearchMethod (tree); mls.setComputeNormals (true); PCL_INFO ("Computing smoothed surface and normals with search_radius %f , sqr_gaussian_param %f, polynomial order %d\n", mls.getSearchRadius(), mls.getSqrGaussParam(), mls.getPolynomialOrder()); TicToc tt; tt.tic (); mls.process (*xyz_cloud_smoothed); 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"); toPCLPointCloud2 (*xyz_cloud_smoothed, output); }
int main (int argc, char **argv) { /* if (argc != 1) { PCL_ERROR ("Syntax: %s input.pcd output.ply\n", argv[0]); return -1; }*/ PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ> ()); io::loadPCDFile (argv[1], *cloud); MovingLeastSquares<PointXYZ, PointXYZ> mls; mls.setInputCloud (cloud); mls.setSearchRadius (4); mls.setPolynomialFit (true); mls.setPolynomialOrder (1); mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, PointXYZ>::SAMPLE_LOCAL_PLANE); mls.setUpsamplingRadius (1); mls.setUpsamplingStepSize (0.03); PointCloud<PointXYZ>::Ptr cloud_smoothed (new PointCloud<PointXYZ> ()); mls.process (*cloud_smoothed); NormalEstimationOMP<PointXYZ, Normal> ne; ne.setNumberOfThreads (8); ne.setInputCloud (cloud_smoothed); ne.setRadiusSearch (0.8); Eigen::Vector4f centroid; compute3DCentroid (*cloud_smoothed, centroid); ne.setViewPoint (centroid[0], centroid[1], centroid[2]); PointCloud<Normal>::Ptr cloud_normals (new PointCloud<Normal> ()); ne.compute (*cloud_normals); for (size_t i = 0; i < cloud_normals->size (); ++i) { cloud_normals->points[i].normal_x *= -1; cloud_normals->points[i].normal_y *= -1; cloud_normals->points[i].normal_z *= -1; } PointCloud<PointNormal>::Ptr cloud_smoothed_normals (new PointCloud<PointNormal> ()); concatenateFields (*cloud_smoothed, *cloud_normals, *cloud_smoothed_normals); Poisson<pcl::PointNormal> poisson; poisson.setDepth (9); poisson.setInputCloud (cloud_smoothed_normals); PolygonMesh mesh_poisson; poisson.reconstruct (mesh_poisson); pcl::io::savePLYFile("mesh.ply", mesh_poisson); return 0; }
static void computeNormals(PointCloudPtr pcl_cloud_input, pcl::PointCloud<PointNormal>::Ptr pcl_cloud_normals, const double search_radius) { typedef pcl::search::KdTree<PointT> KdTree; typedef typename KdTree::Ptr KdTreePtr; // Create a KD-Tree KdTreePtr tree = boost::make_shared<pcl::search::KdTree<PointT> >(); MovingLeastSquares<PointT, PointNormal> mls; mls.setComputeNormals(true); mls.setInputCloud(pcl_cloud_input); //mls.setPolynomialFit(true); mls.setSearchMethod(tree); mls.setSearchRadius(search_radius); mls.process(*pcl_cloud_normals); }
void RScloud::smooth_cloud() { //MLS SMOOTHING MovingLeastSquares<pointT, pointT> mls; mls.setInputCloud (pointcloud); mls.setSearchRadius (0.004); mls.setPolynomialFit (true); mls.setPolynomialOrder (2); PointCloud<pointT>::Ptr cloud_smoothed (new PointCloud<pointT> ()); mls.process (*cloud_smoothed); //remove nans for(int i=0;i<cloud_smoothed->points.size();i++){ if (!(pcl::isFinite(cloud_smoothed->at(i)))){ cloud_smoothed->at(i).x = 0.0; cloud_smoothed->at(i).y = 0.0; cloud_smoothed->at(i).z = 0.0; } } pointcloud = cloud_smoothed; }