GrabBag/Tools/VrEyeView/Src/VrEyeViewWidget.cpp

#include "VrEyeViewWidget.h"
#include "../SpinBoxPasteHelper.h"
#include <QVBoxLayout>
#include <QHBoxLayout>
#include <QGridLayout>
#include <QGroupBox>
#include <QLabel>
#include <QLineEdit>
#include <QMessageBox>
#include <QFileDialog>
#include <QFileInfo>
#include <QPainter>
#include <QScrollArea>
#include <QResizeEvent>
#include <cmath>

VrEyeViewWidget::VrEyeViewWidget(QWidget* parent)
    : QWidget(parent)
    , m_eyeDevice(nullptr)
    , m_detector(nullptr)
    , m_leftImageLabel(nullptr)
    , m_rightImageLabel(nullptr)
    , m_isConnected(false)
    , m_isCapturing(false)
    , m_hasNewImage(false)
{
    // 创建设备和检测器
    IVrEyeDevice::CreateObject(&m_eyeDevice);
    m_detector = CreateChessboardDetectorInstance();

    setupUI();

    // 为所有 SpinBox 安装粘贴过滤器
    SpinBoxPasteHelper::install(this);

    // 初始化设备
    if (m_eyeDevice) {
        m_eyeDevice->InitDevice();
    }

    // 创建显示定时器
    m_displayTimer = new QTimer(this);
    connect(m_displayTimer, &QTimer::timeout, this, &VrEyeViewWidget::onUpdateDisplay);
    m_displayTimer->start(33); // 约30fps
}

VrEyeViewWidget::~VrEyeViewWidget()
{
    if (m_isCapturing) {
        onStopCapture();
    }

    if (m_isConnected) {
        onDisconnectCamera();
    }

    if (m_detector) {
        DestroyChessboardDetectorInstance(m_detector);
        m_detector = nullptr;
    }

    if (m_eyeDevice) {
        delete m_eyeDevice;
        m_eyeDevice = nullptr;
    }
}

