FreeCAD

CylinderFit.cpp
780 строк · 28.4 Кб
Перенос по словам
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
// Definitions:
24
// Cylinder axis goes through a point (Xc,Yc,Zc) and has direction (L,M,N)
25
// Cylinder radius is R
26
// A point on the axis (X0i,Y0i,Z0i) can be described by:
27
//	(X0i,Y0i,Z0i) = (Xc,Yc,Zc) + s(L,M,N)
28
// where s is the distance from (Xc,Yc,Zc) to (X0i,Y0i,Z0i) when (L,M,N) is
29
// of unit length (normalized).
30
// The distance between a cylinder surface point (Xi,Yi,Zi) and its
31
// projection onto the axis (X0i,Y0i,Z0i) is the radius:
32
// (Xi - X0i)^2 + (Yi - Y0i)^2 + (Zi - Z0i)^2 = R^2
33
// Also the vector to a cylinder surface point (Xi,Yi,Zi) from its
34
// projection onto the axis (X0i,Y0i,Z0i) is orthogonal to the axis so we can
35
// write:
36
// (Xi - X0i, Yi - Y0i, Zi - Z0i).(L,M,N) = 0 or
37
// L(Xi - X0i) + M(Yi - Y0i) + N(Zi - Z0i) = 0
38
// If we substitute these various equations into each other and further add
39
// the constraint that L^2 + M^2 + N^2 = 1 then we can arrive at a single
40
// equation for the cylinder surface points:
41
// (Xi - Xc + L*L*(Xc - Xi) + L*M*(Yc - Yi) + L*N*(Zc - Zi))^2 +
42
// (Yi - Yc + M*L*(Xc - Xi) + M*M*(Yc - Yi) + M*N*(Zc - Zi))^2 +
43
// (Zi - Zc + N*L*(Xc - Xi) + N*M*(Yc - Yi) + N*N*(Zc - Zi))^2 - R^2 = 0
44
// This equation is what is used in the least squares solution below. Because
45
// we are constraining the direction vector to a unit length and also because
46
// we need to stop the axis point from moving along the axis we need to fix one
47
// of the ordinates in the solution. So from our initial approximations for the
48
// axis direction (L0,M0,N0):
49
//      if (L0 > M0) && (L0 > N0) then fix Xc = Mx and use L = sqrt(1 - M^2 - N^2)
50
// else if (M0 > L0) && (M0 > N0) then fix Yc = My and use M = sqrt(1 - L^2 - N^2)
51
// else if (N0 > L0) && (N0 > M0) then fix Zc = Mz and use N = sqrt(1 - L^2 - M^2)
52
// where (Mx,My,Mz) is the mean of the input points (centre of gravity)
53
// We thus solve for 5 unknown parameters.
54
// Thus for the solution to succeed the initial axis direction should be reasonable.
55

56
#include "PreCompiled.h"
57

58
#ifndef _PreComp_
59
#include <algorithm>
60
#include <cstdlib>
61
#include <iterator>
62
#endif
63

64
#include <Base/Console.h>
65
#include <Mod/Mesh/App/WildMagic4/Wm4ApprLineFit3.h>
66

67
#include "CylinderFit.h"
68

69

70
using namespace MeshCoreFit;
71

72
CylinderFit::CylinderFit()
73
    : _vBase(0, 0, 0)
74
    , _vAxis(0, 0, 1)
75
{}
76

77
// Set approximations before calling the fitting
78
void CylinderFit::SetApproximations(double radius,
79
                                    const Base::Vector3d& base,
80
                                    const Base::Vector3d& axis)
81
{
82
    _bIsFitted = false;
83
    _fLastResult = FLOAT_MAX;
84
    _numIter = 0;
85
    _dRadius = radius;
86
    _vBase = base;
87
    _vAxis = axis;
88
    _vAxis.Normalize();
89
}
90

91
// Set approximations before calling the fitting. This version computes the radius
92
// using the given axis and the existing surface points (which must already be added!)
93
void CylinderFit::SetApproximations(const Base::Vector3d& base, const Base::Vector3d& axis)
94
{
95
    _bIsFitted = false;
96
    _fLastResult = FLOAT_MAX;
97
    _numIter = 0;
98
    _vBase = base;
99
    _vAxis = axis;
100
    _vAxis.Normalize();
101
    _dRadius = 0.0;
102
    if (!_vPoints.empty()) {
103
        for (std::list<Base::Vector3f>::const_iterator cIt = _vPoints.begin();
104
             cIt != _vPoints.end();
105
             ++cIt) {
106
            _dRadius += Base::Vector3d(cIt->x, cIt->y, cIt->z).DistanceToLine(_vBase, _vAxis);
107
        }
108
        _dRadius /= (double)_vPoints.size();
109
    }
110
}
111

