src/capture/CaptureWorker.cpp - master - klischa/AstraScanner2

src/capture/CaptureWorker.cpp
782 строки38 KB
Arena Agent
fix: neural ICP confirm, cumulative transform direction, CMake cleanup, mesh smoothing, multi-seed BFS
02 авг 2026, 21:44
02 авг 2026, 21:4410986cc
#include "CaptureWorker.h"
#include "AstraCamera.h"
#include "settings/SettingsManager.h"
#include "project/ProjectManager.h"
#include "../marker_tracker/MarkerMap.h"
#include "../marker_tracker/MarkerTracker.h"
#include "../services/RegistrationService.h"
#include "../tracking/NeuralTrackingPipeline.h"
#include <QThread>
#include <QCoreApplication>
#include <QDebug>
#include <QFile>
#include <QSharedPointer>
#include <QElapsedTimer>
#include <QMutexLocker>
#include <opencv2/opencv.hpp>
#include <pcl/filters/voxel_grid.h>
#include <algorithm>
#include <cmath>
#include "DepthCloudConverter.h"  // вынесенная конвертация depth→cloud
#include <limits>
#include <nlohmann/json.hpp>
#include <fstream>

namespace {
constexpr bool kVerboseCaptureLoopLogs = false;
constexpr bool kVerbosePointCloudLogs = false;
}

// Вспомогательная функция для логирования времени выполнения
static void logTimer(QElapsedTimer& timer, const QString& label) {
    qDebug() << "[Timer]" << label << "took" << timer.restart() << "ms";
}

CaptureWorker::CaptureWorker(QObject *parent) : QObject(parent)
    , m_neuralPipeline(std::make_unique<NeuralTrackingPipeline>())
{
    m_disk_diameter_mm = 300.0f;
    m_marker_count = 7;
    m_min_circularity = 0.75f;
    m_min_convexity = 0.75f;
    m_min_inertia_ratio = 0.5f;
    m_min_radius_px = 10.0f;
    m_max_radius_px = 35.0f;
    m_min_area = 200.0f;
    m_max_area = 2800.0f;
}

CaptureWorker::~CaptureWorker() = default;

void CaptureWorker::requestStop()
{
    m_running.store(false, std::memory_order_release);
    // IMPORTANT: do NOT call camera methods via m_activeCamera from a foreign
    // thread here. process() creates `AstraCamera camera` as a stack local
    // and sets m_activeCamera = &camera only while that stack frame is live.
    // requestStop() may race with process() teardown (m_activeCamera already
    // nulled but the thread still running, or — worse — the pointer already
    // pointing to the just-destroyed stack frame). Unblocking color capture
    // is performed at the top of the capture loop (checks m_running and
    // releases via a thread-safe flag consumed inside readFrame()). Instead,
    // we simply request the camera to stop via a thread-safe flag consumed
    // inside AstraCamera::readFrame(). Release of the color capture happens
    // safely in the worker thread itself (see process() cleanup).
}