void VrEyeViewWidget::setupUI()
{
    QVBoxLayout* mainLayout = new QVBoxLayout(this);
    mainLayout->setSpacing(4);

    // ===== 左右目图像并排显示区域 =====
    QHBoxLayout* imageLayout = new QHBoxLayout();

    QVBoxLayout* leftLayout = new QVBoxLayout();
    leftLayout->setSpacing(0);
    QLabel* leftTitle = new QLabel("左目", this);
    leftTitle->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Fixed);
    leftLayout->addWidget(leftTitle);
    m_leftImageLabel = new QLabel(this);
    m_leftImageLabel->setMinimumSize(320, 240);
    m_leftImageLabel->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
    m_leftImageLabel->setAlignment(Qt::AlignCenter);
    m_leftImageLabel->setStyleSheet("QLabel { background-color: black; }");
    leftLayout->addWidget(m_leftImageLabel, 1);
    imageLayout->addLayout(leftLayout);

    QVBoxLayout* rightLayout = new QVBoxLayout();
    rightLayout->setSpacing(0);
    QLabel* rightTitle = new QLabel("右目", this);
    rightTitle->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Fixed);
    rightLayout->addWidget(rightTitle);
    m_rightImageLabel = new QLabel(this);
    m_rightImageLabel->setMinimumSize(320, 240);
    m_rightImageLabel->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
    m_rightImageLabel->setAlignment(Qt::AlignCenter);
    m_rightImageLabel->setStyleSheet("QLabel { background-color: black; }");
    rightLayout->addWidget(m_rightImageLabel, 1);
    imageLayout->addLayout(rightLayout);

    mainLayout->addLayout(imageLayout, 1);

    // ===== 第一行：相机连接 + 参数 + 采集按钮 =====
    QHBoxLayout* row1 = new QHBoxLayout();
    row1->setSpacing(4);

    // 相机IP + 连接/断开
    row1->addWidget(new QLabel("IP:", this));
    m_editCameraIP = new QLineEdit("192.168.1.100", this);
    m_editCameraIP->setFixedWidth(120);
    row1->addWidget(m_editCameraIP);

    m_btnConnect = new QPushButton("连接", this);
    m_btnDisconnect = new QPushButton("断开", this);
    m_btnDisconnect->setEnabled(false);
    connect(m_btnConnect, &QPushButton::clicked, this, &VrEyeViewWidget::onConnectCamera);
    connect(m_btnDisconnect, &QPushButton::clicked, this, &VrEyeViewWidget::onDisconnectCamera);
    row1->addWidget(m_btnConnect);
    row1->addWidget(m_btnDisconnect);

    // 分隔
    row1->addSpacing(8);

    // 曝光/增益
    row1->addWidget(new QLabel("曝光:", this));
    m_sbExposure = new QSpinBox(this);
    m_sbExposure->setRange(100, 10000);
    m_sbExposure->setValue(1000);
    row1->addWidget(m_sbExposure);

    row1->addWidget(new QLabel("增益:", this));
    m_sbGain = new QSpinBox(this);
    m_sbGain->setRange(0, 255);
    m_sbGain->setValue(100);
    row1->addWidget(m_sbGain);

    // 分隔
    row1->addSpacing(8);

    // 采集 + 加载按钮
    m_btnStartCapture = new QPushButton("开始采集", this);
    m_btnStopCapture = new QPushButton("停止采集", this);
    QPushButton* btnLoadLeftImage = new QPushButton("加载左图", this);
    QPushButton* btnLoadRightImage = new QPushButton("加载右图", this);
    m_btnStartCapture->setEnabled(false);
    m_btnStopCapture->setEnabled(false);
    connect(m_btnStartCapture, &QPushButton::clicked, this, &VrEyeViewWidget::onStartCapture);
    connect(m_btnStopCapture, &QPushButton::clicked, this, &VrEyeViewWidget::onStopCapture);
    connect(btnLoadLeftImage, &QPushButton::clicked, this, &VrEyeViewWidget::onLoadLeftImage);
    connect(btnLoadRightImage, &QPushButton::clicked, this, &VrEyeViewWidget::onLoadRightImage);
    row1->addWidget(m_btnStartCapture);
    row1->addWidget(m_btnStopCapture);
    row1->addWidget(btnLoadLeftImage);
    row1->addWidget(btnLoadRightImage);

    row1->addStretch();
    mainLayout->addLayout(row1);

    // ===== 第二行：相机内参(左) + 标定板检测(右) =====
    QHBoxLayout* row2 = new QHBoxLayout();
    row2->setSpacing(4);

    // 相机内参（3x3矩阵）
    QGroupBox* intrinsicsGroup = new QGroupBox("相机内参", this);
    QGridLayout* intrinsicsLayout = new QGridLayout(intrinsicsGroup);
    intrinsicsLayout->setSpacing(2);
    intrinsicsLayout->setContentsMargins(4, 8, 4, 4);

    auto createIntrinsicSpinBox = [this](double minVal, double maxVal, double defaultVal) {
        QDoubleSpinBox* sb = new QDoubleSpinBox(this);
        sb->setRange(minVal, maxVal);
        sb->setValue(defaultVal);
        sb->setDecimals(2);
        return sb;
    };

    auto createFixedLabel = [this](const QString& text) {
        QLabel* lbl = new QLabel(text, this);
        lbl->setAlignment(Qt::AlignCenter);
        lbl->setStyleSheet("QLabel { color: gray; }");
        return lbl;
    };

    m_sbFx = createIntrinsicSpinBox(100, 5000, 2384.8520129909352);
    m_sbFx->setPrefix("fx: ");
    m_sbFx->setToolTip("fx: X方向焦距(像素)");
    m_sbFy = createIntrinsicSpinBox(100, 5000, 2384.8520129909352);
    m_sbFy->setPrefix("fy: ");
    m_sbFy->setToolTip("fy: Y方向焦距(像素)");
    m_sbCx = createIntrinsicSpinBox(0, 2000, 232.37469863891602);
    m_sbCx->setPrefix("cx: ");
    m_sbCx->setToolTip("cx: 主点X坐标(像素)");
    m_sbCy = createIntrinsicSpinBox(0, 2000, 1054.649814605713);
    m_sbCy->setPrefix("cy: ");
    m_sbCy->setToolTip("cy: 主点Y坐标(像素)");

    intrinsicsLayout->addWidget(m_sbFx, 0, 0);
    intrinsicsLayout->addWidget(createFixedLabel("0"), 0, 1);
    intrinsicsLayout->addWidget(m_sbCx, 0, 2);
    intrinsicsLayout->addWidget(createFixedLabel("0"), 1, 0);
    intrinsicsLayout->addWidget(m_sbFy, 1, 1);
    intrinsicsLayout->addWidget(m_sbCy, 1, 2);
    intrinsicsLayout->addWidget(createFixedLabel("0"), 2, 0);
    intrinsicsLayout->addWidget(createFixedLabel("0"), 2, 1);
    intrinsicsLayout->addWidget(createFixedLabel("1"), 2, 2);

    row2->addWidget(intrinsicsGroup);

    // 标定板检测
    QGroupBox* detectionGroup = new QGroupBox("标定板检测", this);
    QGridLayout* detectionLayout = new QGridLayout(detectionGroup);
    detectionLayout->setSpacing(2);
    detectionLayout->setContentsMargins(4, 8, 4, 4);

    detectionLayout->addWidget(new QLabel("角点宽:", this), 0, 0);
    m_sbPatternWidth = new QSpinBox(this);
    m_sbPatternWidth->setRange(2, 20);
    m_sbPatternWidth->setValue(8);
    detectionLayout->addWidget(m_sbPatternWidth, 0, 1);

    detectionLayout->addWidget(new QLabel("角点高:", this), 0, 2);
    m_sbPatternHeight = new QSpinBox(this);
    m_sbPatternHeight->setRange(2, 20);
    m_sbPatternHeight->setValue(11);
    detectionLayout->addWidget(m_sbPatternHeight, 0, 3);

    detectionLayout->addWidget(new QLabel("格子(mm):", this), 1, 0);
    m_sbSquareSize = new QDoubleSpinBox(this);
    m_sbSquareSize->setRange(1.0, 100.0);
    m_sbSquareSize->setValue(30.0);
    m_sbSquareSize->setDecimals(2);
    detectionLayout->addWidget(m_sbSquareSize, 1, 1);

    m_cbAdaptiveThresh = new QCheckBox("自适应阈值", this);
    m_cbAdaptiveThresh->setChecked(true);
    m_cbNormalizeImage = new QCheckBox("归一化图像", this);
    m_cbNormalizeImage->setChecked(true);
    detectionLayout->addWidget(m_cbAdaptiveThresh, 1, 2);
    detectionLayout->addWidget(m_cbNormalizeImage, 1, 3);

    m_cbAutoDetect = new QCheckBox("自动检测", this);
    detectionLayout->addWidget(m_cbAutoDetect, 2, 0, 1, 2);

    m_btnDetect = new QPushButton("计算标定板信息", this);
    m_btnDetect->setEnabled(false);
    connect(m_btnDetect, &QPushButton::clicked, this, &VrEyeViewWidget::onDetectChessboard);
    detectionLayout->addWidget(m_btnDetect, 2, 2, 1, 2);

    row2->addWidget(detectionGroup);

    mainLayout->addLayout(row2);

    // ===== 状态栏 =====
    m_lblStatus = new QLabel("状态: 未连接", this);
    m_lblDetectionResult = new QLabel("检测结果: 无", this);
    mainLayout->addWidget(m_lblStatus);
    mainLayout->addWidget(m_lblDetectionResult);
}

