GrabBag/App/BinocularMark/BinocularMarkApp/BinocularMarkPresenter.cpp

#include "BinocularMarkPresenter.h"
#include "VrLog.h"
#include "VrError.h"
#include <QFile>
#include <QFileInfo>
#include <QDir>
#include <QCoreApplication>
#include <QDomDocument>
#include <QTextStream>
#include <cstring>
#include <sstream>
#include <iomanip>

BinocularMarkPresenter::BinocularMarkPresenter(QObject *parent)
    : QObject(parent)
    , m_pLeftCamera(nullptr)
    , m_pRightCamera(nullptr)
    , m_pCameraReconnectTimer(nullptr)
    , m_bCameraConnected(false)
    , m_bAutoStartDetection(false)
    , m_bIsDetecting(false)
    , m_bLeftImageReady(false)
    , m_bRightImageReady(false)
    , m_nServerPort(5901)
    , m_nLeftCameraIndex(0)
    , m_nRightCameraIndex(1)
    , m_fExposureTime(10000.0)
    , m_fGain(1.0)
    , m_disparityOffset(0.0)
{
    // 初始化GalaxyImageData
    m_leftImageData.pData = nullptr;
    m_leftImageData.width = 0;
    m_leftImageData.height = 0;
    m_leftImageData.dataSize = 0;
    m_leftImageData.pixelFormat = 0;
    m_leftImageData.frameID = 0;
    m_leftImageData.timestamp = 0;

    m_rightImageData.pData = nullptr;
    m_rightImageData.width = 0;
    m_rightImageData.height = 0;
    m_rightImageData.dataSize = 0;
    m_rightImageData.pixelFormat = 0;
    m_rightImageData.frameID = 0;
    m_rightImageData.timestamp = 0;

    // 初始化Q矩阵为单位矩阵 4x4
    m_Q = cv::Mat::eye(4, 4, CV_64F);

    // 初始化Mark配置（默认值）
    m_markInfo.patternSize = cv::Size(3, 3);  // 3x3 mark
    m_markInfo.checkerSize = 60.0f;            // 60mm
    m_markInfo.markerSize = 45.0f;             // 45mm
    m_markInfo.dictType = 1;                   // DICT_6x6

    // 初始化Board配置（默认值）
    m_boardInfo.totalBoardNum = 1;             // 默认1个board
    m_boardInfo.boardIdInterval = 8;           // 间隔8
    m_boardInfo.boardChaucoIDNum = 4;          // 3x3有4个charuco

    // 创建相机重连定时器（5秒间隔）
    m_pCameraReconnectTimer = new QTimer(this);
    m_pCameraReconnectTimer->setInterval(5000);
    connect(m_pCameraReconnectTimer, &QTimer::timeout, this, &BinocularMarkPresenter::onCameraReconnectTimer);

    LOG_INFO("BinocularMarkPresenter created\n");
}

BinocularMarkPresenter::~BinocularMarkPresenter()
{
    // 停止重连定时器
    if (m_pCameraReconnectTimer)
    {
        m_pCameraReconnectTimer->stop();
    }

    stopDetection();
    closeCameras();

    // 释放图像数据
    if (m_leftImageData.pData != nullptr)
    {
        delete[] m_leftImageData.pData;
        m_leftImageData.pData = nullptr;
    }
    if (m_rightImageData.pData != nullptr)
    {
        delete[] m_rightImageData.pData;
        m_rightImageData.pData = nullptr;
    }

    LOG_INFO("BinocularMarkPresenter destroyed\n");
}

int BinocularMarkPresenter::initCameras()
{
    LOG_INFO("Initializing cameras...\n");

    // 尝试连接相机
    int ret = tryConnectCameras();
    if (ret == SUCCESS)
    {
        // 连接成功
        m_bCameraConnected.store(true);
        emit cameraConnectionChanged(true);
        LOG_INFO("Cameras initialized successfully\n");
        return SUCCESS;
    }

    // 连接失败，启动重连定时器
    LOG_WARN("Failed to connect cameras, will retry every 5 seconds...\n");
    m_pCameraReconnectTimer->start();
    emit cameraConnectionChanged(false);

    return ERR_CODE(DEV_OPEN_ERR);
}

