implement calibration

1 files changed, 439 insertions, 0 deletions

diff --git a/src/calibration.cpp b/src/calibration.cpp
new file mode 100644
index 0000000..73bd786
--- /dev/null
+++ b/src/calibration.cpp
@@ -0,0 +1,439 @@
+#include "calibration.h"
+
+#include <execution>
+#include <iostream>
+
+#include <QDebug>
+#include <QDir>
+#include <QFile>
+#include <QImage>
+#include <QJsonArray>
+#include <QJsonDocument>
+#include <QJsonObject>
+
+#include "imagealgos.h"
+
+bool openCalibrationTable(const QString& filename, CalibrationTablePtr& table)
+{
+    QFile f(filename);
+
+    if (!f.open(QFile::ReadOnly)) {
+        qWarning() << Q_FUNC_INFO << "cannot open" << filename
+                   << "for reading:" << f.errorString();
+
+        return false;
+    }
+
+    table.reset(new CalibrationTable{{0}});
+    auto bytes = f.read((char*) table.data(), sizeof(CalibrationTable));
+
+    if (bytes != sizeof(CalibrationTable)) {
+        qWarning() << "cannot read calibration table from" << filename << bytes;
+
+        if (f.error()) {
+            qWarning() << f.errorString() << f.error() << (void*) table.data();
+        } else {
+            qWarning() << "file size:" << f.size() << "; got:" << bytes;
+        }
+
+        return false;
+    }
+
+    // for (const auto& col : *table) {
+    //     qDebug() << "calibration column mid:" << col[640];
+    // }
+
+    return true;
+}
+
+bool dump(const CalibrationTablePtr& table, const QString& filename)
+{
+    qDebug() << Q_FUNC_INFO << "size is" << sizeof(CalibrationTable);
+
+    QFile f(filename);
+
+    if (!f.open(QFile::WriteOnly)) {
+        qWarning() << Q_FUNC_INFO << "cannot open" << filename
+                   << "for writing:" << f.errorString();
+
+        return false;
+    }
+
+    const auto written = f.write((const char*) table.data(),
+                                 sizeof(CalibrationTable));
+
+    if (written != sizeof(CalibrationTable) || !f.flush()) {
+        qWarning() << Q_FUNC_INFO << "cannot write" << filename << ":"
+                   << f.errorString();
+        return false;
+    }
+
+    return true;
+}
+
+void interpolate(CalibrationTablePtr& table)
+{
+    std::for_each(std::execution::par,
+                  table->begin(),
+                  table->end(),
+                  [](auto& column) { interpolate(column); });
+
+    for (size_t i = 9471; i < 9472; i++) {
+        std::cout << "row #" << i << ": ";
+
+        for (size_t j = 0; j < 1280; ++j) {
+            const auto& p = table->at(j).at(i);
+            std::cout << p << ' ';
+        }
+
+        std::cout << std::endl;
+    }
+
+    // for (size_t i = 0; i < discretesInRage; i++) {
+    //     std::cout << "row #" << i << ": ";
+
+    //     for (size_t j = 640 - 5; j < 640 + 5; ++j) {
+    //         const auto& p = table->at(j).at(i);
+    //         std::cout << p << ' ';
+    //     }
+
+    //     std::cout << std::endl;
+    // }
+
+    // for (const auto& p : (*table)[table->size() / 2]) {
+    //     qDebug() << "mid column pixel" << p;
+    // }
+}
+
+void interpolate(CalibrationColumn& column)
+{
+    size_t start{0};
+    auto& c = column;
+
+#define FIND_IF(index, condition) \
+    while (bool(c[index]) != condition && index < calibrationTableHeight) \
+        ++index; \
+\
+    if (index == calibrationTableHeight) \
+    return
+
+    FIND_IF(start, true);
+
+    while (true) {
+        size_t left = start + 1;
+        FIND_IF(left, false);
+        --left;
+
+        size_t right = left + 1;
+        FIND_IF(right, true);
+
+        auto delta = (c[right] - c[left]) / (right - left);
+
+        for (auto i = left + 1; i < right; ++i) {
+            c[i] = c[i - 1] + delta;
+
+            if (c[i] > 190.) {
+                qWarning() << "interpolate: got invalid value mm" << c[i];
+
+                qWarning() << "left/i/right" << left << i << right;
+                qWarning() << "delta" << delta;
+                qWarning() << "c[left/i/right]" << c[left] << c[i] << c[right];
+
+                exit(EXIT_FAILURE);
+            }
+        }
+
+        start = right;
+    }
+}
+
+QImage calibrationTableToImage(const CalibrationTablePtr& calibrationTable)
+{
+    QImage result(QSize(calibrationTable->size(),
+                        calibrationTable->at(0).size()),
+                  QImage::Format::Format_Indexed8);
+
+    // QImage image(QSize(imageWidth, imageHeight), QImage::Format_Indexed8);
+
+    QColor color(Qt::green);
+    auto r = color.redF();
+    auto g = color.greenF();
+    auto b = color.blueF();
+
+    for (int c = 0; c < 256; c++) {
+        QRgb col = qRgb(int(c * r), int(c * g), int(c * b));
+        result.setColor(c, col);
+    }
+
+    int notNull = 0;
+
+    for (size_t colIdx = 0; colIdx < calibrationTable->size(); ++colIdx) {
+        const auto& column = calibrationTable->at(colIdx);
+
+        for (size_t rowIdx = 0; rowIdx < column.size(); ++rowIdx) {
+            bool hasValue = !qFuzzyIsNull(column.at(rowIdx));
+
+            notNull += int(hasValue);
+
+            result.setPixel(colIdx, rowIdx, hasValue ? 255 : 0);
+            // if (column.at(rowIdx) >= 190.) {
+            //     qWarning() << "invalid mm value" << column.at(rowIdx);
+            //     exit(EXIT_FAILURE);
+            // }
+            // result.setPixel(colIdx, rowIdx, (column.at(rowIdx) / 190.) * 255);
+        }
+    }
+
+    qDebug() << "not null count" << notNull << "of"
+             << sizeof(CalibrationTable)
+                    / sizeof(calibrationTable->at(0).at(0));
+
+    return result;
+}
+
+CalibrationTablePtr calibrateZ(const QList<Pixels>& rawProfiles,
+                               const uint32_t& stepsPerMm)
+{
+    CalibrationTablePtr result{new CalibrationTable{{0}}};
+
+    for (const auto& rawProfile : rawProfiles) {
+        // std::cout << "calibration: pos is "
+        //           << rawProfile.counters.encoderPosition << std::endl;
+        const float positionMm{float(rawProfile.counters.encoderPosition)
+                               / float(stepsPerMm)};
+
+        if (positionMm >= 200) {
+            qWarning() << "got invalid calibration distance:" << positionMm;
+            qWarning() << "encoder position:"
+                       << rawProfile.counters.encoderPosition;
+            exit(EXIT_FAILURE);
+        }
+
+        const auto& pixels = rawProfile.pixels;
+        // printPixels(pixels);
+        // static size_t counter { 0 };
+        // qDebug() << "calibrated" << counter++;
+        const float pos = rawProfile.counters.encoderPosition;
+        const float divider = stepsPerMm;
+        // std::cout << pos << " " << divider << " " << pos / divider <<
+        // std::endl; std::cout << std::setw(5) <<
+        // rawProfile.counters.encoderPosition
+        //           << std::setw(5) << requested_params.stepsPerMm
+        //           << std::setw(8) << positionMm
+        //           << std::setw(8) <<
+        //           float(rawProfile.counters.encoderPosition) /
+        //                                  float(requested_params.stepsPerMm)
+        //           << ": ";
+
+        for (size_t columnIdx = 0; columnIdx < pixels.size(); ++columnIdx) {
+            const auto& pixelValue = pixels.at(columnIdx);
+
+            // float[0, img_height] -> uint16_t[0, calibrationColumnHeight]
+            const uint16_t discretePixelValue{
+                uint16_t(pixelValue * discretesInRage / img_height)};
+
+            Q_ASSERT_X(discretePixelValue > 0
+                           && discretePixelValue < calibrationColumnHeight,
+                       Q_FUNC_INFO,
+                       ("got ivalid discrete value "
+                        + QString::number(discretePixelValue)
+                        + ". pixelValues is " + QString::number(pixelValue)
+                        + ". calc result is "
+                        + QString::number(pixelValue * discretesInRage
+                                          / img_height))
+                           .toStdString()
+                           .c_str());
+
+            // auto& calibrationColumn = result[columnIdx];
+            auto& calibrationColumn = (*result)[columnIdx];
+
+            calibrationColumn[discretePixelValue] = positionMm;
+
+            // if (columnIdx >= ((img_width - 10) / 2) &&
+            //     columnIdx < img_width - ((img_width - 10) / 2))
+            // {
+            //     std::cout << discretePixelValue << ";";
+            // }
+        }
+
+        // std::cout << std::endl << std::flush;
+    }
+
+    return result;
+    // TODO: try something interesting
+    // return {};
+}
+
+CalibrationTablePtr calibrateX(const QList<Pixels>& rawProfiles)
+{
+    CalibrationTablePtr result{new CalibrationTable{{0}}};
+
+    static size_t tmp_counter = 0;
+
+    for (const auto& rawProfile : rawProfiles) {
+        const auto& pixels = rawProfile.pixels;
+        auto lines = pixelsToLines(rawProfile);
+
+        tmp_counter++;
+
+        Q_ASSERT_X(lines.count() > 2, Q_FUNC_INFO, "no lines");
+
+        if (tmp_counter == 9471) {
+            for (const auto& l : lines) {
+                qDebug() << "line:" << l;
+            }
+        }
+
+        QList<double> xAnchors(lines.size() + 1);
+
+        std::transform(std::execution::par_unseq,
+                       lines.constBegin(),
+                       lines.constEnd(),
+                       xAnchors.begin(),
+                       [](const auto& l) { return l.x1(); });
+
+        xAnchors.last() = lines.last().x2();
+
+        constexpr double triangleHeightMm{5.};
+        constexpr double triangleBaseMm{8.};
+
+        auto nearestAnchorToX0 = std::min_element(std::execution::par_unseq,
+                                                  xAnchors.constBegin(),
+                                                  xAnchors.constEnd(),
+                                                  [](const auto& a,
+                                                     const auto& b) {
+                                                      return std::abs(a)
+                                                             < std::abs(b);
+                                                  });
+
+        int nearestAnchorToX0Idx = nearestAnchorToX0 - xAnchors.constBegin();
+
+        QList<double> xAnchorsMm(xAnchors.count());
+
+        for (int i = 0; i < xAnchors.size(); ++i) {
+            xAnchorsMm[i] = (i - nearestAnchorToX0Idx) * triangleBaseMm / 2.;
+        }
+
+        auto xAnchorIt = xAnchors.constBegin() + 1;
+        auto xAnchorMmIt = xAnchorsMm.constBegin() + 1;
+
+        for (size_t columnIdx = 0; columnIdx < pixels.size(); ++columnIdx) {
+            // skip points with to the left from left line and to the right from
+            // right line
+            const auto columnX = int(columnIdx) - int(img_width / 2);
+            if (columnX < xAnchors.first() || columnX > xAnchors.last()) {
+                continue;
+            }
+
+            if (columnX > *xAnchorIt) {
+                ++xAnchorIt;
+                ++xAnchorMmIt;
+            }
+            const auto xLeft = *(xAnchorIt - 1);
+            const auto xRight = *(xAnchorIt);
+
+            if (columnX < xLeft || columnX > xRight) {
+                if (rawProfile.counters.encoderPosition >= 0) {
+                    qWarning()
+                        << "x anchor not found" << xLeft << columnX << xRight;
+                    continue;
+                }
+            }
+
+            const auto& pixelValue = pixels.at(columnIdx);
+            const uint16_t discretePixelValue{
+                uint16_t(pixelValue * discretesInRage / img_height)};
+
+            Q_ASSERT_X(discretePixelValue > 0
+                           && discretePixelValue < calibrationColumnHeight,
+                       Q_FUNC_INFO,
+                       ("got ivalid discrete value "
+                        + QString::number(discretePixelValue)
+                        + ". pixelValues is " + QString::number(pixelValue)
+                        + ". calc result is "
+                        + QString::number(pixelValue * discretesInRage
+                                          / img_height))
+                           .toStdString()
+                           .c_str());
+
+            const auto xLineLen = xRight - xLeft;
+            const auto xLeftMm = *(xAnchorMmIt - 1);
+            const auto xRelative = float(columnX - xLeft) / xLineLen;
+            const auto xMmValue = xLeftMm + xRelative * (triangleBaseMm / 2.);
+
+            auto& calibrationColumn = (*result)[columnIdx];
+
+            if (tmp_counter == 9471) {
+                qDebug() << "calibration value" << columnIdx
+                         << discretePixelValue << xMmValue;
+            }
+            calibrationColumn[discretePixelValue] = xMmValue;
+        }
+    }
+
+    return result;
+}
+
+void dumpCalibrationPixels(std::vector<Pixels>&& calibrationPixels)
+{
+    std::vector<Pixels> rawProfiles;
+
+    std::swap(rawProfiles, calibrationPixels);
+
+    const QString dumpSubdir{
+        QDateTime::currentDateTime().toString("yyyy.MM.dd_hh.mm.ss")};
+    const QDir dumpPath{dumpsRoot + "/" + dumpSubdir};
+
+    if (!dumpPath.mkdir(dumpPath.path())) {
+        qWarning() << "cannot create dir: " << dumpPath.path();
+
+        return;
+    }
+
+    for (const auto& rawProfile : rawProfiles) {
+        const auto filename = QLatin1String("raw_profile_meas_%1_enc_%2")
+                                  .arg(QString::number(
+                                      rawProfile.counters.measurementCounter))
+                                  .arg(rawProfile.counters.encoderPosition);
+        const auto filepath = dumpPath.path() + "/" + filename;
+
+        QFile f{filepath};
+
+        if (!f.open(QFile::WriteOnly)) {
+            qWarning() << "cannot open dump dump file" << f.fileName();
+            qWarning() << "error is:" << f.errorString();
+
+            return;
+        }
+
+        QJsonObject jsonCounters{
+            {"timestampUs", qint64(rawProfile.counters.timestampUs)},
+            {"measurementCounter",
+             qint64(rawProfile.counters.measurementCounter)},
+            {"encoderPosition", qint64(rawProfile.counters.encoderPosition)},
+        };
+
+        QJsonObject json;
+
+        json["counters"] = jsonCounters;
+
+        QJsonArray jsonPixels;
+
+        for (const auto& pixel : rawProfile.pixels) {
+            jsonPixels << pixel;
+        }
+
+        json["pixels"] = jsonPixels;
+
+        if (!f.write(QJsonDocument(json).toJson())) {
+            qWarning() << "cannot write file" << f.fileName();
+            qWarning() << "error is" << f.errorString();
+
+            return;
+        }
+
+        qDebug() << "file written: " << f.fileName();
+    }
+
+    qDebug() << "dump finished";
+}