void CaptureWorker::process()
{
    QElapsedTimer globalTimer;
    globalTimer.start();
    
    QThread::currentThread()->setObjectName("CaptureWorker");
    qDebug() << "[Worker] Thread started:" << QThread::currentThread();
    m_firstPointLogged = false;

    qDebug() << "[Worker] Step 1: Starting process() method (" << globalTimer.elapsed() << "ms)";

    AstraCamera camera;
    m_activeCamera.store(&camera, std::memory_order_release);
    try {
        // Инициализация компонентов для оптического трекинга
        qDebug() << "[Worker] Step 2: Creating MarkerTracker (" << globalTimer.elapsed() << "ms)";
        m_marker_tracker = std::make_unique<MarkerTracker>();
        qDebug() << "[Worker] Step 3: Creating MarkerMap (" << globalTimer.elapsed() << "ms)";
        m_marker_map = std::make_unique<MarkerMap>();
        qDebug() << "[Worker] Step 4: Markers initialized (" << globalTimer.elapsed() << "ms)";

        const auto& settings = SettingsManager::instance();
        m_marker_size_mm = static_cast<float>(settings.markerSizeMm());
        m_neuralPipeline->loadThresholds();
        qDebug() << "[Worker] Step 5: Settings loaded (" << globalTimer.elapsed() << "ms)";

        // Получаем тип детектора из настроек и инициализируем MarkerTracker
        QString detectorType = settings.markerDetectorType();
        int numRings = settings.cctagNumRings();
        float minIdentProba = settings.cctagMinIdentProba();

        // MarkerMap сопоставляет маркеры по ID. Circular-детектор ID не даёт,
        // поэтому в MarkerBased-режиме автоматически используем CCTag, если он
        // скомпилирован, либо предупреждаем и отключаем live marker pose ниже.
        const bool markerBasedRequested =
            m_project_manager && m_project_manager->getTrackingMode() == TrackingMode::MarkerBased;
        if (markerBasedRequested && detectorType != QLatin1String("CCTag")) {
#ifdef ASTRA_ENABLE_CCTAG
            qWarning() << "[Worker] MarkerBased mode requires marker IDs; switching detector to CCTag";
            detectorType = QStringLiteral("CCTag");
#else
            qWarning() << "[Worker] MarkerBased mode requires marker IDs, but CCTag support is not compiled";
            emit warning(QStringLiteral("Маркерный режим требует CCTag-детектор с ID; CCTag не собран."));
#endif
        }
        
        qDebug() << "[Worker] Step 6: Updating MarkerTracker params (" << globalTimer.elapsed() << "ms)";
        qDebug() << "[Worker] About to call updateParams with detectorType=" << detectorType << ", numRings=" << numRings;
        m_marker_tracker->updateParams(detectorType, numRings, minIdentProba);
        qDebug() << "[Worker] MarkerTracker updated successfully";

        // Получаем режим трекинга из ProjectManager
        if (m_project_manager) {
            m_marker_tracking_enabled = (m_project_manager->getTrackingMode() == TrackingMode::MarkerBased);
            m_neuralPipeline->setEnabled(m_project_manager->getTrackingMode() == TrackingMode::NeuralBased);
#ifndef ASTRA_ENABLE_CCTAG
            if (m_marker_tracking_enabled) {
                m_marker_tracking_enabled = false;
                emit warning(QStringLiteral("Маркерный режим отключён: CCTag не собран, а Circular не имеет ID маркеров."));
            }
#endif
        }

        qDebug() << "[Worker] Step 7: Creating camera (" << globalTimer.elapsed() << "ms)";
        camera.setColorCameraEnabled(m_colorCameraEnabled.load());
        // initialize() всегда возвращает true — если железо недоступно, камера
        // переходит в эмуляцию и сообщает об этом через isEmulationActive().
        qDebug() << "[Worker] Step 8: Calling camera.initialize() (" << globalTimer.elapsed() << "ms)";
        camera.initialize();
        qDebug() << "[Worker] Step 9: Camera initialized (" << globalTimer.elapsed() << "ms)";
        
        // Загрузка конфигурации диска с маркерами — только для маркерного режима
        if (m_marker_tracking_enabled) {
            qDebug() << "[Worker] Step 10: Loading marker disk config (" << globalTimer.elapsed() << "ms)";
            loadMarkerDiskConfig();
            qDebug() << "[Worker] Step 11: Marker disk config loaded (" << globalTimer.elapsed() << "ms)";
            qDebug() << "[Worker] After loadMarkerDiskConfig: m_marker_count=" << m_marker_count;
        
        // Проверяем совместимость конфигурации с CCTag, но не переключаемся
        // автоматически на Circular: ниже по пайплайну карта маркеров требует
        // валидные ID, а Circular-детектор их не предоставляет.
        if (detectorType == "CCTag" && m_marker_count > 0) {
            const int cctagMaxId = (numRings == 4) ? 15 : 7;
            if (m_marker_count > cctagMaxId) {
                qWarning() << "[Worker] marker_count=" << m_marker_count
                           << "looks incompatible with CCTag" << numRings
                           << "rings (max supported ID" << cctagMaxId << ")";
                emit warning(QString("Конфигурация маркеров (%1) несовместима с CCTag %2 rings. "
                                     "Автопереключение на Circular отключено, чтобы не ломать pose tracking.")
                                 .arg(m_marker_count)
                                 .arg(numRings));
             }
        }
        } // end if (m_marker_tracking_enabled)

        qDebug() << "[Worker] Using marker detector:" << detectorType << "(numRings=" << numRings << ", markerCount=" << m_marker_count << ")";
        if (camera.isEmulationActive()) {
            const QString reason = QString::fromStdString(camera.getLastError());
            qWarning() << "[Worker] Camera emulation is active:" << reason;
            emit warning(QString("Камера недоступна, работает эмуляция. %1").arg(reason));
        }
        qInfo() << "[Worker] Camera initialized";
        
        // Вывод загруженных параметров для отладки
        qInfo() << "[Worker] Marker disk config loaded:";
        qInfo() << "  Disk diameter:" << m_disk_diameter_mm << "mm";
        qInfo() << "  Marker count:" << m_marker_count;
        qInfo() << "  Detection params: minCircularity=" << m_min_circularity
                << "minConvexity=" << m_min_convexity << "minInertiaRatio=" << m_min_inertia_ratio;
        qInfo() << "  Radius range:" << m_min_radius_px << "-" << m_max_radius_px << "px";
        qInfo() << "  Area range:" << m_min_area << "-" << m_max_area << "px²";

        // Инициализируем нейросетевой трекинг на основе режима трекинга
        if (m_project_manager) {
            m_neuralPipeline->setEnabled(m_project_manager->getTrackingMode() == TrackingMode::NeuralBased);
        }

    // Neural tracker is created in GUI thread via setNeuralTracker().
    // If not set yet, try creating here as fallback.
    if (m_neuralPipeline->isEnabled() && !m_neuralTrackerLoaded) {
        qWarning() << "[Worker] NeuralTracker not set via setNeuralTracker()! Creating as fallback.";
        auto tracker = std::make_unique<NeuralTracker>();
        if (tracker->loadModel("models/pose_regressor.onnx")) {
            qInfo() << "[Worker] NeuralTracker model loaded successfully (fallback)";
            m_neuralPipeline->setTracker(std::move(tracker));
            m_neuralTrackerLoaded = true;
        } else {
            qWarning() << "[Worker] Failed to load NeuralTracker model (fallback)";
            m_neuralPipeline->setEnabled(false);
        }
    }

    qDebug() << "[Worker] Step 14: Loading calibration (" << globalTimer.elapsed() << "ms)";
    loadCalibrationOrCameraIntrinsics(camera, globalTimer);
    qDebug() << "[Worker] Step 17: Setting marker tracker intrinsics (" << globalTimer.elapsed() << "ms)";
    configureMarkerDetector(globalTimer);

    qDebug() << "[Worker] Step 20: Starting camera streams (" << globalTimer.elapsed() << "ms)";
    qDebug() << "[Worker] Camera emulation: " << camera.isEmulationActive();
    if (!camera.startStreams()) {
        QString errMsg = QString::fromStdString(camera.getLastError());
        qCritical() << "[Worker] *** startStreams FAILED: " << errMsg << " ***";
        qCritical() << "[Worker] Camera emulation: " << camera.isEmulationActive();
        emit error(errMsg);
        emit finished();
        return;
    }
    qDebug() << "[Worker] Step 21: Camera streams started (" << globalTimer.elapsed() << "ms)";
    qInfo() << "[Worker] Streams started, cloud processing:" << m_cloudProcessingEnabled
            << "depth range:" << m_depthMin.load(std::memory_order_relaxed) << "-" << m_depthMax.load(std::memory_order_relaxed) << "m"
            << "color camera:" << m_colorCameraEnabled.load()
            << "marker tracking:" << m_marker_tracking_enabled.load();

    qDebug() << "[Worker] Step 22: Entering capture loop (" << globalTimer.elapsed() << "ms)";
    cv::Mat color, depth;
    int frameCounter = 0;
    while (m_running) {
        if (!m_running) break;
        if (kVerboseCaptureLoopLogs)
            qDebug() << "[Worker] Loop iteration start (" << globalTimer.elapsed() << "ms)";

        try {
        if (camera.readFrame(color, depth)) {
            if (!m_running) break;  // Check immediately after blocking read
            if (kVerboseCaptureLoopLogs)
                qDebug() << "[Worker] Frame read successful (" << globalTimer.elapsed() << "ms)";
            
            if (color.empty() || depth.empty()) {
                if (kVerboseCaptureLoopLogs)
                    qDebug() << "[Worker] Empty frame, skipping (" << globalTimer.elapsed() << "ms)";
                QThread::msleep(1);
                continue;
            }
            
            if (kVerboseCaptureLoopLogs)
                qDebug() << "[Worker] Step 23: Converting to point cloud (" << globalTimer.elapsed() << "ms)";

            // In preview mode (no cloud processing), emit RGB frames separately
            if (!m_cloudProcessingEnabled && frameCounter % 10 == 0) {
                auto colorPtr = QSharedPointer<cv::Mat>::create(color.clone());
                auto depthPtr = QSharedPointer<cv::Mat>::create(depth.clone());
                emit frameCaptured(colorPtr, depthPtr);
            }

            if (m_cloudProcessingEnabled) {
                // Приводим интринсики к разрешению depth-кадра один раз
                if (!m_intrinsicsScaled && !depth.empty()) {
                    scaleIntrinsicsToDepth(depth.cols, depth.rows);
                    m_intrinsicsScaled = true;
                }
                // Sync RGB preview with depth range filtering + parallax correction
                if (frameCounter % 5 == 0 && !color.empty() && !depth.empty() && depth.type() == CV_16UC1) {
                    try {
                    const float dMin = m_depthMin.load(std::memory_order_relaxed) * 1000.0f;
                    const float dMax = m_depthMax.load(std::memory_order_relaxed) * 1000.0f;
                    const int rows = std::min(color.rows, depth.rows);
                    const int cols = std::min(color.cols, depth.cols);
                    cv::Mat aligned(rows, cols, CV_8UC3, cv::Scalar(0, 0, 0));
                    constexpr float kBaselineMm = 25.5f;
                    const float fxDepth = m_fx;
                    for (int v = 0; v < rows; v++) {
                        const uint16_t* dRow = depth.ptr<uint16_t>(v);
                        cv::Vec3b* aRow = aligned.ptr<cv::Vec3b>(v);
                        const cv::Vec3b* cRow = color.ptr<cv::Vec3b>(v);
                        for (int u = 0; u < cols; u++) {
                            uint16_t d = dRow[u];
                            if (d == 0 || d < dMin || d > dMax) continue;
                            float z = static_cast<float>(d);
                            int shift = static_cast<int>(kBaselineMm * fxDepth / z + 0.5f);
                            int srcU = u - shift;
                            if (srcU >= 0 && srcU < cols) {
                                aRow[u] = cRow[srcU];
                            }
                        }
                    }
                    auto colorPtr = QSharedPointer<cv::Mat>::create(aligned);
                    auto depthPtr = QSharedPointer<cv::Mat>::create(depth.clone());
                    emit frameCaptured(colorPtr, depthPtr);
                    } catch (...) {
                        qWarning() << "[Worker] Exception in parallax correction";
                    }
                }
                auto cloud = convertToPointCloud(depth, color, m_fx, m_fy, m_cx, m_cy, 3);
                if (!m_running) break;
                if (cloud && !cloud->empty()) {
                    updateBaselineScanQuality(cloud, frameCounter);
                    if (!m_running) break;
                    const bool neuralEnabled = (m_neuralPipeline && m_neuralPipeline->isEnabled());
                    if (neuralEnabled) {
                        // Neural pipeline is fed the raw camera-frame cloud here;
                        // it maintains its own anchor and applies ICP confirm internally.
                        auto nnResult = m_neuralPipeline->processFrame(cloud, nullptr);
                        if (!m_running) break;
                        if (nnResult.poseUpdated) {
                            emit poseUpdated(nnResult.pose, 0.0f, true);
                            emit scannerPoseForProject(nnResult.pose);
                        }
                        if (!nnResult.rejectReason.isEmpty()) {
                            qWarning() << "[Worker] Neural pose rejected:" << nnResult.rejectReason;
                        }
                        if (!nnResult.trackingText.isEmpty()) {
                            emitScanQuality(nnResult.trackingText, nnResult.trackingLevel,
                                            nnResult.alignmentText, nnResult.alignmentLevel,
                                            nnResult.depthText, nnResult.depthLevel,
                                            nnResult.driftText, nnResult.driftLevel);
                        }
                    }
                    if (!m_running) break;

                    // Bring camera-frame cloud into world frame for accumulation/display.
                    // - Neural pipeline keeps T_cam_from_world internally and transforms
                    //   via its inverse (world ← cam).
                    // - Marker-based pipeline doesn't transform the cloud from this path;
                    //   it emits its own per-frame pose and relies on processing using
                    //   camera-frame cloud + pose.  To keep accumulation consistent we
                    //   apply the marker pose (T_world_from_cam) here when active.
                    if (neuralEnabled) {
                        m_neuralPipeline->applyCumulativeTransform(cloud);
                    } else if (m_marker_tracking_enabled && m_marker_map && m_marker_map->isInitialized()
                               && m_poseInitialized) {
                        const Eigen::Affine3f pose = m_smoothedPose;          // T_world_from_cam
                        pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed(new pcl::PointCloud<pcl::PointXYZRGB>);
                        transformed->reserve(cloud->size());
                        for (const auto& pt : cloud->points) {
                            if (!std::isfinite(pt.x) || !std::isfinite(pt.y) || !std::isfinite(pt.z)) continue;
                            Eigen::Vector3f p = pose * pt.getVector3fMap();
                            pcl::PointXYZRGB q;
                            q.x = p.x(); q.y = p.y(); q.z = p.z();
                            q.r = pt.r; q.g = pt.g; q.b = pt.b;
                            transformed->push_back(q);
                        }
                        transformed->width = transformed->size();
                        transformed->height = 1;
                        transformed->is_dense = false;
                        cloud = transformed;
                    }

                    emitCloudIfReady(cloud);
                }

                processMarkerTrackingFrame(color, depth);
            }

            ++frameCounter;
            emit frameProcessed(frameCounter);
            if (kVerboseCaptureLoopLogs && frameCounter % 30 == 0) {
                qDebug() << "[Worker] Processed" << frameCounter << "frames";
            }
            QThread::msleep(5);
        } else {
            if (kVerboseCaptureLoopLogs)
                qDebug() << "[Worker] Frame read failed, sleeping (" << globalTimer.elapsed() << "ms)";
            QThread::msleep(1);
        }
        } catch (const std::exception& e) {
            qWarning() << "[Worker] Exception in capture loop:" << e.what();
            QThread::msleep(100);
        } catch (...) {
            qWarning() << "[Worker] Unknown exception in capture loop";
            QThread::msleep(100);
        }
    }
    qDebug() << "[Worker] Capture loop finished (" << globalTimer.elapsed() << "ms)";
    } catch (const std::exception& e) {
        qCritical() << "[Worker] Exception during process:" << e.what();
        emit error(QString("Process error: %1").arg(e.what()));
    } catch (...) {
        qCritical() << "[Worker] Unknown exception during process";
        emit error("Process error: unknown exception");
    }

    qDebug() << "[Worker] Exiting loop";
    camera.requestStop();      // unblock any pending cv::VideoCapture::read inside readFrame()
    m_activeCamera.store(nullptr, std::memory_order_release);
    camera.stopStreams();
    camera.shutdown();
    emit finished();
    qDebug() << "[Worker] Finished (total time: " << globalTimer.elapsed() << "ms)";
}


