src/image.cpp


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115

#include "image.h"

#include "macro.h"
#include "pixels.h"

float process_column(const uint16_t (&column)[])
{
    start_timer(process_column);

    float result = std::numeric_limits<float>::quiet_NaN();

    constexpr uint32_t signalThreshold = 900; // = SKO * sqrt(patternSize)
    static constexpr uint32_t patternOffset = patternSize -
                                              ((patternSize % 2 == 1) ? 1 : 0);
    const uint32_t correlationSize = img_height - patternSize +
                                     ((patternSize % 2 == 1) ? 1 : 0);
    uint32_t correlation[img_height];
    uint32_t integralSum[img_height];
    uint32_t maxSum = signalThreshold * 50;
    uint32_t x1 = 0;
    int32_t y1 = 0;
    int32_t y2 = 0;

    memset(correlation, 0, img_height * sizeof(correlation[0]));
    integralSum[0] = 0;

    for (uint32_t i = 1; i < img_height; ++i)
    {
        // if (column[i] < 100)
        // {
        //     column[i] = 0;
        // }

        integralSum[i] = column[i] / 256 + integralSum[i - 1];
    }

    for (uint32_t i = 0; i < correlationSize; ++i)
        correlation[i + patternSize / 2] = column[i + patternSize / 2] / 256 *
                                           (integralSum[i + patternOffset] -
                                            integralSum[i]);

    for (uint32_t i = 3; i < img_height - 2; ++i)
    {
        const auto sum = correlation[i - 1] + correlation[i] +
                         correlation[i + 1];

        if (sum > maxSum)
        {
            const int32_t rioux0 = int32_t(correlation[i - 2 - 1] +
                                           correlation[i - 1 - 1]) -
                                   int32_t(correlation[i + 1 - 1] +
                                           correlation[i + 2 - 1]);

            if (rioux0 < 0)
            {
                const int32_t rioux1 = int32_t(correlation[i - 2] +
                                               correlation[i - 1]) -
                                       int32_t(correlation[i + 1] +
                                               correlation[i + 2]);

                if (rioux1 >= 0)
                {
                    x1 = i - 1;
                    y1 = rioux0;
                    y2 = rioux1;
                    maxSum = sum;
                }
            }
        }
    }

    result = (y2 != y1) ? (float(x1) - (float(y1) / (y2 - y1)))
                        : std::numeric_limits<float>::quiet_NaN();

    return result;
}

void Image::rotate()
{
    start_timer(rotate);

    using namespace std;

    for (size_t i = 0; i < img_height; ++i)
    {
        for (size_t j = 0; j < img_width; ++j)
        {
            rotated_cw[j][i] = data[img_height - i][j];
        }
    }

    stop_timer(rotate);
}

std::shared_ptr<Pixels> Image::pixels() const
{
    auto result = std::make_shared<Pixels>();
    result->counters = counters;

    start_timer(process_columns);

    for (size_t i = 0; i < width; i++)
    {
        result->pixels[i] = process_column(rotated_cw[i]);
    }

    // for (size_t i = 640 - 5; i < 640 + 5; ++i) {
    //     std::cout << result->pixels[i] << ' ';
    // }
    // std::cout << std::endl;

    stop_timer(process_columns);

    return result;
}