void VrEyeViewWidget::SetDetectionCallback(DetectionCallback callback)
{
    m_detectionCallback = callback;
}

void VrEyeViewWidget::onConnectCamera()
{
    if (!m_eyeDevice) return;

    QString ip = m_editCameraIP->text();
    int ret = m_eyeDevice->OpenDevice(ip.toStdString().c_str(), false, false, false);

    if (ret == 0) {
        m_isConnected = true;
        m_btnConnect->setEnabled(false);
        m_btnDisconnect->setEnabled(true);
        m_btnStartCapture->setEnabled(true);
        m_lblStatus->setText("状态: 已连接");

        // 设置相机参数
        unsigned int exposure = m_sbExposure->value();
        unsigned int gain = m_sbGain->value();
        m_eyeDevice->SetEyeExpose(exposure);
        m_eyeDevice->SetEyeGain(gain);
    } else {
        QMessageBox::warning(this, "错误", QString("连接相机失败，错误码: %1").arg(ret));
    }
}

void VrEyeViewWidget::onDisconnectCamera()
{
    if (!m_eyeDevice) return;

    if (m_isCapturing) {
        onStopCapture();
    }

    m_eyeDevice->CloseDevice();
    m_isConnected = false;
    m_btnConnect->setEnabled(true);
    m_btnDisconnect->setEnabled(false);
    m_btnStartCapture->setEnabled(false);
    m_lblStatus->setText("状态: 未连接");
}