int BinocularMarkPresenter::tryConnectCameras()
{
    // 如果正在检测，先停止，并标记需要自动重启
    bool wasDetecting = m_bIsDetecting.load();
    if (wasDetecting)
    {
        LOG_INFO("Stopping detection before camera reconnection...\n");
        stopDetection();
        m_bAutoStartDetection.store(true);  // 标记重连成功后自动启动
    }

    // 清理之前的相机对象
    if (m_pLeftCamera != nullptr)
    {
        m_pLeftCamera->CloseDevice();
        delete m_pLeftCamera;
        m_pLeftCamera = nullptr;
    }
    if (m_pRightCamera != nullptr)
    {
        m_pRightCamera->CloseDevice();
        delete m_pRightCamera;
        m_pRightCamera = nullptr;
    }

    // 创建左相机对象
    int ret = IGalaxyDevice::CreateObject(&m_pLeftCamera);
    if (ret != 0 || m_pLeftCamera == nullptr)
    {
        LOG_ERROR("Failed to create left camera object\n");
        return ERR_CODE(APP_ERR_EXEC);
    }

    // 创建右相机对象
    ret = IGalaxyDevice::CreateObject(&m_pRightCamera);
    if (ret != 0 || m_pRightCamera == nullptr)
    {
        LOG_ERROR("Failed to create right camera object\n");
        delete m_pLeftCamera;
        m_pLeftCamera = nullptr;
        return ERR_CODE(APP_ERR_EXEC);
    }

    // 初始化Galaxy SDK
    ret = m_pLeftCamera->InitSDK();
    if (ret != SUCCESS)
    {
        LOG_ERROR("Failed to initialize Galaxy SDK\n");
        delete m_pLeftCamera;
        delete m_pRightCamera;
        m_pLeftCamera = nullptr;
        m_pRightCamera = nullptr;
        return ERR_CODE(APP_ERR_EXEC);
    }

    // 枚举设备
    std::vector<GalaxyDeviceInfo> deviceList;
    ret = m_pLeftCamera->EnumerateDevices(deviceList, 2000);
    if (ret != 0 || deviceList.size() < 2)
    {
        LOG_WARN("Failed to enumerate cameras or less than 2 cameras found (found: %zu)\n", deviceList.size());
        delete m_pLeftCamera;
        delete m_pRightCamera;
        m_pLeftCamera = nullptr;
        m_pRightCamera = nullptr;
        return ERR_CODE(APP_ERR_EXEC);
    }

    LOG_INFO("Found %zu cameras\n", deviceList.size());

    // 打开左相机
    ret = m_pLeftCamera->OpenDeviceByIndex(m_nLeftCameraIndex);
    if (ret != SUCCESS)
    {
        LOG_ERROR("Failed to open left camera (index: %u)\n", m_nLeftCameraIndex);
        delete m_pLeftCamera;
        delete m_pRightCamera;
        m_pLeftCamera = nullptr;
        m_pRightCamera = nullptr;
        return ERR_CODE(APP_ERR_EXEC);
    }

    LOG_INFO("Left camera opened (index: %u)\n", m_nLeftCameraIndex);

    // 初始化右相机SDK（每个设备对象需要独立初始化）
    ret = m_pRightCamera->InitSDK();
    if (ret != SUCCESS)
    {
        LOG_ERROR("Failed to initialize Galaxy SDK for right camera\n");
        m_pLeftCamera->CloseDevice();
        delete m_pLeftCamera;
        delete m_pRightCamera;
        m_pLeftCamera = nullptr;
        m_pRightCamera = nullptr;
        return ERR_CODE(APP_ERR_EXEC);
    }

    // 打开右相机
    ret = m_pRightCamera->OpenDeviceByIndex(m_nRightCameraIndex);
    if (ret != SUCCESS)
    {
        LOG_ERROR("Failed to open right camera (index: %u)\n", m_nRightCameraIndex);
        m_pLeftCamera->CloseDevice();
        delete m_pLeftCamera;
        delete m_pRightCamera;
        m_pLeftCamera = nullptr;
        m_pRightCamera = nullptr;
        return ERR_CODE(APP_ERR_EXEC);
    }

    LOG_INFO("Right camera opened (index: %u)\n", m_nRightCameraIndex);

    // 设置左相机参数
    m_pLeftCamera->SetExposureTime(m_fExposureTime);
    m_pLeftCamera->SetGain(m_fGain);
    m_pLeftCamera->SetTriggerMode(false);  // 连续采集模式

    // 设置右相机参数
    m_pRightCamera->SetExposureTime(m_fExposureTime);
    m_pRightCamera->SetGain(m_fGain);
    m_pRightCamera->SetTriggerMode(false);  // 连续采集模式

    LOG_INFO("Cameras connected successfully\n");
    return SUCCESS;
}

