Refinement.cpp 16.2 KB
Newer Older
1
2
3
#define _USE_MATH_DEFINES
#include <cmath>

4
#include "Refinement.h"
5
#include "WrongUsageException.h"
6
#include "eigenSTLContainers.h"
7
8
#include <iostream>
#include <limits>
9
10
11
12
13
14
15

using namespace std;
using namespace Eigen;

static void roundTowardsZero(Matrix3Xf& x);
static void roundAwayFromZero(Matrix3Xf& x);

16
17
18
19
Refinement::Refinement(float tolerance)
    : tolerance(tolerance)
{
    millerIndices.reserve(500);
20
    backprojection = NULL;
21
}
22

23
24
25
26
27
28
29
30
Refinement::Refinement(float tolerance, const Backprojection& backprojection)
    : tolerance(tolerance)
    , backprojection(&backprojection)
{
    millerIndices.reserve(500);
}

void Refinement::refineVariableLattice(Lattice& lattice, const Matrix3Xf& ucsDirections, const Array2Xf& ucsBorderNorms)
31
32
33
34
35
36
37
{
    Matrix3f basis = lattice.getBasis();
    float delta = 1e-8;

    float stepSize = lattice.getBasisVectorNorms().maxCoeff() * 0.002;
    float minStepSize = lattice.getBasisVectorNorms().minCoeff() * 0.00001;
    int maxStepsCount = 200;
38
39
    meanDefects.resize(maxStepsCount);
    meanDefects[0] = getMeanDefect(basis, ucsDirections, ucsBorderNorms);
40
41
    for (int i = 0; i < maxStepsCount; i++)
    {
42
        // cout << meanDefects[i] << endl;
43
44
45
46

        Array33f gradient;
        Matrix3f offsetBasis = basis;
        offsetBasis(0, 0) += delta;
47
        gradient(0, 0) = getMeanDefect(offsetBasis, ucsDirections, ucsBorderNorms, false) - meanDefects[i];
48
49
        offsetBasis(0, 0) = basis(0, 0);
        offsetBasis(1, 0) += delta;
50
        gradient(1, 0) = getMeanDefect(offsetBasis, ucsDirections, ucsBorderNorms, false) - meanDefects[i];
51
52
        offsetBasis(1, 0) = basis(1, 0);
        offsetBasis(2, 0) += delta;
53
        gradient(2, 0) = getMeanDefect(offsetBasis, ucsDirections, ucsBorderNorms, false) - meanDefects[i];
54
55
        offsetBasis(2, 0) = basis(2, 0);
        offsetBasis(0, 1) += delta;
56
        gradient(0, 1) = getMeanDefect(offsetBasis, ucsDirections, ucsBorderNorms, false) - meanDefects[i];
57
58
        offsetBasis(0, 1) = basis(0, 1);
        offsetBasis(1, 1) += delta;
59
        gradient(1, 1) = getMeanDefect(offsetBasis, ucsDirections, ucsBorderNorms, false) - meanDefects[i];
60
61
        offsetBasis(1, 1) = basis(1, 1);
        offsetBasis(2, 1) += delta;
62
        gradient(2, 1) = getMeanDefect(offsetBasis, ucsDirections, ucsBorderNorms, false) - meanDefects[i];
63
64
        offsetBasis(2, 1) = basis(2, 1);
        offsetBasis(0, 2) += delta;
65
        gradient(0, 2) = getMeanDefect(offsetBasis, ucsDirections, ucsBorderNorms) - meanDefects[i];
66
67
        offsetBasis(0, 2) = basis(0, 2);
        offsetBasis(1, 2) += delta;
68
        gradient(1, 2) = getMeanDefect(offsetBasis, ucsDirections, ucsBorderNorms) - meanDefects[i];
69
70
        offsetBasis(1, 2) = basis(1, 2);
        offsetBasis(2, 2) += delta;
71
        gradient(2, 2) = getMeanDefect(offsetBasis, ucsDirections, ucsBorderNorms) - meanDefects[i];
72
73
74
75
76

        float norm = gradient.matrix().norm();
        gradient = gradient / norm * stepSize;
        if (norm == 0)
        {
77
            // throw WrongUsageException("Numerical problems! Delta has been chosen too small for current lattice!\n");
78
            break;
79
80
81
        }

        basis = basis - gradient.matrix();
82
        meanDefects[i + 1] = getMeanDefect(basis, ucsDirections, ucsBorderNorms);
83

84
        if (meanDefects[i + 1] > meanDefects[i])
85
86
87
        {
            stepSize = stepSize * 0.9;

88
            if (i > 10 && (meanDefects.segment(i - 4, 4).maxCoeff() - meanDefects.segment(i - 4, 4).minCoeff()) / meanDefects[i] < 0.01) // settled down
89
90
91
92
93
94
                stepSize = stepSize * 0.2;

            if (stepSize < minStepSize)
                break;
        }
    }
95

96
97
98
    lattice = Lattice(basis);
}

