148 lines
5.9 KiB
C++
148 lines
5.9 KiB
C++
/***************************************************************************
|
|
* Copyright (c) 2016 Werner Mayer <wmayer[at]users.sourceforge.net> *
|
|
* *
|
|
* This file is part of the FreeCAD CAx development system. *
|
|
* *
|
|
* This library is free software; you can redistribute it and/or *
|
|
* modify it under the terms of the GNU Library General Public *
|
|
* License as published by the Free Software Foundation; either *
|
|
* version 2 of the License, or (at your option) any later version. *
|
|
* *
|
|
* This library is distributed in the hope that it will be useful, *
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
|
|
* GNU Library General Public License for more details. *
|
|
* *
|
|
* You should have received a copy of the GNU Library General Public *
|
|
* License along with this library; see the file COPYING.LIB. If not, *
|
|
* write to the Free Software Foundation, Inc., 59 Temple Place, *
|
|
* Suite 330, Boston, MA 02111-1307, USA *
|
|
* *
|
|
***************************************************************************/
|
|
|
|
|
|
#include "PreCompiled.h"
|
|
|
|
#include "RegionGrowing.h"
|
|
#include <Mod/Points/App/Points.h>
|
|
#include <Base/Exception.h>
|
|
|
|
#if defined(HAVE_PCL_FILTERS)
|
|
#include <pcl/filters/passthrough.h>
|
|
#include <pcl/point_types.h>
|
|
#endif
|
|
#if defined(HAVE_PCL_SEGMENTATION)
|
|
#include <pcl/search/search.h>
|
|
#include <pcl/search/kdtree.h>
|
|
#include <pcl/features/normal_3d.h>
|
|
#include <pcl/segmentation/region_growing.h>
|
|
#include <pcl/filters/extract_indices.h>
|
|
|
|
using namespace std;
|
|
using namespace Reen;
|
|
using pcl::PointXYZ;
|
|
using pcl::PointNormal;
|
|
using pcl::PointCloud;
|
|
|
|
RegionGrowing::RegionGrowing(const Points::PointKernel& pts, std::list<std::vector<int> >& clusters)
|
|
: myPoints(pts)
|
|
, myClusters(clusters)
|
|
{
|
|
}
|
|
|
|
void RegionGrowing::perform(int ksearch)
|
|
{
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
|
|
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));
|
|
}
|
|
|
|
//normal estimation
|
|
pcl::search::Search<pcl::PointXYZ>::Ptr tree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZ> > (new pcl::search::KdTree<pcl::PointXYZ>);
|
|
pcl::PointCloud <pcl::Normal>::Ptr normals (new pcl::PointCloud <pcl::Normal>);
|
|
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;
|
|
normal_estimator.setSearchMethod (tree);
|
|
normal_estimator.setInputCloud (cloud);
|
|
normal_estimator.setKSearch (ksearch);
|
|
normal_estimator.compute (*normals);
|
|
|
|
// pass through
|
|
pcl::IndicesPtr indices (new std::vector <int>);
|
|
pcl::PassThrough<pcl::PointXYZ> pass;
|
|
pass.setInputCloud (cloud);
|
|
pass.setFilterFieldName ("z");
|
|
pass.setFilterLimits (0.0, 1.0);
|
|
pass.filter (*indices);
|
|
|
|
pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;
|
|
reg.setMinClusterSize (50);
|
|
reg.setMaxClusterSize (1000000);
|
|
reg.setSearchMethod (tree);
|
|
reg.setNumberOfNeighbours (30);
|
|
reg.setInputCloud (cloud);
|
|
//reg.setIndices (indices);
|
|
reg.setInputNormals (normals);
|
|
reg.setSmoothnessThreshold (3.0 / 180.0 * M_PI);
|
|
reg.setCurvatureThreshold (1.0);
|
|
|
|
std::vector <pcl::PointIndices> clusters;
|
|
reg.extract (clusters);
|
|
|
|
for (std::vector<pcl::PointIndices>::iterator it = clusters.begin (); it != clusters.end (); ++it) {
|
|
myClusters.push_back(std::vector<int>());
|
|
myClusters.back().swap(it->indices);
|
|
}
|
|
}
|
|
|
|
void RegionGrowing::perform(const std::vector<Base::Vector3f>& myNormals)
|
|
{
|
|
if (myPoints.size() != myNormals.size())
|
|
throw Base::RuntimeError("Number of points doesn't match with number of normals");
|
|
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
|
|
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));
|
|
}
|
|
|
|
pcl::search::Search<pcl::PointXYZ>::Ptr tree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZ> > (new pcl::search::KdTree<pcl::PointXYZ>);
|
|
tree->setInputCloud (cloud);
|
|
|
|
pcl::PointCloud <pcl::Normal>::Ptr normals (new pcl::PointCloud <pcl::Normal>);
|
|
normals->reserve(myNormals.size());
|
|
for (std::vector<Base::Vector3f>::const_iterator it = myNormals.begin(); it != myNormals.end(); ++it) {
|
|
normals->push_back(pcl::Normal(it->x, it->y, it->z));
|
|
}
|
|
|
|
// pass through
|
|
pcl::IndicesPtr indices (new std::vector <int>);
|
|
pcl::PassThrough<pcl::PointXYZ> pass;
|
|
pass.setInputCloud (cloud);
|
|
pass.setFilterFieldName ("z");
|
|
pass.setFilterLimits (0.0, 1.0);
|
|
pass.filter (*indices);
|
|
|
|
pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;
|
|
reg.setMinClusterSize (50);
|
|
reg.setMaxClusterSize (1000000);
|
|
reg.setSearchMethod (tree);
|
|
reg.setNumberOfNeighbours (30);
|
|
reg.setInputCloud (cloud);
|
|
//reg.setIndices (indices);
|
|
reg.setInputNormals (normals);
|
|
reg.setSmoothnessThreshold (3.0 / 180.0 * M_PI);
|
|
reg.setCurvatureThreshold (1.0);
|
|
|
|
std::vector <pcl::PointIndices> clusters;
|
|
reg.extract (clusters);
|
|
|
|
for (std::vector<pcl::PointIndices>::iterator it = clusters.begin (); it != clusters.end (); ++it) {
|
|
myClusters.push_back(std::vector<int>());
|
|
myClusters.back().swap(it->indices);
|
|
}
|
|
}
|
|
|
|
#endif // HAVE_PCL_SEGMENTATION
|
|
|