void CaptureWorker::loadMarkerDiskConfig()
{
    try {
        std::ifstream file("data/marker_disk_config.json");
        if (!file.is_open()) {
            qWarning() << "[Worker] Не удалось открыть конфигурационный файл data/marker_disk_config.json";
            qWarning() << "[Worker] Текущая директория:" << QDir::currentPath().toUtf8().data();
            return;
        }
        
        nlohmann::json config;
        file >> config;
        
        qInfo() << "[Worker] marker_disk_config.json loaded successfully";
        
        // Загрузка параметров диска
        if (config.contains("disk_diameter_mm")) {
            m_disk_diameter_mm = config["disk_diameter_mm"].get<float>();
            qInfo() << "[Worker] Loaded disk_diameter_mm:" << m_disk_diameter_mm;
        }
        if (config.contains("marker_count")) {
            m_marker_count = config["marker_count"].get<int>();
            qInfo() << "[Worker] Loaded marker_count:" << m_marker_count;
        } else {
            qWarning() << "[Worker] marker_count not found in config!";
        }
        
        // Загрузка параметров детекции
        if (config.contains("detection")) {
            auto detection = config["detection"];
            if (detection.contains("min_circularity")) {
                m_min_circularity = detection["min_circularity"].get<float>();
            }
            if (detection.contains("min_convexity")) {
                m_min_convexity = detection["min_convexity"].get<float>();
            }
            if (detection.contains("min_inertia_ratio")) {
                m_min_inertia_ratio = detection["min_inertia_ratio"].get<float>();
            }
            if (detection.contains("min_radius_px")) {
                m_min_radius_px = detection["min_radius_px"].get<float>();
            }
            if (detection.contains("max_radius_px")) {
                m_max_radius_px = detection["max_radius_px"].get<float>();
            }
            if (detection.contains("min_area_px2")) {
                m_min_area = detection["min_area_px2"].get<float>();
            }
            if (detection.contains("max_area_px2")) {
                m_max_area = detection["max_area_px2"].get<float>();
            }
            if (detection.contains("marker_size_mm")) {
                m_marker_size_mm = detection["marker_size_mm"].get<float>();
            }
        }
        
        qInfo() << "[Worker] Конфигурация диска с маркерами загружена:";
        qInfo() << "  Диаметр диска:" << m_disk_diameter_mm << "мм";
        qInfo() << "  Количество маркеров:" << m_marker_count;
        qInfo() << "  Параметры детекции: minCircularity=" << m_min_circularity
                << "minConvexity=" << m_min_convexity << "minInertiaRatio=" << m_min_inertia_ratio;
        qInfo() << "  Диапазон радиусов:" << m_min_radius_px << "-" << m_max_radius_px << "пикселей";
        
    } catch (const std::exception& e) {
        qWarning() << "[Worker] Ошибка при загрузке конфигурации диска:" << e.what();
    }
}