99
void Refinement::refineFixedLattice(Lattice& lattice, const Matrix3Xf& ucsDirections, const Array2Xf& ucsBorderNorms)
100
101
102
103
104
105
106
107
108
109
110
111
112
{
    Matrix3f basis = lattice.getBasis();

    float degreeDelta = 0.0001 / 180 * M_PI;
    Matrix3f rotX, rotY, rotZ;
    rotX = AngleAxisf(degreeDelta, Vector3f::UnitX());
    rotY = AngleAxisf(degreeDelta, Vector3f::UnitY());
    rotZ = AngleAxisf(degreeDelta, Vector3f::UnitZ());


    float stepSize = 0.1 / 180 * M_PI;
    float minStepSize = 0.001 / 180 * M_PI;
    int maxStepsCount = 200;
113
114
    meanDefects.resize(maxStepsCount);
    meanDefects[0] = getMeanDefect(basis, ucsDirections, ucsBorderNorms);
115
116
    for (int i = 0; i < maxStepsCount; i++)
    {
117
        // cout << meanDefects[i] << endl;
118
119

        Vector3f gradient;
120
121
122
        gradient(0) = getMeanDefect(rotX * basis, ucsDirections, ucsBorderNorms) - meanDefects[i];
        gradient(1) = getMeanDefect(rotY * basis, ucsDirections, ucsBorderNorms) - meanDefects[i];
        gradient(2) = getMeanDefect(rotZ * basis, ucsDirections, ucsBorderNorms) - meanDefects[i];
123
124
125

        gradient = -gradient.normalized() * stepSize;
        basis = AngleAxisf(gradient(0), Vector3f::UnitX()) * AngleAxisf(gradient(1), Vector3f::UnitY()) * AngleAxisf(gradient(2), Vector3f::UnitZ()) * basis;
126
        meanDefects[i + 1] = getMeanDefect(basis, ucsDirections, ucsBorderNorms);
127

128
        if (meanDefects[i + 1] > meanDefects[i])
129
130
131
        {
            stepSize = stepSize * 0.9;

132
            if (i > 10 && (meanDefects.segment(i - 4, 4).maxCoeff() - meanDefects.segment(i - 4, 4).minCoeff()) / meanDefects[i] < 0.001) // settled down
133
134
135
136
137
138
139
140
141
142
                break;

            if (stepSize < minStepSize)
                break;
        }
    }

    lattice = Lattice(basis);
}

