TEST (PCL, Organized)
{
//construct dataset
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_organized (new pcl::PointCloud<pcl::PointXYZ> ());
cloud_organized->width = 5;
cloud_organized->height = 10;
cloud_organized->points.resize (cloud_organized->width * cloud_organized->height);
int npoints = 0;
for (size_t i = 0; i < cloud_organized->height; i++)
{
for (size_t j = 0; j < cloud_organized->width; j++)
{
cloud_organized->points[npoints].x = static_cast<float> (i);
cloud_organized->points[npoints].y = static_cast<float> (j);
cloud_organized->points[npoints].z = static_cast<float> (cloud_organized->points.size ()); // to avoid shadowing
npoints++;
}
}
int nan_idx = cloud_organized->width*cloud_organized->height - 2*cloud_organized->width + 1;
cloud_organized->points[nan_idx].x = numeric_limits<float>::quiet_NaN ();
cloud_organized->points[nan_idx].y = numeric_limits<float>::quiet_NaN ();
cloud_organized->points[nan_idx].z = numeric_limits<float>::quiet_NaN ();
// Init objects
PolygonMesh triangles;
OrganizedFastMesh<PointXYZ> ofm;
// Set parameters
ofm.setInputCloud (cloud_organized);
ofm.setMaxEdgeLength (1.5);
ofm.setTrianglePixelSize (1);
ofm.setTriangulationType (OrganizedFastMesh<PointXYZ>::TRIANGLE_ADAPTIVE_CUT);
// Reconstruct
ofm.reconstruct (triangles);
//saveVTKFile ("./test/organized.vtk", triangles);
// Check triangles
EXPECT_EQ (triangles.cloud.width, cloud_organized->width);
EXPECT_EQ (triangles.cloud.height, cloud_organized->height);
EXPECT_EQ (int (triangles.polygons.size ()), 2*(triangles.cloud.width-1)*(triangles.cloud.height-1) - 4);
EXPECT_EQ (int (triangles.polygons.at (0).vertices.size ()), 3);
EXPECT_EQ (int (triangles.polygons.at (0).vertices.at (0)), 0);
EXPECT_EQ (int (triangles.polygons.at (0).vertices.at (1)), triangles.cloud.width+1);
EXPECT_EQ (int (triangles.polygons.at (0).vertices.at (2)), 1);
}
void ImageTriangulation::perform()
{
if (myPoints.size() != width * height)
throw Base::RuntimeError("Number of points doesn't match with given width and height");
//construct dataset
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_organized (new pcl::PointCloud<pcl::PointXYZ> ());
cloud_organized->width = width;
cloud_organized->height = height;
cloud_organized->points.resize (cloud_organized->width * cloud_organized->height);
const std::vector<Base::Vector3f>& points = myPoints.getBasicPoints();
int npoints = 0;
for (size_t i = 0; i < cloud_organized->height; i++) {
for (size_t j = 0; j < cloud_organized->width; j++) {
const Base::Vector3f& p = points[npoints];
cloud_organized->points[npoints].x = p.x;
cloud_organized->points[npoints].y = p.y;
cloud_organized->points[npoints].z = p.z;
npoints++;
}
}
OrganizedFastMesh<PointXYZ> ofm;
// Set parameters
ofm.setInputCloud (cloud_organized);
// This parameter is not yet implmented (pcl 1.7)
ofm.setMaxEdgeLength (1.5);
ofm.setTrianglePixelSize (1);
ofm.setTriangulationType (OrganizedFastMesh<PointXYZ>::TRIANGLE_ADAPTIVE_CUT);
ofm.storeShadowedFaces(true);
// Reconstruct
PolygonMesh mesh;
ofm.reconstruct (mesh);
MeshConversion::convert(mesh, myMesh);
// remove invalid points
//
MeshCore::MeshKernel& kernel = myMesh.getKernel();
const MeshCore::MeshFacetArray& face = kernel.GetFacets();
MeshCore::MeshAlgorithm meshAlg(kernel);
meshAlg.SetPointFlag(MeshCore::MeshPoint::INVALID);
std::vector<unsigned long> validPoints;
validPoints.reserve(face.size()*3);
for (MeshCore::MeshFacetArray::_TConstIterator it = face.begin(); it != face.end(); ++it) {
validPoints.push_back(it->_aulPoints[0]);
validPoints.push_back(it->_aulPoints[1]);
validPoints.push_back(it->_aulPoints[2]);
}
// remove duplicates
std::sort(validPoints.begin(), validPoints.end());
validPoints.erase(std::unique(validPoints.begin(), validPoints.end()), validPoints.end());
meshAlg.ResetPointsFlag(validPoints, MeshCore::MeshPoint::INVALID);
unsigned long countInvalid = meshAlg.CountPointFlag(MeshCore::MeshPoint::INVALID);
if (countInvalid > 0) {
std::vector<unsigned long> invalidPoints;
invalidPoints.reserve(countInvalid);
meshAlg.GetPointsFlag(invalidPoints, MeshCore::MeshPoint::INVALID);
kernel.DeletePoints(invalidPoints);
}
}