void CaptureWorker::scaleIntrinsicsToDepth(int depthWidth, int depthHeight)
{
    if (m_calibWidth <= 0 || m_calibHeight <= 0) return;
    if (depthWidth <= 0 || depthHeight <= 0) return;
    if (m_calibWidth == depthWidth && m_calibHeight == depthHeight) return;

    const float sx = static_cast<float>(depthWidth)  / static_cast<float>(m_calibWidth);
    const float sy = static_cast<float>(depthHeight) / static_cast<float>(m_calibHeight);
    m_fx *= sx;
    m_fy *= sy;
    m_cx *= sx;
    m_cy *= sy;
    m_calibWidth  = depthWidth;
    m_calibHeight = depthHeight;
    qInfo() << "[Worker] Scaled intrinsics to depth resolution:"
            << depthWidth << "x" << depthHeight
            << "sx=" << sx << "sy=" << sy
            << "-> fx=" << m_fx << "fy=" << m_fy << "cx=" << m_cx << "cy=" << m_cy;
}

void CaptureWorker::loadCalibrationOrCameraIntrinsics(AstraCamera &camera, QElapsedTimer &globalTimer)
{
    cv::FileStorage fs("data/camera_calibration.xml", cv::FileStorage::READ);
    if (fs.isOpened()) {
        cv::Mat K;
        cv::Size imageSize;
        fs["camera_matrix"] >> K;
        fs["image_size"] >> imageSize;
        if (!K.empty() && K.cols == 3 && K.rows == 3) {
            cv::Mat K64;
            K.convertTo(K64, CV_64F);
            m_fx = static_cast<float>(K64.at<double>(0,0));
            m_fy = static_cast<float>(K64.at<double>(1,1));
            m_cx = static_cast<float>(K64.at<double>(0,2));
            m_cy = static_cast<float>(K64.at<double>(1,2));
            m_calibWidth  = imageSize.width;
            m_calibHeight = imageSize.height;
            m_intrinsicsLoaded = true;
            qInfo() << "[Worker] Loaded calibration: fx=" << m_fx << "fy=" << m_fy
                    << "cx=" << m_cx << "cy=" << m_cy
                    << "at" << m_calibWidth << "x" << m_calibHeight;
        }
        fs.release();
    } else {
        qDebug() << "[Worker] Step 15: Using camera intrinsics (" << globalTimer.elapsed() << "ms)";
        if (camera.getIntrinsics(m_fx, m_fy, m_cx, m_cy)) {
            m_calibWidth  = 0;
            m_calibHeight = 0;
            m_intrinsicsLoaded = true;
            qInfo() << "[Worker] Camera intrinsics: fx=" << m_fx << "fy=" << m_fy << "cx=" << m_cx << "cy=" << m_cy;
        }
    }
    qDebug() << "[Worker] Step 16: Calibration ready (" << globalTimer.elapsed() << "ms)";

    if (std::isnan(m_fx) || m_fx < 100) {
        m_fx = m_fy = 570.0f;
        m_cx = 320.0f; m_cy = 240.0f;
        m_calibWidth = 0;
        m_calibHeight = 0;
        qWarning() << "[Worker] Using default intrinsics";
    }
}

