diff options
Diffstat (limited to 'src/calibration.cpp')
| -rw-r--r-- | src/calibration.cpp | 439 |
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"; +} |