From 9dde2ab53c8e2c97647164fce89cf149260fbc8f Mon Sep 17 00:00:00 2001 From: Nikita Kostovsky Date: Sun, 12 Jan 2025 11:50:34 +0100 Subject: implement calibration --- .clang-format | 15 - CMakeLists.txt | 16 +- fuck_intel.h | 3 + imagealgos.cpp | 34 +-- imagealgos.h | 49 +-- macro.h | 1 - main.cpp | 845 +++++++++++----------------------------------------- printerclient.cpp | 5 +- printerclient.h | 16 +- profile.cpp | 147 +++++++++ profile.h | 29 ++ src/calibration.cpp | 439 +++++++++++++++++++++++++++ src/calibration.h | 31 ++ src/constants.h | 23 ++ src/dumps.cpp | 141 +++++++++ src/dumps.h | 6 + src/pixels.cpp | 95 ++++++ src/pixels.h | 32 ++ src/typedefs.h | 33 ++ 19 files changed, 1187 insertions(+), 773 deletions(-) delete mode 100644 .clang-format create mode 100644 profile.cpp create mode 100644 profile.h create mode 100644 src/calibration.cpp create mode 100644 src/calibration.h create mode 100644 src/constants.h create mode 100644 src/dumps.cpp create mode 100644 src/dumps.h create mode 100644 src/pixels.cpp create mode 100644 src/pixels.h create mode 100644 src/typedefs.h diff --git a/.clang-format b/.clang-format deleted file mode 100644 index 56838cf..0000000 --- a/.clang-format +++ /dev/null @@ -1,15 +0,0 @@ -AccessModifierOffset: -4 -AlignAfterOpenBracket: BlockIndent -AllowAllArgumentsOnNextLine: false -AllowAllParametersOfDeclarationOnNextLine: false -BasedOnStyle: Microsoft -BinPackParameters: false -BinPackArguments: false -BreakConstructorInitializers: BeforeComma -ColumnLimit: 80 -InsertNewlineAtEOF: true -IndentWidth: 4 -SpacesBeforeTrailingComments: 2 -UseTab: Never -LineEnding: LF -PointerAlignment: Left diff --git a/CMakeLists.txt b/CMakeLists.txt index a00e7f3..43cd7c7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,7 +16,10 @@ set(TARGET_SYSROOT /home/nikita/rpi/rpi-sysroot) set(CMAKE_SYSROOT ${TARGET_SYSROOT}) set(CMAKE_LIBRARY_PATH ${CMAKE_SYSROOT}/usr/lib/aarch64-linux-gnu) -set(ENV{PKG_CONFIG_PATH} $PKG_CONFIG_PATH:/usr/lib/aarch64-linux-gnu/pkgconfig) +# arch: +# set(ENV{PKG_CONFIG_PATH} $PKG_CONFIG_PATH:/usr/lib/aarch64-linux-gnu/pkgconfig) +# gentoo: +set(ENV{PKG_CONFIG_PATH} $PKG_CONFIG_PATH:/usr/lib/aarch64-rpi5-linux-gnu/pkgconfig) set(ENV{PKG_CONFIG_LIBDIR} /usr/lib/pkgconfig:/usr/share/pkgconfig/:${TARGET_SYSROOT}/usr/lib/aarch64-linux-gnu/pkgconfig:${TARGET_SYSROOT}/usr/lib/pkgconfig) set(ENV{PKG_CONFIG_SYSROOT_DIR} ${CMAKE_SYSROOT}) @@ -24,7 +27,10 @@ set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -I${TARGET_SYSROOT}/usr/include") set(CMAKE_CXX_FLAGS "${CMAKE_C_FLAGS}") # NOTE: I've added ld symlink on host to be able to run moc +# arch: # sudo ln -s /usr/aarch64-linux-gnu/lib/ld-linux-aarch64.so.1 /lib/ld-linux-aarch64.so.1 +# gentoo: +# sudo ln -s /usr/aarch64-rpi5-linux-gnu/lib/ld-linux-aarch64.so.1 /lib/ld-linux-aarch64.so.1 # # TODO: check how to add this env var to cmake, it's needed to run moc, # otherwise you'll get the following error @@ -64,7 +70,7 @@ find_package(TBB REQUIRED) qt_standard_project_setup(REQUIRES 6.4) -include_directories(. "${CAMERA_INCLUDE_DIRS}") +include_directories(. "${CAMERA_INCLUDE_DIRS}" src) # libpistache pkg_check_modules(Pistache REQUIRED IMPORTED_TARGET libpistache) @@ -86,6 +92,12 @@ qt_add_executable(apporpheus printerclient.h printerclient.cpp fuck_intel.h + src/pixels.h src/pixels.cpp + src/constants.h + src/typedefs.h + src/dumps.h src/dumps.cpp + src/calibration.h src/calibration.cpp + profile.h profile.cpp ) target_link_libraries(app${PROJECT_NAME} PRIVATE diff --git a/fuck_intel.h b/fuck_intel.h index a9fa7d5..6d9f9d5 100644 --- a/fuck_intel.h +++ b/fuck_intel.h @@ -1,5 +1,8 @@ #pragma once +// include this file before including any qt files if you have emit conflict +// with tbb + #ifdef emit #undef emit #include diff --git a/imagealgos.cpp b/imagealgos.cpp index 267e4ae..98d5fb0 100644 --- a/imagealgos.cpp +++ b/imagealgos.cpp @@ -7,23 +7,24 @@ #include #include -#include #include #include #include -#include #include -// #include - -#include "fuck_intel.h" -#include "genetic_algos.h" #include "macro.h" uint8_t pgm_image[64 + img_width * img_height * sizeof(uint16_t)]; size_t pgm_image_size = 0; std::mutex pgm_image_mtx; +template +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(Image *img, FILE *outfile, bool really_save) { std::lock_guard lg(pgm_image_mtx); @@ -279,27 +280,6 @@ Pixels process_columns(Image &image) return result; } -Pixels& Pixels::operator+=(const Pixels& other) -{ - std::transform(std::execution::par, - pixels.begin(), pixels.end(), - other.pixels.begin(), - pixels.begin(), - // [](auto& toAdd) { return dst += src; }); - std::plus<>()); - - return *this; -} - -Pixels& Pixels::operator/=(const float divider) -{ - std::for_each(std::execution::par_unseq, - pixels.begin(), pixels.end(), - [divider](auto& pixel) { pixel /= divider; }); - - return *this; -} - QList pixelsToLines(const Pixels &rawProfile) { const auto& pixels = rawProfile.pixels; diff --git a/imagealgos.h b/imagealgos.h index 12dc8cf..60844b8 100644 --- a/imagealgos.h +++ b/imagealgos.h @@ -1,62 +1,15 @@ #pragma once -#include -#include -#include - -#include - #include #include -constexpr size_t img_width = 1280; -constexpr size_t img_height = 800; -constexpr uint32_t patternSize = 16; - -struct Counters -{ - uint32_t timestampUs { 0 }; - uint32_t measurementCounter { 0 }; - int32_t encoderPosition { 0 }; -}; - -struct Image -{ - int width; - int height; - uint16_t data[img_height][img_width]; - uint16_t rotated_cw[img_width][img_height]; - size_t dataSize; - unsigned int stride; - libcamera::PixelFormat pixelFormat; - // float pixels[img_width]; - Counters counters {}; - // unsigned int measurementCounter; - // uint32_t timestampUs; - // int32_t encoderPosition; -}; - -struct Pixels -{ - Counters counters {}; - std::array pixels { 0.f }; - - Pixels& operator+=(const Pixels& other); - Pixels& operator/=(const float divider); -}; - +#include "pixels.h" size_t pgm_save(Image *img, FILE *outfile, bool really_save = false); void unpack_10bit(uint8_t const *src, Image const &image, uint16_t *dest); void unpack_16bit(uint8_t const *src, Image const &image, uint16_t *dest); -template -T median3(const T& a, const T& b, const T& c) { - using namespace std; - return max(min(a,b), min(max(a,b),c)); -} - void rotate(Image & image); Pixels process_columns(Image & image); diff --git a/macro.h b/macro.h index c92b836..8be6df4 100644 --- a/macro.h +++ b/macro.h @@ -15,4 +15,3 @@ // << std::chrono::duration_cast( \ // end_ ## name - begin_ ## name) \ // << std::endl; - diff --git a/main.cpp b/main.cpp index 9304ed9..431e82d 100644 --- a/main.cpp +++ b/main.cpp @@ -8,12 +8,15 @@ #include #include "LibCamera.h" +#include "calibration.h" +#include "dumps.h" #include "fuck_intel.h" #include "genetic_algos.h" #include "httpservice.h" #include "imagealgos.h" #include "pigpio.h" #include "printerclient.h" +#include "profile.h" #include "rotaryencoder.h" #include @@ -28,47 +31,23 @@ #include #include #include -#include - -// #define FIRST_COLUMN_ONLY - -#define try_apply_config() \ - if (!applyConfig(config)) \ - { \ - camera->release(); \ - cm->stop(); \ - \ - return EXIT_FAILURE; \ +#include + +#define try_apply_config() \ + if (!applyConfig(config)) { \ + camera->release(); \ + cm->stop(); \ +\ + return EXIT_FAILURE; \ } -const QString exposureTimeKey = "exposureTime"; -const QString laserLevelKey = "laserLevel"; - -enum ScanningModeFlags : uint8_t -{ - None = 0, - Calibration -}; - -// ScanningModeFlags operator|(ScanningModeFlags lhs, ScanningModeFlags rhs) -// { -// using T = std::underlying_type::type; -// return static_cast(static_cast(lhs) | -// static_cast(rhs)); -// } - ScanningModeFlags scanningModeFlags{ScanningModeFlags::None}; QElapsedTimer calibrationTimer; extern volatile int32_t positionSteps; -struct requested_params_t -{ - int32_t exposureTime = {1000}; - int32_t laserLevel = {3000}; - uint32_t stepsPerMm{200}; -} requested_params; +requested_params_t requested_params; namespace { @@ -78,8 +57,6 @@ std::vector calibrationPixels; QMutex calibrationPixelsMutex; } // namespace -const QString dumpsRoot{QStringLiteral("/home/user/dumps")}; - using namespace std::chrono_literals; static std::shared_ptr camera; @@ -88,21 +65,10 @@ static std::map> mappedBuffers_; std::vector> requests; libcamera::ControlList lastControls; -static QList openDump(const QString& dumpPath = ""); -static std::optional openRawProfile(const QString& filePath); -constexpr float hardcodedZRangeMm{175.f}; -constexpr size_t calibrationTableHeight{0x4000}; // 16384 -// img_width * calibrationTableHeight -using CalibrationColumn = std::array; -using CalibrationTable = std::array; -constexpr auto calibrationColumnHeight = std::tuple_size(); -// CalibrationTable* calibrationTable { new CalibrationTable {{ 0 }} }; - namespace { -QSharedPointer calibrationTableZ; -QSharedPointer calibrationTableX; -constexpr uint16_t discretesInRage{16384}; +CalibrationTablePtr calibrationTableZ; +CalibrationTablePtr calibrationTableX; } // namespace static bool applyConfig( @@ -110,21 +76,7 @@ static bool applyConfig( ); static void onRequestCompleted(libcamera::Request* completed_request); static void printControls(); -static void dumpCalibrationPixels(); static QList filter(const QList& rawProfiles); -static QSharedPointer calibrateX( - const QList& rawProfiles -); -static QSharedPointer calibrateZ( - const QList& rawProfiles -); - -static QImage calibrationTableToImage( - const QSharedPointer& calibrationTable -); - -static void interpolate(QSharedPointer& table); -static void interpolate(CalibrationColumn& column); auto printPixels = [](const auto& pixels) { for (size_t i = (img_width - 10) / 2; @@ -142,24 +94,87 @@ int main(int argc, char* argv[]) { QCoreApplication app(argc, argv); - if (!initLaser()) - { - return EXIT_FAILURE; - } - - PrinterClient printerClient; - // if (false) qDebug() << "size of raw profile" << sizeof(Pixels); if (true) { - // auto rawProfiles = openDump("/home/user/dumps/2024.11.24_19.17.32"); - // auto rawProfiles = openDump("/home/user/dumps/2024.11.26_21.53.55"); - // auto rawProfiles = - // openDump("/home/user/dumps/2024.11.26_21.53.55_small"); + if (false) { + // z + // if (!openCalibrationTable( + // "/home/user/dumps/binz.calibration_table", + // ::calibrationTableZ + // )) + // { + // exit(EXIT_FAILURE); + // } - { - auto rawProfiles = openDump("/home/user/dumps/2024.12.07_14.09.33"); + // if (!calibrationTableToImage(::calibrationTableZ) + // .save("/home/user/dumps/imageZ.png")) + // { + // qDebug() << "cannot save imageZ.png"; + // exit(EXIT_FAILURE); + // } + + // interpolate(::calibrationTableZ); + // exit(EXIT_SUCCESS); + + // calibrationTableToImage(::calibrationTableZ) + // .save("/home/user/dumps/imageZ_interpolated.png"); + + auto rawProfiles = openDump("/home/user/dumps/binx"); + qDebug() << "raw x-profiles count is" << rawProfiles.size(); + // qDebug() << "height" << calibrationColumnHeight; + + auto filteredRawProfiles = filter(std::move(rawProfiles)); + qDebug() << "filtered x-profiles count is" + << filteredRawProfiles.count(); + + ::calibrationTableX = calibrateX(std::move(filteredRawProfiles)); + + for (size_t i = 9471; i < 9472; i++) { + std::cout << "row #" << i << ": "; + + for (size_t j = 0; j < 1280; ++j) { + const auto& p = ::calibrationTableX->at(j).at(i); + std::cout << p << ' '; + } + + std::cout << std::endl; + } + + // x + qDebug() << "open x table"; + if (!openCalibrationTable("/home/user/dumps/binx.calibration_table", + ::calibrationTableX)) { + exit(EXIT_FAILURE); + } + + // if (!calibrationTableToImage(::calibrationTableX) + // .save("/home/user/dumps/imageX.png")) { + // qDebug() << "cannot save imageX.png"; + // exit(EXIT_FAILURE); + // } + + for (size_t i = 9471; i < 9472; i++) { + std::cout << "row #" << i << ": "; + + for (size_t j = 0; j < 1280; ++j) { + const auto& p = ::calibrationTableX->at(j).at(i); + std::cout << p << ' '; + } + + std::cout << std::endl; + } + + exit(EXIT_SUCCESS); + interpolate(::calibrationTableX); + + // calibrationTableToImage(::calibrationTableX) + // .save("/home/user/dumps/imageX_interpolated.png"); + } + + if (true) { + auto rawProfiles = openDump("/home/user/dumps/binz"); // auto rawProfiles = openDump("/home/user/dumps/z"); qDebug() << "raw z-profiles count is" << rawProfiles.size(); // qDebug() << "height" << calibrationColumnHeight; @@ -168,26 +183,36 @@ int main(int argc, char* argv[]) qDebug() << "filtered z-profiles count is" << filteredRawProfiles.count(); - ::calibrationTableZ = calibrateZ(std::move(filteredRawProfiles)); + ::calibrationTableZ = calibrateZ(std::move(filteredRawProfiles), + requested_params.stepsPerMm); + // if (!dump( + // ::calibrationTableZ, + // "/home/user/dumps/binz.calibration_table" + // )) + // { + // qApp->exit(EXIT_FAILURE); + // } - bool ok = calibrationTableToImage(::calibrationTableZ) - .save("/home/user/dumps/z/imageZ.png"); + // bool ok = calibrationTableToImage(::calibrationTableZ) + // .save("/home/user/dumps/z/imageZ.png"); - if (!ok) - { - qDebug() << "cannot save imageZ.png"; - exit(EXIT_FAILURE); - } + // if (!ok) + // { + // qDebug() << "cannot save imageZ.png"; + // exit(EXIT_FAILURE); + // } interpolate(::calibrationTableZ); - calibrationTableToImage(::calibrationTableZ) - .save("/home/user/dumps/z/imageZ_interpolated.png"); + // calibrationTableToImage(::calibrationTableZ) + // .save("/home/user/dumps/z/imageZ_interpolated.png"); + // exit(EXIT_SUCCESS); } - if (true) - { - auto rawProfiles = openDump("/home/user/dumps/2024.12.07_14.51.07"); + qDebug() << "--------------------------------------------------------"; + + if (true) { + auto rawProfiles = openDump("/home/user/dumps/binx"); qDebug() << "raw x-profiles count is" << rawProfiles.size(); // qDebug() << "height" << calibrationColumnHeight; @@ -197,24 +222,39 @@ int main(int argc, char* argv[]) ::calibrationTableX = calibrateX(std::move(filteredRawProfiles)); - bool ok = calibrationTableToImage(::calibrationTableX) - .save("/home/user/dumps/z/imageX.png"); + // if (!dump( + // ::calibrationTableZ, + // "/home/user/dumps/binx.calibration_table" + // )) + // { + // qApp->exit(EXIT_FAILURE); + // } - if (!ok) - { - qDebug() << "cannot save imageX.png"; - exit(EXIT_FAILURE); - } + // bool ok = calibrationTableToImage(::calibrationTableX) + // .save("/home/user/dumps/z/imageX.png"); + + // if (!ok) + // { + // qDebug() << "cannot save imageX.png"; + // exit(EXIT_FAILURE); + // } interpolate(::calibrationTableX); - calibrationTableToImage(::calibrationTableX) - .save("/home/user/dumps/z/imageX_interpolated.png"); + // calibrationTableToImage(::calibrationTableX) + // .save("/home/user/dumps/z/imageX_interpolated.png"); } } // exit(EXIT_SUCCESS); + if (!initLaser()) + { + return EXIT_FAILURE; + } + + // PrinterClient printerClient; + QElapsedTimer t; t.start(); @@ -499,21 +539,31 @@ int main(int argc, char* argv[]) return QHttpServerResponse(QJsonDocument(json).toJson()); }); - qHttpServer.route( - "/v1/commands/resetEncoder", - [&](const QHttpServerRequest& request) -> QHttpServerResponse { - if (request.method() != QHttpServerRequest::Method::Post) - { - return QHttpServerResponse::StatusCode::NotFound; - } + qHttpServer.route("/v1/profile", [&]() { + std::lock_guard lg(pgm_image_mtx); - qDebug() << "reset encoder"; + const Profile profile(::pixels, + ::calibrationTableZ, + ::calibrationTableX); - positionSteps = 0; + const QJsonObject json{{"profile", QJsonObject(profile)}}; - return QHttpServerResponse::StatusCode::Ok; - } - ); + return QHttpServerResponse(QJsonDocument(json).toJson()); + }); + + qHttpServer + .route("/v1/commands/resetEncoder", + [&](const QHttpServerRequest& request) -> QHttpServerResponse { + if (request.method() != QHttpServerRequest::Method::Post) { + return QHttpServerResponse::StatusCode::NotFound; + } + + qDebug() << "reset encoder"; + + positionSteps = 0; + + return QHttpServerResponse::StatusCode::Ok; + }); qHttpServer.route( "/v1/commands/startCalibration", @@ -545,7 +595,7 @@ int main(int argc, char* argv[]) qDebug() << "send gCode:" << command; - printerClient.sendCommand(command); + // printerClient.sendCommand(command); return QHttpServerResponse::StatusCode::Ok; } @@ -563,7 +613,7 @@ int main(int argc, char* argv[]) qDebug() << "send gCode:" << command; - printerClient.sendCommand(command); + // printerClient.sendCommand(command); return QHttpServerResponse::StatusCode::Ok; } @@ -897,8 +947,16 @@ void onRequestCompleted(libcamera::Request* completed_request) // qDebug() << "stop calibration mode"; scanningModeFlags = ScanningModeFlags::None; - QFuture dumpCalirationPixelsFuture = - QtConcurrent::run(&dumpCalibrationPixels); + { + QMutexLocker l(&calibrationPixelsMutex); + // QFuture dumpCalirationPixelsFuture + // = QtConcurrent::run(&dumpCalibrationPixels, + // calibrationPixels); + auto future = QtConcurrent::run([&]() { + dumpCalibrationPixels( + std::move(::calibrationPixels)); + }); + } } else { @@ -971,9 +1029,9 @@ void onRequestCompleted(libcamera::Request* completed_request) auto fps{1000.f / msPerFrame}; - qDebug() << "fps ideal/real is" << configFps << "/" << fps - << "; ms per frame is" << msPerFrame << "counted fps" - << performanceCounter; + // qDebug() << "fps ideal/real is" << configFps << "/" << fps + // << "; ms per frame is" << msPerFrame << "counted fps" + // << performanceCounter; elapsedSum = 0; performanceCounter = 0; @@ -1048,224 +1106,6 @@ static void printControls() } } -static void dumpCalibrationPixels() -{ - std::vector rawProfiles; - - { - QMutexLocker l(&calibrationPixelsMutex); - 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"; -} - -static QList openDump(const QString& dumpPath) -{ - QString dirToRead{dumpPath}; - qDebug() << "trying to open dump path:" << dirToRead; - - if (dirToRead.isEmpty()) - { - qDebug() << "dumpPath not specified. looking into" << dumpsRoot; - - QDir dumpsRootDir{dumpsRoot}; - - const auto filter = QDir::Dirs | QDir::NoDotAndDotDot | QDir::Readable; - // there is no battery in my rpi5 for now - const auto sort = QDir::Name; - const auto entries = dumpsRootDir.entryList(filter, sort); - - if (entries.isEmpty()) - { - qWarning() << "dumps root" << dumpsRoot << "contains no dumps. " - << "specify existing dump path"; - - return {}; - } - - dirToRead = dumpsRoot + "/" + entries.last(); - } - - QDir dumpDir{dirToRead}; - - const QStringList nameFilters{"*.bin"}; - const auto filter = QDir::Files; - const auto sort = QDir::Name; - - auto filenames = dumpDir.entryList(filter, sort); - - if (filenames.isEmpty()) - { - qDebug() << "no filenames found in" << dumpDir.path(); - return {}; - } - - qDebug() << "create results array" << filenames.size(); - auto resultOptionals = - QScopedPointer(new QList>(filenames.size())); - - QElapsedTimer t; - t.start(); - qDebug() << "open real files"; - std::cout << "here" << std::endl; - - std::transform( - std::execution::par_unseq, - filenames.begin(), - filenames.end(), - resultOptionals->begin(), - [dirToRead](const auto& filename) { - // std::cout << '.'; - auto rawProfile = openRawProfile(dirToRead + "/" + filename); - if (rawProfile.has_value()) - { - QFile f(dirToRead + "/" + filename + ".bin"); - qDebug() << "save" << dirToRead + "/" + filename << "too" - << f.fileName(); - f.open(QFile::WriteOnly); - f.write((const char*)&rawProfile.value(), sizeof(Pixels)); - } - return rawProfile; - // return openRawProfile(dirToRead + "/" + filename); - } - ); - - filenames.clear(); - filenames.squeeze(); - - qDebug() << Q_FUNC_INFO << "open raw profiles: elapsed (ms)" << t.elapsed(); - // std::cout << std::endl; - - std::remove_if( - std::execution::par, - resultOptionals->begin(), - resultOptionals->end(), - [](auto& a) { return !a.has_value(); } - ); - - QList result(resultOptionals->size()); - - std::transform( - std::execution::par, - std::make_move_iterator(resultOptionals->begin()), - std::make_move_iterator(resultOptionals->end()), - result.begin(), - [](auto& p) { return p.value(); } - ); - - qDebug() << Q_FUNC_INFO << "elapsed (ms)" << t.elapsed(); - - return result; -} - -static std::optional openRawProfile(const QString& filePath) -{ - QFile f{filePath}; - - if (!f.open(QFile::ReadOnly)) - { - qWarning() << "cannot open file for reading:" << f.fileName(); - qWarning() << "error string:" << f.errorString(); - - return {}; - } - - // TODO: rewrite to remove manual serialization/deserialization - const auto json = QJsonDocument::fromJson(f.readAll()).object(); - const auto jsonCounters = json["counters"].toObject(); - - Pixels result; - result.counters.timestampUs = jsonCounters["timestampUs"].toInteger(); - result.counters.measurementCounter = - jsonCounters["measurementCounter"].toInteger(); - result.counters.encoderPosition = - jsonCounters["encoderPosition"].toInteger(); - - const auto jsonPixels = json["pixels"].toArray(); - // TODO: check json pixels count -#ifdef FIRST_COLUMN_ONLY - result.pixels[0] = jsonPixels.at(0).toDouble(); -#else - std::transform( - // std::execution::par_unseq, - jsonPixels.constBegin(), - jsonPixels.constEnd(), - result.pixels.begin(), - [](const auto& jsonPixel) { return jsonPixel.toDouble(); } - ); - - // for (size_t i = 0; i < jsonPixels.count() && i < result.pixels.size(); - // ++i) - // { - // result.pixels[i] = jsonPixels[i].toDouble(); - // } -#endif - - return result; -} - static QList filter(const QList& rawProfiles) { QList result; @@ -1315,184 +1155,6 @@ static QList filter(const QList& rawProfiles) return result; } -static QSharedPointer calibrateZ( - const QList& rawProfiles -) -{ - QSharedPointer 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(requested_params.stepsPerMm) - }; - - const auto& pixels = rawProfile.pixels; - // printPixels(pixels); - // static size_t counter { 0 }; - // qDebug() << "calibrated" << counter++; - const float pos = rawProfile.counters.encoderPosition; - const float divider = requested_params.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 {}; -} - -static QSharedPointer calibrateX( - const QList& rawProfiles -) -{ - QSharedPointer result{new CalibrationTable{{0}}}; - - for (const auto& rawProfile : rawProfiles) - { - const auto& pixels = rawProfile.pixels; - auto lines = pixelsToLines(rawProfile); - - Q_ASSERT_X(lines.count() > 2, Q_FUNC_INFO, "no lines"); - - QList 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 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]; - calibrationColumn[discretePixelValue] = xMmValue; - } - } - - return result; -} - bool initLaser() { const QLatin1String pwmChip{"pwmchip2"}; @@ -1568,162 +1230,3 @@ bool initLaser() return true; } - -static QImage calibrationTableToImage( - const QSharedPointer& 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 columnIdx = 0; columnIdx < calibrationTable->size(); - ++columnIdx) - { - const auto& column = calibrationTable->at(columnIdx); - - for (size_t rowIdx = 0; rowIdx < column.size(); ++rowIdx) - { - // if (!qFuzzyIsNull(column.at(rowIdx))) - // { - // qDebug() << "here"; - // } - - bool hasValue = !qFuzzyIsNull(column.at(rowIdx)); - - if (hasValue) - { - // qDebug() << "x/y" << columnIdx << rowIdx; - ++notNull; - } - - result.setPixel(columnIdx, rowIdx, hasValue ? 255 : 0); - } - } - qDebug() << "not null count" << notNull; - - return result; -} - -static void interpolate(QSharedPointer& table) -{ -#ifdef FIRST_COLUMN_ONLY - interpolate(table->at(0)); -#else - std::for_each( - std::execution::par, - table->begin(), - table->end(), - [](auto& column) { interpolate(column); } - ); -#endif -} - -static void interpolate(CalibrationColumn& column) -{ - size_t counter = 0; - // qDebug() << "AZAZA" << 0; - -#ifdef FIRST_COLUMN_ONLY - { - QFile f("/tmp/column0.csv"); - f.open(QFile::WriteOnly); - - for (const auto& p : column) - { - f.write(QString::number(p).toUtf8()); - f.write(";\n"); - } - } -#endif - // qDebug() << "AZAZA" << 1; - auto firstNonZero = - std::find_if(column.begin(), column.end(), [](const auto& pixel) { - return !qFuzzyIsNull(pixel); - }); - // qDebug() << "AZAZA" << 2; - if (firstNonZero == column.end()) - { - return; - } - // qDebug() << "AZAZA" << 3; - for (auto it = firstNonZero; it != column.cend(); ++it) - { - auto firstZero = - std::find_if(it + 1, column.end(), [](const auto& pixel) { - return qFuzzyIsNull(pixel); - }); - - if (firstZero == column.end()) - { - // qDebug() << "AZAZA" << 4; - return; - } - - auto nextNonZero = - std::find_if(firstZero + 1, column.end(), [](const auto& pixel) { - return !qFuzzyIsNull(pixel); - }); - - if (nextNonZero == column.end()) - { - // qDebug() << "AZAZA" << 5 << it - firstNonZero; - return; - } - - auto prevNonZero = firstZero - 1; - - auto diff = *nextNonZero - *prevNonZero; - auto size = nextNonZero - prevNonZero; - auto stepDiff = float(diff) / size; - - // qDebug) << *prevNonZero << *nextNonZero << size << stepDiff; - - for (auto zero = firstZero; zero < nextNonZero; ++zero) - { - *zero = (zero - firstZero + 1) * stepDiff; - // qDebug() << "set zero to" << *zero; - } - - it = nextNonZero; - } - // qDebug() << "AZAZA" << 6; -#ifdef FIRST_COLUMN_ONLY - { - QFile f("/tmp/column0_filled.csv"); - if (!f.open(QFile::WriteOnly)) - { - qDebug() << "AZAZA: cannot open file" << f.fileName(); - qDebug() << "AZAZA" << 7; - exit(0); - } - else - { - qDebug() << "AZAZA: ok"; - } - - for (const auto& p : column) - { - f.write(QString::number(p).toUtf8()); - f.write(";\n"); - } - } -#endif - // qDebug() << "AZAZA" << 8; -} diff --git a/printerclient.cpp b/printerclient.cpp index a78de50..ac269b3 100644 --- a/printerclient.cpp +++ b/printerclient.cpp @@ -19,7 +19,8 @@ QString getFirstTtyUSB() } PrinterClient::PrinterClient(QObject* parent) - : QObject{parent} // , m_serialPort{new QSerialPort{"/dev/ttyUSB0", this}} + : QObject{parent} + // , m_serialPort{new QSerialPort{"/dev/ttyUSB0", this}} , m_serialPort{new QSerialPort{getFirstTtyUSB(), this}} { if (!m_serialPort->setBaudRate(QSerialPort::Baud115200)) { @@ -76,7 +77,7 @@ void PrinterClient::onReadyRead() const auto data = m_serialPort->readAll(); qDebug() << "serialPort: " << data; - emit newData(data); + // emit newData(data); } void PrinterClient::onErrorOccured(QSerialPort::SerialPortError error) diff --git a/printerclient.h b/printerclient.h index 5f4f2a3..08bf0c2 100644 --- a/printerclient.h +++ b/printerclient.h @@ -6,21 +6,23 @@ class QSerialPort; -class PrinterClient : public QObject +class PrinterClient + : public QObject { - Q_OBJECT + // Q_OBJECT public: explicit PrinterClient(QObject *parent = nullptr); - ~PrinterClient() override = default; + // ~PrinterClient() override = default; + // ~PrinterClient -signals: - void newData(const QString output); +// signals: +// void newData(const QString output); -public slots: +public: void sendCommand(const QString command); -private slots: +private: void onReadyRead(); void onErrorOccured(QSerialPort::SerialPortError error); 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 + +#include +#include + +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}}; +} diff --git a/profile.h b/profile.h new file mode 100644 index 0000000..0e2f839 --- /dev/null +++ b/profile.h @@ -0,0 +1,29 @@ +#pragma once + +#include +#include + +#include "calibration.h" +#include "pixels.h" + +class Profile +{ +public: + using PointsMm = std::array; + +public: + // TODO: make private/protected + explicit Profile(const Pixels& pixels, + const CalibrationTablePtr calibrationTableZ, + const CalibrationTablePtr calibrationTableX); + +public: + const Counters& counters() const; + const PointsMm& pointsMm() const; + + operator const QJsonObject() const; + +private: + Counters m_counters{}; + PointsMm m_pointsMm{QPointF{std::nan(""), std::nan("")}}; +}; 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 +#include + +#include +#include +#include +#include +#include +#include +#include + +#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& 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& 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 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 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&& calibrationPixels) +{ + std::vector 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"; +} diff --git a/src/calibration.h b/src/calibration.h new file mode 100644 index 0000000..ddb66a6 --- /dev/null +++ b/src/calibration.h @@ -0,0 +1,31 @@ +#pragma once + +#include + +#include "fuck_intel.h" + +#include + +#include "constants.h" +#include "pixels.h" + +using CalibrationColumn = std::array; +// map [0; discretesInRage] (discretes) to [0; zRangeMm] (mm) +using CalibrationTable = std::array; +using CalibrationTablePtr = QSharedPointer; + +constexpr auto calibrationColumnHeight = std::tuple_size(); + +bool openCalibrationTable(const QString &filename, CalibrationTablePtr &table); + +void dumpCalibrationPixels(std::vector &&calibrationPixels); +bool dump(const CalibrationTablePtr &table, const QString &filename); + +CalibrationTablePtr calibrateX(const QList &rawProfiles); +CalibrationTablePtr calibrateZ(const QList &rawProfiles, + const uint32_t &stepsPerMm); + +QImage calibrationTableToImage(const CalibrationTablePtr &calibrationTable); + +void interpolate(CalibrationTablePtr &table); +void interpolate(CalibrationColumn &column); diff --git a/src/constants.h b/src/constants.h new file mode 100644 index 0000000..1c451ab --- /dev/null +++ b/src/constants.h @@ -0,0 +1,23 @@ +#pragma once + +#include +#include + +#include + +constexpr size_t img_width = 1280; +constexpr size_t img_height = 800; +constexpr uint32_t patternSize = 16; + +constexpr float hardcodedZRangeMm{175.f}; +constexpr size_t calibrationTableHeight{0x4000}; // 16384 + +namespace { +constexpr uint16_t discretesInRage{16384}; +} + +// http json keys +const QString exposureTimeKey = "exposureTime"; +const QString laserLevelKey = "laserLevel"; + +const QString dumpsRoot{QStringLiteral("/home/user/dumps")}; diff --git a/src/dumps.cpp b/src/dumps.cpp new file mode 100644 index 0000000..43a839c --- /dev/null +++ b/src/dumps.cpp @@ -0,0 +1,141 @@ +#include "dumps.h" + +#include +#include +#include +#include +#include +#include +#include + +#include + +QList openDump( + const QString &dumpPath) +{ + QString dirToRead{dumpPath}; + qDebug() << "trying to open dump path:" << dirToRead; + + if (dirToRead.isEmpty()) { + qDebug() << "dumpPath not specified. looking into" << dumpsRoot; + + QDir dumpsRootDir{dumpsRoot}; + + const auto filter = QDir::Dirs | QDir::NoDotAndDotDot | QDir::Readable; + // there is no battery in my rpi5 for now + const auto sort = QDir::Name; + const auto entries = dumpsRootDir.entryList(filter, sort); + + if (entries.isEmpty()) { + qWarning() << "dumps root" << dumpsRoot << "contains no dumps. " + << "specify existing dump path"; + + return {}; + } + + dirToRead = dumpsRoot + "/" + entries.last(); + } + + QDir dumpDir{dirToRead}; + + // const QStringList nameFilters{"*.bin"}; + const QStringList nameFilters{}; + const auto filter = QDir::Files; + const auto sort = QDir::Name; + + auto filenames = dumpDir.entryList(nameFilters, filter, sort); + + if (filenames.isEmpty()) { + qDebug() << "no filenames found in" << dumpDir.path(); + return {}; + } + + qDebug() << "create results array" << filenames.size(); + auto resultOptionals = QScopedPointer( + new QList>(filenames.size())); + + QElapsedTimer t; + t.start(); + qDebug() << "open real files"; + std::cout << "here" << std::endl; + + std::transform( + std::execution::par_unseq, + filenames.begin(), + filenames.end(), + resultOptionals->begin(), + [dirToRead](const auto &filename) { + // std::cout << '.'; + // auto rawProfile = openRawProfile(dirToRead + "/" + filename); + auto rawProfile = Pixels::load(dirToRead + "/" + filename); + + return rawProfile; + }); + + filenames.clear(); + filenames.squeeze(); + + qDebug() << Q_FUNC_INFO << "open raw profiles: elapsed (ms)" << t.elapsed(); + // std::cout << std::endl; + + std::remove_if(std::execution::par, + resultOptionals->begin(), + resultOptionals->end(), + [](auto &a) { return !a.has_value(); }); + + QList result(resultOptionals->size()); + + std::transform(std::execution::par, + std::make_move_iterator(resultOptionals->begin()), + std::make_move_iterator(resultOptionals->end()), + result.begin(), + [](auto &p) { return p.value(); }); + + qDebug() << Q_FUNC_INFO << "elapsed (ms)" << t.elapsed(); + + return result; +} + +std::optional openRawProfile( + const QString &filePath) +{ + QFile f{filePath}; + + if (!f.open(QFile::ReadOnly)) { + qWarning() << "cannot open file for reading:" << f.fileName(); + qWarning() << "error string:" << f.errorString(); + + return {}; + } + + // TODO: rewrite to remove manual serialization/deserialization + const auto json = QJsonDocument::fromJson(f.readAll()).object(); + const auto jsonCounters = json["counters"].toObject(); + + Pixels result; + result.counters.timestampUs = jsonCounters["timestampUs"].toInteger(); + result.counters.measurementCounter = jsonCounters["measurementCounter"] + .toInteger(); + result.counters.encoderPosition = jsonCounters["encoderPosition"].toInteger(); + + const auto jsonPixels = json["pixels"].toArray(); + // TODO: check json pixels count +#ifdef FIRST_COLUMN_ONLY + result.pixels[0] = jsonPixels.at(0).toDouble(); +#else + std::transform( + // std::execution::par_unseq, + jsonPixels.constBegin(), + jsonPixels.constEnd(), + result.pixels.begin(), + [](const auto &jsonPixel) { return jsonPixel.toDouble(); }); + +// for (size_t i = 0; i < jsonPixels.count() && i < result.pixels.size(); +// ++i) +// { +// result.pixels[i] = jsonPixels[i].toDouble(); +// } +#endif + + return result; +} diff --git a/src/dumps.h b/src/dumps.h new file mode 100644 index 0000000..8aa8dc9 --- /dev/null +++ b/src/dumps.h @@ -0,0 +1,6 @@ +#pragma once + +#include "pixels.h" + +QList openDump(const QString &dumpPath = ""); +std::optional openRawProfile(const QString &filePath); diff --git a/src/pixels.cpp b/src/pixels.cpp new file mode 100644 index 0000000..b314e6a --- /dev/null +++ b/src/pixels.cpp @@ -0,0 +1,95 @@ +#include "pixels.h" + +#include +#include +#include +#include + +Pixels& Pixels::operator+=(const Pixels& other) +{ + std::transform( + std::execution::par, + pixels.begin(), + pixels.end(), + other.pixels.begin(), + pixels.begin(), + // [](auto& toAdd) { return dst += src; }); + std::plus<>() + ); + + return *this; +} + +Pixels& Pixels::operator/=(const float divider) +{ + std::for_each( + std::execution::par_unseq, + pixels.begin(), + pixels.end(), + [divider](auto& pixel) { pixel /= divider; } + ); + + return *this; +} + +std::optional Pixels::load(const QString& filename) +{ + const std::filesystem::path filepath{filename.toStdString()}; + + if (!std::filesystem::exists(filepath)) + { + std::cerr << "no such file: " << filepath << std::endl; + + return {}; + } + + std::ifstream ifs(filepath, std::ios::in | std::ios::binary); + + if (!ifs) + return {}; + + Pixels result; + ifs.read(reinterpret_cast(&result), sizeof(Pixels)); + ifs.close(); + + if (!ifs) + { + std::cerr << "cannot read " << filepath << std::endl; + + return {}; + } + + return result; +} + +bool Pixels::save(const QString& filename) +{ + const std::filesystem::path filepath{filename.toStdString()}; + const auto parent_path = filepath.parent_path(); + + if (!std::filesystem::exists(parent_path) && + !std::filesystem::create_directories(parent_path)) + { + std::cerr << "cannot create parent directory for file " << filepath + << std::endl; + + return false; + } + + std::ofstream ofs(filepath, std::ios::out | std::ios::binary); + + if (!ofs) + return false; + + ofs.write(reinterpret_cast(this), sizeof(Pixels)); + ofs.close(); + + if (!ofs) + { + std::cerr << "cannot write " << filepath << std::endl; + + return false; + } + + return true; +} diff --git a/src/pixels.h b/src/pixels.h new file mode 100644 index 0000000..6095220 --- /dev/null +++ b/src/pixels.h @@ -0,0 +1,32 @@ +#pragma once + +#include + +#include "fuck_intel.h" + +#include + +#include "typedefs.h" + +struct Pixels +{ + Counters counters{}; + std::array pixels{0.f}; + + Pixels& operator+=(const Pixels& other); + Pixels& operator/=(const float divider); + + // TODO: tests for everything everywhere + /*! + * \brief load - load binary pixels from file + * \param filename - file path + * \return Pixels on success, empty std::optional otherwise + */ + [[nodiscard]] static std::optional load(const QString& filename); + /*! + * \brief save - save binary profile to file + * \param filename - file path + * \return - true on success, false otherwise + */ + [[nodiscard]] bool save(const QString& filename); +}; diff --git a/src/typedefs.h b/src/typedefs.h new file mode 100644 index 0000000..2af2921 --- /dev/null +++ b/src/typedefs.h @@ -0,0 +1,33 @@ +#pragma once + +#include + +#include "constants.h" + +struct Counters +{ + uint32_t timestampUs{0}; + uint32_t measurementCounter{0}; + int32_t encoderPosition{0}; +}; + +struct Image +{ + int width; + int height; + uint16_t data[img_height][img_width]; + uint16_t rotated_cw[img_width][img_height]; + size_t dataSize; + unsigned int stride; + libcamera::PixelFormat pixelFormat; + Counters counters{}; +}; + +struct requested_params_t +{ + int32_t exposureTime = {1000}; + int32_t laserLevel = {3000}; + uint32_t stepsPerMm{200}; +}; + +enum ScanningModeFlags : uint8_t { None = 0, Calibration }; -- cgit v1.2.3-70-g09d2