void CaptureWorker::configureMarkerDetector(QElapsedTimer &globalTimer)
{
    if (!(m_marker_tracker && m_marker_tracker->detector())) {
        return;
    }

    m_marker_tracker->detector()->setIntrinsics(m_fx, m_fy, m_cx, m_cy);
    m_marker_tracker->detector()->setDiskParams(m_disk_diameter_mm, m_marker_count);
    // Разрешение depth-кадра будет установлено в processMarkerTrackingFrame
    // при первом получении реального кадра (640x480 — дефолт детектора).

    CircularMarkerDetector* circularDetector = dynamic_cast<CircularMarkerDetector*>(m_marker_tracker->detector());
    if (circularDetector) {
        qDebug() << "[Worker] Step 18: Setting circular detector params (" << globalTimer.elapsed() << "ms)";
        circularDetector->setDetectionParams(m_min_circularity, m_min_convexity, m_min_inertia_ratio);
        circularDetector->setRadiusFilter(m_min_radius_px, m_max_radius_px);
        circularDetector->setAreaFilter(m_min_area, m_max_area);
        qDebug() << "[Worker] Step 19: Circular detector params set (" << globalTimer.elapsed() << "ms)";
    }
}

void CaptureWorker::emitScanQuality(const QString &trackingText, int trackingLevel,
                                    const QString &alignmentText, int alignmentLevel,
                                    const QString &depthText, int depthLevel,
                                    const QString &driftText, int driftLevel)
{
    emit scanQualityUpdated(trackingText, trackingLevel,
                            alignmentText, alignmentLevel,
                            depthText, depthLevel,
                            driftText, driftLevel);
}

