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

src/capture/CaptureWorker.h
188 строк8 KB
k k
refactor: capture subsystem overhaul
15 июл 2026, 11:17
15 июл 2026, 11:171123c30
#ifndef CAPTUREWORKER_H
#define CAPTUREWORKER_H

#include <QObject>
#include <QSharedPointer>
#include <QMutex>
#include <QElapsedTimer>
#include <atomic>
#include <utility>
#include <optional>
#include <chrono>
#include <QMutexLocker>
#include <Eigen/Geometry>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

#include <memory>
#include "../marker_tracker/IMarkerDetector.h"
#include "../marker_tracker/CircularMarkerDetector.h"
#include "../marker_tracker/MarkerMap.h"
#include "../project/ProjectManager.h"
#include "../tracking/NeuralTrackingPipeline.h"
#include "../marker_tracker/MarkerTracker.h"

namespace cv {
    class Mat;
}

class ProjectManager;
class AstraCamera;
class RegistrationService;



class CaptureWorker : public QObject
{
    Q_OBJECT
public:
    explicit CaptureWorker(QObject *parent = nullptr);
    ~CaptureWorker();

    void setCloudProcessingEnabled(bool enabled) { m_cloudProcessingEnabled = enabled; }
    void resetTransform() {
        if (m_neuralPipeline) m_neuralPipeline->reset();
    }
    void setDepthRange(float minMeters, float maxMeters) {
        if (minMeters >= maxMeters) std::swap(minMeters, maxMeters);
        m_depthMin.store(minMeters, std::memory_order_relaxed);
        m_depthMax.store(maxMeters, std::memory_order_relaxed);
    }
    void setColorCameraEnabled(bool enabled) { m_colorCameraEnabled = enabled; }

    // Bug 3 fix: requestStop() sets atomic flag AND releases color capture
    // to unblock cv::VideoCapture::read(). The worker loop observes the flag
    // and handles its own OpenNI cleanup after exiting.
    void requestStop();

public slots:
    void process();

signals:
    void frameCaptured(QSharedPointer<cv::Mat> color, QSharedPointer<cv::Mat> depth);
    void pointCloudReady(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud);
    void error(const QString &message);
    // Эмитится на каждый успешно прочитанный кадр с камеры. Используется GUI
    // для точного подсчёта кадров и FPS (frameCaptured дросселируется).
    void frameProcessed(int totalFrames);
    void warning(const QString &message);
    void finished();
    void markersDetected(QSharedPointer<cv::Mat> markersImage);
    void scanQualityUpdated(const QString &trackingText, int trackingLevel,
                            const QString &alignmentText, int alignmentLevel,
                            const QString &depthText, int depthLevel,
                            const QString &driftText, int driftLevel);
    /**
     * @brief Эмитится при обновлении позы сканера.
     * 
     * @param pose Аффинное преобразование позы сканера.
     * @param rotation_angle Угол поворота диска (в радианах).
     * @param reliable Флаг надежности позы (true, если найдено достаточно соответствий).
     */
    void poseUpdated(const Eigen::Affine3f& pose, float rotation_angle, bool reliable);

    /**
     * @brief Эмитится при обновлении позы для проекта (worker→GUI).
     * Заменяет прямой вызов ProjectManager::setScannerPose из рабочего потока.
     */
    void scannerPoseForProject(const Eigen::Affine3f& pose);

