diff --git a/surface/include/pcl/surface/gp3.h b/surface/include/pcl/surface/gp3.h index 27be42ccacc..1a8a736854f 100644 --- a/surface/include/pcl/surface/gp3.h +++ b/surface/include/pcl/surface/gp3.h @@ -513,6 +513,46 @@ namespace pcl } }; + /** \brief Calculate the area of a triangle mesh + * \param[in] cloud the point cloud to which the mesh is applied + * \param[in] triangleMesh a triangle mesh + * \return the mesh area + * \note example of use: + * pcl::GreedyProjectionTriangulation gp; + * ... + * gp.setInputCloud(cloud_with_normals) + * ... + * pcl::PolygonMesh pm; + * gp.reconstruct(pm); + * float functArea=computeTriangleMeshArea(cloud_with_normals, pm.polygons); + * \ingroup + * common + */ + + template + inline float + computeTriangleMeshArea(const shared_ptr>& cloud, + std::vector& triangleMesh) + { + double area = 0; + pcl::PointCloud& cl = (*cloud); + for (auto& triangle_ : triangleMesh) { + if (triangle_.vertices.size() == 3) { + const Eigen::Matrix P( + cl[triangle_.vertices[0]].x - (*cloud)[triangle_.vertices[2]].x, + cl[triangle_.vertices[0]].y - (*cloud)[triangle_.vertices[2]].y, + cl[triangle_.vertices[0]].z - (*cloud)[triangle_.vertices[2]].z); + const Eigen::Matrix Q( + cl[triangle_.vertices[1]].x - (*cloud)[triangle_.vertices[2]].x, + cl[triangle_.vertices[1]].y - (*cloud)[triangle_.vertices[2]].y, + cl[triangle_.vertices[1]].z - (*cloud)[triangle_.vertices[2]].z); + area += 0.5 * P.cross(Q).norm(); + } + } + return area; + } + + } // namespace pcl #ifdef PCL_NO_PRECOMPILE diff --git a/test/surface/test_gp3.cpp b/test/surface/test_gp3.cpp index d7eed86d891..c821cab5e00 100644 --- a/test/surface/test_gp3.cpp +++ b/test/surface/test_gp3.cpp @@ -238,6 +238,111 @@ TEST (PCL, UpdateMesh_With_TextureMapping) } } + + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +TEST(PCL, computeTriangleMeshArea) +{ + // Init objects + PolygonMesh triangles; + GreedyProjectionTriangulation gp3; + + // Set parameters + gp3.setInputCloud(cloud_with_normals); + gp3.setSearchMethod(tree2); + gp3.setSearchRadius(0.025); + gp3.setMu(2.5); + gp3.setMaximumNearestNeighbors(100); + gp3.setMaximumSurfaceAngle(M_PI / 4); // 45 degrees + gp3.setMinimumAngle(M_PI / 18); // 10 degrees + gp3.setMaximumAngle(2 * M_PI / 3); // 120 degrees + gp3.setNormalConsistency(false); + + // Reconstruct + gp3.reconstruct(triangles); + + float functArea = pcl::computeTriangleMeshArea(cloud_with_normals, triangles.polygons); + EXPECT_NEAR((functArea), 0.021, 0.001); + + pcl::PointCloud::Ptr cloud_ (new pcl::PointCloud ()); + pcl::PointXYZ p; + p.x = p.y = p.z = 0.f; + cloud_->push_back (p); + + p.x = 1.f; + p.y = 0.f; + p.z = 0.f; + cloud_->push_back (p); + + p.x = 0.f; + p.y = 1.f; + p.z = 0.f; + cloud_->push_back (p); + + p.x = 1.f; + p.y = 1.f; + p.z = 0.f; + cloud_->push_back (p); + + p.x = 0.f; + p.y = 1.5f; + p.z = 0.5f; + cloud_->push_back (p); + + p.x = 1.f; + p.y = 1.5f; + p.z = 0.5f; + cloud_->push_back (p); + + cloud_->height = 1; + cloud_->width = cloud_->size (); + + search::KdTree::Ptr treeLoc; + // Create search tree + treeLoc.reset (new search::KdTree (false)); + treeLoc->setInputCloud (cloud_); + + // Normal estimation + NormalEstimation nLoc; + PointCloud::Ptr normalsLoc (new PointCloud ()); + nLoc.setInputCloud (cloud_); + //nLoc.setIndices (indices[B); + nLoc.setSearchMethod (treeLoc); + nLoc.setKSearch (20); + nLoc.compute (*normalsLoc); + + // Concatenate XYZ and normal information + PointCloud::Ptr cloud_with_normalsLoc (new PointCloud); + pcl::concatenateFields (*cloud_, *normalsLoc, *cloud_with_normalsLoc); + + // Create search tree + tree2.reset (new search::KdTree); + tree2->setInputCloud (cloud_with_normalsLoc); + + // Init objects + PolygonMesh trianglesLoc; + GreedyProjectionTriangulation gpLoc; + + // Set parameters + gpLoc.setInputCloud(cloud_with_normalsLoc); + gpLoc.setSearchMethod(tree2); + gpLoc.setSearchRadius(2.0); + gpLoc.setMu(2.5); + gpLoc.setMaximumNearestNeighbors(100); + gpLoc.setMaximumSurfaceAngle(M_PI / 4); // 45 degrees + gpLoc.setMinimumAngle(M_PI / 18); // 10 degrees + gpLoc.setMaximumAngle(2 * M_PI / 3); // 120 degrees + gpLoc.setNormalConsistency(false); + + // Reconstruct + gpLoc.reconstruct(trianglesLoc); + + functArea = pcl::computeTriangleMeshArea(cloud_with_normalsLoc, trianglesLoc.polygons); + EXPECT_NEAR((functArea), 1.70710678, 0.1);//1+1*sqrt(0.5^2+0.5^2) Pythagoras th. + +} + + /* ---[ */ int main (int argc, char** argv)