void CaptureWorker::updateBaselineScanQuality(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud, int frameCounter)
{
    if (!cloud || cloud->empty()) return;
    if (frameCounter % 10 != 0) return;

    const auto stats = m_neuralPipeline->analyzeTrackingCloud(cloud);
    if (!stats) {
        emitScanQuality(QStringLiteral("Нет оценки"), QualityWarning,
                        QStringLiteral("Нет live-оценки"), QualityWarning,
                        QStringLiteral("Нет валидных точек"), QualityPoor,
                        QStringLiteral("Неизвестно"), QualityWarning);
        return;
    }

    const QString depthText = m_neuralPipeline->buildDepthQualityText(*stats);
    const int depthLevel = m_neuralPipeline->depthQualityLevel(*stats);

    if (m_marker_tracking_enabled) {
        emitScanQuality(QStringLiteral("Маркеры: ожидание захвата"), QualityWarning,
                        QStringLiteral("Ожидание pose update"), QualityWarning,
                        depthText, depthLevel,
                        QStringLiteral("Не оценён"), QualityWarning);
        return;
    }

    if (m_neuralPipeline->isEnabled()) {
        emitScanQuality(QStringLiteral("Neural: ожидание подтверждения"), QualityWarning,
                        QStringLiteral("Ожидание совпадения кадров"), QualityWarning,
                        depthText, depthLevel,
                        QStringLiteral("Низкий"), QualityGood);
        return;
    }

    emitScanQuality(QStringLiteral("ICP: базовый захват"), QualityGood,
                    QStringLiteral("Нет live-оценки"), QualityWarning,
                    depthText, depthLevel,
                    QStringLiteral("Не оценён"), QualityWarning);
}

