void FSModel::convertPointCloudToSurfaceMesh3()
{
/*pcl::MovingLeastSquares<PointXYZRGB, PointXYZ> mls;
mls.setInputCloud (pointCloud);
mls.setSearchRadius (0.01);
mls.setPolynomialFit (true);
mls.setPolynomialOrder (2);
mls.setUpsamplingMethod (pcl::MovingLeastSquares<PointXYZRGB, PointXYZ>::SAMPLE_LOCAL_PLANE);
mls.setUpsamplingRadius (0.005);
mls.setUpsamplingStepSize (0.003);
pcl::PointCloud<PointXYZ>::Ptr cloud_smoothed (new pcl::PointCloud<PointXYZ> ());
mls.process (*cloud_smoothed);*/
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
cloud->points.resize(pointCloud->size());
for (size_t i = 0; i < pointCloud->points.size(); i++) {
cloud->points[i].x = pointCloud->points[i].x;
cloud->points[i].y = pointCloud->points[i].y;
cloud->points[i].z = pointCloud->points[i].z;
}
pcl::NormalEstimation<pcl::PointXYZ, Normal> ne;
//ne.setNumberOfThreads(8);
ne.setInputCloud (cloud);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
ne.setSearchMethod (tree);
ne.setRadiusSearch (0.01);
Eigen::Vector4f centroid;
compute3DCentroid (*cloud, centroid);
ne.setViewPoint (centroid[0], centroid[1], centroid[2]);
qDebug() << "Centroid:"<<centroid[0] <<centroid[1]<<centroid[2];
PointCloud<Normal>::Ptr cloud_normals (new PointCloud<Normal> ());
ne.compute (*cloud_normals);
//invert all normals, primarly they all point to the centroid
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, *cloud_normals, *cloud_smoothed_normals);
//concatenateFields (*cloud_smoothed, *cloud_normals, *cloud_smoothed_normals);*/
Poisson<PointNormal> poisson;
poisson.setScale(1.0);
poisson.setDepth (9);
poisson.setDegree(2);
poisson.setSamplesPerNode(3);
poisson.setIsoDivide(8);
poisson.setConfidence(0);
poisson.setManifold(0);
poisson.setOutputPolygons(0);
poisson.setSolverDivide(8);
poisson.setInputCloud(cloud_smoothed_normals);
poisson.reconstruct(surfaceMeshPoisson);
//pcl::io::savePLYFile("meshPoisson.ply", surfaceMeshPoisson);
FSController::getInstance()->meshComputed=true;
}
void PoissonReconstruction::perform(int ksearch)
{
PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ>);
PointCloud<PointNormal>::Ptr cloud_with_normals (new PointCloud<PointNormal>);
search::KdTree<PointXYZ>::Ptr tree;
search::KdTree<PointNormal>::Ptr tree2;
cloud->reserve(myPoints.size());
for (Points::PointKernel::const_iterator it = myPoints.begin(); it != myPoints.end(); ++it) {
if (!boost::math::isnan(it->x) && !boost::math::isnan(it->y) && !boost::math::isnan(it->z))
cloud->push_back(PointXYZ(it->x, it->y, it->z));
}
// Create search tree
tree.reset (new search::KdTree<PointXYZ> (false));
tree->setInputCloud (cloud);
// Normal estimation
NormalEstimation<PointXYZ, Normal> n;
PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
n.setInputCloud (cloud);
//n.setIndices (indices[B);
n.setSearchMethod (tree);
n.setKSearch (ksearch);
n.compute (*normals);
// Concatenate XYZ and normal information
pcl::concatenateFields (*cloud, *normals, *cloud_with_normals);
// Create search tree
tree2.reset (new search::KdTree<PointNormal>);
tree2->setInputCloud (cloud_with_normals);
// Init objects
Poisson<PointNormal> poisson;
// Set parameters
poisson.setInputCloud (cloud_with_normals);
poisson.setSearchMethod (tree2);
if (depth >= 1)
poisson.setDepth(depth);
if (solverDivide >= 1)
poisson.setSolverDivide(solverDivide);
if (samplesPerNode >= 1.0f)
poisson.setSamplesPerNode(samplesPerNode);
// Reconstruct
PolygonMesh mesh;
poisson.reconstruct (mesh);
MeshConversion::convert(mesh, myMesh);
}
void PoissonReconstruction::perform(const std::vector<Base::Vector3f>& normals)
{
if (myPoints.size() != normals.size())
throw Base::RuntimeError("Number of points doesn't match with number of normals");
PointCloud<PointNormal>::Ptr cloud_with_normals (new PointCloud<PointNormal>);
search::KdTree<PointNormal>::Ptr tree;
cloud_with_normals->reserve(myPoints.size());
std::size_t num_points = myPoints.size();
const std::vector<Base::Vector3f>& points = myPoints.getBasicPoints();
for (std::size_t index=0; index<num_points; index++) {
const Base::Vector3f& p = points[index];
const Base::Vector3f& n = normals[index];
if (!boost::math::isnan(p.x) && !boost::math::isnan(p.y) && !boost::math::isnan(p.z)) {
PointNormal pn;
pn.x = p.x;
pn.y = p.y;
pn.z = p.z;
pn.normal_x = n.x;
pn.normal_y = n.y;
pn.normal_z = n.z;
cloud_with_normals->push_back(pn);
}
}
// Create search tree
tree.reset (new search::KdTree<PointNormal>);
tree->setInputCloud (cloud_with_normals);
// Init objects
Poisson<PointNormal> poisson;
// Set parameters
poisson.setInputCloud (cloud_with_normals);
poisson.setSearchMethod (tree);
if (depth >= 1)
poisson.setDepth(depth);
if (solverDivide >= 1)
poisson.setSolverDivide(solverDivide);
if (samplesPerNode >= 1.0f)
poisson.setSamplesPerNode(samplesPerNode);
// Reconstruct
PolygonMesh mesh;
poisson.reconstruct (mesh);
MeshConversion::convert(mesh, myMesh);
}
void
compute (const sensor_msgs::PointCloud2::ConstPtr &input, PolygonMesh &output,
int depth, int solver_divide, int iso_divide)
{
PointCloud<PointNormal>::Ptr xyz_cloud (new pcl::PointCloud<PointNormal> ());
fromROSMsg (*input, *xyz_cloud);
Poisson<PointNormal> poisson;
poisson.setDepth (depth);
poisson.setSolverDivide (solver_divide);
poisson.setIsoDivide (iso_divide);
poisson.setInputCloud (xyz_cloud);
TicToc tt;
tt.tic ();
print_highlight ("Computing ");
poisson.reconstruct (output);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms]\n");
}