958 lines
30 KiB
C++
958 lines
30 KiB
C++
#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;
|
||
}
|