143
144
145
146
147
148
149
void Refinement::refineVariableLatticeWithCenter(Lattice& lattice, Vector2f& centerShift, const Eigen::Matrix2Xf& detectorPeaks_m)
{
    Matrix3f basis = lattice.getBasis();
    float delta = 1e-8;

    float stepSize_basis = lattice.getBasisVectorNorms().maxCoeff() * 0.002;
    float minStepSize_basis = lattice.getBasisVectorNorms().minCoeff() * 0.00001;
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
150
    float startStepSize_center = 10e-6;
151
152
153
154
    int maxStepsCount = 200;
    meanDefects.resize(maxStepsCount);
    getCenterShiftedBackprojection(ucsDirections, ucsBorderNorms, detectorPeaks_m, centerShift);
    meanDefects[0] = getMeanDefect(basis, ucsDirections, ucsBorderNorms);
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
155
156
157
158
    if (meanDefects[0] == 1)
    {
        return;
    }
159
160
161
    for (int i = 0; i < maxStepsCount; i++)
    {
        // cout << meanDefects[i] << endl;
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
162
        if (i % 6 == 0)
163
        {
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
164
            refineCenter(centerShift, basis, detectorPeaks_m, startStepSize_center);
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
            startStepSize_center *= 0.85;
            getCenterShiftedBackprojection(ucsDirections, ucsBorderNorms, detectorPeaks_m, centerShift);
            meanDefects[i] = getMeanDefect(basis, ucsDirections, ucsBorderNorms);
        }

        Array33f basisGradient;
        Matrix3f offsetBasis = basis;
        offsetBasis(0, 0) += delta;
        basisGradient(0, 0) = getMeanDefect(offsetBasis, ucsDirections, ucsBorderNorms, false) - meanDefects[i];
        offsetBasis(0, 0) = basis(0, 0);
        offsetBasis(1, 0) += delta;
        basisGradient(1, 0) = getMeanDefect(offsetBasis, ucsDirections, ucsBorderNorms, false) - meanDefects[i];
        offsetBasis(1, 0) = basis(1, 0);
        offsetBasis(2, 0) += delta;
        basisGradient(2, 0) = getMeanDefect(offsetBasis, ucsDirections, ucsBorderNorms, false) - meanDefects[i];
        offsetBasis(2, 0) = basis(2, 0);
        offsetBasis(0, 1) += delta;
        basisGradient(0, 1) = getMeanDefect(offsetBasis, ucsDirections, ucsBorderNorms, false) - meanDefects[i];
        offsetBasis(0, 1) = basis(0, 1);
        offsetBasis(1, 1) += delta;
        basisGradient(1, 1) = getMeanDefect(offsetBasis, ucsDirections, ucsBorderNorms, false) - meanDefects[i];
        offsetBasis(1, 1) = basis(1, 1);
        offsetBasis(2, 1) += delta;
        basisGradient(2, 1) = getMeanDefect(offsetBasis, ucsDirections, ucsBorderNorms, false) - meanDefects[i];
        offsetBasis(2, 1) = basis(2, 1);
        offsetBasis(0, 2) += delta;
        basisGradient(0, 2) = getMeanDefect(offsetBasis, ucsDirections, ucsBorderNorms) - meanDefects[i];
        offsetBasis(0, 2) = basis(0, 2);
        offsetBasis(1, 2) += delta;
        basisGradient(1, 2) = getMeanDefect(offsetBasis, ucsDirections, ucsBorderNorms) - meanDefects[i];
        offsetBasis(1, 2) = basis(1, 2);
        offsetBasis(2, 2) += delta;
        basisGradient(2, 2) = getMeanDefect(offsetBasis, ucsDirections, ucsBorderNorms) - meanDefects[i];

        float norm = basisGradient.matrix().norm();
        basisGradient = basisGradient / norm * stepSize_basis;
        if (norm == 0)
        {
            // throw WrongUsageException("Numerical problems! Delta has been chosen too small for current lattice!\n");
            break;
        }

        basis = basis - basisGradient.matrix();
        meanDefects[i + 1] = getMeanDefect(basis, ucsDirections, ucsBorderNorms);

        if (meanDefects[i + 1] > meanDefects[i])
        {
            stepSize_basis = stepSize_basis * 0.9;

            if (i > 10 && (meanDefects.segment(i - 4, 4).maxCoeff() - meanDefects.segment(i - 4, 4).minCoeff()) / meanDefects[i] < 0.01)
            { // settled down
                stepSize_basis = stepSize_basis * 0.2;
            }

            if (stepSize_basis < minStepSize_basis)
                break;
        }
    }

    lattice = Lattice(basis);
}

Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
227
void Refinement::refineCenter(Eigen::Vector2f& centerShift, const Matrix3f& basis, const Eigen::Matrix2Xf& detectorPeaks_m, float startStepSize)
228
{
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
229
    float deltaCenterShift = 1e-7;
230

Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
231
    float minStepSize_center = 5e-7;
232
233
234
    float stepSize_center = max(startStepSize, minStepSize_center);


Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
235
236
    int maxStepsCount = 35;
    meanDefects_centerAdjustment.resize(maxStepsCount);
237
    getCenterShiftedBackprojection(ucsDirections, ucsBorderNorms, detectorPeaks_m, centerShift);
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
238
239
240
241
242
    meanDefects_centerAdjustment[0] = getMeanDefect(basis, ucsDirections, ucsBorderNorms);
    if (meanDefects_centerAdjustment[0] == 1)
    {
        return;
    }
243
244
245
246
247
248
249
    for (int i = 0; i < maxStepsCount; i++)
    {
        // cout << meanDefects[i] << endl;

        Vector2f centerOffsetGradient;
        Vector2f offsetCenterShift = centerShift + Vector2f(deltaCenterShift, 0);
        getCenterShiftedBackprojection(ucsDirections, ucsBorderNorms, detectorPeaks_m, offsetCenterShift);
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
250
        centerOffsetGradient.x() = getMeanDefect(basis, ucsDirections, ucsBorderNorms) - meanDefects_centerAdjustment[i];
251
252
        offsetCenterShift = centerShift + Vector2f(0, deltaCenterShift);
        getCenterShiftedBackprojection(ucsDirections, ucsBorderNorms, detectorPeaks_m, offsetCenterShift);
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
253
        centerOffsetGradient.y() = getMeanDefect(basis, ucsDirections, ucsBorderNorms) - meanDefects_centerAdjustment[i];
254

Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
255
        float norm = centerOffsetGradient.norm();
256
257
258
259
260
261
262
263
264
        centerOffsetGradient = centerOffsetGradient / norm * stepSize_center;
        if (norm == 0)
        {
            // throw WrongUsageException("Numerical problems! Delta has been chosen too small for current lattice!\n");
            break;
        }

        centerShift = centerShift - centerOffsetGradient;
        getCenterShiftedBackprojection(ucsDirections, ucsBorderNorms, detectorPeaks_m, centerShift);
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
265
        meanDefects_centerAdjustment[i + 1] = getMeanDefect(basis, ucsDirections, ucsBorderNorms);
266

Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
267
        if (meanDefects_centerAdjustment[i + 1] > meanDefects_centerAdjustment[i])
268
269
270
        {
            stepSize_center = stepSize_center * 0.8;

Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
271
272
273
            if (i > 5 && (meanDefects_centerAdjustment.segment(i - 4, 4).maxCoeff() - meanDefects_centerAdjustment.segment(i - 4, 4).minCoeff()) /
                                 meanDefects_centerAdjustment[i] <
                             0.01)
274
275
276
277
278
279
280
281
282
283
284
            { // settled down
                stepSize_center = stepSize_center * 0.2;
            }

            if (stepSize_center < minStepSize_center)
                break;
        }
    }
}

