diff --git a/src/Mod/ReverseEngineering/App/AppReverseEngineering.cpp b/src/Mod/ReverseEngineering/App/AppReverseEngineering.cpp index 47d48f77f..40fa74d35 100644 --- a/src/Mod/ReverseEngineering/App/AppReverseEngineering.cpp +++ b/src/Mod/ReverseEngineering/App/AppReverseEngineering.cpp @@ -53,6 +53,24 @@ #include #endif +/* +Dependency of pcl components: +common: none +features: common, kdtree, octree, search, (range_image) +filters: common, kdtree, octree, sample_consenus, search +geomety: common +io: common, octree +kdtree: common +keypoints: common, features, filters, kdtree, octree, search, (range_image) +octree: common +recognition: common, features, search +registration: common, features, kdtree, sample_consensus +sample_consensus: common +search: common, kdtree, octree +segmentation: common, kdtree, octree, sample_consensus, search +surface: common, kdtree, octree, search +*/ + using namespace Reen; namespace Reen { @@ -95,6 +113,9 @@ public: add_keyword_method("filterVoxelGrid",&Module::filterVoxelGrid, "filterVoxelGrid(dim)." ); + add_keyword_method("normalEstimation",&Module::normalEstimation, + "normalEstimation(Points)." + ); #endif #if defined(HAVE_PCL_SEGMENTATION) add_keyword_method("regionGrowingSegmentation",&Module::regionGrowingSegmentation, @@ -589,6 +610,35 @@ Mesh.show(m) return Py::asObject(new Points::PointsPy(points_sample)); } #endif +#if defined(HAVE_PCL_FILTERS) + Py::Object Module::normalEstimation(const Py::Tuple& args, const Py::Dict& kwds) + { + PyObject *pts; + int ksearch=0; + double searchRadius=0; + + static char* kwds_normals[] = {"Points", "KSearch", "SearchRadius", NULL}; + if (!PyArg_ParseTupleAndKeywords(args.ptr(), kwds.ptr(), "O!|id", kwds_normals, + &(Points::PointsPy::Type), &pts, + &ksearch, &searchRadius)) + throw Py::Exception(); + + Points::PointKernel* points = static_cast(pts)->getPointKernelPtr(); + + std::vector normals; + NormalEstimation estimate(*points); + estimate.setKSearch(ksearch); + estimate.setSearchRadius(searchRadius); + estimate.perform(normals); + + Py::List list; + for (std::vector::iterator it = normals.begin(); it != normals.end(); ++it) { + list.append(Py::Vector(*it)); + } + + return list; + } +#endif #if defined(HAVE_PCL_SEGMENTATION) Py::Object regionGrowingSegmentation(const Py::Tuple& args, const Py::Dict& kwds) { diff --git a/src/Mod/ReverseEngineering/App/Segmentation.cpp b/src/Mod/ReverseEngineering/App/Segmentation.cpp index 0dd2b84c8..2abbaf8d3 100644 --- a/src/Mod/ReverseEngineering/App/Segmentation.cpp +++ b/src/Mod/ReverseEngineering/App/Segmentation.cpp @@ -27,16 +27,23 @@ #include #include +#if defined(HAVE_PCL_FILTERS) +#include +#include +#include +#endif + +#if defined(HAVE_PCL_SAMPLE_CONSENSUS) +#include +#include +#endif + #if defined(HAVE_PCL_SEGMENTATION) #include #include #include -#include -#include -#include -#include -#include #include +#endif using namespace std; using namespace Reen; @@ -44,6 +51,7 @@ using pcl::PointXYZ; using pcl::PointNormal; using pcl::PointCloud; +#if defined(HAVE_PCL_SEGMENTATION) Segmentation::Segmentation(const Points::PointKernel& pts, std::list >& clusters) : myPoints(pts) , myClusters(clusters) @@ -87,7 +95,7 @@ void Segmentation::perform(int ksearch) // Estimate point normals ne.setSearchMethod (tree); ne.setInputCloud (cloud_filtered); - ne.setKSearch (50); + ne.setKSearch (ksearch); ne.compute (*cloud_normals); // Create the segmentation object for the planar model and set all the parameters @@ -146,3 +154,52 @@ void Segmentation::perform(int ksearch) #endif // HAVE_PCL_SEGMENTATION +// ---------------------------------------------------------------------------- + +#if defined (HAVE_PCL_FILTERS) +NormalEstimation::NormalEstimation(const Points::PointKernel& pts) + : myPoints(pts) + , kSearch(0) + , searchRadius(0) +{ +} + +void NormalEstimation::perform(std::vector& normals) +{ + // Copy the points + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + cloud->reserve(myPoints.size()); + for (Points::PointKernel::const_iterator it = myPoints.begin(); it != myPoints.end(); ++it) { + cloud->push_back(pcl::PointXYZ(it->x, it->y, it->z)); + } + + cloud->width = int (cloud->points.size ()); + cloud->height = 1; + + // Build a passthrough filter to remove spurious NaNs + pcl::PointCloud::Ptr cloud_filtered (new pcl::PointCloud); + pcl::PassThrough pass; + pass.setInputCloud (cloud); + pass.setFilterFieldName ("z"); + pass.setFilterLimits (0, 1.5); + pass.filter (*cloud_filtered); + + // Estimate point normals + pcl::PointCloud::Ptr cloud_normals (new pcl::PointCloud); + pcl::search::KdTree::Ptr tree (new pcl::search::KdTree ()); + pcl::NormalEstimation ne; + ne.setSearchMethod (tree); + ne.setInputCloud (cloud_filtered); + if (kSearch > 0) + ne.setKSearch (kSearch); + if (searchRadius > 0) + ne.setRadiusSearch (searchRadius); + ne.compute (*cloud_normals); + + normals.reserve(cloud_normals->size()); + for (pcl::PointCloud::const_iterator it = cloud_normals->begin(); it != cloud_normals->end(); ++it) { + normals.push_back(Base::Vector3d(it->normal_x, it->normal_y, it->normal_z)); + } +} + +#endif // HAVE_PCL_FILTERS diff --git a/src/Mod/ReverseEngineering/App/Segmentation.h b/src/Mod/ReverseEngineering/App/Segmentation.h index 4e55362c4..e2df4a6d6 100644 --- a/src/Mod/ReverseEngineering/App/Segmentation.h +++ b/src/Mod/ReverseEngineering/App/Segmentation.h @@ -46,6 +46,37 @@ private: std::list >& myClusters; }; +class NormalEstimation +{ +public: + NormalEstimation(const Points::PointKernel&); + /** \brief Set the number of k nearest neighbors to use for the feature estimation. + * \param[in] k the number of k-nearest neighbors + */ + inline void + setKSearch (int k) { kSearch = k; } + + /** \brief Set the sphere radius that is to be used for determining the nearest neighbors used for the feature + * estimation. + * \param[in] radius the sphere radius used as the maximum distance to consider a point a neighbor + */ + inline void + setSearchRadius (double radius) + { + searchRadius = radius; + } + + /** \brief Perform the normal estimation. + * \param[out] the estimated normals + */ + void perform(std::vector& normals); + +private: + const Points::PointKernel& myPoints; + int kSearch; + double searchRadius; +}; + } // namespace Reen #endif // REEN_SEGMENTATION_H