implement calibration

1 files changed, 147 insertions, 0 deletions

diff --git a/profile.cpp b/profile.cpp
new file mode 100644
index 0000000..31d88cd
--- /dev/null
+++ b/profile.cpp
@@ -0,0 +1,147 @@
+#include "profile.h"
+
+#include <iostream>
+
+#include <QDebug>
+#include <QJsonArray>
+
+Profile::Profile(
+    const Pixels& pixels,
+    const CalibrationTablePtr calibrationTableZ,
+    const CalibrationTablePtr calibrationTableX)
+    : m_counters(pixels.counters)
+{
+    static bool done{false};
+
+    if (!done) {
+        for (size_t i = 9471; i < 9472; i++) {
+            std::cout << "row #" << i << ": ";
+
+            for (size_t j = 0; j < 1280; ++j) {
+                const auto& x = calibrationTableX->at(j).at(i);
+                const auto& z = calibrationTableZ->at(j).at(i);
+                std::cout << "Profile: table: " << x << ' ' << z << std::endl;
+            }
+
+            std::cout << std::endl;
+        }
+    }
+
+    for (size_t i = 0; i < img_width; ++i) {
+        try {
+            const auto& pixel = pixels.pixels[i];
+            const auto pixelDiscrete = pixel * discretesInRage / img_height;
+            if (Q_UNLIKELY(pixel >= sizeof(CalibrationColumn) / sizeof(float))) {
+                qWarning() << "got invalid calibration pixel at" << i << ":"
+                           << pixel;
+                m_counters = {};
+                m_pointsMm = {QPointF{std::nan(""), std::nan("")}};
+                return;
+            }
+
+            if (Q_UNLIKELY(pixel == sizeof(CalibrationColumn) - 1)) {
+                qDebug() << "Profile: got edge value";
+                const auto& z = calibrationTableZ->at(i).at(pixelDiscrete);
+
+                if (qFuzzyIsNull(z) || std::isnan(z)) {
+                    m_pointsMm.at(i) = {std::nan(""), std::nan("")};
+                    continue;
+                }
+
+                const auto& x = calibrationTableX->at(i).at(pixelDiscrete);
+                m_pointsMm.at(i) = {x, z};
+
+                continue;
+            }
+
+            const auto& leftMmZ = calibrationTableZ->at(i).at(
+                size_t(pixelDiscrete));
+            const auto& rightMmZ = calibrationTableZ->at(i).at(
+                size_t(pixelDiscrete) + 1);
+
+            const auto& leftMmX = calibrationTableX->at(i).at(
+                size_t(pixelDiscrete));
+            const auto& rightMmX = calibrationTableX->at(i).at(
+                size_t(pixelDiscrete) + 1);
+
+            const auto& fract = pixelDiscrete - long(pixelDiscrete);
+
+            const auto z = (leftMmZ * (1 - fract) + rightMmZ * fract);
+
+            // TODO: use only NaN (or zero?) everywhere
+            // NOTE: QJsonValue converts NaN to zero
+            if (qFuzzyIsNull(z) || std::isnan(z)) {
+                qDebug() << "got nan z for discrete" << pixelDiscrete << leftMmZ
+                         << rightMmZ;
+                m_pointsMm.at(i) = {std::nan(""), std::nan("")};
+                continue;
+            }
+
+            const auto x = (leftMmX * (1 - fract) + rightMmX * fract);
+
+            m_pointsMm.at(i) = {x, z};
+
+            if (!done) {
+                qDebug() << "got these values\t" << pixels.pixels[i]
+                         << pixelDiscrete << m_pointsMm[i] << leftMmZ
+                         << rightMmZ << leftMmX << rightMmX;
+            }
+        } catch (const std::out_of_range& ex) {
+            qWarning() << "out of range exception:" << ex.what();
+            qWarning() << "i:" << i; // << "pixels[i]" << pixels.pixels[i];
+            m_counters = {};
+            m_pointsMm = {QPointF{std::nan(""), std::nan("")}};
+
+            return;
+        } catch (const std::exception& ex) {
+            qWarning() << "exception:" << ex.what();
+            qWarning() << "i:" << i << "pixels[i]" << pixels.pixels[i];
+            m_counters = {};
+            m_pointsMm = {QPointF{std::nan(""), std::nan("")}};
+
+            return;
+        } catch (...) {
+            qWarning() << "unknown exception";
+            m_counters = {};
+            m_pointsMm = {QPointF{std::nan(""), std::nan("")}};
+            return;
+        }
+    }
+
+    done = true;
+
+    // static bool printOnce = [&]() -> bool {
+    //     for (size_t i = 0; i < m_pointsMm.size(); ++i) {
+    //         qDebug() << '\t' << pixels.pixels[i] << m_pointsMm[i];
+    //     }
+
+    //     return true;
+    // }();
+}
+
+const Counters& Profile::counters() const
+{
+    return m_counters;
+}
+
+const Profile::PointsMm& Profile::pointsMm() const
+{
+    return m_pointsMm;
+}
+
+Profile::operator const QJsonObject() const
+{
+    QJsonObject counters{
+        {"timestampUs", qint64(m_counters.timestampUs)},
+        {"measurementCounter", qint64(m_counters.measurementCounter)},
+        {"encoderPosition", qint64(m_counters.encoderPosition)},
+    };
+
+    QJsonArray jsonPoints;
+
+    for (const auto& p : m_pointsMm) {
+        jsonPoints << QJsonArray{p.x(), p.y()};
+    }
+
+    return {{"counters", counters}, {"pointsMm", jsonPoints}};
+}