851 lines
31 KiB
C++
851 lines
31 KiB
C++
#include "dialogcameralevel.h"
|
||
#include "ui_dialogcameralevel.h"
|
||
#include "IVrUtils.h"
|
||
#include "PathManager.h"
|
||
#include <QThread>
|
||
#include <QApplication>
|
||
#include <cmath>
|
||
#include <mutex>
|
||
#include <vector>
|
||
|
||
#include "LaserDataLoader.h"
|
||
#include <QCoreApplication>
|
||
#include <QFileInfo>
|
||
#include <QTimer>
|
||
#include <QFileDialog>
|
||
#include <QStandardPaths>
|
||
#include "WorkpiecePresenter.h"
|
||
#include "BasePresenter.h"
|
||
#include "BQ_workpieceCornerExtraction_Export.h"
|
||
|
||
DialogCameraLevel::DialogCameraLevel(QWidget *parent)
|
||
: QDialog(parent)
|
||
, ui(new Ui::DialogCameraLevel)
|
||
{
|
||
ui->setupUi(this);
|
||
|
||
// 隐藏标题栏
|
||
setWindowFlags(Qt::FramelessWindowHint);
|
||
|
||
// 初始化界面
|
||
initializeCameraCombo();
|
||
|
||
// 初始化结果显示区域
|
||
ui->label_level_result->setText("请选择相机并点击调平按钮\n开始相机调平操作");
|
||
ui->label_level_result->setAlignment(Qt::AlignCenter);
|
||
}
|
||
|
||
DialogCameraLevel::~DialogCameraLevel()
|
||
{
|
||
// 清理扫描数据缓存
|
||
clearScanDataCache();
|
||
|
||
// 确保恢复Presenter的状态回调
|
||
restorePresenterStatusCallback();
|
||
|
||
delete ui;
|
||
}
|
||
|
||
void DialogCameraLevel::setCameraList(const std::vector<std::pair<std::string, IVrEyeDevice*>>& cameraList,
|
||
WorkpiecePresenter* presenter)
|
||
{
|
||
m_cameraList = cameraList;
|
||
m_presenter = presenter;
|
||
|
||
// 重新初始化相机选择框
|
||
initializeCameraCombo();
|
||
}
|
||
|
||
void DialogCameraLevel::initializeCameraCombo()
|
||
{
|
||
ui->combo_camera->clear();
|
||
|
||
if (m_cameraList.empty()) {
|
||
ui->combo_camera->setEnabled(false);
|
||
ui->label_level_result->setText("无可用相机设备");
|
||
ui->label_level_result->setAlignment(Qt::AlignCenter);
|
||
} else {
|
||
for (const auto& camera : m_cameraList) {
|
||
ui->combo_camera->addItem(QString::fromStdString(camera.first));
|
||
}
|
||
ui->combo_camera->setEnabled(true);
|
||
|
||
// 检查并显示当前选中相机的标定状态
|
||
checkAndDisplayCalibrationStatus(0); // 默认选中第一个相机
|
||
}
|
||
}
|
||
|
||
void DialogCameraLevel::on_btn_apply_clicked()
|
||
{
|
||
ui->label_level_result->setAlignment(Qt::AlignLeft);
|
||
|
||
#ifndef LEVEL_DEBUG_MODE
|
||
// 检查是否有可用的相机
|
||
if (m_cameraList.empty()) {
|
||
QMessageBox::warning(this, "错误", "无可用相机设备！");
|
||
return;
|
||
}
|
||
|
||
// 获取选中的相机
|
||
int selectedIndex = ui->combo_camera->currentIndex();
|
||
if (selectedIndex < 0 || selectedIndex >= m_cameraList.size()) {
|
||
QMessageBox::warning(this, "错误", "请选择有效的相机！");
|
||
return;
|
||
}
|
||
#endif
|
||
|
||
// 清空之前的结果显示
|
||
ui->label_level_result->setText("调平计算中，请稍候...");
|
||
|
||
// 显示进度提示
|
||
ui->btn_apply->setEnabled(false);
|
||
QApplication::processEvents();
|
||
|
||
try {
|
||
// 执行相机调平
|
||
if (performCameraLeveling()) {
|
||
// 调平成功，关闭对话框（这会触发析构函数中的回调恢复）
|
||
// accept();
|
||
} else {
|
||
// 显示失败信息到界面
|
||
ui->label_level_result->setText("调平失败！\n\n请检查：\n1. 相机连接是否正常\n2. 地面扫描数据是否充足\n3. 扫描区域是否有足够的地面");
|
||
}
|
||
} catch (const std::exception& e) {
|
||
LOG_ERROR("Camera leveling failed with exception: %s\n", e.what());
|
||
QMessageBox::critical(this, "错误", QString("调平过程发生异常：%1").arg(e.what()));
|
||
}
|
||
|
||
// 恢复按钮状态
|
||
ui->btn_apply->setEnabled(true);
|
||
}
|
||
|
||
void DialogCameraLevel::on_btn_cancel_clicked()
|
||
{
|
||
// 直接关闭窗口，不保存任何更改
|
||
reject();
|
||
}
|
||
|
||
bool DialogCameraLevel::performCameraLeveling()
|
||
{
|
||
try {
|
||
// 获取选中的相机索引
|
||
int selectedIndex = ui->combo_camera->currentIndex();
|
||
|
||
// 1. 设置调平状态回调
|
||
setLevelingStatusCallback();
|
||
|
||
#ifdef LEVEL_DEBUG_MODE
|
||
// Debug模式：使用文件对话框选择测试数据
|
||
LOG_INFO("=== DEBUG MODE ENABLED ===\n");
|
||
|
||
// 选择debug数据文件
|
||
QString debugDataFile = selectDebugDataFile();
|
||
if (debugDataFile.isEmpty()) {
|
||
LOG_INFO("Debug data file selection cancelled\n");
|
||
return false;
|
||
}
|
||
|
||
// 使用选择的debug数据进行模拟扫描
|
||
if (!loadDebugDataAndSimulateScan(debugDataFile)) {
|
||
LOG_ERROR("Failed to load debug data for camera leveling\n");
|
||
return false;
|
||
}
|
||
#else
|
||
// 正常模式：使用真实相机
|
||
if (selectedIndex < 0 || selectedIndex >= m_cameraList.size()) {
|
||
LOG_ERROR("Invalid camera index: %d\n", selectedIndex);
|
||
return false;
|
||
}
|
||
|
||
LOG_INFO("Performing camera leveling with camera %d (index %d)\n", selectedIndex + 1, selectedIndex);
|
||
|
||
// 2. 清空之前的扫描数据
|
||
clearScanDataCache();
|
||
|
||
// 3. 启动相机扫描地面数据
|
||
if (!startCameraScan(selectedIndex)) {
|
||
LOG_ERROR("Failed to start camera scan for leveling\n");
|
||
return false;
|
||
}
|
||
|
||
// 4. 等待扫描完成（使用状态回调判断）
|
||
LOG_INFO("Collecting ground scan data, waiting for swing finish signal...\n");
|
||
int waitTime = 0;
|
||
const int maxWaitTime = 30000; // 最大等待10秒
|
||
const int checkInterval = 100; // 每100ms检查一次
|
||
|
||
while (!m_swingFinished && waitTime < maxWaitTime) {
|
||
QThread::msleep(checkInterval);
|
||
QApplication::processEvents(); // 处理UI事件
|
||
waitTime += checkInterval;
|
||
}
|
||
|
||
// 5. 停止扫描
|
||
stopCameraScan(selectedIndex);
|
||
|
||
// 检查是否通过状态回调收到了扫描完成信号
|
||
if (m_swingFinished) {
|
||
LOG_INFO("Camera swing finished signal received, scan completed\n");
|
||
} else if (waitTime >= maxWaitTime) {
|
||
LOG_WARNING("Timeout waiting for camera swing finish signal\n");
|
||
}
|
||
#endif
|
||
|
||
// 5. 检查是否收集到足够的数据
|
||
// 6. 调用调平算法计算
|
||
double planeCalib[9];
|
||
double planeHeight;
|
||
double invRMatrix[9];
|
||
|
||
if (!calculatePlaneCalibration(planeCalib, planeHeight, invRMatrix)) {
|
||
LOG_ERROR("Failed to calculate plane calibration\n");
|
||
return false;
|
||
}
|
||
|
||
LOG_INFO("Camera leveling calculation completed\n");
|
||
|
||
// 7. 更新界面显示
|
||
updateLevelingResults(planeCalib, planeHeight, invRMatrix);
|
||
|
||
// 8. 保存结果到配置
|
||
// 获取相机索引和名称传递给保存方法
|
||
int cameraIndex = selectedIndex + 1; // 转换为1-based索引
|
||
QString cameraName;
|
||
|
||
#ifdef LEVEL_DEBUG_MODE
|
||
// Debug模式下使用默认名称
|
||
cameraIndex = 1;
|
||
cameraName = QString("Camera_%1").arg(cameraIndex);
|
||
#else
|
||
// 正常模式下从列表获取名称
|
||
if (selectedIndex >= 0 && selectedIndex < m_cameraList.size()) {
|
||
cameraName = QString::fromStdString(m_cameraList[selectedIndex].first);
|
||
} else {
|
||
cameraName = QString("Camera_%1").arg(cameraIndex);
|
||
}
|
||
#endif
|
||
|
||
if (!saveLevelingResults(planeCalib, planeHeight, invRMatrix, cameraIndex, cameraName)) {
|
||
LOG_ERROR("Failed to save leveling results\n");
|
||
return false;
|
||
}
|
||
|
||
clearScanDataCache();
|
||
LOG_INFO("Camera leveling completed successfully\n");
|
||
|
||
return true;
|
||
|
||
} catch (const std::exception& e) {
|
||
LOG_ERROR("Exception in performCameraLeveling: %s\n", e.what());
|
||
|
||
return false;
|
||
}
|
||
}
|
||
|
||
void DialogCameraLevel::updateLevelingResults(double planeCalib[9], double planeHeight, double invRMatrix[9])
|
||
{
|
||
// 计算旋转角度的近似值用于显示
|
||
double rotX = atan2(planeCalib[5], planeCalib[8]) * 180.0 / M_PI;
|
||
double rotY = atan2(-planeCalib[2], sqrt(planeCalib[5]*planeCalib[5] + planeCalib[8]*planeCalib[8])) * 180.0 / M_PI;
|
||
|
||
// 构建显示文本，将矩阵直接显示到页面上
|
||
QString resultText;
|
||
#if 0
|
||
// 基本信息
|
||
resultText += QString("旋转角度 X: %1°\n").arg(QString::number(rotX, 'f', 2));
|
||
resultText += QString("旋转角度 Y: %1°\n").arg(QString::number(rotY, 'f', 2));
|
||
#endif
|
||
|
||
resultText += QString("地面高度: %1 mm\n").arg(QString::number(planeHeight, 'f', 2));
|
||
|
||
// 调平矩阵
|
||
resultText += QString("调平矩阵:\n");
|
||
for (int i = 0; i < 3; i++) {
|
||
resultText += QString("[%1, %2, %3]\n")
|
||
.arg(QString::number(planeCalib[i*3], 'f', 4))
|
||
.arg(QString::number(planeCalib[i*3+1], 'f', 4))
|
||
.arg(QString::number(planeCalib[i*3+2], 'f', 4));
|
||
}
|
||
|
||
resultText += QString("逆旋转矩阵:\n");
|
||
for (int i = 0; i < 3; i++) {
|
||
resultText += QString("[%1, %2, %3]\n")
|
||
.arg(QString::number(invRMatrix[i*3], 'f', 4))
|
||
.arg(QString::number(invRMatrix[i*3+1], 'f', 4))
|
||
.arg(QString::number(invRMatrix[i*3+2], 'f', 4));
|
||
}
|
||
|
||
// 将结果显示到界面上
|
||
ui->label_level_result->setText(resultText);
|
||
ui->label_level_result->setAlignment(Qt::AlignLeft | Qt::AlignTop);
|
||
}
|
||
|
||
// 启动相机扫描
|
||
bool DialogCameraLevel::startCameraScan(int cameraIndex)
|
||
{
|
||
if (cameraIndex < 0 || cameraIndex >= m_cameraList.size()) {
|
||
LOG_ERROR("Invalid camera index for scan: %d\n", cameraIndex);
|
||
return false;
|
||
}
|
||
|
||
IVrEyeDevice* camera = m_cameraList[cameraIndex].second;
|
||
if (!camera) {
|
||
LOG_ERROR("Camera device is null at index: %d\n", cameraIndex);
|
||
return false;
|
||
}
|
||
|
||
// 启动相机检测，使用静态回调函数
|
||
int result = camera->StartDetect(&DialogCameraLevel::StaticDetectionCallback, keResultDataType_Position, this);
|
||
if (result != 0) {
|
||
LOG_ERROR("Failed to start camera detection: %d\n", result);
|
||
return false;
|
||
}
|
||
|
||
LOG_INFO("Camera scan started successfully for camera index: %d\n", cameraIndex);
|
||
return true;
|
||
}
|
||
|
||
// 停止相机扫描
|
||
bool DialogCameraLevel::stopCameraScan(int cameraIndex)
|
||
{
|
||
if (cameraIndex < 0 || cameraIndex >= m_cameraList.size()) {
|
||
LOG_ERROR("Invalid camera index for stop scan: %d\n", cameraIndex);
|
||
return false;
|
||
}
|
||
|
||
IVrEyeDevice* camera = m_cameraList[cameraIndex].second;
|
||
if (!camera) {
|
||
LOG_ERROR("Camera device is null at index: %d\n", cameraIndex);
|
||
return false;
|
||
}
|
||
|
||
int result = camera->StopDetect();
|
||
if (result != 0) {
|
||
LOG_WARNING("Failed to stop camera detection, error: %d\n", result);
|
||
return false;
|
||
}
|
||
|
||
LOG_INFO("Camera scan stopped successfully for camera index: %d\n", cameraIndex);
|
||
return true;
|
||
}
|
||
|
||
// 静态检测回调函数
|
||
void DialogCameraLevel::StaticDetectionCallback(EVzResultDataType eDataType, SVzLaserLineData* pLaserLinePoint, void* pUserData)
|
||
{
|
||
DialogCameraLevel* pThis = reinterpret_cast<DialogCameraLevel*>(pUserData);
|
||
if (pThis && pLaserLinePoint) {
|
||
pThis->DetectionCallback(eDataType, pLaserLinePoint);
|
||
}
|
||
}
|
||
|
||
// 静态状态回调函数
|
||
void DialogCameraLevel::StaticStatusCallback(EVzDeviceWorkStatus eStatus, void* pExtData, unsigned int nDataLength, void* pInfoParam)
|
||
{
|
||
DialogCameraLevel* pThis = reinterpret_cast<DialogCameraLevel*>(pInfoParam);
|
||
if (pThis) {
|
||
pThis->StatusCallback(eStatus, pExtData, nDataLength, pInfoParam);
|
||
}
|
||
}
|
||
|
||
// 状态回调函数实例版本
|
||
void DialogCameraLevel::StatusCallback(EVzDeviceWorkStatus eStatus, void* pExtData, unsigned int nDataLength, void* pInfoParam)
|
||
{
|
||
LOG_DEBUG("[Leveling Status Callback] received: status=%d\n", (int)eStatus);
|
||
|
||
switch (eStatus) {
|
||
case EVzDeviceWorkStatus::keDeviceWorkStatus_Device_Swing_Finish:
|
||
{
|
||
LOG_INFO("[Leveling Status Callback] Camera swing finished, scan completed\n");
|
||
m_swingFinished = true; // 摆动完成即表示扫描完成
|
||
break;
|
||
}
|
||
default:
|
||
LOG_DEBUG("[Leveling Status Callback] Other status: %d\n", (int)eStatus);
|
||
break;
|
||
}
|
||
}
|
||
|
||
// 检测数据回调函数实例版本
|
||
void DialogCameraLevel::DetectionCallback(EVzResultDataType eDataType, SVzLaserLineData* pLaserLinePoint)
|
||
{
|
||
if (!pLaserLinePoint) {
|
||
LOG_WARNING("[Leveling Callback] pLaserLinePoint is null\n");
|
||
return;
|
||
}
|
||
|
||
if (pLaserLinePoint->nPointCount <= 0) {
|
||
LOG_WARNING("[Leveling Callback] Point count is zero or negative: %d\n", pLaserLinePoint->nPointCount);
|
||
return;
|
||
}
|
||
|
||
if (!pLaserLinePoint->p3DPoint) {
|
||
LOG_WARNING("[Leveling Callback] p3DPoint is null\n");
|
||
return;
|
||
}
|
||
|
||
// 使用与WorkpiecePresenter相同的方式存储数据
|
||
SVzLaserLineData lineData;
|
||
memset(&lineData, 0, sizeof(SVzLaserLineData));
|
||
|
||
// 根据数据类型分配和复制点云数据
|
||
if (eDataType == keResultDataType_Position) {
|
||
// 复制SVzNL3DPosition数据
|
||
if (pLaserLinePoint->p3DPoint && pLaserLinePoint->nPointCount > 0) {
|
||
lineData.p3DPoint = new SVzNL3DPosition[pLaserLinePoint->nPointCount];
|
||
if (lineData.p3DPoint) {
|
||
memcpy(lineData.p3DPoint, pLaserLinePoint->p3DPoint, sizeof(SVzNL3DPosition) * pLaserLinePoint->nPointCount);
|
||
}
|
||
lineData.p2DPoint = new SVzNL2DPosition[pLaserLinePoint->nPointCount];
|
||
if (lineData.p2DPoint) {
|
||
memcpy(lineData.p2DPoint, pLaserLinePoint->p2DPoint, sizeof(SVzNL2DPosition) * pLaserLinePoint->nPointCount);
|
||
}
|
||
}
|
||
} else if (eDataType == keResultDataType_PointXYZRGBA) {
|
||
// 复制SVzNLPointXYZRGBA数据
|
||
if (pLaserLinePoint->p3DPoint && pLaserLinePoint->nPointCount > 0) {
|
||
lineData.p3DPoint = new SVzNLPointXYZRGBA[pLaserLinePoint->nPointCount];
|
||
if (lineData.p3DPoint) {
|
||
memcpy(lineData.p3DPoint, pLaserLinePoint->p3DPoint, sizeof(SVzNLPointXYZRGBA) * pLaserLinePoint->nPointCount);
|
||
}
|
||
lineData.p2DPoint = new SVzNL2DLRPoint[pLaserLinePoint->nPointCount];
|
||
if (lineData.p2DPoint) {
|
||
memcpy(lineData.p2DPoint, pLaserLinePoint->p2DPoint, sizeof(SVzNL2DLRPoint) * pLaserLinePoint->nPointCount);
|
||
}
|
||
}
|
||
}
|
||
|
||
lineData.nPointCount = pLaserLinePoint->nPointCount;
|
||
lineData.llTimeStamp = pLaserLinePoint->llTimeStamp;
|
||
lineData.llFrameIdx = pLaserLinePoint->llFrameIdx;
|
||
lineData.nEncodeNo = pLaserLinePoint->nEncodeNo;
|
||
lineData.fSwingAngle = pLaserLinePoint->fSwingAngle;
|
||
lineData.bEndOnceScan = pLaserLinePoint->bEndOnceScan;
|
||
|
||
// 将转换后的数据保存到缓存中
|
||
std::lock_guard<std::mutex> lock(m_scanDataMutex);
|
||
m_scanDataCache.push_back(std::make_pair(eDataType, lineData));
|
||
}
|
||
|
||
// 调平算法计算
|
||
bool DialogCameraLevel::calculatePlaneCalibration(double planeCalib[9], double& planeHeight, double invRMatrix[9])
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_scanDataMutex);
|
||
// 检查是否有足够的扫描数据
|
||
if (m_scanDataCache.empty()) {
|
||
LOG_ERROR("No scan data available for plane calibration\n");
|
||
return false;
|
||
}
|
||
|
||
LOG_INFO("Calculating plane calibration from %zu scan lines\n", m_scanDataCache.size());
|
||
|
||
try {
|
||
// 将SVzLaserLineData转换为SVzNL3DLaserLine格式（算法接口需要）
|
||
// 转换为算法需要的XYZ格式
|
||
std::vector<std::vector<SVzNL3DPosition>> xyzData;
|
||
int convertResult = m_dataLoader.ConvertToSVzNL3DPosition(m_scanDataCache, xyzData);
|
||
if (convertResult != SUCCESS || xyzData.empty()) {
|
||
LOG_WARNING("Failed to convert data to XYZ format or no XYZ data available\n");
|
||
return false;
|
||
}
|
||
|
||
// 调用实际的调平算法
|
||
SSG_planeCalibPara calibResult = sx_BQ_getBaseCalibPara(xyzData);
|
||
|
||
// 将结构体中的数据复制到输出参数
|
||
for (int i = 0; i < 9; i++) {
|
||
planeCalib[i] = calibResult.planeCalib[i];
|
||
invRMatrix[i] = calibResult.invRMatrix[i];
|
||
}
|
||
planeHeight = calibResult.planeHeight;
|
||
|
||
// 计算旋转角度用于日志显示
|
||
double rotAngleX = atan2(planeCalib[5], planeCalib[8]) * 180.0 / M_PI;
|
||
double rotAngleY = atan2(-planeCalib[2], sqrt(planeCalib[5]*planeCalib[5] + planeCalib[8]*planeCalib[8])) * 180.0 / M_PI;
|
||
|
||
LOG_INFO("Plane calibration calculated: height=%.3f, rotX=%.2f°, rotY=%.2f°\n", planeHeight, rotAngleX, rotAngleY);
|
||
|
||
#ifdef LEVEL_DEBUG_MODE
|
||
// 统计点云数据
|
||
int totalPoints = 0;
|
||
double avgHeight = 0.0;
|
||
for (const auto& dataPair : m_scanDataCache) {
|
||
const SVzLaserLineData& lineData = dataPair.second;
|
||
if (dataPair.first == keResultDataType_Position && lineData.p3DPoint) {
|
||
SVzNL3DPosition* positions = static_cast<SVzNL3DPosition*>(lineData.p3DPoint);
|
||
for (int i = 0; i < lineData.nPointCount; i++) {
|
||
if (positions[i].pt3D.z > -1000 && positions[i].pt3D.z < 1000) {
|
||
avgHeight += positions[i].pt3D.z;
|
||
totalPoints++;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
LOG_INFO("=== DEBUG MODE CALIBRATION RESULTS ===\n");
|
||
|
||
LOG_INFO("Algorithm results:\n");
|
||
LOG_INFO(" Calculated plane height: %.3f mm\n", calibResult.planeHeight);
|
||
LOG_INFO("Calibration matrix:\n");
|
||
for (int i = 0; i < 3; i++) {
|
||
LOG_INFO(" [%.6f, %.6f, %.6f]\n", calibResult.planeCalib[i*3], calibResult.planeCalib[i*3+1], calibResult.planeCalib[i*3+2]);
|
||
}
|
||
LOG_INFO("Inverse rotation matrix:\n");
|
||
for (int i = 0; i < 3; i++) {
|
||
LOG_INFO(" [%.6f, %.6f, %.6f]\n", calibResult.invRMatrix[i*3], calibResult.invRMatrix[i*3+1], calibResult.invRMatrix[i*3+2]);
|
||
}
|
||
LOG_INFO("=======================================\n");
|
||
#endif
|
||
|
||
return true;
|
||
|
||
} catch (const std::exception& e) {
|
||
LOG_ERROR("Exception in sx_BQ_getBaseCalibPara: %s\n", e.what());
|
||
return false;
|
||
} catch (...) {
|
||
LOG_ERROR("Unknown exception in sx_BQ_getBaseCalibPara\n");
|
||
return false;
|
||
}
|
||
}
|
||
|
||
// 清空扫描数据缓存
|
||
void DialogCameraLevel::clearScanDataCache()
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_scanDataMutex);
|
||
|
||
LOG_DEBUG("Clearing scan data cache, current size: %zu\n", m_scanDataCache.size());
|
||
|
||
// 使用LaserDataLoader释放缓存的内存
|
||
m_dataLoader.FreeLaserScanData(m_scanDataCache);
|
||
|
||
LOG_DEBUG("Scan data cache cleared successfully\n");
|
||
}
|
||
|
||
bool DialogCameraLevel::saveLevelingResults(double planeCalib[9], double planeHeight, double invRMatrix[9], int cameraIndex, const QString& cameraName)
|
||
{
|
||
try {
|
||
if (!m_presenter) {
|
||
LOG_ERROR("Presenter is null, cannot save leveling results\n");
|
||
return false;
|
||
}
|
||
|
||
// 获取配置对象
|
||
IVrConfig* config = m_presenter->GetConfig();
|
||
if (!config) {
|
||
LOG_ERROR("Config is null, cannot save leveling results\n");
|
||
return false;
|
||
}
|
||
|
||
// 验证传入的相机参数
|
||
if (cameraIndex <= 0) {
|
||
LOG_ERROR("Invalid camera index: %d\n", cameraIndex);
|
||
return false;
|
||
}
|
||
|
||
if (cameraName.isEmpty()) {
|
||
LOG_ERROR("Camera name is empty\n");
|
||
return false;
|
||
}
|
||
|
||
// 加载当前配置
|
||
QString configPath = PathManager::GetInstance().GetConfigFilePath();
|
||
LOG_INFO("Config path: %s\n", configPath.toUtf8().constData());
|
||
ConfigResult configResult;
|
||
int ret = config->LoadConfig(configPath.toStdString(), configResult);
|
||
if (ret != LOAD_CONFIG_SUCCESS) {
|
||
LOG_ERROR("Failed to load config file, error code: %d\n", ret);
|
||
return false;
|
||
}
|
||
|
||
// 创建或更新指定相机的调平参数
|
||
VrCameraPlaneCalibParam cameraParam;
|
||
cameraParam.cameraIndex = cameraIndex;
|
||
cameraParam.cameraName = cameraName.toStdString();
|
||
cameraParam.planeHeight = planeHeight;
|
||
cameraParam.isCalibrated = true;
|
||
|
||
// 复制校准矩阵
|
||
for (int i = 0; i < 9; i++) {
|
||
cameraParam.planeCalib[i] = planeCalib[i];
|
||
cameraParam.invRMatrix[i] = invRMatrix[i];
|
||
}
|
||
|
||
// 更新配置中的相机校准参数
|
||
configResult.algorithmParams.planeCalibParam.SetCameraCalibParam(cameraParam);
|
||
|
||
// 保存配置
|
||
bool saveResult = config->SaveConfig(configPath.toStdString(), configResult);
|
||
if (!saveResult) {
|
||
LOG_ERROR("Failed to save config with leveling results\n");
|
||
return false;
|
||
}
|
||
|
||
LOG_INFO("Leveling results saved successfully for camera %d (%s)\n", cameraIndex, cameraName.toUtf8().constData());
|
||
LOG_INFO("Plane height: %.3f\n", planeHeight);
|
||
LOG_INFO("Calibration marked as completed\n");
|
||
|
||
return true;
|
||
|
||
} catch (const std::exception& e) {
|
||
LOG_ERROR("Exception in saveLevelingResults: %s\n", e.what());
|
||
return false;
|
||
}
|
||
}
|
||
|
||
// 选择debug数据文件
|
||
QString DialogCameraLevel::selectDebugDataFile()
|
||
{
|
||
// 获取应用程序目录作为起始目录
|
||
QString appDir = QCoreApplication::applicationDirPath();
|
||
QString testDataDir = appDir + "/TestData";
|
||
|
||
// 如果TestData目录不存在，使用应用程序目录
|
||
if (!QFileInfo::exists(testDataDir)) {
|
||
testDataDir = appDir;
|
||
}
|
||
|
||
// 打开文件选择对话框
|
||
QString fileName = QFileDialog::getOpenFileName(
|
||
this,
|
||
"选择激光扫描数据文件",
|
||
testDataDir,
|
||
"文本文件 (*.txt);;所有文件 (*.*)"
|
||
);
|
||
|
||
if (fileName.isEmpty()) {
|
||
LOG_INFO("No debug data file selected\n");
|
||
return "";
|
||
}
|
||
|
||
// 检查文件是否存在
|
||
if (!QFileInfo::exists(fileName)) {
|
||
LOG_ERROR("Selected debug data file does not exist: %s\n", fileName.toUtf8().constData());
|
||
QMessageBox::warning(this, "文件错误", "选择的文件不存在！");
|
||
return "";
|
||
}
|
||
|
||
LOG_INFO("Selected debug data file: %s\n", fileName.toUtf8().constData());
|
||
|
||
return fileName;
|
||
}
|
||
|
||
// 加载debug数据并模拟扫描过程
|
||
bool DialogCameraLevel::loadDebugDataAndSimulateScan(const QString& filePath)
|
||
{
|
||
LOG_INFO("Loading debug data from: %s\n", filePath.toUtf8().constData());
|
||
|
||
try {
|
||
// 清空之前的扫描数据
|
||
clearScanDataCache();
|
||
|
||
// 使用LaserDataLoader加载测试数据
|
||
int lineNum = 0;
|
||
float scanSpeed = 0.0f;
|
||
int maxTimeStamp = 0;
|
||
int clockPerSecond = 0;
|
||
|
||
int result = m_dataLoader.LoadLaserScanData(filePath.toStdString(), m_scanDataCache,
|
||
lineNum, scanSpeed, maxTimeStamp, clockPerSecond);
|
||
|
||
if (result != 0) {
|
||
LOG_ERROR("Failed to load debug data: %s\n", m_dataLoader.GetLastError().c_str());
|
||
QMessageBox::critical(this, "Debug模式错误",
|
||
QString("加载测试数据失败：\n%1").arg(QString::fromStdString(m_dataLoader.GetLastError())));
|
||
return false;
|
||
}
|
||
|
||
if (m_scanDataCache.empty()) {
|
||
LOG_ERROR("Debug data is empty\n");
|
||
QMessageBox::warning(this, "Debug模式警告", "测试数据文件为空！");
|
||
return false;
|
||
}
|
||
|
||
LOG_INFO("Loaded %d lines of debug data, starting simulation\n", lineNum);
|
||
|
||
// 数据已经直接加载到m_scanDataCache中，无需额外转换
|
||
|
||
// 启动模拟扫描过程
|
||
simulateScanProcess();
|
||
|
||
return true;
|
||
|
||
} catch (const std::exception& e) {
|
||
LOG_ERROR("Exception in loadDebugDataAndSimulateScan: %s\n", e.what());
|
||
QMessageBox::critical(this, "Debug模式异常", QString("加载debug数据时发生异常：\n%1").arg(e.what()));
|
||
return false;
|
||
}
|
||
}
|
||
|
||
// 模拟扫描过程
|
||
void DialogCameraLevel::simulateScanProcess()
|
||
{
|
||
LOG_INFO("Starting scan simulation process\n");
|
||
|
||
// 创建定时器来模拟数据收集过程
|
||
QTimer* simulationTimer = new QTimer();
|
||
simulationTimer->setSingleShot(true);
|
||
|
||
// 模拟扫描需要的时间（2秒）
|
||
simulationTimer->setInterval(2000);
|
||
|
||
// 连接定时器信号
|
||
connect(simulationTimer, &QTimer::timeout, [this, simulationTimer]() {
|
||
LOG_INFO("Scan simulation completed\n");
|
||
m_swingFinished = true; // Debug模式下模拟摆动完成
|
||
|
||
// 显示调试信息
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_scanDataMutex);
|
||
LOG_INFO("Debug scan simulation completed with %zu lines\n", m_scanDataCache.size());
|
||
|
||
// 统计点云数据
|
||
int totalPoints = 0;
|
||
for (const auto& dataPair : m_scanDataCache) {
|
||
totalPoints += dataPair.second.nPointCount;
|
||
}
|
||
LOG_INFO("Total points in debug data: %d\n", totalPoints);
|
||
}
|
||
|
||
// 清理定时器
|
||
simulationTimer->deleteLater();
|
||
});
|
||
|
||
// 启动模拟扫描
|
||
simulationTimer->start();
|
||
|
||
LOG_INFO("Simulation timer started, waiting for completion...\n");
|
||
}
|
||
|
||
// 相机选择改变的槽函数
|
||
void DialogCameraLevel::on_combo_camera_currentIndexChanged(int index)
|
||
{
|
||
if (index >= 0 && index < m_cameraList.size()) {
|
||
LOG_INFO("Camera selection changed to index: %d (%s)\n", index,
|
||
QString::fromStdString(m_cameraList[index].first).toUtf8().constData());
|
||
checkAndDisplayCalibrationStatus(index);
|
||
} else {
|
||
LOG_WARNING("Invalid camera index selected: %d\n", index);
|
||
ui->label_level_result->setText("无效的相机选择");
|
||
ui->label_level_result->setAlignment(Qt::AlignCenter);
|
||
}
|
||
}
|
||
|
||
// 加载相机标定数据
|
||
bool DialogCameraLevel::loadCameraCalibrationData(int cameraIndex, const QString& cameraName,
|
||
double planeCalib[9], double& planeHeight, double invRMatrix[9])
|
||
{
|
||
try {
|
||
if (!m_presenter) {
|
||
LOG_ERROR("Presenter is null, cannot load calibration data\n");
|
||
return false;
|
||
}
|
||
|
||
// 获取配置对象
|
||
IVrConfig* config = m_presenter->GetConfig();
|
||
if (!config) {
|
||
LOG_ERROR("Config is null, cannot load calibration data\n");
|
||
return false;
|
||
}
|
||
|
||
// 加载配置文件
|
||
QString configPath = PathManager::GetInstance().GetConfigFilePath();
|
||
ConfigResult configResult;
|
||
int ret = config->LoadConfig(configPath.toStdString(), configResult);
|
||
if (ret != LOAD_CONFIG_SUCCESS) {
|
||
LOG_ERROR("Failed to load config file, error code: %d\n", ret);
|
||
return false;
|
||
}
|
||
|
||
// 获取指定相机的标定参数
|
||
VrCameraPlaneCalibParam cameraParamValue;
|
||
if (!configResult.algorithmParams.planeCalibParam.GetCameraCalibParam(cameraIndex, cameraParamValue) || !cameraParamValue.isCalibrated) {
|
||
LOG_INFO("No calibration data found for camera %d (%s)\n", cameraIndex, cameraName.toUtf8().constData());
|
||
return false;
|
||
}
|
||
|
||
// 复制标定数据
|
||
for (int i = 0; i < 9; i++) {
|
||
planeCalib[i] = cameraParamValue.planeCalib[i];
|
||
invRMatrix[i] = cameraParamValue.invRMatrix[i];
|
||
}
|
||
planeHeight = cameraParamValue.planeHeight;
|
||
|
||
LOG_INFO("Calibration data loaded successfully for camera %d (%s)\n", cameraIndex, cameraName.toUtf8().constData());
|
||
LOG_INFO("Plane height: %.3f\n", planeHeight);
|
||
|
||
return true;
|
||
|
||
} catch (const std::exception& e) {
|
||
LOG_ERROR("Exception in loadCameraCalibrationData: %s\n", e.what());
|
||
return false;
|
||
}
|
||
}
|
||
|
||
// 检查并显示相机标定状态
|
||
void DialogCameraLevel::checkAndDisplayCalibrationStatus(int cameraIndex)
|
||
{
|
||
if (cameraIndex < 0 || cameraIndex >= m_cameraList.size()) {
|
||
LOG_WARNING("Invalid camera index for status check: %d\n", cameraIndex);
|
||
ui->label_level_result->setText("无效的相机索引");
|
||
ui->label_level_result->setAlignment(Qt::AlignCenter);
|
||
return;
|
||
}
|
||
|
||
QString cameraName = QString::fromStdString(m_cameraList[cameraIndex].first);
|
||
int configCameraIndex = cameraIndex + 1; // 转换为1-based索引
|
||
|
||
// 尝试加载该相机的标定数据
|
||
double planeCalib[9];
|
||
double planeHeight;
|
||
double invRMatrix[9];
|
||
|
||
if (loadCameraCalibrationData(configCameraIndex, cameraName, planeCalib, planeHeight, invRMatrix)) {
|
||
// 有标定数据，显示标定结果
|
||
LOG_INFO("Displaying existing calibration data for camera %s\n", cameraName.toUtf8().constData());
|
||
updateLevelingResults(planeCalib, planeHeight, invRMatrix);
|
||
} else {
|
||
// 没有标定数据，显示提示信息
|
||
LOG_INFO("No calibration data found for camera %s, showing instruction\n", cameraName.toUtf8().constData());
|
||
ui->label_level_result->setText(QString("请选择相机 \"%1\" 并点击调平按钮\n开始相机调平操作").arg(cameraName));
|
||
ui->label_level_result->setAlignment(Qt::AlignCenter);
|
||
}
|
||
}
|
||
|
||
// 设置调平时的状态回调
|
||
void DialogCameraLevel::setLevelingStatusCallback()
|
||
{
|
||
if (!m_presenter) {
|
||
LOG_ERROR("Presenter is null, cannot set leveling status callback\n");
|
||
return;
|
||
}
|
||
|
||
// 为所有相机设置调平状态回调
|
||
m_presenter->SetCameraStatusCallback(&DialogCameraLevel::StaticStatusCallback, this);
|
||
|
||
// 重置状态标志
|
||
m_swingFinished = false;
|
||
m_callbackRestored = false; // 重置恢复标志，允许稍后恢复
|
||
|
||
LOG_INFO("Leveling status callback set for all cameras\n");
|
||
}
|
||
|
||
// 恢复Presenter的状态回调
|
||
void DialogCameraLevel::restorePresenterStatusCallback()
|
||
{
|
||
// 检查是否已经恢复过回调，避免重复调用
|
||
if (m_callbackRestored.exchange(true)) {
|
||
LOG_DEBUG("Presenter status callback already restored, skipping\n");
|
||
return;
|
||
}
|
||
|
||
if (!m_presenter) {
|
||
LOG_ERROR("Presenter is null, cannot restore status callback\n");
|
||
return;
|
||
}
|
||
|
||
// 恢复Presenter的状态回调 - 使用BasePresenter的通用静态回调
|
||
m_presenter->SetCameraStatusCallback(&BasePresenter::_StaticCameraStatusCallback, m_presenter);
|
||
|
||
LOG_INFO("Presenter status callback restored for all cameras\n");
|
||
}
|
||
|