void VrEyeViewWidget::onStartCapture()
{
    if (!m_eyeDevice || !m_isConnected) return;

    int ret = m_eyeDevice->StartCapture(OnImageCallback, this);

    if (ret == 0) {
        m_isCapturing = true;
        m_btnStartCapture->setEnabled(false);
        m_btnStopCapture->setEnabled(true);
        m_btnDetect->setEnabled(true);
        m_lblStatus->setText("状态: 采集中");
    } else {
        QMessageBox::warning(this, "错误", QString("开始采集失败，错误码: %1").arg(ret));
    }
}

void VrEyeViewWidget::onStopCapture()
{
    if (!m_eyeDevice) return;

    m_eyeDevice->StopCapture();
    m_isCapturing = false;
    m_btnStartCapture->setEnabled(true);
    m_btnStopCapture->setEnabled(false);
    m_lblStatus->setText("状态: 已连接");
}

void VrEyeViewWidget::OnImageCallback(SVzNLImageData* pLeftImage,
                                      SVzNLImageData* pRightImage,
                                      SVzNLImageData* /*pCenterImage*/,
                                      const SVzOutputFrameProps* /*pFrameProps*/,
                                      void* pUserData)
{
    VrEyeViewWidget* pThis = static_cast<VrEyeViewWidget*>(pUserData);
    if (pThis) {
        pThis->ProcessImageData(pLeftImage, pRightImage);
    }
}

/**
 * @brief 将 SDK 图像数据转为 QImage
 */
static QImage SvzImageToQImage(SVzNLImageData* pImage)
{
    if (!pImage || !pImage->pBuffer || pImage->nWidth == 0 || pImage->nHeight == 0) {
        return QImage();
    }

    int w = static_cast<int>(pImage->nWidth);
    int h = static_cast<int>(pImage->nHeight);

    if (pImage->eImageType == keVzNLImageType_RGB888) {
        // RGB888: 直接拷贝
        return QImage(pImage->pBuffer, w, h, w * 3, QImage::Format_RGB888).copy();
    } else if (pImage->eImageType == keVzNLImageType_BGR888) {
        // BGR888: 转为 RGB
        QImage img(pImage->pBuffer, w, h, w * 3, QImage::Format_RGB888);
        return img.rgbSwapped();
    } else if (pImage->eImageType == keVzNLImageType_GRAY) {
        // 灰度图
        return QImage(pImage->pBuffer, w, h, w, QImage::Format_Grayscale8).copy();
    } else if (pImage->eImageType == keVzNLImageType_BGRA8888) {
        // BGRA -> ARGB32
        QImage img(pImage->pBuffer, w, h, w * 4, QImage::Format_ARGB32);
        return img.copy();
    }

    // 其他格式：按灰度处理
    if (pImage->nChannels == 1) {
        return QImage(pImage->pBuffer, w, h, w, QImage::Format_Grayscale8).copy();
    } else if (pImage->nChannels == 3) {
        return QImage(pImage->pBuffer, w, h, w * 3, QImage::Format_RGB888).copy();
    }

    return QImage();
}

void VrEyeViewWidget::ProcessImageData(SVzNLImageData* pLeftImage, SVzNLImageData* pRightImage)
{
    QImage left = SvzImageToQImage(pLeftImage);
    QImage right = SvzImageToQImage(pRightImage);

    {
        std::lock_guard<std::mutex> lock(m_captureMutex);
        if (!left.isNull()) m_captureLeft = left;
        if (!right.isNull()) m_captureRight = right;
        m_hasNewImage = true;
    }
}

void VrEyeViewWidget::onUpdateDisplay()
{
    if (!m_hasNewImage) return;

    {
        std::lock_guard<std::mutex> lock(m_captureMutex);
        if (!m_captureLeft.isNull()) m_leftImage = m_captureLeft;
        if (!m_captureRight.isNull()) m_rightImage = m_captureRight;
        m_hasNewImage = false;
    }

    // 清除旧的检测角点
    m_lastLeftCorners.clear();
    m_lastRightCorners.clear();

    UpdateImageDisplay();
}

