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;
}
void
compute (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output,
double search_radius, bool sqr_gauss_param_set, double sqr_gauss_param,
bool use_polynomial_fit, int polynomial_order)
{
PointCloud<PointXYZ>::Ptr xyz_cloud_pre (new pcl::PointCloud<PointXYZ> ()),
xyz_cloud (new pcl::PointCloud<PointXYZ> ());
fromROSMsg (*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 = xyz_cloud->size ();
xyz_cloud->is_dense = false;
// io::savePCDFile ("test.pcd", *xyz_cloud);
PointCloud<PointXYZ>::Ptr xyz_cloud_smoothed (new PointCloud<PointXYZ> ());
MovingLeastSquares<PointXYZ, Normal> mls;
mls.setInputCloud (xyz_cloud);
mls.setSearchRadius (search_radius);
if (sqr_gauss_param_set) mls.setSqrGaussParam (sqr_gauss_param);
mls.setPolynomialFit (use_polynomial_fit);
mls.setPolynomialOrder (polynomial_order);
// mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, Normal>::SAMPLE_LOCAL_PLANE);
mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, Normal>::UNIFORM_DENSITY);
// mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, Normal>::FILL_HOLES);
// mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, Normal>::NONE);
mls.setFillingStepSize (0.02);
mls.setPointDensity (50000*search_radius); // 300 points in a 5 cm radius
mls.setUpsamplingRadius (0.025);
mls.setUpsamplingStepSize (0.015);
search::KdTree<PointXYZ>::Ptr tree (new search::KdTree<PointXYZ> ());
mls.setSearchMethod (tree);
PointCloud<Normal>::Ptr mls_normals (new PointCloud<Normal> ());
mls.setOutputNormals (mls_normals);
PCL_INFO ("Computing smoothed surface and normals with search_radius %f , sqr_gaussian_param %f, polynomial fitting %d, polynomial order %d\n",
mls.getSearchRadius(), mls.getSqrGaussParam(), mls.getPolynomialFit(), mls.getPolynomialOrder());
TicToc tt;
tt.tic ();
mls.reconstruct (*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");
sensor_msgs::PointCloud2 output_positions, output_normals;
// printf ("sizes: %d %d %d\n", xyz_cloud_smoothed->width, xyz_cloud_smoothed->height, xyz_cloud_smoothed->size ());
toROSMsg (*xyz_cloud_smoothed, output_positions);
toROSMsg (*mls_normals, output_normals);
concatenateFields (output_positions, output_normals, output);
PointCloud<PointXYZ> xyz_vg;
VoxelGrid<PointXYZ> vg;
vg.setInputCloud (xyz_cloud_smoothed);
vg.setLeafSize (0.005, 0.005, 0.005);
vg.filter (xyz_vg);
sensor_msgs::PointCloud2 xyz_vg_2;
toROSMsg (xyz_vg, xyz_vg_2);
pcl::io::savePCDFile ("cloud_vg.pcd", xyz_vg_2, Eigen::Vector4f::Zero (),
Eigen::Quaternionf::Identity (), true);
}
