broken img calc; change dir struct

1 files changed, 465 insertions, 0 deletions

diff --git a/src/imagealgos.cpp b/src/imagealgos.cpp
new file mode 100644
index 0000000..2257244
--- /dev/null
+++ b/src/imagealgos.cpp
@@ -0,0 +1,465 @@
+#include "imagealgos.h"
+
+#include <QDebug>
+
+#include <cassert>
+#include <cstdint>
+#include <cstring>
+
+#include <algorithm>
+#include <iostream>
+#include <limits>
+#include <mutex>
+#include <utility>
+
+#include "macro.h"
+
+uint8_t pgm_image[64 + img_width * img_height * sizeof(uint8_t)];
+size_t pgm_image_size = 0;
+std::mutex pgm_image_mtx;
+
+template<typename T>
+T median3(const T &a, const T &b, const T &c)
+{
+    using namespace std;
+    return max(min(a, b), min(max(a, b), c));
+}
+
+// size_t pgm_save(std::shared_ptr<Image> img, FILE *outfile, bool really_save)
+size_t pgm_save(Image *img, FILE *outfile, bool really_save)
+{
+    std::lock_guard<std::mutex> lg(pgm_image_mtx);
+
+    size_t n{0};
+
+    n += sprintf((char*)pgm_image, "P5\n%d %d\n%d\n",
+                 img->width, img->height, 0xFF);
+
+    memcpy(pgm_image + n, img->data.data(), sizeof(img->data));
+    // memcpy(pgm_image + n, img->data->data(), sizeof(img->data));
+    n += sizeof(img->data);
+    pgm_image_size = n;
+
+    if (really_save)
+    {
+        if (outfile) {
+            fwrite(pgm_image, 1, pgm_image_size, outfile);
+            fflush(outfile);
+
+        } else {
+            std::cerr << __func__ << ": output filename is nullptr, cannot save"
+                      << std::endl;
+        }
+    }
+
+    // std::cout << "written pgm image" << std::endl;
+    return n;
+}
+
+// void unpack_10bit(uint8_t const *src, Image const &image, uint16_t *dest)
+// {
+//     unsigned int w_align = image.width & ~3;
+//     for (unsigned int y = 0; y < image.height; y++, src += image.stride)
+//     {
+//         uint8_t const *ptr = src;
+//         unsigned int x;
+//         for (x = 0; x < w_align; x += 4, ptr += 5)
+//         {
+//             *dest++ = (ptr[0] << 2) | ((ptr[4] >> 0) & 3);
+//             *dest++ = (ptr[1] << 2) | ((ptr[4] >> 2) & 3);
+//             *dest++ = (ptr[2] << 2) | ((ptr[4] >> 4) & 3);
+//             *dest++ = (ptr[3] << 2) | ((ptr[4] >> 6) & 3);
+//         }
+//         for (; x < image.width; x++)
+//             *dest++ = (ptr[x & 3] << 2) | ((ptr[4] >> ((x & 3) << 1)) & 3);
+//     }
+// }
+
+// void unpack_16bit(uint8_t const *src, Image const &image, uint16_t *dest)
+// {
+//     start_timer(unpack_16bit);
+//     /* Assume the pixels in memory are already in native byte order */
+//     unsigned int w = image.width;
+
+//     for (unsigned int y = 0; y < image.height; y++)
+//     {
+//         memcpy(dest, src, 2 * w);
+//         dest += w;
+//         src += image.stride;
+//     }
+//     stop_timer(unpack_16bit);
+// }
+
+template<class T, size_t N>
+constexpr size_t mysize(T (&)[N]) { return N; }
+
+// void smooth_column(uint8_t (&column)[])
+// {
+//     for (size_t i = 1; i < img_height - 1; ++i) {
+//         column[i] = median3(column[i - 1], column[i], column[i + 1]);
+//     }
+// }
+
+// float process_column(uint8_t (&column)[])
+// {
+//     std::cout << "aaaaaaaaaaa\n";
+//     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 + 0] < 100) { column[i + 0] = 0; } integralSum[i + 0] = column[i + 0] / 256 + integralSum[i + 0 - 1];
+//         // if (column[i + 1] < 100) { column[i + 1] = 0; } integralSum[i + 1] = column[i + 1] / 256 + integralSum[i + 1 - 1];
+//         // if (column[i + 2] < 100) { column[i + 2] = 0; } integralSum[i + 2] = column[i + 2] / 256 + integralSum[i + 2 - 1];
+//         // if (column[i + 3] < 100) { column[i + 3] = 0; } integralSum[i + 3] = column[i + 3] / 256 + integralSum[i + 3 - 1];
+//         // if (column[i + 4] < 100) { column[i + 4] = 0; } integralSum[i + 4] = column[i + 4] / 256 + integralSum[i + 4 - 1];
+//         // if (column[i + 5] < 100) { column[i + 5] = 0; } integralSum[i + 5] = column[i + 5] / 256 + integralSum[i + 5 - 1];
+//         // if (column[i + 6] < 100) { column[i + 6] = 0; } integralSum[i + 6] = column[i + 6] / 256 + integralSum[i + 6 - 1];
+//         // if (column[i + 7] < 100) { column[i + 7] = 0; } integralSum[i + 7] = column[i + 7] / 256 + integralSum[i + 7 - 1];
+//         // if (column[i + 8] < 100) { column[i + 8] = 0; } integralSum[i + 8] = column[i + 8] / 256 + integralSum[i + 8 - 1];
+//         // if (column[i + 9] < 100) { column[i + 9] = 0; } integralSum[i + 9] = column[i + 9] / 256 + integralSum[i + 9 - 1];
+//         // if (column[i + 10] < 100) { column[i + 10] = 0; } integralSum[i + 10] = column[i + 10] / 256 + integralSum[i + 10 - 1];
+//         // if (column[i + 11] < 100) { column[i + 11] = 0; } integralSum[i + 11] = column[i + 11] / 256 + integralSum[i + 11 - 1];
+//         // if (column[i + 12] < 100) { column[i + 12] = 0; } integralSum[i + 12] = column[i + 12] / 256 + integralSum[i + 12 - 1];
+//         // if (column[i + 13] < 100) { column[i + 13] = 0; } integralSum[i + 13] = column[i + 13] / 256 + integralSum[i + 13 - 1];
+//         // if (column[i + 14] < 100) { column[i + 14] = 0; } integralSum[i + 14] = column[i + 14] / 256 + integralSum[i + 14 - 1];
+//         // if (column[i + 15] < 100) { column[i + 15] = 0; } integralSum[i + 15] = column[i + 15] / 256 + integralSum[i + 15 - 1];
+
+//         if (column[i] < 100) {
+//             column[i] = 0;
+//         }
+//         integralSum[i] = column[i] + integralSum[i - 1];
+//     }
+
+//     // maxSum = 0 ;
+//     // size_t maxIdx { 0 };
+
+//     // for (size_t i = 0; i < img_height - patternSize; ++i) {
+//     //     const auto sum = integralSum[i + patternSize] - integralSum[i];
+//     //     if (sum > maxSum) {
+//     //         maxSum = sum;
+//     //         maxIdx = i;
+//     //     }
+//     // }
+
+//     // Algo genetic(column + maxIdx);
+//     // // std::cout << "maxIdx " << maxIdx << std::endl;
+
+//     // // return maxIdx + genetic.run().a;
+//     // return 500;
+//     // return img_height - maxIdx - genetic.run().a;
+
+//     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();
+
+//     static bool result_done = false;
+//     if (!result_done) {
+//         std::cout << "result " << result << std::endl;
+//         result_done = true;
+//     }
+//     // std::cout << "result is '" << result << "'\n";
+
+//     // stop_timer(process_column);
+
+//     return result;
+
+// // center of mass
+// #if 0
+//     auto max_el = std::max_element(std::begin(accumulated_sum),
+//                                    std::end(accumulated_sum) - window_size);
+
+//     size_t max_sum_idx = max_el - std::begin(accumulated_sum) + window_size;
+
+//     double sum_w = 0;
+//     double prod_wx = 0;
+//     double wmc = 0;
+
+//     for(int i = max_sum_idx - window_size; i < max_sum_idx; ++i)
+//     {
+//         prod_wx += column[i] * i;
+//         sum_w += column[i];
+//     }
+
+//     wmc = float(prod_wx) / float(sum_w);
+
+//     result = img_height - wmc;
+
+//     return result;
+// #endif
+// }
+
+// Pixels process_columns(Image &image)
+// {
+//     Pixels result;
+//     result.counters = image.counters;
+
+//     // std::cout << "here\n";
+//     start_timer(process_columns);
+
+//     for (size_t i = 0; i < image.width; i++)
+//     {
+//         // smooth_column(image.rotated_cw[i]);
+//         result.pixels[i] = process_column(image.rotated_cw[i]);
+//         // Algo genetic(image.rotated_cw[i]);
+
+//         // image.pixels[i] = genetic.run().a;
+
+//         // if (i == 0) {
+//         // std::cout << "pixel: " << image.pixels[i] << std::endl;
+//         // }
+//     }
+
+//     stop_timer(process_columns);
+
+//     return result;
+// }
+
+QList<QLineF> pixelsToLines(const Pixels &rawProfile)
+{
+    const auto& pixels = rawProfile.pixels;
+    QList<QPointF> points(pixels.size());
+
+    for (int i = 0; i < pixels.size(); ++i)
+    {
+        points[i] = { qreal(i) - img_width / 2, pixels.at(i) };
+    }
+
+    // qDebug() << "mid points" << points.mid(points.count() - 3, 6);
+
+    return pointsToLines(std::move(points));
+}
+
+QList<QLineF> pointsToLines(const QList<QPointF> &points)
+{
+    constexpr double maxDistanceFromLine { 3 };
+    constexpr double minLineLength { 10 };
+
+    QList<int> lineAnchors{0, int(points.count() - 1)};
+
+    auto vecLength = [](const QPointF& vector) {
+        return std::hypot(vector.x(), vector.y());
+    };
+
+    auto distanceToLine = [vecLength](const QPointF& point, const QLineF& line) {
+        // const auto d1 = point - line.p1();
+        // const auto d2 = line.p2() - line.p1();
+
+        // const auto norm = vecLength(d2);
+        // qDebug() << "norm" << norm;
+
+        // if (norm <= std::numeric_limits<double>::epsilon())
+        // {
+        //     qDebug() << "AAAAAAAAAAAAAAAAAA";
+        //     return vecLength(d1);
+        // }
+
+        // const auto result = std::fabs(d1.x() * d2.y() - d1.y() * d2.x()) / norm;
+
+        // if (!qFuzzyIsNull(result))
+        // {
+        //     qDebug() << "NOT NULL" << result << point << line;
+        // }
+
+        // return result;
+        // transform to loocal coordinates system (0,0) - (lx, ly)
+        QPointF p1 = line.p1();
+        QPointF p2 = line.p2();
+        const auto& p = point;
+        qreal x = p.x() - p1.x();
+        qreal y = p.y() - p1.y();
+        qreal x2 = p2.x() - p1.x();
+        qreal y2 = p2.y() - p1.y();
+
+        // if line is a point (nodes are the same) =>
+        // just return distance between point and one line node
+        qreal norm = sqrt(x2*x2 + y2*y2);
+        if (norm <= std::numeric_limits<int>::epsilon())
+            return sqrt(x*x + y*y);
+
+        // distance
+        // qDebug() << "dist" << fabs(x*y2 - y*x2) / norm << point << line;
+        return fabs(x * y2 - y * x2) / norm;
+    };
+
+    // for (const auto& p : points)
+    // {
+    //     qDebug() << "\t" << p;
+    // }
+
+    for (int i = 0; i < lineAnchors.count() - 1; ++i)
+    {
+        const auto &lineFirst = i;
+        const auto &lineLast = i + 1;
+        const auto& leftPointIdx = lineAnchors.at(lineFirst);
+        const auto& rightPointIdx = lineAnchors.at(lineLast);
+
+        if (rightPointIdx - leftPointIdx < 2)
+        {
+            continue;
+        }
+
+        const QLineF line { points.at(leftPointIdx), points.at(rightPointIdx) };
+
+        // std::cout << "max between " << leftPointIdx + 1 << " and "
+        // << rightPointIdx - 1 << std::endl;
+
+        const auto farthest = std::max_element(
+            // std::execution::par_unseq,
+            points.cbegin() + leftPointIdx + 1,
+            points.cbegin() + rightPointIdx - 1,
+            [line, distanceToLine](const QPointF& a, const QPointF& b) {
+                return distanceToLine(a, line) < distanceToLine(b, line);
+            }
+        );
+
+        // std::cout << "done max" << std::endl;
+        // auto tmp = *farthest;
+        // std::cout << "done farthest" << std::endl;
+
+        // qDebug() << "farthest point" << distanceToLine(*farthest, line);
+        // qDebug() << "farthest dist" << distanceToLine(*farthest, line);
+        // qDebug() << "that's from line" << line;
+
+        if (distanceToLine(*farthest, line) > maxDistanceFromLine)
+        {
+            // std::cout << "try insert at " << i + 1 << ". lineAnchors.size is
+            // "
+            //           << lineAnchors.size()
+            //           << ". inserting this: " << farthest - points.cbegin()
+            //           << std::endl;
+            lineAnchors.insert(i + 1, farthest - points.cbegin());
+            // std::cout << "done insert" << std::endl;
+            --i;
+            // qDebug() << "I'm here" << i;
+            continue;
+        }
+    }
+
+    struct LineAB
+    {
+        double a { std::numeric_limits<double>::quiet_NaN() };
+        double b { std::numeric_limits<double>::quiet_NaN() };
+
+        LineAB(const QList<QPointF>& points) {
+            if (points.isEmpty())
+            {
+                return;
+            }
+
+            double sumX { 0 };
+            double sumY { 0 };
+            double sumXY { 0 };
+            double sumXX { 0 };
+
+            // FIXME: validate
+            // for (const auto& point : points)
+            const int delta = points.count() * 0.15;
+            for (int i = delta; i < points.count() - delta; ++i)
+            {
+                const auto& point = points.at(i);
+                sumX += point.x();
+                sumY += point.y();
+                sumXY += point.x() * point.y();
+                sumXX += point.x() * point.x();
+            }
+
+            // sumX /= points.count();
+            // sumY /= points.count();
+            // sumXY /= points.count();
+            // sumXX /= points.count();
+
+            const int n = points.count() - delta * 2;
+            Q_ASSERT_X(n > 0, Q_FUNC_INFO, "n <= 0");
+
+            a = (n * sumXY - sumX * sumY) /
+                (n * sumXX - sumX * sumX);
+            b = (sumY - a * sumX) / n;
+        }
+    };
+
+    // auto pointsToLineAB =
+
+    // qDebug() << "line anchors count is" << lineAnchors.count();
+
+    constexpr bool useLsd = true;
+
+    QList<QLineF> result { lineAnchors.length() - 1 };
+
+    for (int i = 0; i < lineAnchors.count() - 1; ++i)
+    {
+        const auto& leftPointIdx = lineAnchors.at(i);
+        const auto& rightPointIdx = lineAnchors.at(i + 1);
+
+        QPointF leftPoint = points.at(leftPointIdx);
+        QPointF rightPoint = points.at(rightPointIdx);
+
+        LineAB lineAB(points.mid(leftPointIdx, rightPointIdx - leftPointIdx));
+
+        leftPoint.setY(lineAB.a * leftPoint.x() + lineAB.b);
+        rightPoint.setY(lineAB.a * rightPoint.x() + lineAB.b);
+
+        if (useLsd)
+            result[i] = QLineF{ std::move(leftPoint), std::move(rightPoint) };
+        else
+            result[i] = QLineF{ points.at(lineAnchors.at(i)), points.at(lineAnchors.at(i + 1)) };
+    }
+
+    if (useLsd)
+    {
+        for (int i = 0; i < result.count() - 1; ++i)
+        {
+            auto& lineA = result[i];
+            auto& lineB = result[i + 1];
+
+            QPointF intersectionPoint {};
+
+            if (lineA.intersects(lineB, &intersectionPoint) != QLineF::NoIntersection)
+            {
+                lineA.setP2(intersectionPoint);
+                lineB.setP1(intersectionPoint);
+            }
+        }
+    }
+
+    // qDebug() << result;
+
+    return result;
+}