void CaptureWorker::emitCloudIfReady(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud)
{
    if (!cloud || cloud->empty()) {
        qInfo() << "[Worker] emitCloudIfReady: cloud is null or empty";
        return;
    }
    if (!NeuralTrackingPipeline::hasValidDepthPoints(cloud)) {
        qInfo() << "[Worker] Skipping frame: no valid depth points, size=" << cloud->size();
        return;
    }
    cloud->is_dense = false;
    qInfo() << "[Worker] Sending cloud:" << cloud->size() << "points";
    emit pointCloudReady(cloud);
}

void CaptureWorker::processMarkerTrackingFrame(const cv::Mat &color, const cv::Mat &depth)
{
    if (!(m_marker_tracking_enabled && m_project_manager && m_marker_tracker && m_marker_tracker->detector() && m_marker_map)) {
        return;
    }

    m_marker_tracker->detector()->setDepthResolution(depth.cols, depth.rows);
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr depth_cloud_markers = convertToPointCloud(
        depth, color, m_fx, m_fy, m_cx, m_cy, 1);

    const float depthMin = std::max(0.4f, m_depthMin.load(std::memory_order_relaxed) - 0.1f);
    const float depthMax = std::min(1.5f, m_depthMax.load(std::memory_order_relaxed) + 0.3f);

    std::vector<DetectedMarker> markers = m_marker_tracker->detector()->detectAndReconstruct(
        color, depth_cloud_markers, depthMin, depthMax);

    cv::Mat debugImg = color.clone();
    int i = 0;
    for (const auto& m : markers) {
        cv::Point2f draw_c(m.center2d);
        cv::circle(debugImg, draw_c, 8, cv::Scalar(0,0,255), 2);
        if (m.id >= 0) {
            cv::putText(debugImg, std::to_string(m.id), draw_c, cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255,0,0), 2);
        } else {
            cv::putText(debugImg, std::to_string(i), draw_c, cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0,255,0), 1);
        }
        i++;
    }

    if (SettingsManager::instance().showMarkers()) {
        emit markersDetected(QSharedPointer<cv::Mat>::create(debugImg));
    } else {
        emit markersDetected(QSharedPointer<cv::Mat>::create(color.clone()));
    }

    if (kVerboseCaptureLoopLogs) {
        qDebug() << "[Worker] Detected" << markers.size() << "markers";
        if (!markers.empty()) {
            for (size_t idx = 0; idx < markers.size(); ++idx) {
                qDebug() << "  Marker [" << idx << "] ID:" << markers[idx].id
                         << " 2D:" << markers[idx].center2d.x << markers[idx].center2d.y
                         << " 3D:" << markers[idx].center3d.x() << markers[idx].center3d.y() << markers[idx].center3d.z();
            }
        }
    }

    auto isValidMarker3D = [](const DetectedMarker& m) {
        return m.id >= 0 &&
               std::isfinite(m.center3d.x()) &&
               std::isfinite(m.center3d.y()) &&
               std::isfinite(m.center3d.z()) &&
               m.center3d.z() > 0.05f &&
               m.center3d.norm() > 0.05f;
    };

    std::vector<DetectedMarker> markers_filtered;
    for (const auto& m : markers) {
        if (isValidMarker3D(m)) {
            markers_filtered.push_back(m);
        }
    }

    if (kVerboseCaptureLoopLogs)
        qDebug() << "[Worker] Detected" << markers_filtered.size() << "markers with IDs";
    if (markers_filtered.empty()) {
        return;
    }

    if (!m_marker_map->isInitialized()) {
        const int min_markers_count = SettingsManager::instance().markerInitCount();
        if (markers_filtered.size() >= static_cast<size_t>(min_markers_count)) {
            if (m_marker_map->initialize(markers_filtered, min_markers_count)) {
                qInfo() << "[Worker] Карта маркеров инициализирована.";
                emit trackingModeChangedForProject(TrackingMode::MarkerBased);
                m_poseInitialized = false;
            }
        }
    }

    if (!m_marker_map->isInitialized()) {
        // While no map exists yet we cannot produce a pose — reset smoothing.
        m_poseInitialized = false;
        return;
    }

    Eigen::Affine3f scanner_pose = m_marker_map->update(markers_filtered);
    // MarkerMap returns the last-valid pose when correspondences drop below 3,
    // but if correspondences are actually low treat it as a tracking loss so
    // smoothing doesn't keep stale data for long (poseUpdated is only emitted
    // when reliable anyway).
    if (m_marker_map->getLastCorrespondenceCount() < 3) {
        m_poseInitialized = false;
    }
    if (!m_poseInitialized) {
        m_smoothedPose = scanner_pose;
        m_poseInitialized = true;
    } else {
        m_smoothedPose.translation() =
            (1.0f - m_poseSmoothing) * m_smoothedPose.translation() +
            m_poseSmoothing * scanner_pose.translation();

        Eigen::Quaternionf q_prev(m_smoothedPose.linear());
        Eigen::Quaternionf q_new(scanner_pose.linear());
        Eigen::Quaternionf q_smooth = q_prev.slerp(m_poseSmoothing, q_new);
        m_smoothedPose.linear() = q_smooth.toRotationMatrix();
    }

    const bool reliable = (m_marker_map->getLastCorrespondenceCount() >= 3);
    const float angle = m_marker_map->getCurrentRotationAngle();

    if (reliable) {
        emit scannerPoseForProject(m_smoothedPose);
    }

    emit poseUpdated(m_smoothedPose, angle, reliable);

    const auto depthStats = m_neuralPipeline->analyzeTrackingCloud(depth_cloud_markers);
    const QString depthText = depthStats ? m_neuralPipeline->buildDepthQualityText(*depthStats)
                                         : QStringLiteral("Нет валидных точек");
    const int depthLevel = depthStats ? m_neuralPipeline->depthQualityLevel(*depthStats) : QualityPoor;

    const int correspondences = m_marker_map->getLastCorrespondenceCount();
    const QString trackingText = QString("Маркеры: %1 видимо, %2 соответствий")
                                     .arg(markers_filtered.size())
                                     .arg(correspondences);
    const int trackingLevel = reliable ? QualityGood
                                       : (correspondences >= 2 ? QualityWarning : QualityPoor);
    const QString alignmentText = reliable
        ? QString("Маркерная поза стабильна (%1°)").arg(angle, 0, 'f', 1)
        : QString("Недостаточно соответствий (%1)").arg(correspondences);
    const int alignmentLevel = reliable ? QualityGood : QualityWarning;
    const QString driftText = reliable ? QStringLiteral("Низкий") : QStringLiteral("Повышен");
    const int driftLevel = reliable ? QualityGood : QualityWarning;

    emitScanQuality(trackingText, trackingLevel,
                    alignmentText, alignmentLevel,
                    depthText, depthLevel,
                    driftText, driftLevel);
}