112
// Set iteration convergence criteria for the fit if special values are needed.
113
// The default values set in the constructor are suitable for most uses
114
void CylinderFit::SetConvergenceCriteria(double posConvLimit,
115
                                         double dirConvLimit,
116
                                         double vConvLimit,
117
                                         int maxIter)
118
{
119
    if (posConvLimit > 0.0) {
120
        _posConvLimit = posConvLimit;
121
    }
122
    if (dirConvLimit > 0.0) {
123
        _dirConvLimit = dirConvLimit;
124
    }
125
    if (vConvLimit > 0.0) {
126
        _vConvLimit = vConvLimit;
127
    }
128
    if (maxIter > 0) {
129
        _maxIter = maxIter;
130
    }
131
}
132

133

134
double CylinderFit::GetRadius() const
135
{
136
    if (_bIsFitted) {
137
        return _dRadius;
138
    }
139
    else {
140
        return 0.0;
141
    }
142
}
143

144
Base::Vector3d CylinderFit::GetBase() const
145
{
146
    if (_bIsFitted) {
147
        return _vBase;
148
    }
149
    else {
150
        return Base::Vector3d();
151
    }
152
}
153

154
Base::Vector3d CylinderFit::GetAxis() const
155
{
156
    if (_bIsFitted) {
157
        return _vAxis;
158
    }
159
    else {
160
        return Base::Vector3d();
161
    }
162
}
163

164
int CylinderFit::GetNumIterations() const
165
{
166
    if (_bIsFitted) {
167
        return _numIter;
168
    }
169
    else {
170
        return 0;
171
    }
172
}
173

174
float CylinderFit::GetDistanceToCylinder(const Base::Vector3f& rcPoint) const
175
{
176
    float fResult = FLOAT_MAX;
177
    if (_bIsFitted) {
178
        fResult = Base::Vector3d(rcPoint.x, rcPoint.y, rcPoint.z).DistanceToLine(_vBase, _vAxis)
179
            - _dRadius;
180
    }
181
    return fResult;
182
}
183

184
float CylinderFit::GetStdDeviation() const
185
{
186
    // Mean: M=(1/N)*SUM Xi
187
    // Variance: VAR=(N/N-1)*[(1/N)*SUM(Xi^2)-M^2]
188
    // Standard deviation: SD=SQRT(VAR)
189
    if (!_bIsFitted) {
190
        return FLOAT_MAX;
191
    }
192

193
    double sumXi = 0.0, sumXi2 = 0.0, dist = 0.0;
194
    for (auto it : _vPoints) {
195
        dist = GetDistanceToCylinder(it);
196
        sumXi += dist;
197
        sumXi2 += (dist * dist);
198
    }
199

200
    double N = static_cast<double>(CountPoints());
201
    double mean = sumXi / N;
202
    return sqrt((N / (N - 1.0)) * (sumXi2 / N - mean * mean));
203
}
204

205
void CylinderFit::ProjectToCylinder()
206
{
207
    Base::Vector3f cBase(_vBase.x, _vBase.y, _vBase.z);
208
    Base::Vector3f cAxis(_vAxis.x, _vAxis.y, _vAxis.z);
209

210
    for (auto& cPnt : _vPoints) {
211
        if (cPnt.DistanceToLine(cBase, cAxis) > 0) {
212
            Base::Vector3f proj;
213
            cBase.ProjectToPlane(cPnt, cAxis, proj);
214
            Base::Vector3f diff = cPnt - proj;
215
            diff.Normalize();
216
            cPnt = proj + diff * _dRadius;
217
        }
218
        else {
219
            // Point is on the cylinder axis, so it can be moved in
220
            // any direction perpendicular to the cylinder axis
221
            Base::Vector3f cMov(cPnt);
222
            do {
223
                float x = (float(rand()) / float(RAND_MAX));
224
                float y = (float(rand()) / float(RAND_MAX));
225
                float z = (float(rand()) / float(RAND_MAX));
226
                cMov.Move(x, y, z);
227
            } while (cMov.DistanceToLine(cBase, cAxis) == 0);
228

229
            Base::Vector3f proj;
230
            cMov.ProjectToPlane(cPnt, cAxis, proj);
231
            Base::Vector3f diff = cPnt - proj;
232
            diff.Normalize();
233
            cPnt = proj + diff * _dRadius;
234
        }
235
    }
236
}
237

