FreeCAD

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
#endif
35

36
#if defined(HAVE_PCL_SAMPLE_CONSENSUS)
37
#include <pcl/sample_consensus/method_types.h>
38
#include <pcl/sample_consensus/model_types.h>
39
#endif
40

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
#endif
47

48
using namespace std;
49
using namespace Reen;
50

51
#if defined(HAVE_PCL_FILTERS)
52
using pcl::PointCloud;
53
using pcl::PointNormal;
54
using pcl::PointXYZ;
55
#endif
56

57
#if defined(HAVE_PCL_SEGMENTATION)
58
Segmentation::Segmentation(const Points::PointKernel& pts, std::list<std::vector<int>>& clusters)
59
    : myPoints(pts)
60
    , myClusters(clusters)
61
{}
62

63
void Segmentation::perform(int ksearch)
64
{
65
    // All the objects needed
66
    pcl::PassThrough<PointXYZ> pass;
67
    pcl::NormalEstimation<PointXYZ, pcl::Normal> ne;
68
    pcl::SACSegmentationFromNormals<PointXYZ, pcl::Normal> seg;
69
    pcl::ExtractIndices<PointXYZ> extract;
70
    pcl::ExtractIndices<pcl::Normal> extract_normals;
71
    pcl::search::KdTree<PointXYZ>::Ptr tree(new pcl::search::KdTree<PointXYZ>());
72

73
    // Datasets
74
    pcl::PointCloud<PointXYZ>::Ptr cloud(new pcl::PointCloud<PointXYZ>);
75
    pcl::PointCloud<PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<PointXYZ>);
76
    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
77
    pcl::PointCloud<PointXYZ>::Ptr cloud_filtered2(new pcl::PointCloud<PointXYZ>);
78
    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2(new pcl::PointCloud<pcl::Normal>);
79
    pcl::ModelCoefficients::Ptr coefficients_plane(new pcl::ModelCoefficients),
80
        coefficients_cylinder(new pcl::ModelCoefficients);
81
    pcl::PointIndices::Ptr inliers_plane(new pcl::PointIndices),
82
        inliers_cylinder(new pcl::PointIndices);
83

84
    // Copy the points
85
    cloud->reserve(myPoints.size());
86
    for (Points::PointKernel::const_iterator it = myPoints.begin(); it != myPoints.end(); ++it) {
87
        cloud->push_back(pcl::PointXYZ(it->x, it->y, it->z));
88
    }
89

90
    cloud->width = int(cloud->points.size());
91
    cloud->height = 1;
92

93
    // Build a passthrough filter to remove spurious NaNs
94
    pass.setInputCloud(cloud);
95
    pass.setFilterFieldName("z");
96
    pass.setFilterLimits(0, 1.5);
97
    pass.filter(*cloud_filtered);
98

99
    // Estimate point normals
100
    ne.setSearchMethod(tree);
101
    ne.setInputCloud(cloud_filtered);
102
    ne.setKSearch(ksearch);
103
    ne.compute(*cloud_normals);
104

105
    // Create the segmentation object for the planar model and set all the parameters
106
    seg.setOptimizeCoefficients(true);
107
    seg.setModelType(pcl::SACMODEL_NORMAL_PLANE);
108
    seg.setNormalDistanceWeight(0.1);
109
    seg.setMethodType(pcl::SAC_RANSAC);
110
    seg.setMaxIterations(100);
111
    seg.setDistanceThreshold(0.03);
112
    seg.setInputCloud(cloud_filtered);
113
    seg.setInputNormals(cloud_normals);
114

115
    // Obtain the plane inliers and coefficients
116
    seg.segment(*inliers_plane, *coefficients_plane);
117
    myClusters.push_back(inliers_plane->indices);
118

119
    // Extract the planar inliers from the input cloud
120
    extract.setInputCloud(cloud_filtered);
121
    extract.setIndices(inliers_plane);
122
    extract.setNegative(false);
123

124
    // Write the planar inliers to disk
125
    pcl::PointCloud<PointXYZ>::Ptr cloud_plane(new pcl::PointCloud<PointXYZ>());