void BinocularMarkPresenter::closeCameras()
{
    if (m_pLeftCamera != nullptr)
    {
        m_pLeftCamera->CloseDevice();
        delete m_pLeftCamera;
        m_pLeftCamera = nullptr;
        LOG_INFO("Left camera closed\n");
    }

    if (m_pRightCamera != nullptr)
    {
        m_pRightCamera->CloseDevice();
        delete m_pRightCamera;
        m_pRightCamera = nullptr;
        LOG_INFO("Right camera closed\n");
    }

    m_bCameraConnected.store(false);
}

void BinocularMarkPresenter::onCameraReconnectTimer()
{
    // 尝试连接相机
    int ret = tryConnectCameras();
    if (ret == SUCCESS)
    {
        // 连接成功，停止定时器
        m_pCameraReconnectTimer->stop();
        m_bCameraConnected.store(true);
        emit cameraConnectionChanged(true);
        LOG_INFO("Camera reconnection successful!\n");

        // 如果之前尝试启动检测但失败了，现在自动重新启动
        if (m_bAutoStartDetection.load())
        {
            LOG_INFO("Auto-starting detection after camera reconnection...\n");
            m_bAutoStartDetection.store(false);  // 重置标志
            startDetection();
        }
    }
    else
    {
        // 连接失败，定时器会继续运行
        LOG_WARN("Camera reconnection failed, will retry in 5 seconds...\n");
    }
}

int BinocularMarkPresenter::startDetection()
{
    if (m_bIsDetecting.load())
    {
        LOG_WARN("Detection already started\n");
        return SUCCESS;
    }

    if (m_pLeftCamera == nullptr || m_pRightCamera == nullptr)
    {
        LOG_WARN("Cameras not initialized, will auto-start detection after camera reconnection\n");
        // 设置标志，待相机重连成功后自动启动检测
        m_bAutoStartDetection.store(true);
        return ERR_CODE(APP_ERR_EXEC);
    }

    // 注册左相机图像回调（使用lambda捕获this指针）
    int ret = m_pLeftCamera->RegisterImageCallback(
        [this](const GalaxyImageData& imageData) {
            this->leftCameraCallback(imageData);
        }
    );
    if (ret != SUCCESS)
    {
        LOG_ERROR("Failed to register left camera callback\n");
        return ERR_CODE(APP_ERR_EXEC);
    }

    // 注册右相机图像回调
    ret = m_pRightCamera->RegisterImageCallback(
        [this](const GalaxyImageData& imageData) {
            this->rightCameraCallback(imageData);
        }
    );
    if (ret != SUCCESS)
    {
        LOG_ERROR("Failed to register right camera callback\n");
        m_pLeftCamera->UnregisterImageCallback();
        return ERR_CODE(APP_ERR_EXEC);
    }

    // 启动左相机采集
    ret = m_pLeftCamera->StartAcquisition();
    if (ret != SUCCESS)
    {
        LOG_ERROR("Failed to start left camera acquisition\n");
        m_pLeftCamera->UnregisterImageCallback();
        m_pRightCamera->UnregisterImageCallback();
        return ERR_CODE(APP_ERR_EXEC);
    }

    // 启动右相机采集
    ret = m_pRightCamera->StartAcquisition();
    if (ret != SUCCESS)
    {
        LOG_ERROR("Failed to start right camera acquisition\n");
        m_pLeftCamera->StopAcquisition();
        m_pLeftCamera->UnregisterImageCallback();
        m_pRightCamera->UnregisterImageCallback();
        return ERR_CODE(APP_ERR_EXEC);
    }

    m_bIsDetecting.store(true);
    m_bAutoStartDetection.store(false);  // 清除自动启动标志

    // 启动采集线程
    m_captureThread = std::thread(&BinocularMarkPresenter::captureThreadFunc, this);

    LOG_INFO("Detection started\n");
    return SUCCESS;
}

void BinocularMarkPresenter::stopDetection()
{
    if (!m_bIsDetecting.load())
        return;

    m_bIsDetecting.store(false);
    m_bAutoStartDetection.store(false);  // 清除自动启动标志（用户主动停止）

    // 停止相机采集
    if (m_pLeftCamera != nullptr)
    {
        m_pLeftCamera->StopAcquisition();
        m_pLeftCamera->UnregisterImageCallback();
    }
    if (m_pRightCamera != nullptr)
    {
        m_pRightCamera->StopAcquisition();
        m_pRightCamera->UnregisterImageCallback();
    }

    // 等待线程结束
    if (m_captureThread.joinable())
    {
        m_captureThread.join();
    }

    LOG_INFO("Detection stopped\n");
}