int Refinement::getFittedPeaksCount(Lattice& lattice, const Eigen::Matrix3Xf& ucsDirections, const Eigen::Array2Xf& ucsBorderNorms)
285
286
287
288
289
290
291
{
    getDefects(defects, lattice.getBasis(), ucsDirections, ucsBorderNorms);
    int fittedPeaksCount = (defects < tolerance).count();

    return fittedPeaksCount;
}

292
int Refinement::getFittedPeaks(Lattice& lattice, Eigen::Array<bool, Eigen::Dynamic, 1>& fittedPeaks, const Eigen::Matrix3Xf& ucsDirections,
293
                               const Eigen::Array2Xf& ucsBorderNorms)
294
295
296
297
298
299
300
{
    getDefects(defects, lattice.getBasis(), ucsDirections, ucsBorderNorms);
    fittedPeaks = (defects < tolerance);

    return fittedPeaks.count();
}

301

302
void Refinement::getDefects(ArrayXf& defects, const Matrix3f& basis, const Matrix3Xf& ucsDirections, const Array2Xf& ucsBorderNorms,
303
                            bool significantChangesToPreviousCall)
304
{
305
    Matrix3f basis_inverse = basis.inverse();
306
307
308
309
310
311
312
313
314
315

    if (significantChangesToPreviousCall)
    {
        ucsBorderNormsSquared = ucsBorderNorms.array().square().matrix();

        millerIndices_close = basis_inverse * (ucsDirections.array().rowwise() * ucsBorderNorms.array().row(0)).matrix();
        roundTowardsZero(millerIndices_close);
        millerIndices_far = basis_inverse * (ucsDirections.array().rowwise() * ucsBorderNorms.array().row(1)).matrix();
        roundAwayFromZero(millerIndices_far);
    }
316
317

    int peakCount = ucsDirections.cols();
318
    defects.resize(peakCount);
319
320
321
322
    for (int i = 0; i < peakCount; i++)
    {
        // create miller indices close to UCS
        millerIndices.clear();
323
        for (float k = min(millerIndices_close(0, i), millerIndices_far(0, i)), maxK = max(millerIndices_close(0, i), millerIndices_far(0, i)); k <= maxK; k++)
324
        {
325
326
            for (float l = min(millerIndices_close(1, i), millerIndices_far(1, i)), maxL = max(millerIndices_close(1, i), millerIndices_far(1, i)); l <= maxL;
                 l++)
327
            {
328
329
                for (float m = min(millerIndices_close(2, i), millerIndices_far(2, i)), maxM = max(millerIndices_close(2, i), millerIndices_far(2, i));
                     m <= maxM; m++)
330
331
332
333
334
335
                {
                    millerIndices.emplace_back(k, l, m);
                }
            }
        }

336
        candidatePeaks.noalias() = basis * Map<Matrix3Xf>((float*)millerIndices.data(), 3, millerIndices.size());
337
338
        candidatePeaksNormsSquared = candidatePeaks.colwise().squaredNorm();
        // clear peaks that exceed the borders
339
        validCandidatePeaksCount = 0;
340
341
        for (int j = 0, end = candidatePeaksNormsSquared.size(); j < end; j++)
        {
Yaroslav Gevorkov's avatar
bug fix    
Yaroslav Gevorkov committed
342
            if ((candidatePeaksNormsSquared(j) > ucsBorderNormsSquared(0, i)) & (candidatePeaksNormsSquared(j) < ucsBorderNormsSquared(1, i)))
343
344
345
346
347
348
349
            {
                candidatePeaks.col(validCandidatePeaksCount) = candidatePeaks.col(j);
                validCandidatePeaksCount++;
            }
        }
        if (validCandidatePeaksCount == 0)
        {
350
            defects(i) = 1;
351
352
353
            continue;
        }

354
        projectedVectorNorms.noalias() = candidatePeaks.leftCols(validCandidatePeaksCount).transpose() * ucsDirections.col(i);
355
356
357
358
        defectVectors_absolute = candidatePeaks.leftCols(validCandidatePeaksCount);
        defectVectors_absolute.noalias() -= ucsDirections.col(i) * projectedVectorNorms;
        defectVectors_relative.noalias() = basis_inverse * defectVectors_absolute;
        defects(i) = defectVectors_relative.cwiseAbs().colwise().maxCoeff().minCoeff();
359
    }
360
361
}