238
// Compute approximations for the parameters using all points by computing a
239
// line through the points. This doesn't work well if the points are only from
240
// one small surface area.
241
// In that case rather use SetApproximations() with a better estimate.
242
void CylinderFit::ComputeApproximationsLine()
243
{
244
    _bIsFitted = false;
245
    _fLastResult = FLOAT_MAX;
246
    _numIter = 0;
247
    _vBase.Set(0.0, 0.0, 0.0);
248
    _vAxis.Set(0.0, 0.0, 0.0);
249
    _dRadius = 0.0;
250
    if (!_vPoints.empty()) {
251
        std::vector<Wm4::Vector3d> input;
252
        std::transform(_vPoints.begin(),
253
                       _vPoints.end(),
254
                       std::back_inserter(input),
255
                       [](const Base::Vector3f& v) {
256
                           return Wm4::Vector3d(v.x, v.y, v.z);
257
                       });
258
        Wm4::Line3<double> kLine = Wm4::OrthogonalLineFit3(input.size(), input.data());
259
        _vBase.Set(kLine.Origin.X(), kLine.Origin.Y(), kLine.Origin.Z());
260
        _vAxis.Set(kLine.Direction.X(), kLine.Direction.Y(), kLine.Direction.Z());
261

262
        for (std::list<Base::Vector3f>::const_iterator cIt = _vPoints.begin();
263
             cIt != _vPoints.end();
264
             ++cIt) {
265
            _dRadius += Base::Vector3d(cIt->x, cIt->y, cIt->z).DistanceToLine(_vBase, _vAxis);
266
        }
267
        _dRadius /= (double)_vPoints.size();
268
    }
269
}
270

271
float CylinderFit::Fit()
272
{
273
    _bIsFitted = false;
274
    _fLastResult = FLOAT_MAX;
275
    _numIter = 0;
276

277
    // A minimum of 5 surface points is needed to define a cylinder
278
    if (CountPoints() < 5) {
279
        return FLOAT_MAX;
280
    }
281

282
    // If approximations have not been set/computed then compute some now using the line fit method
283
    if (_dRadius == 0.0) {
284
        ComputeApproximationsLine();
285
    }
286

287
    // Check parameters to define the best solution direction
288
    // There are 7 parameters but 2 are actually dependent on the others
289
    // so we are actually solving for 5 parameters.
290
    // order of parameters depending on the solution direction:
291
    //		solL:	Yc, Zc, M, N, R
292
    //		solM:	Xc, Zc, L, N, R
293
    //		solN:	Xc, Yc, L, M, R
294
    SolutionD solDir {};
295
    findBestSolDirection(solDir);
296

297
    // Initialise some matrices and vectors
298
    std::vector<Base::Vector3d> residuals(CountPoints(), Base::Vector3d(0.0, 0.0, 0.0));
299
    Matrix5x5 atpa;
300
    Eigen::VectorXd atpl(5);
301

302
    // Iteration loop...
303
    double sigma0 {};
304
    bool cont = true;
305
    while (cont && (_numIter < _maxIter)) {
306
        ++_numIter;
307

308
        // Set up the quasi parametric normal equations
309
        setupNormalEquationMatrices(solDir, residuals, atpa, atpl);
310

311
        // Solve the equations for the unknown corrections
312
        Eigen::LLT<Matrix5x5> llt(atpa);
313
        if (llt.info() != Eigen::Success) {
314
            return FLOAT_MAX;
315
        }
316
        Eigen::VectorXd x = llt.solve(atpl);
317

318
        // Check parameter convergence
319
        cont = false;
320
        if ((fabs(x(0)) > _posConvLimit) || (fabs(x(1)) > _posConvLimit)
321
            ||  // the two position parameter corrections
322
            (fabs(x(2)) > _dirConvLimit) || (fabs(x(3)) > _dirConvLimit)
323
            ||                               // the two direction parameter corrections
324
            (fabs(x(4)) > _posConvLimit)) {  // the radius correction
325
            cont = true;
326
        }
327

328
        // Before updating the unknowns, compute the residuals and sigma0 and check the residual
329
        // convergence
330
        bool vConverged {};
331
        if (!computeResiduals(solDir, x, residuals, sigma0, _vConvLimit, vConverged)) {
332
            return FLOAT_MAX;
333
        }
334
        if (!vConverged) {
335
            cont = true;
336
        }
337

338
        // Update the parameters
339
        if (!updateParameters(solDir, x)) {
340
            return FLOAT_MAX;
341
        }
342
    }
343

344
    // Check for convergence
345
    if (cont) {
346
        return FLOAT_MAX;
347
    }
348

349
    _bIsFitted = true;
350
    _fLastResult = sigma0;
351

352
    return _fLastResult;
353
}
354

