From 5e6df9ccf832d166bd231297b521394beec1fec6 Mon Sep 17 00:00:00 2001 From: Nikita Kostovsky Date: Mon, 2 Dec 2024 19:42:15 +0100 Subject: line search --- main.cpp | 432 +++++++++++++++++++++++++++++++++++++++++++++++++++++---------- 1 file changed, 363 insertions(+), 69 deletions(-) (limited to 'main.cpp') diff --git a/main.cpp b/main.cpp index 33f94ee..06b7700 100644 --- a/main.cpp +++ b/main.cpp @@ -1,13 +1,21 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include "LibCamera.h" +#include "printerclient.h" #include "httpservice.h" #include "genetic_algos.h" -#include "imagealgos.h" -#include "LibCamera.h" #include "pigpio.h" -#include "printerclient.h" #include "rotaryencoder.h" +#include "imagealgos.h" +#include "fuck_intel.h" -#define QT_NO_KEYWORDS #include #include #include @@ -20,14 +28,6 @@ #include #include #include -#undef QT_NO_KEYWORDS - -#include -#include -#include -#include -#include -#include #define try_apply_config() \ if(!applyConfig(config)) \ @@ -76,21 +76,83 @@ const QString dumpsRoot { QStringLiteral("/home/user/dumps") }; using namespace std::chrono_literals; -static std::shared_ptr camera; -std::unique_ptr config; +static std::shared_ptr camera; +std::unique_ptr config; static std::map> mappedBuffers_; -std::vector> requests; -ControlList lastControls; - -static bool applyConfig(const std::unique_ptr & config); -static void onRequestCompleted(Request *completed_request); +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 }} }; + +static bool applyConfig(const std::unique_ptr & config); +static void onRequestCompleted(libcamera::Request *completed_request); static void printControls(); static void dumpCalibrationPixels(); -static std::vector openDump(const QString dumpPath = ""); +static QList filter(const QList& rawProfiles); +static QSharedPointer calibrate(const QList& rawProfiles); + +auto printPixels = [](const auto& pixels) { + for (size_t i = (img_width - 10) / 2; i < img_width - ((img_width - 10) / 2); ++i) + { + std::cout << pixels[i] << " "; + } + std::cout << std::endl; +}; + +bool initLaser(); + +class A +{ +public: + A(const int var) : m_var{ var } { std::cout << m_var << std::endl; } + +private: + int m_var { 0 }; +}; + +class B : public A +{ +public: + B(const int var) : A{var} {} +}; int main(int argc, char *argv[]) { QCoreApplication app(argc, argv); + // if (!initLaser()) + // { + // return EXIT_FAILURE; + // } + + if (false) + { + // 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"); + qDebug() << "raw profiles count is" << rawProfiles.size(); + // qDebug() << "height" << calibrationColumnHeight; + + auto filteredRawProfiles = filter(rawProfiles); + qDebug() << "filtered profiles count is" << filteredRawProfiles.count(); + + auto calibrationTable = calibrate(filteredRawProfiles); + // auto calibrationTable = calibrate(rawProfiles); + // CalibrationTable* aaa { new CalibrationTable {{ 0 }} }; + // (*aaa)[1][0] = 123; + // CalibrationTable bbb {{ 0 }}; + // bbb[1][0] = 123; + + // float* values = new float[calibrationColumnHeight]; + } + QElapsedTimer t; t.start(); @@ -102,7 +164,7 @@ int main(int argc, char *argv[]) { qDebug() << "msecs before camera:" << t.elapsed(); // FIXME: don't use one var for everything int ret; - std::unique_ptr cm = std::make_unique(); + std::unique_ptr cm = std::make_unique(); cm->start(); const auto cameras = cm->cameras(); @@ -139,7 +201,7 @@ int main(int argc, char *argv[]) { // FIXME: nullptr // std::unique_ptr config = camera->generateConfiguration( { StreamRole::Viewfinder } ); - /*std::unique_ptr */config = camera->generateConfiguration( { StreamRole::Raw } ); + /*std::unique_ptr */config = camera->generateConfiguration( { libcamera::StreamRole::Raw } ); if (config->empty()) { @@ -162,7 +224,7 @@ int main(int argc, char *argv[]) { // } // FIXME: nullptr - StreamConfiguration &streamConfig = config->at(0); + libcamera::StreamConfiguration &streamConfig = config->at(0); std::cout << "Default viewfinder configuration is: " << streamConfig.toString() << std::endl; std::cout << "Pixel format is: " << streamConfig.pixelFormat.toString() << std::endl; std::cout << "Buffer count is: " << streamConfig.bufferCount << std::endl; @@ -179,7 +241,7 @@ int main(int argc, char *argv[]) { // streamConfig.pixelFormat = PixelFormat::fromString("Y8_1X8"); // streamConfig.pixelFormat = formats::R8; - streamConfig.pixelFormat = formats::R16; + streamConfig.pixelFormat = libcamera::formats::R16; streamConfig.bufferCount = 2; // what is default R10_CSI2P? MONO_PISP_COMP1? // MONO_PISP_COMP1 - check rpicam-apps sources for decoding algos @@ -215,7 +277,7 @@ int main(int argc, char *argv[]) { // TODO: try custom FrameBufferAllocator and compare performance - auto allocator = std::make_shared(camera); + auto allocator = std::make_shared(camera); auto stream = streamConfig.stream(); @@ -232,13 +294,13 @@ int main(int argc, char *argv[]) { size_t allocated = size_t(ret); std::cout << "Allocated " << allocated << " buffers for stream" << std::endl; - const std::vector> &buffers = allocator->buffers(stream); + const std::vector> &buffers = allocator->buffers(stream); // for (size_t i = 0; i < buffers.size(); ++i) static int expOffset = 0; for (const auto & buffer : buffers) { - std::unique_ptr request = camera->createRequest(); + std::unique_ptr request = camera->createRequest(); if (!request) { @@ -328,7 +390,7 @@ int main(int argc, char *argv[]) { for (size_t i = 0; i < img_width; ++i) { // pixels << img_height - img.pixels[i]; - pixels << img_height - ::pixels.pixels[i]; + pixels << ::pixels.pixels[i]; } QJsonObject json; @@ -337,6 +399,22 @@ int main(int argc, char *argv[]) { json["measurementCounter"] = qint64 { img.counters.measurementCounter }; json["timestampUs"] = qint64(img.counters.timestampUs); + const auto lines = pixelsToLines(::pixels); + + qDebug() << "lines count is " << lines.count(); + + QJsonArray jsonLines; + + for (const auto& l : lines) + { + jsonLines << QJsonArray { + QJsonArray { l.p1().x(), l.p1().y() }, + QJsonArray { l.p2().x(), l.p2().y() } + }; + } + + json["lines"] = jsonLines; + return QHttpServerResponse(QJsonDocument(json).toJson()); }); @@ -403,14 +481,14 @@ int main(int argc, char *argv[]) { std::lock_guard lg(pgm_image_mtx); QJsonObject json; - const ControlIdMap & ctrlIdMap = camera->controls().idmap(); + const libcamera::ControlIdMap & ctrlIdMap = camera->controls().idmap(); qDebug() << "readParams:" << lastControls.size(); qDebug() << request.method(); for (const auto & [id, value]: lastControls) { - const ControlId * controlId = ctrlIdMap.at(id); + const libcamera::ControlId * controlId = ctrlIdMap.at(id); auto name = QString::fromStdString(controlId->name()); const auto valueStr = QString::fromStdString(value.toString()); qDebug() << "\t param:" @@ -611,8 +689,10 @@ int main(int argc, char *argv[]) { * important not to block the thread for a long time, as this blocks internal * processing of the camera pipelines, and can affect realtime performance. */ -void onRequestCompleted(Request *completed_request) +void onRequestCompleted(libcamera::Request *completed_request) { + using namespace libcamera; + static std::chrono::steady_clock::time_point fpsTimstamp = std::chrono::steady_clock::now(); QElapsedTimer t; @@ -683,8 +763,7 @@ void onRequestCompleted(Request *completed_request) // qDebug() << "calibration mode" << scanningModeFlags; if (scanningModeFlags == ScanningModeFlags::Calibration) { - constexpr int32_t hardcodedZRangeMm { 175 }; - const int32_t maxEncoderPosition = hardcodedZRangeMm * requested_params.stepsPerMm; + const int32_t maxEncoderPosition = int32_t(hardcodedZRangeMm) * requested_params.stepsPerMm; // qDebug() << "calibration max range" << maxEncoderPosition; // qDebug() << "calibration encoder pos" << pixels.counters.encoderPosition; if (pixels.counters.encoderPosition >= 0 && @@ -789,12 +868,14 @@ void onRequestCompleted(Request *completed_request) } -static bool applyConfig(const std::unique_ptr & config) +static bool applyConfig(const std::unique_ptr & config) { + using namespace libcamera; + auto status = config->validate(); // WARNING: unsafe - StreamConfiguration &streamConfig = config->at(0); + libcamera::StreamConfiguration &streamConfig = config->at(0); switch (status) { case CameraConfiguration::Status::Valid: @@ -819,7 +900,8 @@ static bool applyConfig(const std::unique_ptr & config) static void printControls() { - const ControlInfoMap & control_map = camera->controls(); + using namespace libcamera; + const libcamera::ControlInfoMap & control_map = camera->controls(); // for (const auto & [id, info]: control_map) for (const std::pair & pair : control_map) @@ -888,10 +970,10 @@ static void dumpCalibrationPixels() } QJsonObject jsonCounters { - { "timestampUs", qint64(rawProfile.counters.timestampUs) }, - { "measurementCounter", qint64(rawProfile.counters.measurementCounter) }, - { "encoderPosition", qint64(rawProfile.counters.encoderPosition) }, - }; + { "timestampUs", qint64(rawProfile.counters.timestampUs) }, + { "measurementCounter", qint64(rawProfile.counters.measurementCounter) }, + { "encoderPosition", qint64(rawProfile.counters.encoderPosition) }, + }; QJsonObject json; @@ -920,11 +1002,10 @@ static void dumpCalibrationPixels() qDebug() << "dump finished"; } -static std::vector openDump(const QString dumpPath) +static QList openDump(const QString &dumpPath) { - std::vector result; - QString dirToRead { dumpPath }; + qDebug() << "trying to open dump path:" << dirToRead; if (dirToRead.isEmpty()) { @@ -945,10 +1026,10 @@ static std::vector openDump(const QString dumpPath) return {}; } - dirToRead = entries.last(); + dirToRead = dumpsRoot + "/" + entries.last(); } - QDir dumpDir { dumpsRoot + "/" + dirToRead }; + QDir dumpDir { dirToRead }; const auto filter = QDir::Files; const auto sort = QDir::Name; @@ -958,47 +1039,260 @@ static std::vector openDump(const QString dumpPath) if (filenames.isEmpty()) { qDebug() << "no filenames found in" << dumpDir.path(); + return {}; } - for (const auto& filename : filenames) + QList> resultOptionals { filenames.size() }; + + QElapsedTimer t; + t.start(); + + std::transform( + std::execution::par, + filenames.begin(), filenames.end(), + resultOptionals.begin(), + [dirToRead](const auto& filename) { + std::cout << '.'; + return openRawProfile(dirToRead + "/" + filename); + }); + + 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)) { - qDebug() << "raw profile:" << filename; + 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(); - QFile f { dumpDir.path() + "/" + filename }; + Pixels result; + result.counters.timestampUs = jsonCounters["timestampUs"].toInteger(); + result.counters.measurementCounter = jsonCounters["measurementCounter"].toInteger(); + result.counters.encoderPosition = jsonCounters["encoderPosition"].toInteger(); - if (!f.open(QFile::ReadOnly)) + const auto jsonPixels = json["pixels"].toArray(); + + for (size_t i = 0; i < jsonPixels.count() && i < result.pixels.size(); ++i) + { + result.pixels[i] = jsonPixels[i].toDouble(); + } + + return result; +} + +static QList filter(const QList& rawProfiles) +{ + QList result; + + QList::const_iterator it = rawProfiles.constBegin(); + + // for (size_t i = 0; i < 10; ++i) + // { + // std::cout << "pos - " << rawProfiles.at(i).counters.encoderPosition << std::endl; + // } + + // ++it; + while (it != rawProfiles.constEnd()) + { + Pixels sum = *it; + std::cout << "current pos is " << sum.counters.encoderPosition << std::endl; + + printPixels(sum.pixels); + + size_t count { 1 }; + ++it; + + std::cout << "here\n"; + while (it != rawProfiles.constEnd() && it->counters.encoderPosition == sum.counters.encoderPosition) { - qWarning() << "cannot open file for reading:" << f.fileName(); - qWarning() << "error string:" << f.errorString(); + std::cout << "here2\n"; + std::cout << "\tadd to pos " << it->counters.encoderPosition << std::endl; + sum += *it; + std::cout << "\tadded" << std::endl; + ++count; + ++it; - return {}; } - // TODO: rewrite to remove manual serialization/deserialization - const auto json = QJsonDocument::fromJson(f.readAll()).object(); - const auto jsonCounters = json["counters"].toObject(); - qDebug() << jsonCounters; + std::cout << "here3\n"; + sum /= float(count); + printPixels(sum.pixels); - Pixels rawProfile; + result << sum; - rawProfile.counters.timestampUs = jsonCounters["timestampUs"].toInteger(); - rawProfile.counters.measurementCounter = jsonCounters["measurementCounter"].toInteger(); - rawProfile.counters.encoderPosition = jsonCounters["encoderPosition"].toInteger(); + // return result; + } - const auto jsonPixels = json["pixels"].toArray(); - qDebug() << jsonPixels.count() << rawProfile.pixels.size(); + return result; +} + +static QSharedPointer calibrate(const QList& rawProfiles) +{ + QSharedPointer result { new CalibrationTable {{ 0 }} }; - for (size_t i = 0; i < jsonPixels.count() && i < rawProfile.pixels.size(); ++i) + constexpr uint16_t discretesInRage { 16384 }; + + 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) { - rawProfile.pixels[i] = jsonPixels[i].toDouble(); + 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 << ";"; + } } - } - // { - // QMutexLocker l(&calibrationPixelsMutex); - // std::swap(result, ::calibrationPixels); - // } + std::cout << std::endl << std::flush; + } return result; + // TODO: try something interesting + // return {}; +} + +bool initLaser() +{ + const QLatin1String pwmChip { "pwmchip2" }; + const uint16_t pwmChannel { 1 }; + const QLatin1String pwmSystemRoot { "/sys/class/pwm" }; + const QString pwmChipRoot { pwmSystemRoot + "/" + pwmChip }; + + const QString pwmExportFile { pwmChipRoot + "/export" }; + + QFile f { pwmExportFile }; + + if (!f.open(QFile::WriteOnly)) + { + qWarning() << "cannot open" << f.fileName() << "for writing"; + qWarning() << "error:" << f.errorString(); + + return false; + } + + QTextStream s { &f }; + s << pwmChannel; + + const QString pwm { QLatin1String("pwm%1").arg(QString::number(pwmChannel)) }; + const QString pwmRoot { pwmChipRoot + "/" + pwm }; + + const QString periodFilename { pwmRoot + "/period" }; + f.close(); + f.setFileName(periodFilename); + + if (!f.open(QFile::WriteOnly)) + { + qWarning() << "cannot open" << f.fileName() << "for writing"; + qWarning() << "error:" << f.errorString(); + + return false; + } + + const unsigned periodHz { 50'000 }; + + s << periodHz; + + const QString dutyCycleFilename { pwmRoot + "/duty_cycle" }; + f.close(); + f.setFileName(dutyCycleFilename); + + if (!f.open(QFile::WriteOnly)) + { + qWarning() << "cannot open" << f.fileName() << "for writing"; + qWarning() << "error:" << f.errorString(); + + return false; + } + + const unsigned dutyCycle { 3'000 }; + + s << dutyCycle; + + const QString enableFilename { pwmRoot + "/enable" }; + f.close(); + f.setFileName(enableFilename); + + if (!f.open(QFile::WriteOnly)) + { + qWarning() << "cannot open" << f.fileName() << "for writing"; + qWarning() << "error:" << f.errorString(); + + return false; + } + + const int enable { 1 }; + + s << enable; + + return true; } -- cgit v1.2.3-70-g09d2