FreeCAD

1
/***************************************************************************
2
 *   Copyright (c) 2015 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
#if defined(HAVE_PCL_OPENNURBS)
25
#ifndef _PreComp_
26
#include <map>
27

28
#include <Geom_BSplineSurface.hxx>
29
#include <TColStd_Array1OfInteger.hxx>
30
#include <TColStd_Array1OfReal.hxx>
31
#include <TColStd_Array2OfReal.hxx>
32
#include <TColgp_Array2OfPnt.hxx>
33
#endif
34

35
#include <Mod/Points/App/PointsPy.h>
36

37
#include "BSplineFitting.h"
38

39
#include <pcl/pcl_config.h>
40
#if PCL_VERSION_COMPARE(>=, 1, 7, 0)
41
#include <pcl/io/pcd_io.h>
42
#include <pcl/point_cloud.h>
43
#include <pcl/point_types.h>
44
#include <pcl/surface/on_nurbs/fitting_curve_2d_asdm.h>
45
#include <pcl/surface/on_nurbs/fitting_surface_tdm.h>
46
#endif
47

48
using namespace Reen;
49

50

51
BSplineFitting::BSplineFitting(const std::vector<Base::Vector3f>& pts)
52
    : myPoints(pts)
53
    , myIterations(10)
54
    , myOrder(3)
55
    , myRefinement(4)
56
    , myInteriorSmoothness(0.2)
57
    , myInteriorWeight(1.0)
58
    , myBoundarySmoothness(0.2)
59
    , myBoundaryWeight(0.0)
60
{}
61

62
void BSplineFitting::setIterations(unsigned value)
63
{
64
    myIterations = value;
65
}
66

67
void BSplineFitting::setOrder(unsigned value)
68
{
69
    myOrder = value;
70
}
71

72
void BSplineFitting::setRefinement(unsigned value)
73
{
74
    myRefinement = value;
75
}
76

77
void BSplineFitting::setInteriorSmoothness(double value)
78
{
79
    myInteriorSmoothness = value;
80
}
81

82
void BSplineFitting::setInteriorWeight(double value)
83
{
84
    myInteriorWeight = value;
85
}
86

87
void BSplineFitting::setBoundarySmoothness(double value)
88
{
89
    myBoundarySmoothness = value;
90
}
91

92
void BSplineFitting::setBoundaryWeight(double value)
93
{
94
    myBoundaryWeight = value;
95
}
96

97
Handle(Geom_BSplineSurface) BSplineFitting::perform()
98
{
99
#if PCL_VERSION_COMPARE(>=, 1, 7, 0)
100
    pcl::on_nurbs::NurbsDataSurface data;
101
    for (std::vector<Base::Vector3f>::const_iterator it = myPoints.begin(); it != myPoints.end();
102
         ++it) {
103
        if (!pcl_isnan(it->x) && !pcl_isnan(it->y) && !pcl_isnan(it->z)) {
104
            data.interior.push_back(Eigen::Vector3d(it->x, it->y, it->z));
105
        }
106
    }
107

108

109
    // fit B-spline surface
110
    //
111

112
    pcl::on_nurbs::FittingSurface::Parameter params;
113
    params.interior_smoothness = myInteriorSmoothness;
114
    params.interior_weight = myInteriorWeight;
115
    params.boundary_smoothness = myBoundarySmoothness;
116
    params.boundary_weight = myBoundaryWeight;
117

118
    // initialize
119
    ON_NurbsSurface nurbs = pcl::on_nurbs::FittingSurface::initNurbsPCABoundingBox(myOrder, &data);
120
    pcl::on_nurbs::FittingSurface fit(&data, nurbs);
121
    //  fit.setQuiet (false); // enable/disable debug output
122

123
    // surface refinement
124
    for (unsigned i = 0; i < myRefinement; i++) {
125
        fit.refine(0);
126
        fit.refine(1);
127
        fit.assemble(params);
128
        fit.solve();
129
    }
130

131
    // surface fitting with final refinement level
132
    for (unsigned i = 0; i < myIterations; i++) {
133
        fit.assemble(params);
134
        fit.solve();
135
    }
136

137
    // fit B-spline curve
138
#if 0
139
    // parameters
140
    pcl::on_nurbs::FittingCurve2dAPDM::FitParameter curve_params;
141
    curve_params.addCPsAccuracy = 5e-2;
142
    curve_params.addCPsIteration = 3;
143
    curve_params.maxCPs = 200;
144
    curve_params.accuracy = 1e-3;
145
    curve_params.iterations = 100;
146

147
    curve_params.param.closest_point_resolution = 0;
148
    curve_params.param.closest_point_weight = 1.0;
149
    curve_params.param.closest_point_sigma2 = 0.1;
150
    curve_params.param.interior_sigma2 = 0.00001;
151
    curve_params.param.smooth_concavity = 1.0;
152
    curve_params.param.smoothness = 1.0;
153

154
    // initialisation (circular)
155
    pcl::on_nurbs::NurbsDataCurve2d curve_data;
156
    curve_data.interior = data.interior_param;
157
    curve_data.interior_weight_function.push_back(true);
158
    ON_NurbsCurve curve_nurbs = pcl::on_nurbs::FittingCurve2dAPDM::initNurbsCurve2D(order, curve_data.interior);
159

160
    // curve fitting
161
    pcl::on_nurbs::FittingCurve2dASDM curve_fit (&curve_data, curve_nurbs);
162
    // curve_fit.setQuiet (false); // enable/disable debug output
163
    curve_fit.fitting (curve_params);
164
#endif
165

166
    // u parameters
167
    int numUKnots = fit.m_nurbs.KnotCount(0);
168
    int numUPoles = fit.m_nurbs.CVCount(0);
169
    int uDegree = fit.m_nurbs.Degree(0);
170
    bool uPeriodic = fit.m_nurbs.IsPeriodic(0) ? true : false;
171
    std::map<Standard_Real, int> uKnots;
172

173
    // v parameters
174
    int numVKnots = fit.m_nurbs.KnotCount(1);
175
    int numVPoles = fit.m_nurbs.CVCount(1);
176
    int vDegree = fit.m_nurbs.Degree(1);
177
    bool vPeriodic = fit.m_nurbs.IsPeriodic(1) ? true : false;
178
    std::map<Standard_Real, int> vKnots;
179

180
    TColgp_Array2OfPnt poles(1, numUPoles, 1, numVPoles);
181
    TColStd_Array2OfReal weights(1, numUPoles, 1, numVPoles);
182

183
    for (int i = 0; i < numUPoles; i++) {
184
        for (int j = 0; j < numVPoles; j++) {
185
            ON_3dPoint cv;
186
            fit.m_nurbs.GetCV(i, j, cv);
187
            poles.SetValue(i + 1, j + 1, gp_Pnt(cv.x, cv.y, cv.z));
188

189
            Standard_Real weight = fit.m_nurbs.Weight(i, j);
190
            weights.SetValue(i + 1, j + 1, weight);
191
        }
192
    }
193

194
    uKnots[fit.m_nurbs.SuperfluousKnot(0, 0)] = 1;
195
    uKnots[fit.m_nurbs.SuperfluousKnot(0, 1)] = 1;
196
    for (int i = 0; i < numUKnots; i++) {
197
        Standard_Real value = fit.m_nurbs.Knot(0, i);
198
        std::map<Standard_Real, int>::iterator it = uKnots.find(value);
199
        if (it == uKnots.end()) {
200
            uKnots[value] = 1;
201
        }
202
        else {
203
            it->second++;
204
        }
205
    }
206

207
    vKnots[fit.m_nurbs.SuperfluousKnot(1, 0)] = 1;
208
    vKnots[fit.m_nurbs.SuperfluousKnot(1, 1)] = 1;
209
    for (int i = 0; i < numVKnots; i++) {
210
        Standard_Real value = fit.m_nurbs.Knot(1, i);
211
        std::map<Standard_Real, int>::iterator it = vKnots.find(value);
212
        if (it == vKnots.end()) {
213
            vKnots[value] = 1;
214
        }
215
        else {
216
            it->second++;
217
        }
218
    }
219

220
    TColStd_Array1OfReal uKnotArray(1, uKnots.size());
221
    TColStd_Array1OfInteger uMultArray(1, uKnots.size());
222
    int index = 1;
223
    for (std::map<Standard_Real, int>::iterator it = uKnots.begin(); it != uKnots.end();
224
         ++it, index++) {
225
        uKnotArray.SetValue(index, it->first);
226
        uMultArray.SetValue(index, it->second);
227
    }
228

229
    TColStd_Array1OfReal vKnotArray(1, vKnots.size());
230
    TColStd_Array1OfInteger vMultArray(1, vKnots.size());
231
    index = 1;
232
    for (std::map<Standard_Real, int>::iterator it = vKnots.begin(); it != vKnots.end();
233
         ++it, index++) {
234
        vKnotArray.SetValue(index, it->first);
235
        vMultArray.SetValue(index, it->second);
236
    }
237

238
    Handle(Geom_BSplineSurface) spline = new Geom_BSplineSurface(poles,
239
                                                                 weights,
240
                                                                 uKnotArray,
241
                                                                 vKnotArray,
242
                                                                 uMultArray,
243
                                                                 vMultArray,
244
                                                                 uDegree,
245
                                                                 vDegree,
246
                                                                 uPeriodic,
247
                                                                 vPeriodic);
248
    return spline;
249
#else
250
    return Handle(Geom_BSplineSurface)();
251
#endif
252
}
253
#endif  // HAVE_PCL_OPENNURBS
254