FreeCAD

1
/***************************************************************************
2
 *   Copyright (c) 2020 Graeme van der Vlugt                               *
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
#ifndef _PreComp_
26
#include <algorithm>
27
#include <cstdlib>
28
#include <iterator>
29
#endif
30

31
#include "SphereFit.h"
32

33

34
using namespace MeshCoreFit;
35

36
SphereFit::SphereFit()
37
    : _vCenter(0, 0, 0)
38
{}
39

40
// Set approximations before calling the fitting
41
void SphereFit::SetApproximations(double radius, const Base::Vector3d& center)
42
{
43
    _bIsFitted = false;
44
    _fLastResult = FLOAT_MAX;
45
    _numIter = 0;
46
    _dRadius = radius;
47
    _vCenter = center;
48
}
49

50
// Set iteration convergence criteria for the fit if special values are needed.
51
// The default values set in the constructor are suitable for most uses
52
void SphereFit::SetConvergenceCriteria(double posConvLimit, double vConvLimit, int maxIter)
53
{
54
    if (posConvLimit > 0.0) {
55
        _posConvLimit = posConvLimit;
56
    }
57
    if (vConvLimit > 0.0) {
58
        _vConvLimit = vConvLimit;
59
    }
60
    if (maxIter > 0) {
61
        _maxIter = maxIter;
62
    }
63
}
64

65

66
double SphereFit::GetRadius() const
67
{
68
    if (_bIsFitted) {
69
        return _dRadius;
70
    }
71
    else {
72
        return 0.0;
73
    }
74
}
75

76
Base::Vector3d SphereFit::GetCenter() const
77
{
78
    if (_bIsFitted) {
79
        return _vCenter;
80
    }
81
    else {
82
        return Base::Vector3d();
83
    }
84
}
85

86
int SphereFit::GetNumIterations() const
87
{
88
    if (_bIsFitted) {
89
        return _numIter;
90
    }
91
    else {
92
        return 0;
93
    }
94
}
95

96
float SphereFit::GetDistanceToSphere(const Base::Vector3f& rcPoint) const
97
{
98
    float fResult = FLOAT_MAX;
99
    if (_bIsFitted) {
100
        fResult = Base::Vector3d((double)rcPoint.x - _vCenter.x,
101
                                 (double)rcPoint.y - _vCenter.y,
102
                                 (double)rcPoint.z - _vCenter.z)
103
                      .Length()
104
            - _dRadius;
105
    }
106
    return fResult;
107
}
108

109
float SphereFit::GetStdDeviation() const
110
{
111
    // Mean: M=(1/N)*SUM Xi
112
    // Variance: VAR=(N/N-1)*[(1/N)*SUM(Xi^2)-M^2]
113
    // Standard deviation: SD=SQRT(VAR)
114
    if (!_bIsFitted) {
115
        return FLOAT_MAX;
116
    }
117

118
    double sumXi = 0.0, sumXi2 = 0.0, dist = 0.0;
119
    for (auto it : _vPoints) {
120
        dist = GetDistanceToSphere(it);
121
        sumXi += dist;
122
        sumXi2 += (dist * dist);
123
    }
124

125
    double N = static_cast<double>(CountPoints());
126
    double mean = sumXi / N;
127
    return sqrt((N / (N - 1.0)) * (sumXi2 / N - mean * mean));
128
}
129

130
void SphereFit::ProjectToSphere()
131
{
132
    for (auto& cPnt : _vPoints) {
133
        // Compute unit vector from sphere centre to point.
134
        // Because this vector is orthogonal to the sphere's surface at the
135
        // intersection point we can easily compute the projection point on the
136
        // closest surface point using the radius of the sphere
137
        Base::Vector3d diff((double)cPnt.x - _vCenter.x,
138
                            (double)cPnt.y - _vCenter.y,
139
                            (double)cPnt.z - _vCenter.z);
140
        double length = diff.Length();
141
        if (length == 0.0) {
142
            // Point is exactly at the sphere center, so it can be projected in any direction onto
143
            // the sphere! So here just project in +Z direction
144
            cPnt.z += (float)_dRadius;
145
        }
146
        else {
147
            diff /= length;  // normalizing the vector
148
            Base::Vector3d proj = _vCenter + diff * _dRadius;
149
            cPnt.x = (float)proj.x;
150
            cPnt.y = (float)proj.y;
151
            cPnt.z = (float)proj.z;
152
        }
153
    }
154
}
155

156
// Compute approximations for the parameters using all points:
157
// Set centre to centre of gravity of points and radius to the average
158
// distance from the centre of gravity to the points.
159
void SphereFit::ComputeApproximations()
160
{
161
    _bIsFitted = false;
162
    _fLastResult = FLOAT_MAX;
163
    _numIter = 0;
164
    _vCenter.Set(0.0, 0.0, 0.0);
165
    _dRadius = 0.0;
166
    if (!_vPoints.empty()) {
167
        std::list<Base::Vector3f>::const_iterator cIt;
168
        for (cIt = _vPoints.begin(); cIt != _vPoints.end(); ++cIt) {
169
            _vCenter.x += cIt->x;
170
            _vCenter.y += cIt->y;
171
            _vCenter.z += cIt->z;
172
        }
173
        _vCenter /= (double)_vPoints.size();
174

175
        for (cIt = _vPoints.begin(); cIt != _vPoints.end(); ++cIt) {
176
            Base::Vector3d diff((double)cIt->x - _vCenter.x,
177
                                (double)cIt->y - _vCenter.y,
178
                                (double)cIt->z - _vCenter.z);
179
            _dRadius += diff.Length();
180
        }
181
        _dRadius /= (double)_vPoints.size();
182
    }
183
}
184

185
float SphereFit::Fit()
186
{
187
    _bIsFitted = false;
188
    _fLastResult = FLOAT_MAX;
189
    _numIter = 0;
190

191
    // A minimum of 4 surface points is needed to define a sphere
192
    if (CountPoints() < 4) {
193
        return FLOAT_MAX;
194
    }
195

196
    // If approximations have not been set/computed then compute some now
197
    if (_dRadius == 0.0) {
198
        ComputeApproximations();
199
    }
200

201
    // Initialise some matrices and vectors
202
    std::vector<Base::Vector3d> residuals(CountPoints(), Base::Vector3d(0.0, 0.0, 0.0));
203
    Matrix4x4 atpa;
204
    Eigen::VectorXd atpl(4);
205

206
    // Iteration loop...
207
    double sigma0 {};
208
    bool cont = true;
209
    while (cont && (_numIter < _maxIter)) {
210
        ++_numIter;
211

212
        // Set up the quasi parametric normal equations
213
        setupNormalEquationMatrices(residuals, atpa, atpl);
214

215
        // Solve the equations for the unknown corrections
216
        Eigen::LLT<Matrix4x4> llt(atpa);
217
        if (llt.info() != Eigen::Success) {
218
            return FLOAT_MAX;
219
        }
220
        Eigen::VectorXd x = llt.solve(atpl);
221

222
        // Check parameter convergence (order of parameters: X,Y,Z,R)
223
        cont = false;
224
        if ((fabs(x(0)) > _posConvLimit) || (fabs(x(1)) > _posConvLimit)
225
            || (fabs(x(2)) > _posConvLimit) || (fabs(x(3)) > _posConvLimit)) {
226
            cont = true;
227
        }
228

229
        // Before updating the unknowns, compute the residuals and sigma0 and check the residual
230
        // convergence
231
        bool vConverged {};
232
        if (!computeResiduals(x, residuals, sigma0, _vConvLimit, vConverged)) {
233
            return FLOAT_MAX;
234
        }
235
        if (!vConverged) {
236
            cont = true;
237
        }
238

239
        // Update the parameters (order of parameters: X,Y,Z,R)
240
        _vCenter.x += x(0);
241
        _vCenter.y += x(1);
242
        _vCenter.z += x(2);
243
        _dRadius += x(3);
244
    }
245

246
    // Check for convergence
247
    if (cont) {
248
        return FLOAT_MAX;
249
    }
250

251
    _bIsFitted = true;
252
    _fLastResult = sigma0;
253

254
    return _fLastResult;
255
}
256

257
// Set up the normal equation matrices
258
// atpa ... 4x4 normal matrix
259
// atpl ... 4x1 matrix (right-hand side of equation)
260
void SphereFit::setupNormalEquationMatrices(const std::vector<Base::Vector3d>& residuals,
261
                                            Matrix4x4& atpa,
262
                                            Eigen::VectorXd& atpl) const
263
{
264
    // Zero matrices
265
    atpa.setZero();
266
    atpl.setZero();
267

268
    // For each point, setup the observation equation coefficients and add their
269
    // contribution into the normal equation matrices
270
    double a[4] {}, b[3] {};
271
    double f0 {}, qw {};
272
    std::vector<Base::Vector3d>::const_iterator vIt = residuals.begin();
273
    std::list<Base::Vector3f>::const_iterator cIt;
274
    for (cIt = _vPoints.begin(); cIt != _vPoints.end(); ++cIt, ++vIt) {
275
        // if (using this point) { // currently all given points are used (could modify this if
276
        // eliminating outliers, etc....
277
        setupObservation(*cIt, *vIt, a, f0, qw, b);
278
        addObservationU(a, f0, qw, atpa, atpl);
279
        // }
280
    }
281
    setLowerPart(atpa);
282
}
283

284
// Sets up contributions of given observation to the quasi parametric
285
// normal equation matrices. Assumes uncorrelated coordinates.
286
// point ... point
287
// residual ... residual for this point computed from previous iteration (zero for first iteration)
288
// a[4] ... parameter partials (order of parameters: X,Y,Z,R)
289
// f0   ... reference to f0 term
290
// qw   ... reference to quasi weight (here we are assuming equal unit weights for each observed
291
// point coordinate) b[3] ... observation partials
292
void SphereFit::setupObservation(const Base::Vector3f& point,
293
                                 const Base::Vector3d& residual,
294
                                 double a[4],
295
                                 double& f0,
296
                                 double& qw,
297
                                 double b[3]) const
298
{
299
    // This adjustment requires an update of the observation approximations
300
    // because the residuals do not have a linear relationship.
301
    // New estimates for the observations:
302
    double xEstimate = (double)point.x + residual.x;
303
    double yEstimate = (double)point.y + residual.y;
304
    double zEstimate = (double)point.z + residual.z;
305

306
    // partials of the observations
307
    double dx = xEstimate - _vCenter.x;
308
    double dy = yEstimate - _vCenter.y;
309
    double dz = zEstimate - _vCenter.z;
310
    b[0] = 2.0 * dx;
311
    b[1] = 2.0 * dy;
312
    b[2] = 2.0 * dz;
313

314
    // partials of the parameters
315
    a[0] = -b[0];
316
    a[1] = -b[1];
317
    a[2] = -b[2];
318
    a[3] = -2.0 * _dRadius;
319

320
    // free term
321
    f0 = _dRadius * _dRadius - dx * dx - dy * dy - dz * dz + b[0] * residual.x + b[1] * residual.y
322
        + b[2] * residual.z;
323

324
    // quasi weight (using equal weights for sphere point coordinate observations)
325
    // w[0] = 1.0;
326
    // w[1] = 1.0;
327
    // w[2] = 1.0;
328
    // qw = 1.0 / (b[0] * b[0] / w[0] + b[1] * b[1] / w[1] + b[2] * b[2] / w[2]);
329
    qw = 1.0 / (b[0] * b[0] + b[1] * b[1] + b[2] * b[2]);
330
}
331

332
// Computes contribution of the given observation equation on the normal equation matrices
333
// Call this for each observation (point)
334
// Here we only add the contribution to  the upper part of the normal matrix
335
// and then after all observations have been added we need to set the lower part
336
// (which is symmetrical to the upper part)
337
// a[4] ... parameter partials
338
// li   ... free term (f0)
339
// pi   ... weight of observation (= quasi weight qw for this solution)
340
// atpa ... 4x4 normal equation matrix
341
// atpl ... 4x1 matrix/vector (right-hand side of equations)
342
void SphereFit::addObservationU(double a[4],
343
                                double li,
344
                                double pi,
345
                                Matrix4x4& atpa,
346
                                Eigen::VectorXd& atpl) const
347
{
348
    for (int i = 0; i < 4; ++i) {
349
        double aipi = a[i] * pi;
350
        for (int j = i; j < 4; ++j) {
351
            atpa(i, j) += aipi * a[j];
352
            // atpa(j, i) = atpa(i, j);	// it's a symmetrical matrix, we'll set this later after all
353
            // observations processed
354
        }
355
        atpl(i) += aipi * li;
356
    }
357
}
358

359
// Set the lower part of the normal matrix equal to the upper part
360
// This is done after all the observations have been added
361
void SphereFit::setLowerPart(Matrix4x4& atpa) const
362
{
363
    for (int i = 0; i < 4; ++i) {
364
        for (int j = i + 1; j < 4; ++j) {  // skip the diagonal elements
365
            atpa(j, i) = atpa(i, j);
366
        }
367
    }
368
}
369

370
// Compute the residuals and sigma0 and check the residual convergence
371
bool SphereFit::computeResiduals(const Eigen::VectorXd& x,
372
                                 std::vector<Base::Vector3d>& residuals,
373
                                 double& sigma0,
374
                                 double vConvLimit,
375
                                 bool& vConverged) const
376
{
377
    vConverged = true;
378
    int nPtsUsed = 0;
379
    sigma0 = 0.0;
380
    double a[4] {}, b[3] {};
381
    double f0 {}, qw {};
382
    // double maxdVx = 0.0;
383
    // double maxdVy = 0.0;
384
    // double maxdVz = 0.0;
385
    // double rmsVv = 0.0;
386
    std::vector<Base::Vector3d>::iterator vIt = residuals.begin();
387
    std::list<Base::Vector3f>::const_iterator cIt;
388
    for (cIt = _vPoints.begin(); cIt != _vPoints.end(); ++cIt, ++vIt) {
389
        // if (using this point) { // currently all given points are used (could modify this if
390
        // eliminating outliers, etc....
391
        ++nPtsUsed;
392
        Base::Vector3d& v = *vIt;
393
        setupObservation(*cIt, v, a, f0, qw, b);
394
        double qv = -f0;
395
        for (int i = 0; i < 4; ++i) {
396
            qv += a[i] * x(i);
397
        }
398

399
        // We are using equal weights for sphere point coordinate observations (see
400
        // setupObservation) i.e. w[0] = w[1] = w[2] = 1.0;
401
        // double vx = -qw * qv * b[0] / w[0];
402
        // double vy = -qw * qv * b[1] / w[1];
403
        // double vz = -qw * qv * b[2] / w[2];
404
        double vx = -qw * qv * b[0];
405
        double vy = -qw * qv * b[1];
406
        double vz = -qw * qv * b[2];
407
        double dVx = fabs(vx - v.x);
408
        double dVy = fabs(vy - v.y);
409
        double dVz = fabs(vz - v.z);
410
        v.x = vx;
411
        v.y = vy;
412
        v.z = vz;
413

414
        // double vv = v.x * v.x + v.y * v.y + v.z * v.z;
415
        // rmsVv += vv * vv;
416

417
        // sigma0 += v.x * w[0] * v.x + v.y * w[1] * v.y + v.z * w[2] * v.z;
418
        sigma0 += v.x * v.x + v.y * v.y + v.z * v.z;
419

420
        if ((dVx > vConvLimit) || (dVy > vConvLimit) || (dVz > vConvLimit)) {
421
            vConverged = false;
422
        }
423

424
        // if (dVx > maxdVx)
425
        //	maxdVx = dVx;
426
        // if (dVy > maxdVy)
427
        //	maxdVy = dVy;
428
        // if (dVz > maxdVz)
429
        //	maxdVz = dVz;
430
    }
431

432
    // Compute degrees of freedom and sigma0
433
    if (nPtsUsed < 4)  // A minimum of 4 surface points is needed to define a sphere
434
    {
435
        sigma0 = 0.0;
436
        return false;
437
    }
438
    int df = nPtsUsed - 4;
439
    if (df == 0) {
440
        sigma0 = 0.0;
441
    }
442
    else {
443
        sigma0 = sqrt(sigma0 / (double)df);
444
    }
445

446
    // rmsVv = sqrt(rmsVv / (double)nPtsUsed);
447
    // Base::Console().Message("X: %0.3e %0.3e %0.3e %0.3e , Max dV: %0.4f %0.4f %0.4f , RMS Vv:
448
    // %0.4f\n", x(0), x(1), x(2), x(3), maxdVx, maxdVy, maxdVz, rmsVv);
449

450
    return true;
451
}
452