pcl::PointCloud<pcl::PointXYZRGB>::Ptr CaptureWorker::convertToPointCloud(
    const cv::Mat &depth, const cv::Mat &color,
    float fx, float fy, float cx, float cy, int stride)
{
    DepthCloudParams p;
    p.fx = fx; p.fy = fy; p.cx = cx; p.cy = cy;
    p.depthMin = m_depthMin.load(std::memory_order_relaxed);
    p.depthMax = m_depthMax.load(std::memory_order_relaxed);
    p.colorEnabled = m_colorCameraEnabled;
    p.stride = stride;

    auto cloud = convertDepthToCloud(depth, color, p);

    if (!m_firstPointLogged && cloud && !cloud->empty()) {
        qDebug() << "[Worker] First point generated";
        m_firstPointLogged = true;
    }

    return cloud;
}

void CaptureWorker::setNeuralTracker(std::unique_ptr<NeuralTracker> tracker)
{
    m_neuralPipeline->setTracker(std::move(tracker));
}

void CaptureWorker::onPoseReady(const Eigen::Matrix4f& transform)
{
    // Обработка успешного определения позы
    qInfo() << "[Worker] Pose determined successfully";
    // Здесь можно добавить логику обработки трансформации
}

void CaptureWorker::onPoseError(const QString& error)
{
    // Обработка ошибки при определении позы
    qWarning() << "[Worker] Pose error:" << error;
}