355
// Checks initial parameter values and defines the best solution direction to use
356
// Axis direction = (L,M,N)
357
// solution L: L is biggest axis component and L = f(M,N) and X = Mx (we move the base point along
358
// axis to this x-value) solution M: M is biggest axis component and M = f(L,N) and Y = My (we move
359
// the base point along axis to this y-value) solution N: N is biggest axis component and N = f(L,M)
360
// and Z = Mz (we move the base point along axis to this z-value) where (Mx,My,Mz) is the mean of
361
// the input points (centre of gravity)
362
void CylinderFit::findBestSolDirection(SolutionD& solDir)
363
{
364
    // Choose the best of the three solution 'directions' to use
365
    // This is to avoid a square root of a negative number when computing the dependent parameters
366
    Base::Vector3d dir = _vAxis;
367
    Base::Vector3d pos = _vBase;
368
    dir.Normalize();
369
    double biggest = dir.x;
370
    solDir = solL;
371
    if (fabs(dir.y) > fabs(biggest)) {
372
        biggest = dir.y;
373
        solDir = solM;
374
    }
375
    if (fabs(dir.z) > fabs(biggest)) {
376
        biggest = dir.z;
377
        solDir = solN;
378
    }
379
    if (biggest < 0.0) {
380
        dir.Set(-dir.x, -dir.y, -dir.z);  // multiplies by -1
381
    }
382

383
    double fixedVal = 0.0;
384
    double lambda;
385
    switch (solDir) {
386
        case solL:
387
            fixedVal = meanXObs();
388
            lambda = (fixedVal - pos.x) / dir.x;
389
            pos.x = fixedVal;
390
            pos.y = pos.y + lambda * dir.y;
391
            pos.z = pos.z + lambda * dir.z;
392
            break;
393
        case solM:
394
            fixedVal = meanYObs();
395
            lambda = (fixedVal - pos.y) / dir.y;
396
            pos.x = pos.x + lambda * dir.x;
397
            pos.y = fixedVal;
398
            pos.z = pos.z + lambda * dir.z;
399
            break;
400
        case solN:
401
            fixedVal = meanZObs();
402
            lambda = (fixedVal - pos.z) / dir.z;
403
            pos.x = pos.x + lambda * dir.x;
404
            pos.y = pos.y + lambda * dir.y;
405
            pos.z = fixedVal;
406
            break;
407
    }
408
    _vAxis = dir;
409
    _vBase = pos;
410
}
411

412
double CylinderFit::meanXObs()
413
{
414
    double mx = 0.0;
415
    if (!_vPoints.empty()) {
416
        for (std::list<Base::Vector3f>::const_iterator cIt = _vPoints.begin();
417
             cIt != _vPoints.end();
418
             ++cIt) {
419
            mx += cIt->x;
420
        }
421
        mx /= double(_vPoints.size());
422
    }
423
    return mx;
424
}
425

426
double CylinderFit::meanYObs()
427
{
428
    double my = 0.0;
429
    if (!_vPoints.empty()) {
430
        for (std::list<Base::Vector3f>::const_iterator cIt = _vPoints.begin();
431
             cIt != _vPoints.end();
432
             ++cIt) {
433
            my += cIt->y;
434
        }
435
        my /= double(_vPoints.size());
436
    }
437
    return my;
438
}
439