void VrEyeViewWidget::onDetectChessboard()
{
    if (!m_detector) return;

    if (m_leftImage.isNull() || m_rightImage.isNull()) {
        m_lblDetectionResult->setText("检测结果: 请先加载左右目图片");
        return;
    }

    // 设置检测参数
    m_detector->SetDetectionFlags(
        m_cbAdaptiveThresh->isChecked(),
        m_cbNormalizeImage->isChecked(),
        false);

    // 准备相机内参
    CameraIntrinsics intrinsics;
    intrinsics.fx = m_sbFx->value();
    intrinsics.fy = m_sbFy->value();
    intrinsics.cx = m_sbCx->value();
    intrinsics.cy = m_sbCy->value();

    // 检测左目标定板
    QImage leftRgb = m_leftImage.convertToFormat(QImage::Format_RGB888);
    ChessboardDetectResult leftResult;
    int ret = m_detector->DetectChessboardWithPose(
        leftRgb.bits(),
        leftRgb.width(),
        leftRgb.height(),
        3,
        m_sbPatternWidth->value(),
        m_sbPatternHeight->value(),
        m_sbSquareSize->value(),
        intrinsics,
        leftResult);

    // 检测右目标定板
    QImage rightRgb = m_rightImage.convertToFormat(QImage::Format_RGB888);
    ChessboardDetectResult rightResult;
    int retRight = m_detector->DetectChessboardWithPose(
        rightRgb.bits(),
        rightRgb.width(),
        rightRgb.height(),
        3,
        m_sbPatternWidth->value(),
        m_sbPatternHeight->value(),
        m_sbSquareSize->value(),
        intrinsics,
        rightResult);

    // 在两张图上绘制角点并显示
    if (ret == 0 && leftResult.detected) {
        m_lastLeftCorners = leftResult.corners;
    } else {
        m_lastLeftCorners.clear();
    }
    if (retRight == 0 && rightResult.detected) {
        m_lastRightCorners = rightResult.corners;
    } else {
        m_lastRightCorners.clear();
    }

    // 在副本上绘制角点并显示（不修改原图）
    QImage leftDisplay = m_leftImage.copy();
    QImage rightDisplay = m_rightImage.copy();
    DrawCorners(leftDisplay, m_lastLeftCorners);
    DrawCorners(rightDisplay, m_lastRightCorners);

    QPixmap leftPm = QPixmap::fromImage(leftDisplay);
    m_leftImageLabel->setPixmap(
        leftPm.scaled(m_leftImageLabel->size(), Qt::KeepAspectRatio, Qt::SmoothTransformation));
    QPixmap rightPm = QPixmap::fromImage(rightDisplay);
    m_rightImageLabel->setPixmap(
        rightPm.scaled(m_rightImageLabel->size(), Qt::KeepAspectRatio, Qt::SmoothTransformation));

    if (ret == 0 && leftResult.detected && retRight == 0 && rightResult.detected) {
        m_lastDetection.detected = true;

        if (leftResult.hasPose) {
            m_lastDetection.x = leftResult.center.x;
            m_lastDetection.y = leftResult.center.y;
            m_lastDetection.z = leftResult.center.z;

            m_lastDetection.nx = leftResult.normal.x;
            m_lastDetection.ny = leftResult.normal.y;
            m_lastDetection.nz = leftResult.normal.z;

            m_lastDetection.rx = leftResult.eulerAngles[0];
            m_lastDetection.ry = leftResult.eulerAngles[1];
            m_lastDetection.rz = leftResult.eulerAngles[2];

            QString resultText = QString("检测结果: "
                                        "左目 %1 个角点, 右目 %2 个角点 | "
                                        "位置: (%3, %4, %5)mm | "
                                        "法向量: (%6, %7, %8) | "
                                        "姿态: (%9, %10, %11)")
                .arg(m_lastLeftCorners.size())
                .arg(m_lastRightCorners.size())
                .arg(m_lastDetection.x, 0, 'f', 3)
                .arg(m_lastDetection.y, 0, 'f', 3)
                .arg(m_lastDetection.z, 0, 'f', 3)
                .arg(m_lastDetection.nx, 0, 'f', 3)
                .arg(m_lastDetection.ny, 0, 'f', 3)
                .arg(m_lastDetection.nz, 0, 'f', 3)
                .arg(m_lastDetection.rx, 0, 'f', 3)
                .arg(m_lastDetection.ry, 0, 'f', 3)
                .arg(m_lastDetection.rz, 0, 'f', 3);
            m_lblDetectionResult->setText(resultText);

            if (m_detectionCallback) {
                m_detectionCallback(m_lastDetection);
            }
            emit chessboardDetected(m_lastDetection);
        }
    } else {
        m_lastDetection.detected = false;

        QString errorMsg = "检测结果: ";
        if (ret != 0 || !leftResult.detected) {
            errorMsg += "左目未检测到标定板 ";
        }
        if (retRight != 0 || !rightResult.detected) {
            errorMsg += "右目未检测到标定板";
        }
        m_lblDetectionResult->setText(errorMsg);
    }
}