void BinocularMarkPresenter::leftCameraCallback(const GalaxyImageData& imageData)
{
    QMutexLocker locker(&m_imageMutex);

    // 检查是否需要重新分配内存
    if (m_leftImageData.dataSize != imageData.dataSize)
    {
        // 大小不同，需要重新分配
        if (m_leftImageData.pData != nullptr)
        {
            delete[] m_leftImageData.pData;
            m_leftImageData.pData = nullptr;
        }

        if (imageData.dataSize > 0)
        {
            m_leftImageData.pData = new unsigned char[imageData.dataSize];
        }
    }

    // 复制图像数据
    m_leftImageData.width = imageData.width;
    m_leftImageData.height = imageData.height;
    m_leftImageData.dataSize = imageData.dataSize;
    m_leftImageData.pixelFormat = imageData.pixelFormat;
    m_leftImageData.frameID = imageData.frameID;
    m_leftImageData.timestamp = imageData.timestamp;

    if (m_leftImageData.pData != nullptr && imageData.pData != nullptr)
    {
        memcpy(m_leftImageData.pData, imageData.pData, imageData.dataSize);
    }

    m_bLeftImageReady = true;
}

void BinocularMarkPresenter::rightCameraCallback(const GalaxyImageData& imageData)
{
    QMutexLocker locker(&m_imageMutex);

    // 检查是否需要重新分配内存
    if (m_rightImageData.dataSize != imageData.dataSize)
    {
        // 大小不同，需要重新分配
        if (m_rightImageData.pData != nullptr)
        {
            delete[] m_rightImageData.pData;
            m_rightImageData.pData = nullptr;
        }

        if (imageData.dataSize > 0)
        {
            m_rightImageData.pData = new unsigned char[imageData.dataSize];
        }
    }

    // 复制图像数据
    m_rightImageData.width = imageData.width;
    m_rightImageData.height = imageData.height;
    m_rightImageData.dataSize = imageData.dataSize;
    m_rightImageData.pixelFormat = imageData.pixelFormat;
    m_rightImageData.frameID = imageData.frameID;
    m_rightImageData.timestamp = imageData.timestamp;

    if (m_rightImageData.pData != nullptr && imageData.pData != nullptr)
    {
        memcpy(m_rightImageData.pData, imageData.pData, imageData.dataSize);
    }

    m_bRightImageReady = true;
}

void BinocularMarkPresenter::captureThreadFunc()
{
    LOG_INFO("Capture thread started\n");

    while (m_bIsDetecting.load())
    {
        // 等待双目图像都准备好
        bool leftReady = false;
        bool rightReady = false;

        {
            QMutexLocker locker(&m_imageMutex);
            leftReady = m_bLeftImageReady;
            rightReady = m_bRightImageReady;
        }

        if (leftReady && rightReady)
        {
            // 处理图像并进行检测
            processImages();

            // 清除就绪标志
            {
                QMutexLocker locker(&m_imageMutex);
                m_bLeftImageReady = false;
                m_bRightImageReady = false;
            }
        }
        else
        {
            // 等待图像
            std::this_thread::sleep_for(std::chrono::milliseconds(10));
        }
    }

    LOG_INFO("Capture thread stopped\n");
}

void BinocularMarkPresenter::processImages()
{
    // 直接在互斥锁中转换为 cv::Mat（深拷贝），避免多余的临时变量和二次拷贝
    cv::Mat leftImage, rightImage;
    {
        QMutexLocker locker(&m_imageMutex);

        // 直接转换为 cv::Mat（imageDataToMat 会执行深拷贝）
        leftImage = imageDataToMat(m_leftImageData);
        rightImage = imageDataToMat(m_rightImageData);
    }

    // 调用算法进行检测
    std::vector<SWD_charuco3DMark> marks;

    // 使用新的算法接口
    wd_BQ_getCharuco3DMark(
        leftImage,
        rightImage,
        m_cameraMatrixL,
        m_distCoeffsL,
        m_cameraMatrixR,
        m_distCoeffsR,
        m_R1,
        m_R2,
        m_P1,
        m_P2,
        m_Q,
        m_markInfo,
        m_boardInfo,
        m_disparityOffset,
        marks
    );

    // 发送检测结果信号（错误码通过marks的数量判断：0个mark可能表示检测失败）
    int errCode = marks.empty() ? -1 : 0;
    emit detectionResult(marks, leftImage, rightImage, errCode);

    LOG_DEBUG("Processed image pair, detected %zu marks\n", marks.size());
}