440
double CylinderFit::meanZObs()
441
{
442
    double mz = 0.0;
443
    if (!_vPoints.empty()) {
444
        for (std::list<Base::Vector3f>::const_iterator cIt = _vPoints.begin();
445
             cIt != _vPoints.end();
446
             ++cIt) {
447
            mz += cIt->z;
448
        }
449
        mz /= double(_vPoints.size());
450
    }
451
    return mz;
452
}
453

454
// Set up the normal equation matrices
455
// atpa ... 5x5 normal matrix
456
// atpl ... 5x1 matrix (right-hand side of equation)
457
void CylinderFit::setupNormalEquationMatrices(SolutionD solDir,
458
                                              const std::vector<Base::Vector3d>& residuals,
459
                                              Matrix5x5& atpa,
460
                                              Eigen::VectorXd& atpl) const
461
{
462
    // Zero matrices
463
    atpa.setZero();
464
    atpl.setZero();
465

466
    // For each point, setup the observation equation coefficients and add their
467
    // contribution into the normal equation matrices
468
    double a[5] {}, b[3] {};
469
    double f0 {}, qw {};
470
    std::vector<Base::Vector3d>::const_iterator vIt = residuals.begin();
471
    std::list<Base::Vector3f>::const_iterator cIt;
472
    for (cIt = _vPoints.begin(); cIt != _vPoints.end(); ++cIt, ++vIt) {
473
        // if (using this point) { // currently all given points are used (could modify this if
474
        // eliminating outliers, etc....
475
        setupObservation(solDir, *cIt, *vIt, a, f0, qw, b);
476
        addObservationU(a, f0, qw, atpa, atpl);
477
        // }
478
    }
479
    setLowerPart(atpa);
480
}
481

482
// Sets up contributions of given observation to the quasi parametric
483
// normal equation matrices. Assumes uncorrelated coordinates.
484
// point ... point
485
// residual ... residual for this point computed from previous iteration (zero for first iteration)
486
// a[5] ... parameter partials
487
// f0   ... reference to f0 term
488
// qw   ... reference to quasi weight (here we are assuming equal unit weights for each observed
489
// point coordinate) b[3] ... observation partials
490
void CylinderFit::setupObservation(SolutionD solDir,
491
                                   const Base::Vector3f& point,
492
                                   const Base::Vector3d& residual,
493
                                   double a[5],
494
                                   double& f0,
495
                                   double& qw,
496
                                   double b[3]) const
