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
#ifndef _PreComp_
25
#include <boost/math/special_functions/fpclassify.hpp>
26
#endif
27

28
#include <Mod/Points/App/Points.h>
29

30
#include "RegionGrowing.h"
31

32

33
#if defined(HAVE_PCL_FILTERS)
34
#include <pcl/filters/passthrough.h>
35
#include <pcl/point_types.h>
36
#endif
37
#if defined(HAVE_PCL_SEGMENTATION)
38
#include <pcl/features/normal_3d.h>
39
#include <pcl/filters/extract_indices.h>
40
#include <pcl/search/kdtree.h>
41
#include <pcl/search/search.h>
42
#include <pcl/segmentation/region_growing.h>
43

44
using namespace std;
45
using namespace Reen;
46
using pcl::PointCloud;
47
using pcl::PointNormal;
48
using pcl::PointXYZ;
49

50
RegionGrowing::RegionGrowing(const Points::PointKernel& pts, std::list<std::vector<int>>& clusters)
51
    : myPoints(pts)
52
    , myClusters(clusters)
53
{}
54

55
void RegionGrowing::perform(int ksearch)
56
{
57
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
58
    cloud->reserve(myPoints.size());
59
    for (Points::PointKernel::const_iterator it = myPoints.begin(); it != myPoints.end(); ++it) {
60
        if (!boost::math::isnan(it->x) && !boost::math::isnan(it->y)
61
            && !boost::math::isnan(it->z)) {
62
            cloud->push_back(pcl::PointXYZ(it->x, it->y, it->z));
63
        }
64
    }
65

66
    // normal estimation
67
    pcl::search::Search<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
68
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
69
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;
70
    normal_estimator.setSearchMethod(tree);
71
    normal_estimator.setInputCloud(cloud);
72
    normal_estimator.setKSearch(ksearch);
73
    normal_estimator.compute(*normals);
74

75
    // pass through
76
    pcl::IndicesPtr indices(new std::vector<int>);
77
    pcl::PassThrough<pcl::PointXYZ> pass;
78
    pass.setInputCloud(cloud);
79
    pass.setFilterFieldName("z");
80
    pass.setFilterLimits(0.0, 1.0);
81
    pass.filter(*indices);
82

83
    pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;
84
    reg.setMinClusterSize(50);
85
    reg.setMaxClusterSize(1000000);
86
    reg.setSearchMethod(tree);
87
    reg.setNumberOfNeighbours(30);
88
    reg.setInputCloud(cloud);
89
    // reg.setIndices (indices);
90
    reg.setInputNormals(normals);
91
    reg.setSmoothnessThreshold(3.0 / 180.0 * M_PI);
92
    reg.setCurvatureThreshold(1.0);
93

94
    std::vector<pcl::PointIndices> clusters;
95
    reg.extract(clusters);
96

97
    for (std::vector<pcl::PointIndices>::iterator it = clusters.begin(); it != clusters.end();
98
         ++it) {
99
        myClusters.push_back(std::vector<int>());
100
        myClusters.back().swap(it->indices);
101
    }
102
}
103

104
void RegionGrowing::perform(const std::vector<Base::Vector3f>& myNormals)
105
{
106
    if (myPoints.size() != myNormals.size()) {
107
        throw Base::RuntimeError("Number of points doesn't match with number of normals");
108
    }
109

110
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
111
    cloud->reserve(myPoints.size());
112
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
113
    normals->reserve(myNormals.size());
114

115
    std::size_t num_points = myPoints.size();
116
    const std::vector<Base::Vector3f>& points = myPoints.getBasicPoints();
117
    for (std::size_t index = 0; index < num_points; index++) {
118
        const Base::Vector3f& p = points[index];
119
        const Base::Vector3f& n = myNormals[index];
120
        if (!boost::math::isnan(p.x) && !boost::math::isnan(p.y) && !boost::math::isnan(p.z)) {
121
            cloud->push_back(pcl::PointXYZ(p.x, p.y, p.z));
122
            normals->push_back(pcl::Normal(n.x, n.y, n.z));
123
        }
124
    }
125

126
    pcl::search::Search<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
127
    tree->setInputCloud(cloud);
128

129
    // pass through
130
    pcl::IndicesPtr indices(new std::vector<int>);
131
    pcl::PassThrough<pcl::PointXYZ> pass;
132
    pass.setInputCloud(cloud);
133
    pass.setFilterFieldName("z");
134
    pass.setFilterLimits(0.0, 1.0);
135
    pass.filter(*indices);
136

137
    pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;
138
    reg.setMinClusterSize(50);
139
    reg.setMaxClusterSize(1000000);
140
    reg.setSearchMethod(tree);
141
    reg.setNumberOfNeighbours(30);
142
    reg.setInputCloud(cloud);
143
    // reg.setIndices (indices);
144
    reg.setInputNormals(normals);
145
    reg.setSmoothnessThreshold(3.0 / 180.0 * M_PI);
146
    reg.setCurvatureThreshold(1.0);
147

148
    std::vector<pcl::PointIndices> clusters;
149
    reg.extract(clusters);
150

151
    for (std::vector<pcl::PointIndices>::iterator it = clusters.begin(); it != clusters.end();
152
         ++it) {
153
        myClusters.push_back(std::vector<int>());
154
        myClusters.back().swap(it->indices);
155
    }
156
}
157

158
#endif  // HAVE_PCL_SEGMENTATION
159