diff --git a/App/WheelMeasure/Doc/TCP_Protocol.md b/App/WheelMeasure/Doc/TCP_Protocol.md new file mode 100644 index 00000000..00e7ee37 --- /dev/null +++ b/App/WheelMeasure/Doc/TCP_Protocol.md @@ -0,0 +1,131 @@ +# 轮眉测量 TCP/IP 通信协议 + +## 概述 + +- **协议类型**: TCP/IP 文本协议 +- **默认端口**: 5000（可在配置文件中修改） +- **数据格式**: 纯文本，UTF-8编码 +- **通信模式**: 客户端-服务器模式（视觉系统作为服务器） + +## 协议格式 + +### 1. 触发检测命令（客户端 → 服务器） + +``` +start,100 +``` + +**格式说明**： +- `start`: 命令关键字 +- `100`: 参数（当前未使用，保留用于扩展） + +**示例**： +``` +start,100 +``` + +### 2. 检测结果响应（服务器 → 客户端） + +#### 成功情况（所有相机检测成功） + +``` +1,100,200;2,100,200;3,100,200;4,100,200 +``` + +**格式说明**： +- 多个相机结果用分号 `;` 分隔 +- 每个相机结果格式：`相机ID,中心点到地面距离,轮眉到地面距离` + - `相机ID`: 1-4（对应4个相机） + - `中心点到地面距离`: 整数，单位mm + - `轮眉到地面距离`: 整数，单位mm + +**示例**： +``` +1,150,250;2,148,252;3,151,249;4,149,251 +``` +表示： +- 相机1：中心距离150mm，轮眉距离250mm +- 相机2：中心距离148mm，轮眉距离252mm +- 相机3：中心距离151mm，轮眉距离249mm +- 相机4：中心距离149mm，轮眉距离251mm + +#### 失败情况（部分相机检测失败） + +``` +1,400;2,100,200;3,100,200;4,100,200 +``` + +**错误码说明**： +- `400`: 扫描/匹配失败 +- `401`: 工件为空 + +**格式说明**： +- 失败的相机只返回：`相机ID,错误码` +- 成功的相机返回：`相机ID,中心距离,轮眉距离` + +**示例**： +``` +1,400;2,150,250;3,401;4,149,251 +``` +表示： +- 相机1：扫描失败（错误码400） +- 相机2：成功，中心距离150mm，轮眉距离250mm +- 相机3：工件为空（错误码401） +- 相机4：成功，中心距离149mm，轮眉距离251mm + +## 通信流程 + +``` +客户端 服务器（视觉系统） + | | + |-------- start,100 ------------------------>| + | | + | | 触发所有相机顺序检测 + | | （相机1 → 相机2 → 相机3 → 相机4） + | | + |<------- 1,150,250;2,148,252;... -----------| + | | +``` + +**时序说明**： +1. 客户端发送 `start,100` 命令 +2. 服务器收到命令后，依次触发所有启用的相机进行检测 +3. 所有相机检测完成后，服务器发送汇总结果 +4. 客户端接收结果并处理 + +## 错误处理 + +### 连接错误 +- 如果TCP连接断开，服务器会清除当前客户端的请求状态 +- 客户端需要重新连接并发送命令 + +### 超时处理 +- 建议客户端设置接收超时（推荐60秒） +- 如果超时未收到响应，可以重新发送命令 + +### 数据解析 +- 客户端应验证返回数据格式 +- 检查相机ID是否在1-4范围内 +- 检查是否包含错误码（400/401） + +## 配置说明 + +TCP服务器端口可在配置文件中修改： + +```xml + + + TCPServer + 0.0.0.0 + 5000 + + +``` + +## 注意事项 + +1. **数据单位**：所有距离值单位为毫米（mm） +2. **相机顺序**：相机ID固定为1-4，对应配置文件中的相机顺序 +3. **检测时间**：完整检测所有相机通常需要30-50秒，请设置合理的超时时间 +4. **并发限制**：服务器同时只处理一个检测请求，多个客户端连接时按先后顺序处理 +5. **数据精度**：返回的距离值已四舍五入为整数 diff --git a/App/WheelMeasure/WheelMeasureApp/Presenter/Src/WheelMeasurePresenter.cpp b/App/WheelMeasure/WheelMeasureApp/Presenter/Src/WheelMeasurePresenter.cpp index fd90298f..29aeff71 100644 --- a/App/WheelMeasure/WheelMeasureApp/Presenter/Src/WheelMeasurePresenter.cpp +++ b/App/WheelMeasure/WheelMeasureApp/Presenter/Src/WheelMeasurePresenter.cpp @@ -73,8 +73,11 @@ int WheelMeasurePresenter::InitApp() } } - // 初始化TCP服务器 - int tcpPort = 6800; // 默认端口 + // 初始化TCP服务器（从配置读取端口） + int tcpPort = 5000; // 默认端口 + if (!m_configResult.servers.empty()) { + tcpPort = m_configResult.servers[0].port; + } int tcpResult = m_tcpProtocol.Initialize(tcpPort); if (tcpResult != 0) { LOG_ERROR("Failed to initialize TCP server on port %d, error code: %d\n", tcpPort, tcpResult); @@ -306,6 +309,9 @@ void WheelMeasurePresenter::StartAllDetection() LOG_INFO("Sequential detection started, total cameras: %d\n", m_sequentialTotalCount); + + m_statusUpdate->OnStatusUpdate(QString("开始所有设备的检测")); + // 开始检测第一个设备 continueSequentialDetection(); } @@ -517,7 +523,7 @@ void WheelMeasurePresenter::processScanData(std::vectorwheelRoi3d_xMin; + wheelRoi3d.xRange.max = calibParam->wheelRoi3d_xMax; + wheelRoi3d.yRange.min = calibParam->wheelRoi3d_yMin; + wheelRoi3d.yRange.max = calibParam->wheelRoi3d_yMax; + wheelRoi3d.zRange.min = calibParam->wheelRoi3d_zMin; + wheelRoi3d.zRange.max = calibParam->wheelRoi3d_zMax; + + LOG_INFO("Using ROI range for wheel presence detection:\n"); + LOG_INFO(" X: [%.1f, %.1f], Y: [%.1f, %.1f], Z: [%.1f, %.1f]\n", + wheelRoi3d.xRange.min, wheelRoi3d.xRange.max, + wheelRoi3d.yRange.min, wheelRoi3d.yRange.max, + wheelRoi3d.zRange.min, wheelRoi3d.zRange.max); + } else { + // 使用默认ROI范围 + wheelRoi3d.xRange.min = -1000.0; + wheelRoi3d.xRange.max = 1000.0; + wheelRoi3d.yRange.min = -1000.0; + wheelRoi3d.yRange.max = 1000.0; + wheelRoi3d.zRange.min = -1000.0; + wheelRoi3d.zRange.max = 1000.0; + + LOG_INFO("Using default ROI range for wheel presence detection: ±1000.0 mm\n"); + } + + // 调用轮胎存在检测 + bool wheelPresent = wd_wheelPresenseDetection(scanLines, wheelRoi3d); + LOG_INFO("Wheel presence detection result: %s\n", wheelPresent ? "PRESENT" : "NOT PRESENT"); + + if (!wheelPresent) { + LOG_WARNING("No wheel detected in ROI range, skipping measurement\n"); + if (m_statusUpdate) { + m_statusUpdate->OnStatusUpdate(QString("未检测到轮胎，请检查ROI范围配置")); + } + + // 如果是TCP触发的检测，缓存401错误结果 + if (m_tcpDetectionMode) { + WheelMeasureTCPProtocol::CameraMeasureResult tcpResult; + tcpResult.cameraId = m_currentCameraIndex; + tcpResult.errorCode = 401; // 工件为空 + tcpResult.centerDistance = 0.0; + tcpResult.archDistance = 0.0; + m_tcpResults[m_currentCameraIndex] = tcpResult; + LOG_INFO("TCP检测结果已缓存: 相机%d, 错误码=401 (未检测到轮胎)\n", m_currentCameraIndex); + } + + // 如果正在进行顺序检测，继续检测下一个设备 + if (m_sequentialDetecting) { + m_sequentialCurrentIndex++; + + // TCP模式下，检查是否所有相机都检测完成 + if (m_tcpDetectionMode && m_tcpResults.size() >= m_sequentialTotalCount) { + LOG_INFO("TCP模式：所有相机检测完成，准备发送结果\n"); + m_sequentialDetecting = false; + sendTCPMeasureResults(); + } else { + // 继续检测下一个设备 + QMetaObject::invokeMethod(this, [this]() { + continueSequentialDetection(); + }, Qt::QueuedConnection); + } + } + + return; + } + + // 4. 准备调平参数并执行调平处理（使用当前相机的调平参数） SSG_planeCalibPara groundCalibPara; memset(&groundCalibPara, 0, sizeof(groundCalibPara)); @@ -544,8 +622,6 @@ void WheelMeasurePresenter::processScanData(std::vectorisCalibrated) { for (int i = 0; i < 9; ++i) { groundCalibPara.planeCalib[i] = calibParam->planeCalib[i]; @@ -568,8 +644,7 @@ void WheelMeasurePresenter::processScanData(std::vector= m_sequentialTotalCount) { + if (m_tcpDetectionMode && m_tcpResults.size() >= m_sequentialTotalCount) { + LOG_INFO("TCP模式：所有相机检测完成，准备发送结果\n"); + // 先结束顺序检测 + m_sequentialDetecting = false; + // 发送TCP结果 sendTCPMeasureResults(); + } else { + // 继续检测下一个设备 + QMetaObject::invokeMethod(this, [this]() { + continueSequentialDetection(); + }, Qt::QueuedConnection); } } } diff --git a/App/WheelMeasure/WheelMeasureApp/Version.h b/App/WheelMeasure/WheelMeasureApp/Version.h index a8fe569a..4b268e0b 100644 --- a/App/WheelMeasure/WheelMeasureApp/Version.h +++ b/App/WheelMeasure/WheelMeasureApp/Version.h @@ -1,22 +1,22 @@ -#ifndef VERSION_H -#define VERSION_H - - -#define WHEELMEASURE_VERSION_STRING "1.0.1" -#define WHEELMEASURE_BUILD_STRING "5" -#define WHEELMEASURE_FULL_VERSION_STRING "V" WHEELMEASURE_VERSION_STRING "_" WHEELMEASURE_BUILD_STRING - -// 获取版本信息的便捷函数 -inline const char* GetWheelMeasureVersion() { - return WHEELMEASURE_VERSION_STRING; -} - -inline const char* GetWheelMeasureBuild() { - return WHEELMEASURE_BUILD_STRING; -} - -inline const char* GetWheelMeasureFullVersion() { - return WHEELMEASURE_FULL_VERSION_STRING; -} - -#endif // VERSION_H +#ifndef VERSION_H +#define VERSION_H + + +#define WHEELMEASURE_VERSION_STRING "1.0.1" +#define WHEELMEASURE_BUILD_STRING "6" +#define WHEELMEASURE_FULL_VERSION_STRING "V" WHEELMEASURE_VERSION_STRING "_" WHEELMEASURE_BUILD_STRING + +// 获取版本信息的便捷函数 +inline const char* GetWheelMeasureVersion() { + return WHEELMEASURE_VERSION_STRING; +} + +inline const char* GetWheelMeasureBuild() { + return WHEELMEASURE_BUILD_STRING; +} + +inline const char* GetWheelMeasureFullVersion() { + return WHEELMEASURE_FULL_VERSION_STRING; +} + +#endif // VERSION_H diff --git a/App/WheelMeasure/WheelMeasureApp/WheelMeasureApp.pro b/App/WheelMeasure/WheelMeasureApp/WheelMeasureApp.pro index 2ce6eb59..e4f914b7 100644 --- a/App/WheelMeasure/WheelMeasureApp/WheelMeasureApp.pro +++ b/App/WheelMeasure/WheelMeasureApp/WheelMeasureApp.pro @@ -27,6 +27,7 @@ INCLUDEPATH += ../../../Utils/VrCommon/Inc INCLUDEPATH += ../../../AppUtils/UICommon/Inc INCLUDEPATH += ../../../Utils/CloudUtils/Inc INCLUDEPATH += ../../../AppUtils/AppCommon/Inc +INCLUDEPATH += ../../../Module/AuthModule/Inc INCLUDEPATH += ../../../SDK/Device/VzNLSDK/Inc INCLUDEPATH += ../../../AppAlgo/wheelArchHeigthMeasure/Inc @@ -38,8 +39,9 @@ win32:CONFIG(debug, debug|release) { LIBS += -L../../../Utils/VrUtils/debug -lVrUtils LIBS += -L../../../Device/VrEyeDevice/debug -lVrEyeDevice LIBS += -L../../../AppUtils/UICommon/debug -lUICommon - LIBS += -L../../../Utils/CloudUtils/debug -lCloudUtils + LIBS += -L../../../Utils/CloudUtils/debug -lCloudUtils LIBS += -L../../../AppUtils/AppCommon/debug -lAppCommon + LIBS += -L../../../Module/AuthModule/debug -lAuthModule LIBS += -L../../../Module/ModbusTCPServer/debug -lModbusTCPServer LIBS += -L../../../VrNets/debug -lVrModbus } else:win32:CONFIG(release, debug|release) { @@ -48,8 +50,9 @@ win32:CONFIG(debug, debug|release) { LIBS += -L../../../Utils/VrUtils/release -lVrUtils LIBS += -L../../../Device/VrEyeDevice/release -lVrEyeDevice LIBS += -L../../../AppUtils/UICommon/release -lUICommon - LIBS += -L../../../Utils/CloudUtils/release -lCloudUtils + LIBS += -L../../../Utils/CloudUtils/release -lCloudUtils LIBS += -L../../../AppUtils/AppCommon/release -lAppCommon + LIBS += -L../../../Module/AuthModule/release -lAuthModule LIBS += -L../../../Module/ModbusTCPServer/release -lModbusTCPServer LIBS += -L../../../VrNets/release -lVrModbus }else:unix:!macx { @@ -57,7 +60,8 @@ win32:CONFIG(debug, debug|release) { LIBS += -L../WheelMeasureConfig -lWheelMeasureConfig LIBS += -L../../../AppUtils/AppCommon -lAppCommon LIBS += -L../../../AppUtils/UICommon -lUICommon - LIBS += -L../../../Utils/CloudUtils -lCloudUtils + LIBS += -L../../../Module/AuthModule -lAuthModule + LIBS += -L../../../Utils/CloudUtils -lCloudUtils LIBS += -L../../../Device/VrEyeDevice -lVrEyeDevice LIBS += -L../../../VrNets -lVrTcpClient -lVrTcpServer LIBS += -L../../../Utils/VrUtils -lVrUtils diff --git a/App/WheelMeasure/WheelMeasureApp/dialogcameralevel.cpp b/App/WheelMeasure/WheelMeasureApp/dialogcameralevel.cpp index a1a79b90..0665a188 100644 --- a/App/WheelMeasure/WheelMeasureApp/dialogcameralevel.cpp +++ b/App/WheelMeasure/WheelMeasureApp/dialogcameralevel.cpp @@ -1,705 +1,833 @@ -#include "dialogcameralevel.h" -#include "ui_dialogcameralevel.h" -#include "WheelMeasurePresenter.h" -#include "PathManager.h" -#include "VrLog.h" -#include "wheelArchHeigthMeasure_Export.h" - -#include -#include -#include -#include -#include -#include - -#ifndef M_PI -#define M_PI 3.14159265358979323846 -#endif - -DialogCameraLevel::DialogCameraLevel(QWidget *parent) - : QDialog(parent) - , ui(new Ui::DialogCameraLevel) - , m_pConfig(nullptr) - , m_pConfigResult(nullptr) - , m_currentCameraIndex(-1) -{ - ui->setupUi(this); - - // 初始化结果显示区域 - 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>& cameraList, - WheelMeasurePresenter* presenter) -{ - m_cameraList = cameraList; - m_presenter = presenter; - - LOG_INFO("setCameraList called with %zu cameras\n", cameraList.size()); - - // 详细记录每个相机的信息 - for (size_t i = 0; i < cameraList.size(); ++i) { - const auto& camera = cameraList[i]; - LOG_INFO(" Camera %zu: name='%s', device=%s\n", - i + 1, - camera.first.c_str(), - (camera.second != nullptr ? "connected" : "not connected")); - } - - // 初始化/重新初始化相机选择框 - initializeCameraCombo(); -} - -void DialogCameraLevel::setConfig(IVrWheelMeasureConfig* config, WheelMeasureConfigResult* configResult) -{ - m_pConfig = config; - m_pConfigResult = configResult; - - // 如果相机已经选择，重新加载当前相机的标定状态 - // 修复：打开页面时配置可能在相机列表之后设置，导致初始加载失败 - if (m_currentCameraIndex >= 0 && m_currentCameraIndex < static_cast(m_cameraList.size())) { - checkAndDisplayCalibrationStatus(m_currentCameraIndex); - } -} - -void DialogCameraLevel::initializeCameraCombo() -{ - LOG_INFO("initializeCameraCombo called, camera list size: %zu\n", m_cameraList.size()); - - ui->combo_camera->clear(); - - if (m_cameraList.empty()) { - ui->combo_camera->setEnabled(false); - - if (!m_presenter) { - ui->label_level_result->setText("Presenter未初始化\n无法获取相机列表"); - LOG_ERROR("Presenter is null in initializeCameraCombo\n"); - } else { - ui->label_level_result->setText("相机列表为空\n\n可能原因：\n1. 配置文件中未配置相机\n2. 系统正在初始化中\n3. 所有相机连接失败"); - LOG_WARNING("Camera list is empty in initializeCameraCombo\n"); - } - - ui->label_level_result->setAlignment(Qt::AlignCenter); - } else { - LOG_INFO("Adding %zu cameras to combo box\n", m_cameraList.size()); - - // 添加所有相机到下拉列表 - for (size_t i = 0; i < m_cameraList.size(); ++i) { - const auto& camera = m_cameraList[i]; - QString cameraName = QString::fromStdString(camera.first); - - // 如果相机没有连接，在名称后添加标记 - if (camera.second == nullptr) { - cameraName += " [未连接]"; - } - - ui->combo_camera->addItem(cameraName); - LOG_INFO(" Added camera %zu: %s (device=%s)\n", - i + 1, - camera.first.c_str(), - (camera.second != nullptr ? "OK" : "NULL")); - } - ui->combo_camera->setEnabled(true); - - // 获取默认相机索引 - int defaultCameraIndex = 0; - if (m_presenter) { - int presenterDefaultIndex = m_presenter->GetDefaultCameraIndex(); - LOG_INFO("Presenter default camera index (1-based): %d\n", presenterDefaultIndex); - - if (presenterDefaultIndex > 0 && presenterDefaultIndex <= static_cast(m_cameraList.size())) { - defaultCameraIndex = presenterDefaultIndex - 1; - } - } - - // 设置默认选中的相机 - if (defaultCameraIndex >= 0 && defaultCameraIndex < static_cast(m_cameraList.size())) { - ui->combo_camera->setCurrentIndex(defaultCameraIndex); - m_currentCameraIndex = defaultCameraIndex; - } - - // 检查并显示当前选中相机的标定状态 - if (m_currentCameraIndex >= 0) { - checkAndDisplayCalibrationStatus(m_currentCameraIndex); - } - } -} - -void DialogCameraLevel::on_btn_apply_clicked() -{ - ui->label_level_result->setAlignment(Qt::AlignLeft); - - // 检查是否有可用的相机 - if (m_cameraList.empty()) { - QMessageBox::warning(this, "错误", "无可用相机设备！"); - return; - } - - // 获取选中的相机 - int selectedIndex = ui->combo_camera->currentIndex(); - if (selectedIndex < 0 || selectedIndex >= static_cast(m_cameraList.size())) { - QMessageBox::warning(this, "错误", "请选择有效的相机！"); - return; - } - - // 清空之前的结果显示 - ui->label_level_result->setText("调平计算中，请稍候..."); - - // 显示进度提示 - ui->btn_apply->setEnabled(false); - QApplication::processEvents(); - - try { - // 执行相机调平 - if (performCameraLeveling()) { - // 调平成功 - } 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(); - - // 先检查索引有效性，再设置回调 - if (selectedIndex < 0 || selectedIndex >= static_cast(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); - - // 1. 设置调平状态回调（在索引检查之后） - setLevelingStatusCallback(); - - // 2. 清空之前的扫描数据 - clearScanDataCache(); - - // 3. 启动相机扫描地面数据 - if (!startCameraScan(selectedIndex)) { - LOG_ERROR("Failed to start camera scan for leveling\n"); - restorePresenterStatusCallback(); // 恢复回调 - return false; - } - - // 4. 等待扫描完成 - LOG_INFO("Collecting ground scan data, waiting for swing finish signal...\n"); - int waitTime = 0; - const int maxWaitTime = 10000; // 最大等待10秒 - const int checkInterval = 100; - - while (!m_swingFinished && waitTime < maxWaitTime) { - QThread::msleep(checkInterval); - QApplication::processEvents(); - 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"); - } - - // 6. 调用调平算法计算 - double planeCalib[9]; - double planeHeight; - double invRMatrix[9]; - - if (!calculatePlaneCalibration(planeCalib, planeHeight, invRMatrix)) { - LOG_ERROR("Failed to calculate plane calibration\n"); - restorePresenterStatusCallback(); // 恢复回调 - return false; - } - - LOG_INFO("Camera leveling calculation completed\n"); - - // 7. 更新界面显示 - updateLevelingResults(planeCalib, planeHeight, invRMatrix); - - // 8. 保存结果到配置 - int cameraIndex = m_currentCameraIndex + 1; // 转换为1-based索引 - QString cameraName; - - if (m_currentCameraIndex >= 0 && m_currentCameraIndex < static_cast(m_cameraList.size())) { - cameraName = QString::fromStdString(m_cameraList[m_currentCameraIndex].first); - } else { - cameraName = QString("Camera_%1").arg(cameraIndex); - } - - if (!saveLevelingResults(planeCalib, planeHeight, invRMatrix, cameraIndex, cameraName)) { - LOG_ERROR("Failed to save leveling results\n"); - restorePresenterStatusCallback(); // 恢复回调 - return false; - } - - clearScanDataCache(); - - // 9. 调平完成后恢复回调 - restorePresenterStatusCallback(); - - LOG_INFO("Camera leveling completed successfully\n"); - - return true; - - } catch (const std::exception& e) { - LOG_ERROR("Exception in performCameraLeveling: %s\n", e.what()); - restorePresenterStatusCallback(); // 异常时也恢复回调 - return false; - } -} - -void DialogCameraLevel::updateLevelingResults(double planeCalib[9], double planeHeight, double invRMatrix[9]) -{ - // 构建显示文本 - QString resultText; - - 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 >= static_cast(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 >= static_cast(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(pUserData); - if (pThis && pLaserLinePoint) { - pThis->DetectionCallback(eDataType, pLaserLinePoint); - } -} - -void DialogCameraLevel::StaticStatusCallback(EVzDeviceWorkStatus eStatus, void* pExtData, unsigned int nDataLength, void* pInfoParam) -{ - DialogCameraLevel* pThis = reinterpret_cast(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; - } - - // 将数据添加到缓存 - std::vector lineData; - lineData.reserve(pLaserLinePoint->nPointCount); - - // p3DPoint 是 void*，需要转换为 SVzNL3DPosition* - SVzNL3DPosition* p3DPoints = reinterpret_cast(pLaserLinePoint->p3DPoint); - for (int i = 0; i < pLaserLinePoint->nPointCount; ++i) { - lineData.push_back(p3DPoints[i]); - } - - std::lock_guard lock(m_scanDataMutex); - m_scanDataCache.push_back(std::move(lineData)); -} - -bool DialogCameraLevel::calculatePlaneCalibration(double planeCalib[9], double& planeHeight, double invRMatrix[9]) -{ - std::lock_guard 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 { - // 调用 wheelArchHeigthMeasure SDK 的调平算法 - SSG_planeCalibPara calibResult = wd_horizonCamera_getGroundCalibPara(m_scanDataCache); - - // 复制调平矩阵 - for (int i = 0; i < 9; i++) { - planeCalib[i] = calibResult.planeCalib[i]; - invRMatrix[i] = calibResult.invRMatrix[i]; - } - planeHeight = calibResult.planeHeight; - - LOG_INFO("Plane calibration calculated successfully\n"); - LOG_INFO(" planeHeight: %.3f\n", planeHeight); - LOG_INFO(" planeCalib: [%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f]\n", - planeCalib[0], planeCalib[1], planeCalib[2], - planeCalib[3], planeCalib[4], planeCalib[5], - planeCalib[6], planeCalib[7], planeCalib[8]); - LOG_INFO(" invRMatrix: [%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f]\n", - invRMatrix[0], invRMatrix[1], invRMatrix[2], - invRMatrix[3], invRMatrix[4], invRMatrix[5], - invRMatrix[6], invRMatrix[7], invRMatrix[8]); - - return true; - - } catch (const std::exception& e) { - LOG_ERROR("Exception in calculatePlaneCalibration: %s\n", e.what()); - return false; - } -} - -void DialogCameraLevel::clearScanDataCache() -{ - std::lock_guard lock(m_scanDataMutex); - - LOG_DEBUG("Clearing scan data cache, current size: %zu\n", m_scanDataCache.size()); - m_scanDataCache.clear(); - 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_pConfig || !m_pConfigResult) { - 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; - } - - // 获取UI中的误差补偿值 - double errorCompensation = ui->edit_error_compensation->text().toDouble(); - - // 创建或更新相机调平参数 - WheelCameraPlaneCalibParam cameraParam; - cameraParam.cameraIndex = cameraIndex; - cameraParam.cameraName = cameraName.toStdString(); - cameraParam.planeHeight = planeHeight; - cameraParam.isCalibrated = true; - cameraParam.errorCompensation = errorCompensation; - - // 复制校准矩阵 - for (int i = 0; i < 9; i++) { - cameraParam.planeCalib[i] = planeCalib[i]; - cameraParam.invRMatrix[i] = invRMatrix[i]; - } - - // 查找是否已有该相机的调平参数 - bool found = false; - for (auto& param : m_pConfigResult->planeCalibParams) { - if (param.cameraIndex == cameraIndex) { - param = cameraParam; - found = true; - break; - } - } - - if (!found) { - m_pConfigResult->planeCalibParams.push_back(cameraParam); - } - - // 保存配置 - QString configPath = PathManager::GetInstance().GetConfigFilePath(); - bool saveResult = m_pConfig->SaveConfig(configPath.toStdString(), *m_pConfigResult); - 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, Error compensation: %.2f\n", planeHeight, errorCompensation); - - return true; - - } catch (const std::exception& e) { - LOG_ERROR("Exception in saveLevelingResults: %s\n", e.what()); - return false; - } -} - -bool DialogCameraLevel::loadCameraCalibrationData(int cameraIndex, const QString& cameraName, - double planeCalib[9], double& planeHeight, double invRMatrix[9]) -{ - try { - if (!m_pConfigResult) { - LOG_ERROR("Config result is null, cannot load calibration data\n"); - return false; - } - - // 查找对应相机的调平参数 - for (const auto& param : m_pConfigResult->planeCalibParams) { - if (param.cameraIndex == cameraIndex && param.isCalibrated) { - for (int i = 0; i < 9; i++) { - planeCalib[i] = param.planeCalib[i]; - invRMatrix[i] = param.invRMatrix[i]; - } - planeHeight = param.planeHeight; - - // 加载该相机的误差补偿值到UI - ui->edit_error_compensation->setText(QString::number(param.errorCompensation, 'f', 1)); - - LOG_INFO("Calibration data loaded successfully for camera %d (%s)\n", - cameraIndex, cameraName.toUtf8().constData()); - return true; - } - } - - // 没有找到标定数据时，设置默认误差补偿值 - ui->edit_error_compensation->setText(QString::number(-5.0, 'f', 1)); - - LOG_INFO("No calibration data found for camera %d (%s)\n", - cameraIndex, cameraName.toUtf8().constData()); - return false; - - } 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 >= static_cast(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; - - 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\n", cameraName.toUtf8().constData()); - ui->label_level_result->setText(QString("相机: %1\n\n请点击调平按钮开始调平操作").arg(cameraName)); - ui->label_level_result->setAlignment(Qt::AlignCenter); - } -} - -void DialogCameraLevel::on_combo_camera_currentIndexChanged(int index) -{ - m_currentCameraIndex = index; - - if (index >= 0 && index < static_cast(m_cameraList.size())) { - LOG_INFO("Camera selection changed to index: %d (%s)\n", index, - QString::fromStdString(m_cameraList[index].first).toUtf8().constData()); - - checkAndDisplayCalibrationStatus(m_currentCameraIndex); - } else { - LOG_WARNING("Invalid camera index selected: %d\n", index); - ui->label_level_result->setText("无效的相机选择"); - 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"); -} - -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; - } - - m_presenter->SetCameraStatusCallback(&WheelMeasurePresenter::_StaticCameraNotify, m_presenter); - - LOG_INFO("Presenter status callback restored for all cameras\n"); -} - -void DialogCameraLevel::on_btn_save_compensation_clicked() -{ - // 保存误差补偿值到当前相机的调平参数 - if (!m_pConfig || !m_pConfigResult) { - LOG_ERROR("Config is null, cannot save error compensation\n"); - return; - } - - if (m_currentCameraIndex < 0 || m_currentCameraIndex >= static_cast(m_cameraList.size())) { - LOG_WARNING("Invalid camera index: %d\n", m_currentCameraIndex); - return; - } - - // 获取UI中的误差补偿值 - double errorCompensation = ui->edit_error_compensation->text().toDouble(); - int cameraIndex = m_currentCameraIndex + 1; // 转换为1-based索引 - QString cameraName = QString::fromStdString(m_cameraList[m_currentCameraIndex].first); - - LOG_INFO("Saving error compensation for camera %d (%s): %.2f\n", - cameraIndex, cameraName.toUtf8().constData(), errorCompensation); - - // 查找或创建相机调平参数 - bool found = false; - for (auto& param : m_pConfigResult->planeCalibParams) { - if (param.cameraIndex == cameraIndex) { - param.errorCompensation = errorCompensation; - found = true; - break; - } - } - - // 如果没有找到现有记录，创建一个新的（仅包含误差补偿，其他值为默认） - if (!found) { - WheelCameraPlaneCalibParam newParam; - newParam.cameraIndex = cameraIndex; - newParam.cameraName = cameraName.toStdString(); - newParam.errorCompensation = errorCompensation; - newParam.isCalibrated = false; // 尚未标定 - m_pConfigResult->planeCalibParams.push_back(newParam); - } - - // 保存配置到文件 - QString configPath = PathManager::GetInstance().GetConfigFilePath(); - bool saveResult = m_pConfig->SaveConfig(configPath.toStdString(), *m_pConfigResult); - if (saveResult) { - LOG_INFO("Error compensation saved successfully for camera %d: %.2f\n", cameraIndex, errorCompensation); - } else { - LOG_ERROR("Failed to save error compensation\n"); - } -} +#include "dialogcameralevel.h" +#include "ui_dialogcameralevel.h" +#include "WheelMeasurePresenter.h" +#include "PathManager.h" +#include "VrLog.h" +#include "wheelArchHeigthMeasure_Export.h" + +#include +#include +#include +#include +#include +#include + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +DialogCameraLevel::DialogCameraLevel(QWidget *parent) + : QDialog(parent) + , ui(new Ui::DialogCameraLevel) + , m_pConfig(nullptr) + , m_pConfigResult(nullptr) + , m_currentCameraIndex(-1) +{ + ui->setupUi(this); + + // 初始化结果显示区域 + 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>& cameraList, + WheelMeasurePresenter* presenter) +{ + m_cameraList = cameraList; + m_presenter = presenter; + + LOG_INFO("setCameraList called with %zu cameras\n", cameraList.size()); + + // 详细记录每个相机的信息 + for (size_t i = 0; i < cameraList.size(); ++i) { + const auto& camera = cameraList[i]; + LOG_INFO(" Camera %zu: name='%s', device=%s\n", + i + 1, + camera.first.c_str(), + (camera.second != nullptr ? "connected" : "not connected")); + } + + // 初始化/重新初始化相机选择框 + initializeCameraCombo(); +} + +void DialogCameraLevel::setConfig(IVrWheelMeasureConfig* config, WheelMeasureConfigResult* configResult) +{ + m_pConfig = config; + m_pConfigResult = configResult; + + // 如果相机已经选择，重新加载当前相机的标定状态 + // 修复：打开页面时配置可能在相机列表之后设置，导致初始加载失败 + if (m_currentCameraIndex >= 0 && m_currentCameraIndex < static_cast(m_cameraList.size())) { + checkAndDisplayCalibrationStatus(m_currentCameraIndex); + loadCameraRoiRange(m_currentCameraIndex); + } +} + +void DialogCameraLevel::initializeCameraCombo() +{ + LOG_INFO("initializeCameraCombo called, camera list size: %zu\n", m_cameraList.size()); + + ui->combo_camera->clear(); + + if (m_cameraList.empty()) { + ui->combo_camera->setEnabled(false); + + if (!m_presenter) { + ui->label_level_result->setText("Presenter未初始化\n无法获取相机列表"); + LOG_ERROR("Presenter is null in initializeCameraCombo\n"); + } else { + ui->label_level_result->setText("相机列表为空\n\n可能原因：\n1. 配置文件中未配置相机\n2. 系统正在初始化中\n3. 所有相机连接失败"); + LOG_WARNING("Camera list is empty in initializeCameraCombo\n"); + } + + ui->label_level_result->setAlignment(Qt::AlignCenter); + } else { + LOG_INFO("Adding %zu cameras to combo box\n", m_cameraList.size()); + + // 添加所有相机到下拉列表 + for (size_t i = 0; i < m_cameraList.size(); ++i) { + const auto& camera = m_cameraList[i]; + QString cameraName = QString::fromStdString(camera.first); + + // 如果相机没有连接，在名称后添加标记 + if (camera.second == nullptr) { + cameraName += " [未连接]"; + } + + ui->combo_camera->addItem(cameraName); + LOG_INFO(" Added camera %zu: %s (device=%s)\n", + i + 1, + camera.first.c_str(), + (camera.second != nullptr ? "OK" : "NULL")); + } + ui->combo_camera->setEnabled(true); + + // 获取默认相机索引 + int defaultCameraIndex = 0; + if (m_presenter) { + int presenterDefaultIndex = m_presenter->GetDefaultCameraIndex(); + LOG_INFO("Presenter default camera index (1-based): %d\n", presenterDefaultIndex); + + if (presenterDefaultIndex > 0 && presenterDefaultIndex <= static_cast(m_cameraList.size())) { + defaultCameraIndex = presenterDefaultIndex - 1; + } + } + + // 设置默认选中的相机 + if (defaultCameraIndex >= 0 && defaultCameraIndex < static_cast(m_cameraList.size())) { + ui->combo_camera->setCurrentIndex(defaultCameraIndex); + m_currentCameraIndex = defaultCameraIndex; + } + + // 检查并显示当前选中相机的标定状态 + if (m_currentCameraIndex >= 0) { + checkAndDisplayCalibrationStatus(m_currentCameraIndex); + } + } +} + +void DialogCameraLevel::on_btn_apply_clicked() +{ + ui->label_level_result->setAlignment(Qt::AlignLeft); + + // 检查是否有可用的相机 + if (m_cameraList.empty()) { + QMessageBox::warning(this, "错误", "无可用相机设备！"); + return; + } + + // 获取选中的相机 + int selectedIndex = ui->combo_camera->currentIndex(); + if (selectedIndex < 0 || selectedIndex >= static_cast(m_cameraList.size())) { + QMessageBox::warning(this, "错误", "请选择有效的相机！"); + return; + } + + // 清空之前的结果显示 + ui->label_level_result->setText("调平计算中，请稍候..."); + + // 显示进度提示 + ui->btn_apply->setEnabled(false); + QApplication::processEvents(); + + try { + // 执行相机调平 + if (performCameraLeveling()) { + // 调平成功 + } 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(); + + // 先检查索引有效性，再设置回调 + if (selectedIndex < 0 || selectedIndex >= static_cast(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); + + // 1. 设置调平状态回调（在索引检查之后） + setLevelingStatusCallback(); + + // 2. 清空之前的扫描数据 + clearScanDataCache(); + + // 3. 启动相机扫描地面数据 + if (!startCameraScan(selectedIndex)) { + LOG_ERROR("Failed to start camera scan for leveling\n"); + restorePresenterStatusCallback(); // 恢复回调 + return false; + } + + // 4. 等待扫描完成 + LOG_INFO("Collecting ground scan data, waiting for swing finish signal...\n"); + int waitTime = 0; + const int maxWaitTime = 10000; // 最大等待10秒 + const int checkInterval = 100; + + while (!m_swingFinished && waitTime < maxWaitTime) { + QThread::msleep(checkInterval); + QApplication::processEvents(); + 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"); + } + + // 6. 调用调平算法计算 + double planeCalib[9]; + double planeHeight; + double invRMatrix[9]; + + if (!calculatePlaneCalibration(planeCalib, planeHeight, invRMatrix)) { + LOG_ERROR("Failed to calculate plane calibration\n"); + restorePresenterStatusCallback(); // 恢复回调 + return false; + } + + LOG_INFO("Camera leveling calculation completed\n"); + + // 7. 更新界面显示 + updateLevelingResults(planeCalib, planeHeight, invRMatrix); + + // 8. 保存结果到配置 + int cameraIndex = m_currentCameraIndex + 1; // 转换为1-based索引 + QString cameraName; + + if (m_currentCameraIndex >= 0 && m_currentCameraIndex < static_cast(m_cameraList.size())) { + cameraName = QString::fromStdString(m_cameraList[m_currentCameraIndex].first); + } else { + cameraName = QString("Camera_%1").arg(cameraIndex); + } + + if (!saveLevelingResults(planeCalib, planeHeight, invRMatrix, cameraIndex, cameraName)) { + LOG_ERROR("Failed to save leveling results\n"); + restorePresenterStatusCallback(); // 恢复回调 + return false; + } + + clearScanDataCache(); + + // 9. 调平完成后恢复回调 + restorePresenterStatusCallback(); + + LOG_INFO("Camera leveling completed successfully\n"); + + return true; + + } catch (const std::exception& e) { + LOG_ERROR("Exception in performCameraLeveling: %s\n", e.what()); + restorePresenterStatusCallback(); // 异常时也恢复回调 + return false; + } +} + +void DialogCameraLevel::updateLevelingResults(double planeCalib[9], double planeHeight, double invRMatrix[9]) +{ + // 构建显示文本 + QString resultText; + + 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 >= static_cast(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 >= static_cast(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(pUserData); + if (pThis && pLaserLinePoint) { + pThis->DetectionCallback(eDataType, pLaserLinePoint); + } +} + +void DialogCameraLevel::StaticStatusCallback(EVzDeviceWorkStatus eStatus, void* pExtData, unsigned int nDataLength, void* pInfoParam) +{ + DialogCameraLevel* pThis = reinterpret_cast(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; + } + + // 将数据添加到缓存 + std::vector lineData; + lineData.reserve(pLaserLinePoint->nPointCount); + + // p3DPoint 是 void*，需要转换为 SVzNL3DPosition* + SVzNL3DPosition* p3DPoints = reinterpret_cast(pLaserLinePoint->p3DPoint); + for (int i = 0; i < pLaserLinePoint->nPointCount; ++i) { + lineData.push_back(p3DPoints[i]); + } + + std::lock_guard lock(m_scanDataMutex); + m_scanDataCache.push_back(std::move(lineData)); +} + +bool DialogCameraLevel::calculatePlaneCalibration(double planeCalib[9], double& planeHeight, double invRMatrix[9]) +{ + std::lock_guard 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 { + // 调用 wheelArchHeigthMeasure SDK 的调平算法 + SSG_planeCalibPara calibResult = wd_horizonCamera_getGroundCalibPara(m_scanDataCache); + + // 复制调平矩阵 + for (int i = 0; i < 9; i++) { + planeCalib[i] = calibResult.planeCalib[i]; + invRMatrix[i] = calibResult.invRMatrix[i]; + } + planeHeight = calibResult.planeHeight; + + LOG_INFO("Plane calibration calculated successfully\n"); + LOG_INFO(" planeHeight: %.3f\n", planeHeight); + LOG_INFO(" planeCalib: [%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f]\n", + planeCalib[0], planeCalib[1], planeCalib[2], + planeCalib[3], planeCalib[4], planeCalib[5], + planeCalib[6], planeCalib[7], planeCalib[8]); + LOG_INFO(" invRMatrix: [%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f]\n", + invRMatrix[0], invRMatrix[1], invRMatrix[2], + invRMatrix[3], invRMatrix[4], invRMatrix[5], + invRMatrix[6], invRMatrix[7], invRMatrix[8]); + + return true; + + } catch (const std::exception& e) { + LOG_ERROR("Exception in calculatePlaneCalibration: %s\n", e.what()); + return false; + } +} + +void DialogCameraLevel::clearScanDataCache() +{ + std::lock_guard lock(m_scanDataMutex); + + LOG_DEBUG("Clearing scan data cache, current size: %zu\n", m_scanDataCache.size()); + m_scanDataCache.clear(); + 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_pConfig || !m_pConfigResult) { + 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; + } + + // 获取UI中的误差补偿值 + double errorCompensation = ui->edit_error_compensation->text().toDouble(); + + // 创建或更新相机调平参数 + WheelCameraPlaneCalibParam cameraParam; + cameraParam.cameraIndex = cameraIndex; + cameraParam.cameraName = cameraName.toStdString(); + cameraParam.planeHeight = planeHeight; + cameraParam.isCalibrated = true; + cameraParam.errorCompensation = errorCompensation; + + // 复制校准矩阵 + for (int i = 0; i < 9; i++) { + cameraParam.planeCalib[i] = planeCalib[i]; + cameraParam.invRMatrix[i] = invRMatrix[i]; + } + + // 查找是否已有该相机的调平参数 + bool found = false; + for (auto& param : m_pConfigResult->planeCalibParams) { + if (param.cameraIndex == cameraIndex) { + param = cameraParam; + found = true; + break; + } + } + + if (!found) { + m_pConfigResult->planeCalibParams.push_back(cameraParam); + } + + // 保存配置 + QString configPath = PathManager::GetInstance().GetConfigFilePath(); + bool saveResult = m_pConfig->SaveConfig(configPath.toStdString(), *m_pConfigResult); + 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, Error compensation: %.2f\n", planeHeight, errorCompensation); + + return true; + + } catch (const std::exception& e) { + LOG_ERROR("Exception in saveLevelingResults: %s\n", e.what()); + return false; + } +} + +bool DialogCameraLevel::loadCameraCalibrationData(int cameraIndex, const QString& cameraName, + double planeCalib[9], double& planeHeight, double invRMatrix[9]) +{ + try { + if (!m_pConfigResult) { + LOG_ERROR("Config result is null, cannot load calibration data\n"); + return false; + } + + // 查找对应相机的调平参数 + for (const auto& param : m_pConfigResult->planeCalibParams) { + if (param.cameraIndex == cameraIndex && param.isCalibrated) { + for (int i = 0; i < 9; i++) { + planeCalib[i] = param.planeCalib[i]; + invRMatrix[i] = param.invRMatrix[i]; + } + planeHeight = param.planeHeight; + + // 加载该相机的误差补偿值到UI + ui->edit_error_compensation->setText(QString::number(param.errorCompensation, 'f', 1)); + + LOG_INFO("Calibration data loaded successfully for camera %d (%s)\n", + cameraIndex, cameraName.toUtf8().constData()); + return true; + } + } + + // 没有找到标定数据时，设置默认误差补偿值 + ui->edit_error_compensation->setText(QString::number(-5.0, 'f', 1)); + + LOG_INFO("No calibration data found for camera %d (%s)\n", + cameraIndex, cameraName.toUtf8().constData()); + return false; + + } 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 >= static_cast(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; + + 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\n", cameraName.toUtf8().constData()); + ui->label_level_result->setText(QString("相机: %1\n\n请点击调平按钮开始调平操作").arg(cameraName)); + ui->label_level_result->setAlignment(Qt::AlignCenter); + } +} + +void DialogCameraLevel::on_combo_camera_currentIndexChanged(int index) +{ + m_currentCameraIndex = index; + + if (index >= 0 && index < static_cast(m_cameraList.size())) { + LOG_INFO("Camera selection changed to index: %d (%s)\n", index, + QString::fromStdString(m_cameraList[index].first).toUtf8().constData()); + + checkAndDisplayCalibrationStatus(m_currentCameraIndex); + loadCameraRoiRange(m_currentCameraIndex); + } else { + LOG_WARNING("Invalid camera index selected: %d\n", index); + ui->label_level_result->setText("无效的相机选择"); + 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"); +} + +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; + } + + m_presenter->SetCameraStatusCallback(&WheelMeasurePresenter::_StaticCameraNotify, m_presenter); + + LOG_INFO("Presenter status callback restored for all cameras\n"); +} + +void DialogCameraLevel::on_btn_save_compensation_clicked() +{ + // 保存误差补偿值到当前相机的调平参数 + if (!m_pConfig || !m_pConfigResult) { + LOG_ERROR("Config is null, cannot save error compensation\n"); + return; + } + + if (m_currentCameraIndex < 0 || m_currentCameraIndex >= static_cast(m_cameraList.size())) { + LOG_WARNING("Invalid camera index: %d\n", m_currentCameraIndex); + return; + } + + // 获取UI中的误差补偿值 + double errorCompensation = ui->edit_error_compensation->text().toDouble(); + int cameraIndex = m_currentCameraIndex + 1; // 转换为1-based索引 + QString cameraName = QString::fromStdString(m_cameraList[m_currentCameraIndex].first); + + LOG_INFO("Saving error compensation for camera %d (%s): %.2f\n", + cameraIndex, cameraName.toUtf8().constData(), errorCompensation); + + // 查找或创建相机调平参数 + bool found = false; + for (auto& param : m_pConfigResult->planeCalibParams) { + if (param.cameraIndex == cameraIndex) { + param.errorCompensation = errorCompensation; + found = true; + break; + } + } + + // 如果没有找到现有记录，创建一个新的（仅包含误差补偿，其他值为默认） + if (!found) { + WheelCameraPlaneCalibParam newParam; + newParam.cameraIndex = cameraIndex; + newParam.cameraName = cameraName.toStdString(); + newParam.errorCompensation = errorCompensation; + newParam.isCalibrated = false; // 尚未标定 + m_pConfigResult->planeCalibParams.push_back(newParam); + } + + // 保存配置到文件 + QString configPath = PathManager::GetInstance().GetConfigFilePath(); + bool saveResult = m_pConfig->SaveConfig(configPath.toStdString(), *m_pConfigResult); + if (saveResult) { + LOG_INFO("Error compensation saved successfully for camera %d: %.2f\n", cameraIndex, errorCompensation); + } else { + LOG_ERROR("Failed to save error compensation\n"); + } +} + +void DialogCameraLevel::loadCameraRoiRange(int cameraIndex) +{ + if (!m_pConfig || !m_pConfigResult) { + LOG_WARNING("Config is null, cannot load ROI range\n"); + return; + } + + if (cameraIndex < 0 || cameraIndex >= static_cast(m_cameraList.size())) { + LOG_WARNING("Invalid camera index: %d\n", cameraIndex); + return; + } + + int configCameraIndex = cameraIndex + 1; // 转换为1-based索引 + + // 查找相机的调平参数 + for (const auto& param : m_pConfigResult->planeCalibParams) { + if (param.cameraIndex == configCameraIndex) { + // 加载ROI范围到UI + ui->edit_roi_x_min->setText(QString::number(param.wheelRoi3d_xMin, 'f', 1)); + ui->edit_roi_x_max->setText(QString::number(param.wheelRoi3d_xMax, 'f', 1)); + ui->edit_roi_y_min->setText(QString::number(param.wheelRoi3d_yMin, 'f', 1)); + ui->edit_roi_y_max->setText(QString::number(param.wheelRoi3d_yMax, 'f', 1)); + ui->edit_roi_z_min->setText(QString::number(param.wheelRoi3d_zMin, 'f', 1)); + ui->edit_roi_z_max->setText(QString::number(param.wheelRoi3d_zMax, 'f', 1)); + + LOG_INFO("Loaded ROI range for camera %d: X[%.1f, %.1f], Y[%.1f, %.1f], Z[%.1f, %.1f]\n", + configCameraIndex, + param.wheelRoi3d_xMin, param.wheelRoi3d_xMax, + param.wheelRoi3d_yMin, param.wheelRoi3d_yMax, + param.wheelRoi3d_zMin, param.wheelRoi3d_zMax); + return; + } + } + + // 如果没有找到，使用默认值 + ui->edit_roi_x_min->setText("-1000.0"); + ui->edit_roi_x_max->setText("1000.0"); + ui->edit_roi_y_min->setText("-1000.0"); + ui->edit_roi_y_max->setText("1000.0"); + ui->edit_roi_z_min->setText("-1000.0"); + ui->edit_roi_z_max->setText("1000.0"); + LOG_INFO("No ROI range found for camera %d, using default values\n", configCameraIndex); +} + +void DialogCameraLevel::saveCameraRoiRange() +{ + if (!m_pConfig || !m_pConfigResult) { + LOG_ERROR("Config is null, cannot save ROI range\n"); + QMessageBox::warning(this, "错误", "配置对象为空，无法保存ROI范围"); + return; + } + + if (m_currentCameraIndex < 0 || m_currentCameraIndex >= static_cast(m_cameraList.size())) { + LOG_WARNING("Invalid camera index: %d\n", m_currentCameraIndex); + QMessageBox::warning(this, "错误", "请先选择相机"); + return; + } + + // 获取UI中的ROI范围值 + double xMin = ui->edit_roi_x_min->text().toDouble(); + double xMax = ui->edit_roi_x_max->text().toDouble(); + double yMin = ui->edit_roi_y_min->text().toDouble(); + double yMax = ui->edit_roi_y_max->text().toDouble(); + double zMin = ui->edit_roi_z_min->text().toDouble(); + double zMax = ui->edit_roi_z_max->text().toDouble(); + + // 验证范围有效性 + if (xMin >= xMax || yMin >= yMax || zMin >= zMax) { + QMessageBox::warning(this, "错误", "ROI范围无效：最小值必须小于最大值"); + return; + } + + int cameraIndex = m_currentCameraIndex + 1; // 转换为1-based索引 + QString cameraName = QString::fromStdString(m_cameraList[m_currentCameraIndex].first); + + LOG_INFO("Saving ROI range for camera %d (%s): X[%.1f, %.1f], Y[%.1f, %.1f], Z[%.1f, %.1f]\n", + cameraIndex, cameraName.toUtf8().constData(), + xMin, xMax, yMin, yMax, zMin, zMax); + + // 查找或创建相机调平参数 + bool found = false; + for (auto& param : m_pConfigResult->planeCalibParams) { + if (param.cameraIndex == cameraIndex) { + param.wheelRoi3d_xMin = xMin; + param.wheelRoi3d_xMax = xMax; + param.wheelRoi3d_yMin = yMin; + param.wheelRoi3d_yMax = yMax; + param.wheelRoi3d_zMin = zMin; + param.wheelRoi3d_zMax = zMax; + found = true; + break; + } + } + + // 如果没有找到现有记录，创建一个新的 + if (!found) { + WheelCameraPlaneCalibParam newParam; + newParam.cameraIndex = cameraIndex; + newParam.cameraName = cameraName.toStdString(); + newParam.wheelRoi3d_xMin = xMin; + newParam.wheelRoi3d_xMax = xMax; + newParam.wheelRoi3d_yMin = yMin; + newParam.wheelRoi3d_yMax = yMax; + newParam.wheelRoi3d_zMin = zMin; + newParam.wheelRoi3d_zMax = zMax; + newParam.isCalibrated = false; // 尚未标定 + m_pConfigResult->planeCalibParams.push_back(newParam); + } + + // 保存配置到文件 + QString configPath = PathManager::GetInstance().GetConfigFilePath(); + bool saveResult = m_pConfig->SaveConfig(configPath.toStdString(), *m_pConfigResult); + if (saveResult) { + LOG_INFO("ROI range saved successfully for camera %d\n", cameraIndex); + QMessageBox::information(this, "成功", "ROI范围已保存"); + } else { + LOG_ERROR("Failed to save ROI range\n"); + QMessageBox::critical(this, "错误", "保存ROI范围失败"); + } +} + +void DialogCameraLevel::on_btn_save_roi_clicked() +{ + saveCameraRoiRange(); +} diff --git a/App/WheelMeasure/WheelMeasureApp/dialogcameralevel.h b/App/WheelMeasure/WheelMeasureApp/dialogcameralevel.h index b2ff263d..785d08e3 100644 --- a/App/WheelMeasure/WheelMeasureApp/dialogcameralevel.h +++ b/App/WheelMeasure/WheelMeasureApp/dialogcameralevel.h @@ -1,118 +1,123 @@ -#ifndef DIALOGCAMERALEVEL_H -#define DIALOGCAMERALEVEL_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "IVrEyeDevice.h" -#include "IVrWheelMeasureConfig.h" -#include "VZNL_Types.h" - -// 前向声明 -class WheelMeasurePresenter; - -namespace Ui { -class DialogCameraLevel; -} - -class DialogCameraLevel : public QDialog -{ - Q_OBJECT - -public: - explicit DialogCameraLevel(QWidget *parent = nullptr); - ~DialogCameraLevel(); - - /** - * @brief 设置相机列表和presenter - * @param cameraList 相机列表 - * @param presenter Presenter指针 - */ - void setCameraList(const std::vector>& cameraList, - WheelMeasurePresenter* presenter); - - /** - * @brief 设置配置对象 - * @param config 配置接口 - * @param configResult 配置结果 - */ - void setConfig(IVrWheelMeasureConfig* config, WheelMeasureConfigResult* configResult); - -private slots: - void on_btn_apply_clicked(); - void on_btn_cancel_clicked(); - void on_combo_camera_currentIndexChanged(int index); - void on_btn_save_compensation_clicked(); - -private: - Ui::DialogCameraLevel *ui; - - // 相机列表和名称 - std::vector> m_cameraList; - WheelMeasurePresenter* m_presenter = nullptr; - - // 配置对象 - IVrWheelMeasureConfig* m_pConfig = nullptr; - WheelMeasureConfigResult* m_pConfigResult = nullptr; - - // 当前选中的相机索引 - int m_currentCameraIndex = -1; - - // 扫描数据缓存 - std::vector> m_scanDataCache; - std::mutex m_scanDataMutex; - - // 状态回调相关 - std::atomic m_swingFinished{false}; - std::atomic m_callbackRestored{false}; - - // 初始化相机选择框 - void initializeCameraCombo(); - - // 执行相机调平 - bool performCameraLeveling(); - - // 直接使用相机接口进行扫描 - bool startCameraScan(int cameraIndex); - bool stopCameraScan(int cameraIndex); - - // 检测数据回调函数 - static void StaticDetectionCallback(EVzResultDataType eDataType, SVzLaserLineData* pLaserLinePoint, void* pUserData); - void DetectionCallback(EVzResultDataType eDataType, SVzLaserLineData* pLaserLinePoint); - - // 状态回调函数 - static void StaticStatusCallback(EVzDeviceWorkStatus eStatus, void* pExtData, unsigned int nDataLength, void* pInfoParam); - void StatusCallback(EVzDeviceWorkStatus eStatus, void* pExtData, unsigned int nDataLength, void* pInfoParam); - - // 设置和恢复状态回调 - void setLevelingStatusCallback(); - void restorePresenterStatusCallback(); - - // 处理扫描到的地面数据进行调平计算 - bool calculatePlaneCalibration(double planeCalib[9], double& planeHeight, double invRMatrix[9]); - - // 清空扫描数据缓存 - void clearScanDataCache(); - - // 更新调平结果显示 - void updateLevelingResults(double planeCalib[9], double planeHeight, double invRMatrix[9]); - - // 保存调平结果到配置 - bool saveLevelingResults(double planeCalib[9], double planeHeight, double invRMatrix[9], - int cameraIndex, const QString& cameraName); - - // 加载相机标定数据 - bool loadCameraCalibrationData(int cameraIndex, const QString& cameraName, - double planeCalib[9], double& planeHeight, double invRMatrix[9]); - - // 检查并显示相机标定状态 - void checkAndDisplayCalibrationStatus(int cameraIndex); -}; - -#endif // DIALOGCAMERALEVEL_H +#ifndef DIALOGCAMERALEVEL_H +#define DIALOGCAMERALEVEL_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "IVrEyeDevice.h" +#include "IVrWheelMeasureConfig.h" +#include "VZNL_Types.h" + +// 前向声明 +class WheelMeasurePresenter; + +namespace Ui { +class DialogCameraLevel; +} + +class DialogCameraLevel : public QDialog +{ + Q_OBJECT + +public: + explicit DialogCameraLevel(QWidget *parent = nullptr); + ~DialogCameraLevel(); + + /** + * @brief 设置相机列表和presenter + * @param cameraList 相机列表 + * @param presenter Presenter指针 + */ + void setCameraList(const std::vector>& cameraList, + WheelMeasurePresenter* presenter); + + /** + * @brief 设置配置对象 + * @param config 配置接口 + * @param configResult 配置结果 + */ + void setConfig(IVrWheelMeasureConfig* config, WheelMeasureConfigResult* configResult); + +private slots: + void on_btn_apply_clicked(); + void on_btn_cancel_clicked(); + void on_combo_camera_currentIndexChanged(int index); + void on_btn_save_compensation_clicked(); + void on_btn_save_roi_clicked(); + +private: + Ui::DialogCameraLevel *ui; + + // 相机列表和名称 + std::vector> m_cameraList; + WheelMeasurePresenter* m_presenter = nullptr; + + // 配置对象 + IVrWheelMeasureConfig* m_pConfig = nullptr; + WheelMeasureConfigResult* m_pConfigResult = nullptr; + + // 当前选中的相机索引 + int m_currentCameraIndex = -1; + + // 扫描数据缓存 + std::vector> m_scanDataCache; + std::mutex m_scanDataMutex; + + // 状态回调相关 + std::atomic m_swingFinished{false}; + std::atomic m_callbackRestored{false}; + + // 初始化相机选择框 + void initializeCameraCombo(); + + // 执行相机调平 + bool performCameraLeveling(); + + // 直接使用相机接口进行扫描 + bool startCameraScan(int cameraIndex); + bool stopCameraScan(int cameraIndex); + + // 检测数据回调函数 + static void StaticDetectionCallback(EVzResultDataType eDataType, SVzLaserLineData* pLaserLinePoint, void* pUserData); + void DetectionCallback(EVzResultDataType eDataType, SVzLaserLineData* pLaserLinePoint); + + // 状态回调函数 + static void StaticStatusCallback(EVzDeviceWorkStatus eStatus, void* pExtData, unsigned int nDataLength, void* pInfoParam); + void StatusCallback(EVzDeviceWorkStatus eStatus, void* pExtData, unsigned int nDataLength, void* pInfoParam); + + // 设置和恢复状态回调 + void setLevelingStatusCallback(); + void restorePresenterStatusCallback(); + + // 处理扫描到的地面数据进行调平计算 + bool calculatePlaneCalibration(double planeCalib[9], double& planeHeight, double invRMatrix[9]); + + // 清空扫描数据缓存 + void clearScanDataCache(); + + // 更新调平结果显示 + void updateLevelingResults(double planeCalib[9], double planeHeight, double invRMatrix[9]); + + // 保存调平结果到配置 + bool saveLevelingResults(double planeCalib[9], double planeHeight, double invRMatrix[9], + int cameraIndex, const QString& cameraName); + + // 加载相机标定数据 + bool loadCameraCalibrationData(int cameraIndex, const QString& cameraName, + double planeCalib[9], double& planeHeight, double invRMatrix[9]); + + // 检查并显示相机标定状态 + void checkAndDisplayCalibrationStatus(int cameraIndex); + + // 加载和保存ROI范围 + void loadCameraRoiRange(int cameraIndex); + void saveCameraRoiRange(); +}; + +#endif // DIALOGCAMERALEVEL_H diff --git a/App/WheelMeasure/WheelMeasureApp/dialogcameralevel.ui b/App/WheelMeasure/WheelMeasureApp/dialogcameralevel.ui index c62508ce..b9378f76 100644 --- a/App/WheelMeasure/WheelMeasureApp/dialogcameralevel.ui +++ b/App/WheelMeasure/WheelMeasureApp/dialogcameralevel.ui @@ -1,306 +1,754 @@ - - - DialogCameraLevel - - - - 0 - 0 - 659 - 453 - - - - 相机调平 - - - background-color: rgb(25, 26, 28);color: rgb(221, 225, 233); - - - - - 20 - 110 - 101 - 31 - - - - - 16 - - - - color: rgb(221, 225, 233); - - - 调平结果： - - - - - - 20 - 10 - 621 - 45 - - - - - 18 - - - - color: rgb(221, 225, 233); - - - 相机调平 - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - - - - 170 - 400 - 101 - 38 - - - - - 18 - - - - image: url(:/common/resource/dialog_ok.png); - - - - - - - - - 380 - 400 - 111 - 38 - - - - - 18 - - - - image: url(:/common/resource/dialog_cancel.png); - - - - - - - - - 130 - 110 - 511 - 271 - - - - - 14 - - - - color: rgb(221, 225, 233); -background-color: rgb(47, 48, 52); -border: 1px solid #3B3D47; -padding: 5px; - - - - - - Qt::AlignCenter - - - - - - 20 - 60 - 634 - 41 - - - - - - - - 100 - 0 - - - - - 16 - - - - color: rgb(221, 225, 233); - - - 选择相机： - - - - - - - - 200 - 35 - - - - - 16 - - - - color: rgb(221, 225, 233); -background-color: rgb(47, 48, 52); - - - - - - - - 100 - 0 - - - - - 16 - - - - color: rgb(221, 225, 233); - - - 误差补偿： - - - - - - - - 100 - 35 - - - - - 100 - 16777215 - - - - - 16 - - - - color: rgb(221, 225, 233); -background-color: rgb(47, 48, 52); - - - -5.0 - - - Qt::AlignCenter - - - - - - - - 14 - - - - color: rgb(221, 225, 233); - - - mm - - - - - - - - 60 - 35 - - - - - 60 - 35 - - - - - 14 - - - - QPushButton { - color: rgb(221, 225, 233); - background-color: rgb(60, 63, 65); - border: 1px solid #3B3D47; - border-radius: 4px; -} -QPushButton:hover { - background-color: rgb(80, 83, 85); -} -QPushButton:pressed { - background-color: rgb(45, 48, 50); -} - - - 保存 - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - - + + + DialogCameraLevel + + + + 0 + 0 + 659 + 599 + + + + 相机调平 + + + background-color: rgb(25, 26, 28);color: rgb(221, 225, 233); + + + + + 20 + 110 + 101 + 31 + + + + + 16 + + + + color: rgb(221, 225, 233); + + + 调平结果： + + + + + + 20 + 10 + 621 + 45 + + + + + 18 + + + + color: rgb(221, 225, 233); + + + 相机调平 + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + + + + 170 + 550 + 101 + 38 + + + + + 18 + + + + image: url(:/common/resource/dialog_ok.png); + + + + + + + + + 380 + 550 + 111 + 38 + + + + + 18 + + + + image: url(:/common/resource/dialog_cancel.png); + + + + + + + + + 130 + 110 + 511 + 271 + + + + + 14 + + + + color: rgb(221, 225, 233); +background-color: rgb(47, 48, 52); +border: 1px solid #3B3D47; +padding: 5px; + + + + + + Qt::AlignCenter + + + + + + 20 + 60 + 634 + 41 + + + + + + + + 100 + 0 + + + + + 16 + + + + color: rgb(221, 225, 233); + + + 选择相机： + + + + + + + + 200 + 35 + + + + + 16 + + + + color: rgb(221, 225, 233); +background-color: rgb(47, 48, 52); + + + + + + + + 100 + 0 + + + + + 16 + + + + color: rgb(221, 225, 233); + + + 误差补偿： + + + + + + + + 100 + 35 + + + + + 100 + 16777215 + + + + + 16 + + + + color: rgb(221, 225, 233); +background-color: rgb(47, 48, 52); + + + -5.0 + + + Qt::AlignCenter + + + + + + + + 14 + + + + color: rgb(221, 225, 233); + + + mm + + + + + + + + 60 + 35 + + + + + 60 + 35 + + + + + 14 + + + + QPushButton { + color: rgb(221, 225, 233); + background-color: rgb(60, 63, 65); + border: 1px solid #3B3D47; + border-radius: 4px; +} +QPushButton:hover { + background-color: rgb(80, 83, 85); +} +QPushButton:pressed { + background-color: rgb(45, 48, 50); +} + + + 保存 + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + 20 + 390 + 621 + 150 + + + + + 14 + + + + QGroupBox { + color: rgb(221, 225, 233); + border: 1px solid #3B3D47; + border-radius: 4px; + margin-top: 10px; + padding-top: 10px; +} +QGroupBox::title { + subcontrol-origin: margin; + subcontrol-position: top left; + padding: 0 5px; +} + + + 轮胎存在检测ROI范围 (mm) + + + + 15 + + + 15 + + + + + + 20 + 0 + + + + + 20 + 16777215 + + + + + 13 + + + + color: rgb(221, 225, 233); + + + ~ + + + Qt::AlignCenter + + + + + + + + 80 + 30 + + + + + 80 + 30 + + + + + 13 + + + + color: rgb(221, 225, 233); +background-color: rgb(47, 48, 52); + + + -1000.0 + + + Qt::AlignCenter + + + + + + + + 80 + 35 + + + + + 80 + 35 + + + + + 14 + + + + QPushButton { + color: rgb(221, 225, 233); + background-color: rgb(60, 63, 65); + border: 1px solid #3B3D47; + border-radius: 4px; +} +QPushButton:hover { + background-color: rgb(80, 83, 85); +} +QPushButton:pressed { + background-color: rgb(45, 48, 50); +} + + + 保存 + + + + + + + + 80 + 30 + + + + + 80 + 30 + + + + + 13 + + + + color: rgb(221, 225, 233); +background-color: rgb(47, 48, 52); + + + 500.0 + + + Qt::AlignCenter + + + + + + + + 70 + 0 + + + + + 70 + 16777215 + + + + + 13 + + + + color: rgb(221, 225, 233); + + + X范围： + + + + + + + + 70 + 0 + + + + + 70 + 16777215 + + + + + 13 + + + + color: rgb(221, 225, 233); + + + Z范围： + + + + + + + + 80 + 30 + + + + + 80 + 30 + + + + + 13 + + + + color: rgb(221, 225, 233); +background-color: rgb(47, 48, 52); + + + -500.0 + + + Qt::AlignCenter + + + + + + + + 20 + 0 + + + + + 20 + 16777215 + + + + + 13 + + + + color: rgb(221, 225, 233); + + + ~ + + + Qt::AlignCenter + + + + + + + + 20 + 0 + + + + + 20 + 16777215 + + + + + 13 + + + + color: rgb(221, 225, 233); + + + ~ + + + Qt::AlignCenter + + + + + + + + 80 + 30 + + + + + 80 + 30 + + + + + 13 + + + + color: rgb(221, 225, 233); +background-color: rgb(47, 48, 52); + + + 1000.0 + + + Qt::AlignCenter + + + + + + + + 80 + 30 + + + + + 80 + 30 + + + + + 13 + + + + color: rgb(221, 225, 233); +background-color: rgb(47, 48, 52); + + + 1000.0 + + + Qt::AlignCenter + + + + + + + + 80 + 30 + + + + + 80 + 30 + + + + + 13 + + + + color: rgb(221, 225, 233); +background-color: rgb(47, 48, 52); + + + -1000.0 + + + Qt::AlignCenter + + + + + + + + 70 + 0 + + + + + 70 + 16777215 + + + + + 13 + + + + color: rgb(221, 225, 233); + + + Y范围： + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + diff --git a/App/WheelMeasure/WheelMeasureApp/main.cpp b/App/WheelMeasure/WheelMeasureApp/main.cpp index daced818..e8634b95 100644 --- a/App/WheelMeasure/WheelMeasureApp/main.cpp +++ b/App/WheelMeasure/WheelMeasureApp/main.cpp @@ -1,75 +1,82 @@ -#include "mainwindow.h" -#include "IWheelMeasureStatus.h" -#include "IVrWheelMeasureConfig.h" -#include "CrashHandler.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "Version.h" - -int main(int argc, char *argv[]) -{ - QApplication a(argc, argv); - - // 设置应用程序信息 - a.setApplicationName("WheelMeasureApp"); - a.setApplicationVersion(GetWheelMeasureFullVersion()); - a.setOrganizationName("VisionRobot"); - - // 注册Qt元类型 - qRegisterMetaType>("QVector"); - qRegisterMetaType>("QList"); - qRegisterMetaType("QAbstractItemModel::LayoutChangeHint"); - qRegisterMetaType("Qt::SortOrder"); - - // 注册自定义元类型 - qRegisterMetaType("WheelCameraParam"); - qRegisterMetaType("WheelCameraPlaneCalibParam"); - qRegisterMetaType("WheelMeasureConfigResult"); - qRegisterMetaType("WheelMeasureData"); - qRegisterMetaType("WheelMeasureResult"); - - // 单实例检查 - const QString appKey = "WheelMeasureApp_SingleInstance_Key"; - - QSystemSemaphore semaphore(appKey + "_semaphore", 1); - semaphore.acquire(); - - QSharedMemory sharedMemory(appKey + "_memory"); - - bool isRunning = false; - - if (sharedMemory.attach()) { - isRunning = true; - } else { - if (!sharedMemory.create(1)) { - qDebug() << "Unable to create shared memory segment:" << sharedMemory.errorString(); - isRunning = true; - } - } - - semaphore.release(); - - if (isRunning) { - QMessageBox::information(nullptr, - QObject::tr("应用程序已运行"), - QObject::tr("车轮拱高测量应用程序已经在运行中，请勿重复启动！"), - QMessageBox::Ok); - return 0; - } - - MainWindow w; - w.show(); - return a.exec(); -} +#include "mainwindow.h" +#include "IWheelMeasureStatus.h" +#include "IVrWheelMeasureConfig.h" +#include "CrashHandler.h" +#include "AuthView.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "Version.h" + +int main(int argc, char *argv[]) +{ + QApplication a(argc, argv); + + // 设置应用程序信息 + a.setApplicationName("WheelMeasureApp"); + a.setApplicationVersion(GetWheelMeasureFullVersion()); + a.setOrganizationName("VisionRobot"); + + // 注册Qt元类型 + qRegisterMetaType>("QVector"); + qRegisterMetaType>("QList"); + qRegisterMetaType("QAbstractItemModel::LayoutChangeHint"); + qRegisterMetaType("Qt::SortOrder"); + + // 注册自定义元类型 + qRegisterMetaType("WheelCameraParam"); + qRegisterMetaType("WheelCameraPlaneCalibParam"); + qRegisterMetaType("WheelMeasureConfigResult"); + qRegisterMetaType("WheelMeasureData"); + qRegisterMetaType("WheelMeasureResult"); + + // 单实例检查 + const QString appKey = "WheelMeasureApp_SingleInstance_Key"; + + QSystemSemaphore semaphore(appKey + "_semaphore", 1); + semaphore.acquire(); + + QSharedMemory sharedMemory(appKey + "_memory"); + + bool isRunning = false; + + if (sharedMemory.attach()) { + isRunning = true; + } else { + if (!sharedMemory.create(1)) { + qDebug() << "Unable to create shared memory segment:" << sharedMemory.errorString(); + isRunning = true; + } + } + + semaphore.release(); + + if (isRunning) { + QMessageBox::information(nullptr, + QObject::tr("应用程序已运行"), + QObject::tr("车轮拱高测量应用程序已经在运行中，请勿重复启动！"), + QMessageBox::Ok); + return 0; + } + + // 检查授权，无授权则显示授权对话框 + if (!AuthView::CheckAndShow(nullptr)) { + // 用户取消授权或授权失败，退出程序 + return 0; + } + + MainWindow w; + w.show(); + return a.exec(); +} diff --git a/App/WheelMeasure/WheelMeasureApp/mainwindow.cpp b/App/WheelMeasure/WheelMeasureApp/mainwindow.cpp index a102dc08..9cc6afcb 100644 --- a/App/WheelMeasure/WheelMeasureApp/mainwindow.cpp +++ b/App/WheelMeasure/WheelMeasureApp/mainwindow.cpp @@ -1,561 +1,563 @@ -#include "mainwindow.h" -#include "ui_mainwindow.h" -#include "Version.h" -#include "widgets/ImageGridWidget.h" -#include "widgets/DeviceStatusWidget.h" -#include "widgets/MeasureResultListWidget.h" -#include "dialogcameralevel.h" -#include "dialogalgoarg.h" -#include "dialogcamera.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "VrLog.h" -#include "IVrUtils.h" -#include "StyledMessageBox.h" - -MainWindow::MainWindow(QWidget *parent) - : QMainWindow(parent) - , ui(new Ui::MainWindow) -{ - ui->setupUi(this); - - // 设置窗口图标 - this->setWindowIcon(QIcon(":/common/resource/logo.png")); - - // 设置状态栏字体 - QFont statusFont = statusBar()->font(); - statusFont.setPointSize(12); - statusBar()->setFont(statusFont); - - // 设置状态栏颜色和padding - statusBar()->setStyleSheet("QStatusBar { color: rgb(239, 241, 245); padding: 20px; }"); - - // 隐藏标题栏 - setWindowFlags(Qt::FramelessWindowHint); - - // 启动后自动最大化显示 - this->showMaximized(); - - // 初始化时隐藏工作状态标签 - ui->label_work->setVisible(false); - - // 创建组合控件 - m_gridView = new ImageGridWidget(); - m_deviceStatusWidget = new DeviceStatusWidget(); - m_measureResultWidget = new MeasureResultListWidget(); - - // 将设备状态widget添加到frame_dev中 - QVBoxLayout* frameResultImageLayout = new QVBoxLayout(ui->detect_result); - frameResultImageLayout->setContentsMargins(0, 0, 0, 0); - frameResultImageLayout->addWidget(m_gridView); - - // 将MeasureResultListWidget添加到detect_data中 - QVBoxLayout* frameDataLayout = new QVBoxLayout(ui->detect_data); - frameDataLayout->setContentsMargins(0, 0, 0, 0); - frameDataLayout->addWidget(m_measureResultWidget); - - QVBoxLayout* frameDevLayout = new QVBoxLayout(ui->device_status); - frameDevLayout->setContentsMargins(0, 0, 0, 0); - frameDevLayout->addWidget(m_deviceStatusWidget); - - // 初始化Presenter - m_presenter = new WheelMeasurePresenter(); - - // 将设备状态控件与Presenter关联 - m_presenter->setStatusUpdate(this); - - // 连接配置更新信号 - connect(m_presenter, &WheelMeasurePresenter::configUpdated, this, &MainWindow::onConfigSaved); - - // 连接设备点击信号到重新检测槽函数 - connect(m_deviceStatusWidget, &DeviceStatusWidget::deviceClicked, this, &MainWindow::onDeviceClicked); - - // 连接图像右键点击信号到保存数据槽函数 - connect(m_gridView, &ImageGridWidget::tileRightClicked, this, &MainWindow::onTileRightClicked); - - // 设置版本信息显示 - setupVersionDisplay(); - - // 初始化日志辅助类 - m_logHelper = new DetectLogHelper(ui->detect_log, this); - - m_presenter->Init(); -} - -MainWindow::~MainWindow() -{ - // 先清除回调，防止后台线程继续调用 - if (m_presenter) { - m_presenter->setStatusUpdate(nullptr); - } - - if (m_gridView) { - delete m_gridView; - m_gridView = nullptr; - } - - if (m_deviceStatusWidget) { - delete m_deviceStatusWidget; - m_deviceStatusWidget = nullptr; - } - - if (m_measureResultWidget) { - delete m_measureResultWidget; - m_measureResultWidget = nullptr; - } - - if (m_versionLabel) { - delete m_versionLabel; - m_versionLabel = nullptr; - } - - if (m_dialogAlgoArg) { - delete m_dialogAlgoArg; - m_dialogAlgoArg = nullptr; - } - - if (m_dialogCamera) { - delete m_dialogCamera; - m_dialogCamera = nullptr; - } - - if (m_dialogCameraLevel) { - delete m_dialogCameraLevel; - m_dialogCameraLevel = nullptr; - } - - if (m_presenter) { - delete m_presenter; - m_presenter = nullptr; - } - - delete ui; - LOG_DEBUG("~MainWindow finish \n"); -} - -void MainWindow::resizeEvent(QResizeEvent* event) -{ - QMainWindow::resizeEvent(event); -} - -void MainWindow::on_btn_hide_clicked() -{ - this->showMinimized(); -} - -void MainWindow::on_btn_close_clicked() -{ - this->close(); -} - -void MainWindow::on_btn_test_clicked() -{ - if (!m_presenter) { - QMessageBox::warning(this, "错误", "系统未初始化完成！"); - return; - } - - // 设置选中状态 - ui->btn_test->setStyleSheet( - "QPushButton { image: url(:/common/resource/config_data_test_s.png); background-color: rgb(38, 40, 47); border: none; }" - ); - - // 打开文件选择对话框 - QString fileName = QFileDialog::getOpenFileName( - this, - tr("选择调试数据文件"), - QString(), - tr("激光数据文件 (*.txt);;所有文件 (*.*)") - ); - - // 恢复未选中状态 - ui->btn_test->setStyleSheet( - "QPushButton { image: url(:/common/resource/config_data_test.png); background-color: rgb(38, 40, 47); border: none; }" - "QPushButton:pressed { image: url(:/common/resource/config_data_test_s.png); }" - ); - - if (fileName.isEmpty()) { - return; - } - - // 清空当前检测数据列表 - if (m_measureResultWidget) { - m_measureResultWidget->clearAllResults(); - } - - // if (m_logHelper) m_logHelper->appendLog(QString("正在加载调试数据: %1").arg(fileName)); - - // 在后台线程中执行加载和检测 - std::thread t([this, fileName]() { - int result = m_presenter->LoadDebugDataAndDetect(fileName.toStdString()); - if (result == 0) { - QMetaObject::invokeMethod(this, [this]() { - if (m_logHelper) m_logHelper->appendLog("调试数据加载和检测成功"); - }, Qt::QueuedConnection); - } else { - QMetaObject::invokeMethod(this, [this, result]() { - if (m_logHelper) m_logHelper->appendLog(QString("调试数据复检失败: %1").arg(result)); - }, Qt::QueuedConnection); - } - }); - t.detach(); -} - -void MainWindow::on_btn_start_clicked() -{ - if (!m_presenter) { - QMessageBox::warning(this, "错误", "系统未初始化完成！"); - return; - } - - // 启动所有相机的检测 - m_presenter->StartAllDetection(); - if (m_logHelper) m_logHelper->appendLog("已启动所有相机检测"); -} - -void MainWindow::on_btn_stop_clicked() -{ - if (!m_presenter) { - return; - } - - // 停止所有相机的检测 - m_presenter->StopAllDetection(); - if (m_logHelper) m_logHelper->appendLog("已停止所有相机检测"); -} - -void MainWindow::on_btn_camera_config_clicked() -{ - if (!m_presenter) { - QMessageBox::warning(this, "错误", "系统未初始化完成！"); - return; - } - - if (!m_dialogCamera) { - m_dialogCamera = new DialogCamera(this); - m_dialogCamera->SetPresenter(m_presenter); - connect(m_dialogCamera, &DialogCamera::configSaved, this, &MainWindow::onConfigSaved); - // 连接关闭信号恢复按钮状态 - connect(m_dialogCamera, &QDialog::finished, this, [this]() { - ui->btn_camera_config->setStyleSheet( - "QPushButton { image: url(:/common/resource/config_camera.png); background-color: rgb(38, 40, 47); border: none; }" - "QPushButton:pressed { image: url(:/common/resource/config_camera_s.png); }" - ); - }); - } - - // 设置选中状态 - ui->btn_camera_config->setStyleSheet( - "QPushButton { image: url(:/common/resource/config_camera_s.png); background-color: rgb(38, 40, 47); border: none; }" - ); - m_dialogCamera->show(); -} - -void MainWindow::on_btn_algo_config_clicked() -{ - if (!m_presenter) { - QMessageBox::warning(this, "错误", "系统未初始化完成！"); - return; - } - - if (!m_dialogAlgoArg) { - m_dialogAlgoArg = new DialogAlgoArg(m_presenter->GetConfig(), - m_presenter->GetConfigResult(), this); - // 连接关闭信号恢复按钮状态 - connect(m_dialogAlgoArg, &QDialog::finished, this, [this]() { - ui->btn_algo_config->setStyleSheet( - "QPushButton { image: url(:/common/resource/config_algo.png); background-color: rgb(38, 40, 47); border: none; }" - "QPushButton:pressed { image: url(:/common/resource/config_algo_s.png); }" - ); - }); - } - - // 设置选中状态 - ui->btn_algo_config->setStyleSheet( - "QPushButton { image: url(:/common/resource/config_algo_s.png); background-color: rgb(38, 40, 47); border: none; }" - ); - m_dialogAlgoArg->Init(); - m_dialogAlgoArg->show(); -} - -void MainWindow::on_btn_camera_level_clicked() -{ - if (!m_presenter) { - QMessageBox::warning(this, "错误", "系统未初始化完成！"); - return; - } - - if (!m_dialogCameraLevel) { - m_dialogCameraLevel = new DialogCameraLevel(this); - // 连接关闭信号恢复按钮状态 - connect(m_dialogCameraLevel, &QDialog::finished, this, [this]() { - ui->btn_camera_level->setStyleSheet( - "QPushButton { image: url(:/common/resource/config_camera_level.png); background-color: rgb(38, 40, 47); border: none; }" - "QPushButton:pressed { image: url(:/common/resource/config_camera_level_s.png); }" - ); - }); - } - - // 设置相机列表 - m_dialogCameraLevel->setCameraList(m_presenter->GetCameraList(), m_presenter); - - // 设置配置对象 - m_dialogCameraLevel->setConfig(m_presenter->GetConfig(), m_presenter->GetConfigResult()); - - // 设置选中状态 - ui->btn_camera_level->setStyleSheet( - "QPushButton { image: url(:/common/resource/config_camera_level_s.png); background-color: rgb(38, 40, 47); border: none; }" - ); - m_dialogCameraLevel->show(); -} - -void MainWindow::onDeviceClicked(const QString& deviceName) -{ - if (!m_presenter) { - QMessageBox::warning(this, "错误", "系统未初始化完成！"); - return; - } - - // 查找相机索引 - auto cameraList = m_presenter->GetCameraList(); - int foundIndex = -1; - for (int i = 0; i < static_cast(cameraList.size()); ++i) { - if (QString::fromStdString(cameraList[i].first) == deviceName) { - foundIndex = i; - break; - } - } - - if (foundIndex < 0) { - if (m_logHelper) m_logHelper->appendLog(QString("未找到相机: %1").arg(deviceName)); - return; - } - - // 清空指定设备的检测结果 - if (m_measureResultWidget) { - m_measureResultWidget->clearDeviceResult(deviceName); - } - - // 直接开始检测 - m_presenter->ResetDetect(foundIndex); - if (m_logHelper) m_logHelper->appendLog(QString("已启动相机 \"%1\" 的检测").arg(deviceName)); -} - -void MainWindow::onConfigSaved() -{ - // 配置已在 WheelMeasurePresenter::OnConfigChanged 中更新 - // 算法参数和调平参数的修改不需要重新初始化相机 - LOG_INFO("Config saved, configuration updated in memory\n"); -} - -void MainWindow::onTileRightClicked(int index, const QString& alias) -{ - if (!m_presenter) { - StyledMessageBox::warning(this, "错误", "系统未初始化完成！"); - return; - } - - // 获取当前检测的相机索引（0-based） - int currentDetectIndex = m_presenter->GetDetectIndex() - 1; // 转换为0-based - - // 检查点击的是否是当前检测的相机 - if (index != currentDetectIndex) { - if (m_logHelper) m_logHelper->appendLog(QString("设备 \"%1\" 无缓存数据").arg(alias)); - StyledMessageBox::information(this, "提示", - QString("设备 \"%1\" 无缓存数据\n\n当前缓存的是设备 %2 的数据") - .arg(alias) - .arg(currentDetectIndex >= 0 ? m_gridView->getAlias(currentDetectIndex) : "无")); - return; - } - - // 检查是否有缓存数据 - int cacheSize = m_presenter->GetDetectionDataCacheSize(); - if (cacheSize <= 0) { - if (m_logHelper) m_logHelper->appendLog(QString("设备 \"%1\" 无缓存数据").arg(alias)); - StyledMessageBox::information(this, "提示", QString("设备 \"%1\" 无缓存数据").arg(alias)); - return; - } - - // 弹出文件保存对话框 - QString defaultFileName = QString("%1_%2.txt") - .arg(alias) - .arg(QDateTime::currentDateTime().toString("yyyyMMdd_HHmmss")); - - QString filePath = QFileDialog::getSaveFileName( - this, - tr("保存检测数据"), - defaultFileName, - tr("文本文件 (*.txt);;所有文件 (*.*)") - ); - - if (filePath.isEmpty()) { - return; - } - - // 调用Presenter保存数据 - int result = m_presenter->SaveDetectionDataToFile(filePath.toStdString()); - if (result == 0) { - if (m_logHelper) m_logHelper->appendLog(QString("检测数据已保存: %1").arg(filePath)); - StyledMessageBox::information(this, "成功", QString("检测数据已保存到:\n%1").arg(filePath)); - } else { - if (m_logHelper) m_logHelper->appendLog(QString("保存检测数据失败: %1").arg(result)); - StyledMessageBox::warning(this, "失败", QString("保存检测数据失败，错误码: %1").arg(result)); - } -} - -// IWheelMeasureStatus接口方法实现 -void MainWindow::OnStatusUpdate(const QString &statusMessage) -{ - if (m_logHelper) m_logHelper->appendLog(statusMessage); - LOG_DEBUG("Status update: %s \n", statusMessage.toStdString().c_str()); -} - -void MainWindow::OnNeedShowImageCount(const QStringList &cameraNames) -{ - if (cameraNames.isEmpty()) { - m_gridView->initImages(0); - LOG_DEBUG("No images to display\n"); - } else { - m_gridView->initImages(cameraNames.size()); - - // 为每个tile设置别名 - for (int i = 0; i < cameraNames.size(); ++i) { - m_gridView->setTileAlias(i, cameraNames.at(i)); - } - - // 初始化设备状态列表 - if (m_deviceStatusWidget && m_presenter) { - QList devices; - for (const QString& name : cameraNames) { - devices.append(WheelDeviceDisplayInfo(name, name, "", DeviceStatus::Offline, true)); - } - m_deviceStatusWidget->setDevices(devices); - } - - // 初始化检测结果列表 - if (m_measureResultWidget) { - m_measureResultWidget->setDeviceList(cameraNames); - } - } -} - -void MainWindow::OnMeasureResult(const WheelMeasureResult &result) -{ - // 直接调用处理方法（Presenter在后台线程，需要使用invokeMethod确保线程安全） - QMetaObject::invokeMethod(this, [this, result]() { - // 如果图像有效，显示在网格控件中 - if (result.bImageValid && !result.image.isNull()) { - m_gridView->setImages(result.aliasName, result.image); - } - - // 更新设备检测结果列表 - if (m_measureResultWidget) { - // 获取第一个结果数据（如果有的话） - WheelMeasureData data; - if (!result.result.empty()) { - data = result.result[0]; - } - m_measureResultWidget->updateDeviceResult(result.cameraName, data, result.bResultValid); - } - }, Qt::QueuedConnection); -} - -void MainWindow::OnCameraConnected(const QString &cameraName) -{ - QString message = QString("相机已连接: %1").arg(cameraName); - if (m_logHelper) m_logHelper->appendLog(message); - LOG_DEBUG("%s \n", message.toStdString().c_str()); -} - -void MainWindow::OnCameraDisconnected(const QString &cameraName) -{ - QString message = QString("相机断开连接: %1").arg(cameraName); - if (m_logHelper) m_logHelper->appendLog(message); - LOG_DEBUG("%s \n", message.toStdString().c_str()); -} - -void MainWindow::OnWorkStatusChangedImpl(WorkStatus status) -{ - QString statusStr; - switch (status) { - case WorkStatus::InitIng: - statusStr = "初始化中"; - break; - case WorkStatus::Ready: - statusStr = "准备就绪"; - break; - case WorkStatus::Working: - statusStr = "正在检测"; - break; - case WorkStatus::Detecting: - statusStr = "正在检测"; - break; - case WorkStatus::Completed: - statusStr = "检测完成"; - break; - case WorkStatus::Error: - statusStr = "设备异常"; - break; - default: - statusStr = "未知状态"; - break; - } - - QString message = QString("工作状态: %1").arg(statusStr); - if (m_logHelper) m_logHelper->appendLog(message); - LOG_DEBUG("%s \n", message.toStdString().c_str()); -} - -void MainWindow::OnErrorOccurred(const QString &errorMessage) -{ - if (m_logHelper) m_logHelper->appendLog("错误: " + errorMessage); - LOG_ERROR("Error occurred: %s \n", errorMessage.toStdString().c_str()); -} - -void MainWindow::OnDeviceStatusChanged(const QString &deviceName, int deviceStatus) -{ - if (m_deviceStatusWidget) { - m_deviceStatusWidget->updateDeviceStatus(deviceName, static_cast(deviceStatus)); - } -} - -void MainWindow::OnClearMeasureData() -{ - if (m_measureResultWidget) { - m_measureResultWidget->clearAllResults(); - } -} - -void MainWindow::setupVersionDisplay() -{ - m_versionLabel = new QLabel(this); - - // 使用与 Workpiece 一致的版本格式 - QString versionText = QString("%1_%2%3%4%5%6%7") - .arg(GetWheelMeasureFullVersion()) - .arg(YEAR) - .arg(MONTH, 2, 10, QChar('0')) - .arg(DAY, 2, 10, QChar('0')) - .arg(HOUR, 2, 10, QChar('0')) - .arg(MINUTE, 2, 10, QChar('0')) - .arg(SECOND, 2, 10, QChar('0')); - - m_versionLabel->setText(versionText); - m_versionLabel->setAlignment(Qt::AlignRight | Qt::AlignVCenter); - - m_versionLabel->setStyleSheet("QLabel { color: rgb(239, 241, 245); font-size: 24px; margin-right: 10px; }"); - - statusBar()->addPermanentWidget(m_versionLabel); - - LOG_INFO("Version display initialized: %s\n", versionText.toStdString().c_str()); -} +#include "mainwindow.h" +#include "ui_mainwindow.h" +#include "Version.h" +#include "widgets/ImageGridWidget.h" +#include "widgets/DeviceStatusWidget.h" +#include "widgets/MeasureResultListWidget.h" +#include "dialogcameralevel.h" +#include "dialogalgoarg.h" +#include "dialogcamera.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "VrLog.h" +#include "IVrUtils.h" +#include "StyledMessageBox.h" + +MainWindow::MainWindow(QWidget *parent) + : QMainWindow(parent) + , ui(new Ui::MainWindow) +{ + ui->setupUi(this); + + // 设置窗口图标 + this->setWindowIcon(QIcon(":/common/resource/logo.png")); + + // 设置状态栏字体 + QFont statusFont = statusBar()->font(); + statusFont.setPointSize(12); + statusBar()->setFont(statusFont); + + // 设置状态栏颜色和padding + statusBar()->setStyleSheet("QStatusBar { color: rgb(239, 241, 245); padding: 20px; }"); + + // 隐藏标题栏 + setWindowFlags(Qt::FramelessWindowHint); + + // 启动后自动最大化显示 + this->showMaximized(); + + // 初始化时隐藏工作状态标签 + ui->label_work->setVisible(false); + + // 创建组合控件 + m_gridView = new ImageGridWidget(); + m_deviceStatusWidget = new DeviceStatusWidget(); + m_measureResultWidget = new MeasureResultListWidget(); + + // 将设备状态widget添加到frame_dev中 + QVBoxLayout* frameResultImageLayout = new QVBoxLayout(ui->detect_result); + frameResultImageLayout->setContentsMargins(0, 0, 0, 0); + frameResultImageLayout->addWidget(m_gridView); + + // 将MeasureResultListWidget添加到detect_data中 + QVBoxLayout* frameDataLayout = new QVBoxLayout(ui->detect_data); + frameDataLayout->setContentsMargins(0, 0, 0, 0); + frameDataLayout->addWidget(m_measureResultWidget); + + QVBoxLayout* frameDevLayout = new QVBoxLayout(ui->device_status); + frameDevLayout->setContentsMargins(0, 0, 0, 0); + frameDevLayout->addWidget(m_deviceStatusWidget); + + // 初始化Presenter + m_presenter = new WheelMeasurePresenter(); + + // 将设备状态控件与Presenter关联 + m_presenter->setStatusUpdate(this); + + // 连接配置更新信号 + connect(m_presenter, &WheelMeasurePresenter::configUpdated, this, &MainWindow::onConfigSaved); + + // 连接设备点击信号到重新检测槽函数 + connect(m_deviceStatusWidget, &DeviceStatusWidget::deviceClicked, this, &MainWindow::onDeviceClicked); + + // 连接图像右键点击信号到保存数据槽函数 + connect(m_gridView, &ImageGridWidget::tileRightClicked, this, &MainWindow::onTileRightClicked); + + // 设置版本信息显示 + setupVersionDisplay(); + + // 初始化日志辅助类 + m_logHelper = new DetectLogHelper(ui->detect_log, this); + + m_presenter->Init(); +} + +MainWindow::~MainWindow() +{ + // 先清除回调，防止后台线程继续调用 + if (m_presenter) { + m_presenter->setStatusUpdate(nullptr); + } + + if (m_gridView) { + delete m_gridView; + m_gridView = nullptr; + } + + if (m_deviceStatusWidget) { + delete m_deviceStatusWidget; + m_deviceStatusWidget = nullptr; + } + + if (m_measureResultWidget) { + delete m_measureResultWidget; + m_measureResultWidget = nullptr; + } + + if (m_versionLabel) { + delete m_versionLabel; + m_versionLabel = nullptr; + } + + if (m_dialogAlgoArg) { + delete m_dialogAlgoArg; + m_dialogAlgoArg = nullptr; + } + + if (m_dialogCamera) { + delete m_dialogCamera; + m_dialogCamera = nullptr; + } + + if (m_dialogCameraLevel) { + delete m_dialogCameraLevel; + m_dialogCameraLevel = nullptr; + } + + if (m_presenter) { + delete m_presenter; + m_presenter = nullptr; + } + + delete ui; + LOG_DEBUG("~MainWindow finish \n"); +} + +void MainWindow::resizeEvent(QResizeEvent* event) +{ + QMainWindow::resizeEvent(event); +} + +void MainWindow::on_btn_hide_clicked() +{ + this->showMinimized(); +} + +void MainWindow::on_btn_close_clicked() +{ + this->close(); +} + +void MainWindow::on_btn_test_clicked() +{ + if (!m_presenter) { + QMessageBox::warning(this, "错误", "系统未初始化完成！"); + return; + } + + // 设置选中状态 + ui->btn_test->setStyleSheet( + "QPushButton { image: url(:/common/resource/config_data_test_s.png); background-color: rgb(38, 40, 47); border: none; }" + ); + + // 打开文件选择对话框 + QString fileName = QFileDialog::getOpenFileName( + this, + tr("选择调试数据文件"), + QString(), + tr("激光数据文件 (*.txt);;所有文件 (*.*)") + ); + + // 恢复未选中状态 + ui->btn_test->setStyleSheet( + "QPushButton { image: url(:/common/resource/config_data_test.png); background-color: rgb(38, 40, 47); border: none; }" + "QPushButton:pressed { image: url(:/common/resource/config_data_test_s.png); }" + ); + + if (fileName.isEmpty()) { + return; + } + + // 清空当前检测数据列表 + if (m_measureResultWidget) { + m_measureResultWidget->clearAllResults(); + } + + // if (m_logHelper) m_logHelper->appendLog(QString("正在加载调试数据: %1").arg(fileName)); + + // 在后台线程中执行加载和检测 + std::thread t([this, fileName]() { + int result = m_presenter->LoadDebugDataAndDetect(fileName.toStdString()); + if (result == 0) { + QMetaObject::invokeMethod(this, [this]() { + if (m_logHelper) m_logHelper->appendLog("调试数据加载和检测成功"); + }, Qt::QueuedConnection); + } else { + QMetaObject::invokeMethod(this, [this, result]() { + if (m_logHelper) m_logHelper->appendLog(QString("调试数据复检失败: %1").arg(result)); + }, Qt::QueuedConnection); + } + }); + t.detach(); +} + +void MainWindow::on_btn_start_clicked() +{ + if (!m_presenter) { + QMessageBox::warning(this, "错误", "系统未初始化完成！"); + return; + } + + // 启动所有相机的检测 + m_presenter->StartAllDetection(); + if (m_logHelper) m_logHelper->appendLog("已启动所有相机检测"); +} + +void MainWindow::on_btn_stop_clicked() +{ + if (!m_presenter) { + return; + } + + // 停止所有相机的检测 + m_presenter->StopAllDetection(); + if (m_logHelper) m_logHelper->appendLog("已停止所有相机检测"); +} + +void MainWindow::on_btn_camera_config_clicked() +{ + if (!m_presenter) { + QMessageBox::warning(this, "错误", "系统未初始化完成！"); + return; + } + + if (!m_dialogCamera) { + m_dialogCamera = new DialogCamera(this); + m_dialogCamera->SetPresenter(m_presenter); + connect(m_dialogCamera, &DialogCamera::configSaved, this, &MainWindow::onConfigSaved); + // 连接关闭信号恢复按钮状态 + connect(m_dialogCamera, &QDialog::finished, this, [this]() { + ui->btn_camera_config->setStyleSheet( + "QPushButton { image: url(:/common/resource/config_camera.png); background-color: rgb(38, 40, 47); border: none; }" + "QPushButton:pressed { image: url(:/common/resource/config_camera_s.png); }" + ); + }); + } + + // 设置选中状态 + ui->btn_camera_config->setStyleSheet( + "QPushButton { image: url(:/common/resource/config_camera_s.png); background-color: rgb(38, 40, 47); border: none; }" + ); + m_dialogCamera->show(); +} + +void MainWindow::on_btn_algo_config_clicked() +{ + if (!m_presenter) { + QMessageBox::warning(this, "错误", "系统未初始化完成！"); + return; + } + + if (!m_dialogAlgoArg) { + m_dialogAlgoArg = new DialogAlgoArg(m_presenter->GetConfig(), + m_presenter->GetConfigResult(), this); + // 连接关闭信号恢复按钮状态 + connect(m_dialogAlgoArg, &QDialog::finished, this, [this]() { + ui->btn_algo_config->setStyleSheet( + "QPushButton { image: url(:/common/resource/config_algo.png); background-color: rgb(38, 40, 47); border: none; }" + "QPushButton:pressed { image: url(:/common/resource/config_algo_s.png); }" + ); + }); + } + + // 设置选中状态 + ui->btn_algo_config->setStyleSheet( + "QPushButton { image: url(:/common/resource/config_algo_s.png); background-color: rgb(38, 40, 47); border: none; }" + ); + m_dialogAlgoArg->Init(); + m_dialogAlgoArg->show(); +} + +void MainWindow::on_btn_camera_level_clicked() +{ + if (!m_presenter) { + QMessageBox::warning(this, "错误", "系统未初始化完成！"); + return; + } + + if (!m_dialogCameraLevel) { + m_dialogCameraLevel = new DialogCameraLevel(this); + // 连接关闭信号恢复按钮状态 + connect(m_dialogCameraLevel, &QDialog::finished, this, [this]() { + ui->btn_camera_level->setStyleSheet( + "QPushButton { image: url(:/common/resource/config_camera_level.png); background-color: rgb(38, 40, 47); border: none; }" + "QPushButton:pressed { image: url(:/common/resource/config_camera_level_s.png); }" + ); + }); + } + + // 设置相机列表 + m_dialogCameraLevel->setCameraList(m_presenter->GetCameraList(), m_presenter); + + // 设置配置对象 + m_dialogCameraLevel->setConfig(m_presenter->GetConfig(), m_presenter->GetConfigResult()); + + // 设置选中状态 + ui->btn_camera_level->setStyleSheet( + "QPushButton { image: url(:/common/resource/config_camera_level_s.png); background-color: rgb(38, 40, 47); border: none; }" + ); + m_dialogCameraLevel->show(); +} + +void MainWindow::onDeviceClicked(const QString& deviceName) +{ + if (!m_presenter) { + QMessageBox::warning(this, "错误", "系统未初始化完成！"); + return; + } + + // 查找相机索引 + auto cameraList = m_presenter->GetCameraList(); + int foundIndex = -1; + for (int i = 0; i < static_cast(cameraList.size()); ++i) { + if (QString::fromStdString(cameraList[i].first) == deviceName) { + foundIndex = i; + break; + } + } + + if (foundIndex < 0) { + if (m_logHelper) m_logHelper->appendLog(QString("未找到相机: %1").arg(deviceName)); + return; + } + + // 清空指定设备的检测结果 + if (m_measureResultWidget) { + m_measureResultWidget->clearDeviceResult(deviceName); + } + + // 直接开始检测 + m_presenter->ResetDetect(foundIndex); + if (m_logHelper) m_logHelper->appendLog(QString("已启动相机 \"%1\" 的检测").arg(deviceName)); +} + +void MainWindow::onConfigSaved() +{ + // 配置已在 WheelMeasurePresenter::OnConfigChanged 中更新 + // 算法参数和调平参数的修改不需要重新初始化相机 + LOG_INFO("Config saved, configuration updated in memory\n"); +} + +void MainWindow::onTileRightClicked(int index, const QString& alias) +{ + if (!m_presenter) { + StyledMessageBox::warning(this, "错误", "系统未初始化完成！"); + return; + } + + // 获取当前检测的相机索引（0-based） + int currentDetectIndex = m_presenter->GetDetectIndex() - 1; // 转换为0-based + + // 检查点击的是否是当前检测的相机 + if (index != currentDetectIndex) { + if (m_logHelper) m_logHelper->appendLog(QString("设备 \"%1\" 无缓存数据").arg(alias)); + StyledMessageBox::information(this, "提示", + QString("设备 \"%1\" 无缓存数据\n\n当前缓存的是设备 %2 的数据") + .arg(alias) + .arg(currentDetectIndex >= 0 ? m_gridView->getAlias(currentDetectIndex) : "无")); + return; + } + + // 检查是否有缓存数据 + int cacheSize = m_presenter->GetDetectionDataCacheSize(); + if (cacheSize <= 0) { + if (m_logHelper) m_logHelper->appendLog(QString("设备 \"%1\" 无缓存数据").arg(alias)); + StyledMessageBox::information(this, "提示", QString("设备 \"%1\" 无缓存数据").arg(alias)); + return; + } + + // 弹出文件保存对话框 + QString defaultFileName = QString("%1_%2.txt") + .arg(alias) + .arg(QDateTime::currentDateTime().toString("yyyyMMdd_HHmmss")); + + QString filePath = QFileDialog::getSaveFileName( + this, + tr("保存检测数据"), + defaultFileName, + tr("文本文件 (*.txt);;所有文件 (*.*)") + ); + + if (filePath.isEmpty()) { + return; + } + + // 调用Presenter保存数据 + int result = m_presenter->SaveDetectionDataToFile(filePath.toStdString()); + if (result == 0) { + if (m_logHelper) m_logHelper->appendLog(QString("检测数据已保存: %1").arg(filePath)); + StyledMessageBox::information(this, "成功", QString("检测数据已保存到:\n%1").arg(filePath)); + } else { + if (m_logHelper) m_logHelper->appendLog(QString("保存检测数据失败: %1").arg(result)); + StyledMessageBox::warning(this, "失败", QString("保存检测数据失败，错误码: %1").arg(result)); + } +} + +// IWheelMeasureStatus接口方法实现 +void MainWindow::OnStatusUpdate(const QString &statusMessage) +{ + if (m_logHelper) m_logHelper->appendLog(statusMessage); + LOG_DEBUG("Status update: %s \n", statusMessage.toStdString().c_str()); +} + +void MainWindow::OnNeedShowImageCount(const QStringList &cameraNames) +{ + if (cameraNames.isEmpty()) { + m_gridView->initImages(0); + LOG_DEBUG("No images to display\n"); + } else { + m_gridView->initImages(cameraNames.size()); + + // 为每个tile设置别名 + for (int i = 0; i < cameraNames.size(); ++i) { + m_gridView->setTileAlias(i, cameraNames.at(i)); + } + + // 初始化设备状态列表 + if (m_deviceStatusWidget && m_presenter) { + QList devices; + for (const QString& name : cameraNames) { + devices.append(WheelDeviceDisplayInfo(name, name, "", DeviceStatus::Offline, true)); + } + m_deviceStatusWidget->setDevices(devices); + } + + // 初始化检测结果列表 + if (m_measureResultWidget) { + m_measureResultWidget->setDeviceList(cameraNames); + } + } +} + +void MainWindow::OnMeasureResult(const WheelMeasureResult &result) +{ + // 直接调用处理方法（Presenter在后台线程，需要使用invokeMethod确保线程安全） + QMetaObject::invokeMethod(this, [this, result]() { + // 如果图像有效，显示在网格控件中 + if (result.bImageValid && !result.image.isNull()) { + m_gridView->setImages(result.aliasName, result.image); + } + + // 更新设备检测结果列表 + if (m_measureResultWidget) { + // 获取第一个结果数据（如果有的话） + WheelMeasureData data; + if (!result.result.empty()) { + data = result.result[0]; + } + m_measureResultWidget->updateDeviceResult(result.cameraName, data, result.bResultValid); + } + }, Qt::QueuedConnection); +} + +void MainWindow::OnCameraConnected(const QString &cameraName) +{ + QString message = QString("相机已连接: %1").arg(cameraName); + if (m_logHelper) m_logHelper->appendLog(message); + LOG_DEBUG("%s \n", message.toStdString().c_str()); +} + +void MainWindow::OnCameraDisconnected(const QString &cameraName) +{ + QString message = QString("相机断开连接: %1").arg(cameraName); + if (m_logHelper) m_logHelper->appendLog(message); + LOG_DEBUG("%s \n", message.toStdString().c_str()); +} + +void MainWindow::OnWorkStatusChangedImpl(WorkStatus status) +{ + QString statusStr; + switch (status) { + case WorkStatus::InitIng: + statusStr = "初始化中"; + break; + case WorkStatus::Ready: + statusStr = "准备就绪"; + break; + case WorkStatus::Working: + statusStr = "正在检测"; + break; + case WorkStatus::Detecting: + statusStr = "正在检测"; + break; + case WorkStatus::Completed: + statusStr = "检测完成"; + break; + case WorkStatus::Error: + statusStr = "设备异常"; + break; + default: + statusStr = "未知状态"; + break; + } + + QString message = QString("工作状态: %1").arg(statusStr); + if (m_logHelper) m_logHelper->appendLog(message); + LOG_DEBUG("%s \n", message.toStdString().c_str()); +} + +void MainWindow::OnErrorOccurred(const QString &errorMessage) +{ + if (m_logHelper) m_logHelper->appendLog("错误: " + errorMessage); + LOG_ERROR("Error occurred: %s \n", errorMessage.toStdString().c_str()); +} + +void MainWindow::OnDeviceStatusChanged(const QString &deviceName, int deviceStatus) +{ + if (m_deviceStatusWidget) { + m_deviceStatusWidget->updateDeviceStatus(deviceName, static_cast(deviceStatus)); + } +} + +void MainWindow::OnClearMeasureData() +{ + if (m_logHelper) m_logHelper->clearLog(); + + if (m_measureResultWidget) { + m_measureResultWidget->clearAllResults(); + } +} + +void MainWindow::setupVersionDisplay() +{ + m_versionLabel = new QLabel(this); + + // 使用与 Workpiece 一致的版本格式 + QString versionText = QString("%1_%2%3%4%5%6%7") + .arg(GetWheelMeasureFullVersion()) + .arg(YEAR) + .arg(MONTH, 2, 10, QChar('0')) + .arg(DAY, 2, 10, QChar('0')) + .arg(HOUR, 2, 10, QChar('0')) + .arg(MINUTE, 2, 10, QChar('0')) + .arg(SECOND, 2, 10, QChar('0')); + + m_versionLabel->setText(versionText); + m_versionLabel->setAlignment(Qt::AlignRight | Qt::AlignVCenter); + + m_versionLabel->setStyleSheet("QLabel { color: rgb(239, 241, 245); font-size: 24px; margin-right: 10px; }"); + + statusBar()->addPermanentWidget(m_versionLabel); + + LOG_INFO("Version display initialized: %s\n", versionText.toStdString().c_str()); +} diff --git a/App/WheelMeasure/WheelMeasureConfig/Inc/IVrWheelMeasureConfig.h b/App/WheelMeasure/WheelMeasureConfig/Inc/IVrWheelMeasureConfig.h index 991c79c4..96deaa67 100644 --- a/App/WheelMeasure/WheelMeasureConfig/Inc/IVrWheelMeasureConfig.h +++ b/App/WheelMeasure/WheelMeasureConfig/Inc/IVrWheelMeasureConfig.h @@ -1,236 +1,246 @@ -#ifndef IVRWHEELMEASURECONFIG_H -#define IVRWHEELMEASURECONFIG_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include "VrCommonConfig.h" // 包含公共配置结构体 - -/** - * @brief 数据类型枚举 - */ -enum class WheelMeasureDataType { - Text = 0x01, - Image = 0x02, - ReadConfig = 0x03, - WriteConfig = 0x04, -}; - -/** - * @brief 服务器信息结构 - */ -struct WheelServerInfo -{ - std::string name; // 服务器名称 - std::string ip; // 服务器IP地址 - int port = 5800; // 服务器端口 -}; - -/** - * @brief 相机配置参数 - */ -struct WheelCameraParam -{ - int cameraIndex = 0; // 相机索引（1-based） - std::string name = ""; // 相机名称 - std::string cameraIP = ""; // 相机IP地址 - bool enabled = true; // 是否启用 - - // 相机工作参数（参考 GrabBag） - double exposure = 100.0; // 曝光时间 (微秒) - double gain = 1.0; // 增益值 - double frameRate = 500.0; // 帧率 - double swingSpeed = 30.0; // 摆动速度 - double swingStartAngle = 0.0; // 开始角度 - double swingStopAngle = 76.0; // 结束角度 -}; - -/** - * @brief 相机调平参数 - */ -struct WheelCameraPlaneCalibParam -{ - int cameraIndex = 0; // 相机索引 - std::string cameraName = ""; // 相机名称 - double planeCalib[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1}; // 旋转矩阵（将点云旋转到水平） - double planeHeight = -1; // 地面高度 - double invRMatrix[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1}; // 逆旋转矩阵（回到原坐标系） - bool isCalibrated = false; // 是否已标定 - double errorCompensation = -5.0; // 误差补偿（mm） -}; - -/** - * @brief 角点参数 - */ -struct WheelCornerParam -{ - double minEndingGap = 20.0; // y方向，最短结束间隙 - double minEndingGap_z = 20.0; // z方向，最短结束间隙 - double scale = 50.0; // 计算方向角的窗口比例 - double cornerTh = 45.0; // 空角阈值，大于此阈值为有效空点 - double jumpCornerTh_1 = 10.0; // 判断空角是否为跳变的阈值 - double jumpCornerTh_2 = 60.0; // 判断空角是否为跳变的阈值 -}; - -/** - * @brief 线段参数 - */ -struct WheelLineSegParam -{ - double segGapTh_y = 5.0; // y方向，最短段间隙阈值 - double segGapTh_z = 10.0; // z方向，最短段间隙阈值 - double maxDist = 1.0; // 最大距离 -}; - -/** - * @brief 离群点过滤参数 - */ -struct WheelOutlierFilterParam -{ - double continuityTh = 20.0; // 连续性阈值 - double outlierTh = 5.0; // 离群点阈值 -}; - -/** - * @brief 树生长参数 - */ -struct WheelTreeGrowParam -{ - double yDeviation_max = 5.0; // 生长时允许的最大Y偏差 - double zDeviation_max = 2.0; // 生长时允许的最大Z偏差 - int maxLineSkipNum = 10; // 生长时允许的最大跳线数 - double maxSkipDistance = 5.0; // 当maxLineSkipNum为-1时使用 - double minLTypeTreeLen = 100.0; // 最少的L型节点数目 - double minVTypeTreeLen = 100.0; // 最少的V型节点数目 -}; - -/** - * @brief 车轮拱高测量算法参数 - */ -struct WheelMeasureAlgorithmParams -{ - WheelCornerParam cornerParam; // 角点参数 - WheelLineSegParam lineSegParam; // 线段参数 - WheelOutlierFilterParam filterParam; // 离群点过滤参数 - WheelTreeGrowParam growParam; // 树生长参数 -}; - -/** - * @brief 配置加载结果 - */ -struct WheelMeasureConfigResult -{ - std::vector cameras; // 相机列表 - std::vector planeCalibParams; // 相机调平参数列表 - std::vector servers; // 服务器列表 - WheelMeasureAlgorithmParams algorithmParams; // 算法参数 - VrDebugParam debugParam; // 调试参数（使用公共VrDebugParam） - - int serverPort = 5900; // 服务器端口 - int tcpPort = 5800; // TCP协议端口 - - // 构造函数 - WheelMeasureConfigResult() {} -}; - -/** - * @brief 测量结果数据 - */ -struct WheelMeasureData -{ - int id = 0; // 测量ID - double archToCenterHeight = 0.0; // 拱高到中心的高度 - double archToGroundHeight = 0.0; // 拱高到地面的高度 - double wheelArchPosX = 0.0; // 拱点X坐标 - double wheelArchPosY = 0.0; // 拱点Y坐标 - double wheelArchPosZ = 0.0; // 拱点Z坐标 - double wheelUpPosX = 0.0; // 上点X坐标 - double wheelUpPosY = 0.0; // 上点Y坐标 - double wheelUpPosZ = 0.0; // 上点Z坐标 - double wheelDownPosX = 0.0; // 下点X坐标 - double wheelDownPosY = 0.0; // 下点Y坐标 - double wheelDownPosZ = 0.0; // 下点Z坐标 - QString timestamp = ""; // 时间戳 -}; - -/** - * @brief 测量结果 - */ -struct WheelMeasureResult -{ - QString cameraName = ""; // 相机名称 - QString aliasName = ""; // 别名 - QImage image; // 图像 - bool bImageValid = false; // 图像是否有效 - bool bResultValid = false; // 结果是否有效 - std::vector result; // 测量结果列表 -}; - -/** - * @brief 配置改变通知接口 - */ -class IVrWheelMeasureConfigChangeNotify -{ -public: - virtual ~IVrWheelMeasureConfigChangeNotify() {} - - /** - * @brief 配置数据改变通知 - * @param configResult 新的配置数据 - */ - virtual void OnConfigChanged(const WheelMeasureConfigResult& configResult) = 0; -}; - -/** - * @brief WheelMeasureConfig接口类 - */ -class IVrWheelMeasureConfig -{ -public: - /** - * @brief 虚析构函数 - */ - virtual ~IVrWheelMeasureConfig() = default; - - /** - * @brief 创建实例 - * @return 实例 - */ - static bool CreateInstance(IVrWheelMeasureConfig** ppVrConfig); - - /** - * @brief 加载配置文件 - * @param filePath 配置文件路径 - * @return 加载的配置结果 - */ - virtual WheelMeasureConfigResult LoadConfig(const std::string& filePath) = 0; - - /** - * @brief 保存配置文件 - * @param filePath 配置文件路径 - * @param configResult 配置结果 - * @return 是否保存成功 - */ - virtual bool SaveConfig(const std::string& filePath, WheelMeasureConfigResult& configResult) = 0; - - /** - * @brief 设置配置改变通知回调 - * @param notify 通知接口指针 - */ - virtual void SetConfigChangeNotify(IVrWheelMeasureConfigChangeNotify* notify) = 0; -}; - -// 声明元类型，以便在QVariant中使用 -Q_DECLARE_METATYPE(WheelServerInfo) -Q_DECLARE_METATYPE(WheelCameraParam) -Q_DECLARE_METATYPE(WheelCameraPlaneCalibParam) -Q_DECLARE_METATYPE(WheelMeasureConfigResult) -Q_DECLARE_METATYPE(WheelMeasureData) -Q_DECLARE_METATYPE(WheelMeasureResult) - -#endif // IVRWHEELMEASURECONFIG_H +#ifndef IVRWHEELMEASURECONFIG_H +#define IVRWHEELMEASURECONFIG_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include "VrCommonConfig.h" // 包含公共配置结构体 + +/** + * @brief 数据类型枚举 + */ +enum class WheelMeasureDataType { + Text = 0x01, + Image = 0x02, + ReadConfig = 0x03, + WriteConfig = 0x04, +}; + +/** + * @brief 服务器信息结构 + */ +struct WheelServerInfo +{ + std::string name; // 服务器名称 + std::string ip; // 服务器IP地址 + int port = 5000; // 服务器端口 +}; + +/** + * @brief 相机配置参数 + */ +struct WheelCameraParam +{ + int cameraIndex = 0; // 相机索引（1-based） + std::string name = ""; // 相机名称 + std::string cameraIP = ""; // 相机IP地址 + bool enabled = true; // 是否启用 + + // 相机工作参数（参考 GrabBag） + double exposure = 100.0; // 曝光时间 (微秒) + double gain = 1.0; // 增益值 + double frameRate = 500.0; // 帧率 + double swingSpeed = 30.0; // 摆动速度 + double swingStartAngle = 0.0; // 开始角度 + double swingStopAngle = 76.0; // 结束角度 +}; + +/** + * @brief 相机调平参数 + */ +struct WheelCameraPlaneCalibParam +{ + int cameraIndex = 0; // 相机索引 + std::string cameraName = ""; // 相机名称 + double planeCalib[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1}; // 旋转矩阵（将点云旋转到水平） + double planeHeight = -1; // 地面高度 + double invRMatrix[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1}; // 逆旋转矩阵（回到原坐标系） + bool isCalibrated = false; // 是否已标定 + double errorCompensation = -5.0; // 误差补偿（mm） + + // 轮胎存在检测的3D ROI范围 + double wheelRoi3d_xMin = -1000.0; // X方向最小值（mm） + double wheelRoi3d_xMax = 1000.0; // X方向最大值（mm） + double wheelRoi3d_yMin = -1000.0; // Y方向最小值（mm） + double wheelRoi3d_yMax = 1000.0; // Y方向最大值（mm） + double wheelRoi3d_zMin = -1000.0; // Z方向最小值（mm） + double wheelRoi3d_zMax = 1000.0; // Z方向最大值（mm） +}; + +/** + * @brief 角点参数 + */ +struct WheelCornerParam +{ + double minEndingGap = 20.0; // y方向，最短结束间隙 + double minEndingGap_z = 20.0; // z方向，最短结束间隙 + double scale = 50.0; // 计算方向角的窗口比例 + double cornerTh = 45.0; // 空角阈值，大于此阈值为有效空点 + double jumpCornerTh_1 = 10.0; // 判断空角是否为跳变的阈值 + double jumpCornerTh_2 = 60.0; // 判断空角是否为跳变的阈值 +}; + +/** + * @brief 线段参数 + */ +struct WheelLineSegParam +{ + double segGapTh_y = 5.0; // y方向，最短段间隙阈值 + double segGapTh_z = 10.0; // z方向，最短段间隙阈值 + double maxDist = 1.0; // 最大距离 +}; + +/** + * @brief 离群点过滤参数 + */ +struct WheelOutlierFilterParam +{ + double continuityTh = 20.0; // 连续性阈值 + double outlierTh = 5.0; // 离群点阈值 +}; + +/** + * @brief 树生长参数 + */ +struct WheelTreeGrowParam +{ + double yDeviation_max = 5.0; // 生长时允许的最大Y偏差 + double zDeviation_max = 2.0; // 生长时允许的最大Z偏差 + int maxLineSkipNum = 10; // 生长时允许的最大跳线数 + double maxSkipDistance = 5.0; // 当maxLineSkipNum为-1时使用 + double minLTypeTreeLen = 100.0; // 最少的L型节点数目 + double minVTypeTreeLen = 100.0; // 最少的V型节点数目 +}; + +/** + * @brief 车轮拱高测量算法参数 + */ +struct WheelMeasureAlgorithmParams +{ + WheelCornerParam cornerParam; // 角点参数 + WheelLineSegParam lineSegParam; // 线段参数 + WheelOutlierFilterParam filterParam; // 离群点过滤参数 + WheelTreeGrowParam growParam; // 树生长参数 +}; + +/** + * @brief 配置加载结果 + */ +struct WheelMeasureConfigResult +{ + std::vector cameras; // 相机列表 + std::vector planeCalibParams; // 相机调平参数列表 + std::vector servers; // 服务器列表 + WheelMeasureAlgorithmParams algorithmParams; // 算法参数 + VrDebugParam debugParam; // 调试参数（使用公共VrDebugParam） + + int serverPort = 5900; // 服务器端口 + int tcpPort = 5800; // TCP协议端口 + + // 构造函数 + WheelMeasureConfigResult() {} +}; + +/** + * @brief 测量结果数据 + */ +struct WheelMeasureData +{ + int id = 0; // 测量ID + double archToCenterHeight = 0.0; // 拱高到中心的高度 + double archToGroundHeight = 0.0; // 拱高到地面的高度 + double wheelArchPosX = 0.0; // 拱点X坐标 + double wheelArchPosY = 0.0; // 拱点Y坐标 + double wheelArchPosZ = 0.0; // 拱点Z坐标 + double wheelUpPosX = 0.0; // 上点X坐标 + double wheelUpPosY = 0.0; // 上点Y坐标 + double wheelUpPosZ = 0.0; // 上点Z坐标 + double wheelDownPosX = 0.0; // 下点X坐标 + double wheelDownPosY = 0.0; // 下点Y坐标 + double wheelDownPosZ = 0.0; // 下点Z坐标 + QString timestamp = ""; // 时间戳 +}; + +/** + * @brief 测量结果 + */ +struct WheelMeasureResult +{ + QString cameraName = ""; // 相机名称 + QString aliasName = ""; // 别名 + QImage image; // 图像 + bool bImageValid = false; // 图像是否有效 + bool bResultValid = false; // 结果是否有效 + int errorCode = 0; // 错误码（0表示成功，401表示工件为空） + QString errorMessage = ""; // 错误信息 + std::vector result; // 测量结果列表 +}; + +/** + * @brief 配置改变通知接口 + */ +class IVrWheelMeasureConfigChangeNotify +{ +public: + virtual ~IVrWheelMeasureConfigChangeNotify() {} + + /** + * @brief 配置数据改变通知 + * @param configResult 新的配置数据 + */ + virtual void OnConfigChanged(const WheelMeasureConfigResult& configResult) = 0; +}; + +/** + * @brief WheelMeasureConfig接口类 + */ +class IVrWheelMeasureConfig +{ +public: + /** + * @brief 虚析构函数 + */ + virtual ~IVrWheelMeasureConfig() = default; + + /** + * @brief 创建实例 + * @return 实例 + */ + static bool CreateInstance(IVrWheelMeasureConfig** ppVrConfig); + + /** + * @brief 加载配置文件 + * @param filePath 配置文件路径 + * @return 加载的配置结果 + */ + virtual WheelMeasureConfigResult LoadConfig(const std::string& filePath) = 0; + + /** + * @brief 保存配置文件 + * @param filePath 配置文件路径 + * @param configResult 配置结果 + * @return 是否保存成功 + */ + virtual bool SaveConfig(const std::string& filePath, WheelMeasureConfigResult& configResult) = 0; + + /** + * @brief 设置配置改变通知回调 + * @param notify 通知接口指针 + */ + virtual void SetConfigChangeNotify(IVrWheelMeasureConfigChangeNotify* notify) = 0; +}; + +// 声明元类型，以便在QVariant中使用 +Q_DECLARE_METATYPE(WheelServerInfo) +Q_DECLARE_METATYPE(WheelCameraParam) +Q_DECLARE_METATYPE(WheelCameraPlaneCalibParam) +Q_DECLARE_METATYPE(WheelMeasureConfigResult) +Q_DECLARE_METATYPE(WheelMeasureData) +Q_DECLARE_METATYPE(WheelMeasureResult) + +#endif // IVRWHEELMEASURECONFIG_H diff --git a/App/WheelMeasure/WheelMeasureConfig/Src/VrWheelMeasureConfig.cpp b/App/WheelMeasure/WheelMeasureConfig/Src/VrWheelMeasureConfig.cpp index 3d2ffd14..a8cee672 100644 --- a/App/WheelMeasure/WheelMeasureConfig/Src/VrWheelMeasureConfig.cpp +++ b/App/WheelMeasure/WheelMeasureConfig/Src/VrWheelMeasureConfig.cpp @@ -1,435 +1,461 @@ -#include "VrWheelMeasureConfig.h" -#include "IVrWheelMeasureConfig.h" -#include -#include -#include "VrLog.h" -#include -#include -#include -#include -#include -#include - -VrWheelMeasureConfig::VrWheelMeasureConfig() - : m_notify(nullptr) -{ -} - -VrWheelMeasureConfig::~VrWheelMeasureConfig() -{ -} - -// 静态工厂方法 -bool IVrWheelMeasureConfig::CreateInstance(IVrWheelMeasureConfig** ppVrConfig) -{ - if (!ppVrConfig) { - return false; - } - - *ppVrConfig = new VrWheelMeasureConfig(); - return true; -} - -WheelMeasureConfigResult VrWheelMeasureConfig::LoadConfig(const std::string& filePath) -{ - WheelMeasureConfigResult result; - - // 使用QString处理可能包含中文的路径 - QString qFilePath = QString::fromStdString(filePath); - QFile file(qFilePath); - - // 检查文件是否存在并可读 - if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) { - LOG_DEBUG("Failed to open file: %s\n", filePath.c_str()); - return result; - } - - // 使用QXmlStreamReader解析XML内容 - QXmlStreamReader xml(&file); - - // 读取到根元素 - if (xml.readNextStartElement()) { - if (xml.name() != "WheelMeasureConfig") { - xml.raiseError(QObject::tr("Not a WheelMeasureConfig file")); - } - } else { - xml.raiseError(QObject::tr("Failed to read root element")); - } - - // 解析XML内容 - while (!xml.atEnd() && !xml.hasError()) { - xml.readNext(); - - // 解析相机配置 - if (xml.isStartElement() && xml.name() == "Cameras") { - while (xml.readNextStartElement()) { - if (xml.name() == "Camera") { - WheelCameraParam camera; - camera.cameraIndex = xml.attributes().value("index").toInt(); - camera.name = xml.attributes().value("name").toString().toStdString(); - camera.cameraIP = xml.attributes().value("ip").toString().toStdString(); - camera.enabled = xml.attributes().value("enabled").toInt() != 0; - result.cameras.push_back(camera); - xml.skipCurrentElement(); - } - } - } - - // 解析相机调平参数 - else if (xml.isStartElement() && xml.name() == "PlaneCalibParams") { - while (xml.readNextStartElement()) { - if (xml.name() == "CameraCalib") { - WheelCameraPlaneCalibParam calibParam; - calibParam.cameraIndex = xml.attributes().value("index").toInt(); - calibParam.cameraName = xml.attributes().value("name").toString().toStdString(); - calibParam.planeHeight = xml.attributes().value("planeHeight").toDouble(); - calibParam.isCalibrated = xml.attributes().value("isCalibrated").toInt() != 0; - // 读取误差补偿参数，默认值为-5.0 - if (xml.attributes().hasAttribute("errorCompensation")) { - calibParam.errorCompensation = xml.attributes().value("errorCompensation").toDouble(); - } else { - calibParam.errorCompensation = -5.0; - } - - // 读取planeCalib矩阵 - QString planeCalibStr = xml.attributes().value("planeCalib").toString(); - QStringList planeCalibList = planeCalibStr.split(","); - for (int i = 0; i < 9 && i < planeCalibList.size(); ++i) { - calibParam.planeCalib[i] = planeCalibList[i].toDouble(); - } - - // 读取invRMatrix矩阵 - QString invRMatrixStr = xml.attributes().value("invRMatrix").toString(); - QStringList invRMatrixList = invRMatrixStr.split(","); - for (int i = 0; i < 9 && i < invRMatrixList.size(); ++i) { - calibParam.invRMatrix[i] = invRMatrixList[i].toDouble(); - } - - result.planeCalibParams.push_back(calibParam); - xml.skipCurrentElement(); - } - } - } - - // 解析算法参数 - else if (xml.isStartElement() && xml.name() == "AlgorithmParams") { - while (xml.readNextStartElement()) { - // 角点参数 - if (xml.name() == "CornerParam") { - result.algorithmParams.cornerParam.minEndingGap = - xml.attributes().value("minEndingGap").toDouble(); - result.algorithmParams.cornerParam.minEndingGap_z = - xml.attributes().value("minEndingGap_z").toDouble(); - result.algorithmParams.cornerParam.scale = - xml.attributes().value("scale").toDouble(); - result.algorithmParams.cornerParam.cornerTh = - xml.attributes().value("cornerTh").toDouble(); - result.algorithmParams.cornerParam.jumpCornerTh_1 = - xml.attributes().value("jumpCornerTh_1").toDouble(); - result.algorithmParams.cornerParam.jumpCornerTh_2 = - xml.attributes().value("jumpCornerTh_2").toDouble(); - - // 设置默认值 - if (result.algorithmParams.cornerParam.minEndingGap == 0.0) { - result.algorithmParams.cornerParam.minEndingGap = 3.0; - } - if (result.algorithmParams.cornerParam.minEndingGap_z == 0.0) { - result.algorithmParams.cornerParam.minEndingGap_z = 5.0; - } - if (result.algorithmParams.cornerParam.scale == 0.0) { - result.algorithmParams.cornerParam.scale = 10.0; - } - if (result.algorithmParams.cornerParam.cornerTh == 0.0) { - result.algorithmParams.cornerParam.cornerTh = 130.0; - } - if (result.algorithmParams.cornerParam.jumpCornerTh_1 == 0.0) { - result.algorithmParams.cornerParam.jumpCornerTh_1 = 5.0; - } - if (result.algorithmParams.cornerParam.jumpCornerTh_2 == 0.0) { - result.algorithmParams.cornerParam.jumpCornerTh_2 = 2.0; - } - - xml.skipCurrentElement(); - } - // 线段参数 - else if (xml.name() == "LineSegParam") { - result.algorithmParams.lineSegParam.segGapTh_y = - xml.attributes().value("segGapTh_y").toDouble(); - result.algorithmParams.lineSegParam.segGapTh_z = - xml.attributes().value("segGapTh_z").toDouble(); - result.algorithmParams.lineSegParam.maxDist = - xml.attributes().value("maxDist").toDouble(); - - // 设置默认值 - if (result.algorithmParams.lineSegParam.segGapTh_y == 0.0) { - result.algorithmParams.lineSegParam.segGapTh_y = 5.0; - } - if (result.algorithmParams.lineSegParam.segGapTh_z == 0.0) { - result.algorithmParams.lineSegParam.segGapTh_z = 10.0; - } - if (result.algorithmParams.lineSegParam.maxDist == 0.0) { - result.algorithmParams.lineSegParam.maxDist = 50.0; - } - - xml.skipCurrentElement(); - } - // 离群点过滤参数 - else if (xml.name() == "OutlierFilterParam") { - result.algorithmParams.filterParam.continuityTh = - xml.attributes().value("continuityTh").toDouble(); - result.algorithmParams.filterParam.outlierTh = - xml.attributes().value("outlierTh").toDouble(); - - // 设置默认值 - if (result.algorithmParams.filterParam.continuityTh == 0.0) { - result.algorithmParams.filterParam.continuityTh = 5.0; - } - if (result.algorithmParams.filterParam.outlierTh == 0.0) { - result.algorithmParams.filterParam.outlierTh = 3.0; - } - - xml.skipCurrentElement(); - } - // 树生长参数 - else if (xml.name() == "TreeGrowParam") { - result.algorithmParams.growParam.yDeviation_max = - xml.attributes().value("yDeviation_max").toDouble(); - result.algorithmParams.growParam.zDeviation_max = - xml.attributes().value("zDeviation_max").toDouble(); - result.algorithmParams.growParam.maxLineSkipNum = - xml.attributes().value("maxLineSkipNum").toInt(); - result.algorithmParams.growParam.maxSkipDistance = - xml.attributes().value("maxSkipDistance").toDouble(); - result.algorithmParams.growParam.minLTypeTreeLen = - xml.attributes().value("minLTypeTreeLen").toDouble(); - result.algorithmParams.growParam.minVTypeTreeLen = - xml.attributes().value("minVTypeTreeLen").toDouble(); - - // 设置默认值 - if (result.algorithmParams.growParam.yDeviation_max == 0.0) { - result.algorithmParams.growParam.yDeviation_max = 20.0; - } - if (result.algorithmParams.growParam.zDeviation_max == 0.0) { - result.algorithmParams.growParam.zDeviation_max = 30.0; - } - if (result.algorithmParams.growParam.maxLineSkipNum == 0) { - result.algorithmParams.growParam.maxLineSkipNum = 5; - } - if (result.algorithmParams.growParam.minLTypeTreeLen == 0.0) { - result.algorithmParams.growParam.minLTypeTreeLen = 10.0; - } - if (result.algorithmParams.growParam.minVTypeTreeLen == 0.0) { - result.algorithmParams.growParam.minVTypeTreeLen = 10.0; - } - - xml.skipCurrentElement(); - } - else { - xml.skipCurrentElement(); - } - } - } - - // 解析调试参数 - else if (xml.isStartElement() && xml.name() == "DebugParam") { - result.debugParam.enableDebug = xml.attributes().value("enableDebug").toInt(); - result.debugParam.savePointCloud = xml.attributes().value("savePointCloud").toInt(); - result.debugParam.saveDebugImage = xml.attributes().value("saveDebugImage").toInt(); - result.debugParam.printDetailLog = xml.attributes().value("printDetailLog").toInt(); - result.debugParam.debugOutputPath = xml.attributes().value("debugOutputPath").toString().toStdString(); - xml.skipCurrentElement(); - } - - // 解析服务端配置 - else if (xml.isStartElement() && xml.name() == "LocalServerConfig") { - while (xml.readNextStartElement()) { - if (xml.name() == "ServerPort") { - result.serverPort = xml.attributes().value("port").toInt(); - xml.skipCurrentElement(); - } else if (xml.name() == "TcpPort") { - result.tcpPort = xml.attributes().value("port").toInt(); - if (result.tcpPort == 0) { - result.tcpPort = 5800; // 默认值 - } - xml.skipCurrentElement(); - } else { - xml.skipCurrentElement(); - } - } - } - - // 解析服务器列表 - else if (xml.isStartElement() && xml.name() == "Servers") { - while (xml.readNextStartElement()) { - if (xml.name() == "Server") { - WheelServerInfo server; - server.name = xml.attributes().value("name").toString().toStdString(); - server.ip = xml.attributes().value("ip").toString().toStdString(); - server.port = xml.attributes().value("port").toInt(); - if (server.port == 0) { - server.port = 5800; // 默认端口 - } - result.servers.push_back(server); - xml.skipCurrentElement(); - } else { - xml.skipCurrentElement(); - } - } - } - } - - file.close(); - - // 检查解析错误 - if (xml.hasError()) { - LOG_ERROR("XML parsing error: %s\n", xml.errorString().toStdString().c_str()); - return WheelMeasureConfigResult(); // 返回空结果 - } - - return result; -} - -bool VrWheelMeasureConfig::SaveConfig(const std::string& filePath, WheelMeasureConfigResult& configResult) -{ - // 使用QString处理可能包含中文的路径 - QString qFilePath = QString::fromStdString(filePath); - QFile file(qFilePath); - - // 打开文件进行写入 - if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) { - LOG_DEBUG("Failed to open file for writing: %s\n", filePath.c_str()); - return false; - } - - // 使用QXmlStreamWriter写入XML内容 - QXmlStreamWriter xml(&file); - xml.setAutoFormatting(true); - xml.setCodec("UTF-8"); - xml.writeStartDocument(); - xml.writeStartElement("WheelMeasureConfig"); - - // 保存相机配置 - xml.writeStartElement("Cameras"); - for (const auto& camera : configResult.cameras) { - xml.writeStartElement("Camera"); - xml.writeAttribute("index", QString::number(camera.cameraIndex)); - xml.writeAttribute("name", QString::fromStdString(camera.name)); - xml.writeAttribute("ip", QString::fromStdString(camera.cameraIP)); - xml.writeAttribute("enabled", QString::number(camera.enabled ? 1 : 0)); - xml.writeEndElement(); // Camera - } - xml.writeEndElement(); // Cameras - - // 保存相机调平参数 - xml.writeStartElement("PlaneCalibParams"); - for (const auto& calibParam : configResult.planeCalibParams) { - xml.writeStartElement("CameraCalib"); - xml.writeAttribute("index", QString::number(calibParam.cameraIndex)); - xml.writeAttribute("name", QString::fromStdString(calibParam.cameraName)); - xml.writeAttribute("planeHeight", QString::number(calibParam.planeHeight, 'f', 6)); - xml.writeAttribute("isCalibrated", QString::number(calibParam.isCalibrated ? 1 : 0)); - xml.writeAttribute("errorCompensation", QString::number(calibParam.errorCompensation, 'f', 2)); - - // 保存planeCalib矩阵 - QStringList planeCalibList; - for (int i = 0; i < 9; ++i) { - planeCalibList.append(QString::number(calibParam.planeCalib[i], 'f', 8)); - } - xml.writeAttribute("planeCalib", planeCalibList.join(",")); - - // 保存invRMatrix矩阵 - QStringList invRMatrixList; - for (int i = 0; i < 9; ++i) { - invRMatrixList.append(QString::number(calibParam.invRMatrix[i], 'f', 8)); - } - xml.writeAttribute("invRMatrix", invRMatrixList.join(",")); - - xml.writeEndElement(); // CameraCalib - } - xml.writeEndElement(); // PlaneCalibParams - - // 保存算法参数 - xml.writeStartElement("AlgorithmParams"); - - // 角点参数 - xml.writeStartElement("CornerParam"); - xml.writeAttribute("minEndingGap", QString::number(configResult.algorithmParams.cornerParam.minEndingGap)); - xml.writeAttribute("minEndingGap_z", QString::number(configResult.algorithmParams.cornerParam.minEndingGap_z)); - xml.writeAttribute("scale", QString::number(configResult.algorithmParams.cornerParam.scale)); - xml.writeAttribute("cornerTh", QString::number(configResult.algorithmParams.cornerParam.cornerTh)); - xml.writeAttribute("jumpCornerTh_1", QString::number(configResult.algorithmParams.cornerParam.jumpCornerTh_1)); - xml.writeAttribute("jumpCornerTh_2", QString::number(configResult.algorithmParams.cornerParam.jumpCornerTh_2)); - xml.writeEndElement(); // CornerParam - - // 线段参数 - xml.writeStartElement("LineSegParam"); - xml.writeAttribute("segGapTh_y", QString::number(configResult.algorithmParams.lineSegParam.segGapTh_y)); - xml.writeAttribute("segGapTh_z", QString::number(configResult.algorithmParams.lineSegParam.segGapTh_z)); - xml.writeAttribute("maxDist", QString::number(configResult.algorithmParams.lineSegParam.maxDist)); - xml.writeEndElement(); // LineSegParam - - // 离群点过滤参数 - xml.writeStartElement("OutlierFilterParam"); - xml.writeAttribute("continuityTh", QString::number(configResult.algorithmParams.filterParam.continuityTh)); - xml.writeAttribute("outlierTh", QString::number(configResult.algorithmParams.filterParam.outlierTh)); - xml.writeEndElement(); // OutlierFilterParam - - // 树生长参数 - xml.writeStartElement("TreeGrowParam"); - xml.writeAttribute("yDeviation_max", QString::number(configResult.algorithmParams.growParam.yDeviation_max)); - xml.writeAttribute("zDeviation_max", QString::number(configResult.algorithmParams.growParam.zDeviation_max)); - xml.writeAttribute("maxLineSkipNum", QString::number(configResult.algorithmParams.growParam.maxLineSkipNum)); - xml.writeAttribute("maxSkipDistance", QString::number(configResult.algorithmParams.growParam.maxSkipDistance)); - xml.writeAttribute("minLTypeTreeLen", QString::number(configResult.algorithmParams.growParam.minLTypeTreeLen)); - xml.writeAttribute("minVTypeTreeLen", QString::number(configResult.algorithmParams.growParam.minVTypeTreeLen)); - xml.writeEndElement(); // TreeGrowParam - - xml.writeEndElement(); // AlgorithmParams - - // 保存调试参数 - xml.writeStartElement("DebugParam"); - xml.writeAttribute("enableDebug", QString::number(configResult.debugParam.enableDebug)); - xml.writeAttribute("savePointCloud", QString::number(configResult.debugParam.savePointCloud)); - xml.writeAttribute("saveDebugImage", QString::number(configResult.debugParam.saveDebugImage)); - xml.writeAttribute("printDetailLog", QString::number(configResult.debugParam.printDetailLog)); - xml.writeAttribute("debugOutputPath", QString::fromStdString(configResult.debugParam.debugOutputPath)); - xml.writeEndElement(); // DebugParam - - // 保存服务端配置 - xml.writeStartElement("LocalServerConfig"); - xml.writeStartElement("ServerPort"); - xml.writeAttribute("port", QString::number(configResult.serverPort)); - xml.writeEndElement(); // ServerPort - xml.writeStartElement("TcpPort"); - xml.writeAttribute("port", QString::number(configResult.tcpPort)); - xml.writeEndElement(); // TcpPort - xml.writeEndElement(); // LocalServerConfig - - // 保存服务器列表 - xml.writeStartElement("Servers"); - for (const auto& server : configResult.servers) { - xml.writeStartElement("Server"); - xml.writeAttribute("name", QString::fromStdString(server.name)); - xml.writeAttribute("ip", QString::fromStdString(server.ip)); - xml.writeAttribute("port", QString::number(server.port)); - xml.writeEndElement(); // Server - } - xml.writeEndElement(); // Servers - - xml.writeEndElement(); // WheelMeasureConfig - xml.writeEndDocument(); - - file.close(); - - // 通知配置改变 - if (m_notify) { - m_notify->OnConfigChanged(configResult); - } - - return true; -} - -void VrWheelMeasureConfig::SetConfigChangeNotify(IVrWheelMeasureConfigChangeNotify* notify) -{ - m_notify = notify; -} +#include "VrWheelMeasureConfig.h" +#include "IVrWheelMeasureConfig.h" +#include +#include +#include "VrLog.h" +#include +#include +#include +#include +#include +#include + +VrWheelMeasureConfig::VrWheelMeasureConfig() + : m_notify(nullptr) +{ +} + +VrWheelMeasureConfig::~VrWheelMeasureConfig() +{ +} + +// 静态工厂方法 +bool IVrWheelMeasureConfig::CreateInstance(IVrWheelMeasureConfig** ppVrConfig) +{ + if (!ppVrConfig) { + return false; + } + + *ppVrConfig = new VrWheelMeasureConfig(); + return true; +} + +WheelMeasureConfigResult VrWheelMeasureConfig::LoadConfig(const std::string& filePath) +{ + WheelMeasureConfigResult result; + + // 使用QString处理可能包含中文的路径 + QString qFilePath = QString::fromStdString(filePath); + QFile file(qFilePath); + + // 检查文件是否存在并可读 + if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) { + LOG_DEBUG("Failed to open file: %s\n", filePath.c_str()); + return result; + } + + // 使用QXmlStreamReader解析XML内容 + QXmlStreamReader xml(&file); + + // 读取到根元素 + if (xml.readNextStartElement()) { + if (xml.name() != "WheelMeasureConfig") { + xml.raiseError(QObject::tr("Not a WheelMeasureConfig file")); + } + } else { + xml.raiseError(QObject::tr("Failed to read root element")); + } + + // 解析XML内容 + while (!xml.atEnd() && !xml.hasError()) { + xml.readNext(); + + // 解析相机配置 + if (xml.isStartElement() && xml.name() == "Cameras") { + while (xml.readNextStartElement()) { + if (xml.name() == "Camera") { + WheelCameraParam camera; + camera.cameraIndex = xml.attributes().value("index").toInt(); + camera.name = xml.attributes().value("name").toString().toStdString(); + camera.cameraIP = xml.attributes().value("ip").toString().toStdString(); + camera.enabled = xml.attributes().value("enabled").toInt() != 0; + result.cameras.push_back(camera); + xml.skipCurrentElement(); + } + } + } + + // 解析相机调平参数 + else if (xml.isStartElement() && xml.name() == "PlaneCalibParams") { + while (xml.readNextStartElement()) { + if (xml.name() == "CameraCalib") { + WheelCameraPlaneCalibParam calibParam; + calibParam.cameraIndex = xml.attributes().value("index").toInt(); + calibParam.cameraName = xml.attributes().value("name").toString().toStdString(); + calibParam.planeHeight = xml.attributes().value("planeHeight").toDouble(); + calibParam.isCalibrated = xml.attributes().value("isCalibrated").toInt() != 0; + // 读取误差补偿参数，默认值为-5.0 + if (xml.attributes().hasAttribute("errorCompensation")) { + calibParam.errorCompensation = xml.attributes().value("errorCompensation").toDouble(); + } else { + calibParam.errorCompensation = -5.0; + } + + // 读取轮胎存在检测的3D ROI范围 + if (xml.attributes().hasAttribute("wheelRoi3d_xMin")) { + calibParam.wheelRoi3d_xMin = xml.attributes().value("wheelRoi3d_xMin").toDouble(); + calibParam.wheelRoi3d_xMax = xml.attributes().value("wheelRoi3d_xMax").toDouble(); + calibParam.wheelRoi3d_yMin = xml.attributes().value("wheelRoi3d_yMin").toDouble(); + calibParam.wheelRoi3d_yMax = xml.attributes().value("wheelRoi3d_yMax").toDouble(); + calibParam.wheelRoi3d_zMin = xml.attributes().value("wheelRoi3d_zMin").toDouble(); + calibParam.wheelRoi3d_zMax = xml.attributes().value("wheelRoi3d_zMax").toDouble(); + } else { + // 默认值 + calibParam.wheelRoi3d_xMin = -1000.0; + calibParam.wheelRoi3d_xMax = 1000.0; + calibParam.wheelRoi3d_yMin = -1000.0; + calibParam.wheelRoi3d_yMax = 1000.0; + calibParam.wheelRoi3d_zMin = -1000.0; + calibParam.wheelRoi3d_zMax = 1000.0; + } + + // 读取planeCalib矩阵 + QString planeCalibStr = xml.attributes().value("planeCalib").toString(); + QStringList planeCalibList = planeCalibStr.split(","); + for (int i = 0; i < 9 && i < planeCalibList.size(); ++i) { + calibParam.planeCalib[i] = planeCalibList[i].toDouble(); + } + + // 读取invRMatrix矩阵 + QString invRMatrixStr = xml.attributes().value("invRMatrix").toString(); + QStringList invRMatrixList = invRMatrixStr.split(","); + for (int i = 0; i < 9 && i < invRMatrixList.size(); ++i) { + calibParam.invRMatrix[i] = invRMatrixList[i].toDouble(); + } + + result.planeCalibParams.push_back(calibParam); + xml.skipCurrentElement(); + } + } + } + + // 解析算法参数 + else if (xml.isStartElement() && xml.name() == "AlgorithmParams") { + while (xml.readNextStartElement()) { + // 角点参数 + if (xml.name() == "CornerParam") { + result.algorithmParams.cornerParam.minEndingGap = + xml.attributes().value("minEndingGap").toDouble(); + result.algorithmParams.cornerParam.minEndingGap_z = + xml.attributes().value("minEndingGap_z").toDouble(); + result.algorithmParams.cornerParam.scale = + xml.attributes().value("scale").toDouble(); + result.algorithmParams.cornerParam.cornerTh = + xml.attributes().value("cornerTh").toDouble(); + result.algorithmParams.cornerParam.jumpCornerTh_1 = + xml.attributes().value("jumpCornerTh_1").toDouble(); + result.algorithmParams.cornerParam.jumpCornerTh_2 = + xml.attributes().value("jumpCornerTh_2").toDouble(); + + // 设置默认值 + if (result.algorithmParams.cornerParam.minEndingGap == 0.0) { + result.algorithmParams.cornerParam.minEndingGap = 3.0; + } + if (result.algorithmParams.cornerParam.minEndingGap_z == 0.0) { + result.algorithmParams.cornerParam.minEndingGap_z = 5.0; + } + if (result.algorithmParams.cornerParam.scale == 0.0) { + result.algorithmParams.cornerParam.scale = 10.0; + } + if (result.algorithmParams.cornerParam.cornerTh == 0.0) { + result.algorithmParams.cornerParam.cornerTh = 130.0; + } + if (result.algorithmParams.cornerParam.jumpCornerTh_1 == 0.0) { + result.algorithmParams.cornerParam.jumpCornerTh_1 = 5.0; + } + if (result.algorithmParams.cornerParam.jumpCornerTh_2 == 0.0) { + result.algorithmParams.cornerParam.jumpCornerTh_2 = 2.0; + } + + xml.skipCurrentElement(); + } + // 线段参数 + else if (xml.name() == "LineSegParam") { + result.algorithmParams.lineSegParam.segGapTh_y = + xml.attributes().value("segGapTh_y").toDouble(); + result.algorithmParams.lineSegParam.segGapTh_z = + xml.attributes().value("segGapTh_z").toDouble(); + result.algorithmParams.lineSegParam.maxDist = + xml.attributes().value("maxDist").toDouble(); + + // 设置默认值 + if (result.algorithmParams.lineSegParam.segGapTh_y == 0.0) { + result.algorithmParams.lineSegParam.segGapTh_y = 5.0; + } + if (result.algorithmParams.lineSegParam.segGapTh_z == 0.0) { + result.algorithmParams.lineSegParam.segGapTh_z = 10.0; + } + if (result.algorithmParams.lineSegParam.maxDist == 0.0) { + result.algorithmParams.lineSegParam.maxDist = 50.0; + } + + xml.skipCurrentElement(); + } + // 离群点过滤参数 + else if (xml.name() == "OutlierFilterParam") { + result.algorithmParams.filterParam.continuityTh = + xml.attributes().value("continuityTh").toDouble(); + result.algorithmParams.filterParam.outlierTh = + xml.attributes().value("outlierTh").toDouble(); + + // 设置默认值 + if (result.algorithmParams.filterParam.continuityTh == 0.0) { + result.algorithmParams.filterParam.continuityTh = 5.0; + } + if (result.algorithmParams.filterParam.outlierTh == 0.0) { + result.algorithmParams.filterParam.outlierTh = 3.0; + } + + xml.skipCurrentElement(); + } + // 树生长参数 + else if (xml.name() == "TreeGrowParam") { + result.algorithmParams.growParam.yDeviation_max = + xml.attributes().value("yDeviation_max").toDouble(); + result.algorithmParams.growParam.zDeviation_max = + xml.attributes().value("zDeviation_max").toDouble(); + result.algorithmParams.growParam.maxLineSkipNum = + xml.attributes().value("maxLineSkipNum").toInt(); + result.algorithmParams.growParam.maxSkipDistance = + xml.attributes().value("maxSkipDistance").toDouble(); + result.algorithmParams.growParam.minLTypeTreeLen = + xml.attributes().value("minLTypeTreeLen").toDouble(); + result.algorithmParams.growParam.minVTypeTreeLen = + xml.attributes().value("minVTypeTreeLen").toDouble(); + + // 设置默认值 + if (result.algorithmParams.growParam.yDeviation_max == 0.0) { + result.algorithmParams.growParam.yDeviation_max = 20.0; + } + if (result.algorithmParams.growParam.zDeviation_max == 0.0) { + result.algorithmParams.growParam.zDeviation_max = 30.0; + } + if (result.algorithmParams.growParam.maxLineSkipNum == 0) { + result.algorithmParams.growParam.maxLineSkipNum = 5; + } + if (result.algorithmParams.growParam.minLTypeTreeLen == 0.0) { + result.algorithmParams.growParam.minLTypeTreeLen = 10.0; + } + if (result.algorithmParams.growParam.minVTypeTreeLen == 0.0) { + result.algorithmParams.growParam.minVTypeTreeLen = 10.0; + } + + xml.skipCurrentElement(); + } + else { + xml.skipCurrentElement(); + } + } + } + + // 解析调试参数 + else if (xml.isStartElement() && xml.name() == "DebugParam") { + result.debugParam.enableDebug = xml.attributes().value("enableDebug").toInt(); + result.debugParam.savePointCloud = xml.attributes().value("savePointCloud").toInt(); + result.debugParam.saveDebugImage = xml.attributes().value("saveDebugImage").toInt(); + result.debugParam.printDetailLog = xml.attributes().value("printDetailLog").toInt(); + result.debugParam.debugOutputPath = xml.attributes().value("debugOutputPath").toString().toStdString(); + xml.skipCurrentElement(); + } + + // 解析服务端配置 + else if (xml.isStartElement() && xml.name() == "LocalServerConfig") { + while (xml.readNextStartElement()) { + if (xml.name() == "ServerPort") { + result.serverPort = xml.attributes().value("port").toInt(); + xml.skipCurrentElement(); + } else if (xml.name() == "TcpPort") { + result.tcpPort = xml.attributes().value("port").toInt(); + if (result.tcpPort == 0) { + result.tcpPort = 5800; // 默认值 + } + xml.skipCurrentElement(); + } else { + xml.skipCurrentElement(); + } + } + } + + // 解析服务器列表 + else if (xml.isStartElement() && xml.name() == "Servers") { + while (xml.readNextStartElement()) { + if (xml.name() == "Server") { + WheelServerInfo server; + server.name = xml.attributes().value("name").toString().toStdString(); + server.ip = xml.attributes().value("ip").toString().toStdString(); + server.port = xml.attributes().value("port").toInt(); + if (server.port == 0) { + server.port = 5800; // 默认端口 + } + result.servers.push_back(server); + xml.skipCurrentElement(); + } else { + xml.skipCurrentElement(); + } + } + } + } + + file.close(); + + // 检查解析错误 + if (xml.hasError()) { + LOG_ERROR("XML parsing error: %s\n", xml.errorString().toStdString().c_str()); + return WheelMeasureConfigResult(); // 返回空结果 + } + + return result; +} + +bool VrWheelMeasureConfig::SaveConfig(const std::string& filePath, WheelMeasureConfigResult& configResult) +{ + // 使用QString处理可能包含中文的路径 + QString qFilePath = QString::fromStdString(filePath); + QFile file(qFilePath); + + // 打开文件进行写入 + if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) { + LOG_DEBUG("Failed to open file for writing: %s\n", filePath.c_str()); + return false; + } + + // 使用QXmlStreamWriter写入XML内容 + QXmlStreamWriter xml(&file); + xml.setAutoFormatting(true); + xml.setCodec("UTF-8"); + xml.writeStartDocument(); + xml.writeStartElement("WheelMeasureConfig"); + + // 保存相机配置 + xml.writeStartElement("Cameras"); + for (const auto& camera : configResult.cameras) { + xml.writeStartElement("Camera"); + xml.writeAttribute("index", QString::number(camera.cameraIndex)); + xml.writeAttribute("name", QString::fromStdString(camera.name)); + xml.writeAttribute("ip", QString::fromStdString(camera.cameraIP)); + xml.writeAttribute("enabled", QString::number(camera.enabled ? 1 : 0)); + xml.writeEndElement(); // Camera + } + xml.writeEndElement(); // Cameras + + // 保存相机调平参数 + xml.writeStartElement("PlaneCalibParams"); + for (const auto& calibParam : configResult.planeCalibParams) { + xml.writeStartElement("CameraCalib"); + xml.writeAttribute("index", QString::number(calibParam.cameraIndex)); + xml.writeAttribute("name", QString::fromStdString(calibParam.cameraName)); + xml.writeAttribute("planeHeight", QString::number(calibParam.planeHeight, 'f', 6)); + xml.writeAttribute("isCalibrated", QString::number(calibParam.isCalibrated ? 1 : 0)); + xml.writeAttribute("errorCompensation", QString::number(calibParam.errorCompensation, 'f', 2)); + + // 保存轮胎存在检测的3D ROI范围 + xml.writeAttribute("wheelRoi3d_xMin", QString::number(calibParam.wheelRoi3d_xMin, 'f', 2)); + xml.writeAttribute("wheelRoi3d_xMax", QString::number(calibParam.wheelRoi3d_xMax, 'f', 2)); + xml.writeAttribute("wheelRoi3d_yMin", QString::number(calibParam.wheelRoi3d_yMin, 'f', 2)); + xml.writeAttribute("wheelRoi3d_yMax", QString::number(calibParam.wheelRoi3d_yMax, 'f', 2)); + xml.writeAttribute("wheelRoi3d_zMin", QString::number(calibParam.wheelRoi3d_zMin, 'f', 2)); + xml.writeAttribute("wheelRoi3d_zMax", QString::number(calibParam.wheelRoi3d_zMax, 'f', 2)); + + // 保存planeCalib矩阵 + QStringList planeCalibList; + for (int i = 0; i < 9; ++i) { + planeCalibList.append(QString::number(calibParam.planeCalib[i], 'f', 8)); + } + xml.writeAttribute("planeCalib", planeCalibList.join(",")); + + // 保存invRMatrix矩阵 + QStringList invRMatrixList; + for (int i = 0; i < 9; ++i) { + invRMatrixList.append(QString::number(calibParam.invRMatrix[i], 'f', 8)); + } + xml.writeAttribute("invRMatrix", invRMatrixList.join(",")); + + xml.writeEndElement(); // CameraCalib + } + xml.writeEndElement(); // PlaneCalibParams + + // 保存算法参数 + xml.writeStartElement("AlgorithmParams"); + + // 角点参数 + xml.writeStartElement("CornerParam"); + xml.writeAttribute("minEndingGap", QString::number(configResult.algorithmParams.cornerParam.minEndingGap)); + xml.writeAttribute("minEndingGap_z", QString::number(configResult.algorithmParams.cornerParam.minEndingGap_z)); + xml.writeAttribute("scale", QString::number(configResult.algorithmParams.cornerParam.scale)); + xml.writeAttribute("cornerTh", QString::number(configResult.algorithmParams.cornerParam.cornerTh)); + xml.writeAttribute("jumpCornerTh_1", QString::number(configResult.algorithmParams.cornerParam.jumpCornerTh_1)); + xml.writeAttribute("jumpCornerTh_2", QString::number(configResult.algorithmParams.cornerParam.jumpCornerTh_2)); + xml.writeEndElement(); // CornerParam + + // 线段参数 + xml.writeStartElement("LineSegParam"); + xml.writeAttribute("segGapTh_y", QString::number(configResult.algorithmParams.lineSegParam.segGapTh_y)); + xml.writeAttribute("segGapTh_z", QString::number(configResult.algorithmParams.lineSegParam.segGapTh_z)); + xml.writeAttribute("maxDist", QString::number(configResult.algorithmParams.lineSegParam.maxDist)); + xml.writeEndElement(); // LineSegParam + + // 离群点过滤参数 + xml.writeStartElement("OutlierFilterParam"); + xml.writeAttribute("continuityTh", QString::number(configResult.algorithmParams.filterParam.continuityTh)); + xml.writeAttribute("outlierTh", QString::number(configResult.algorithmParams.filterParam.outlierTh)); + xml.writeEndElement(); // OutlierFilterParam + + // 树生长参数 + xml.writeStartElement("TreeGrowParam"); + xml.writeAttribute("yDeviation_max", QString::number(configResult.algorithmParams.growParam.yDeviation_max)); + xml.writeAttribute("zDeviation_max", QString::number(configResult.algorithmParams.growParam.zDeviation_max)); + xml.writeAttribute("maxLineSkipNum", QString::number(configResult.algorithmParams.growParam.maxLineSkipNum)); + xml.writeAttribute("maxSkipDistance", QString::number(configResult.algorithmParams.growParam.maxSkipDistance)); + xml.writeAttribute("minLTypeTreeLen", QString::number(configResult.algorithmParams.growParam.minLTypeTreeLen)); + xml.writeAttribute("minVTypeTreeLen", QString::number(configResult.algorithmParams.growParam.minVTypeTreeLen)); + xml.writeEndElement(); // TreeGrowParam + + xml.writeEndElement(); // AlgorithmParams + + // 保存调试参数 + xml.writeStartElement("DebugParam"); + xml.writeAttribute("enableDebug", QString::number(configResult.debugParam.enableDebug)); + xml.writeAttribute("savePointCloud", QString::number(configResult.debugParam.savePointCloud)); + xml.writeAttribute("saveDebugImage", QString::number(configResult.debugParam.saveDebugImage)); + xml.writeAttribute("printDetailLog", QString::number(configResult.debugParam.printDetailLog)); + xml.writeAttribute("debugOutputPath", QString::fromStdString(configResult.debugParam.debugOutputPath)); + xml.writeEndElement(); // DebugParam + + // 保存服务端配置 + xml.writeStartElement("LocalServerConfig"); + xml.writeStartElement("ServerPort"); + xml.writeAttribute("port", QString::number(configResult.serverPort)); + xml.writeEndElement(); // ServerPort + xml.writeStartElement("TcpPort"); + xml.writeAttribute("port", QString::number(configResult.tcpPort)); + xml.writeEndElement(); // TcpPort + xml.writeEndElement(); // LocalServerConfig + + // 保存服务器列表 + xml.writeStartElement("Servers"); + for (const auto& server : configResult.servers) { + xml.writeStartElement("Server"); + xml.writeAttribute("name", QString::fromStdString(server.name)); + xml.writeAttribute("ip", QString::fromStdString(server.ip)); + xml.writeAttribute("port", QString::number(server.port)); + xml.writeEndElement(); // Server + } + xml.writeEndElement(); // Servers + + xml.writeEndElement(); // WheelMeasureConfig + xml.writeEndDocument(); + + file.close(); + + // 通知配置改变 + if (m_notify) { + m_notify->OnConfigChanged(configResult); + } + + return true; +} + +void VrWheelMeasureConfig::SetConfigChangeNotify(IVrWheelMeasureConfigChangeNotify* notify) +{ + m_notify = notify; +} diff --git a/AppAlgo/wheelArchHeigthMeasure/Arm/aarch64/libbaseAlgorithm.so b/AppAlgo/wheelArchHeigthMeasure/Arm/aarch64/libbaseAlgorithm.so index 452ad8e7..1b637bd8 100644 Binary files a/AppAlgo/wheelArchHeigthMeasure/Arm/aarch64/libbaseAlgorithm.so and b/AppAlgo/wheelArchHeigthMeasure/Arm/aarch64/libbaseAlgorithm.so differ diff --git a/AppAlgo/wheelArchHeigthMeasure/Arm/aarch64/libwheelArchHeigthMeasure.so b/AppAlgo/wheelArchHeigthMeasure/Arm/aarch64/libwheelArchHeigthMeasure.so index 561247c9..561bd7cd 100644 Binary files a/AppAlgo/wheelArchHeigthMeasure/Arm/aarch64/libwheelArchHeigthMeasure.so and b/AppAlgo/wheelArchHeigthMeasure/Arm/aarch64/libwheelArchHeigthMeasure.so differ diff --git a/AppAlgo/wheelArchHeigthMeasure/Inc/SG_algo_Export.h b/AppAlgo/wheelArchHeigthMeasure/Inc/SG_algo_Export.h index f53e18ea..2ab702ea 100644 --- a/AppAlgo/wheelArchHeigthMeasure/Inc/SG_algo_Export.h +++ b/AppAlgo/wheelArchHeigthMeasure/Inc/SG_algo_Export.h @@ -1,22 +1,22 @@ -#pragma once - -#if defined(_MSC_VER) || defined(WIN64) || defined(_WIN64) || defined(__WIN64__) || defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__NT__) -# define Q_DECL_EXPORT __declspec(dllexport) -# define Q_DECL_IMPORT __declspec(dllimport) -#else -# define Q_DECL_EXPORT __attribute__((visibility("default"))) -# define Q_DECL_IMPORT __attribute__((visibility("default"))) -#endif - -#if defined(SG_API_LIBRARY) -# define SG_APISHARED_EXPORT Q_DECL_EXPORT -#else -# define SG_APISHARED_EXPORT Q_DECL_IMPORT -#endif - -#include "SG_baseDataType.h" - - -#ifndef M_PI -#define M_PI 3.14159265358979323846 // pi -#endif // !M_PI +#pragma once + +#if defined(_MSC_VER) || defined(WIN64) || defined(_WIN64) || defined(__WIN64__) || defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__NT__) +# define Q_DECL_EXPORT __declspec(dllexport) +# define Q_DECL_IMPORT __declspec(dllimport) +#else +# define Q_DECL_EXPORT __attribute__((visibility("default"))) +# define Q_DECL_IMPORT __attribute__((visibility("default"))) +#endif + +#if defined(SG_API_LIBRARY) +# define SG_APISHARED_EXPORT Q_DECL_EXPORT +#else +# define SG_APISHARED_EXPORT Q_DECL_IMPORT +#endif + +#include "SG_baseDataType.h" + + +#ifndef M_PI +#define M_PI 3.14159265358979323846 // pi +#endif // !M_PI diff --git a/AppAlgo/wheelArchHeigthMeasure/Inc/SG_baseDataType.h b/AppAlgo/wheelArchHeigthMeasure/Inc/SG_baseDataType.h index 6630ea81..267b8539 100644 --- a/AppAlgo/wheelArchHeigthMeasure/Inc/SG_baseDataType.h +++ b/AppAlgo/wheelArchHeigthMeasure/Inc/SG_baseDataType.h @@ -1,547 +1,614 @@ -#pragma once - -#include -#include -#include - -#define PI 3.14159265358979323846 - -// ŷǽṹ壨λȣ -typedef struct{ - double roll; // Xת - double pitch; // Yת - double yaw; // Zתƫ -}SSG_EulerAngles; - -//ԹΪX(ɨ跽򣩣ƽеZᴹֱΪYᣬֱΪZ -typedef struct -{ - bool validFlag; //ָʾǷЧ - double x; - double y; - double z; - double pitchAngle; //ǣYƫת, - double rollAngle; //תǣXƫת, - double yawAngle; //ƫתǣZƫת, -}SSG_6AxisAttitude; - -typedef struct -{ - bool validFlag; //ָʾǷЧ - double x; - double y; - double z; -}SSG_3AxisAttitude; - -typedef enum -{ - keSG_PoseSorting_Uknown = 0, - keSG_PoseSorting_ZMin2Max, ///Z - keSG_PoseSorting_L2R_T2B, ///ңϵ - keSG_PoseSorting_T2B_L2R, ///ϵ£ -} ESG_poseSortingMode; - -typedef struct -{ - int data_0; - int data_1; - int idx; -}SSG_intPair; - -typedef struct -{ - int featurType; - int featureIdx_v; - int featureIdx_h; - int clusterID; - int flag; - int lineIdx; - int ptIdx; -}SSG_featureClusteringInfo; - -typedef struct -{ - double left; - double right; - double top; - double bottom; -}SSG_ROIRectD; - -struct HSV { - double h; // ɫ (0-360) - double s; // Ͷ (0-1) - double v; // (0-1) -}; - -struct RGB { - int r; - int g; - int b; -}; - -#define LINE_FEATURE_NUM 16 -#define LINE_FEATURE_UNDEF 0 -#define LINE_FEATURE_L_JUMP_H2L 1 -#define LINE_FEATURE_L_JUMP_L2H 2 -#define LINE_FEATURE_L_SLOPE_H2L 3 -#define LINE_FEATURE_L_SLOPE_L2H 4 -#define LINE_FEATURE_V_SLOPE 5 -#define LINE_FEATURE_LINE_ENDING_0 6 //ending -#define LINE_FEATURE_LINE_ENDING_1 7 //endingյ -#define LINE_FEATURE_RGN_EDGE 8 //ʱѾõĿıԵ -#define LINE_FEATURE_RIGHT_ANGLE_HR 9 //ֱ:ˮƽ- -#define LINE_FEATURE_RIGHT_ANGLE_HF 10 //ֱ:ˮƽ-½ -#define LINE_FEATURE_RIGHT_ANGLE_RH 11 //ֱ:-ˮƽ -#define LINE_FEATURE_RIGHT_ANGLE_FH 12 //ֱ:½-ˮƽ -#define LINE_FEATURE_PEAK_TOP 13 -#define LINE_FEATURE_PEAK_BOTTOM 14 -#define LINE_FEATURE_CORNER_V 15 - - -typedef struct -{ - int featureType; - SVzNL2DPoint jumpPos2D; - SVzNL3DPoint jumpPos; - double featureValue; -}SSG_basicFeature1D; - -typedef struct -{ - int lineIdx; - double gap_start; - double gap_width; - double gap_depth; - SVzNL3DPosition gapPt_0; //gapĶ˵ - SVzNL3DPosition gapPt_1; //gapĶ˵ - SSG_ROIRectD roi; -}SSG_basicFeatureGap; - -#define FEATURE_FLAG_UNDEF 0 -#define FEATURE_FLAG_INVLD_START 1 -#define FEATURE_FLAG_INVLD_END 2 -#define FEATURE_FLAG_VALID_START 3 -#define FEATURE_FLAG_VALID_END 4 -#define FEATURE_FLAG_INVALID 5 -typedef struct -{ - int flag; - int lineIdx; - int startPtIdx; - int endPtIdx; - int midPtIdx; - SVzNL3DPoint midPt; - double width; - double pkHeight; -}SSG_featureSemiCircle; - -typedef struct -{ - int lineIdx; - std::vector features; - std::vector endings; -}SSG_lineFeature; - -typedef struct -{ - /* ǷޣСڴޣΪ */ - double continuityTh; //ޡʹzжʱΪzޣʹõжʱΪ - /* жޣεĵСڴޣΪ */ - double outlierTh; -}SSG_outlierFilterParam; - -typedef struct -{ - double LSlopeZWin; //LSlope¶ȼĴڳ - double validSlopeH; - double minLJumpH; - double minEndingGap;//ޡڴޣΪ -}SSG_slopeParam; - -typedef struct -{ - double minEndingGap; //yޡڴޣΪ - double minEndingGap_z; //zޡڴޣΪ - double scale; //㷽ǵĴڱ - double cornerTh; //սޣڴޣΪЧյ - double jumpCornerTh_1; //жϹսǷΪޡһ޴jumpCornerTh_1һСjumpCornerTh_2ʱ - double jumpCornerTh_2; -}SSG_cornerParam; - -typedef struct -{ - double segGapTh_y; //yޡڴޣΪ - double segGapTh_z; //zޡڴޣΪ - double maxDist; //㷽ǵĴڱ -}SSG_lineSegParam; - -typedef struct -{ - double scale_angle; //㷽ǵĴڱ - double scale_corner; //㷽תǵĴڱ - double cornerTh; //սޣڴޣΪЧArc -}SSG_gloveArcParam; - -typedef struct -{ - double H_len; //ֱˮƽεij - double V_len; //ֱֱεij - double maxDelta; -}SSG_lineRightAngleParam; - -typedef struct -{ - double valleyMinH; - double valleyMaxW; -}SSG_VFeatureParam; - -typedef struct -{ - int lineIdx; - int startPtIdx; - int endPtIdx; - SVzNL3DPoint startPt; - SVzNL3DPoint endPt; -}SWD_segFeature; - -typedef struct -{ - double sameGapTh; - int gapChkWin; -}SSG_tearFeatureExtactPara; -typedef struct -{ - double yDeviation_max;//ʱYƫ - double zDeviation_max; //ʱZƫ - int maxLineSkipNum; //ʱ߼ -1ʱʹmaxDkipDistance - double maxSkipDistance; //maxLineSkipNumΪ-1 ʹô˲.Ϊ-1ʱ˲Ч - double minLTypeTreeLen; //ٵĽڵĿСڴĿƳ - double minVTypeTreeLen; //ٵĽڵĿСڴĿƳ -}SSG_treeGrowParam; - -typedef struct -{ - double bagL; // - double bagW; // - double bagH; // -}SSG_bagParam; - -typedef struct -{ - double length; // - double width; // - double height; // -}SWD_sizeParam; - -typedef struct -{ - SVzNLRangeD xRange; //< XΧ - SVzNLRangeD yRange; //< YΧ - SVzNLRangeD zRange; //< ZΧ - double scale_x; - double scale_y; -} SWD_pointCloudPara; - -typedef struct -{ - ESG_poseSortingMode sortMode; -}SSG_objSortParam; - -typedef struct -{ - double angleStep; - double radiusStep; -}SSG_polarScanParam; - -typedef struct -{ - int treeState; - int treeType; - int sLineIdx; - int eLineIdx; - double tree_value; - SSG_ROIRectD roi; - std::vector< SSG_basicFeature1D> treeNodes; - int angleChkScalePos; //ڼangleCheckٶ -}SSG_featureTree; - -typedef struct -{ - int treeState; - int treeType; - int sLineIdx; - int eLineIdx; - double tree_value; - SSG_ROIRectD roi; - std::vector< SSG_basicFeatureGap> treeNodes; - int angleChkScalePos; //ڼangleCheckٶ -}SSG_gapFeatureTree; - -typedef struct -{ - int treeState; - int treeType; - int sLineIdx; - int eLineIdx; - SSG_ROIRectD roi; - SVzNL3DPoint centerPt; - SVzNL2DPoint centerPos; - int treeMidType; //1:Ѿӹ - std::vector< SSG_featureSemiCircle> treeNodes; -}SSG_semiCircleFeatureTree; - -typedef struct -{ - int treeState; - int sLineIdx; - int eLineIdx; - double tree_value; - std::vector< SWD_segFeature> treeNodes; -}SWD_segFeatureTree; - -typedef struct -{ - int vTreeFlag; - int treeIdx; - int treeType; - int sLineIdx; - int eLineIdx; - SSG_ROIRectD roi; -}SSG_treeInfo; - -typedef struct -{ - int seachW_lines; - int searchW_pts; -}SSG_localPkParam; - -typedef struct -{ - int x; - int y; - int value; - double valueD; - int sideID; //1-T, 2-B, 3-L, 4-R -}SSG_2DValueI; - -typedef struct -{ - int start; - int len; - int value; -}SSG_RUN; - -typedef struct -{ - int start; - int len; - int value; - bool start_zRising; //z־ - bool end_zRising;//յz־ -}SSG_RUN_EX; - -typedef struct -{ - double mean; - double var; -}SSG_meanVar; - -typedef struct -{ - int x; - int y; - int type; - int edgeId; - int blockFlag;//ڵ־ - double scanDist; - SVzNL3DPoint edgePt; -}SSG_contourPtInfo; - -typedef struct -{ - int lineIdx; - std::vector contourPts; -}SSG_lineConotours; - -typedef struct -{ - int lineIdx; - int edgeId_0; - int edgeId_1; - double ptPairDist; - SSG_contourPtInfo contourPt_0; - SSG_contourPtInfo contourPt_1; -}SSG_conotourPair; - -typedef struct -{ - double x; - double y; - double z; - double x_roll; - double y_pitch; - double z_yaw; -}SSG_6DOF; - -typedef struct -{ - double L; - double W; - double H; - double angle; - SVzNL3DPoint endings[4]; -} SSG_boxCarDimension; - -typedef struct -{ - SVzNL3DPoint opCenter; //λ - double objR; -}SWD_motorStatorPosition; - -typedef struct -{ - int objID; - double grasperAngle; - double rotateAngle; //ץ0λʱΪѾΣ120ȡ - double graspR; -}SWD_statorOuterGrasper; - -typedef struct -{ - int pkRgnIdx; - SVzNLSizeD objSize; - SVzNL2DPoint pos2D; - SSG_6DOF centerPos; -}SSG_peakRgnInfo; - -typedef struct -{ - int pkRgnIdx; - SVzNLSizeD objSize; - SVzNL2DPoint pos2D; - SSG_6DOF centerPos; - int orienFlag; //0-δ֪1 - 棬 2- -}SSG_peakOrienRgnInfo; - -typedef struct -{ - SVzNL3DPoint sPt; - SVzNL3DPoint ePt; - double scanDist; - int ptNum; -}SSG_contourEdgeInfo; - -typedef struct -{ - double meanDist; - double varDist; - double minDist; - double maxDist; - int matchNum; - int level2_num; //level2Ϊ߿Ŷ(ΪJumpEnding; - int level1_num; //level1-ΪпŶȣһΪJumpEnding - SSG_ROIRectD roi; -}SSG_edgeMatchInfo; - -typedef struct -{ - int id1; - int id2; - int matchPts; - int matchType; //Ŷȣ 2Ϊ߿Ŷ(ΪJumpEnding; 1-ΪпŶȣһΪJumpEnding0-ΪͿŶ - double matchValue; - double varValue; - double minMatchValue; - double maxMatchValue; - SSG_ROIRectD roi; -}SSG_matchPair; - -typedef struct -{ - int objIdx; - bool isValid; //ǷΪϸĿꡣĿ꣬֯ţɨ赽ߣΪϸĿ - SSG_ROIRectD objROI; - SVzNL3DPoint objPos; - SSG_6DOF graspPos; -}SSG_sideBagInfo; - -typedef struct -{ - SVzNLRect roi; - int ptCounter; - int labelID; -}SSG_Region; - -typedef struct -{ - double planeCalib[9]; //ת󣬽Ƶƽ - double planeHeight;//ƽĸ߶ȣȥ - double invRMatrix[9]; //ת󣬻صԭϵ -}SSG_planeCalibPara; - -typedef struct -{ - int pntIdx; - int type; - double forwardAngle; //ǰ - double backwardAngle; // - double corner; //ս - double forwardDiffZ; - double backwardDiffZ; - double pre_stepDist; - double post_stepDist; - double forward_z; - double backward_z; -}SSG_pntDirAngle; - -// ͼݽṹ -typedef struct { - int width; - int height; - std::vector> gray; // Ҷͼ - std::vector> markers; // ͼ-1ʾˮ룬0ʾδǣ>0ʾ -}SWD_waterShedImage; - -//ƶάMark -typedef struct -{ - int markID; - SVzNL3DPoint mark3D; -}SWD_charuco3DMark; - -typedef struct -{ - SVzNL3DPoint pt1; - SVzNL3DPoint pt2; -}SWD_3DPointPair; - -typedef struct -{ - int pkId; - int lineIdx; - int ptIdx; - int cptIndex; //Բɨϵĵ - //double cornerAngle; //ԵΪĵҵļн - double R; - double angle; - double x; - double y; - double z; -}SWD_polarPt; - -typedef struct -{ - int cptIndex; - int L1_ptIndex; - int L2_ptIndex; - double cornerAngle; - int cornerDir; -}SWD_polarPeakInfo; - -typedef struct -{ - int clusterIdx; - int ptSize; - SVzNL3DRangeD roi3D; - SVzNLRect roi2D; -}SWD_clustersInfo; +#pragma once + +#include +#include +#include + +#define PI 3.14159265358979323846 + +// ŷǽṹ壨λȣ +typedef struct{ + double roll; // Xת + double pitch; // Yת + double yaw; // Zתƫ +}SSG_EulerAngles; + +//ԹΪX(ɨ跽򣩣ƽеZᴹֱΪYᣬֱΪZ +typedef struct +{ + bool validFlag; //ָʾǷЧ + double x; + double y; + double z; + double pitchAngle; //ǣYƫת, + double rollAngle; //תǣXƫת, + double yawAngle; //ƫתǣZƫת, +}SSG_6AxisAttitude; + +typedef struct +{ + double x; + double y; + double z; +}SWD3DPoint; + +typedef struct +{ + int lineIdx; + int ptIdx; + SWD3DPoint point; +}SWDIndexing3DPoint; + +typedef struct +{ + int start; + int end; +}SWD_Interval; // + +typedef struct +{ + bool validFlag; //ָʾǷЧ + double x; + double y; + double z; +}SSG_3AxisAttitude; + +typedef enum +{ + keSG_PoseSorting_Uknown = 0, + keSG_PoseSorting_ZMin2Max, ///Z + keSG_PoseSorting_L2R_T2B, ///ңϵ + keSG_PoseSorting_T2B_L2R, ///ϵ£ +} ESG_poseSortingMode; + +typedef enum +{ + KeWD_Mask_ValidPt = 0, + KeWD_Mask_NullPt, +}EWD_maskMode; + +typedef struct +{ + int data_0; + int data_1; + int idx; +}SSG_intPair; + +typedef struct +{ + int flag; + int validFlag; + int clusterID; +}SSG_clusterLabel; + +typedef struct +{ + int featurType; + int featureIdx_v; + int featureIdx_h; + int clusterID; + int flag; + int lineIdx; + int ptIdx; +}SSG_featureClusteringInfo; + +typedef struct +{ + double left; + double right; + double top; + double bottom; +}SSG_ROIRectD; + +typedef struct +{ + double width; + double height; +}SSG_size2D; + +typedef struct +{ + SVzNL3DPoint center; + double radius; +}SWD_HoleInfo; + +struct HSV { + double h; // ɫ (0-360) + double s; // Ͷ (0-1) + double v; // (0-1) +}; + +struct RGB { + int r; + int g; + int b; +}; + +#define LINE_FEATURE_NUM 16 +#define LINE_FEATURE_UNDEF 0 +#define LINE_FEATURE_L_JUMP_H2L 1 +#define LINE_FEATURE_L_JUMP_L2H 2 +#define LINE_FEATURE_L_SLOPE_H2L 3 +#define LINE_FEATURE_L_SLOPE_L2H 4 +#define LINE_FEATURE_V_SLOPE 5 +#define LINE_FEATURE_LINE_ENDING_0 6 //ending +#define LINE_FEATURE_LINE_ENDING_1 7 //endingյ +#define LINE_FEATURE_RGN_EDGE 8 //ʱѾõĿıԵ +#define LINE_FEATURE_RIGHT_ANGLE_HR 9 //ֱ:ˮƽ- +#define LINE_FEATURE_RIGHT_ANGLE_HF 10 //ֱ:ˮƽ-½ +#define LINE_FEATURE_RIGHT_ANGLE_RH 11 //ֱ:-ˮƽ +#define LINE_FEATURE_RIGHT_ANGLE_FH 12 //ֱ:½-ˮƽ +#define LINE_FEATURE_PEAK_TOP 13 +#define LINE_FEATURE_PEAK_BOTTOM 14 +#define LINE_FEATURE_CORNER_V 15 + + +typedef struct +{ + int featureType; + SVzNL2DPoint jumpPos2D; + SVzNL3DPoint jumpPos; + double featureValue; +}SSG_basicFeature1D; + +typedef struct +{ + int lineIdx; + double gap_start; + double gap_width; + double gap_depth; + SVzNL3DPosition gapPt_0; //gapĶ˵ + SVzNL3DPosition gapPt_1; //gapĶ˵ + SSG_ROIRectD roi; +}SSG_basicFeatureGap; + +#define FEATURE_FLAG_UNDEF 0 +#define FEATURE_FLAG_INVLD_START 1 +#define FEATURE_FLAG_INVLD_END 2 +#define FEATURE_FLAG_VALID_START 3 +#define FEATURE_FLAG_VALID_END 4 +#define FEATURE_FLAG_INVALID 5 +typedef struct +{ + int flag; + int lineIdx; + int startPtIdx; + int endPtIdx; + int midPtIdx; + SVzNL3DPoint midPt; + double width; + double pkHeight; +}SSG_featureSemiCircle; + +typedef struct +{ + int lineIdx; + std::vector features; + std::vector endings; +}SSG_lineFeature; + +typedef struct +{ + /* ǷޣСڴޣΪ */ + double continuityTh; //ޡʹzжʱΪzޣʹõжʱΪ + /* жޣεĵСڴޣΪ */ + double outlierTh; +}SSG_outlierFilterParam; + +typedef struct +{ + double LSlopeZWin; //LSlope¶ȼĴڳ + double validSlopeH; + double minLJumpH; + double minEndingGap;//ޡڴޣΪ +}SSG_slopeParam; + +typedef struct +{ + double minEndingGap; //yޡڴޣΪ + double minEndingGap_z; //zޡڴޣΪ + double scale; //㷽ǵĴڱ + double cornerTh; //սޣڴޣΪЧյ + double jumpCornerTh_1; //жϹսǷΪޡһ޴jumpCornerTh_1һСjumpCornerTh_2ʱ + double jumpCornerTh_2; +}SSG_cornerParam; + +typedef struct +{ + double segGapTh_y; //yޡڴޣΪ + double segGapTh_z; //zޡڴޣΪ + double distScale; //㷽ǵĴڱ +}SSG_lineSegParam; + +typedef struct +{ + double minJumpZ; //z + double minK; //Сб + SVzNLRangeD widthRange; //zޡڴޣΪ +}SSG_raisedFeatureParam; + +typedef struct +{ + double scale_angle; //㷽ǵĴڱ + double scale_corner; //㷽תǵĴڱ + double cornerTh; //սޣڴޣΪЧArc +}SSG_gloveArcParam; + +typedef struct +{ + double H_len; //ֱˮƽεij + double V_len; //ֱֱεij + double maxDelta; +}SSG_lineRightAngleParam; + +typedef struct +{ + double valleyMinH; + double valleyMaxW; +}SSG_VFeatureParam; + +typedef struct +{ + int lineIdx; + int startPtIdx; + int endPtIdx; + SVzNL3DPoint startPt; + SVzNL3DPoint endPt; + double featureValue; +}SWD_segFeature; + +typedef struct +{ + double sameGapTh; + int gapChkWin; +}SSG_tearFeatureExtactPara; +typedef struct +{ + double yDeviation_max;//ʱYƫ + double zDeviation_max; //ʱZƫ + int maxLineSkipNum; //ʱ߼ -1ʱʹmaxDkipDistance + double maxSkipDistance; //maxLineSkipNumΪ-1 ʹô˲.Ϊ-1ʱ˲Ч + double minLTypeTreeLen; //ٵĽڵĿСڴĿƳ + double minVTypeTreeLen; //ٵĽڵĿСڴĿƳ +}SSG_treeGrowParam; + +typedef struct +{ + double bagL; // + double bagW; // + double bagH; // +}SSG_bagParam; + +typedef struct +{ + double length; // + double width; // + double height; // +}SWD_sizeParam; + +typedef struct +{ + SVzNLRangeD xRange; //< XΧ + SVzNLRangeD yRange; //< YΧ + SVzNLRangeD zRange; //< ZΧ + double scale_x; + double scale_y; +} SWD_pointCloudPara; + +typedef struct +{ + ESG_poseSortingMode sortMode; +}SSG_objSortParam; + +typedef struct +{ + double angleStep; + double radiusStep; +}SSG_polarScanParam; + +typedef struct +{ + int treeState; + int treeType; + int sLineIdx; + int eLineIdx; + double tree_value; + SSG_ROIRectD roi; + std::vector< SSG_basicFeature1D> treeNodes; + int angleChkScalePos; //ڼangleCheckٶ +}SSG_featureTree; + +typedef struct +{ + int treeState; + int treeType; + int sLineIdx; + int eLineIdx; + double tree_value; + SSG_ROIRectD roi; + std::vector< SSG_basicFeatureGap> treeNodes; + int angleChkScalePos; //ڼangleCheckٶ +}SSG_gapFeatureTree; + +typedef struct +{ + int treeState; + int treeType; + int sLineIdx; + int eLineIdx; + SSG_ROIRectD roi; + SVzNL3DPoint centerPt; + SVzNL2DPoint centerPos; + int treeMidType; //1:Ѿӹ + std::vector< SSG_featureSemiCircle> treeNodes; +}SSG_semiCircleFeatureTree; + +typedef struct +{ + int treeState; + int sLineIdx; + int eLineIdx; + double tree_value; + std::vector< SWD_segFeature> treeNodes; +}SWD_segFeatureTree; + +typedef struct +{ + int vTreeFlag; + int treeIdx; + int treeType; + int sLineIdx; + int eLineIdx; + SSG_ROIRectD roi; +}SSG_treeInfo; + +typedef struct +{ + int seachW_lines; + int searchW_pts; +}SSG_localPkParam; + +typedef struct +{ + int x; + int y; + int value; + double valueD; + int sideID; //1-T, 2-B, 3-L, 4-R +}SSG_2DValueI; + +typedef struct +{ + int start; + int len; + int value; +}SSG_RUN; + +typedef struct +{ + int start; + int len; + int value; + bool start_zRising; //z־ + bool end_zRising;//յz־ +}SSG_RUN_EX; + +typedef struct +{ + double mean; + double var; +}SSG_meanVar; + +typedef struct +{ + int x; + int y; + int type; + int edgeId; + int blockFlag;//ڵ־ + double scanDist; + SVzNL3DPoint edgePt; +}SSG_contourPtInfo; + +typedef struct +{ + int lineIdx; + std::vector contourPts; +}SSG_lineConotours; + +typedef struct +{ + int lineIdx; + int edgeId_0; + int edgeId_1; + double ptPairDist; + SSG_contourPtInfo contourPt_0; + SSG_contourPtInfo contourPt_1; +}SSG_conotourPair; + +typedef struct +{ + double x; + double y; + double z; + double x_roll; + double y_pitch; + double z_yaw; +}SSG_6DOF; + +typedef struct +{ + double L; + double W; + double H; + double angle; + SVzNL3DPoint endings[4]; +} SSG_boxCarDimension; + +typedef struct +{ + int objID; + SSG_6DOF opCenter; //λ +}SWD_statorInnerGrasper; + +typedef struct +{ + int objID; + double grasperAngle; + double rotateAngle; //ץ0λʱΪѾΣ120ȡ + double graspR; +}SWD_statorOuterGrasper; + +typedef struct +{ + int pkRgnIdx; + SVzNLSizeD objSize; + SVzNL2DPoint pos2D; + SSG_6DOF centerPos; +}SSG_peakRgnInfo; + +typedef struct +{ + int pkRgnIdx; + SVzNLSizeD objSize; + SVzNL2DPoint pos2D; + SSG_6DOF centerPos; + int orienFlag; //0-δ֪1 - 棬 2- +}SSG_peakOrienRgnInfo; + +typedef struct +{ + SVzNL3DPoint sPt; + SVzNL3DPoint ePt; + double scanDist; + int ptNum; +}SSG_contourEdgeInfo; + +typedef struct +{ + double meanDist; + double varDist; + double minDist; + double maxDist; + int matchNum; + int level2_num; //level2Ϊ߿Ŷ(ΪJumpEnding; + int level1_num; //level1-ΪпŶȣһΪJumpEnding + SSG_ROIRectD roi; +}SSG_edgeMatchInfo; + +typedef struct +{ + int id1; + int id2; + int matchPts; + int matchType; //Ŷȣ 2Ϊ߿Ŷ(ΪJumpEnding; 1-ΪпŶȣһΪJumpEnding0-ΪͿŶ + double matchValue; + double varValue; + double minMatchValue; + double maxMatchValue; + SSG_ROIRectD roi; +}SSG_matchPair; + +typedef struct +{ + int objIdx; + bool isValid; //ǷΪϸĿꡣĿ꣬֯ţɨ赽ߣΪϸĿ + SSG_ROIRectD objROI; + SVzNL3DPoint objPos; + SSG_6DOF graspPos; +}SSG_sideBagInfo; + +typedef struct +{ + SVzNLRect roi; + int ptCounter; + int labelID; +}SSG_Region; + +typedef struct +{ + double planeCalib[9]; //ת󣬽Ƶƽ + double planeHeight;//ƽĸ߶ȣȥ + double invRMatrix[9]; //ת󣬻صԭϵ +}SSG_planeCalibPara; + +typedef struct +{ + int pkId; + int lineIdx; + int ptIdx; + int cptIndex; //Բɨϵĵ + //double cornerAngle; //ԵΪĵҵļн + double R; + double angle; + double x; + double y; + double z; +}SWD_polarPt; + +typedef struct +{ + int pntIdx; + int type; + double forwardAngle; //ǰ + double backwardAngle; // + double corner; //ս + double forwardDiffZ; + double backwardDiffZ; + double pre_stepDist; + double post_stepDist; + double forward_z; + double backward_z; +}SSG_pntDirAngle; + +typedef struct +{ + double forwardAngle; //ǰ + double backwardAngle; // + double corner; //ս + int pntIdx; + int forward_pntIdx; + int backward_pntIdx; + int flag; + SWD_polarPt point; +}SSG_dirCornerAngle; + +// ͼݽṹ +typedef struct { + int width; + int height; + std::vector> gray; // Ҷͼ + std::vector> markers; // ͼ-1ʾˮ룬0ʾδǣ>0ʾ +}SWD_waterShedImage; + +//ƶάMark +typedef struct +{ + int markID; + SVzNL3DPoint mark3D; +}SWD_charuco3DMark; + +typedef struct +{ + SVzNL3DPoint pt1; + SVzNL3DPoint pt2; +}SWD_3DPointPair; + +typedef struct +{ + int cptIndex; + int L1_ptIndex; + int L2_ptIndex; + double cornerAngle; + int cornerDir; + int flag; + SWD_polarPt point; +}SWD_polarPeakInfo; + +typedef struct +{ + int clusterIdx; + int ptSize; + SVzNL3DRangeD roi3D; + SVzNLRect roi2D; +}SWD_clustersInfo; diff --git a/AppAlgo/wheelArchHeigthMeasure/Inc/SG_errCode.h b/AppAlgo/wheelArchHeigthMeasure/Inc/SG_errCode.h index 0c90f71d..2b493b57 100644 --- a/AppAlgo/wheelArchHeigthMeasure/Inc/SG_errCode.h +++ b/AppAlgo/wheelArchHeigthMeasure/Inc/SG_errCode.h @@ -1,30 +1,38 @@ -#pragma once - -#define SG_ERR_3D_DATA_INVLD -1000 -#define SG_ERR_3D_DATA_NULL -1001 -#define SG_ERR_FOUND_NO_TOP_PLANE -1002 -#define SG_ERR_NOT_GRID_FORMAT -1003 -#define SG_ERR_LABEL_INFO_ERROR -1004 -#define SG_ERR_INVLD_SORTING_MODE -1005 -#define SG_ERR_INVLD_Q_SCALE -1006 - -//BQ_workpiece -#define SX_ERR_INVLD_VTREE_NUM -2001 -#define SX_ERR_INVLD_HTREE_NUM -2002 -#define SX_ERR_INVLD_EDGE_LINK_NUM -2003 -#define SX_ERR_INVLD_CLOSES_PT -2004 -#define SX_ERR_ZERO_CONTOUR_PT -2005 -#define SX_ERR_INVLID_RPEAK_NUM -2006 -#define SX_ERR_INVLID_RPEAK_PAIR -2007 -#define SX_ERR_INVLID_MARK_NUM -2008 - -//ץȡ -#define SX_ERR_INVLID_CUTTING_Z -2101 -#define SX_ERR_ZERO_OBJ -2102 - -// -#define SX_BAG_TRAY_EMPTY -2201 - -//ü߶Ȳ -#define SX_ERR_INVALID_ARC -2301 - +#pragma once + +#define SG_ERR_3D_DATA_INVLD -1000 +#define SG_ERR_3D_DATA_NULL -1001 +#define SG_ERR_FOUND_NO_TOP_PLANE -1002 +#define SG_ERR_NOT_GRID_FORMAT -1003 +#define SG_ERR_LABEL_INFO_ERROR -1004 +#define SG_ERR_INVLD_SORTING_MODE -1005 +#define SG_ERR_INVLD_Q_SCALE -1006 +#define SG_ERR_ZERO_OBJECTS -1007 +#define SG_ERR_LASER_DIR_NOT_SUPPORTED -1008 +#define SG_ERR_SCAN_DIR_NOT_SUPPORTED -1009 + +//BQ_workpiece +#define SX_ERR_INVLD_VTREE_NUM -2001 +#define SX_ERR_INVLD_HTREE_NUM -2002 +#define SX_ERR_INVLD_EDGE_LINK_NUM -2003 +#define SX_ERR_INVLD_CLOSES_PT -2004 +#define SX_ERR_ZERO_CONTOUR_PT -2005 +#define SX_ERR_INVLID_RPEAK_NUM -2006 +#define SX_ERR_INVLID_RPEAK_PAIR -2007 +#define SX_ERR_INVLID_MARK_NUM -2008 + +//ץȡ +#define SX_ERR_INVLID_CUTTING_Z -2101 +#define SX_ERR_ZERO_OBJ_TOPLAYER -2102 +#define SX_ERR_ZERO_OBJ_BTMLAYER -2103 +#define SX_ERR_GET_INVALID_PALTE -2104 //ȡ + +// +#define SX_BAG_TRAY_EMPTY -2201 + +//ü߶Ȳ +#define SX_ERR_INVALID_ARC -2301 + +//ǴӲ +#define SX_ERR_NO_MARK -2401 + diff --git a/AppAlgo/wheelArchHeigthMeasure/Inc/wheelArchHeigthMeasure_Export.h b/AppAlgo/wheelArchHeigthMeasure/Inc/wheelArchHeigthMeasure_Export.h index d2b89128..0f952de9 100644 --- a/AppAlgo/wheelArchHeigthMeasure/Inc/wheelArchHeigthMeasure_Export.h +++ b/AppAlgo/wheelArchHeigthMeasure/Inc/wheelArchHeigthMeasure_Export.h @@ -1,44 +1,49 @@ -#pragma once - -#include "SG_algo_Export.h" -#include - -#define _OUTPUT_DEBUG_DATA 1 - -typedef struct -{ - SVzNL3DPoint wheelArchPos; - SVzNL3DPoint wheelUpPos; - SVzNL3DPoint wheelDownPos; - SVzNL3DPoint arcLine[2]; - SVzNL3DPoint upLine[2]; - SVzNL3DPoint downLine[2]; - SVzNL3DPoint centerLine[2]; - double archToCenterHeigth; - double archToGroundHeigth; -}WD_wheelArchInfo; - -//汾 -SG_APISHARED_EXPORT const char* wd_wheelArchHeigthMeasureVersion(void); - -//ˮƽװƽ -//ZƽеʱҪԵΪգˮƽ -//תΪƽƽ淨ΪֱIJ -SG_APISHARED_EXPORT SSG_planeCalibPara wd_horizonCamera_getGroundCalibPara( - std::vector< std::vector>& scanLines); - -//ˮƽʱ̬ƽȥ -SG_APISHARED_EXPORT void wd_horizonCamera_lineDataR( - std::vector< SVzNL3DPosition>& a_line, - const double* camPoseR, - double groundH); - -//ȡǵ㼰λϢ -SG_APISHARED_EXPORT WD_wheelArchInfo wd_wheelArchHeigthMeasure( - std::vector< std::vector>& scanLines, - const SSG_cornerParam cornerPara, - const SSG_lineSegParam lineSegPara, - const SSG_outlierFilterParam filterParam, - const SSG_treeGrowParam growParam, - const SSG_planeCalibPara groundCalibPara, - int* errCode); +#pragma once + +#include "SG_algo_Export.h" +#include + +#define _OUTPUT_DEBUG_DATA 1 + +typedef struct +{ + SVzNL3DPoint wheelArchPos; + SVzNL3DPoint wheelUpPos; + SVzNL3DPoint wheelDownPos; + SVzNL3DPoint arcLine[2]; + SVzNL3DPoint upLine[2]; + SVzNL3DPoint downLine[2]; + SVzNL3DPoint centerLine[2]; + double archToCenterHeigth; + double archToGroundHeigth; +}WD_wheelArchInfo; + +//汾 +SG_APISHARED_EXPORT const char* wd_wheelArchHeigthMeasureVersion(void); + +//ˮƽװƽ +//ZƽеʱҪԵΪգˮƽ +//תΪƽƽ淨ΪֱIJ +SG_APISHARED_EXPORT SSG_planeCalibPara wd_horizonCamera_getGroundCalibPara( + std::vector< std::vector>& scanLines); + +//ˮƽʱ̬ƽȥ +SG_APISHARED_EXPORT void wd_horizonCamera_lineDataR( + std::vector< SVzNL3DPosition>& a_line, + const double* camPoseR, + double groundH); + +//ˮƽʱ̬ƽȥ +SG_APISHARED_EXPORT bool wd_wheelPresenseDetection( + std::vector>& scanLine, + const SVzNL3DRangeD wheelRoi3d); + +//ȡǵ㼰λϢ +SG_APISHARED_EXPORT WD_wheelArchInfo wd_wheelArchHeigthMeasure( + std::vector< std::vector>& scanLines, + const SSG_cornerParam cornerPara, + const SSG_lineSegParam lineSegPara, + const SSG_outlierFilterParam filterParam, + const SSG_treeGrowParam growParam, + const SSG_planeCalibPara groundCalibPara, + int* errCode); diff --git a/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Debug/baseAlgorithm.dll b/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Debug/baseAlgorithm.dll index 51ec9995..39a69344 100644 Binary files a/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Debug/baseAlgorithm.dll and b/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Debug/baseAlgorithm.dll differ diff --git a/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Debug/baseAlgorithm.lib b/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Debug/baseAlgorithm.lib index a5111f24..9f759a09 100644 Binary files a/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Debug/baseAlgorithm.lib and b/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Debug/baseAlgorithm.lib differ diff --git a/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Debug/baseAlgorithm.pdb b/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Debug/baseAlgorithm.pdb index 2dd5e466..1e91f1a1 100644 Binary files a/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Debug/baseAlgorithm.pdb and b/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Debug/baseAlgorithm.pdb differ diff --git a/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Debug/wheelArchHeigthMeasure.dll b/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Debug/wheelArchHeigthMeasure.dll index c5773bdb..8f85a243 100644 Binary files a/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Debug/wheelArchHeigthMeasure.dll and b/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Debug/wheelArchHeigthMeasure.dll differ diff --git a/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Debug/wheelArchHeigthMeasure.lib b/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Debug/wheelArchHeigthMeasure.lib index 1cd911f9..e191b052 100644 Binary files a/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Debug/wheelArchHeigthMeasure.lib and b/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Debug/wheelArchHeigthMeasure.lib differ diff --git a/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Debug/wheelArchHeigthMeasure.pdb b/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Debug/wheelArchHeigthMeasure.pdb index fe7d6754..982d1627 100644 Binary files a/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Debug/wheelArchHeigthMeasure.pdb and b/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Debug/wheelArchHeigthMeasure.pdb differ diff --git a/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Release/baseAlgorithm.dll b/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Release/baseAlgorithm.dll index 818d8d15..fd5f4d9a 100644 Binary files a/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Release/baseAlgorithm.dll and b/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Release/baseAlgorithm.dll differ diff --git a/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Release/baseAlgorithm.lib b/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Release/baseAlgorithm.lib index 2aeb591c..b77e18b4 100644 Binary files a/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Release/baseAlgorithm.lib and b/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Release/baseAlgorithm.lib differ diff --git a/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Release/baseAlgorithm.pdb b/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Release/baseAlgorithm.pdb index a1f35417..2ce77bcc 100644 Binary files a/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Release/baseAlgorithm.pdb and b/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Release/baseAlgorithm.pdb differ diff --git a/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Release/wheelArchHeigthMeasure.dll b/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Release/wheelArchHeigthMeasure.dll index 8458365c..d3553fc0 100644 Binary files a/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Release/wheelArchHeigthMeasure.dll and b/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Release/wheelArchHeigthMeasure.dll differ diff --git a/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Release/wheelArchHeigthMeasure.lib b/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Release/wheelArchHeigthMeasure.lib index 2a0ff173..11866f50 100644 Binary files a/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Release/wheelArchHeigthMeasure.lib and b/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Release/wheelArchHeigthMeasure.lib differ diff --git a/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Release/wheelArchHeigthMeasure.pdb b/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Release/wheelArchHeigthMeasure.pdb index e9cc296b..099c0d52 100644 Binary files a/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Release/wheelArchHeigthMeasure.pdb and b/AppAlgo/wheelArchHeigthMeasure/Windows/x64/Release/wheelArchHeigthMeasure.pdb differ diff --git a/AppAlgo/wheelArchHeigthMeasure/wheelArchHeigthMeasure_test.cpp b/AppAlgo/wheelArchHeigthMeasure/wheelArchHeigthMeasure_test.cpp index e70f1b0f..20081c8d 100644 --- a/AppAlgo/wheelArchHeigthMeasure/wheelArchHeigthMeasure_test.cpp +++ b/AppAlgo/wheelArchHeigthMeasure/wheelArchHeigthMeasure_test.cpp @@ -1,2839 +1,2849 @@ -// wheelArchHeigthMeasure_test.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。 -// -#include -#include -#include -#include -#include -#include -#include "wheelArchHeigthMeasure_Export.h" -#include -#ifdef _WIN32 -#include -#include "direct.h" -#endif - -typedef struct -{ - int nPointIdx; - double x; - double y; - double z; - float r; - float g; - float b; -} SPointXYZRGB; - -SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9]) -{ - SVzNL3DPoint _r_pt; - _r_pt.x = pt3D.x * matrix3d[0] + pt3D.y * matrix3d[1] + pt3D.z * matrix3d[2]; - _r_pt.y = pt3D.x * matrix3d[3] + pt3D.y * matrix3d[4] + pt3D.z * matrix3d[5]; - _r_pt.z = pt3D.x * matrix3d[6] + pt3D.y * matrix3d[7] + pt3D.z * matrix3d[8]; - return _r_pt; -} - -SVzNLPointXYZRGBA _ptRotate_RGBD(SVzNLPointXYZRGBA pt3D, double matrix3d[9]) -{ - SVzNLPointXYZRGBA _r_pt; - _r_pt.x = pt3D.x * matrix3d[0] + pt3D.y * matrix3d[1] + pt3D.z * matrix3d[2]; - _r_pt.y = pt3D.x * matrix3d[3] + pt3D.y * matrix3d[4] + pt3D.z * matrix3d[5]; - _r_pt.z = pt3D.x * matrix3d[6] + pt3D.y * matrix3d[7] + pt3D.z * matrix3d[8]; - return _r_pt; -} - -#define DATA_VER_OLD 0 -#define DATA_VER_NEW 1 -#define DATA_VER_FROM_CUSTOM 2 -#define VZ_LASER_LINE_PT_MAX_NUM 4096 -SVzNLXYZRGBDLaserLine* vzReadLaserScanPointFromFile_XYZRGB(const char* fileName, int* scanLineNum, float* scanV, - int* dataCalib, int* scanMaxStamp, int* canClockUnit, bool removeNullLines) -{ - std::ifstream inputFile(fileName); - std::string linedata; - - if (inputFile.is_open() == false) - return NULL; - - SVzNLXYZRGBDLaserLine* _scanLines = NULL; - - int lines = 0; - int dataElements = 4; - int firstIndex = -1; - - int dataFileVer = DATA_VER_OLD; - std::getline(inputFile, linedata); //第一行 - int lineNum = 0; - if (0 == strncmp("LineNum:", linedata.c_str(), 8)) - { - sscanf_s(linedata.c_str(), "LineNum:%d", &lines); - if (lines == 0) - return NULL; - lineNum = lines; - _scanLines = (SVzNLXYZRGBDLaserLine*)malloc(sizeof(SVzNLXYZRGBDLaserLine) * (lineNum + 1)); - memset(_scanLines, 0, sizeof(SVzNLXYZRGBDLaserLine) * (lineNum + 1)); - } - if (_scanLines == NULL) - return NULL; - - int lineIdx = 0; - int ptIdx = 0; - int ptNum = 0; - int pre_ptNum = -1; - std::vector< SVzNLPointXYZRGBA> a_line; - int vldLineIdx = 0; - int vldPtNum = 0; - unsigned int timeStamp = 0; - while (getline(inputFile, linedata)) - { - if (0 == strncmp("ScanSpeed:", linedata.c_str(), 10)) - { - double lineV = 0; - sscanf_s(linedata.c_str(), "ScanSpeed:%lf", &lineV); - if (scanV) - *scanV = (float)lineV; - } - else if (0 == strncmp("PointAdjust:", linedata.c_str(), 12)) - { - int ptAdjusted = 0; - sscanf_s(linedata.c_str(), "PointAdjust:%d", &ptAdjusted); - if (dataCalib) - *dataCalib = ptAdjusted; - } - else if (0 == strncmp("MaxTimeStamp:", linedata.c_str(), 13)) - { - unsigned int maxTimeStamp = 0; - unsigned int timePerStamp = 0; - sscanf_s(linedata.c_str(), "MaxTimeStamp:%u_%u", &maxTimeStamp, &timePerStamp); - if (scanMaxStamp) - *scanMaxStamp = maxTimeStamp; - if (canClockUnit) - *canClockUnit = timePerStamp; - } - else if (0 == strncmp("Line_", linedata.c_str(), 5)) - { - int lineIndex; - unsigned int curr_timeStamp; - sscanf_s(linedata.c_str(), "Line_%d_%u_%d", &lineIndex, &curr_timeStamp, &ptNum); - if (firstIndex < 0) - firstIndex = lineIndex; - - lineIndex = lineIndex - firstIndex; - if ((lineIndex < 0) || (lineIndex >= lines)) - break; - - int recvPtNum = (int)a_line.size(); - if ((recvPtNum == pre_ptNum) && ((vldPtNum > 0) || (false == removeNullLines))) - { - SVzNLPointXYZRGBA* p3DPoint; - if (pre_ptNum > 0) - p3DPoint = (SVzNLPointXYZRGBA*)malloc(sizeof(SVzNLPointXYZRGBA) * pre_ptNum); - else - p3DPoint = NULL; - _scanLines[vldLineIdx].nPointCnt = pre_ptNum; - _scanLines[vldLineIdx].nTimeStamp = timeStamp; - _scanLines[vldLineIdx].p3DPoint = p3DPoint; - for (int m = 0; m < pre_ptNum; m++) - p3DPoint[m] = a_line[m]; - vldLineIdx++; - } - //new Line - timeStamp = curr_timeStamp; - vldPtNum = 0; - a_line.clear(); - pre_ptNum = ptNum; - } - else if (0 == strncmp("{", linedata.c_str(), 1)) - { - float X, Y, Z; - int imageY = 0; - float leftX, leftY; - float rightX, rightY; - float r, g, b; - sscanf_s(linedata.c_str(), "{%f,%f,%f,%f,%f,%f }-{%f,%f}-{%f,%f}", &X, &Y, &Z, &r, &g, &b, &leftX, &leftY, &rightX, &rightY); - { - if (lineIdx == 537) - int kkk = 1; - - SVzNLPointXYZRGBA a_pt; - - a_pt.x = X; - a_pt.y = Y; - a_pt.z = Z; - int nr = (int)(r * 255); - int ng = (int)(g * 255); - int nb = (int)(b * 255); - nb <<= 8; - nb += ng; - nb <<= 8; - nb += nr; - a_pt.nRGB = nb; - if (a_pt.z > 1e-4) - vldPtNum++; - a_line.push_back(a_pt); - } - } - } - //last line - int recvPtNum = (int)a_line.size(); - if ((recvPtNum == pre_ptNum) && ((vldPtNum > 0) || (false == removeNullLines))) - { - SVzNLPointXYZRGBA* p3DPoint; - if (pre_ptNum > 0) - p3DPoint = (SVzNLPointXYZRGBA*)malloc(sizeof(SVzNLPointXYZRGBA) * pre_ptNum); - else - p3DPoint = NULL; - _scanLines[vldLineIdx].nPointCnt = pre_ptNum; - _scanLines[vldLineIdx].nTimeStamp = timeStamp; - _scanLines[vldLineIdx].p3DPoint = p3DPoint; - for (int m = 0; m < pre_ptNum; m++) - p3DPoint[m] = a_line[m]; - vldLineIdx++; - } - - if (scanLineNum) - *scanLineNum = vldLineIdx; - inputFile.close(); - return _scanLines; -} - -void vzReadLaserScanPointFromFile_XYZRGB_vector(const char* fileName, std::vector>&scanData) -{ - std::ifstream inputFile(fileName); - std::string linedata; - - if (inputFile.is_open() == false) - return; - - std::vector< SPointXYZRGB> a_line; - while (getline(inputFile, linedata)) - { - if (0 == strncmp("Line_", linedata.c_str(), 5)) //new line - { - if (a_line.size() > 0) - { - scanData.push_back(a_line); - a_line.clear(); - } - } - else if (0 == strncmp("{", linedata.c_str(), 1)) - { - float leftX, leftY; - float rightX, rightY; - SPointXYZRGB a_pt; - sscanf_s(linedata.c_str(), "{ %lf, %lf, %lf, %f, %f, %f }-{ %f, %f }-{ %f, %f }", &a_pt.x, &a_pt.y, &a_pt.z, &a_pt.r, &a_pt.g, &a_pt.b, &leftX, &leftY, &rightX, &rightY); - a_line.push_back(a_pt); - } - } - if (a_line.size() > 0) - scanData.push_back(a_line); - - inputFile.close(); - return; -} - -void vzReadPlyTxtPointFromFile_XYZRGB_vector(const char* fileName, std::vector>& scanData) -{ - std::ifstream inputFile(fileName); - std::string linedata; - - if (inputFile.is_open() == false) - return; - - const double lineYGap = 50; - std::vector< SVzNLPointXYZRGBA> a_line; - - double pre_y = FLT_MIN; - while (getline(inputFile, linedata)) - { - SVzNLPointXYZRGBA a_pt; - memset(&a_pt, 0, sizeof(SVzNLPointXYZRGBA)); - double _x, _y, _z; - sscanf_s(linedata.c_str(), "%lf %lf %lf", &_x, &_y, &_z); - a_pt.x = (float)_x; - a_pt.y = (float)_y; - a_pt.z = (float)_z; - if(a_pt.y < pre_y - lineYGap) //新的扫描行 - { - if (a_line.size() > 0) - { - scanData.push_back(a_line); - a_line.clear(); - } - } - pre_y = a_pt.y; - a_line.push_back(a_pt); - } - if (a_line.size() > 0) - scanData.push_back(a_line); - - inputFile.close(); - return; -} - -SVzNL3DLaserLine* vzReadLaserScanPointFromFile_XYZ(const char* fileName, int* scanLineNum, float* scanV, - int* dataCalib, int* scanMaxStamp, int* canClockUnit) -{ - std::ifstream inputFile(fileName); - std::string linedata; - - if (inputFile.is_open() == false) - return NULL; - - SVzNL3DLaserLine* _scanLines = NULL; - - int lines = 0; - int dataElements = 4; - int firstIndex = -1; - - int dataFileVer = DATA_VER_OLD; - std::getline(inputFile, linedata); //第一行 - int lineNum = 0; - if (0 == strncmp("LineNum:", linedata.c_str(), 8)) - { - dataFileVer = DATA_VER_NEW; - sscanf_s(linedata.c_str(), "LineNum:%d", &lines); - if (lines == 0) - return NULL; - lineNum = lines; - _scanLines = (SVzNL3DLaserLine*)malloc(sizeof(SVzNL3DLaserLine) * (lineNum + 1)); - memset(_scanLines, 0, sizeof(SVzNL3DLaserLine) * (lineNum + 1)); - if (scanLineNum) - *scanLineNum = lines; - } - else if (0 == strncmp("LineNum_", linedata.c_str(), 8)) - { - dataFileVer = DATA_VER_OLD; - sscanf_s(linedata.c_str(), "LineNum_%d", &lines); - if (lines == 0) - return NULL; - lineNum = lines; - _scanLines = (SVzNL3DLaserLine*)malloc(sizeof(SVzNL3DLaserLine) * (lineNum + 1)); - memset(_scanLines, 0, sizeof(SVzNL3DLaserLine) * (lineNum + 1)); - if (scanLineNum) - *scanLineNum = lines; - } - if (_scanLines == NULL) - return NULL; - - int ptNum = 0; - int lineIdx = -1; - int ptIdx = 0; - SVzNL3DPosition* p3DPoint = NULL; - if (dataFileVer == DATA_VER_NEW) - { - while (getline(inputFile, linedata)) - { - if (0 == strncmp("ScanSpeed:", linedata.c_str(), 10)) - { - double lineV = 0; - sscanf_s(linedata.c_str(), "ScanSpeed:%lf", &lineV); - if (scanV) - *scanV = (float)lineV; - } - else if (0 == strncmp("PointAdjust:", linedata.c_str(), 12)) - { - int ptAdjusted = 0; - sscanf_s(linedata.c_str(), "PointAdjust:%d", &ptAdjusted); - if (dataCalib) - *dataCalib = ptAdjusted; - } - else if (0 == strncmp("MaxTimeStamp:", linedata.c_str(), 13)) - { - unsigned int maxTimeStamp = 0; - unsigned int timePerStamp = 0; - sscanf_s(linedata.c_str(), "MaxTimeStamp:%u_%u", &maxTimeStamp, &timePerStamp); - if (scanMaxStamp) - *scanMaxStamp = maxTimeStamp; - if (canClockUnit) - *canClockUnit = timePerStamp; - } - else if (0 == strncmp("Line_", linedata.c_str(), 5)) - { - int lineIndex; - unsigned int timeStamp; - sscanf_s(linedata.c_str(), "Line_%d_%u_%d", &lineIndex, &timeStamp, &ptNum); - if (firstIndex < 0) - firstIndex = lineIndex; - - lineIndex = lineIndex - firstIndex; - if ((lineIndex < 0) || (lineIndex >= lines)) - break; - - //new Line - lineIdx++; - if (ptNum > 0) - { - p3DPoint = (SVzNL3DPosition*)malloc(sizeof(SVzNL3DPosition) * ptNum); - memset(p3DPoint, 0, sizeof(SVzNL3DPosition) * ptNum); - } - else - p3DPoint = NULL; - _scanLines[lineIdx].nPositionCnt = 0; - _scanLines[lineIdx].nTimeStamp = timeStamp; - _scanLines[lineIdx].p3DPosition = p3DPoint; - - } - else if (0 == strncmp("{", linedata.c_str(), 1)) - { - float X, Y, Z; - int imageY = 0; - float leftX, leftY; - float rightX, rightY; - sscanf_s(linedata.c_str(), "{%f,%f,%f}-{%f,%f}-{%f,%f}", &X, &Y, &Z, &leftX, &leftY, &rightX, &rightY); - int id = _scanLines[lineIdx].nPositionCnt; - if (id < ptNum) - { - p3DPoint[id].pt3D.x = X; - p3DPoint[id].pt3D.y = Y; - p3DPoint[id].pt3D.z = Z; - _scanLines[lineIdx].nPositionCnt = id + 1; - } - } - } - - } - else if (dataFileVer == DATA_VER_OLD) - { - while (getline(inputFile, linedata)) - { - if (0 == strncmp("DataElements_", linedata.c_str(), 13)) - { - sscanf_s(linedata.c_str(), "DataElements_%d", &dataElements); - if ((dataElements != 3) && (dataElements != 4)) - break; - } - if (0 == strncmp("LineV_", linedata.c_str(), 6)) - { - double lineV = 0; - sscanf_s(linedata.c_str(), "LineV_%lf", &lineV); - } - else if (0 == strncmp("Line_", linedata.c_str(), 5)) - { - int lineIndex; - unsigned int timeStamp; - sscanf_s(linedata.c_str(), "Line_%d_%u", &lineIndex, &timeStamp); -#if 0 - if (scanLineListTail == NULL) - firstIndex = lineIndex; -#endif - lineIndex = lineIndex - firstIndex; - if ((lineIndex < 0) || (lineIndex >= lines)) - break; - //new Line - //new Line - lineIdx++; - p3DPoint = (SVzNL3DPosition*)malloc(sizeof(SVzNL3DPosition) * VZ_LASER_LINE_PT_MAX_NUM); - memset(p3DPoint, 0, sizeof(SVzNL3DPosition) * VZ_LASER_LINE_PT_MAX_NUM); - _scanLines[lineIdx].nPositionCnt = 0; - _scanLines[lineIdx].nTimeStamp = timeStamp; - _scanLines[lineIdx].p3DPosition = p3DPoint; - } - else if (0 == strncmp("(", linedata.c_str(), 1)) - { - float X, Y, Z; - int imageY = 0; - if (dataElements == 4) - sscanf_s(linedata.c_str(), "(%f,%f,%f,%d)", &X, &Y, &Z, &imageY); - else - sscanf_s(linedata.c_str(), "(%f,%f,%f)", &X, &Y, &Z); - int id = _scanLines[lineIdx].nPositionCnt; - if (id < VZ_LASER_LINE_PT_MAX_NUM) - { - p3DPoint[id].pt3D.x = X; - p3DPoint[id].pt3D.y = Y; - p3DPoint[id].pt3D.z = Z; - _scanLines[lineIdx].nPositionCnt = id + 1; - } - } - } - } - inputFile.close(); - return _scanLines; -} - -//从RGBD点云中读取XYZ信息 -SVzNL3DLaserLine* vzReadXYZPointFromFile_XYZRGB(const char* fileName, int* scanLineNum, float* scanV, - int* dataCalib, int* scanMaxStamp, int* canClockUnit) -{ - std::ifstream inputFile(fileName); - std::string linedata; - - if (inputFile.is_open() == false) - return NULL; - - SVzNL3DLaserLine* _scanLines = NULL; - - int lines = 0; - int dataElements = 4; - int firstIndex = -1; - - int dataFileVer = DATA_VER_OLD; - std::getline(inputFile, linedata); //第一行 - int lineNum = 0; - if (0 == strncmp("LineNum:", linedata.c_str(), 8)) - { - dataFileVer = DATA_VER_NEW; - sscanf_s(linedata.c_str(), "LineNum:%d", &lines); - if (lines == 0) - return NULL; - lineNum = lines; - _scanLines = (SVzNL3DLaserLine*)malloc(sizeof(SVzNL3DLaserLine) * (lineNum + 1)); - memset(_scanLines, 0, sizeof(SVzNL3DLaserLine) * (lineNum + 1)); - if (scanLineNum) - *scanLineNum = lines; - } - else if (0 == strncmp("LineNum_", linedata.c_str(), 8)) - { - dataFileVer = DATA_VER_OLD; - sscanf_s(linedata.c_str(), "LineNum_%d", &lines); - if (lines == 0) - return NULL; - lineNum = lines; - _scanLines = (SVzNL3DLaserLine*)malloc(sizeof(SVzNL3DLaserLine) * (lineNum + 1)); - memset(_scanLines, 0, sizeof(SVzNL3DLaserLine) * (lineNum + 1)); - if (scanLineNum) - *scanLineNum = lines; - } - if (_scanLines == NULL) - return NULL; - - int ptNum = 0; - int lineIdx = -1; - int ptIdx = 0; - SVzNL3DPosition* p3DPoint = NULL; - if (dataFileVer == DATA_VER_NEW) - { - while (getline(inputFile, linedata)) - { - if (0 == strncmp("ScanSpeed:", linedata.c_str(), 10)) - { - double lineV = 0; - sscanf_s(linedata.c_str(), "ScanSpeed:%lf", &lineV); - if (scanV) - *scanV = (float)lineV; - } - else if (0 == strncmp("PointAdjust:", linedata.c_str(), 12)) - { - int ptAdjusted = 0; - sscanf_s(linedata.c_str(), "PointAdjust:%d", &ptAdjusted); - if (dataCalib) - *dataCalib = ptAdjusted; - } - else if (0 == strncmp("MaxTimeStamp:", linedata.c_str(), 13)) - { - unsigned int maxTimeStamp = 0; - unsigned int timePerStamp = 0; - sscanf_s(linedata.c_str(), "MaxTimeStamp:%u_%u", &maxTimeStamp, &timePerStamp); - if (scanMaxStamp) - *scanMaxStamp = maxTimeStamp; - if (canClockUnit) - *canClockUnit = timePerStamp; - } - else if (0 == strncmp("Line_", linedata.c_str(), 5)) - { - int lineIndex; - unsigned int timeStamp; - sscanf_s(linedata.c_str(), "Line_%d_%u_%d", &lineIndex, &timeStamp, &ptNum); - if (firstIndex < 0) - firstIndex = lineIndex; - - lineIndex = lineIndex - firstIndex; - if ((lineIndex < 0) || (lineIndex >= lines)) - break; - - //new Line - lineIdx++; - if (ptNum > 0) - { - p3DPoint = (SVzNL3DPosition*)malloc(sizeof(SVzNL3DPosition) * ptNum); - memset(p3DPoint, 0, sizeof(SVzNL3DPosition) * ptNum); - } - else - p3DPoint = NULL; - _scanLines[lineIdx].nPositionCnt = 0; - _scanLines[lineIdx].nTimeStamp = timeStamp; - _scanLines[lineIdx].p3DPosition = p3DPoint; - - } - else if (0 == strncmp("{", linedata.c_str(), 1)) - { - float X, Y, Z; - float leftX, leftY; - float rightX, rightY; - float r, g, b; - sscanf_s(linedata.c_str(), "{%f,%f,%f,%f,%f,%f }-{%f,%f}-{%f,%f}", &X, &Y, &Z, &r, &g, &b, &leftX, &leftY, &rightX, &rightY); - int id = _scanLines[lineIdx].nPositionCnt; - if (id < ptNum) - { - p3DPoint[id].pt3D.x = X; - p3DPoint[id].pt3D.y = Y; - p3DPoint[id].pt3D.z = Z; - _scanLines[lineIdx].nPositionCnt = id + 1; - } - } - } - - } - inputFile.close(); - return _scanLines; -} - -void vzReadLaserScanPointFromFile_XYZ_vector(const char* fileName, std::vector>& scanData) -{ - std::ifstream inputFile(fileName); - std::string linedata; - - if (inputFile.is_open() == false) - return; - - std::vector< SVzNL3DPosition> a_line; - int ptIdx = 0; - while (getline(inputFile, linedata)) - { - if (0 == strncmp("Line_", linedata.c_str(), 5)) - { - int ptSize = (int)a_line.size(); - if (ptSize > 0) - { - scanData.push_back(a_line); - } - a_line.clear(); - ptIdx = 0; - } - else if (0 == strncmp("{", linedata.c_str(), 1)) - { - float X, Y, Z; - int imageY = 0; - float leftX, leftY; - float rightX, rightY; - sscanf_s(linedata.c_str(), "{%f,%f,%f}-{%f,%f}-{%f,%f}", &X, &Y, &Z, &leftX, &leftY, &rightX, &rightY); - SVzNL3DPosition a_pt; - a_pt.pt3D.x = X; - a_pt.pt3D.y = Y; - a_pt.pt3D.z = Z; - a_pt.nPointIdx = ptIdx; - ptIdx++; - a_line.push_back(a_pt); - } - } - //last line - int ptSize = (int)a_line.size(); - if (ptSize > 0) - { - scanData.push_back(a_line); - a_line.clear(); - } - - inputFile.close(); - return; -} - -void _convertToGridData_XYZRGB(std::vector>& scanData, double _F, std::vector>& gridData) -{ - int lineNum = (int)scanData.size(); - int min_y = 100000000; - int max_y = -10000000; - int validStartLine = -1; - int validEndLine = -1; - for (int line = 0; line < lineNum; line++) - { - if (scanData[line].size() > 0) - { - if (validStartLine < 0) - { - validStartLine = line; - validEndLine = line; - } - else - validEndLine = line; - } - - for (int i = 0; i < (int)scanData[line].size(); i++) - { - SVzNLPointXYZRGBA& a_pt = scanData[line][i]; - if (a_pt.z > 1e-4) - { - double v = _F * a_pt.y / a_pt.z + 2000; - a_pt.nRGB = (int)(v + 0.5); - max_y = max_y < (int)a_pt.nRGB ? (int)a_pt.nRGB : max_y; - min_y = min_y > (int)a_pt.nRGB ? (int)a_pt.nRGB : min_y; - } - } - } - if (min_y == 100000000) - return; - - int vldLineNum = validEndLine - validStartLine + 1; - gridData.resize(vldLineNum); - int pt_counter = max_y - min_y + 1; - for (int i = 0; i < vldLineNum; i++) - { - gridData[i].resize(pt_counter); - for (int j = 0; j < pt_counter; j++) - { - gridData[i][j].nPointIdx = j; - gridData[i][j].pt3D.x = 0; - gridData[i][j].pt3D.y = 0; - gridData[i][j].pt3D.z = 0; - } - } - for (int line = validStartLine; line <= validEndLine; line++) - { - int gridLine = line - validStartLine; - for (int i = 0; i < (int)scanData[line].size(); i++) - { - SVzNLPointXYZRGBA& a_pt = scanData[line][i]; - if (a_pt.z > 1e-4) - { - int pt_id = a_pt.nRGB - min_y; - gridData[gridLine][pt_id].pt3D.x = a_pt.x; - gridData[gridLine][pt_id].pt3D.y = a_pt.y; - gridData[gridLine][pt_id].pt3D.z = a_pt.z; - } - } - } - return; -} - -void _convertToGridData_XYZRGB_vector(std::vector>&scanData, double _F, std::vector>&scanData_grid) -{ - int min_y = 100000000; - int max_y = -10000000; - int lineNum = scanData.size(); - for (int line = 0; line < lineNum; line++) - { - std::vector< SPointXYZRGB>& a_line = scanData[line]; - int nPointCnt = a_line.size(); - for (int i = 0; i < nPointCnt; i++) - { - SPointXYZRGB* a_pt = &scanData[line][i]; - if (a_pt->z > 1e-4) - { - double v = _F * a_pt->y / a_pt->z + 2000; - a_pt->nPointIdx = (int)(v + 0.5); - max_y = max_y < (int)a_pt->nPointIdx ? (int)a_pt->nPointIdx : max_y; - min_y = min_y > (int)a_pt->nPointIdx ? (int)a_pt->nPointIdx : min_y; - } - } - } - if (min_y == 100000000) - return; - - int pt_counter = max_y - min_y + 1; - for (int line = 0; line < lineNum; line++) - { - std::vector< SPointXYZRGB> gridData; - gridData.resize(pt_counter); - for (int i = 0; i < pt_counter; i++) - gridData[i] = { 0, 0.0, 0.0, 0.0, 0.0f, 0.0f, 0.0f }; - - std::vector< SPointXYZRGB>& a_line = scanData[line]; - int nPointCnt = a_line.size(); - for (int i = 0; i < nPointCnt; i++) - { - SPointXYZRGB a_pt = a_line[i]; - if (a_pt.z > 1e-4) - { - int pt_id = a_pt.nPointIdx - min_y; - gridData[pt_id] = a_pt; - } - } - scanData_grid.push_back(gridData); - } - return; -} - -void _outputCalibPara(char* fileName, SSG_planeCalibPara calibPara) -{ - std::ofstream sw(fileName); - char dataStr[250]; - //调平矩阵 - sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.planeCalib[0], calibPara.planeCalib[1], calibPara.planeCalib[2]); - sw << dataStr << std::endl; - sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.planeCalib[3], calibPara.planeCalib[4], calibPara.planeCalib[5]); - sw << dataStr << std::endl; - sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.planeCalib[6], calibPara.planeCalib[7], calibPara.planeCalib[8]); - sw << dataStr << std::endl; - //地面高度 - sprintf_s(dataStr, 250, "%g", calibPara.planeHeight); - sw << dataStr << std::endl; - //反向旋转矩阵 - sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.invRMatrix[0], calibPara.invRMatrix[1], calibPara.invRMatrix[2]); - sw << dataStr << std::endl; - sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.invRMatrix[3], calibPara.invRMatrix[4], calibPara.invRMatrix[5]); - sw << dataStr << std::endl; - sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.invRMatrix[6], calibPara.invRMatrix[7], calibPara.invRMatrix[8]); - sw << dataStr << std::endl; - - sw.close(); -} - -SSG_planeCalibPara _readCalibPara(char* fileName) -{ - //设置初始结果 - double initCalib[9] = { - 1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0 }; - SSG_planeCalibPara planePara; - for (int i = 0; i < 9; i++) - planePara.planeCalib[i] = initCalib[i]; - planePara.planeHeight = -1.0; - for (int i = 0; i < 9; i++) - planePara.invRMatrix[i] = initCalib[i]; - - std::ifstream inputFile(fileName); - std::string linedata; - - if (inputFile.is_open() == false) - return planePara; - - //调平矩阵 - std::getline(inputFile, linedata); - sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[0], &planePara.planeCalib[1], &planePara.planeCalib[2]); - std::getline(inputFile, linedata); - sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[3], &planePara.planeCalib[4], &planePara.planeCalib[5]); - std::getline(inputFile, linedata); - sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[6], &planePara.planeCalib[7], &planePara.planeCalib[8]); - //地面高度 - std::getline(inputFile, linedata); - sscanf_s(linedata.c_str(), "%lf", &planePara.planeHeight); - //反向旋转矩阵 - std::getline(inputFile, linedata); - sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[0], &planePara.invRMatrix[1], &planePara.invRMatrix[2]); - std::getline(inputFile, linedata); - sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[3], &planePara.invRMatrix[4], &planePara.invRMatrix[5]); - std::getline(inputFile, linedata); - sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[6], &planePara.invRMatrix[7], &planePara.invRMatrix[8]); - - inputFile.close(); - return planePara; -} - -void _outputObjResult(char* fileName, std::vector&objOps) -{ - std::ofstream sw(fileName); - char dataStr[250]; - - int objSize = (int)objOps.size(); - for (int i = 0; i < objSize; i++) - { - sw << "obj_" << i << std::endl; - sprintf_s(dataStr, 250, " %g, %g, %g, %g", objOps[i].centerPos.x, objOps[i].centerPos.y, objOps[i].centerPos.z, objOps[i].centerPos.z_yaw); - sw << dataStr << std::endl; - } - sw.close(); -} - -void _outputScanDataFile_self(char* fileName, std::vector>& gridData, - float lineV, int maxTimeStamp, int clockPerSecond) -{ - int lineNum = (int)gridData.size(); - std::ofstream sw(fileName); - sw << "LineNum:" << lineNum << std::endl; - sw << "DataType: 0" << std::endl; - sw << "ScanSpeed:" << lineV << std::endl; - sw << "PointAdjust: 1" << std::endl; - sw << "MaxTimeStamp:" << maxTimeStamp << "_" << clockPerSecond << std::endl; - - for (int line = 0; line < lineNum; line++) - { - int nPositionCnt = (int)gridData[line].size(); - sw << "Line_" << line << "_0_" << nPositionCnt << std::endl; - for (int i = 0; i < nPositionCnt; i++) - { - SVzNL3DPosition& pt3D = gridData[line][i]; - float x = (float)pt3D.pt3D.x; - float y = (float)pt3D.pt3D.y; - float z = (float)pt3D.pt3D.z; - sw << "{" << x << "," << y << "," << z << "}-"; - sw << "{0,0}-{0,0}" << std::endl; - } - } - sw.close(); -} - -void _outputScanDataFile_SPointXYZRGB_vector(char* fileName, std::vector>&scanData) -{ - std::ofstream sw(fileName); - int lineNum = scanData.size(); - sw << "LineNum:" << lineNum << std::endl; - sw << "DataType: 0" << std::endl; - sw << "ScanSpeed: 0" << std::endl; - sw << "PointAdjust: 1" << std::endl; - sw << "MaxTimeStamp: 0_0" << std::endl; - - for (int line = 0; line < lineNum; line++) - { - int nPositionCnt = scanData[line].size(); - sw << "Line_" << line << "_0_" << nPositionCnt << std::endl; - for (int i = 0; i < nPositionCnt; i++) - { - SVzNLPointXYZRGBA& pt3D = scanData[line][i]; - char str[250]; - sprintf_s(str, "{ %lf, %lf, %lf, 0, 0, 0 } - { 0, 0 } - { 0, 0 }", - pt3D.x, pt3D.y, pt3D.z); - - sw << str << std::endl; - } - } - sw.close(); -} - -typedef struct -{ - int r; - int g; - int b; -}SG_color; -void _outputScanDataFile_RGBD_obj(char* fileName, std::vector>& scanData, - float lineV, int maxTimeStamp, int clockPerSecond, WD_wheelArchInfo& wheelArcHeight) -{ - int lineNum = (int)scanData.size(); - std::ofstream sw(fileName); - int realLines = lineNum; - realLines++; - sw << "LineNum:" << realLines << std::endl; - sw << "DataType: 0" << std::endl; - sw << "ScanSpeed:" << lineV << std::endl; - sw << "PointAdjust: 1" << std::endl; - sw << "MaxTimeStamp:" << maxTimeStamp << "_" << clockPerSecond << std::endl; - - int maxLineIndex = 0; - int max_stamp = 0; - - SG_color rgb = { 0, 0, 0 }; - - SG_color objColor[8] = { - {245,222,179},//淡黄色 - {210,105, 30},//巧克力色 - {240,230,140},//黄褐色 - {135,206,235},//天蓝色 - {250,235,215},//古董白 - {189,252,201},//薄荷色 - {221,160,221},//梅红色 - {188,143,143},//玫瑰红色 - }; - int size = 1; - int nTimeStamp = 0; - double alpha = 0.8; - for (int line = 0; line < lineNum; line++) - { - int nPositionCnt = (int)scanData[line].size(); - sw << "Line_" << line << "_0_" << nPositionCnt << std::endl; - for (int i = 0; i < nPositionCnt; i++) - { - SVzNL3DPosition& pt3D = scanData[line][i]; - int type = pt3D.nPointIdx; - if (1 == type) - { - rgb = { 0,255,0 }; - rgb.r = (int)((double)rgb.r * alpha); - rgb.g = (int)((double)rgb.g * alpha); - rgb.b = (int)((double)rgb.b * alpha); - size = 2; - } - else if (2 == type) - { - rgb = { 0,0,255 }; - rgb.r = (int)((double)rgb.r * alpha); - rgb.g = (int)((double)rgb.g * alpha); - rgb.b = (int)((double)rgb.b * alpha); - size = 2; - } - else if (3 == type) //轮眉 - { - rgb = { 255, 0, 0 }; - size = 3; - } - else if (4 == type) // - { - rgb = { 255, 255, 0 }; - size = 3; - } - else if (5 == type) // - { - rgb = { 255, 255, 0 }; - size = 3; - } - else - { - rgb = { 100, 100, 100 }; - size = 1; - } - - float x = (float)pt3D.pt3D.x; - float y = (float)pt3D.pt3D.y; - float z = (float)pt3D.pt3D.z; - sw << "{" << x << "," << y << "," << z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; - } - } - //if (objOps.size() > 0) - { - int ptNum = 3; - sw << "Line_" << lineNum << "_" << (nTimeStamp + 1000) << "_" << ptNum << std::endl; - - rgb = { 255, 0, 0 }; - - size = 10; - float x = (float)wheelArcHeight.wheelArchPos.x; - float y = (float)wheelArcHeight.wheelArchPos.y; - float z = (float)wheelArcHeight.wheelArchPos.z; - sw << "{" << x << "," << y << "," << z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; - - x = (float)wheelArcHeight.wheelUpPos.x; - y = (float)wheelArcHeight.wheelUpPos.y; - z = (float)wheelArcHeight.wheelUpPos.z; - sw << "{" << x << "," << y << "," << z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; - - x = (float)wheelArcHeight.wheelDownPos.x; - y = (float)wheelArcHeight.wheelDownPos.y; - z = (float)wheelArcHeight.wheelDownPos.z; - sw << "{" << x << "," << y << "," << z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; - - x = (float)wheelArcHeight.wheelArchPos.x; - y = (float)wheelArcHeight.wheelArchPos.y; - z = (float)wheelArcHeight.wheelArchPos.z; - sw << "{" << x << "," << y << "," << z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; - } - //画出方向线 - rgb = { 255, 0, 0 }; - size = 3; - int lineIdx = 0; - sw << "Poly_" << lineIdx << "_2" << std::endl; - sw << "{" << (float)wheelArcHeight.arcLine[0].x << "," << (float)wheelArcHeight.arcLine[0].y << "," << (float)wheelArcHeight.arcLine[0].z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; - sw << "{" << (float)wheelArcHeight.arcLine[1].x << "," << (float)wheelArcHeight.arcLine[1].y << "," << (float)wheelArcHeight.arcLine[1].z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; - - lineIdx++; - sw << "Poly_" << lineIdx << "_2" << std::endl; - sw << "{" << (float)wheelArcHeight.upLine[0].x << "," << (float)wheelArcHeight.upLine[0].y << "," << (float)wheelArcHeight.upLine[0].z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; - sw << "{" << (float)wheelArcHeight.upLine[1].x << "," << (float)wheelArcHeight.upLine[1].y << "," << (float)wheelArcHeight.upLine[1].z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; - - lineIdx++; - sw << "Poly_" << lineIdx << "_2" << std::endl; - sw << "{" << (float)wheelArcHeight.downLine[0].x << "," << (float)wheelArcHeight.downLine[0].y << "," << (float)wheelArcHeight.downLine[0].z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; - sw << "{" << (float)wheelArcHeight.downLine[1].x << "," << (float)wheelArcHeight.downLine[1].y << "," << (float)wheelArcHeight.downLine[1].z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; - - lineIdx++; - sw << "Poly_" << lineIdx << "_2" << std::endl; - sw << "{" << (float)wheelArcHeight.centerLine[0].x << "," << (float)wheelArcHeight.centerLine[0].y << "," << (float)wheelArcHeight.centerLine[0].z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; - sw << "{" << (float)wheelArcHeight.centerLine[1].x << "," << (float)wheelArcHeight.centerLine[1].y << "," << (float)wheelArcHeight.centerLine[1].z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; - - lineIdx++; - sw << "Poly_" << lineIdx << "_2" << std::endl; - sw << "{" << (float)wheelArcHeight.arcLine[0].x << "," << (float)wheelArcHeight.arcLine[0].y << "," << (float)wheelArcHeight.arcLine[0].z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; - sw << "{" << (float)wheelArcHeight.arcLine[1].x << "," << (float)wheelArcHeight.arcLine[1].y << "," << (float)wheelArcHeight.arcLine[1].z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; - - sw.close(); -} - -void _outputScanDataFile_obj_vector(char* fileName, std::vector>scanData, - std::vector&objOps) -{ - int lineNum = scanData.size(); - std::ofstream sw(fileName); - int realLines = lineNum; - if (objOps.size() > 0) - realLines++; - sw << "LineNum:" << realLines << std::endl; - sw << "DataType: 0" << std::endl; - sw << "ScanSpeed: 0" << std::endl; - sw << "PointAdjust: 1" << std::endl; - sw << "MaxTimeStamp: 0_0" << std::endl; - - int maxLineIndex = 0; - int max_stamp = 0; - - SG_color rgb = { 0, 0, 0 }; - - SG_color objColor[8] = { - {245,222,179},//淡黄色 - {210,105, 30},//巧克力色 - {240,230,140},//黄褐色 - {135,206,235},//天蓝色 - {250,235,215},//古董白 - {189,252,201},//薄荷色 - {221,160,221},//梅红色 - {188,143,143},//玫瑰红色 - }; - int size = 1; - int nTimeStamp = 0; - for (int line = 0; line < lineNum; line++) - { - int nPositionCnt = scanData[line].size(); - sw << "Line_" << line << "_0_" << nPositionCnt << std::endl; - for (int i = 0; i < nPositionCnt; i++) - { - SVzNL3DPosition* pt3D = &scanData[line][i]; - int vType = pt3D->nPointIdx & 0xff; - int hType = vType >> 4; - int objId = (pt3D->nPointIdx >> 16) & 0xffff; - vType = vType & 0x0f; - if (LINE_FEATURE_L_JUMP_H2L == vType) - { - rgb = { 255, 97, 0 }; - size = 3; - } - else if (LINE_FEATURE_L_JUMP_L2H == vType) - { - rgb = { 255, 255, 0 }; - size = 3; - } - else if (LINE_FEATURE_V_SLOPE == vType) - { - rgb = { 255, 0, 255 }; - size = 3; - } - else if (LINE_FEATURE_L_SLOPE_H2L == vType) - { - rgb = { 160, 82, 45 }; - size = 3; - } - else if ((LINE_FEATURE_LINE_ENDING_0 == vType) || (LINE_FEATURE_LINE_ENDING_1 == vType)) - { - rgb = { 255, 0, 0 }; - size = 3; - } - else if (LINE_FEATURE_L_SLOPE_L2H == vType) - { - rgb = { 233, 150, 122 }; - size = 3; - } - else if (LINE_FEATURE_L_JUMP_H2L == hType) - { - rgb = { 0, 0, 255 }; - size = 3; - } - else if (LINE_FEATURE_L_JUMP_L2H == hType) - { - rgb = { 0, 255, 255 }; - size = 3; - } - else if (LINE_FEATURE_V_SLOPE == hType) - { - rgb = { 0, 255, 0 }; - size = 3; - } - else if (LINE_FEATURE_L_SLOPE_H2L == hType) - { - rgb = { 85, 107, 47 }; - size = 3; - } - else if (LINE_FEATURE_L_SLOPE_L2H == hType) - { - rgb = { 0, 255, 154 }; - size = 3; - } - else if ((LINE_FEATURE_LINE_ENDING_0 == hType) || (LINE_FEATURE_LINE_ENDING_1 == hType)) - { - rgb = { 255, 0, 0 }; - size = 3; - } - else if (objId > 0) //目标 - { - rgb = objColor[objId % 8]; - size = 5; - } - else - { - rgb = { 200, 200, 200 }; - size = 1; - } - - float x = (float)pt3D->pt3D.x; - float y = (float)pt3D->pt3D.y; - float z = (float)pt3D->pt3D.z; - sw << "{" << x << "," << y << "," << z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; - } - } - if (objOps.size() > 0) - { - int ptNum = objOps.size(); - sw << "Line_" << lineNum << "_0_" << ptNum << std::endl; - for (int i = 0; i < objOps.size(); i++) - { - if (i == 0) - rgb = { 255, 0, 0 }; - else - rgb = { 255, 255, 0 }; - size = 25; - float x = (float)objOps[i].centerPos.x; - float y = (float)objOps[i].centerPos.y; - float z = (float)objOps[i].centerPos.z; - - sw << "{" << x << "," << y << "," << z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; - if (i == 0) - { - sw << "{" << x << "," << y << "," << z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; - } - } - } - //画出方向线 - sw.close(); -} - -void _outputRGBDScanDataFile_RGBD_obj(char* fileName, SVzNLXYZRGBDLaserLine * scanData, int lineNum, - float lineV, int maxTimeStamp, int clockPerSecond, std::vector&objOps) -{ - std::ofstream sw(fileName); - int realLines = lineNum; - if (objOps.size() > 0) - realLines++; - sw << "LineNum:" << realLines << std::endl; - sw << "DataType: 0" << std::endl; - sw << "ScanSpeed:" << lineV << std::endl; - sw << "PointAdjust: 1" << std::endl; - sw << "MaxTimeStamp:" << maxTimeStamp << "_" << clockPerSecond << std::endl; - - int maxLineIndex = 0; - int max_stamp = 0; - - SG_color rgb = { 0, 0, 0 }; - - SG_color objColor[8] = { - {245,222,179},//淡黄色 - {210,105, 30},//巧克力色 - {240,230,140},//黄褐色 - {135,206,235},//天蓝色 - {250,235,215},//古董白 - {189,252,201},//薄荷色 - {221,160,221},//梅红色 - {188,143,143},//玫瑰红色 - }; - int size = 1; - int nTimeStamp = 0; - for (int line = 0; line < lineNum; line++) - { - sw << "Line_" << line << "_" << scanData[line].nTimeStamp << "_" << scanData[line].nPointCnt << std::endl; - nTimeStamp = scanData[line].nTimeStamp; - for (int i = 0; i < scanData[line].nPointCnt; i++) - { - SVzNLPointXYZRGBA* pt3D = &scanData[line].p3DPoint[i]; -#if 0 - int vType = pt3D->nRGB & 0xff; - int hType = vType >> 4; - int objId = (pt3D->nRGB >> 16) & 0xffff; - vType = vType & 0x0f; - if (LINE_FEATURE_L_JUMP_H2L == vType) - { - rgb = { 255, 97, 0 }; - size = 3; - } - else if (LINE_FEATURE_L_JUMP_L2H == vType) - { - rgb = { 255, 255, 0 }; - size = 3; - } - else if (LINE_FEATURE_V_SLOPE == vType) - { - rgb = { 255, 0, 255 }; - size = 3; - } - else if (LINE_FEATURE_L_SLOPE_H2L == vType) - { - rgb = { 160, 82, 45 }; - size = 3; - } - else if ((LINE_FEATURE_LINE_ENDING_0 == vType) || (LINE_FEATURE_LINE_ENDING_1 == vType)) - { - rgb = { 255, 0, 0 }; - size = 3; - } - else if (LINE_FEATURE_L_SLOPE_L2H == vType) - { - rgb = { 233, 150, 122 }; - size = 3; - } - else if (LINE_FEATURE_L_JUMP_H2L == hType) - { - rgb = { 0, 0, 255 }; - size = 3; - } - else if (LINE_FEATURE_L_JUMP_L2H == hType) - { - rgb = { 0, 255, 255 }; - size = 3; - } - else if (LINE_FEATURE_V_SLOPE == hType) - { - rgb = { 0, 255, 0 }; - size = 3; - } - else if (LINE_FEATURE_L_SLOPE_H2L == hType) - { - rgb = { 85, 107, 47 }; - size = 3; - } - else if (LINE_FEATURE_L_SLOPE_L2H == hType) - { - rgb = { 0, 255, 154 }; - size = 3; - } - else if ((LINE_FEATURE_LINE_ENDING_0 == hType) || (LINE_FEATURE_LINE_ENDING_1 == hType)) - { - rgb = { 255, 0, 0 }; - size = 3; - } - else if (objId > 0) //目标 - { - rgb = objColor[objId % 8]; - size = 5; - } - else -#endif - { - rgb = { 200, 200, 200 }; - size = 1; - } - float x = (float)pt3D->x; - float y = (float)pt3D->y; - float z = (float)pt3D->z; - sw << "{" << x << "," << y << "," << z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; - } - } - if (objOps.size() > 0) - { - int ptNum = objOps.size(); - sw << "Line_" << lineNum << "_" << (nTimeStamp + 1000) << "_" << ptNum << std::endl; - for (int i = 0; i < objOps.size(); i++) - { - if (i == 0) - rgb = { 255, 0, 0 }; - else - rgb = { 255, 255, 0 }; - size = 25; - float x = (float)objOps[i].centerPos.x; - float y = (float)objOps[i].centerPos.y; - float z = (float)objOps[i].centerPos.z; - - sw << "{" << x << "," << y << "," << z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; - if (i == 0) - { - sw << "{" << x << "," << y << "," << z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; - } - } - } - sw.close(); -} - -void _outputScanDataFile_RGBD_sideBagObj(char* fileName, SVzNL3DLaserLine * scanData, int lineNum, - float lineV, int maxTimeStamp, int clockPerSecond, std::vector&objOps) -{ - std::ofstream sw(fileName); - int realLines = lineNum; - if (objOps.size() > 0) - realLines++; - sw << "LineNum:" << realLines << std::endl; - sw << "DataType: 0" << std::endl; - sw << "ScanSpeed:" << lineV << std::endl; - sw << "PointAdjust: 1" << std::endl; - sw << "MaxTimeStamp:" << maxTimeStamp << "_" << clockPerSecond << std::endl; - - int maxLineIndex = 0; - int max_stamp = 0; - - SG_color rgb = { 0, 0, 0 }; - - SG_color objColor[8] = { - {245,222,179},//淡黄色 - {210,105, 30},//巧克力色 - {240,230,140},//黄褐色 - {135,206,235},//天蓝色 - {250,235,215},//古董白 - {189,252,201},//薄荷色 - {221,160,221},//梅红色 - {188,143,143},//玫瑰红色 - }; - int size = 1; - int nTimeStamp = 0; - for (int line = 0; line < lineNum; line++) - { - int nPntCount = 0; - for (int i = 0; i < scanData[line].nPositionCnt; i++) - { - SVzNL3DPosition* pt3D = &scanData[line].p3DPosition[i]; - if (pt3D->pt3D.z > 1e-4) - nPntCount++; - } - sw << "Line_" << line << "_" << scanData[line].nTimeStamp << "_" << nPntCount << std::endl; - nTimeStamp = scanData[line].nTimeStamp; - for (int i = 0; i < scanData[line].nPositionCnt; i++) - { - SVzNL3DPosition* pt3D = &scanData[line].p3DPosition[i]; - if (pt3D->pt3D.z < 1e-4) - continue; - - int vType = pt3D->nPointIdx & 0xff; - int hType = vType >> 4; - int objId = (pt3D->nPointIdx >> 16) & 0xffff; - vType = vType & 0x0f; - if (LINE_FEATURE_L_JUMP_H2L == vType) - { - rgb = { 255, 97, 0 }; - size = 3; - } - else if (LINE_FEATURE_L_JUMP_L2H == vType) - { - rgb = { 255, 255, 0 }; - size = 3; - } - else if (LINE_FEATURE_V_SLOPE == vType) - { - rgb = { 255, 0, 255 }; - size = 3; - } - else if (LINE_FEATURE_L_SLOPE_H2L == vType) - { - rgb = { 160, 82, 45 }; - size = 3; - } - else if ((LINE_FEATURE_LINE_ENDING_0 == vType) || (LINE_FEATURE_LINE_ENDING_1 == vType)) - { - rgb = { 255, 0, 0 }; - size = 3; - } - else if (LINE_FEATURE_L_SLOPE_L2H == vType) - { - rgb = { 233, 150, 122 }; - size = 3; - } - else if (LINE_FEATURE_L_JUMP_H2L == hType) - { - rgb = { 0, 0, 255 }; - size = 3; - } - else if (LINE_FEATURE_L_JUMP_L2H == hType) - { - rgb = { 0, 255, 255 }; - size = 3; - } - else if (LINE_FEATURE_V_SLOPE == hType) - { - rgb = { 0, 255, 0 }; - size = 3; - } - else if (LINE_FEATURE_L_SLOPE_H2L == hType) - { - rgb = { 85, 107, 47 }; - size = 3; - } - else if (LINE_FEATURE_L_SLOPE_L2H == hType) - { - rgb = { 0, 255, 154 }; - size = 3; - } - else if ((LINE_FEATURE_LINE_ENDING_0 == hType) || (LINE_FEATURE_LINE_ENDING_1 == hType)) - { - rgb = { 255, 0, 0 }; - size = 3; - } - else if (objId > 0) //目标 - { - rgb = objColor[objId % 8]; - size = 5; - } - else - { - rgb = { 200, 200, 200 }; - size = 1; - } - - float x = (float)pt3D->pt3D.x; - float y = (float)pt3D->pt3D.y; - float z = (float)pt3D->pt3D.z; - sw << "{" << x << "," << y << "," << z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; - } - } - if (objOps.size() > 0) - { - int ptNum = objOps.size(); - sw << "Line_" << lineNum << "_" << (nTimeStamp + 1000) << "_" << ptNum << std::endl; - for (int i = 0; i < objOps.size(); i++) - { - if (i == 0) - rgb = { 255, 0, 0 }; - else - rgb = { 255, 255, 0 }; - size = 25; - float x = (float)objOps[i].graspPos.x; - float y = (float)objOps[i].graspPos.y; - float z = (float)objOps[i].graspPos.z; - - sw << "{" << x << "," << y << "," << z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; - if (i == 0) - { - sw << "{" << x << "," << y << "," << z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; - } - } - } - sw.close(); -} - -void EulerRpyToRotation1(double rpy[3], double matrix3d[9]) { - double cos0 = cos(rpy[0] * PI / 180); - double sin0 = sin(rpy[0] * PI / 180); - double cos1 = cos(rpy[1] * PI / 180); - double sin1 = sin(rpy[1] * PI / 180); - double cos2 = cos(rpy[2] * PI / 180); - double sin2 = sin(rpy[2] * PI / 180); - matrix3d[0] = cos2 * cos1; - matrix3d[1] = cos2 * sin1 * sin0 - sin2 * cos0; - matrix3d[2] = cos2 * sin1 * cos0 + sin2 * sin0; - matrix3d[3] = sin2 * cos1; - matrix3d[4] = sin2 * sin1 * sin0 + cos2 * cos0; - matrix3d[5] = sin2 * sin1 * cos0 - cos2 * sin0; - matrix3d[6] = -sin1; - matrix3d[7] = cos1 * sin0; - matrix3d[8] = cos1 * cos0; - return; -} - -void _rotateCloudPts(SVzNL3DLaserLine * scanData, int lineNum, double matrix3d[9], std::vector>&rotateLines, SVzNLRangeD * rx_range, SVzNLRangeD * ry_range) -{ - rx_range->min = 0; - rx_range->max = -1; - ry_range->min = 0; - ry_range->max = -1; - for (int line = 0; line < lineNum; line++) - { - std::vector< SVzNL3DPosition> linePts; - for (int i = 0; i < scanData[line].nPositionCnt; i++) - { - SVzNL3DPosition* pt3D = &scanData[line].p3DPosition[i]; - if (pt3D->pt3D.z < 1e-4) - continue; - - SVzNL3DPosition r_pt; - r_pt.pt3D = _ptRotate(pt3D->pt3D, matrix3d); - r_pt.nPointIdx = pt3D->nPointIdx; - if (rx_range->max < rx_range->min) - { - rx_range->min = r_pt.pt3D.x; - rx_range->max = r_pt.pt3D.x; - } - else - { - if (rx_range->min > r_pt.pt3D.x) - rx_range->min = r_pt.pt3D.x; - if (rx_range->max < r_pt.pt3D.x) - rx_range->max = r_pt.pt3D.x; - } - if (ry_range->max < ry_range->min) - { - ry_range->min = r_pt.pt3D.y; - ry_range->max = r_pt.pt3D.y; - } - else - { - if (ry_range->min > r_pt.pt3D.y) - ry_range->min = r_pt.pt3D.y; - if (ry_range->max < r_pt.pt3D.y) - ry_range->max = r_pt.pt3D.y; - } - linePts.push_back(r_pt); - - } - rotateLines.push_back(linePts); - } -} - -void _rotateCloudPts_RGBD(SVzNLXYZRGBDLaserLine * scanData, int lineNum, double matrix3d[9], std::vector>&rotateLines, SVzNLRangeD * rx_range, SVzNLRangeD * ry_range) -{ - rx_range->min = 0; - rx_range->max = -1; - ry_range->min = 0; - ry_range->max = -1; - for (int line = 0; line < lineNum; line++) - { - std::vector< SVzNLPointXYZRGBA> linePts; - for (int i = 0; i < scanData[line].nPointCnt; i++) - { - SVzNLPointXYZRGBA* pt3D = &scanData[line].p3DPoint[i]; - if (pt3D->z < 1e-4) - continue; - - SVzNLPointXYZRGBA r_pt; - r_pt = _ptRotate_RGBD(*pt3D, matrix3d); - if (rx_range->max < rx_range->min) - { - rx_range->min = r_pt.x; - rx_range->max = r_pt.x; - } - else - { - if (rx_range->min > r_pt.x) - rx_range->min = r_pt.x; - if (rx_range->max < r_pt.x) - rx_range->max = r_pt.x; - } - if (ry_range->max < ry_range->min) - { - ry_range->min = r_pt.y; - ry_range->max = r_pt.y; - } - else - { - if (ry_range->min > r_pt.y) - ry_range->min = r_pt.y; - if (ry_range->max < r_pt.y) - ry_range->max = r_pt.y; - } - linePts.push_back(r_pt); - } - rotateLines.push_back(linePts); - } -} - -void _XOYprojection( - cv::Mat & img, - std::vector>&dataLines, - std::vector&objOps, - const double x_scale, - const double y_scale, - const SVzNLRangeD x_range, - const SVzNLRangeD y_range, - bool drawDirAngle, - int dirAngleLen) -{ - int x_skip = 16; - int y_skip = 16; - - cv::Vec3b rgb = cv::Vec3b(0, 0, 0); - cv::Vec3b objColor[8] = { - {245,222,179},//淡黄色 - {210,105, 30},//巧克力色 - {240,230,140},//黄褐色 - {135,206,235},//天蓝色 - {250,235,215},//古董白 - {189,252,201},//薄荷色 - {221,160,221},//梅红色 - {188,143,143},//玫瑰红色 - }; - int size = 1; - for (int line = 0; line < dataLines.size(); line++) - { - std::vector< SVzNL3DPosition>& a_line = dataLines[line]; - for (int i = 0; i < a_line.size(); i++) - { - SVzNL3DPosition* pt3D = &a_line[i]; - if (pt3D->pt3D.z < 1e-4) - continue; - - int vType = pt3D->nPointIdx & 0xff; - int hType = vType >> 4; - int objId = (pt3D->nPointIdx >> 16) & 0xff; - vType = vType & 0x0f; - if (LINE_FEATURE_L_JUMP_H2L == vType) - { - rgb = { 255, 97, 0 }; - size = 2; - } - else if (LINE_FEATURE_L_JUMP_L2H == vType) - { - rgb = { 255, 255, 0 }; - size = 2; - } - else if (LINE_FEATURE_V_SLOPE == vType) - { - rgb = { 255, 0, 255 }; - size = 2; - } - else if (LINE_FEATURE_L_SLOPE_H2L == vType) - { - rgb = { 160, 82, 45 }; - size = 2; - } - else if ((LINE_FEATURE_LINE_ENDING_0 == vType) || (LINE_FEATURE_LINE_ENDING_1 == vType)) - { - rgb = { 255, 0, 0 }; - size = 2; - } - else if (LINE_FEATURE_L_SLOPE_L2H == vType) - { - rgb = { 233, 150, 122 }; - size = 2; - } - else if (LINE_FEATURE_L_JUMP_H2L == hType) - { - rgb = { 0, 0, 255 }; - size = 2; - } - else if (LINE_FEATURE_L_JUMP_L2H == hType) - { - rgb = { 0, 255, 255 }; - size = 2; - } - else if (LINE_FEATURE_V_SLOPE == hType) - { - rgb = { 0, 255, 0 }; - size = 2; - } - else if (LINE_FEATURE_L_SLOPE_H2L == hType) - { - rgb = { 85, 107, 47 }; - size = 2; - } - else if (LINE_FEATURE_L_SLOPE_L2H == hType) - { - rgb = { 0, 255, 154 }; - size = 2; - } - else if ((LINE_FEATURE_LINE_ENDING_0 == hType) || (LINE_FEATURE_LINE_ENDING_1 == hType)) - { - rgb = { 255, 0, 0 }; - size = 2; - } - else if (objId > 0) //目标 - { - rgb = objColor[objId % 8]; - size = 1; - } - else - { - rgb = { 150, 150, 150 }; - size = 1; - } - - - double x = pt3D->pt3D.x; - double y = pt3D->pt3D.y; - - int px = (int)((x - x_range.min) / x_scale + x_skip); - int py = (int)((y - y_range.min) / y_scale + y_skip); - if (size == 1) - img.at(py, px) = cv::Vec3b(rgb[2], rgb[1], rgb[0]); - else - cv::circle(img, cv::Point(px, py), size, cv::Scalar(rgb[2], rgb[1], rgb[0]), -1); - } - } - if (objOps.size() > 0) - { - for (int i = 0; i < objOps.size(); i++) - { - if (i == 0) - { - rgb = { 255, 0, 0 }; - size = 20; - } - else - { - rgb = { 255, 255, 0 }; - size = 10; - } - int px = (int)((objOps[i].centerPos.x - x_range.min) / x_scale + x_skip); - int py = (int)((objOps[i].centerPos.y - y_range.min) / y_scale + y_skip); - cv::circle(img, cv::Point(px, py), size, cv::Scalar(rgb[2], rgb[1], rgb[0]), -1); - if (true == drawDirAngle) - { - //画线 - double R = (double)dirAngleLen / 2.0; - const double deg2rad = PI / 180.0; - const double yaw = objOps[i].centerPos.z_yaw * deg2rad; - double cy = cos(yaw); double sy = sin(yaw); - double x1 = objOps[i].centerPos.x + R * cy; double y1 = objOps[i].centerPos.y - R * sy; - double x2 = objOps[i].centerPos.x - R * cy; double y2 = objOps[i].centerPos.y + R * sy; - int px1 = (int)((x1 - x_range.min) / x_scale + x_skip); - int py1 = (int)((y1 - y_range.min) / y_scale + y_skip); - int px2 = (int)((x2 - x_range.min) / x_scale + x_skip); - int py2 = (int)((y2 - y_range.min) / y_scale + y_skip); - cv::line(img, cv::Point(px1, py1), cv::Point(px2, py2), cv::Scalar(rgb[2], rgb[1], rgb[0]), 2); - } - } - } - -} - -void _XOYprojection_RGBD( - cv::Mat & img, - std::vector>&dataLines, - std::vector&objOps, - const double x_scale, - const double y_scale, - const SVzNLRangeD x_range, - const SVzNLRangeD y_range, - bool drawDirAngle, - int dirAngleLen) -{ - int x_skip = 16; - int y_skip = 16; - - cv::Vec3b rgb = cv::Vec3b(0, 0, 0); - cv::Vec3b objColor[8] = { - {245,222,179},//淡黄色 - {210,105, 30},//巧克力色 - {240,230,140},//黄褐色 - {135,206,235},//天蓝色 - {250,235,215},//古董白 - {189,252,201},//薄荷色 - {221,160,221},//梅红色 - {188,143,143},//玫瑰红色 - }; - int size = 1; - for (int line = 0; line < dataLines.size(); line++) - { - std::vector< SVzNLPointXYZRGBA>& a_line = dataLines[line]; - for (int i = 0; i < a_line.size(); i++) - { - SVzNLPointXYZRGBA* pt3D = &a_line[i]; - if (pt3D->z < 1e-4) - continue; - - int nRGB = pt3D->nRGB; - int r = nRGB & 0xff; - nRGB >>= 8; - int g = nRGB & 0xff; - nRGB >>= 8; - int b = nRGB & 0xff; - - rgb[0] = r; - rgb[1] = g; - rgb[2] = b; - size = 1; - double x = pt3D->x; - double y = pt3D->y; - int px = (int)((x - x_range.min) / x_scale + x_skip); - int py = (int)((y - y_range.min) / y_scale + y_skip); - if ((px == 666) && (py == 828)) - int kkk = 1; - if (size == 1) - img.at(py, px) = cv::Vec3b(rgb[2], rgb[1], rgb[0]); - else - cv::circle(img, cv::Point(px, py), size, cv::Scalar(rgb[2], rgb[1], rgb[0]), -1); - } - } - if (objOps.size() > 0) - { - for (int i = 0; i < objOps.size(); i++) - { - if (i == 0) - { - rgb = { 255, 0, 0 }; - size = 20; - } - else - { - rgb = { 255, 255, 0 }; - size = 10; - } - int px = (int)((objOps[i].centerPos.x - x_range.min) / x_scale + x_skip); - int py = (int)((objOps[i].centerPos.y - y_range.min) / y_scale + y_skip); - cv::circle(img, cv::Point(px, py), size, cv::Scalar(rgb[2], rgb[1], rgb[0]), -1); - if (true == drawDirAngle) - { - //画线 - double R = (double)dirAngleLen / 2.0; - const double deg2rad = PI / 180.0; - const double yaw = objOps[i].centerPos.z_yaw * deg2rad; - double cy = cos(yaw); double sy = sin(yaw); - double arrowLen = R / 3; - double arrowAngle = 30; - double ca = cos(arrowAngle * deg2rad); - double sa = sin(arrowAngle * deg2rad); - SVzNL2DPointD endingPt[4]; - endingPt[0] = { R, 0 }; - endingPt[1] = { -R, 0 }; - endingPt[2] = { R - arrowLen * ca, -arrowLen * sa }; - endingPt[3] = { R - arrowLen * ca, arrowLen * sa }; - for (int m = 0; m < 4; m++) - { - double tmp_x = endingPt[m].x * cy - endingPt[m].y * sy; - double tmp_y = -endingPt[m].x * sy - endingPt[m].y * cy; - endingPt[m].x = tmp_x + objOps[i].centerPos.x; - endingPt[m].y = tmp_y + objOps[i].centerPos.y; - } - int px1 = (int)((endingPt[0].x - x_range.min) / x_scale + x_skip); - int py1 = (int)((endingPt[0].y - y_range.min) / y_scale + y_skip); - int px2 = (int)((endingPt[1].x - x_range.min) / x_scale + x_skip); - int py2 = (int)((endingPt[1].y - y_range.min) / y_scale + y_skip); - cv::line(img, cv::Point(px1, py1), cv::Point(px2, py2), cv::Scalar(rgb[2], rgb[1], rgb[0]), 2); - int px3 = (int)((endingPt[2].x - x_range.min) / x_scale + x_skip); - int py3 = (int)((endingPt[2].y - y_range.min) / y_scale + y_skip); - int px4 = (int)((endingPt[3].x - x_range.min) / x_scale + x_skip); - int py4 = (int)((endingPt[3].y - y_range.min) / y_scale + y_skip); - if (objOps[i].orienFlag == 1) - { - cv::line(img, cv::Point(px1, py1), cv::Point(px3, py3), cv::Scalar(0, 255, 0), 2); - cv::line(img, cv::Point(px1, py1), cv::Point(px4, py4), cv::Scalar(0, 255, 0), 2); - //cv::circle(img, cv::Point(px1, py1), 5, cv::Scalar(0, 255, 0), -1); - } - else if (objOps[i].orienFlag == 2) - { - cv::line(img, cv::Point(px1, py1), cv::Point(px3, py3), cv::Scalar(0, 0, 255), 2); - cv::line(img, cv::Point(px1, py1), cv::Point(px4, py4), cv::Scalar(0, 0, 255), 2); - //cv::circle(img, cv::Point(px1, py1), 5, cv::Scalar(0, 0, 255), -1); - } - } - } - } - -} - -void _XOYprojection_sideBagInfo(cv::Mat & img, std::vector>&dataLines, std::vector&objOps, - const double x_scale, const double y_scale, const SVzNLRangeD x_range, const SVzNLRangeD y_range) -{ - int x_skip = 16; - int y_skip = 16; - - cv::Vec3b rgb = cv::Vec3b(0, 0, 0); - cv::Vec3b objColor[8] = { - {245,222,179},//淡黄色 - {210,105, 30},//巧克力色 - {240,230,140},//黄褐色 - {135,206,235},//天蓝色 - {250,235,215},//古董白 - {189,252,201},//薄荷色 - {221,160,221},//梅红色 - {188,143,143},//玫瑰红色 - }; - int size = 1; - for (int line = 0; line < dataLines.size(); line++) - { - std::vector< SVzNL3DPosition>& a_line = dataLines[line]; - for (int i = 0; i < a_line.size(); i++) - { - SVzNL3DPosition* pt3D = &a_line[i]; - if (pt3D->pt3D.z < 1e-4) - continue; - - int vType = pt3D->nPointIdx & 0xff; - int hType = vType >> 4; - int objId = (pt3D->nPointIdx >> 16) & 0xffff; - vType = vType & 0x0f; - if (LINE_FEATURE_L_JUMP_H2L == vType) - { - rgb = { 255, 97, 0 }; - size = 2; - } - else if (LINE_FEATURE_L_JUMP_L2H == vType) - { - rgb = { 255, 255, 0 }; - size = 2; - } - else if (LINE_FEATURE_V_SLOPE == vType) - { - rgb = { 255, 0, 255 }; - size = 2; - } - else if (LINE_FEATURE_L_SLOPE_H2L == vType) - { - rgb = { 160, 82, 45 }; - size = 2; - } - else if ((LINE_FEATURE_LINE_ENDING_0 == vType) || (LINE_FEATURE_LINE_ENDING_1 == vType)) - { - rgb = { 255, 0, 0 }; - size = 2; - } - else if (LINE_FEATURE_L_SLOPE_L2H == vType) - { - rgb = { 233, 150, 122 }; - size = 2; - } - else if (LINE_FEATURE_L_JUMP_H2L == hType) - { - rgb = { 0, 0, 255 }; - size = 2; - } - else if (LINE_FEATURE_L_JUMP_L2H == hType) - { - rgb = { 0, 255, 255 }; - size = 2; - } - else if (LINE_FEATURE_V_SLOPE == hType) - { - rgb = { 0, 255, 0 }; - size = 2; - } - else if (LINE_FEATURE_L_SLOPE_H2L == hType) - { - rgb = { 85, 107, 47 }; - size = 2; - } - else if (LINE_FEATURE_L_SLOPE_L2H == hType) - { - rgb = { 0, 255, 154 }; - size = 2; - } - else if ((LINE_FEATURE_LINE_ENDING_0 == hType) || (LINE_FEATURE_LINE_ENDING_1 == hType)) - { - rgb = { 255, 0, 0 }; - size = 2; - } - else if ((objId > 0) && (objId < 1000)) //目标 - { - rgb = objColor[objId % 8]; - size = 3; - } - else - { - rgb = { 150, 150, 150 }; - size = 1; - } - - - double x = pt3D->pt3D.x; - double y = pt3D->pt3D.y; - - int px = (int)((x - x_range.min) / x_scale + x_skip); - int py = (int)((y - y_range.min) / y_scale + y_skip); - if (size == 1) - img.at(py, px) = cv::Vec3b(rgb[2], rgb[1], rgb[0]); - else - cv::circle(img, cv::Point(px, py), size, cv::Scalar(rgb[2], rgb[1], rgb[0]), -1); - } - } - if (objOps.size() > 0) - { - for (int i = 0; i < objOps.size(); i++) - { - if (i == 0) - { - rgb = { 255, 0, 0 }; - size = 20; - } - else - { - rgb = { 255, 255, 0 }; - size = 10; - } - int px = (int)((objOps[i].graspPos.x - x_range.min) / x_scale + x_skip); - int py = (int)((objOps[i].graspPos.y - y_range.min) / y_scale + y_skip); - cv::circle(img, cv::Point(px, py), size, cv::Scalar(rgb[2], rgb[1], rgb[0]), -1); - - //画ROI - size = 3; - cv::Point2d vec2d[4]; - vec2d[0].x = objOps[i].objROI.left; vec2d[0].y = objOps[i].objROI.top; - vec2d[1].x = objOps[i].objROI.right; vec2d[1].y = objOps[i].objROI.top; - vec2d[2].x = objOps[i].objROI.right; vec2d[2].y = objOps[i].objROI.bottom; - vec2d[3].x = objOps[i].objROI.left; vec2d[3].y = objOps[i].objROI.bottom; - cv::Point vec[4]; - for (int j = 0; j < 4; j++) - { - vec[j].x = (int)((vec2d[j].x - x_range.min) / x_scale + x_skip); - vec[j].y = (int)((vec2d[j].y - y_range.min) / y_scale + y_skip); - } - for (int j = 0; j < 4; j++) - { - int nxtIdx = (j + 1) % 4; - cv::line(img, vec[j], vec[nxtIdx], cv::Scalar(rgb[2], rgb[1], rgb[0]), size); - } - - //画倾角 - double r = 50; - double angle = objOps[i].graspPos.z_yaw; - angle = -angle * PI / 180; - cv::Point2d line_pt[2]; - line_pt[0].x = (int)(r * cos(angle) + px); - line_pt[0].y = (int)(-r * sin(angle) + py); - line_pt[1].x = (int)(-r * cos(angle) + px); - line_pt[1].y = (int)(r * sin(angle) + py); - cv::line(img, line_pt[0], line_pt[1], cv::Scalar(rgb[2], rgb[1], rgb[0]), size); - } - - - } - -} - -void _genXOYProjectionImage(cv::String & fileName, SVzNL3DLaserLine * scanData, int lineNum, std::vector&objOps, double rpy[3], double dirLen) -{ - //统计X和Y的范围 - std::vector> scan_lines; - SVzNLRangeD x_range = { 0, -1 }; - SVzNLRangeD y_range = { 0, -1 }; - for (int line = 0; line < lineNum; line++) - { - std::vector< SVzNL3DPosition> a_line; - for (int i = 0; i < scanData[line].nPositionCnt; i++) - { - SVzNL3DPosition* pt3D = &scanData[line].p3DPosition[i]; - if (pt3D->pt3D.z < 1e-4) - continue; - - a_line.push_back(*pt3D); - if (x_range.max < x_range.min) - { - x_range.min = pt3D->pt3D.x; - x_range.max = pt3D->pt3D.x; - } - else - { - if (x_range.min > pt3D->pt3D.x) - x_range.min = pt3D->pt3D.x; - if (x_range.max < pt3D->pt3D.x) - x_range.max = pt3D->pt3D.x; - } - if (y_range.max < y_range.min) - { - y_range.min = pt3D->pt3D.y; - y_range.max = pt3D->pt3D.y; - } - else - { - if (y_range.min > pt3D->pt3D.y) - y_range.min = pt3D->pt3D.y; - if (y_range.max < pt3D->pt3D.y) - y_range.max = pt3D->pt3D.y; - } - } - scan_lines.push_back(a_line); - } - - int imgRows = 992; - int imgCols = 1056; - double y_rows = 960.0; - double x_cols = 1024.0; - cv::Mat img = cv::Mat::zeros(imgRows, imgCols, CV_8UC3); - //计算投影比例 - double x_scale = (x_range.max - x_range.min) / x_cols; - double y_scale = (y_range.max - y_range.min) / y_rows; - if (x_scale < y_scale) - x_scale = y_scale; - else - y_scale = x_scale; - - int angleDrawLen = dirLen;// / x_scale; - _XOYprojection(img, scan_lines, objOps, x_scale, y_scale, x_range, y_range, true, angleDrawLen); - - //旋转视角显示 - double matrix3d[9]; - EulerRpyToRotation1(rpy, matrix3d); - std::vector r_objOps; - r_objOps.insert(r_objOps.end(), objOps.begin(), objOps.end()); - for (int i = 0; i < objOps.size(); i++) - { - SVzNL3DPoint c_pt = { objOps[i].centerPos.x, objOps[i].centerPos.y, objOps[i].centerPos.z }; - SVzNL3DPoint r_c_pt = _ptRotate(c_pt, matrix3d); - r_objOps[i].centerPos.x = r_c_pt.x; - r_objOps[i].centerPos.y = r_c_pt.y; - r_objOps[i].centerPos.z = r_c_pt.z; - } - std::vector> rotateLines; - SVzNLRangeD rx_range, ry_range; - _rotateCloudPts(scanData, lineNum, matrix3d, rotateLines, &rx_range, &ry_range); - cv::Mat r_img = cv::Mat::zeros(imgRows, imgCols, CV_8UC3); - //计算投影比例 - double rx_scale = (rx_range.max - rx_range.min) / x_cols; - double ry_scale = (ry_range.max - ry_range.min) / y_rows; - _XOYprojection(r_img, rotateLines, r_objOps, rx_scale, ry_scale, rx_range, ry_range, false, angleDrawLen); - - cv::Mat dis_img; - cv::hconcat(img, r_img, dis_img); - cv::imwrite(fileName, dis_img); - return; -} - -void _RGBDto2DImage(char* fileName, SVzNLXYZRGBDLaserLine * scanData, int lineNum) -{ - cv::String imgName(fileName); - //统计X和Y的范围 - SVzNLRangeD x_range = { 0, -1 }; - SVzNLRangeD y_range = { 0, -1 }; - for (int line = 0; line < lineNum; line++) - { - for (int i = 0; i < scanData[line].nPointCnt; i++) - { - SVzNLPointXYZRGBA* pt3D = &scanData[line].p3DPoint[i]; - if (pt3D->z < 1e-4) - continue; - - if (x_range.max < x_range.min) - { - x_range.min = pt3D->x; - x_range.max = pt3D->x; - } - else - { - if (x_range.min > pt3D->x) - x_range.min = pt3D->x; - if (x_range.max < pt3D->x) - x_range.max = pt3D->x; - } - if (y_range.max < y_range.min) - { - y_range.min = pt3D->y; - y_range.max = pt3D->y; - } - else - { - if (y_range.min > pt3D->y) - y_range.min = pt3D->y; - if (y_range.max < pt3D->y) - y_range.max = pt3D->y; - } - } - } - - int x_skip = 16; - int y_skip = 16; - int imgRows = 992; - int imgCols = 1056; - double y_rows = (double)(imgRows - y_skip * 2); - double x_cols = (double)(imgCols - x_skip * 2); - cv::Mat img = cv::Mat::zeros(imgRows, imgCols, CV_8UC3); - //计算投影比例 - double x_scale = (x_range.max - x_range.min) / x_cols; - double y_scale = (y_range.max - y_range.min) / y_rows; - if (x_scale < y_scale) - x_scale = y_scale; - else - y_scale = x_scale; - - int size = 1; - cv::Vec3b rgb = cv::Vec3b(0, 0, 0); - for (int line = 0; line < lineNum; line++) - { - for (int i = 0; i < scanData[line].nPointCnt; i++) - { - SVzNLPointXYZRGBA* pt3D = &scanData[line].p3DPoint[i]; - if (pt3D->z < 1e-4) - continue; - - int nRGB = pt3D->nRGB; - int r = nRGB & 0xff; - nRGB >>= 8; - int g = nRGB & 0xff; - nRGB >>= 8; - int b = nRGB & 0xff; - - rgb[0] = r; - rgb[1] = g; - rgb[2] = b; - size = 1; - double x = pt3D->x; - double y = pt3D->y; - int px = (int)((x - x_range.min) / x_scale + x_skip); - int py = (int)((y - y_range.min) / y_scale + y_skip); - if ((px == 666) && (py == 828)) - int kkk = 1; - if (size == 1) - img.at(py, px) = cv::Vec3b(rgb[2], rgb[1], rgb[0]); - else - cv::circle(img, cv::Point(px, py), size, cv::Scalar(rgb[2], rgb[1], rgb[0]), -1); - } - } - cv::imwrite(imgName, img); - return; -} - -void _genXOYProjectionImage_RGBD(cv::String & fileName, SVzNLXYZRGBDLaserLine * scanData, int lineNum, std::vector&objOps, double rpy[3], double dirLen) -{ - //统计X和Y的范围 - std::vector> scan_lines; - SVzNLRangeD x_range = { 0, -1 }; - SVzNLRangeD y_range = { 0, -1 }; - for (int line = 0; line < lineNum; line++) - { - std::vector< SVzNLPointXYZRGBA> a_line; - for (int i = 0; i < scanData[line].nPointCnt; i++) - { - SVzNLPointXYZRGBA* pt3D = &scanData[line].p3DPoint[i]; - if (pt3D->z < 1e-4) - continue; - - a_line.push_back(*pt3D); - if (x_range.max < x_range.min) - { - x_range.min = pt3D->x; - x_range.max = pt3D->x; - } - else - { - if (x_range.min > pt3D->x) - x_range.min = pt3D->x; - if (x_range.max < pt3D->x) - x_range.max = pt3D->x; - } - if (y_range.max < y_range.min) - { - y_range.min = pt3D->y; - y_range.max = pt3D->y; - } - else - { - if (y_range.min > pt3D->y) - y_range.min = pt3D->y; - if (y_range.max < pt3D->y) - y_range.max = pt3D->y; - } - } - scan_lines.push_back(a_line); - } - - int imgRows = 992; - int imgCols = 1056; - double y_rows = 960.0; - double x_cols = 1024.0; - cv::Mat img = cv::Mat::zeros(imgRows, imgCols, CV_8UC3); - //计算投影比例 - double x_scale = (x_range.max - x_range.min) / x_cols; - double y_scale = (y_range.max - y_range.min) / y_rows; - if (x_scale < y_scale) - x_scale = y_scale; - else - y_scale = x_scale; - - int angleDrawLen = dirLen / x_scale; - _XOYprojection_RGBD(img, scan_lines, objOps, x_scale, y_scale, x_range, y_range, true, angleDrawLen); - - //旋转视角显示 - double matrix3d[9]; - EulerRpyToRotation1(rpy, matrix3d); - std::vector r_objOps; - r_objOps.insert(r_objOps.end(), objOps.begin(), objOps.end()); - for (int i = 0; i < objOps.size(); i++) - { - SVzNL3DPoint c_pt = { objOps[i].centerPos.x, objOps[i].centerPos.y, objOps[i].centerPos.z }; - SVzNL3DPoint r_c_pt = _ptRotate(c_pt, matrix3d); - r_objOps[i].centerPos.x = r_c_pt.x; - r_objOps[i].centerPos.y = r_c_pt.y; - r_objOps[i].centerPos.z = r_c_pt.z; - } - std::vector> rotateLines; - SVzNLRangeD rx_range, ry_range; - _rotateCloudPts_RGBD(scanData, lineNum, matrix3d, rotateLines, &rx_range, &ry_range); - cv::Mat r_img = cv::Mat::zeros(imgRows, imgCols, CV_8UC3); - //计算投影比例 - double rx_scale = (rx_range.max - rx_range.min) / x_cols; - double ry_scale = (ry_range.max - ry_range.min) / y_rows; - _XOYprojection_RGBD(r_img, rotateLines, r_objOps, rx_scale, ry_scale, rx_range, ry_range, false, angleDrawLen); - - cv::Mat dis_img; - cv::hconcat(img, r_img, dis_img); - cv::imwrite(fileName, dis_img); - return; -} - -void _genXOYProjectionImage_sideBagInfo(cv::String & fileName, SVzNL3DLaserLine * scanData, int lineNum, std::vector&objOps, double rpy[3]) -{ - //统计X和Y的范围 - std::vector> scan_lines; - SVzNLRangeD x_range = { 0, -1 }; - SVzNLRangeD y_range = { 0, -1 }; - for (int line = 0; line < lineNum; line++) - { - std::vector< SVzNL3DPosition> a_line; - for (int i = 0; i < scanData[line].nPositionCnt; i++) - { - SVzNL3DPosition* pt3D = &scanData[line].p3DPosition[i]; - if (pt3D->pt3D.z < 1e-4) - continue; - - a_line.push_back(*pt3D); - if (x_range.max < x_range.min) - { - x_range.min = pt3D->pt3D.x; - x_range.max = pt3D->pt3D.x; - } - else - { - if (x_range.min > pt3D->pt3D.x) - x_range.min = pt3D->pt3D.x; - if (x_range.max < pt3D->pt3D.x) - x_range.max = pt3D->pt3D.x; - } - if (y_range.max < y_range.min) - { - y_range.min = pt3D->pt3D.y; - y_range.max = pt3D->pt3D.y; - } - else - { - if (y_range.min > pt3D->pt3D.y) - y_range.min = pt3D->pt3D.y; - if (y_range.max < pt3D->pt3D.y) - y_range.max = pt3D->pt3D.y; - } - } - scan_lines.push_back(a_line); - } - - double x_scale = 1.0; - double y_scale = 1.0; - int x_rows = int((x_range.max - x_range.min) / x_scale); - int y_rows = int((y_range.max - y_range.min) / y_scale); - if (x_rows % 2 > 0) - x_rows += 1; - if (y_rows % 2 > 0) - y_rows += 1; - int imgRows = y_rows + 32; - int imgCols = x_rows + 32; - - cv::Mat img = cv::Mat::zeros(imgRows, imgCols, CV_8UC3); - //计算投影比例 - - _XOYprojection_sideBagInfo(img, scan_lines, objOps, x_scale, y_scale, x_range, y_range); - -#if 0 - //旋转视角显示 - double matrix3d[9]; - EulerRpyToRotation1(rpy, matrix3d); - std::vector r_objOps; - r_objOps.insert(r_objOps.end(), objOps.begin(), objOps.end()); - for (int i = 0; i < objOps.size(); i++) - { - SVzNL3DPoint c_pt = { objOps[i].graspPos.x, objOps[i].graspPos.y, objOps[i].graspPos.z }; - SVzNL3DPoint r_c_pt = _ptRotate(c_pt, matrix3d); - r_objOps[i].graspPos.x = r_c_pt.x; - r_objOps[i].graspPos.y = r_c_pt.y; - r_objOps[i].graspPos.z = r_c_pt.z; - } - std::vector> rotateLines; - SVzNLRangeD rx_range, ry_range; - _rotateCloudPts(scanData, lineNum, matrix3d, rotateLines, &rx_range, &ry_range); - cv::Mat r_img = cv::Mat::zeros(imgRows, imgCols, CV_8UC3); - //计算投影比例 - double rx_scale = (rx_range.max - rx_range.min) / x_cols; - double ry_scale = (ry_range.max - ry_range.min) / y_rows; - _XOYprojection_sideBagInfo(r_img, rotateLines, r_objOps, rx_scale, ry_scale, rx_range, ry_range); - - cv::Mat dis_img; - cv::hconcat(img, r_img, dis_img); - cv::imwrite(fileName, dis_img); -#else - cv::Mat rot; - cv::rotate(img, rot, cv::ROTATE_90_CLOCKWISE); // 顺时针90°旋转 - cv::flip(rot, img, 1); // 左右翻转 - cv::imwrite(fileName, img); -#endif - return; -} - - -//量化成640*640左右大小的图像。 -void project2DWithInterpolate(cv::Mat & img, SVzNL3DLaserLine * scanData, int lineNum, double fixed_xy_scale, double z_scale) -{ - double namedSize = 640.0;//目标图像最大尺度为640像素 - - SVzNLRangeD x_range = { 0, -1 }; - SVzNLRangeD y_range = { 0, -1 }; - SVzNLRangeD z_range = { 0, -1 }; - for (int line = 0; line < lineNum; line++) - { - for (int i = 0; i < scanData[line].nPositionCnt; i++) - { - SVzNL3DPosition* pt3D = &scanData[line].p3DPosition[i]; - if (pt3D->pt3D.z < 1e-4) - continue; - - if (x_range.max < x_range.min) - { - x_range.min = pt3D->pt3D.x; - x_range.max = pt3D->pt3D.x; - } - else - { - if (x_range.min > pt3D->pt3D.x) - x_range.min = pt3D->pt3D.x; - if (x_range.max < pt3D->pt3D.x) - x_range.max = pt3D->pt3D.x; - } - if (y_range.max < y_range.min) - { - y_range.min = pt3D->pt3D.y; - y_range.max = pt3D->pt3D.y; - } - else - { - if (y_range.min > pt3D->pt3D.y) - y_range.min = pt3D->pt3D.y; - if (y_range.max < pt3D->pt3D.y) - y_range.max = pt3D->pt3D.y; - } - if (z_range.max < z_range.min) - { - z_range.min = pt3D->pt3D.z; - z_range.max = pt3D->pt3D.z; - } - else - { - if (z_range.min > pt3D->pt3D.z) - z_range.min = pt3D->pt3D.z; - if (z_range.max < pt3D->pt3D.z) - z_range.max = pt3D->pt3D.z; - } - } - } - - double xy_scale = fixed_xy_scale; - if (fixed_xy_scale < 1e-4) - { - double x_scale = (x_range.max - x_range.min) / namedSize; - double y_scale = (y_range.max - y_range.min) / namedSize; - double xy_scale; - if (x_scale < y_scale) - xy_scale = y_scale; - else - xy_scale = x_scale; - } - int img_rows = (int)((y_range.max - y_range.min) / xy_scale) + 1; - if (img_rows % 2 > 0) - img_rows += 1; - int img_cols = (int)((x_range.max - x_range.min) / xy_scale) + 1; - if (img_cols % 2 > 0) - img_cols += 1; - img = cv::Mat::zeros(img_rows, img_cols, CV_8UC1); - - int polateWin = 5; - for (int line = 0; line < lineNum; line++) - { - //同时进行垂直插值 - int pre_py = -1; - SVzNL3DPosition* pre_pt3D = NULL; - int pre_i = -1; - for (int i = 0; i < scanData[line].nPositionCnt; i++) - { - SVzNL3DPosition* pt3D = &scanData[line].p3DPosition[i]; - if (pt3D->pt3D.z < 1e-4) - continue; - double x = pt3D->pt3D.x; - double y = pt3D->pt3D.y; - int px = (int)((x - x_range.min) / xy_scale); - int py = (int)((y - y_range.min) / xy_scale); - int value = (int)((pt3D->pt3D.z - z_range.min) / z_scale); - if (value > 254) - value = 254; - value = 255 - value; - img.at(py, px) = (uchar)value; - //检查是否需要插值 - if ((py > pre_py + 1) && (py <= pre_py + polateWin + 1) && (pre_py >= 0) && (pre_i >= 0) && (i == pre_i + 1)) //3D点连续，量化点不连续，插值 - { - double dist = double(py - pre_py); - for (int iy = pre_py + 1; iy < py; iy++) - { - double k1 = (double)(iy - pre_py) / dist; - double k0 = 1 - k1; - double inter_x = pre_pt3D->pt3D.x * k0 + pt3D->pt3D.x * k1; - double inter_y = pre_pt3D->pt3D.y * k0 + pt3D->pt3D.y * k1; - double inter_z = pre_pt3D->pt3D.z * k0 + pt3D->pt3D.z * k1; - int polate_px = (int)((inter_x - x_range.min) / xy_scale); - int polate_py = (int)((inter_y - y_range.min) / xy_scale); - int polate_value = (int)((inter_z - z_range.min) / z_scale); - if (polate_value > 254) - polate_value = 254; - polate_value = 255 - polate_value; - img.at(polate_py, polate_px) = (uchar)polate_value; - } - } - pre_i = i; - pre_py = py; - pre_pt3D = pt3D; - } - } - //水平插值 - for (int y = 0; y < img.rows; y++) - { - int pre_x = -1; - uchar pre_value = 0; - for (int x = 0; x < img.cols; x++) - { - uchar value = img.at(y, x); - if (value > 0) - { - if ((x > pre_x + 1) && (x <= pre_x + polateWin + 1) && (pre_x >= 0)) //水平不连续，插值 - { - double dist = double(x - pre_x); - for (int ix = pre_x + 1; ix < x; ix++) - { - double k1 = (double)(ix - pre_x) / dist; - double k0 = 1 - k1; - double inter_data = (double)pre_value * k0 + (double)value * k1; - if (inter_data > 255) - inter_data = 255; - uchar polate_value = (uchar)inter_data; - img.at(y, ix) = polate_value; - } - } - pre_x = x; - pre_value = value; - } - } - } -} -void _outputScanDataFile_removeZeros(char* fileName, SVzNL3DLaserLine * scanData, int lineNum, - float lineV, int maxTimeStamp, int clockPerSecond) -{ - std::ofstream sw(fileName); - sw << "LineNum:" << lineNum << std::endl; - sw << "DataType: 0" << std::endl; - sw << "ScanSpeed:" << lineV << std::endl; - sw << "PointAdjust: 1" << std::endl; - sw << "MaxTimeStamp:" << maxTimeStamp << "_" << clockPerSecond << std::endl; - - for (int line = 0; line < lineNum; line++) - { - int realNum = 0; - for (int i = 0; i < scanData[line].nPositionCnt; i++) - { - if (scanData[line].p3DPosition[i].pt3D.z > 1e-4) - realNum++; - } - sw << "Line_" << line << "_" << scanData[line].nTimeStamp << "_" << realNum << std::endl; - for (int i = 0; i < scanData[line].nPositionCnt; i++) - { - if (scanData[line].p3DPosition[i].pt3D.z > 1e-4) - { - SVzNL3DPosition* pt3D = &scanData[line].p3DPosition[i]; - float x = (float)pt3D->pt3D.x; - float y = (float)pt3D->pt3D.y; - float z = (float)pt3D->pt3D.z; - sw << "{ " << x << "," << y << "," << z << " }-"; - sw << "{0,0}-{0,0}" << std::endl; - } - } - } - sw.close(); -} - -#define TEST_CONVERT_TO_GRID 0 -#define TEST_COMPUTE_WHEEL_ARCH 1 -#define TEST_COMPUTE_CALIB_PARA 0 - -#define TEST_GROUP 1 -int main() -{ -#if TEST_CONVERT_TO_GRID - //将数据转换成栅格格式格式 - char _scan_dir[256]; - int lineNum = 0; - float lineV = 0.0f; - int dataCalib = 0; - int maxTimeStamp = 0; - int clockPerSecond = 0; - sprintf_s(_scan_dir, "F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/"); - char _scan_src_file[256]; - double _F = 1177.2; //f - for (int i = 1; i <= 1; i++) - { - //read ply txt - sprintf_s(_scan_src_file, "%sLaserLine%d.txt", _scan_dir, i); - std::vector> scanData; - vzReadPlyTxtPointFromFile_XYZRGB_vector(_scan_src_file, scanData); - sprintf_s(_scan_src_file, "%sscanData%d.txt", _scan_dir, i); - _outputScanDataFile_SPointXYZRGB_vector(_scan_src_file, scanData); - -#if 0 - double rpy[3] = { 0,-18, 0 }; - double camPoseR[9]; - EulerRpyToRotation1(rpy, camPoseR); -#else - double camPoseR[9] = { - 1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0 }; -#endif - std::vector> gridData; - _convertToGridData_XYZRGB(scanData, _F, gridData); - char _out_file[256]; - sprintf_s(_out_file, "%sLaserLine%d_grid.txt", _scan_dir, i); - _outputScanDataFile_self(_out_file, gridData, 0, 0, 0); - printf("%s: convert done!\n", _scan_src_file); - } -#endif - -#if TEST_COMPUTE_CALIB_PARA - char _calib_datafile[256]; - sprintf_s(_calib_datafile, "F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/LaserLine1_grid.txt"); - - std::vector< std::vector> scanLines; - vzReadLaserScanPointFromFile_XYZ_vector(_calib_datafile, scanLines); - if(scanLines.size() > 0) - { - SSG_planeCalibPara calibPara = wd_horizonCamera_getGroundCalibPara(scanLines); -#if 1 - //结果进行验证 - int lineNum = (int)scanLines.size(); - for (int i = 0; i < lineNum; i++) - { - if (i == 14) - int kkk = 1; - //行处理 - //调平，去除地面 - wd_horizonCamera_lineDataR(scanLines[i], calibPara.planeCalib, -1);// calibPara.planeHeight); - } -#endif - // - char calibFile[250]; - sprintf_s(calibFile, "F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/ground_calib_para.txt"); - _outputCalibPara(calibFile, calibPara); - char _out_file[256]; - sprintf_s(_out_file, "F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/LaserLine1_grid_calib.txt"); - _outputScanDataFile_self(_out_file, scanLines, 0, 0, 0); - printf("%s: calib done!\n", _calib_datafile); - } - -#endif - -#if TEST_COMPUTE_WHEEL_ARCH - const char* dataPath[TEST_GROUP] = { - "F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/", //0 - }; - - SVzNLRange fileIdx[TEST_GROUP] = { - {1,1}, - }; - - SSG_planeCalibPara poseCalibPara; - //初始化成单位阵 - poseCalibPara.planeCalib[0] = 1.0; - poseCalibPara.planeCalib[1] = 0.0; - poseCalibPara.planeCalib[2] = 0.0; - poseCalibPara.planeCalib[3] = 0.0; - poseCalibPara.planeCalib[4] = 1.0; - poseCalibPara.planeCalib[5] = 0.0; - poseCalibPara.planeCalib[6] = 0.0; - poseCalibPara.planeCalib[7] = 0.0; - poseCalibPara.planeCalib[8] = 1.0; - poseCalibPara.planeHeight = -1.0; - for (int i = 0; i < 9; i++) - poseCalibPara.invRMatrix[i] = poseCalibPara.planeCalib[i]; - - const char* ver = wd_wheelArchHeigthMeasureVersion(); - printf("ver:%s\n", ver); - - char _scan_file[256]; - - int endGroup = TEST_GROUP - 1; - for (int grp = 0; grp <= endGroup; grp++) - { - char calibFile[250]; - sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[grp]); - poseCalibPara = _readCalibPara(calibFile); - for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) - { - //fidx = 193; - sprintf_s(_scan_file, "%sLaserLine1_grid.txt", dataPath[grp], fidx); - std::vector> scanData; - vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanData); - if (scanData.size() == 0) - continue; - - //algoParam.filterParam.continuityTh = 20.0; //噪声滤除。当相邻点的z跳变大于此门限时，检查是否为噪声。若长度小于outlierLen， 视为噪声 - //algoParam.filterParam.outlierTh = 5; - - long t1 = GetTickCount64(); - int lineNum = (int)scanData.size(); - for (int i = 0; i < lineNum; i++) - { - if (i == 14) - int kkk = 1; - //行处理 - //调平，去除地面 - wd_horizonCamera_lineDataR(scanData[i], poseCalibPara.planeCalib, poseCalibPara.planeHeight); - } -#if 0 //数据转存 - sprintf_s(_scan_file, "%sLaserLine%d_grid_RTadjust.txt", dataPath[grp], fidx); - _outputScanDataFile_self(_scan_file, scanData, 0, 0, 0); -#endif - SSG_cornerParam cornerParam; - cornerParam.cornerTh = 60; //45度角 - cornerParam.scale = 50; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; - cornerParam.minEndingGap = 20; // algoParam.bagParam.bagW / 4; - cornerParam.minEndingGap_z = 20; - cornerParam.jumpCornerTh_1 = 10; //水平角度，小于此角度视为水平 - cornerParam.jumpCornerTh_2 = 60; - - SSG_lineSegParam lineSegPara; - lineSegPara.maxDist = 1.0; - lineSegPara.segGapTh_y = 5.0; //y方向间隔大于5mm认为是分段 - lineSegPara.segGapTh_z = 10.0; //z方向间隔大于10mm认为是分段 - - SSG_outlierFilterParam filterParam; - filterParam.continuityTh = 20.0; //噪声滤除。当相邻点的z跳变大于此门限时，检查是否为噪声。若长度小于outlierLen， 视为噪声 - filterParam.outlierTh = 5; - - SSG_treeGrowParam growParam; - growParam.maxLineSkipNum = 10; - growParam.yDeviation_max = 5.0; - growParam.maxSkipDistance = 5.0; - growParam.zDeviation_max = 2.0;// - growParam.minLTypeTreeLen = 100; //mm - growParam.minVTypeTreeLen = 100; //mm - - int errCode = 0; - WD_wheelArchInfo wheelArcHeight = wd_wheelArchHeigthMeasure( - scanData, - cornerParam, - lineSegPara, - filterParam, - growParam, - poseCalibPara, - &errCode); -#endif - long t2 = GetTickCount64(); - - char _dbg_file[256]; -#if 1 - - sprintf_s(_dbg_file, "%sresult\\LaserLine%d_result.txt", dataPath[grp], fidx); - _outputScanDataFile_RGBD_obj(_dbg_file, scanData, 0, 0, 0, wheelArcHeight); -#endif - printf("%s: height=%f, %d(ms)!\n", _scan_file, wheelArcHeight.archToCenterHeigth, (int)(t2 - t1)); - - - } - } - printf("all done!\n"); -} - - -// 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单 -// 调试程序: F5 或调试 >“开始调试”菜单 - -// 入门使用技巧: -// 1. 使用解决方案资源管理器窗口添加/管理文件 -// 2. 使用团队资源管理器窗口连接到源代码管理 -// 3. 使用输出窗口查看生成输出和其他消息 -// 4. 使用错误列表窗口查看错误 -// 5. 转到“项目”>“添加新项”以创建新的代码文件，或转到“项目”>“添加现有项”以将现有代码文件添加到项目 -// 6. 将来，若要再次打开此项目，请转到“文件”>“打开”>“项目”并选择 .sln 文件 +// wheelArchHeigthMeasure_test.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。 +// +#include +#include +#include +#include +#include +#include +#include "wheelArchHeigthMeasure_Export.h" +#include +#ifdef _WIN32 +#include +#include "direct.h" +#endif + +typedef struct +{ + int nPointIdx; + double x; + double y; + double z; + float r; + float g; + float b; +} SPointXYZRGB; + +SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9]) +{ + SVzNL3DPoint _r_pt; + _r_pt.x = pt3D.x * matrix3d[0] + pt3D.y * matrix3d[1] + pt3D.z * matrix3d[2]; + _r_pt.y = pt3D.x * matrix3d[3] + pt3D.y * matrix3d[4] + pt3D.z * matrix3d[5]; + _r_pt.z = pt3D.x * matrix3d[6] + pt3D.y * matrix3d[7] + pt3D.z * matrix3d[8]; + return _r_pt; +} + +SVzNLPointXYZRGBA _ptRotate_RGBD(SVzNLPointXYZRGBA pt3D, double matrix3d[9]) +{ + SVzNLPointXYZRGBA _r_pt; + _r_pt.x = pt3D.x * matrix3d[0] + pt3D.y * matrix3d[1] + pt3D.z * matrix3d[2]; + _r_pt.y = pt3D.x * matrix3d[3] + pt3D.y * matrix3d[4] + pt3D.z * matrix3d[5]; + _r_pt.z = pt3D.x * matrix3d[6] + pt3D.y * matrix3d[7] + pt3D.z * matrix3d[8]; + return _r_pt; +} + +#define DATA_VER_OLD 0 +#define DATA_VER_NEW 1 +#define DATA_VER_FROM_CUSTOM 2 +#define VZ_LASER_LINE_PT_MAX_NUM 4096 +SVzNLXYZRGBDLaserLine* vzReadLaserScanPointFromFile_XYZRGB(const char* fileName, int* scanLineNum, float* scanV, + int* dataCalib, int* scanMaxStamp, int* canClockUnit, bool removeNullLines) +{ + std::ifstream inputFile(fileName); + std::string linedata; + + if (inputFile.is_open() == false) + return NULL; + + SVzNLXYZRGBDLaserLine* _scanLines = NULL; + + int lines = 0; + int dataElements = 4; + int firstIndex = -1; + + int dataFileVer = DATA_VER_OLD; + std::getline(inputFile, linedata); //第一行 + int lineNum = 0; + if (0 == strncmp("LineNum:", linedata.c_str(), 8)) + { + sscanf_s(linedata.c_str(), "LineNum:%d", &lines); + if (lines == 0) + return NULL; + lineNum = lines; + _scanLines = (SVzNLXYZRGBDLaserLine*)malloc(sizeof(SVzNLXYZRGBDLaserLine) * (lineNum + 1)); + memset(_scanLines, 0, sizeof(SVzNLXYZRGBDLaserLine) * (lineNum + 1)); + } + if (_scanLines == NULL) + return NULL; + + int lineIdx = 0; + int ptIdx = 0; + int ptNum = 0; + int pre_ptNum = -1; + std::vector< SVzNLPointXYZRGBA> a_line; + int vldLineIdx = 0; + int vldPtNum = 0; + unsigned int timeStamp = 0; + while (getline(inputFile, linedata)) + { + if (0 == strncmp("ScanSpeed:", linedata.c_str(), 10)) + { + double lineV = 0; + sscanf_s(linedata.c_str(), "ScanSpeed:%lf", &lineV); + if (scanV) + *scanV = (float)lineV; + } + else if (0 == strncmp("PointAdjust:", linedata.c_str(), 12)) + { + int ptAdjusted = 0; + sscanf_s(linedata.c_str(), "PointAdjust:%d", &ptAdjusted); + if (dataCalib) + *dataCalib = ptAdjusted; + } + else if (0 == strncmp("MaxTimeStamp:", linedata.c_str(), 13)) + { + unsigned int maxTimeStamp = 0; + unsigned int timePerStamp = 0; + sscanf_s(linedata.c_str(), "MaxTimeStamp:%u_%u", &maxTimeStamp, &timePerStamp); + if (scanMaxStamp) + *scanMaxStamp = maxTimeStamp; + if (canClockUnit) + *canClockUnit = timePerStamp; + } + else if (0 == strncmp("Line_", linedata.c_str(), 5)) + { + int lineIndex; + unsigned int curr_timeStamp; + sscanf_s(linedata.c_str(), "Line_%d_%u_%d", &lineIndex, &curr_timeStamp, &ptNum); + if (firstIndex < 0) + firstIndex = lineIndex; + + lineIndex = lineIndex - firstIndex; + if ((lineIndex < 0) || (lineIndex >= lines)) + break; + + int recvPtNum = (int)a_line.size(); + if ((recvPtNum == pre_ptNum) && ((vldPtNum > 0) || (false == removeNullLines))) + { + SVzNLPointXYZRGBA* p3DPoint; + if (pre_ptNum > 0) + p3DPoint = (SVzNLPointXYZRGBA*)malloc(sizeof(SVzNLPointXYZRGBA) * pre_ptNum); + else + p3DPoint = NULL; + _scanLines[vldLineIdx].nPointCnt = pre_ptNum; + _scanLines[vldLineIdx].nTimeStamp = timeStamp; + _scanLines[vldLineIdx].p3DPoint = p3DPoint; + for (int m = 0; m < pre_ptNum; m++) + p3DPoint[m] = a_line[m]; + vldLineIdx++; + } + //new Line + timeStamp = curr_timeStamp; + vldPtNum = 0; + a_line.clear(); + pre_ptNum = ptNum; + } + else if (0 == strncmp("{", linedata.c_str(), 1)) + { + float X, Y, Z; + int imageY = 0; + float leftX, leftY; + float rightX, rightY; + float r, g, b; + sscanf_s(linedata.c_str(), "{%f,%f,%f,%f,%f,%f }-{%f,%f}-{%f,%f}", &X, &Y, &Z, &r, &g, &b, &leftX, &leftY, &rightX, &rightY); + { + if (lineIdx == 537) + int kkk = 1; + + SVzNLPointXYZRGBA a_pt; + + a_pt.x = X; + a_pt.y = Y; + a_pt.z = Z; + int nr = (int)(r * 255); + int ng = (int)(g * 255); + int nb = (int)(b * 255); + nb <<= 8; + nb += ng; + nb <<= 8; + nb += nr; + a_pt.nRGB = nb; + if (a_pt.z > 1e-4) + vldPtNum++; + a_line.push_back(a_pt); + } + } + } + //last line + int recvPtNum = (int)a_line.size(); + if ((recvPtNum == pre_ptNum) && ((vldPtNum > 0) || (false == removeNullLines))) + { + SVzNLPointXYZRGBA* p3DPoint; + if (pre_ptNum > 0) + p3DPoint = (SVzNLPointXYZRGBA*)malloc(sizeof(SVzNLPointXYZRGBA) * pre_ptNum); + else + p3DPoint = NULL; + _scanLines[vldLineIdx].nPointCnt = pre_ptNum; + _scanLines[vldLineIdx].nTimeStamp = timeStamp; + _scanLines[vldLineIdx].p3DPoint = p3DPoint; + for (int m = 0; m < pre_ptNum; m++) + p3DPoint[m] = a_line[m]; + vldLineIdx++; + } + + if (scanLineNum) + *scanLineNum = vldLineIdx; + inputFile.close(); + return _scanLines; +} + +void vzReadLaserScanPointFromFile_XYZRGB_vector(const char* fileName, std::vector>&scanData) +{ + std::ifstream inputFile(fileName); + std::string linedata; + + if (inputFile.is_open() == false) + return; + + std::vector< SPointXYZRGB> a_line; + while (getline(inputFile, linedata)) + { + if (0 == strncmp("Line_", linedata.c_str(), 5)) //new line + { + if (a_line.size() > 0) + { + scanData.push_back(a_line); + a_line.clear(); + } + } + else if (0 == strncmp("{", linedata.c_str(), 1)) + { + float leftX, leftY; + float rightX, rightY; + SPointXYZRGB a_pt; + sscanf_s(linedata.c_str(), "{ %lf, %lf, %lf, %f, %f, %f }-{ %f, %f }-{ %f, %f }", &a_pt.x, &a_pt.y, &a_pt.z, &a_pt.r, &a_pt.g, &a_pt.b, &leftX, &leftY, &rightX, &rightY); + a_line.push_back(a_pt); + } + } + if (a_line.size() > 0) + scanData.push_back(a_line); + + inputFile.close(); + return; +} + +void vzReadPlyTxtPointFromFile_XYZRGB_vector(const char* fileName, std::vector>& scanData) +{ + std::ifstream inputFile(fileName); + std::string linedata; + + if (inputFile.is_open() == false) + return; + + const double lineYGap = 50; + std::vector< SVzNLPointXYZRGBA> a_line; + + double pre_y = FLT_MIN; + while (getline(inputFile, linedata)) + { + SVzNLPointXYZRGBA a_pt; + memset(&a_pt, 0, sizeof(SVzNLPointXYZRGBA)); + double _x, _y, _z; + sscanf_s(linedata.c_str(), "%lf %lf %lf", &_x, &_y, &_z); + a_pt.x = (float)_x; + a_pt.y = (float)_y; + a_pt.z = (float)_z; + if(a_pt.y < pre_y - lineYGap) //新的扫描行 + { + if (a_line.size() > 0) + { + scanData.push_back(a_line); + a_line.clear(); + } + } + pre_y = a_pt.y; + a_line.push_back(a_pt); + } + if (a_line.size() > 0) + scanData.push_back(a_line); + + inputFile.close(); + return; +} + +SVzNL3DLaserLine* vzReadLaserScanPointFromFile_XYZ(const char* fileName, int* scanLineNum, float* scanV, + int* dataCalib, int* scanMaxStamp, int* canClockUnit) +{ + std::ifstream inputFile(fileName); + std::string linedata; + + if (inputFile.is_open() == false) + return NULL; + + SVzNL3DLaserLine* _scanLines = NULL; + + int lines = 0; + int dataElements = 4; + int firstIndex = -1; + + int dataFileVer = DATA_VER_OLD; + std::getline(inputFile, linedata); //第一行 + int lineNum = 0; + if (0 == strncmp("LineNum:", linedata.c_str(), 8)) + { + dataFileVer = DATA_VER_NEW; + sscanf_s(linedata.c_str(), "LineNum:%d", &lines); + if (lines == 0) + return NULL; + lineNum = lines; + _scanLines = (SVzNL3DLaserLine*)malloc(sizeof(SVzNL3DLaserLine) * (lineNum + 1)); + memset(_scanLines, 0, sizeof(SVzNL3DLaserLine) * (lineNum + 1)); + if (scanLineNum) + *scanLineNum = lines; + } + else if (0 == strncmp("LineNum_", linedata.c_str(), 8)) + { + dataFileVer = DATA_VER_OLD; + sscanf_s(linedata.c_str(), "LineNum_%d", &lines); + if (lines == 0) + return NULL; + lineNum = lines; + _scanLines = (SVzNL3DLaserLine*)malloc(sizeof(SVzNL3DLaserLine) * (lineNum + 1)); + memset(_scanLines, 0, sizeof(SVzNL3DLaserLine) * (lineNum + 1)); + if (scanLineNum) + *scanLineNum = lines; + } + if (_scanLines == NULL) + return NULL; + + int ptNum = 0; + int lineIdx = -1; + int ptIdx = 0; + SVzNL3DPosition* p3DPoint = NULL; + if (dataFileVer == DATA_VER_NEW) + { + while (getline(inputFile, linedata)) + { + if (0 == strncmp("ScanSpeed:", linedata.c_str(), 10)) + { + double lineV = 0; + sscanf_s(linedata.c_str(), "ScanSpeed:%lf", &lineV); + if (scanV) + *scanV = (float)lineV; + } + else if (0 == strncmp("PointAdjust:", linedata.c_str(), 12)) + { + int ptAdjusted = 0; + sscanf_s(linedata.c_str(), "PointAdjust:%d", &ptAdjusted); + if (dataCalib) + *dataCalib = ptAdjusted; + } + else if (0 == strncmp("MaxTimeStamp:", linedata.c_str(), 13)) + { + unsigned int maxTimeStamp = 0; + unsigned int timePerStamp = 0; + sscanf_s(linedata.c_str(), "MaxTimeStamp:%u_%u", &maxTimeStamp, &timePerStamp); + if (scanMaxStamp) + *scanMaxStamp = maxTimeStamp; + if (canClockUnit) + *canClockUnit = timePerStamp; + } + else if (0 == strncmp("Line_", linedata.c_str(), 5)) + { + int lineIndex; + unsigned int timeStamp; + sscanf_s(linedata.c_str(), "Line_%d_%u_%d", &lineIndex, &timeStamp, &ptNum); + if (firstIndex < 0) + firstIndex = lineIndex; + + lineIndex = lineIndex - firstIndex; + if ((lineIndex < 0) || (lineIndex >= lines)) + break; + + //new Line + lineIdx++; + if (ptNum > 0) + { + p3DPoint = (SVzNL3DPosition*)malloc(sizeof(SVzNL3DPosition) * ptNum); + memset(p3DPoint, 0, sizeof(SVzNL3DPosition) * ptNum); + } + else + p3DPoint = NULL; + _scanLines[lineIdx].nPositionCnt = 0; + _scanLines[lineIdx].nTimeStamp = timeStamp; + _scanLines[lineIdx].p3DPosition = p3DPoint; + + } + else if (0 == strncmp("{", linedata.c_str(), 1)) + { + float X, Y, Z; + int imageY = 0; + float leftX, leftY; + float rightX, rightY; + sscanf_s(linedata.c_str(), "{%f,%f,%f}-{%f,%f}-{%f,%f}", &X, &Y, &Z, &leftX, &leftY, &rightX, &rightY); + int id = _scanLines[lineIdx].nPositionCnt; + if (id < ptNum) + { + p3DPoint[id].pt3D.x = X; + p3DPoint[id].pt3D.y = Y; + p3DPoint[id].pt3D.z = Z; + _scanLines[lineIdx].nPositionCnt = id + 1; + } + } + } + + } + else if (dataFileVer == DATA_VER_OLD) + { + while (getline(inputFile, linedata)) + { + if (0 == strncmp("DataElements_", linedata.c_str(), 13)) + { + sscanf_s(linedata.c_str(), "DataElements_%d", &dataElements); + if ((dataElements != 3) && (dataElements != 4)) + break; + } + if (0 == strncmp("LineV_", linedata.c_str(), 6)) + { + double lineV = 0; + sscanf_s(linedata.c_str(), "LineV_%lf", &lineV); + } + else if (0 == strncmp("Line_", linedata.c_str(), 5)) + { + int lineIndex; + unsigned int timeStamp; + sscanf_s(linedata.c_str(), "Line_%d_%u", &lineIndex, &timeStamp); +#if 0 + if (scanLineListTail == NULL) + firstIndex = lineIndex; +#endif + lineIndex = lineIndex - firstIndex; + if ((lineIndex < 0) || (lineIndex >= lines)) + break; + //new Line + //new Line + lineIdx++; + p3DPoint = (SVzNL3DPosition*)malloc(sizeof(SVzNL3DPosition) * VZ_LASER_LINE_PT_MAX_NUM); + memset(p3DPoint, 0, sizeof(SVzNL3DPosition) * VZ_LASER_LINE_PT_MAX_NUM); + _scanLines[lineIdx].nPositionCnt = 0; + _scanLines[lineIdx].nTimeStamp = timeStamp; + _scanLines[lineIdx].p3DPosition = p3DPoint; + } + else if (0 == strncmp("(", linedata.c_str(), 1)) + { + float X, Y, Z; + int imageY = 0; + if (dataElements == 4) + sscanf_s(linedata.c_str(), "(%f,%f,%f,%d)", &X, &Y, &Z, &imageY); + else + sscanf_s(linedata.c_str(), "(%f,%f,%f)", &X, &Y, &Z); + int id = _scanLines[lineIdx].nPositionCnt; + if (id < VZ_LASER_LINE_PT_MAX_NUM) + { + p3DPoint[id].pt3D.x = X; + p3DPoint[id].pt3D.y = Y; + p3DPoint[id].pt3D.z = Z; + _scanLines[lineIdx].nPositionCnt = id + 1; + } + } + } + } + inputFile.close(); + return _scanLines; +} + +//从RGBD点云中读取XYZ信息 +SVzNL3DLaserLine* vzReadXYZPointFromFile_XYZRGB(const char* fileName, int* scanLineNum, float* scanV, + int* dataCalib, int* scanMaxStamp, int* canClockUnit) +{ + std::ifstream inputFile(fileName); + std::string linedata; + + if (inputFile.is_open() == false) + return NULL; + + SVzNL3DLaserLine* _scanLines = NULL; + + int lines = 0; + int dataElements = 4; + int firstIndex = -1; + + int dataFileVer = DATA_VER_OLD; + std::getline(inputFile, linedata); //第一行 + int lineNum = 0; + if (0 == strncmp("LineNum:", linedata.c_str(), 8)) + { + dataFileVer = DATA_VER_NEW; + sscanf_s(linedata.c_str(), "LineNum:%d", &lines); + if (lines == 0) + return NULL; + lineNum = lines; + _scanLines = (SVzNL3DLaserLine*)malloc(sizeof(SVzNL3DLaserLine) * (lineNum + 1)); + memset(_scanLines, 0, sizeof(SVzNL3DLaserLine) * (lineNum + 1)); + if (scanLineNum) + *scanLineNum = lines; + } + else if (0 == strncmp("LineNum_", linedata.c_str(), 8)) + { + dataFileVer = DATA_VER_OLD; + sscanf_s(linedata.c_str(), "LineNum_%d", &lines); + if (lines == 0) + return NULL; + lineNum = lines; + _scanLines = (SVzNL3DLaserLine*)malloc(sizeof(SVzNL3DLaserLine) * (lineNum + 1)); + memset(_scanLines, 0, sizeof(SVzNL3DLaserLine) * (lineNum + 1)); + if (scanLineNum) + *scanLineNum = lines; + } + if (_scanLines == NULL) + return NULL; + + int ptNum = 0; + int lineIdx = -1; + int ptIdx = 0; + SVzNL3DPosition* p3DPoint = NULL; + if (dataFileVer == DATA_VER_NEW) + { + while (getline(inputFile, linedata)) + { + if (0 == strncmp("ScanSpeed:", linedata.c_str(), 10)) + { + double lineV = 0; + sscanf_s(linedata.c_str(), "ScanSpeed:%lf", &lineV); + if (scanV) + *scanV = (float)lineV; + } + else if (0 == strncmp("PointAdjust:", linedata.c_str(), 12)) + { + int ptAdjusted = 0; + sscanf_s(linedata.c_str(), "PointAdjust:%d", &ptAdjusted); + if (dataCalib) + *dataCalib = ptAdjusted; + } + else if (0 == strncmp("MaxTimeStamp:", linedata.c_str(), 13)) + { + unsigned int maxTimeStamp = 0; + unsigned int timePerStamp = 0; + sscanf_s(linedata.c_str(), "MaxTimeStamp:%u_%u", &maxTimeStamp, &timePerStamp); + if (scanMaxStamp) + *scanMaxStamp = maxTimeStamp; + if (canClockUnit) + *canClockUnit = timePerStamp; + } + else if (0 == strncmp("Line_", linedata.c_str(), 5)) + { + int lineIndex; + unsigned int timeStamp; + sscanf_s(linedata.c_str(), "Line_%d_%u_%d", &lineIndex, &timeStamp, &ptNum); + if (firstIndex < 0) + firstIndex = lineIndex; + + lineIndex = lineIndex - firstIndex; + if ((lineIndex < 0) || (lineIndex >= lines)) + break; + + //new Line + lineIdx++; + if (ptNum > 0) + { + p3DPoint = (SVzNL3DPosition*)malloc(sizeof(SVzNL3DPosition) * ptNum); + memset(p3DPoint, 0, sizeof(SVzNL3DPosition) * ptNum); + } + else + p3DPoint = NULL; + _scanLines[lineIdx].nPositionCnt = 0; + _scanLines[lineIdx].nTimeStamp = timeStamp; + _scanLines[lineIdx].p3DPosition = p3DPoint; + + } + else if (0 == strncmp("{", linedata.c_str(), 1)) + { + float X, Y, Z; + float leftX, leftY; + float rightX, rightY; + float r, g, b; + sscanf_s(linedata.c_str(), "{%f,%f,%f,%f,%f,%f }-{%f,%f}-{%f,%f}", &X, &Y, &Z, &r, &g, &b, &leftX, &leftY, &rightX, &rightY); + int id = _scanLines[lineIdx].nPositionCnt; + if (id < ptNum) + { + p3DPoint[id].pt3D.x = X; + p3DPoint[id].pt3D.y = Y; + p3DPoint[id].pt3D.z = Z; + _scanLines[lineIdx].nPositionCnt = id + 1; + } + } + } + + } + inputFile.close(); + return _scanLines; +} + +void vzReadLaserScanPointFromFile_XYZ_vector(const char* fileName, std::vector>& scanData) +{ + std::ifstream inputFile(fileName); + std::string linedata; + + if (inputFile.is_open() == false) + return; + + std::vector< SVzNL3DPosition> a_line; + int ptIdx = 0; + while (getline(inputFile, linedata)) + { + if (0 == strncmp("Line_", linedata.c_str(), 5)) + { + int ptSize = (int)a_line.size(); + if (ptSize > 0) + { + scanData.push_back(a_line); + } + a_line.clear(); + ptIdx = 0; + } + else if (0 == strncmp("{", linedata.c_str(), 1)) + { + float X, Y, Z; + int imageY = 0; + float leftX, leftY; + float rightX, rightY; + sscanf_s(linedata.c_str(), "{%f,%f,%f}-{%f,%f}-{%f,%f}", &X, &Y, &Z, &leftX, &leftY, &rightX, &rightY); + SVzNL3DPosition a_pt; + a_pt.pt3D.x = X; + a_pt.pt3D.y = Y; + a_pt.pt3D.z = Z; + a_pt.nPointIdx = ptIdx; + ptIdx++; + a_line.push_back(a_pt); + } + } + //last line + int ptSize = (int)a_line.size(); + if (ptSize > 0) + { + scanData.push_back(a_line); + a_line.clear(); + } + + inputFile.close(); + return; +} + +void _convertToGridData_XYZRGB(std::vector>& scanData, double _F, std::vector>& gridData) +{ + int lineNum = (int)scanData.size(); + int min_y = 100000000; + int max_y = -10000000; + int validStartLine = -1; + int validEndLine = -1; + for (int line = 0; line < lineNum; line++) + { + if (scanData[line].size() > 0) + { + if (validStartLine < 0) + { + validStartLine = line; + validEndLine = line; + } + else + validEndLine = line; + } + + for (int i = 0; i < (int)scanData[line].size(); i++) + { + SVzNLPointXYZRGBA& a_pt = scanData[line][i]; + if (a_pt.z > 1e-4) + { + double v = _F * a_pt.y / a_pt.z + 2000; + a_pt.nRGB = (int)(v + 0.5); + max_y = max_y < (int)a_pt.nRGB ? (int)a_pt.nRGB : max_y; + min_y = min_y > (int)a_pt.nRGB ? (int)a_pt.nRGB : min_y; + } + } + } + if (min_y == 100000000) + return; + + int vldLineNum = validEndLine - validStartLine + 1; + gridData.resize(vldLineNum); + int pt_counter = max_y - min_y + 1; + for (int i = 0; i < vldLineNum; i++) + { + gridData[i].resize(pt_counter); + for (int j = 0; j < pt_counter; j++) + { + gridData[i][j].nPointIdx = j; + gridData[i][j].pt3D.x = 0; + gridData[i][j].pt3D.y = 0; + gridData[i][j].pt3D.z = 0; + } + } + for (int line = validStartLine; line <= validEndLine; line++) + { + int gridLine = line - validStartLine; + for (int i = 0; i < (int)scanData[line].size(); i++) + { + SVzNLPointXYZRGBA& a_pt = scanData[line][i]; + if (a_pt.z > 1e-4) + { + int pt_id = a_pt.nRGB - min_y; + gridData[gridLine][pt_id].pt3D.x = a_pt.x; + gridData[gridLine][pt_id].pt3D.y = a_pt.y; + gridData[gridLine][pt_id].pt3D.z = a_pt.z; + } + } + } + return; +} + +void _convertToGridData_XYZRGB_vector(std::vector>&scanData, double _F, std::vector>&scanData_grid) +{ + int min_y = 100000000; + int max_y = -10000000; + int lineNum = scanData.size(); + for (int line = 0; line < lineNum; line++) + { + std::vector< SPointXYZRGB>& a_line = scanData[line]; + int nPointCnt = a_line.size(); + for (int i = 0; i < nPointCnt; i++) + { + SPointXYZRGB* a_pt = &scanData[line][i]; + if (a_pt->z > 1e-4) + { + double v = _F * a_pt->y / a_pt->z + 2000; + a_pt->nPointIdx = (int)(v + 0.5); + max_y = max_y < (int)a_pt->nPointIdx ? (int)a_pt->nPointIdx : max_y; + min_y = min_y > (int)a_pt->nPointIdx ? (int)a_pt->nPointIdx : min_y; + } + } + } + if (min_y == 100000000) + return; + + int pt_counter = max_y - min_y + 1; + for (int line = 0; line < lineNum; line++) + { + std::vector< SPointXYZRGB> gridData; + gridData.resize(pt_counter); + for (int i = 0; i < pt_counter; i++) + gridData[i] = { 0, 0.0, 0.0, 0.0, 0.0f, 0.0f, 0.0f }; + + std::vector< SPointXYZRGB>& a_line = scanData[line]; + int nPointCnt = a_line.size(); + for (int i = 0; i < nPointCnt; i++) + { + SPointXYZRGB a_pt = a_line[i]; + if (a_pt.z > 1e-4) + { + int pt_id = a_pt.nPointIdx - min_y; + gridData[pt_id] = a_pt; + } + } + scanData_grid.push_back(gridData); + } + return; +} + +void _outputCalibPara(char* fileName, SSG_planeCalibPara calibPara) +{ + std::ofstream sw(fileName); + char dataStr[250]; + //调平矩阵 + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.planeCalib[0], calibPara.planeCalib[1], calibPara.planeCalib[2]); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.planeCalib[3], calibPara.planeCalib[4], calibPara.planeCalib[5]); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.planeCalib[6], calibPara.planeCalib[7], calibPara.planeCalib[8]); + sw << dataStr << std::endl; + //地面高度 + sprintf_s(dataStr, 250, "%g", calibPara.planeHeight); + sw << dataStr << std::endl; + //反向旋转矩阵 + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.invRMatrix[0], calibPara.invRMatrix[1], calibPara.invRMatrix[2]); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.invRMatrix[3], calibPara.invRMatrix[4], calibPara.invRMatrix[5]); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.invRMatrix[6], calibPara.invRMatrix[7], calibPara.invRMatrix[8]); + sw << dataStr << std::endl; + + sw.close(); +} + +SSG_planeCalibPara _readCalibPara(char* fileName) +{ + //设置初始结果 + double initCalib[9] = { + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0 }; + SSG_planeCalibPara planePara; + for (int i = 0; i < 9; i++) + planePara.planeCalib[i] = initCalib[i]; + planePara.planeHeight = -1.0; + for (int i = 0; i < 9; i++) + planePara.invRMatrix[i] = initCalib[i]; + + std::ifstream inputFile(fileName); + std::string linedata; + + if (inputFile.is_open() == false) + return planePara; + + //调平矩阵 + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[0], &planePara.planeCalib[1], &planePara.planeCalib[2]); + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[3], &planePara.planeCalib[4], &planePara.planeCalib[5]); + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[6], &planePara.planeCalib[7], &planePara.planeCalib[8]); + //地面高度 + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf", &planePara.planeHeight); + //反向旋转矩阵 + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[0], &planePara.invRMatrix[1], &planePara.invRMatrix[2]); + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[3], &planePara.invRMatrix[4], &planePara.invRMatrix[5]); + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[6], &planePara.invRMatrix[7], &planePara.invRMatrix[8]); + + inputFile.close(); + return planePara; +} + +void _outputObjResult(char* fileName, std::vector&objOps) +{ + std::ofstream sw(fileName); + char dataStr[250]; + + int objSize = (int)objOps.size(); + for (int i = 0; i < objSize; i++) + { + sw << "obj_" << i << std::endl; + sprintf_s(dataStr, 250, " %g, %g, %g, %g", objOps[i].centerPos.x, objOps[i].centerPos.y, objOps[i].centerPos.z, objOps[i].centerPos.z_yaw); + sw << dataStr << std::endl; + } + sw.close(); +} + +void _outputScanDataFile_self(char* fileName, std::vector>& gridData, + float lineV, int maxTimeStamp, int clockPerSecond) +{ + int lineNum = (int)gridData.size(); + std::ofstream sw(fileName); + sw << "LineNum:" << lineNum << std::endl; + sw << "DataType: 0" << std::endl; + sw << "ScanSpeed:" << lineV << std::endl; + sw << "PointAdjust: 1" << std::endl; + sw << "MaxTimeStamp:" << maxTimeStamp << "_" << clockPerSecond << std::endl; + + for (int line = 0; line < lineNum; line++) + { + int nPositionCnt = (int)gridData[line].size(); + sw << "Line_" << line << "_0_" << nPositionCnt << std::endl; + for (int i = 0; i < nPositionCnt; i++) + { + SVzNL3DPosition& pt3D = gridData[line][i]; + float x = (float)pt3D.pt3D.x; + float y = (float)pt3D.pt3D.y; + float z = (float)pt3D.pt3D.z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}" << std::endl; + } + } + sw.close(); +} + +void _outputScanDataFile_SPointXYZRGB_vector(char* fileName, std::vector>&scanData) +{ + std::ofstream sw(fileName); + int lineNum = scanData.size(); + sw << "LineNum:" << lineNum << std::endl; + sw << "DataType: 0" << std::endl; + sw << "ScanSpeed: 0" << std::endl; + sw << "PointAdjust: 1" << std::endl; + sw << "MaxTimeStamp: 0_0" << std::endl; + + for (int line = 0; line < lineNum; line++) + { + int nPositionCnt = scanData[line].size(); + sw << "Line_" << line << "_0_" << nPositionCnt << std::endl; + for (int i = 0; i < nPositionCnt; i++) + { + SVzNLPointXYZRGBA& pt3D = scanData[line][i]; + char str[250]; + sprintf_s(str, "{ %lf, %lf, %lf, 0, 0, 0 } - { 0, 0 } - { 0, 0 }", + pt3D.x, pt3D.y, pt3D.z); + + sw << str << std::endl; + } + } + sw.close(); +} + +typedef struct +{ + int r; + int g; + int b; +}SG_color; +void _outputScanDataFile_RGBD_obj(char* fileName, std::vector>& scanData, + float lineV, int maxTimeStamp, int clockPerSecond, WD_wheelArchInfo& wheelArcHeight) +{ + int lineNum = (int)scanData.size(); + std::ofstream sw(fileName); + int realLines = lineNum; + realLines++; + sw << "LineNum:" << realLines << std::endl; + sw << "DataType: 0" << std::endl; + sw << "ScanSpeed:" << lineV << std::endl; + sw << "PointAdjust: 1" << std::endl; + sw << "MaxTimeStamp:" << maxTimeStamp << "_" << clockPerSecond << std::endl; + + int maxLineIndex = 0; + int max_stamp = 0; + + SG_color rgb = { 0, 0, 0 }; + + SG_color objColor[8] = { + {245,222,179},//淡黄色 + {210,105, 30},//巧克力色 + {240,230,140},//黄褐色 + {135,206,235},//天蓝色 + {250,235,215},//古董白 + {189,252,201},//薄荷色 + {221,160,221},//梅红色 + {188,143,143},//玫瑰红色 + }; + int size = 1; + int nTimeStamp = 0; + double alpha = 0.8; + for (int line = 0; line < lineNum; line++) + { + int nPositionCnt = (int)scanData[line].size(); + sw << "Line_" << line << "_0_" << nPositionCnt << std::endl; + for (int i = 0; i < nPositionCnt; i++) + { + SVzNL3DPosition& pt3D = scanData[line][i]; + int type = pt3D.nPointIdx; + if (1 == type) + { + rgb = { 0,255,0 }; + rgb.r = (int)((double)rgb.r * alpha); + rgb.g = (int)((double)rgb.g * alpha); + rgb.b = (int)((double)rgb.b * alpha); + size = 2; + } + else if (2 == type) + { + rgb = { 0,0,255 }; + rgb.r = (int)((double)rgb.r * alpha); + rgb.g = (int)((double)rgb.g * alpha); + rgb.b = (int)((double)rgb.b * alpha); + size = 2; + } + else if (3 == type) //轮眉 + { + rgb = { 255, 0, 0 }; + size = 3; + } + else if (4 == type) // + { + rgb = { 255, 255, 0 }; + size = 3; + } + else if (5 == type) // + { + rgb = { 255, 255, 0 }; + size = 3; + } + else + { + rgb = { 100, 100, 100 }; + size = 1; + } + + float x = (float)pt3D.pt3D.x; + float y = (float)pt3D.pt3D.y; + float z = (float)pt3D.pt3D.z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + } + } + //if (objOps.size() > 0) + { + int ptNum = 3; + sw << "Line_" << lineNum << "_" << (nTimeStamp + 1000) << "_" << ptNum << std::endl; + + rgb = { 255, 0, 0 }; + + size = 10; + float x = (float)wheelArcHeight.wheelArchPos.x; + float y = (float)wheelArcHeight.wheelArchPos.y; + float z = (float)wheelArcHeight.wheelArchPos.z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + + x = (float)wheelArcHeight.wheelUpPos.x; + y = (float)wheelArcHeight.wheelUpPos.y; + z = (float)wheelArcHeight.wheelUpPos.z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + + x = (float)wheelArcHeight.wheelDownPos.x; + y = (float)wheelArcHeight.wheelDownPos.y; + z = (float)wheelArcHeight.wheelDownPos.z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + + x = (float)wheelArcHeight.wheelArchPos.x; + y = (float)wheelArcHeight.wheelArchPos.y; + z = (float)wheelArcHeight.wheelArchPos.z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + } + //画出方向线 + rgb = { 255, 0, 0 }; + size = 3; + int lineIdx = 0; + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << (float)wheelArcHeight.arcLine[0].x << "," << (float)wheelArcHeight.arcLine[0].y << "," << (float)wheelArcHeight.arcLine[0].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << (float)wheelArcHeight.arcLine[1].x << "," << (float)wheelArcHeight.arcLine[1].y << "," << (float)wheelArcHeight.arcLine[1].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + + lineIdx++; + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << (float)wheelArcHeight.upLine[0].x << "," << (float)wheelArcHeight.upLine[0].y << "," << (float)wheelArcHeight.upLine[0].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << (float)wheelArcHeight.upLine[1].x << "," << (float)wheelArcHeight.upLine[1].y << "," << (float)wheelArcHeight.upLine[1].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + + lineIdx++; + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << (float)wheelArcHeight.downLine[0].x << "," << (float)wheelArcHeight.downLine[0].y << "," << (float)wheelArcHeight.downLine[0].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << (float)wheelArcHeight.downLine[1].x << "," << (float)wheelArcHeight.downLine[1].y << "," << (float)wheelArcHeight.downLine[1].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + + lineIdx++; + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << (float)wheelArcHeight.centerLine[0].x << "," << (float)wheelArcHeight.centerLine[0].y << "," << (float)wheelArcHeight.centerLine[0].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << (float)wheelArcHeight.centerLine[1].x << "," << (float)wheelArcHeight.centerLine[1].y << "," << (float)wheelArcHeight.centerLine[1].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + + lineIdx++; + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << (float)wheelArcHeight.arcLine[0].x << "," << (float)wheelArcHeight.arcLine[0].y << "," << (float)wheelArcHeight.arcLine[0].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << (float)wheelArcHeight.arcLine[1].x << "," << (float)wheelArcHeight.arcLine[1].y << "," << (float)wheelArcHeight.arcLine[1].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + + sw.close(); +} + +void _outputScanDataFile_obj_vector(char* fileName, std::vector>scanData, + std::vector&objOps) +{ + int lineNum = scanData.size(); + std::ofstream sw(fileName); + int realLines = lineNum; + if (objOps.size() > 0) + realLines++; + sw << "LineNum:" << realLines << std::endl; + sw << "DataType: 0" << std::endl; + sw << "ScanSpeed: 0" << std::endl; + sw << "PointAdjust: 1" << std::endl; + sw << "MaxTimeStamp: 0_0" << std::endl; + + int maxLineIndex = 0; + int max_stamp = 0; + + SG_color rgb = { 0, 0, 0 }; + + SG_color objColor[8] = { + {245,222,179},//淡黄色 + {210,105, 30},//巧克力色 + {240,230,140},//黄褐色 + {135,206,235},//天蓝色 + {250,235,215},//古董白 + {189,252,201},//薄荷色 + {221,160,221},//梅红色 + {188,143,143},//玫瑰红色 + }; + int size = 1; + int nTimeStamp = 0; + for (int line = 0; line < lineNum; line++) + { + int nPositionCnt = scanData[line].size(); + sw << "Line_" << line << "_0_" << nPositionCnt << std::endl; + for (int i = 0; i < nPositionCnt; i++) + { + SVzNL3DPosition* pt3D = &scanData[line][i]; + int vType = pt3D->nPointIdx & 0xff; + int hType = vType >> 4; + int objId = (pt3D->nPointIdx >> 16) & 0xffff; + vType = vType & 0x0f; + if (LINE_FEATURE_L_JUMP_H2L == vType) + { + rgb = { 255, 97, 0 }; + size = 3; + } + else if (LINE_FEATURE_L_JUMP_L2H == vType) + { + rgb = { 255, 255, 0 }; + size = 3; + } + else if (LINE_FEATURE_V_SLOPE == vType) + { + rgb = { 255, 0, 255 }; + size = 3; + } + else if (LINE_FEATURE_L_SLOPE_H2L == vType) + { + rgb = { 160, 82, 45 }; + size = 3; + } + else if ((LINE_FEATURE_LINE_ENDING_0 == vType) || (LINE_FEATURE_LINE_ENDING_1 == vType)) + { + rgb = { 255, 0, 0 }; + size = 3; + } + else if (LINE_FEATURE_L_SLOPE_L2H == vType) + { + rgb = { 233, 150, 122 }; + size = 3; + } + else if (LINE_FEATURE_L_JUMP_H2L == hType) + { + rgb = { 0, 0, 255 }; + size = 3; + } + else if (LINE_FEATURE_L_JUMP_L2H == hType) + { + rgb = { 0, 255, 255 }; + size = 3; + } + else if (LINE_FEATURE_V_SLOPE == hType) + { + rgb = { 0, 255, 0 }; + size = 3; + } + else if (LINE_FEATURE_L_SLOPE_H2L == hType) + { + rgb = { 85, 107, 47 }; + size = 3; + } + else if (LINE_FEATURE_L_SLOPE_L2H == hType) + { + rgb = { 0, 255, 154 }; + size = 3; + } + else if ((LINE_FEATURE_LINE_ENDING_0 == hType) || (LINE_FEATURE_LINE_ENDING_1 == hType)) + { + rgb = { 255, 0, 0 }; + size = 3; + } + else if (objId > 0) //目标 + { + rgb = objColor[objId % 8]; + size = 5; + } + else + { + rgb = { 200, 200, 200 }; + size = 1; + } + + float x = (float)pt3D->pt3D.x; + float y = (float)pt3D->pt3D.y; + float z = (float)pt3D->pt3D.z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + } + } + if (objOps.size() > 0) + { + int ptNum = objOps.size(); + sw << "Line_" << lineNum << "_0_" << ptNum << std::endl; + for (int i = 0; i < objOps.size(); i++) + { + if (i == 0) + rgb = { 255, 0, 0 }; + else + rgb = { 255, 255, 0 }; + size = 25; + float x = (float)objOps[i].centerPos.x; + float y = (float)objOps[i].centerPos.y; + float z = (float)objOps[i].centerPos.z; + + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + if (i == 0) + { + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + } + } + } + //画出方向线 + sw.close(); +} + +void _outputRGBDScanDataFile_RGBD_obj(char* fileName, SVzNLXYZRGBDLaserLine * scanData, int lineNum, + float lineV, int maxTimeStamp, int clockPerSecond, std::vector&objOps) +{ + std::ofstream sw(fileName); + int realLines = lineNum; + if (objOps.size() > 0) + realLines++; + sw << "LineNum:" << realLines << std::endl; + sw << "DataType: 0" << std::endl; + sw << "ScanSpeed:" << lineV << std::endl; + sw << "PointAdjust: 1" << std::endl; + sw << "MaxTimeStamp:" << maxTimeStamp << "_" << clockPerSecond << std::endl; + + int maxLineIndex = 0; + int max_stamp = 0; + + SG_color rgb = { 0, 0, 0 }; + + SG_color objColor[8] = { + {245,222,179},//淡黄色 + {210,105, 30},//巧克力色 + {240,230,140},//黄褐色 + {135,206,235},//天蓝色 + {250,235,215},//古董白 + {189,252,201},//薄荷色 + {221,160,221},//梅红色 + {188,143,143},//玫瑰红色 + }; + int size = 1; + int nTimeStamp = 0; + for (int line = 0; line < lineNum; line++) + { + sw << "Line_" << line << "_" << scanData[line].nTimeStamp << "_" << scanData[line].nPointCnt << std::endl; + nTimeStamp = scanData[line].nTimeStamp; + for (int i = 0; i < scanData[line].nPointCnt; i++) + { + SVzNLPointXYZRGBA* pt3D = &scanData[line].p3DPoint[i]; +#if 0 + int vType = pt3D->nRGB & 0xff; + int hType = vType >> 4; + int objId = (pt3D->nRGB >> 16) & 0xffff; + vType = vType & 0x0f; + if (LINE_FEATURE_L_JUMP_H2L == vType) + { + rgb = { 255, 97, 0 }; + size = 3; + } + else if (LINE_FEATURE_L_JUMP_L2H == vType) + { + rgb = { 255, 255, 0 }; + size = 3; + } + else if (LINE_FEATURE_V_SLOPE == vType) + { + rgb = { 255, 0, 255 }; + size = 3; + } + else if (LINE_FEATURE_L_SLOPE_H2L == vType) + { + rgb = { 160, 82, 45 }; + size = 3; + } + else if ((LINE_FEATURE_LINE_ENDING_0 == vType) || (LINE_FEATURE_LINE_ENDING_1 == vType)) + { + rgb = { 255, 0, 0 }; + size = 3; + } + else if (LINE_FEATURE_L_SLOPE_L2H == vType) + { + rgb = { 233, 150, 122 }; + size = 3; + } + else if (LINE_FEATURE_L_JUMP_H2L == hType) + { + rgb = { 0, 0, 255 }; + size = 3; + } + else if (LINE_FEATURE_L_JUMP_L2H == hType) + { + rgb = { 0, 255, 255 }; + size = 3; + } + else if (LINE_FEATURE_V_SLOPE == hType) + { + rgb = { 0, 255, 0 }; + size = 3; + } + else if (LINE_FEATURE_L_SLOPE_H2L == hType) + { + rgb = { 85, 107, 47 }; + size = 3; + } + else if (LINE_FEATURE_L_SLOPE_L2H == hType) + { + rgb = { 0, 255, 154 }; + size = 3; + } + else if ((LINE_FEATURE_LINE_ENDING_0 == hType) || (LINE_FEATURE_LINE_ENDING_1 == hType)) + { + rgb = { 255, 0, 0 }; + size = 3; + } + else if (objId > 0) //目标 + { + rgb = objColor[objId % 8]; + size = 5; + } + else +#endif + { + rgb = { 200, 200, 200 }; + size = 1; + } + float x = (float)pt3D->x; + float y = (float)pt3D->y; + float z = (float)pt3D->z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + } + } + if (objOps.size() > 0) + { + int ptNum = objOps.size(); + sw << "Line_" << lineNum << "_" << (nTimeStamp + 1000) << "_" << ptNum << std::endl; + for (int i = 0; i < objOps.size(); i++) + { + if (i == 0) + rgb = { 255, 0, 0 }; + else + rgb = { 255, 255, 0 }; + size = 25; + float x = (float)objOps[i].centerPos.x; + float y = (float)objOps[i].centerPos.y; + float z = (float)objOps[i].centerPos.z; + + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + if (i == 0) + { + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + } + } + } + sw.close(); +} + +void _outputScanDataFile_RGBD_sideBagObj(char* fileName, SVzNL3DLaserLine * scanData, int lineNum, + float lineV, int maxTimeStamp, int clockPerSecond, std::vector&objOps) +{ + std::ofstream sw(fileName); + int realLines = lineNum; + if (objOps.size() > 0) + realLines++; + sw << "LineNum:" << realLines << std::endl; + sw << "DataType: 0" << std::endl; + sw << "ScanSpeed:" << lineV << std::endl; + sw << "PointAdjust: 1" << std::endl; + sw << "MaxTimeStamp:" << maxTimeStamp << "_" << clockPerSecond << std::endl; + + int maxLineIndex = 0; + int max_stamp = 0; + + SG_color rgb = { 0, 0, 0 }; + + SG_color objColor[8] = { + {245,222,179},//淡黄色 + {210,105, 30},//巧克力色 + {240,230,140},//黄褐色 + {135,206,235},//天蓝色 + {250,235,215},//古董白 + {189,252,201},//薄荷色 + {221,160,221},//梅红色 + {188,143,143},//玫瑰红色 + }; + int size = 1; + int nTimeStamp = 0; + for (int line = 0; line < lineNum; line++) + { + int nPntCount = 0; + for (int i = 0; i < scanData[line].nPositionCnt; i++) + { + SVzNL3DPosition* pt3D = &scanData[line].p3DPosition[i]; + if (pt3D->pt3D.z > 1e-4) + nPntCount++; + } + sw << "Line_" << line << "_" << scanData[line].nTimeStamp << "_" << nPntCount << std::endl; + nTimeStamp = scanData[line].nTimeStamp; + for (int i = 0; i < scanData[line].nPositionCnt; i++) + { + SVzNL3DPosition* pt3D = &scanData[line].p3DPosition[i]; + if (pt3D->pt3D.z < 1e-4) + continue; + + int vType = pt3D->nPointIdx & 0xff; + int hType = vType >> 4; + int objId = (pt3D->nPointIdx >> 16) & 0xffff; + vType = vType & 0x0f; + if (LINE_FEATURE_L_JUMP_H2L == vType) + { + rgb = { 255, 97, 0 }; + size = 3; + } + else if (LINE_FEATURE_L_JUMP_L2H == vType) + { + rgb = { 255, 255, 0 }; + size = 3; + } + else if (LINE_FEATURE_V_SLOPE == vType) + { + rgb = { 255, 0, 255 }; + size = 3; + } + else if (LINE_FEATURE_L_SLOPE_H2L == vType) + { + rgb = { 160, 82, 45 }; + size = 3; + } + else if ((LINE_FEATURE_LINE_ENDING_0 == vType) || (LINE_FEATURE_LINE_ENDING_1 == vType)) + { + rgb = { 255, 0, 0 }; + size = 3; + } + else if (LINE_FEATURE_L_SLOPE_L2H == vType) + { + rgb = { 233, 150, 122 }; + size = 3; + } + else if (LINE_FEATURE_L_JUMP_H2L == hType) + { + rgb = { 0, 0, 255 }; + size = 3; + } + else if (LINE_FEATURE_L_JUMP_L2H == hType) + { + rgb = { 0, 255, 255 }; + size = 3; + } + else if (LINE_FEATURE_V_SLOPE == hType) + { + rgb = { 0, 255, 0 }; + size = 3; + } + else if (LINE_FEATURE_L_SLOPE_H2L == hType) + { + rgb = { 85, 107, 47 }; + size = 3; + } + else if (LINE_FEATURE_L_SLOPE_L2H == hType) + { + rgb = { 0, 255, 154 }; + size = 3; + } + else if ((LINE_FEATURE_LINE_ENDING_0 == hType) || (LINE_FEATURE_LINE_ENDING_1 == hType)) + { + rgb = { 255, 0, 0 }; + size = 3; + } + else if (objId > 0) //目标 + { + rgb = objColor[objId % 8]; + size = 5; + } + else + { + rgb = { 200, 200, 200 }; + size = 1; + } + + float x = (float)pt3D->pt3D.x; + float y = (float)pt3D->pt3D.y; + float z = (float)pt3D->pt3D.z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + } + } + if (objOps.size() > 0) + { + int ptNum = objOps.size(); + sw << "Line_" << lineNum << "_" << (nTimeStamp + 1000) << "_" << ptNum << std::endl; + for (int i = 0; i < objOps.size(); i++) + { + if (i == 0) + rgb = { 255, 0, 0 }; + else + rgb = { 255, 255, 0 }; + size = 25; + float x = (float)objOps[i].graspPos.x; + float y = (float)objOps[i].graspPos.y; + float z = (float)objOps[i].graspPos.z; + + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + if (i == 0) + { + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + } + } + } + sw.close(); +} + +void EulerRpyToRotation1(double rpy[3], double matrix3d[9]) { + double cos0 = cos(rpy[0] * PI / 180); + double sin0 = sin(rpy[0] * PI / 180); + double cos1 = cos(rpy[1] * PI / 180); + double sin1 = sin(rpy[1] * PI / 180); + double cos2 = cos(rpy[2] * PI / 180); + double sin2 = sin(rpy[2] * PI / 180); + matrix3d[0] = cos2 * cos1; + matrix3d[1] = cos2 * sin1 * sin0 - sin2 * cos0; + matrix3d[2] = cos2 * sin1 * cos0 + sin2 * sin0; + matrix3d[3] = sin2 * cos1; + matrix3d[4] = sin2 * sin1 * sin0 + cos2 * cos0; + matrix3d[5] = sin2 * sin1 * cos0 - cos2 * sin0; + matrix3d[6] = -sin1; + matrix3d[7] = cos1 * sin0; + matrix3d[8] = cos1 * cos0; + return; +} + +void _rotateCloudPts(SVzNL3DLaserLine * scanData, int lineNum, double matrix3d[9], std::vector>&rotateLines, SVzNLRangeD * rx_range, SVzNLRangeD * ry_range) +{ + rx_range->min = 0; + rx_range->max = -1; + ry_range->min = 0; + ry_range->max = -1; + for (int line = 0; line < lineNum; line++) + { + std::vector< SVzNL3DPosition> linePts; + for (int i = 0; i < scanData[line].nPositionCnt; i++) + { + SVzNL3DPosition* pt3D = &scanData[line].p3DPosition[i]; + if (pt3D->pt3D.z < 1e-4) + continue; + + SVzNL3DPosition r_pt; + r_pt.pt3D = _ptRotate(pt3D->pt3D, matrix3d); + r_pt.nPointIdx = pt3D->nPointIdx; + if (rx_range->max < rx_range->min) + { + rx_range->min = r_pt.pt3D.x; + rx_range->max = r_pt.pt3D.x; + } + else + { + if (rx_range->min > r_pt.pt3D.x) + rx_range->min = r_pt.pt3D.x; + if (rx_range->max < r_pt.pt3D.x) + rx_range->max = r_pt.pt3D.x; + } + if (ry_range->max < ry_range->min) + { + ry_range->min = r_pt.pt3D.y; + ry_range->max = r_pt.pt3D.y; + } + else + { + if (ry_range->min > r_pt.pt3D.y) + ry_range->min = r_pt.pt3D.y; + if (ry_range->max < r_pt.pt3D.y) + ry_range->max = r_pt.pt3D.y; + } + linePts.push_back(r_pt); + + } + rotateLines.push_back(linePts); + } +} + +void _rotateCloudPts_RGBD(SVzNLXYZRGBDLaserLine * scanData, int lineNum, double matrix3d[9], std::vector>&rotateLines, SVzNLRangeD * rx_range, SVzNLRangeD * ry_range) +{ + rx_range->min = 0; + rx_range->max = -1; + ry_range->min = 0; + ry_range->max = -1; + for (int line = 0; line < lineNum; line++) + { + std::vector< SVzNLPointXYZRGBA> linePts; + for (int i = 0; i < scanData[line].nPointCnt; i++) + { + SVzNLPointXYZRGBA* pt3D = &scanData[line].p3DPoint[i]; + if (pt3D->z < 1e-4) + continue; + + SVzNLPointXYZRGBA r_pt; + r_pt = _ptRotate_RGBD(*pt3D, matrix3d); + if (rx_range->max < rx_range->min) + { + rx_range->min = r_pt.x; + rx_range->max = r_pt.x; + } + else + { + if (rx_range->min > r_pt.x) + rx_range->min = r_pt.x; + if (rx_range->max < r_pt.x) + rx_range->max = r_pt.x; + } + if (ry_range->max < ry_range->min) + { + ry_range->min = r_pt.y; + ry_range->max = r_pt.y; + } + else + { + if (ry_range->min > r_pt.y) + ry_range->min = r_pt.y; + if (ry_range->max < r_pt.y) + ry_range->max = r_pt.y; + } + linePts.push_back(r_pt); + } + rotateLines.push_back(linePts); + } +} + +void _XOYprojection( + cv::Mat & img, + std::vector>&dataLines, + std::vector&objOps, + const double x_scale, + const double y_scale, + const SVzNLRangeD x_range, + const SVzNLRangeD y_range, + bool drawDirAngle, + int dirAngleLen) +{ + int x_skip = 16; + int y_skip = 16; + + cv::Vec3b rgb = cv::Vec3b(0, 0, 0); + cv::Vec3b objColor[8] = { + {245,222,179},//淡黄色 + {210,105, 30},//巧克力色 + {240,230,140},//黄褐色 + {135,206,235},//天蓝色 + {250,235,215},//古董白 + {189,252,201},//薄荷色 + {221,160,221},//梅红色 + {188,143,143},//玫瑰红色 + }; + int size = 1; + for (int line = 0; line < dataLines.size(); line++) + { + std::vector< SVzNL3DPosition>& a_line = dataLines[line]; + for (int i = 0; i < a_line.size(); i++) + { + SVzNL3DPosition* pt3D = &a_line[i]; + if (pt3D->pt3D.z < 1e-4) + continue; + + int vType = pt3D->nPointIdx & 0xff; + int hType = vType >> 4; + int objId = (pt3D->nPointIdx >> 16) & 0xff; + vType = vType & 0x0f; + if (LINE_FEATURE_L_JUMP_H2L == vType) + { + rgb = { 255, 97, 0 }; + size = 2; + } + else if (LINE_FEATURE_L_JUMP_L2H == vType) + { + rgb = { 255, 255, 0 }; + size = 2; + } + else if (LINE_FEATURE_V_SLOPE == vType) + { + rgb = { 255, 0, 255 }; + size = 2; + } + else if (LINE_FEATURE_L_SLOPE_H2L == vType) + { + rgb = { 160, 82, 45 }; + size = 2; + } + else if ((LINE_FEATURE_LINE_ENDING_0 == vType) || (LINE_FEATURE_LINE_ENDING_1 == vType)) + { + rgb = { 255, 0, 0 }; + size = 2; + } + else if (LINE_FEATURE_L_SLOPE_L2H == vType) + { + rgb = { 233, 150, 122 }; + size = 2; + } + else if (LINE_FEATURE_L_JUMP_H2L == hType) + { + rgb = { 0, 0, 255 }; + size = 2; + } + else if (LINE_FEATURE_L_JUMP_L2H == hType) + { + rgb = { 0, 255, 255 }; + size = 2; + } + else if (LINE_FEATURE_V_SLOPE == hType) + { + rgb = { 0, 255, 0 }; + size = 2; + } + else if (LINE_FEATURE_L_SLOPE_H2L == hType) + { + rgb = { 85, 107, 47 }; + size = 2; + } + else if (LINE_FEATURE_L_SLOPE_L2H == hType) + { + rgb = { 0, 255, 154 }; + size = 2; + } + else if ((LINE_FEATURE_LINE_ENDING_0 == hType) || (LINE_FEATURE_LINE_ENDING_1 == hType)) + { + rgb = { 255, 0, 0 }; + size = 2; + } + else if (objId > 0) //目标 + { + rgb = objColor[objId % 8]; + size = 1; + } + else + { + rgb = { 150, 150, 150 }; + size = 1; + } + + + double x = pt3D->pt3D.x; + double y = pt3D->pt3D.y; + + int px = (int)((x - x_range.min) / x_scale + x_skip); + int py = (int)((y - y_range.min) / y_scale + y_skip); + if (size == 1) + img.at(py, px) = cv::Vec3b(rgb[2], rgb[1], rgb[0]); + else + cv::circle(img, cv::Point(px, py), size, cv::Scalar(rgb[2], rgb[1], rgb[0]), -1); + } + } + if (objOps.size() > 0) + { + for (int i = 0; i < objOps.size(); i++) + { + if (i == 0) + { + rgb = { 255, 0, 0 }; + size = 20; + } + else + { + rgb = { 255, 255, 0 }; + size = 10; + } + int px = (int)((objOps[i].centerPos.x - x_range.min) / x_scale + x_skip); + int py = (int)((objOps[i].centerPos.y - y_range.min) / y_scale + y_skip); + cv::circle(img, cv::Point(px, py), size, cv::Scalar(rgb[2], rgb[1], rgb[0]), -1); + if (true == drawDirAngle) + { + //画线 + double R = (double)dirAngleLen / 2.0; + const double deg2rad = PI / 180.0; + const double yaw = objOps[i].centerPos.z_yaw * deg2rad; + double cy = cos(yaw); double sy = sin(yaw); + double x1 = objOps[i].centerPos.x + R * cy; double y1 = objOps[i].centerPos.y - R * sy; + double x2 = objOps[i].centerPos.x - R * cy; double y2 = objOps[i].centerPos.y + R * sy; + int px1 = (int)((x1 - x_range.min) / x_scale + x_skip); + int py1 = (int)((y1 - y_range.min) / y_scale + y_skip); + int px2 = (int)((x2 - x_range.min) / x_scale + x_skip); + int py2 = (int)((y2 - y_range.min) / y_scale + y_skip); + cv::line(img, cv::Point(px1, py1), cv::Point(px2, py2), cv::Scalar(rgb[2], rgb[1], rgb[0]), 2); + } + } + } + +} + +void _XOYprojection_RGBD( + cv::Mat & img, + std::vector>&dataLines, + std::vector&objOps, + const double x_scale, + const double y_scale, + const SVzNLRangeD x_range, + const SVzNLRangeD y_range, + bool drawDirAngle, + int dirAngleLen) +{ + int x_skip = 16; + int y_skip = 16; + + cv::Vec3b rgb = cv::Vec3b(0, 0, 0); + cv::Vec3b objColor[8] = { + {245,222,179},//淡黄色 + {210,105, 30},//巧克力色 + {240,230,140},//黄褐色 + {135,206,235},//天蓝色 + {250,235,215},//古董白 + {189,252,201},//薄荷色 + {221,160,221},//梅红色 + {188,143,143},//玫瑰红色 + }; + int size = 1; + for (int line = 0; line < dataLines.size(); line++) + { + std::vector< SVzNLPointXYZRGBA>& a_line = dataLines[line]; + for (int i = 0; i < a_line.size(); i++) + { + SVzNLPointXYZRGBA* pt3D = &a_line[i]; + if (pt3D->z < 1e-4) + continue; + + int nRGB = pt3D->nRGB; + int r = nRGB & 0xff; + nRGB >>= 8; + int g = nRGB & 0xff; + nRGB >>= 8; + int b = nRGB & 0xff; + + rgb[0] = r; + rgb[1] = g; + rgb[2] = b; + size = 1; + double x = pt3D->x; + double y = pt3D->y; + int px = (int)((x - x_range.min) / x_scale + x_skip); + int py = (int)((y - y_range.min) / y_scale + y_skip); + if ((px == 666) && (py == 828)) + int kkk = 1; + if (size == 1) + img.at(py, px) = cv::Vec3b(rgb[2], rgb[1], rgb[0]); + else + cv::circle(img, cv::Point(px, py), size, cv::Scalar(rgb[2], rgb[1], rgb[0]), -1); + } + } + if (objOps.size() > 0) + { + for (int i = 0; i < objOps.size(); i++) + { + if (i == 0) + { + rgb = { 255, 0, 0 }; + size = 20; + } + else + { + rgb = { 255, 255, 0 }; + size = 10; + } + int px = (int)((objOps[i].centerPos.x - x_range.min) / x_scale + x_skip); + int py = (int)((objOps[i].centerPos.y - y_range.min) / y_scale + y_skip); + cv::circle(img, cv::Point(px, py), size, cv::Scalar(rgb[2], rgb[1], rgb[0]), -1); + if (true == drawDirAngle) + { + //画线 + double R = (double)dirAngleLen / 2.0; + const double deg2rad = PI / 180.0; + const double yaw = objOps[i].centerPos.z_yaw * deg2rad; + double cy = cos(yaw); double sy = sin(yaw); + double arrowLen = R / 3; + double arrowAngle = 30; + double ca = cos(arrowAngle * deg2rad); + double sa = sin(arrowAngle * deg2rad); + SVzNL2DPointD endingPt[4]; + endingPt[0] = { R, 0 }; + endingPt[1] = { -R, 0 }; + endingPt[2] = { R - arrowLen * ca, -arrowLen * sa }; + endingPt[3] = { R - arrowLen * ca, arrowLen * sa }; + for (int m = 0; m < 4; m++) + { + double tmp_x = endingPt[m].x * cy - endingPt[m].y * sy; + double tmp_y = -endingPt[m].x * sy - endingPt[m].y * cy; + endingPt[m].x = tmp_x + objOps[i].centerPos.x; + endingPt[m].y = tmp_y + objOps[i].centerPos.y; + } + int px1 = (int)((endingPt[0].x - x_range.min) / x_scale + x_skip); + int py1 = (int)((endingPt[0].y - y_range.min) / y_scale + y_skip); + int px2 = (int)((endingPt[1].x - x_range.min) / x_scale + x_skip); + int py2 = (int)((endingPt[1].y - y_range.min) / y_scale + y_skip); + cv::line(img, cv::Point(px1, py1), cv::Point(px2, py2), cv::Scalar(rgb[2], rgb[1], rgb[0]), 2); + int px3 = (int)((endingPt[2].x - x_range.min) / x_scale + x_skip); + int py3 = (int)((endingPt[2].y - y_range.min) / y_scale + y_skip); + int px4 = (int)((endingPt[3].x - x_range.min) / x_scale + x_skip); + int py4 = (int)((endingPt[3].y - y_range.min) / y_scale + y_skip); + if (objOps[i].orienFlag == 1) + { + cv::line(img, cv::Point(px1, py1), cv::Point(px3, py3), cv::Scalar(0, 255, 0), 2); + cv::line(img, cv::Point(px1, py1), cv::Point(px4, py4), cv::Scalar(0, 255, 0), 2); + //cv::circle(img, cv::Point(px1, py1), 5, cv::Scalar(0, 255, 0), -1); + } + else if (objOps[i].orienFlag == 2) + { + cv::line(img, cv::Point(px1, py1), cv::Point(px3, py3), cv::Scalar(0, 0, 255), 2); + cv::line(img, cv::Point(px1, py1), cv::Point(px4, py4), cv::Scalar(0, 0, 255), 2); + //cv::circle(img, cv::Point(px1, py1), 5, cv::Scalar(0, 0, 255), -1); + } + } + } + } + +} + +void _XOYprojection_sideBagInfo(cv::Mat & img, std::vector>&dataLines, std::vector&objOps, + const double x_scale, const double y_scale, const SVzNLRangeD x_range, const SVzNLRangeD y_range) +{ + int x_skip = 16; + int y_skip = 16; + + cv::Vec3b rgb = cv::Vec3b(0, 0, 0); + cv::Vec3b objColor[8] = { + {245,222,179},//淡黄色 + {210,105, 30},//巧克力色 + {240,230,140},//黄褐色 + {135,206,235},//天蓝色 + {250,235,215},//古董白 + {189,252,201},//薄荷色 + {221,160,221},//梅红色 + {188,143,143},//玫瑰红色 + }; + int size = 1; + for (int line = 0; line < dataLines.size(); line++) + { + std::vector< SVzNL3DPosition>& a_line = dataLines[line]; + for (int i = 0; i < a_line.size(); i++) + { + SVzNL3DPosition* pt3D = &a_line[i]; + if (pt3D->pt3D.z < 1e-4) + continue; + + int vType = pt3D->nPointIdx & 0xff; + int hType = vType >> 4; + int objId = (pt3D->nPointIdx >> 16) & 0xffff; + vType = vType & 0x0f; + if (LINE_FEATURE_L_JUMP_H2L == vType) + { + rgb = { 255, 97, 0 }; + size = 2; + } + else if (LINE_FEATURE_L_JUMP_L2H == vType) + { + rgb = { 255, 255, 0 }; + size = 2; + } + else if (LINE_FEATURE_V_SLOPE == vType) + { + rgb = { 255, 0, 255 }; + size = 2; + } + else if (LINE_FEATURE_L_SLOPE_H2L == vType) + { + rgb = { 160, 82, 45 }; + size = 2; + } + else if ((LINE_FEATURE_LINE_ENDING_0 == vType) || (LINE_FEATURE_LINE_ENDING_1 == vType)) + { + rgb = { 255, 0, 0 }; + size = 2; + } + else if (LINE_FEATURE_L_SLOPE_L2H == vType) + { + rgb = { 233, 150, 122 }; + size = 2; + } + else if (LINE_FEATURE_L_JUMP_H2L == hType) + { + rgb = { 0, 0, 255 }; + size = 2; + } + else if (LINE_FEATURE_L_JUMP_L2H == hType) + { + rgb = { 0, 255, 255 }; + size = 2; + } + else if (LINE_FEATURE_V_SLOPE == hType) + { + rgb = { 0, 255, 0 }; + size = 2; + } + else if (LINE_FEATURE_L_SLOPE_H2L == hType) + { + rgb = { 85, 107, 47 }; + size = 2; + } + else if (LINE_FEATURE_L_SLOPE_L2H == hType) + { + rgb = { 0, 255, 154 }; + size = 2; + } + else if ((LINE_FEATURE_LINE_ENDING_0 == hType) || (LINE_FEATURE_LINE_ENDING_1 == hType)) + { + rgb = { 255, 0, 0 }; + size = 2; + } + else if ((objId > 0) && (objId < 1000)) //目标 + { + rgb = objColor[objId % 8]; + size = 3; + } + else + { + rgb = { 150, 150, 150 }; + size = 1; + } + + + double x = pt3D->pt3D.x; + double y = pt3D->pt3D.y; + + int px = (int)((x - x_range.min) / x_scale + x_skip); + int py = (int)((y - y_range.min) / y_scale + y_skip); + if (size == 1) + img.at(py, px) = cv::Vec3b(rgb[2], rgb[1], rgb[0]); + else + cv::circle(img, cv::Point(px, py), size, cv::Scalar(rgb[2], rgb[1], rgb[0]), -1); + } + } + if (objOps.size() > 0) + { + for (int i = 0; i < objOps.size(); i++) + { + if (i == 0) + { + rgb = { 255, 0, 0 }; + size = 20; + } + else + { + rgb = { 255, 255, 0 }; + size = 10; + } + int px = (int)((objOps[i].graspPos.x - x_range.min) / x_scale + x_skip); + int py = (int)((objOps[i].graspPos.y - y_range.min) / y_scale + y_skip); + cv::circle(img, cv::Point(px, py), size, cv::Scalar(rgb[2], rgb[1], rgb[0]), -1); + + //画ROI + size = 3; + cv::Point2d vec2d[4]; + vec2d[0].x = objOps[i].objROI.left; vec2d[0].y = objOps[i].objROI.top; + vec2d[1].x = objOps[i].objROI.right; vec2d[1].y = objOps[i].objROI.top; + vec2d[2].x = objOps[i].objROI.right; vec2d[2].y = objOps[i].objROI.bottom; + vec2d[3].x = objOps[i].objROI.left; vec2d[3].y = objOps[i].objROI.bottom; + cv::Point vec[4]; + for (int j = 0; j < 4; j++) + { + vec[j].x = (int)((vec2d[j].x - x_range.min) / x_scale + x_skip); + vec[j].y = (int)((vec2d[j].y - y_range.min) / y_scale + y_skip); + } + for (int j = 0; j < 4; j++) + { + int nxtIdx = (j + 1) % 4; + cv::line(img, vec[j], vec[nxtIdx], cv::Scalar(rgb[2], rgb[1], rgb[0]), size); + } + + //画倾角 + double r = 50; + double angle = objOps[i].graspPos.z_yaw; + angle = -angle * PI / 180; + cv::Point2d line_pt[2]; + line_pt[0].x = (int)(r * cos(angle) + px); + line_pt[0].y = (int)(-r * sin(angle) + py); + line_pt[1].x = (int)(-r * cos(angle) + px); + line_pt[1].y = (int)(r * sin(angle) + py); + cv::line(img, line_pt[0], line_pt[1], cv::Scalar(rgb[2], rgb[1], rgb[0]), size); + } + + + } + +} + +void _genXOYProjectionImage(cv::String & fileName, SVzNL3DLaserLine * scanData, int lineNum, std::vector&objOps, double rpy[3], double dirLen) +{ + //统计X和Y的范围 + std::vector> scan_lines; + SVzNLRangeD x_range = { 0, -1 }; + SVzNLRangeD y_range = { 0, -1 }; + for (int line = 0; line < lineNum; line++) + { + std::vector< SVzNL3DPosition> a_line; + for (int i = 0; i < scanData[line].nPositionCnt; i++) + { + SVzNL3DPosition* pt3D = &scanData[line].p3DPosition[i]; + if (pt3D->pt3D.z < 1e-4) + continue; + + a_line.push_back(*pt3D); + if (x_range.max < x_range.min) + { + x_range.min = pt3D->pt3D.x; + x_range.max = pt3D->pt3D.x; + } + else + { + if (x_range.min > pt3D->pt3D.x) + x_range.min = pt3D->pt3D.x; + if (x_range.max < pt3D->pt3D.x) + x_range.max = pt3D->pt3D.x; + } + if (y_range.max < y_range.min) + { + y_range.min = pt3D->pt3D.y; + y_range.max = pt3D->pt3D.y; + } + else + { + if (y_range.min > pt3D->pt3D.y) + y_range.min = pt3D->pt3D.y; + if (y_range.max < pt3D->pt3D.y) + y_range.max = pt3D->pt3D.y; + } + } + scan_lines.push_back(a_line); + } + + int imgRows = 992; + int imgCols = 1056; + double y_rows = 960.0; + double x_cols = 1024.0; + cv::Mat img = cv::Mat::zeros(imgRows, imgCols, CV_8UC3); + //计算投影比例 + double x_scale = (x_range.max - x_range.min) / x_cols; + double y_scale = (y_range.max - y_range.min) / y_rows; + if (x_scale < y_scale) + x_scale = y_scale; + else + y_scale = x_scale; + + int angleDrawLen = dirLen;// / x_scale; + _XOYprojection(img, scan_lines, objOps, x_scale, y_scale, x_range, y_range, true, angleDrawLen); + + //旋转视角显示 + double matrix3d[9]; + EulerRpyToRotation1(rpy, matrix3d); + std::vector r_objOps; + r_objOps.insert(r_objOps.end(), objOps.begin(), objOps.end()); + for (int i = 0; i < objOps.size(); i++) + { + SVzNL3DPoint c_pt = { objOps[i].centerPos.x, objOps[i].centerPos.y, objOps[i].centerPos.z }; + SVzNL3DPoint r_c_pt = _ptRotate(c_pt, matrix3d); + r_objOps[i].centerPos.x = r_c_pt.x; + r_objOps[i].centerPos.y = r_c_pt.y; + r_objOps[i].centerPos.z = r_c_pt.z; + } + std::vector> rotateLines; + SVzNLRangeD rx_range, ry_range; + _rotateCloudPts(scanData, lineNum, matrix3d, rotateLines, &rx_range, &ry_range); + cv::Mat r_img = cv::Mat::zeros(imgRows, imgCols, CV_8UC3); + //计算投影比例 + double rx_scale = (rx_range.max - rx_range.min) / x_cols; + double ry_scale = (ry_range.max - ry_range.min) / y_rows; + _XOYprojection(r_img, rotateLines, r_objOps, rx_scale, ry_scale, rx_range, ry_range, false, angleDrawLen); + + cv::Mat dis_img; + cv::hconcat(img, r_img, dis_img); + cv::imwrite(fileName, dis_img); + return; +} + +void _RGBDto2DImage(char* fileName, SVzNLXYZRGBDLaserLine * scanData, int lineNum) +{ + cv::String imgName(fileName); + //统计X和Y的范围 + SVzNLRangeD x_range = { 0, -1 }; + SVzNLRangeD y_range = { 0, -1 }; + for (int line = 0; line < lineNum; line++) + { + for (int i = 0; i < scanData[line].nPointCnt; i++) + { + SVzNLPointXYZRGBA* pt3D = &scanData[line].p3DPoint[i]; + if (pt3D->z < 1e-4) + continue; + + if (x_range.max < x_range.min) + { + x_range.min = pt3D->x; + x_range.max = pt3D->x; + } + else + { + if (x_range.min > pt3D->x) + x_range.min = pt3D->x; + if (x_range.max < pt3D->x) + x_range.max = pt3D->x; + } + if (y_range.max < y_range.min) + { + y_range.min = pt3D->y; + y_range.max = pt3D->y; + } + else + { + if (y_range.min > pt3D->y) + y_range.min = pt3D->y; + if (y_range.max < pt3D->y) + y_range.max = pt3D->y; + } + } + } + + int x_skip = 16; + int y_skip = 16; + int imgRows = 992; + int imgCols = 1056; + double y_rows = (double)(imgRows - y_skip * 2); + double x_cols = (double)(imgCols - x_skip * 2); + cv::Mat img = cv::Mat::zeros(imgRows, imgCols, CV_8UC3); + //计算投影比例 + double x_scale = (x_range.max - x_range.min) / x_cols; + double y_scale = (y_range.max - y_range.min) / y_rows; + if (x_scale < y_scale) + x_scale = y_scale; + else + y_scale = x_scale; + + int size = 1; + cv::Vec3b rgb = cv::Vec3b(0, 0, 0); + for (int line = 0; line < lineNum; line++) + { + for (int i = 0; i < scanData[line].nPointCnt; i++) + { + SVzNLPointXYZRGBA* pt3D = &scanData[line].p3DPoint[i]; + if (pt3D->z < 1e-4) + continue; + + int nRGB = pt3D->nRGB; + int r = nRGB & 0xff; + nRGB >>= 8; + int g = nRGB & 0xff; + nRGB >>= 8; + int b = nRGB & 0xff; + + rgb[0] = r; + rgb[1] = g; + rgb[2] = b; + size = 1; + double x = pt3D->x; + double y = pt3D->y; + int px = (int)((x - x_range.min) / x_scale + x_skip); + int py = (int)((y - y_range.min) / y_scale + y_skip); + if ((px == 666) && (py == 828)) + int kkk = 1; + if (size == 1) + img.at(py, px) = cv::Vec3b(rgb[2], rgb[1], rgb[0]); + else + cv::circle(img, cv::Point(px, py), size, cv::Scalar(rgb[2], rgb[1], rgb[0]), -1); + } + } + cv::imwrite(imgName, img); + return; +} + +void _genXOYProjectionImage_RGBD(cv::String & fileName, SVzNLXYZRGBDLaserLine * scanData, int lineNum, std::vector&objOps, double rpy[3], double dirLen) +{ + //统计X和Y的范围 + std::vector> scan_lines; + SVzNLRangeD x_range = { 0, -1 }; + SVzNLRangeD y_range = { 0, -1 }; + for (int line = 0; line < lineNum; line++) + { + std::vector< SVzNLPointXYZRGBA> a_line; + for (int i = 0; i < scanData[line].nPointCnt; i++) + { + SVzNLPointXYZRGBA* pt3D = &scanData[line].p3DPoint[i]; + if (pt3D->z < 1e-4) + continue; + + a_line.push_back(*pt3D); + if (x_range.max < x_range.min) + { + x_range.min = pt3D->x; + x_range.max = pt3D->x; + } + else + { + if (x_range.min > pt3D->x) + x_range.min = pt3D->x; + if (x_range.max < pt3D->x) + x_range.max = pt3D->x; + } + if (y_range.max < y_range.min) + { + y_range.min = pt3D->y; + y_range.max = pt3D->y; + } + else + { + if (y_range.min > pt3D->y) + y_range.min = pt3D->y; + if (y_range.max < pt3D->y) + y_range.max = pt3D->y; + } + } + scan_lines.push_back(a_line); + } + + int imgRows = 992; + int imgCols = 1056; + double y_rows = 960.0; + double x_cols = 1024.0; + cv::Mat img = cv::Mat::zeros(imgRows, imgCols, CV_8UC3); + //计算投影比例 + double x_scale = (x_range.max - x_range.min) / x_cols; + double y_scale = (y_range.max - y_range.min) / y_rows; + if (x_scale < y_scale) + x_scale = y_scale; + else + y_scale = x_scale; + + int angleDrawLen = dirLen / x_scale; + _XOYprojection_RGBD(img, scan_lines, objOps, x_scale, y_scale, x_range, y_range, true, angleDrawLen); + + //旋转视角显示 + double matrix3d[9]; + EulerRpyToRotation1(rpy, matrix3d); + std::vector r_objOps; + r_objOps.insert(r_objOps.end(), objOps.begin(), objOps.end()); + for (int i = 0; i < objOps.size(); i++) + { + SVzNL3DPoint c_pt = { objOps[i].centerPos.x, objOps[i].centerPos.y, objOps[i].centerPos.z }; + SVzNL3DPoint r_c_pt = _ptRotate(c_pt, matrix3d); + r_objOps[i].centerPos.x = r_c_pt.x; + r_objOps[i].centerPos.y = r_c_pt.y; + r_objOps[i].centerPos.z = r_c_pt.z; + } + std::vector> rotateLines; + SVzNLRangeD rx_range, ry_range; + _rotateCloudPts_RGBD(scanData, lineNum, matrix3d, rotateLines, &rx_range, &ry_range); + cv::Mat r_img = cv::Mat::zeros(imgRows, imgCols, CV_8UC3); + //计算投影比例 + double rx_scale = (rx_range.max - rx_range.min) / x_cols; + double ry_scale = (ry_range.max - ry_range.min) / y_rows; + _XOYprojection_RGBD(r_img, rotateLines, r_objOps, rx_scale, ry_scale, rx_range, ry_range, false, angleDrawLen); + + cv::Mat dis_img; + cv::hconcat(img, r_img, dis_img); + cv::imwrite(fileName, dis_img); + return; +} + +void _genXOYProjectionImage_sideBagInfo(cv::String & fileName, SVzNL3DLaserLine * scanData, int lineNum, std::vector&objOps, double rpy[3]) +{ + //统计X和Y的范围 + std::vector> scan_lines; + SVzNLRangeD x_range = { 0, -1 }; + SVzNLRangeD y_range = { 0, -1 }; + for (int line = 0; line < lineNum; line++) + { + std::vector< SVzNL3DPosition> a_line; + for (int i = 0; i < scanData[line].nPositionCnt; i++) + { + SVzNL3DPosition* pt3D = &scanData[line].p3DPosition[i]; + if (pt3D->pt3D.z < 1e-4) + continue; + + a_line.push_back(*pt3D); + if (x_range.max < x_range.min) + { + x_range.min = pt3D->pt3D.x; + x_range.max = pt3D->pt3D.x; + } + else + { + if (x_range.min > pt3D->pt3D.x) + x_range.min = pt3D->pt3D.x; + if (x_range.max < pt3D->pt3D.x) + x_range.max = pt3D->pt3D.x; + } + if (y_range.max < y_range.min) + { + y_range.min = pt3D->pt3D.y; + y_range.max = pt3D->pt3D.y; + } + else + { + if (y_range.min > pt3D->pt3D.y) + y_range.min = pt3D->pt3D.y; + if (y_range.max < pt3D->pt3D.y) + y_range.max = pt3D->pt3D.y; + } + } + scan_lines.push_back(a_line); + } + + double x_scale = 1.0; + double y_scale = 1.0; + int x_rows = int((x_range.max - x_range.min) / x_scale); + int y_rows = int((y_range.max - y_range.min) / y_scale); + if (x_rows % 2 > 0) + x_rows += 1; + if (y_rows % 2 > 0) + y_rows += 1; + int imgRows = y_rows + 32; + int imgCols = x_rows + 32; + + cv::Mat img = cv::Mat::zeros(imgRows, imgCols, CV_8UC3); + //计算投影比例 + + _XOYprojection_sideBagInfo(img, scan_lines, objOps, x_scale, y_scale, x_range, y_range); + +#if 0 + //旋转视角显示 + double matrix3d[9]; + EulerRpyToRotation1(rpy, matrix3d); + std::vector r_objOps; + r_objOps.insert(r_objOps.end(), objOps.begin(), objOps.end()); + for (int i = 0; i < objOps.size(); i++) + { + SVzNL3DPoint c_pt = { objOps[i].graspPos.x, objOps[i].graspPos.y, objOps[i].graspPos.z }; + SVzNL3DPoint r_c_pt = _ptRotate(c_pt, matrix3d); + r_objOps[i].graspPos.x = r_c_pt.x; + r_objOps[i].graspPos.y = r_c_pt.y; + r_objOps[i].graspPos.z = r_c_pt.z; + } + std::vector> rotateLines; + SVzNLRangeD rx_range, ry_range; + _rotateCloudPts(scanData, lineNum, matrix3d, rotateLines, &rx_range, &ry_range); + cv::Mat r_img = cv::Mat::zeros(imgRows, imgCols, CV_8UC3); + //计算投影比例 + double rx_scale = (rx_range.max - rx_range.min) / x_cols; + double ry_scale = (ry_range.max - ry_range.min) / y_rows; + _XOYprojection_sideBagInfo(r_img, rotateLines, r_objOps, rx_scale, ry_scale, rx_range, ry_range); + + cv::Mat dis_img; + cv::hconcat(img, r_img, dis_img); + cv::imwrite(fileName, dis_img); +#else + cv::Mat rot; + cv::rotate(img, rot, cv::ROTATE_90_CLOCKWISE); // 顺时针90°旋转 + cv::flip(rot, img, 1); // 左右翻转 + cv::imwrite(fileName, img); +#endif + return; +} + + +//量化成640*640左右大小的图像。 +void project2DWithInterpolate(cv::Mat & img, SVzNL3DLaserLine * scanData, int lineNum, double fixed_xy_scale, double z_scale) +{ + double namedSize = 640.0;//目标图像最大尺度为640像素 + + SVzNLRangeD x_range = { 0, -1 }; + SVzNLRangeD y_range = { 0, -1 }; + SVzNLRangeD z_range = { 0, -1 }; + for (int line = 0; line < lineNum; line++) + { + for (int i = 0; i < scanData[line].nPositionCnt; i++) + { + SVzNL3DPosition* pt3D = &scanData[line].p3DPosition[i]; + if (pt3D->pt3D.z < 1e-4) + continue; + + if (x_range.max < x_range.min) + { + x_range.min = pt3D->pt3D.x; + x_range.max = pt3D->pt3D.x; + } + else + { + if (x_range.min > pt3D->pt3D.x) + x_range.min = pt3D->pt3D.x; + if (x_range.max < pt3D->pt3D.x) + x_range.max = pt3D->pt3D.x; + } + if (y_range.max < y_range.min) + { + y_range.min = pt3D->pt3D.y; + y_range.max = pt3D->pt3D.y; + } + else + { + if (y_range.min > pt3D->pt3D.y) + y_range.min = pt3D->pt3D.y; + if (y_range.max < pt3D->pt3D.y) + y_range.max = pt3D->pt3D.y; + } + if (z_range.max < z_range.min) + { + z_range.min = pt3D->pt3D.z; + z_range.max = pt3D->pt3D.z; + } + else + { + if (z_range.min > pt3D->pt3D.z) + z_range.min = pt3D->pt3D.z; + if (z_range.max < pt3D->pt3D.z) + z_range.max = pt3D->pt3D.z; + } + } + } + + double xy_scale = fixed_xy_scale; + if (fixed_xy_scale < 1e-4) + { + double x_scale = (x_range.max - x_range.min) / namedSize; + double y_scale = (y_range.max - y_range.min) / namedSize; + double xy_scale; + if (x_scale < y_scale) + xy_scale = y_scale; + else + xy_scale = x_scale; + } + int img_rows = (int)((y_range.max - y_range.min) / xy_scale) + 1; + if (img_rows % 2 > 0) + img_rows += 1; + int img_cols = (int)((x_range.max - x_range.min) / xy_scale) + 1; + if (img_cols % 2 > 0) + img_cols += 1; + img = cv::Mat::zeros(img_rows, img_cols, CV_8UC1); + + int polateWin = 5; + for (int line = 0; line < lineNum; line++) + { + //同时进行垂直插值 + int pre_py = -1; + SVzNL3DPosition* pre_pt3D = NULL; + int pre_i = -1; + for (int i = 0; i < scanData[line].nPositionCnt; i++) + { + SVzNL3DPosition* pt3D = &scanData[line].p3DPosition[i]; + if (pt3D->pt3D.z < 1e-4) + continue; + double x = pt3D->pt3D.x; + double y = pt3D->pt3D.y; + int px = (int)((x - x_range.min) / xy_scale); + int py = (int)((y - y_range.min) / xy_scale); + int value = (int)((pt3D->pt3D.z - z_range.min) / z_scale); + if (value > 254) + value = 254; + value = 255 - value; + img.at(py, px) = (uchar)value; + //检查是否需要插值 + if ((py > pre_py + 1) && (py <= pre_py + polateWin + 1) && (pre_py >= 0) && (pre_i >= 0) && (i == pre_i + 1)) //3D点连续，量化点不连续，插值 + { + double dist = double(py - pre_py); + for (int iy = pre_py + 1; iy < py; iy++) + { + double k1 = (double)(iy - pre_py) / dist; + double k0 = 1 - k1; + double inter_x = pre_pt3D->pt3D.x * k0 + pt3D->pt3D.x * k1; + double inter_y = pre_pt3D->pt3D.y * k0 + pt3D->pt3D.y * k1; + double inter_z = pre_pt3D->pt3D.z * k0 + pt3D->pt3D.z * k1; + int polate_px = (int)((inter_x - x_range.min) / xy_scale); + int polate_py = (int)((inter_y - y_range.min) / xy_scale); + int polate_value = (int)((inter_z - z_range.min) / z_scale); + if (polate_value > 254) + polate_value = 254; + polate_value = 255 - polate_value; + img.at(polate_py, polate_px) = (uchar)polate_value; + } + } + pre_i = i; + pre_py = py; + pre_pt3D = pt3D; + } + } + //水平插值 + for (int y = 0; y < img.rows; y++) + { + int pre_x = -1; + uchar pre_value = 0; + for (int x = 0; x < img.cols; x++) + { + uchar value = img.at(y, x); + if (value > 0) + { + if ((x > pre_x + 1) && (x <= pre_x + polateWin + 1) && (pre_x >= 0)) //水平不连续，插值 + { + double dist = double(x - pre_x); + for (int ix = pre_x + 1; ix < x; ix++) + { + double k1 = (double)(ix - pre_x) / dist; + double k0 = 1 - k1; + double inter_data = (double)pre_value * k0 + (double)value * k1; + if (inter_data > 255) + inter_data = 255; + uchar polate_value = (uchar)inter_data; + img.at(y, ix) = polate_value; + } + } + pre_x = x; + pre_value = value; + } + } + } +} +void _outputScanDataFile_removeZeros(char* fileName, SVzNL3DLaserLine * scanData, int lineNum, + float lineV, int maxTimeStamp, int clockPerSecond) +{ + std::ofstream sw(fileName); + sw << "LineNum:" << lineNum << std::endl; + sw << "DataType: 0" << std::endl; + sw << "ScanSpeed:" << lineV << std::endl; + sw << "PointAdjust: 1" << std::endl; + sw << "MaxTimeStamp:" << maxTimeStamp << "_" << clockPerSecond << std::endl; + + for (int line = 0; line < lineNum; line++) + { + int realNum = 0; + for (int i = 0; i < scanData[line].nPositionCnt; i++) + { + if (scanData[line].p3DPosition[i].pt3D.z > 1e-4) + realNum++; + } + sw << "Line_" << line << "_" << scanData[line].nTimeStamp << "_" << realNum << std::endl; + for (int i = 0; i < scanData[line].nPositionCnt; i++) + { + if (scanData[line].p3DPosition[i].pt3D.z > 1e-4) + { + SVzNL3DPosition* pt3D = &scanData[line].p3DPosition[i]; + float x = (float)pt3D->pt3D.x; + float y = (float)pt3D->pt3D.y; + float z = (float)pt3D->pt3D.z; + sw << "{ " << x << "," << y << "," << z << " }-"; + sw << "{0,0}-{0,0}" << std::endl; + } + } + } + sw.close(); +} + +#define TEST_CONVERT_TO_GRID 0 +#define TEST_COMPUTE_WHEEL_ARCH 1 +#define TEST_COMPUTE_CALIB_PARA 1 + +#define TEST_GROUP 1 +int main() +{ +#if TEST_CONVERT_TO_GRID + //将数据转换成栅格格式格式 + char _scan_dir[256]; + int lineNum = 0; + float lineV = 0.0f; + int dataCalib = 0; + int maxTimeStamp = 0; + int clockPerSecond = 0; + sprintf_s(_scan_dir, "F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/"); + char _scan_src_file[256]; + double _F = 1177.2; //f + for (int i = 1; i <= 1; i++) + { + //read ply txt + sprintf_s(_scan_src_file, "%sLaserLine%d.txt", _scan_dir, i); + std::vector> scanData; + vzReadPlyTxtPointFromFile_XYZRGB_vector(_scan_src_file, scanData); + sprintf_s(_scan_src_file, "%sscanData%d.txt", _scan_dir, i); + _outputScanDataFile_SPointXYZRGB_vector(_scan_src_file, scanData); + +#if 0 + double rpy[3] = { 0,-18, 0 }; + double camPoseR[9]; + EulerRpyToRotation1(rpy, camPoseR); +#else + double camPoseR[9] = { + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0 }; +#endif + std::vector> gridData; + _convertToGridData_XYZRGB(scanData, _F, gridData); + char _out_file[256]; + sprintf_s(_out_file, "%sLaserLine%d_grid.txt", _scan_dir, i); + _outputScanDataFile_self(_out_file, gridData, 0, 0, 0); + printf("%s: convert done!\n", _scan_src_file); + } +#endif + +#if TEST_COMPUTE_CALIB_PARA + char _calib_datafile[256]; + sprintf_s(_calib_datafile, "F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/现场数据/ground_LaserData.txt"); + + std::vector< std::vector> scanLines; + vzReadLaserScanPointFromFile_XYZ_vector(_calib_datafile, scanLines); + if(scanLines.size() > 0) + { + SSG_planeCalibPara calibPara = wd_horizonCamera_getGroundCalibPara(scanLines); +#if 1 + //结果进行验证 + int lineNum = (int)scanLines.size(); + for (int i = 0; i < lineNum; i++) + { + if (i == 14) + int kkk = 1; + //行处理 + //调平，去除地面 + wd_horizonCamera_lineDataR(scanLines[i], calibPara.planeCalib, -1);// calibPara.planeHeight); + } +#endif + // + char calibFile[250]; + sprintf_s(calibFile, "F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/现场数据/ground_calib_para.txt"); + _outputCalibPara(calibFile, calibPara); + char _out_file[256]; + sprintf_s(_out_file, "F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/现场数据/LaserLine_grund_calib.txt"); + _outputScanDataFile_self(_out_file, scanLines, 0, 0, 0); + printf("%s: calib done!\n", _calib_datafile); + } + +#endif + +#if TEST_COMPUTE_WHEEL_ARCH + const char* dataPath[TEST_GROUP] = { + "F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/现场数据/", //0 + }; + + SVzNLRange fileIdx[TEST_GROUP] = { + {1,2}, + }; + + SSG_planeCalibPara poseCalibPara; + //初始化成单位阵 + poseCalibPara.planeCalib[0] = 1.0; + poseCalibPara.planeCalib[1] = 0.0; + poseCalibPara.planeCalib[2] = 0.0; + poseCalibPara.planeCalib[3] = 0.0; + poseCalibPara.planeCalib[4] = 1.0; + poseCalibPara.planeCalib[5] = 0.0; + poseCalibPara.planeCalib[6] = 0.0; + poseCalibPara.planeCalib[7] = 0.0; + poseCalibPara.planeCalib[8] = 1.0; + poseCalibPara.planeHeight = -1.0; + for (int i = 0; i < 9; i++) + poseCalibPara.invRMatrix[i] = poseCalibPara.planeCalib[i]; + + const char* ver = wd_wheelArchHeigthMeasureVersion(); + printf("ver:%s\n", ver); + + char _scan_file[256]; + + int endGroup = TEST_GROUP - 1; + for (int grp = 0; grp <= endGroup; grp++) + { + char calibFile[250]; + sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[grp]); + poseCalibPara = _readCalibPara(calibFile); + for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) + { + //fidx = 193; + sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx); + std::vector> scanData; + vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanData); + if (scanData.size() == 0) + continue; + + //algoParam.filterParam.continuityTh = 20.0; //噪声滤除。当相邻点的z跳变大于此门限时，检查是否为噪声。若长度小于outlierLen， 视为噪声 + //algoParam.filterParam.outlierTh = 5; + + long t1 = GetTickCount64(); + + SVzNL3DRangeD wheelRoi3d = { {-1000, 1000}, {-2000, 2000}, {500, 2500} }; + SSG_cornerParam cornerParam; + cornerParam.cornerTh = 45; //45度角 + cornerParam.scale = 50; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; + cornerParam.minEndingGap = 20; // algoParam.bagParam.bagW / 4; + cornerParam.minEndingGap_z = 20; + cornerParam.jumpCornerTh_1 = 10; //水平角度，小于此角度视为水平 + cornerParam.jumpCornerTh_2 = 60; + + SSG_lineSegParam lineSegPara; + lineSegPara.distScale = 1.0; + lineSegPara.segGapTh_y = 5.0; //y方向间隔大于5mm认为是分段 + lineSegPara.segGapTh_z = 10.0; //z方向间隔大于10mm认为是分段 + + SSG_outlierFilterParam filterParam; + filterParam.continuityTh = 20.0; //噪声滤除。当相邻点的z跳变大于此门限时，检查是否为噪声。若长度小于outlierLen， 视为噪声 + filterParam.outlierTh = 5; + + SSG_treeGrowParam growParam; + growParam.maxLineSkipNum = 10; + growParam.yDeviation_max = 5.0; + growParam.maxSkipDistance = 5.0; + growParam.zDeviation_max = 2.0;// + growParam.minLTypeTreeLen = 100; //mm + growParam.minVTypeTreeLen = 100; //mm + + WD_wheelArchInfo wheelArcHeight; + memset(&wheelArcHeight, 0, sizeof(WD_wheelArchInfo)); + + bool wheePresense = wd_wheelPresenseDetection( scanData, wheelRoi3d); + if (true == wheePresense) + { + int lineNum = (int)scanData.size(); + for (int i = 0; i < lineNum; i++) + { + if (i == 14) + int kkk = 1; + //行处理 + //调平，去除地面 + wd_horizonCamera_lineDataR(scanData[i], poseCalibPara.planeCalib, poseCalibPara.planeHeight - 5); + } +#if 0 //数据转存 + sprintf_s(_scan_file, "%sLaserLine%d_grid_RTadjust.txt", dataPath[grp], fidx); + _outputScanDataFile_self(_scan_file, scanData, 0, 0, 0); +#endif + + + int errCode = 0; + WD_wheelArchInfo wheelArcHeight = wd_wheelArchHeigthMeasure( + scanData, + cornerParam, + lineSegPara, + filterParam, + growParam, + poseCalibPara, + &errCode); + } + long t2 = GetTickCount64(); + + + char _dbg_file[256]; +#if 1 + + sprintf_s(_dbg_file, "%sresult\\LaserLine%d_result.txt", dataPath[grp], fidx); + _outputScanDataFile_RGBD_obj(_dbg_file, scanData, 0, 0, 0, wheelArcHeight); +#endif + printf("%s: height=%f, arcToGrund=%f, time=%d(ms)!\n", _scan_file, wheelArcHeight.archToCenterHeigth, wheelArcHeight.archToGroundHeigth, (int)(t2 - t1)); + } + } +#endif + printf("all done!\n"); +} + + +// 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单 +// 调试程序: F5 或调试 >“开始调试”菜单 + +// 入门使用技巧: +// 1. 使用解决方案资源管理器窗口添加/管理文件 +// 2. 使用团队资源管理器窗口连接到源代码管理 +// 3. 使用输出窗口查看生成输出和其他消息 +// 4. 使用错误列表窗口查看错误 +// 5. 转到“项目”>“添加新项”以创建新的代码文件，或转到“项目”>“添加现有项”以将现有代码文件添加到项目 +// 6. 将来，若要再次打开此项目，请转到“文件”>“打开”>“项目”并选择 .sln 文件 diff --git a/GrabBagPrj/pkg_wheelmeasure.sh b/GrabBagPrj/pkg_wheelmeasure.sh index 1cea49e0..38aea4a1 100644 --- a/GrabBagPrj/pkg_wheelmeasure.sh +++ b/GrabBagPrj/pkg_wheelmeasure.sh @@ -65,6 +65,12 @@ echo "复制 Qt 运行时环境..." cp -rfd ${QT_PKG_PATH}/ext ${PKG_PATH}/opt/firefly_qt5.15 cp ${QT_PKG_PATH}/target_qtEnv.sh ${PKG_PATH}/etc/profile.d/ +echo "清理不需要的 Qt WebEngine 库文件..." +rm -f ${PKG_PATH}/opt/firefly_qt5.15/lib/libQt5WebEngine* +rm -rf ${PKG_PATH}/opt/firefly_qt5.15/libexec/QtWebEngineProcess +rm -rf ${PKG_PATH}/opt/firefly_qt5.15/resources/qtwebengine* +rm -rf ${PKG_PATH}/opt/firefly_qt5.15/translations/qtwebengine* + # 复制 Qt 库文件 for libfile in ${QT_LIB_PATH}/*.so*; do # 获取文件名用于比较