126
    extract.filter(*cloud_plane);
127

128
    // Remove the planar inliers, extract the rest
129
    extract.setNegative(true);
130
    extract.filter(*cloud_filtered2);
131
    extract_normals.setNegative(true);
132
    extract_normals.setInputCloud(cloud_normals);
133
    extract_normals.setIndices(inliers_plane);
134
    extract_normals.filter(*cloud_normals2);
135

136
    // Create the segmentation object for cylinder segmentation and set all the parameters
137
    seg.setOptimizeCoefficients(true);
138
    seg.setModelType(pcl::SACMODEL_CYLINDER);
139
    seg.setNormalDistanceWeight(0.1);
140
    seg.setMethodType(pcl::SAC_RANSAC);
141
    seg.setMaxIterations(10000);
142
    seg.setDistanceThreshold(0.05);
143
    seg.setRadiusLimits(0, 0.1);
144
    seg.setInputCloud(cloud_filtered2);
145
    seg.setInputNormals(cloud_normals2);
146

147
    // Obtain the cylinder inliers and coefficients
148
    seg.segment(*inliers_cylinder, *coefficients_cylinder);
149
    myClusters.push_back(inliers_cylinder->indices);
150

151
    // Write the cylinder inliers to disk
152
    extract.setInputCloud(cloud_filtered2);
153
    extract.setIndices(inliers_cylinder);
154
    extract.setNegative(false);
155
    pcl::PointCloud<PointXYZ>::Ptr cloud_cylinder(new pcl::PointCloud<PointXYZ>());
156
    extract.filter(*cloud_cylinder);
157
}
158

159
#endif  // HAVE_PCL_SEGMENTATION
160

161
// ----------------------------------------------------------------------------
162

163
#if defined(HAVE_PCL_FILTERS)
164
NormalEstimation::NormalEstimation(const Points::PointKernel& pts)
165
    : myPoints(pts)
166
    , kSearch(0)
167
    , searchRadius(0)
168
{}
169

170
void NormalEstimation::perform(std::vector<Base::Vector3d>& normals)
171
{
172
    // Copy the points
173
    pcl::PointCloud<PointXYZ>::Ptr cloud(new pcl::PointCloud<PointXYZ>);
174
    cloud->reserve(myPoints.size());
175
    for (Points::PointKernel::const_iterator it = myPoints.begin(); it != myPoints.end(); ++it) {
176
        cloud->push_back(pcl::PointXYZ(it->x, it->y, it->z));
177
    }
178

179
    cloud->width = int(cloud->points.size());
180
    cloud->height = 1;
181

182
#if 0
183
    // Build a passthrough filter to remove spurious NaNs
184
    pcl::PointCloud<PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<PointXYZ>);
185
    pcl::PassThrough<PointXYZ> pass;
186
    pass.setInputCloud (cloud);
187
    pass.setFilterFieldName ("z");
188
    pass.setFilterLimits (0, 1.5);
189
    pass.filter (*cloud_filtered);
190
#endif
191

192
    // Estimate point normals
193
    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
194
    pcl::search::KdTree<PointXYZ>::Ptr tree(new pcl::search::KdTree<PointXYZ>());
195
    pcl::NormalEstimation<PointXYZ, pcl::Normal> ne;
196
    ne.setSearchMethod(tree);
197
    // ne.setInputCloud (cloud_filtered);
198
    ne.setInputCloud(cloud);
199
    if (kSearch > 0) {
200
        ne.setKSearch(kSearch);
201
    }
202
    if (searchRadius > 0) {
203
        ne.setRadiusSearch(searchRadius);
204
    }
205
    ne.compute(*cloud_normals);
206

207
    normals.reserve(cloud_normals->size());
208
    for (pcl::PointCloud<pcl::Normal>::const_iterator it = cloud_normals->begin();
209
         it != cloud_normals->end();
210
         ++it) {
211
        normals.push_back(Base::Vector3d(it->normal_x, it->normal_y, it->normal_z));
212
    }
213
}
214

215
#endif  // HAVE_PCL_FILTERS
216