cv::Mat BinocularMarkPresenter::imageDataToMat(const GalaxyImageData& imageData)
{
    // 根据像素格式创建cv::Mat
    // 假设是单通道8位灰度图或3通道BGR图像
    int cvType = CV_8UC1;  // 默认灰度图

    // 这里需要根据imageData.pixelFormat判断类型
    // 简化处理：假设是灰度图
    if (imageData.dataSize == imageData.width * imageData.height)
    {
        cvType = CV_8UC1;  // 灰度图
    }
    else if (imageData.dataSize == imageData.width * imageData.height * 3)
    {
        cvType = CV_8UC3;  // BGR图
    }

    cv::Mat image(imageData.height, imageData.width, cvType);

    // 复制数据
    memcpy(image.data, imageData.pData, imageData.dataSize);

    return image;
}

bool BinocularMarkPresenter::loadConfiguration(const QString& configFilePath)
{
    QFile file(configFilePath);
    if (!file.open(QIODevice::ReadOnly | QIODevice::Text))
    {
        LOG_WARN("Failed to open config file: %s\n", configFilePath.toStdString().c_str());
        return false;
    }

    QDomDocument doc;
    QString errorMsg;
    int errorLine, errorColumn;
    if (!doc.setContent(&file, &errorMsg, &errorLine, &errorColumn))
    {
        LOG_ERROR("Failed to parse config XML: %s (Line: %d, Column: %d)\n",
                  errorMsg.toStdString().c_str(), errorLine, errorColumn);
        file.close();
        return false;
    }
    file.close();

    QDomElement root = doc.documentElement();
    if (root.tagName() != "BinocularMarkConfig")
    {
        LOG_ERROR("Invalid config file: root element should be 'BinocularMarkConfig'\n");
        return false;
    }

    LOG_DEBUG("========== 开始读取配置文件: %s ==========\n", configFilePath.toStdString().c_str());

    // 读取TCP服务器配置
    QDomElement serverConfig = root.firstChildElement("ServerConfig");
    if (!serverConfig.isNull())
    {
        m_nServerPort = static_cast<quint16>(serverConfig.attribute("port", "5901").toInt());
        LOG_DEBUG("  [ServerConfig] port = %d\n", m_nServerPort);
    }
    else
    {
        LOG_DEBUG("  [ServerConfig] 未找到，使用默认端口: %d\n", m_nServerPort);
    }

    // 读取相机配置
    QDomElement cameras = root.firstChildElement("Cameras");
    if (!cameras.isNull())
    {
        LOG_DEBUG("  [Cameras] 开始读取相机配置...\n");
        QDomElement camera = cameras.firstChildElement("Camera");
        while (!camera.isNull())
        {
            int index = camera.attribute("index", "0").toInt();
            double exposureTime = camera.attribute("exposureTime", "10000.0").toDouble();
            double gain = camera.attribute("gain", "1.0").toDouble();

            LOG_DEBUG("    Camera[%d]: exposureTime=%.2f, gain=%.2f\n", index, exposureTime, gain);

            if (index == 0)
            {
                m_nLeftCameraIndex = 0;
                m_fExposureTime = exposureTime;
                m_fGain = gain;
                LOG_DEBUG("      -> 设置为左相机\n");
            }
            else if (index == 1)
            {
                m_nRightCameraIndex = 1;
                LOG_DEBUG("      -> 设置为右相机\n");
                // 假设左右相机使用相同的曝光和增益
            }

            camera = camera.nextSiblingElement("Camera");
        }
    }
    else
    {
        LOG_DEBUG("  [Cameras] 未找到，使用默认相机配置\n");
    }

    // 读取Mark板参数
    QDomElement markInfo = root.firstChildElement("MarkInfo");
    if (!markInfo.isNull())
    {
        m_markInfo.patternSize.width = markInfo.attribute("patternWidth", "3").toInt();
        m_markInfo.patternSize.height = markInfo.attribute("patternHeight", "3").toInt();
        m_markInfo.checkerSize = markInfo.attribute("checkerSize", "60.0").toFloat();
        m_markInfo.markerSize = markInfo.attribute("markerSize", "45.0").toFloat();
        m_markInfo.dictType = markInfo.attribute("dictType", "1").toInt();

        LOG_DEBUG("  [MarkInfo]\n");
        LOG_DEBUG("    patternSize: %d x %d\n", m_markInfo.patternSize.width, m_markInfo.patternSize.height);
        LOG_DEBUG("    checkerSize: %.2f mm\n", m_markInfo.checkerSize);
        LOG_DEBUG("    markerSize: %.2f mm\n", m_markInfo.markerSize);
        LOG_DEBUG("    dictType: %d\n", m_markInfo.dictType);
    }
    else
    {
        LOG_DEBUG("  [MarkInfo] 未找到，使用默认值\n");
    }

    // 读取Board配置
    QDomElement boardInfo = root.firstChildElement("BoardInfo");
    if (!boardInfo.isNull())
    {
        m_boardInfo.totalBoardNum = boardInfo.attribute("totalBoardNum", "1").toInt();
        m_boardInfo.boardIdInterval = boardInfo.attribute("boardIdInterval", "8").toInt();
        m_boardInfo.boardChaucoIDNum = boardInfo.attribute("boardCharucoIdNum", "4").toInt();

        LOG_DEBUG("  [BoardInfo]\n");
        LOG_DEBUG("    totalBoardNum: %d\n", m_boardInfo.totalBoardNum);
        LOG_DEBUG("    boardIdInterval: %d\n", m_boardInfo.boardIdInterval);
        LOG_DEBUG("    boardCharucoIdNum: %d\n", m_boardInfo.boardChaucoIDNum);
    }
    else
    {
        LOG_DEBUG("  [BoardInfo] 未找到，使用默认值\n");
    }

    // 读取算法参数
    QDomElement algoParams = root.firstChildElement("AlgorithmParams");
    if (!algoParams.isNull())
    {
        m_disparityOffset = algoParams.attribute("disparityOffset", "0.0").toDouble();
        LOG_DEBUG("  [AlgorithmParams]\n");
        LOG_DEBUG("    disparityOffset: %.6f\n", m_disparityOffset);
    }
    else
    {
        LOG_DEBUG("  [AlgorithmParams] 未找到，使用默认值\n");
    }

    // 读取标定文件路径（必须配置）
    QDomElement calibFileElem = root.firstChildElement("CalibrationFile");
    if (calibFileElem.isNull())
    {
        LOG_ERROR("Missing <CalibrationFile> element in config file!\n");
        LOG_ERROR("Please add: <CalibrationFile path=\"path/to/StereoCamera.xml\" />\n");
        return false;
    }

    QString calibFile = calibFileElem.attribute("path", "");
    LOG_DEBUG("  [CalibrationFile] path (原始) = %s\n", calibFile.toStdString().c_str());

    if (calibFile.isEmpty())
    {
        LOG_ERROR("CalibrationFile path is empty in config file!\n");
        LOG_ERROR("Please set path attribute: <CalibrationFile path=\"../Calib/Mark_13度/StereoCamera.xml\" />\n");
        return false;
    }

    // 如果是相对路径，则相对于配置文件所在目录
    if (QFileInfo(calibFile).isRelative())
    {
        QFileInfo configFileInfo(configFilePath);
        calibFile = configFileInfo.dir().absoluteFilePath(calibFile);
        LOG_DEBUG("  [CalibrationFile] path (绝对路径) = %s\n", calibFile.toStdString().c_str());
    }

    LOG_DEBUG("========== 配置文件读取完成 ==========\n\n");

    // 加载标定文件
    if (!loadCalibration(calibFile))
    {
        LOG_ERROR("Failed to load calibration file: %s\n", calibFile.toStdString().c_str());
        return false;
    }

    LOG_INFO("Configuration loaded from %s\n", configFilePath.toStdString().c_str());
    LOG_INFO("  Server port: %d\n", m_nServerPort);
    LOG_INFO("  Camera indices: L=%d, R=%d\n", m_nLeftCameraIndex, m_nRightCameraIndex);
    LOG_INFO("  Mark pattern: %dx%d, checker=%fmm, marker=%fmm\n",
             m_markInfo.patternSize.width, m_markInfo.patternSize.height,
             m_markInfo.checkerSize, m_markInfo.markerSize);
    LOG_INFO("  Board info: total=%d, interval=%d, charucoNum=%d\n",
             m_boardInfo.totalBoardNum, m_boardInfo.boardIdInterval, m_boardInfo.boardChaucoIDNum);
    LOG_INFO("  Disparity offset: %f\n", m_disparityOffset);

    return true;
}