497
{
498
    // This adjustment requires an update of the observation approximations
499
    // because the residuals do not have a linear relationship.
500
    // New estimates for the observations:
501
    double xEstimate = (double)point.x + residual.x;
502
    double yEstimate = (double)point.y + residual.y;
503
    double zEstimate = (double)point.z + residual.z;
504

505
    // intermediate parameters
506
    double lambda = _vAxis.x * (xEstimate - _vBase.x) + _vAxis.y * (yEstimate - _vBase.y)
507
        + _vAxis.z * (zEstimate - _vBase.z);
508
    double x0 = _vBase.x + lambda * _vAxis.x;
509
    double y0 = _vBase.y + lambda * _vAxis.y;
510
    double z0 = _vBase.z + lambda * _vAxis.z;
511
    double dx = xEstimate - x0;
512
    double dy = yEstimate - y0;
513
    double dz = zEstimate - z0;
514
    double dx00 = _vBase.x - xEstimate;
515
    double dy00 = _vBase.y - yEstimate;
516
    double dz00 = _vBase.z - zEstimate;
517

518
    // partials of the observations
519
    b[0] =
520
        2.0 * (dx - _vAxis.x * _vAxis.x * dx - _vAxis.x * _vAxis.y * dy - _vAxis.x * _vAxis.z * dz);
521
    b[1] =
522
        2.0 * (dy - _vAxis.x * _vAxis.y * dx - _vAxis.y * _vAxis.y * dy - _vAxis.y * _vAxis.z * dz);
523
    b[2] =
524
        2.0 * (dz - _vAxis.x * _vAxis.z * dx - _vAxis.y * _vAxis.z * dy - _vAxis.z * _vAxis.z * dz);
525

526
    double ddxdl {}, ddydl {}, ddzdl {};
527
    double ddxdm {}, ddydm {}, ddzdm {};
528
    double ddxdn {}, ddydn {}, ddzdn {};
529

530
    // partials of the parameters
531
    switch (solDir) {
532
        case solL:
533
            // order of parameters: Yc, Zc, M, N, R
534
            ddxdm = -2.0 * _vAxis.y * dx00 + (_vAxis.x - _vAxis.y * _vAxis.y / _vAxis.x) * dy00
535
                - (_vAxis.y * _vAxis.z / _vAxis.x) * dz00;
536
            ddydm = (_vAxis.x - _vAxis.y * _vAxis.y / _vAxis.x) * dx00 + 2.0 * _vAxis.y * dy00
537
                + _vAxis.z * dz00;
538
            ddzdm = -(_vAxis.y * _vAxis.z / _vAxis.x) * dx00 + _vAxis.z * dy00;
539
            ddxdn = -2.0 * _vAxis.z * dx00 - (_vAxis.y * _vAxis.z / _vAxis.x) * dy00
540
                + (_vAxis.x - _vAxis.z * _vAxis.z / _vAxis.x) * dz00;
541
            ddydn = -(_vAxis.y * _vAxis.z / _vAxis.x) * dx00 + _vAxis.y * dz00;
542
            ddzdn = (_vAxis.x - _vAxis.z * _vAxis.z / _vAxis.x) * dx00 + _vAxis.y * dy00
543
                + 2.0 * _vAxis.z * dz00;
544
            a[0] = -b[1];
545
            a[1] = -b[2];
546
            a[2] = 2.0 * (dx * ddxdm + dy * ddydm + dz * ddzdm);
547
            a[3] = 2.0 * (dx * ddxdn + dy * ddydn + dz * ddzdn);
548
            a[4] = -2.0 * _dRadius;
549
            break;
550
        case solM:
551
            // order of parameters: Xc, Zc, L, N, R
552
            ddxdl = 2.0 * _vAxis.x * dx00 + (_vAxis.y - _vAxis.x * _vAxis.x / _vAxis.y) * dy00
553
                + _vAxis.z * dz00;
554
            ddydl = (_vAxis.y - _vAxis.x * _vAxis.x / _vAxis.y) * dx00 - 2.0 * _vAxis.x * dy00
555
                - (_vAxis.x * _vAxis.z / _vAxis.y) * dz00;
556
            ddzdl = _vAxis.z * dx00 - (_vAxis.x * _vAxis.z / _vAxis.y) * dy00;
557
            ddxdn = -(_vAxis.x * _vAxis.z / _vAxis.y) * dy00 + _vAxis.x * dz00;
558
            ddydn = -(_vAxis.x * _vAxis.z / _vAxis.y) * dx00 - 2.0 * _vAxis.z * dy00
559
                + (_vAxis.y - _vAxis.z * _vAxis.z / _vAxis.y) * dz00;
560
            ddzdn = _vAxis.x * dx00 + (_vAxis.y - _vAxis.z * _vAxis.z / _vAxis.y) * dy00
561
                + 2.0 * _vAxis.z * dz00;
562
            a[0] = -b[0];
563
            a[1] = -b[2];
564
            a[2] = 2.0 * (dx * ddxdl + dy * ddydl + dz * ddzdl);
565
            a[3] = 2.0 * (dx * ddxdn + dy * ddydn + dz * ddzdn);
566
            a[4] = -2.0 * _dRadius;
567
            break;
568
        case solN:
569
            // order of parameters: Xc, Yc, L, M, R
570
            ddxdl = 2.0 * _vAxis.x * dx00 + _vAxis.y * dy00
571
                + (_vAxis.z - _vAxis.x * _vAxis.x / _vAxis.z) * dz00;
572
            ddydl = _vAxis.y * dx00 - (_vAxis.x * _vAxis.y / _vAxis.z) * dz00;
573
            ddzdl = (_vAxis.z - _vAxis.x * _vAxis.x / _vAxis.z) * dx00
574
                - (_vAxis.x * _vAxis.y / _vAxis.z) * dy00 - 2.0 * _vAxis.x * dz00;
575
            ddxdm = _vAxis.x * dy00 - (_vAxis.x * _vAxis.y / _vAxis.z) * dz00;
576
            ddydm = _vAxis.x * dx00 + 2.0 * _vAxis.y * dy00
577
                + (_vAxis.z - _vAxis.y * _vAxis.y / _vAxis.z) * dz00;
578
            ddzdm = -(_vAxis.x * _vAxis.y / _vAxis.z) * dx00
579
                + (_vAxis.z - _vAxis.y * _vAxis.y / _vAxis.z) * dy00 - 2.0 * _vAxis.y * dz00;
580
            a[0] = -b[0];
581
            a[1] = -b[1];
582
            a[2] = 2.0 * (dx * ddxdl + dy * ddydl + dz * ddzdl);
583
            a[3] = 2.0 * (dx * ddxdm + dy * ddydm + dz * ddzdm);
584
            a[4] = -2.0 * _dRadius;
585
            break;
586
    }
587

588
    // free term
589
    f0 = _dRadius * _dRadius - dx * dx - dy * dy - dz * dz + b[0] * residual.x + b[1] * residual.y
590
        + b[2] * residual.z;
591

592
    // quasi weight (using equal weights for cylinder point coordinate observations)
593
    // w[0] = 1.0;
594
    // w[1] = 1.0;
595
    // w[2] = 1.0;
596
    // qw = 1.0 / (b[0] * b[0] / w[0] + b[1] * b[1] / w[1] + b[2] * b[2] / w[2]);
597
    qw = 1.0 / (b[0] * b[0] + b[1] * b[1] + b[2] * b[2]);
598
}
599