void VrEyeViewWidget::onLoadLeftImage()
{
    QString fileName = QFileDialog::getOpenFileName(
        this, "选择左目图片", "",
        "图片文件 (*.png *.jpg *.jpeg *.bmp);;所有文件 (*.*)");

    if (fileName.isEmpty()) return;

    QImage image(fileName);
    if (image.isNull()) {
        QMessageBox::warning(this, "错误",
            QString("无法加载左目图片:\n%1").arg(fileName));
        return;
    }

    m_leftImage = image;
    UpdateImageDisplay();

    QFileInfo fi(fileName);
    m_lblStatus->setText(QString("状态: 已加载左目 %1 (%2x%3)")
                         .arg(fi.fileName())
                         .arg(image.width()).arg(image.height()));

    if (!m_leftImage.isNull() && !m_rightImage.isNull()) {
        m_btnDetect->setEnabled(true);
    }
}

void VrEyeViewWidget::onLoadRightImage()
{
    QString fileName = QFileDialog::getOpenFileName(
        this, "选择右目图片", "",
        "图片文件 (*.png *.jpg *.jpeg *.bmp);;所有文件 (*.*)");

    if (fileName.isEmpty()) return;

    QImage image(fileName);
    if (image.isNull()) {
        QMessageBox::warning(this, "错误",
            QString("无法加载右目图片:\n%1").arg(fileName));
        return;
    }

    m_rightImage = image;
    UpdateImageDisplay();

    QFileInfo fi(fileName);
    m_lblStatus->setText(QString("状态: 已加载右目 %1 (%2x%3)")
                         .arg(fi.fileName())
                         .arg(image.width()).arg(image.height()));

    if (!m_leftImage.isNull() && !m_rightImage.isNull()) {
        m_btnDetect->setEnabled(true);
    }
}

void VrEyeViewWidget::UpdateImageDisplay()
{
    // 左目：保持比例缩放到 label 大小，不拉伸
    if (!m_leftImage.isNull()) {
        QImage leftDisplay = m_leftImage.copy();
        if (!m_lastLeftCorners.empty()) {
            DrawCorners(leftDisplay, m_lastLeftCorners);
        }
        QPixmap pm = QPixmap::fromImage(leftDisplay);
        m_leftImageLabel->setPixmap(
            pm.scaled(m_leftImageLabel->size(), Qt::KeepAspectRatio, Qt::SmoothTransformation));
    }

    // 右目
    if (!m_rightImage.isNull()) {
        QImage rightDisplay = m_rightImage.copy();
        if (!m_lastRightCorners.empty()) {
            DrawCorners(rightDisplay, m_lastRightCorners);
        }
        QPixmap pm = QPixmap::fromImage(rightDisplay);
        m_rightImageLabel->setPixmap(
            pm.scaled(m_rightImageLabel->size(), Qt::KeepAspectRatio, Qt::SmoothTransformation));
    }
}

void VrEyeViewWidget::resizeEvent(QResizeEvent* event)
{
    QWidget::resizeEvent(event);
    UpdateImageDisplay();
}

void VrEyeViewWidget::DrawCorners(QImage& image, const std::vector<Point2D>& corners)
{
    if (corners.empty()) return;

    QPainter painter(&image);
    painter.setPen(QPen(Qt::green, 3));

    for (const auto& pt : corners) {
        painter.drawEllipse(QPointF(pt.x, pt.y), 5, 5);
    }
}