void laserCloudSub(const sensor_msgs::PointCloud2ConstPtr &msg)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr color_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZ>::Ptr laser_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PCLPointCloud2 pcl_pc2;
pcl_conversions::toPCL(*msg, pcl_pc2);
pcl::fromPCLPointCloud2(pcl_pc2, *laser_cloud);
copyPointCloud(*laser_cloud, *color_cloud);
addCloud(color_cloud);
}
void mls(std::string fname, std::string outname) {
// Load input file into a PointCloud<T> with an appropriate type
sensor_msgs::PointCloud2 cloud_blob;
// run this from BodyScanner/build/surface_reconstructor/
pcl::io::loadPCDFile(fname, cloud_blob);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr color_cloud (new pcl::PointCloud<pcl::PointXYZRGB> ());
pcl::fromROSMsg (cloud_blob, *color_cloud);
// drop the colors
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::copyPointCloud(*color_cloud, *cloud);
// Create a KD-Tree
pcl::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::KdTreeFLANN<pcl::PointXYZ>);
tree->setInputCloud (cloud);
// Output has the same type as the input one, it will be only smoothed
pcl::PointCloud<pcl::PointXYZ> mls_points;
// Init object (second point type is for the normals, even if unused)
pcl::MovingLeastSquares<pcl::PointXYZ, pcl::Normal> mls;
// Optionally, a pointer to a cloud can be provided, to be set by MLS
pcl::PointCloud<pcl::Normal>::Ptr mls_normals (new pcl::PointCloud<pcl::Normal> ());
mls.setOutputNormals (mls_normals);
// Set parameters
mls.setInputCloud (cloud);
mls.setPolynomialFit (true);
mls.setSearchMethod (tree);
mls.setSearchRadius (0.03);
// Reconstruct
mls.reconstruct (mls_points);
// Concatenate fields for saving
pcl::PointCloud<pcl::PointNormal> mls_cloud;
pcl::concatenateFields (mls_points, *mls_normals, mls_cloud);
// Save output
pcl::io::savePCDFile(outname, mls_cloud);
}