600
// Computes contribution of the given observation equation on the normal equation matrices
601
// Call this for each observation (point)
602
// Here we only add the contribution to  the upper part of the normal matrix
603
// and then after all observations have been added we need to set the lower part
604
// (which is symmetrical to the upper part)
605
// a[5] ... parameter partials
606
// li   ... free term (f0)
607
// pi   ... weight of observation (= quasi weight qw for this solution)
608
// atpa ... 5x5 normal equation matrix
609
// atpl ... 5x1 matrix/vector (right-hand side of equations)
610
void CylinderFit::addObservationU(double a[5],
611
                                  double li,
612
                                  double pi,
613
                                  Matrix5x5& atpa,
614
                                  Eigen::VectorXd& atpl) const
615
{
616
    for (int i = 0; i < 5; ++i) {
617
        double aipi = a[i] * pi;
618
        for (int j = i; j < 5; ++j) {
619
            atpa(i, j) += aipi * a[j];
620
            // atpa(j, i) = atpa(i, j);	// it's a symmetrical matrix, we'll set this later after all
621
            // observations processed
622
        }
623
        atpl(i) += aipi * li;
624
    }
625
}
626

627
// Set the lower part of the normal matrix equal to the upper part
628
// This is done after all the observations have been added
629
void CylinderFit::setLowerPart(Matrix5x5& atpa) const
630
{
631
    for (int i = 0; i < 5; ++i) {
632
        for (int j = i + 1; j < 5; ++j) {  // skip the diagonal elements
633
            atpa(j, i) = atpa(i, j);
634
        }
635
    }
636
}
637

638
// Compute the residuals and sigma0 and check the residual convergence
639
bool CylinderFit::computeResiduals(SolutionD solDir,
640
                                   const Eigen::VectorXd& x,
641
                                   std::vector<Base::Vector3d>& residuals,
642
                                   double& sigma0,
643
                                   double vConvLimit,
644
                                   bool& vConverged) const