362
double Refinement::getMeanDefect(const Matrix3f& basis, const Matrix3Xf& ucsDirections, const Array2Xf& ucsBorderNorms, bool significantChangesToPreviousCall)
363
{
364
    getDefects(defects, basis, ucsDirections, ucsBorderNorms, significantChangesToPreviousCall);
365
366
367
368

    notPredictablePeaks = defects > tolerance;
    int16_t notPredictablePeaksCount = notPredictablePeaks.count();
    // cout << "np " << notPredictablePeaksCount << endl;
369

370
    if (notPredictablePeaksCount == defects.size())
371
    {
372
        return 1;
373
374
    }

375
    sort((float*)defects.data(), (float*)defects.data() + defects.size());
376
    return defects.head(round(0.9 * (defects.size() - notPredictablePeaksCount))).mean();
377
378
}

Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
379
380
void Refinement::getCenterShiftedBackprojection(Eigen::Matrix3Xf& ucsDirections_local, Eigen::Array2Xf& ucsBorderNorms_local,
                                                const Eigen::Matrix2Xf& detectorPeaks_m, const Eigen::Vector2f& centerShift)
381
382
{
    detectorPeaks_m_shifted = detectorPeaks_m.colwise() + centerShift;
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
383
    backprojection->backProject(detectorPeaks_m_shifted, ucsDirections_local, ucsBorderNorms_local);
384
385
}

386
387
388
389
390
391
392
393
394
static void roundTowardsZero(Matrix3Xf& x)
{
    x = x.array().abs().floor() * x.array().sign();
}

static void roundAwayFromZero(Matrix3Xf& x)
{
    x = x.array().abs().ceil() * x.array().sign();
}