bool BinocularMarkPresenter::loadCalibration(const QString& calibFilePath)
{
    LOG_DEBUG("========== 开始加载标定文件: %s ==========\n", calibFilePath.toStdString().c_str());

    // 使用OpenCV的FileStorage读取XML文件
    cv::FileStorage fs(calibFilePath.toStdString(), cv::FileStorage::READ);
    if (!fs.isOpened())
    {
        LOG_ERROR("Failed to open calibration file: %s\n", calibFilePath.toStdString().c_str());
        return false;
    }

    // 读取左相机参数
    cv::FileNode leftCameraNode = fs["LeftCamera"];
    if (!leftCameraNode.empty())
    {
        LOG_DEBUG("  [LeftCamera]\n");
        leftCameraNode["CameraMatrix"] >> m_cameraMatrixL;
        leftCameraNode["DistCoeffs"] >> m_distCoeffsL;

        if (!m_cameraMatrixL.empty())
        {
            LOG_DEBUG("    CameraMatrix (3x3):\n");
            LOG_DEBUG("      [%.6f, %.6f, %.6f]\n", m_cameraMatrixL.at<double>(0, 0), m_cameraMatrixL.at<double>(0, 1), m_cameraMatrixL.at<double>(0, 2));
            LOG_DEBUG("      [%.6f, %.6f, %.6f]\n", m_cameraMatrixL.at<double>(1, 0), m_cameraMatrixL.at<double>(1, 1), m_cameraMatrixL.at<double>(1, 2));
            LOG_DEBUG("      [%.6f, %.6f, %.6f]\n", m_cameraMatrixL.at<double>(2, 0), m_cameraMatrixL.at<double>(2, 1), m_cameraMatrixL.at<double>(2, 2));
        }

        if (!m_distCoeffsL.empty())
        {
            std::stringstream ss;
            ss << "    DistCoeffs (" << m_distCoeffsL.cols << "个系数): [";
            for (int i = 0; i < m_distCoeffsL.cols; i++)
            {
                ss << std::fixed << std::setprecision(6) << m_distCoeffsL.at<double>(0, i);
                if (i < m_distCoeffsL.cols - 1) ss << ", ";
            }
            ss << "]";
            LOG_DEBUG("%s\n", ss.str().c_str());
        }
    }
    else
    {
        LOG_ERROR("  [LeftCamera] 节点未找到！\n");
    }

    // 读取右相机参数
    cv::FileNode rightCameraNode = fs["RightCamera"];
    if (!rightCameraNode.empty())
    {
        LOG_DEBUG("  [RightCamera] \n");
        rightCameraNode["CameraMatrix"] >> m_cameraMatrixR;
        rightCameraNode["DistCoeffs"] >> m_distCoeffsR;

        if (!m_cameraMatrixR.empty())
        {
            LOG_DEBUG("    CameraMatrix (3x3):\n");
            LOG_DEBUG("      [%.6f, %.6f, %.6f]\n", m_cameraMatrixR.at<double>(0, 0), m_cameraMatrixR.at<double>(0, 1), m_cameraMatrixR.at<double>(0, 2));
            LOG_DEBUG("      [%.6f, %.6f, %.6f]\n", m_cameraMatrixR.at<double>(1, 0), m_cameraMatrixR.at<double>(1, 1), m_cameraMatrixR.at<double>(1, 2));
            LOG_DEBUG("      [%.6f, %.6f, %.6f]\n", m_cameraMatrixR.at<double>(1, 0), m_cameraMatrixR.at<double>(1, 1), m_cameraMatrixR.at<double>(1, 2));
            LOG_DEBUG("      [%.6f, %.6f, %.6f]\n", m_cameraMatrixR.at<double>(2, 0), m_cameraMatrixR.at<double>(2, 1), m_cameraMatrixR.at<double>(2, 2));
        }

        if (!m_distCoeffsR.empty())
        {
            std::stringstream ss;
            ss << "    DistCoeffs (" << m_distCoeffsR.cols << "个系数): [";
            for (int i = 0; i < m_distCoeffsR.cols; i++)
            {
                ss << std::fixed << std::setprecision(6) << m_distCoeffsR.at<double>(0, i);
                if (i < m_distCoeffsR.cols - 1) ss << ", ";
            }
            ss << "]";
            LOG_DEBUG("%s\n", ss.str().c_str());
        }
    }
    else
    {
        LOG_ERROR("  [RightCamera] 节点未找到！\n");
    }

    // 读取双目标定参数（用于参考，算法主要使用校正后的参数）
    cv::FileNode stereoNode = fs["Stereo"];
    if (!stereoNode.empty())
    {
        double baseline = stereoNode["Baseline"];
        LOG_DEBUG("  [Stereo] baseline = %.6f mm\n", baseline);
    }
    else
    {
        LOG_DEBUG("  [Stereo] 节点未找到\n");
    }

    // 读取校正参数（Rectification）
    cv::FileNode rectNode = fs["Rectification"];
    if (!rectNode.empty())
    {
        LOG_DEBUG("  [Rectification] 节点找到\n");

        rectNode["R1"] >> m_R1;
        rectNode["R2"] >> m_R2;
        rectNode["P1"] >> m_P1;
        rectNode["P2"] >> m_P2;
        rectNode["Q"] >> m_Q;

        if (!m_R1.empty())
        {
            LOG_DEBUG("    R1 (3x3):\n");
            std::stringstream ss;
            for (int i = 0; i < 3; i++)
            {
                ss.str("");
                ss << "      [";
                ss << std::fixed << std::setprecision(6) << m_R1.at<double>(i, 0) << ", ";
                ss << m_R1.at<double>(i, 1) << ", ";
                ss << m_R1.at<double>(i, 2) << "]";
                LOG_DEBUG("%s\n", ss.str().c_str());
            }
        }

        if (!m_R2.empty())
        {
            LOG_DEBUG("    R2 (3x3):\n");
            std::stringstream ss;
            for (int i = 0; i < 3; i++)
            {
                ss.str("");
                ss << "      [";
                ss << std::fixed << std::setprecision(6) << m_R2.at<double>(i, 0) << ", ";
                ss << m_R2.at<double>(i, 1) << ", ";
                ss << m_R2.at<double>(i, 2) << "]";
                LOG_DEBUG("%s\n", ss.str().c_str());
            }
        }

        if (!m_P1.empty())
        {
            LOG_DEBUG("    P1 (3x4):\n");
            std::stringstream ss;
            for (int i = 0; i < 3; i++)
            {
                ss.str("");
                ss << "      [";
                ss << std::fixed << std::setprecision(6) << m_P1.at<double>(i, 0) << ", ";
                ss << m_P1.at<double>(i, 1) << ", ";
                ss << m_P1.at<double>(i, 2) << ", ";
                ss << m_P1.at<double>(i, 3) << "]";
                LOG_DEBUG("%s\n", ss.str().c_str());
            }
        }

        if (!m_P2.empty())
        {
            LOG_DEBUG("    P2 (3x4):\n");
            std::stringstream ss;
            for (int i = 0; i < 3; i++)
            {
                ss.str("");
                ss << "      [";
                ss << std::fixed << std::setprecision(6) << m_P2.at<double>(i, 0) << ", ";
                ss << m_P2.at<double>(i, 1) << ", ";
                ss << m_P2.at<double>(i, 2) << ", ";
                ss << m_P2.at<double>(i, 3) << "]";
                LOG_DEBUG("%s\n", ss.str().c_str());
            }
        }

        if (!m_Q.empty())
        {
            LOG_DEBUG("    Q (4x4):\n");
            std::stringstream ss;
            for (int i = 0; i < 4; i++)
            {
                ss.str("");
                ss << "      [";
                ss << std::fixed << std::setprecision(6) << m_Q.at<double>(i, 0) << ", ";
                ss << m_Q.at<double>(i, 1) << ", ";
                ss << m_Q.at<double>(i, 2) << ", ";
                ss << m_Q.at<double>(i, 3) << "]";
                LOG_DEBUG("%s\n", ss.str().c_str());
            }
        }
    }
    else
    {
        LOG_ERROR("  [Rectification] 节点未找到！\n");
    }

    fs.release();

    // 验证关键参数是否加载成功
    bool success = !m_cameraMatrixL.empty() && !m_cameraMatrixR.empty() &&
                   !m_distCoeffsL.empty() && !m_distCoeffsR.empty() &&
                   !m_R1.empty() && !m_R2.empty() &&
                   !m_P1.empty() && !m_P2.empty() && !m_Q.empty();

    if (success)
    {
        LOG_DEBUG("========== 标定文件加载成功 ==========\n\n");
        LOG_INFO("Calibration loaded successfully from %s\n", calibFilePath.toStdString().c_str());
    }
    else
    {
        LOG_ERROR("========== 标定文件加载失败或不完整 ==========\n\n");
        LOG_ERROR("Calibration file incomplete or invalid\n");
    }

    return success;
}