    /**
     * @brief Эмитится при изменении режима трекинга (worker→GUI).
     * Заменяет прямой вызов ProjectManager::setTrackingMode из рабочего потока.
     */
    void trackingModeChangedForProject(TrackingMode mode);


private:
    std::atomic<bool> m_running{true};
    std::atomic<bool> m_cloudProcessingEnabled{true};
    std::atomic<bool> m_colorCameraEnabled{true};
    std::atomic<AstraCamera*> m_activeCamera{nullptr};
    bool m_neuralTrackerLoaded = false;
    std::atomic<float> m_depthMin{0.1f};   // метры
    std::atomic<float> m_depthMax{10.0f};  // метры
    // Интринсики, *уже* приведённые к разрешению depth-кадра, с которым
    // работает convertToPointCloud. scaleIntrinsicsToDepth() делает пересчёт.
    float m_fx = 570.0f, m_fy = 570.0f, m_cx = 320.0f, m_cy = 240.0f;
    // Разрешение, на котором была откалибрована RGB-камера. Берётся из
    // data/camera_calibration.xml (image_size), либо остаётся 0 если
    // использованы default-интринсики.
    int m_calibWidth = 0;
    int m_calibHeight = 0;
    bool m_intrinsicsLoaded = false;
    bool m_firstPointLogged = false;
    
    // Загружает конфигурацию диска с маркерами
    void loadMarkerDiskConfig();
    
    // Параметры диска с маркерами
    float m_disk_diameter_mm = 300.0f;
    int m_marker_count = 7;
    // Синхронизируем дефолты с SettingsManager для совместимости
    float m_min_circularity = 0.75f;
    float m_min_convexity = 0.75f;
    float m_min_inertia_ratio = 0.5f;
    float m_min_radius_px = 10.0f;
    float m_max_radius_px = 35.0f;
    // Дефолты из SettingsManager: minArea=200, maxArea=2800
    float m_min_area = 200.0f;
    float m_max_area = 2800.0f;

    enum ScanQualityLevel { QualityGood = 0, QualityWarning = 1, QualityPoor = 2 };

    // --- Компоненты для оптического трекинга ---
    std::unique_ptr<MarkerTracker> m_marker_tracker;
    std::unique_ptr<MarkerMap> m_marker_map;
    std::atomic<bool> m_marker_tracking_enabled{false};

    // Параметры для 3D-реконструкции маркеров
    float m_marker_size_mm = 20.0f; // Размер маркера в миллиметрах (из настроек)

    // Таймер для инициализации карты маркеров
    std::chrono::steady_clock::time_point m_stable_time_start;
    bool m_stable_time_reached = false;

    // Флаг, указывающий, что масштабирование интринсик уже выполнено
    bool m_intrinsicsScaled = false;

    // Счётчик кадров для нейросетевого трекинга
    int m_frameCounterNN = 0;

    // Масштабирование интринсик с разрешения калибровки (m_calibWidth x
    // m_calibHeight) на разрешение depth-кадра (depthWidth x depthHeight).
    void scaleIntrinsicsToDepth(int depthWidth, int depthHeight);
    void loadCalibrationOrCameraIntrinsics(AstraCamera &camera, QElapsedTimer &globalTimer);
    void configureMarkerDetector(QElapsedTimer &globalTimer);
    void emitScanQuality(const QString &trackingText, int trackingLevel,
                         const QString &alignmentText, int alignmentLevel,
                         const QString &depthText, int depthLevel,
                         const QString &driftText, int driftLevel);
    void updateBaselineScanQuality(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud, int frameCounter);
    void applyCumulativeTransform(pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud);
    void emitCloudIfReady(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud);
    void processMarkerTrackingFrame(const cv::Mat &color, const cv::Mat &depth);

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr convertToPointCloud(
        const cv::Mat &depth, const cv::Mat &color,
        float fx, float fy, float cx, float cy, int stride = 3);

    // Поля для сглаживания позы
    Eigen::Affine3f m_smoothedPose = Eigen::Affine3f::Identity();
    bool m_poseInitialized = false;
    float m_poseSmoothing = 0.3f;

    // Neural tracking pipeline (owns all neural state + logic)
    std::unique_ptr<NeuralTrackingPipeline> m_neuralPipeline;

public:
    void setNeuralTracker(std::unique_ptr<NeuralTracker> tracker);
    void setProjectManager(ProjectManager* pm) { m_project_manager = pm; }
    ProjectManager* m_project_manager = nullptr;

public slots:
    void onPoseReady(const Eigen::Matrix4f &transform);
    void onPoseError(const QString &error);
};

#endif // CAPTUREWORKER_H