645
{
646
    vConverged = true;
647
    int nPtsUsed = 0;
648
    sigma0 = 0.0;
649
    double a[5] {}, b[3] {};
650
    double f0 {}, qw {};
651
    // double maxdVx = 0.0;
652
    // double maxdVy = 0.0;
653
    // double maxdVz = 0.0;
654
    // double rmsVv = 0.0;
655
    std::vector<Base::Vector3d>::iterator vIt = residuals.begin();
656
    std::list<Base::Vector3f>::const_iterator cIt;
657
    for (cIt = _vPoints.begin(); cIt != _vPoints.end(); ++cIt, ++vIt) {
658
        // if (using this point) { // currently all given points are used (could modify this if
659
        // eliminating outliers, etc....
660
        ++nPtsUsed;
661
        Base::Vector3d& v = *vIt;
662
        setupObservation(solDir, *cIt, v, a, f0, qw, b);
663
        double qv = -f0;
664
        for (int i = 0; i < 5; ++i) {
665
            qv += a[i] * x(i);
666
        }
667

668
        // We are using equal weights for cylinder point coordinate observations (see
669
        // setupObservation) i.e. w[0] = w[1] = w[2] = 1.0;
670
        // double vx = -qw * qv * b[0] / w[0];
671
        // double vy = -qw * qv * b[1] / w[1];
672
        // double vz = -qw * qv * b[2] / w[2];
673
        double vx = -qw * qv * b[0];
674
        double vy = -qw * qv * b[1];
675
        double vz = -qw * qv * b[2];
676
        double dVx = fabs(vx - v.x);
677
        double dVy = fabs(vy - v.y);
678
        double dVz = fabs(vz - v.z);
679
        v.x = vx;
680
        v.y = vy;
681
        v.z = vz;
682

683
        // double vv = v.x * v.x + v.y * v.y + v.z * v.z;
684
        // rmsVv += vv * vv;
685

686
        // sigma0 += v.x * w[0] * v.x + v.y * w[1] * v.y + v.z * w[2] * v.z;
687
        sigma0 += v.x * v.x + v.y * v.y + v.z * v.z;
688

689
        if ((dVx > vConvLimit) || (dVy > vConvLimit) || (dVz > vConvLimit)) {
690
            vConverged = false;
691
        }
692

693
        // if (dVx > maxdVx)
694
        //	maxdVx = dVx;
695
        // if (dVy > maxdVy)
696
        //	maxdVy = dVy;
697
        // if (dVz > maxdVz)
698
        //	maxdVz = dVz;
699
    }
700

701
    // Compute degrees of freedom and sigma0
702
    if (nPtsUsed < 5)  // A minimum of 5 surface points is needed to define a cylinder
703
    {
704
        sigma0 = 0.0;
705
        return false;
706
    }
707
    int df = nPtsUsed - 5;
708
    if (df == 0) {
709
        sigma0 = 0.0;
710
    }
711
    else {
712
        sigma0 = sqrt(sigma0 / (double)df);
713
    }
714

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

719
    return true;
720
}
721

722
// Update the parameters after solving the normal equations
723
bool CylinderFit::updateParameters(SolutionD solDir, const Eigen::VectorXd& x)
724
{
725
    // Update the parameters used as unknowns in the solution
726
    switch (solDir) {
727
        case solL:  // order of parameters: Yc, Zc, M, N, R
728
            _vBase.y += x(0);
729
            _vBase.z += x(1);
730
            _vAxis.y += x(2);
731
            _vAxis.z += x(3);
732
            _dRadius += x(4);
733
            break;
734
        case solM:  // order of parameters: Xc, Zc, L, N, R
735
            _vBase.x += x(0);
736
            _vBase.z += x(1);
737
            _vAxis.x += x(2);
738
            _vAxis.z += x(3);
739
            _dRadius += x(4);
740
            break;
741
        case solN:  // order of parameters: Xc, Yc, L, M, R
742
            _vBase.x += x(0);
743
            _vBase.y += x(1);
744
            _vAxis.x += x(2);
745
            _vAxis.y += x(3);
746
            _dRadius += x(4);
747
            break;
748
    }
749

750
    // Update the dependent axis direction parameter
751
    double l2 {}, m2 {}, n2 {};
752
    switch (solDir) {
753
        case solL:
754
            l2 = 1.0 - _vAxis.y * _vAxis.y - _vAxis.z * _vAxis.z;
755
            if (l2 <= 0.0) {
756
                return false;  // L*L <= 0 !
757
            }
758
            _vAxis.x = sqrt(l2);
759
            //_vBase.x is fixed
760
            break;
761
        case solM:
762
            m2 = 1.0 - _vAxis.x * _vAxis.x - _vAxis.z * _vAxis.z;
763
            if (m2 <= 0.0) {
764
                return false;  // M*M <= 0 !
765
            }
766
            _vAxis.y = sqrt(m2);
767
            //_vBase.y is fixed
768
            break;
769
        case solN:
770
            n2 = 1.0 - _vAxis.x * _vAxis.x - _vAxis.y * _vAxis.y;
771
            if (n2 <= 0.0) {
772
                return false;  // N*N <= 0 !
773
            }
774
            _vAxis.z = sqrt(n2);
775
            //_vBase.z is fixed
776
            break;
777
    }
778

779
    return true;
780
}
781
FreeCAD

Использование cookies
