void GridReconstruction::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
GridProjection<PointNormal> grid;
// Set parameters
grid.setResolution(0.005);
grid.setPaddingSize(3);
grid.setNearestNeighborNum(100);
grid.setMaxBinarySearchLevel(10);
grid.setInputCloud (cloud_with_normals);
grid.setSearchMethod (tree2);
// Reconstruct
PolygonMesh mesh;
grid.reconstruct (mesh);
MeshConversion::convert(mesh, myMesh);
}
void GridReconstruction::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
GridProjection<PointNormal> grid;
// Set parameters
grid.setResolution(0.005);
grid.setPaddingSize(3);
grid.setNearestNeighborNum(100);
grid.setMaxBinarySearchLevel(10);
grid.setInputCloud (cloud_with_normals);
grid.setSearchMethod (tree);
// Reconstruct
PolygonMesh mesh;
grid.reconstruct (mesh);
MeshConversion::convert(mesh, myMesh);
}
TEST (PCL, GridProjection)
{
// Init objects
PolygonMesh grid;
GridProjection<PointNormal> gp;
// Set parameters
gp.setInputCloud (cloud_with_normals);
gp.setSearchMethod (tree2);
gp.setResolution (0.005);
gp.setPaddingSize (3);
// Reconstruct
gp.reconstruct (grid);
//saveVTKFile ("./test/bun0-grid.vtk", grid);
EXPECT_GE (grid.cloud.width, 5180);
EXPECT_GE (int (grid.polygons.size ()), 1295);
EXPECT_EQ (int (grid.polygons.at (0).vertices.size ()), 4);
EXPECT_EQ (int (grid.polygons.at (0).vertices.at (0)), 0);
}
