FreeCAD
215 строк · 8.0 Кб
1/***************************************************************************
2* Copyright (c) 2016 Werner Mayer <wmayer[at]users.sourceforge.net> *
3* *
4* This file is part of the FreeCAD CAx development system. *
5* *
6* This library is free software; you can redistribute it and/or *
7* modify it under the terms of the GNU Library General Public *
8* License as published by the Free Software Foundation; either *
9* version 2 of the License, or (at your option) any later version. *
10* *
11* This library is distributed in the hope that it will be useful, *
12* but WITHOUT ANY WARRANTY; without even the implied warranty of *
13* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
14* GNU Library General Public License for more details. *
15* *
16* You should have received a copy of the GNU Library General Public *
17* License along with this library; see the file COPYING.LIB. If not, *
18* write to the Free Software Foundation, Inc., 59 Temple Place, *
19* Suite 330, Boston, MA 02111-1307, USA *
20* *
21***************************************************************************/
22
23#include "PreCompiled.h"24
25#include <Mod/Points/App/Points.h>26
27#include "Segmentation.h"28
29
30#if defined(HAVE_PCL_FILTERS)31#include <pcl/features/normal_3d.h>32#include <pcl/filters/extract_indices.h>33#include <pcl/filters/passthrough.h>34#endif35
36#if defined(HAVE_PCL_SAMPLE_CONSENSUS)37#include <pcl/sample_consensus/method_types.h>38#include <pcl/sample_consensus/model_types.h>39#endif40
41#if defined(HAVE_PCL_SEGMENTATION)42#include <pcl/ModelCoefficients.h>43#include <pcl/io/pcd_io.h>44#include <pcl/point_types.h>45#include <pcl/segmentation/sac_segmentation.h>46#endif47
48using namespace std;49using namespace Reen;50
51#if defined(HAVE_PCL_FILTERS)52using pcl::PointCloud;53using pcl::PointNormal;54using pcl::PointXYZ;55#endif56
57#if defined(HAVE_PCL_SEGMENTATION)58Segmentation::Segmentation(const Points::PointKernel& pts, std::list<std::vector<int>>& clusters)59: myPoints(pts)60, myClusters(clusters)61{}62
63void Segmentation::perform(int ksearch)64{
65// All the objects needed66pcl::PassThrough<PointXYZ> pass;67pcl::NormalEstimation<PointXYZ, pcl::Normal> ne;68pcl::SACSegmentationFromNormals<PointXYZ, pcl::Normal> seg;69pcl::ExtractIndices<PointXYZ> extract;70pcl::ExtractIndices<pcl::Normal> extract_normals;71pcl::search::KdTree<PointXYZ>::Ptr tree(new pcl::search::KdTree<PointXYZ>());72
73// Datasets74pcl::PointCloud<PointXYZ>::Ptr cloud(new pcl::PointCloud<PointXYZ>);75pcl::PointCloud<PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<PointXYZ>);76pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);77pcl::PointCloud<PointXYZ>::Ptr cloud_filtered2(new pcl::PointCloud<PointXYZ>);78pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2(new pcl::PointCloud<pcl::Normal>);79pcl::ModelCoefficients::Ptr coefficients_plane(new pcl::ModelCoefficients),80coefficients_cylinder(new pcl::ModelCoefficients);81pcl::PointIndices::Ptr inliers_plane(new pcl::PointIndices),82inliers_cylinder(new pcl::PointIndices);83
84// Copy the points85cloud->reserve(myPoints.size());86for (Points::PointKernel::const_iterator it = myPoints.begin(); it != myPoints.end(); ++it) {87cloud->push_back(pcl::PointXYZ(it->x, it->y, it->z));88}89
90cloud->width = int(cloud->points.size());91cloud->height = 1;92
93// Build a passthrough filter to remove spurious NaNs94pass.setInputCloud(cloud);95pass.setFilterFieldName("z");96pass.setFilterLimits(0, 1.5);97pass.filter(*cloud_filtered);98
99// Estimate point normals100ne.setSearchMethod(tree);101ne.setInputCloud(cloud_filtered);102ne.setKSearch(ksearch);103ne.compute(*cloud_normals);104
105// Create the segmentation object for the planar model and set all the parameters106seg.setOptimizeCoefficients(true);107seg.setModelType(pcl::SACMODEL_NORMAL_PLANE);108seg.setNormalDistanceWeight(0.1);109seg.setMethodType(pcl::SAC_RANSAC);110seg.setMaxIterations(100);111seg.setDistanceThreshold(0.03);112seg.setInputCloud(cloud_filtered);113seg.setInputNormals(cloud_normals);114
115// Obtain the plane inliers and coefficients116seg.segment(*inliers_plane, *coefficients_plane);117myClusters.push_back(inliers_plane->indices);118
119// Extract the planar inliers from the input cloud120extract.setInputCloud(cloud_filtered);121extract.setIndices(inliers_plane);122extract.setNegative(false);123
124// Write the planar inliers to disk125pcl::PointCloud<PointXYZ>::Ptr cloud_plane(new pcl::PointCloud<PointXYZ>());126extract.filter(*cloud_plane);127
128// Remove the planar inliers, extract the rest129extract.setNegative(true);130extract.filter(*cloud_filtered2);131extract_normals.setNegative(true);132extract_normals.setInputCloud(cloud_normals);133extract_normals.setIndices(inliers_plane);134extract_normals.filter(*cloud_normals2);135
136// Create the segmentation object for cylinder segmentation and set all the parameters137seg.setOptimizeCoefficients(true);138seg.setModelType(pcl::SACMODEL_CYLINDER);139seg.setNormalDistanceWeight(0.1);140seg.setMethodType(pcl::SAC_RANSAC);141seg.setMaxIterations(10000);142seg.setDistanceThreshold(0.05);143seg.setRadiusLimits(0, 0.1);144seg.setInputCloud(cloud_filtered2);145seg.setInputNormals(cloud_normals2);146
147// Obtain the cylinder inliers and coefficients148seg.segment(*inliers_cylinder, *coefficients_cylinder);149myClusters.push_back(inliers_cylinder->indices);150
151// Write the cylinder inliers to disk152extract.setInputCloud(cloud_filtered2);153extract.setIndices(inliers_cylinder);154extract.setNegative(false);155pcl::PointCloud<PointXYZ>::Ptr cloud_cylinder(new pcl::PointCloud<PointXYZ>());156extract.filter(*cloud_cylinder);157}
158
159#endif // HAVE_PCL_SEGMENTATION160
161// ----------------------------------------------------------------------------
162
163#if defined(HAVE_PCL_FILTERS)164NormalEstimation::NormalEstimation(const Points::PointKernel& pts)165: myPoints(pts)166, kSearch(0)167, searchRadius(0)168{}169
170void NormalEstimation::perform(std::vector<Base::Vector3d>& normals)171{
172// Copy the points173pcl::PointCloud<PointXYZ>::Ptr cloud(new pcl::PointCloud<PointXYZ>);174cloud->reserve(myPoints.size());175for (Points::PointKernel::const_iterator it = myPoints.begin(); it != myPoints.end(); ++it) {176cloud->push_back(pcl::PointXYZ(it->x, it->y, it->z));177}178
179cloud->width = int(cloud->points.size());180cloud->height = 1;181
182#if 0183// Build a passthrough filter to remove spurious NaNs184pcl::PointCloud<PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<PointXYZ>);185pcl::PassThrough<PointXYZ> pass;186pass.setInputCloud (cloud);187pass.setFilterFieldName ("z");188pass.setFilterLimits (0, 1.5);189pass.filter (*cloud_filtered);190#endif191
192// Estimate point normals193pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);194pcl::search::KdTree<PointXYZ>::Ptr tree(new pcl::search::KdTree<PointXYZ>());195pcl::NormalEstimation<PointXYZ, pcl::Normal> ne;196ne.setSearchMethod(tree);197// ne.setInputCloud (cloud_filtered);198ne.setInputCloud(cloud);199if (kSearch > 0) {200ne.setKSearch(kSearch);201}202if (searchRadius > 0) {203ne.setRadiusSearch(searchRadius);204}205ne.compute(*cloud_normals);206
207normals.reserve(cloud_normals->size());208for (pcl::PointCloud<pcl::Normal>::const_iterator it = cloud_normals->begin();209it != cloud_normals->end();210++it) {211normals.push_back(Base::Vector3d(it->normal_x, it->normal_y, it->normal_z));212}213}
214
215#endif // HAVE_PCL_FILTERS216