GrabBag/App/Workpiece/Doc/工件角点检测功能实现说明.md

# 工件角点检测功能实现说明

## 功能概述
本文档说明如何在 WorkpieceApp 中集成工件角点提取功能，使用 SDK/workpieceCornerExtraction 算法库进行检测，并通过 JSON 格式将检测结果发送给客户端。

## 实现步骤

### 1. 配置结构更新 ✅

#### 1.1 更新 IVrConfig.h
在 `VrWorkpieceParam` 结构中添加了 `lineLen` 参数：
```cpp
struct VrWorkpieceParam
{
    double lapHeight = 2.0;        // 搭接厚度
    double weldMinLen = 2.0;       // 最小焊缝长度
    int weldRefPoints = 2;         // 输出参考点数量
    WeldScanMode scanMode = WeldScanMode::ScanMode_V;
    double lineLen = 100.0;        // 工件角点提取：直线段长度阈值
};
```

#### 1.2 更新配置文件 config.xml
```xml
<WorkpieceParam lapHeight="2.0" weldMinLen="2.0" weldRefPoints="2" lineLen="100.0" />
```

#### 1.3 更新 VrConfig.cpp
- 加载配置时读取 `lineLen` 参数
- 保存配置时写入 `lineLen` 参数

### 2. DetectPresenter 算法集成 (待实现)

#### 2.1 添加SDK头文件引用
```cpp
#include "BQ_workpieceCornerExtraction_Export.h"
```

#### 2.2 实现角点检测方法
在 `DetectWorkpiece` 方法中添加工件角点提取调用：

```cpp
int DetectPresenter::DetectWorkpiece(
    int cameraIndex,
    std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& laserLines,
    const VrAlgorithmParams& algorithmParams,
    const VrDebugParam& debugParam,
    LaserDataLoader& dataLoader,
    const double clibMatrix[16],
    DetectionResult& detectionResult)
{
    // 1. 转换激光线数据为算法需要的格式
    std::vector<std::vector<SVzNL3DPosition>> scanLines;
    // ... 数据转换代码 ...

    // 2. 准备算法参数
    SSX_BQworkpiecePara workpieceParam;
    workpieceParam.lineLen = algorithmParams.workpieceParam.lineLen;

    SSG_cornerParam cornerParam;
    cornerParam.cornerTh = algorithmParams.cornerParam.cornerTh;
    cornerParam.scale = algorithmParams.cornerParam.scale;
    cornerParam.minEndingGap = algorithmParams.cornerParam.minEndingGap;
    cornerParam.minEndingGap_z = algorithmParams.cornerParam.minEndingGap_z;
    cornerParam.jumpCornerTh_1 = algorithmParams.cornerParam.jumpCornerTh_1;
    cornerParam.jumpCornerTh_2 = algorithmParams.cornerParam.jumpCornerTh_2;

    SSG_outlierFilterParam filterParam;
    filterParam.continuityTh = algorithmParams.filterParam.continuityTh;
    filterParam.outlierTh = algorithmParams.filterParam.outlierTh;

    SSG_treeGrowParam growParam;
    growParam.maxLineSkipNum = algorithmParams.growParam.maxLineSkipNum;
    growParam.yDeviation_max = algorithmParams.growParam.yDeviation_max;
    growParam.maxSkipDistance = algorithmParams.growParam.maxSkipDistance;
    growParam.zDeviation_max = algorithmParams.growParam.zDeviation_max;
    growParam.minLTypeTreeLen = algorithmParams.growParam.minLTypeTreeLen;
    growParam.minVTypeTreeLen = algorithmParams.growParam.minVTypeTreeLen;

    // 3. 获取调平参数
    SSG_planeCalibPara groundCalibPara;
    const VrCameraPlaneCalibParam* cameraCalib =
        algorithmParams.planeCalibParam.GetCameraCalibParam(cameraIndex);
    if (cameraCalib && cameraCalib->isCalibrated) {
        memcpy(groundCalibPara.planeCalib, cameraCalib->planeCalib, sizeof(double) * 9);
        memcpy(groundCalibPara.invRMatrix, cameraCalib->invRMatrix, sizeof(double) * 9);
        groundCalibPara.planeHeight = cameraCalib->planeHeight;
    } else {
        // 使用单位矩阵
        double identity[9] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
        memcpy(groundCalibPara.planeCalib, identity, sizeof(double) * 9);
        memcpy(groundCalibPara.invRMatrix, identity, sizeof(double) * 9);
        groundCalibPara.planeHeight = -1.0;
    }

    // 4. 调用算法
    SSX_debugInfo debugContours[100];  // 调试信息
    int errCode = 0;

    SSX_BQworkpieceResult result = sx_BQ_getWorkpieceCorners(
        scanLines,
        cornerParam,
        filterParam,
        growParam,
        groundCalibPara,
        workpieceParam,
#if _OUTPUT_DEBUG_DATA
        debugContours,
#endif
        &errCode
    );

    if (errCode != 0) {
        LOG_ERROR("工件角点检测失败，错误码: %d\n", errCode);
        return errCode;
    }

    // 5. 将结果转换为手眼坐标系
    // 手眼标定变换矩阵应用到检测结果
    // ... 坐标转换代码 ...

    // 6. 填充检测结果
    detectionResult.cameraIndex = cameraIndex;
    detectionResult.positions.clear();

    // 添加左侧角点
    for (int i = 0; i < 3; i++) {
        WorkpiecePosition pos;
        pos.x = result.corner_L[i].x;
        pos.y = result.corner_L[i].y;
        pos.z = result.corner_L[i].z;
        detectionResult.positions.push_back(pos);
    }

    // 添加右侧角点
    for (int i = 0; i < 3; i++) {
        WorkpiecePosition pos;
        pos.x = result.corner_R[i].x;
        pos.y = result.corner_R[i].y;
        pos.z = result.corner_R[i].z;
        detectionResult.positions.push_back(pos);
    }

    // 添加顶部角点
    for (int i = 0; i < 3; i++) {
        WorkpiecePosition pos;
        pos.x = result.corner_T[i].x;
        pos.y = result.corner_T[i].y;
        pos.z = result.corner_T[i].z;
        detectionResult.positions.push_back(pos);
    }

    // 添加底部角点
    for (int i = 0; i < 3; i++) {
        WorkpiecePosition pos;
        pos.x = result.corner_B[i].x;
        pos.y = result.corner_B[i].y;
        pos.z = result.corner_B[i].z;
        detectionResult.positions.push_back(pos);
    }

    // 7. 生成可视化图像（如果需要）
    if (debugParam.saveDebugImage) {
        // 创建点云可视化图像
        // ... 图像生成代码 ...
    }

    return SUCCESS;
}
```

### 3. WorkpiecePresenter 业务逻辑集成 (待实现)

#### 3.1 更新检测任务方法
在 `_DetectTask()` 方法中调用 `DetectPresenter::DetectWorkpiece()`

```cpp
int WorkpiecePresenter::_DetectTask()
{
    LOG_INFO("[Algo Thread] Start workpiece corner extraction detection\n");

    std::lock_guard<std::mutex> lock(m_detectionDataMutex);

    if (m_detectionDataCache.empty()) {
        LOG_WARNING("No cached detection data available\n");
        return ERR_CODE(DEV_DATA_INVALID);
    }

    // 执行检测
    DetectionResult detectionResult;
    int nRet = m_pDetectPresenter->DetectWorkpiece(
        m_currentCameraIndex,
        m_detectionDataCache,
        m_algorithmParams,
        m_debugParam,
        m_dataLoader,
        m_clibMatrixList[m_currentCameraIndex - 1].clibMatrix,
        detectionResult
    );

    if (nRet != SUCCESS) {
        LOG_ERROR("Detection failed with error: %d\n", nRet);
        if (m_pStatus) {
            m_pStatus->OnStatusUpdate("检测失败");
        }
        return nRet;
    }

    // 通知UI显示结果
    detectionResult.cameraIndex = m_currentCameraIndex;
    m_pStatus->OnDetectionResult(detectionResult);

    // 发送结果到TCP客户端
    _SendDetectionResultToTCP(detectionResult, m_currentCameraIndex);

    // 更新状态
    m_currentWorkStatus = WorkStatus::Completed;
    m_pStatus->OnWorkStatusChanged(WorkStatus::Completed);

    return SUCCESS;
}
```

#### 3.2 实现TCP结果发送方法
```cpp
void WorkpiecePresenter::_SendDetectionResultToTCP(
    const DetectionResult& detectionResult,
    int cameraIndex)
{
    if (!m_pTCPServer || !m_bTCPConnected) {
        LOG_WARNING("TCP not connected, skip sending detection result\n");
        return;
    }

    // 构建JSON格式的检测结果
    QJsonObject jsonResult;
    jsonResult["cameraIndex"] = cameraIndex;
    jsonResult["timestamp"] = QDateTime::currentMSecsSinceEpoch();
    jsonResult["cornerCount"] = static_cast<int>(detectionResult.positions.size());

    QJsonArray cornersArray;
    for (const auto& pos : detectionResult.positions) {
        QJsonObject cornerObj;
        cornerObj["x"] = pos.x;
        cornerObj["y"] = pos.y;
        cornerObj["z"] = pos.z;
        cornerObj["roll"] = pos.roll;
        cornerObj["pitch"] = pos.pitch;
        cornerObj["yaw"] = pos.yaw;
        cornersArray.append(cornerObj);
    }
    jsonResult["corners"] = cornersArray;

    QJsonDocument jsonDoc(jsonResult);
    QByteArray jsonData = jsonDoc.toJson(QJsonDocument::Compact);

    // 发送JSON数据
    m_pTCPServer->SendToAll(jsonData.data(), jsonData.size());

    LOG_INFO("Sent detection result to TCP client: %d corners\n",
             detectionResult.positions.size());
}
```

### 4. 参数配置界面 (待实现)

#### 4.1 更新 dialogalgoarg.ui
在算法参数对话框中添加工件参数配置控件：
- `lineEdit_lineLen`: 直线段长度阈值输入框
- `label_lineLen`: 参数标签

#### 4.2 更新 dialogalgoarg.cpp
```cpp
void DialogAlgoarg::LoadConfigToUI()
{
    // ... 现有加载代码 ...

    // 加载工件参数
    ui->lineEdit_lineLen->setText(
        QString::number(m_configData.algorithmParams.workpieceParam.lineLen));
}

bool DialogAlgoarg::SaveConfigFromUI()
{
    // ... 现有保存代码 ...

    // 保存工件参数
    m_configData.algorithmParams.workpieceParam.lineLen =
        ui->lineEdit_lineLen->text().toDouble();

    // 保存到配置文件
    QString configPath = PathManager::GetConfigFilePath();
    return m_vrConfig->SaveConfig(configPath.toStdString(), m_configData);
}
```

### 5. 主窗口显示结果 (待实现)

#### 5.1 更新 mainwindow.cpp
实现 `OnDetectionResult` 回调方法：

```cpp
void MainWindow::OnDetectionResult(const DetectionResult& result)
{
    // 1. 显示检测图像
    if (!result.image.isNull()) {
        ui->label_image->setPixmap(QPixmap::fromImage(result.image));
    }

    // 2. 更新结果列表
    ui->listWidget_results->clear();
    for (size_t i = 0; i < result.positions.size(); i++) {
        const auto& pos = result.positions[i];
        QString resultText = QString("角点 %1: X=%.2f, Y=%.2f, Z=%.2f")
            .arg(i + 1)
            .arg(pos.x)
            .arg(pos.y)
            .arg(pos.z);
        ui->listWidget_results->addItem(resultText);
    }

    // 3. 更新状态栏
    QString statusText = QString("检测完成，找到 %1 个角点")
        .arg(result.positions.size());
    ui->statusBar->showMessage(statusText);
}
```

### 6. 项目配置更新 (待实现)

#### 6.1 更新 WorkpieceApp.pro
添加工件角点提取SDK依赖：

```qmake
# 工件角点提取算法SDK
INCLUDEPATH += ../../../SDK/workpieceCornerExtraction/Inc

win32:CONFIG(release, debug|release): {
    LIBS += -L$$PWD/../../../SDK/workpieceCornerExtraction/Windows/x64/Release
    LIBS += -lBQ_workpieceCornerExtraction -lbaseAlgorithm
}
else:win32:CONFIG(debug, debug|release): {
    LIBS += -L$$PWD/../../../SDK/workpieceCornerExtraction/Windows/x64/Debug
    LIBS += -lBQ_workpieceCornerExtraction -lbaseAlgorithm
}
else:unix:!macx: {
    LIBS += -L$$PWD/../../../SDK/workpieceCornerExtraction/Arm/aarch64
    LIBS += -lworkpieceCornerExtraction -lbaseAlgorithm
}
```

### 7. TCP通信协议

#### 7.1 客户端触发检测
客户端发送触发命令：
```
@,1,1,Trig,$
```
格式：`@，视觉号，视觉模版号，启动信息，$`

#### 7.2 服务端返回检测结果
服务端以JSON格式返回角点检测结果：

```json
{
    "cameraIndex": 1,
    "timestamp": 1640000000000,
    "cornerCount": 12,
    "corners": [
        {
            "x": 100.5,
            "y": 200.3,
            "z": 50.2,
            "roll": 0.0,
            "pitch": 0.0,
            "yaw": 0.0
        },
        // ... 更多角点数据 ...
    ]
}
```

## 数据流程

```
客户端触发 → TCPServerProtocol → WorkpiecePresenter::StartDetection
    ↓
相机采集点云数据 → _DetectionCallback → m_detectionDataCache
    ↓
相机扫描完成 → _AlgoDetectThread → _DetectTask
    ↓
DetectPresenter::DetectWorkpiece → sx_BQ_getWorkpieceCorners (SDK算法)
    ↓
坐标转换（手眼标定）→ DetectionResult
    ↓
MainWindow::OnDetectionResult (UI显示) + _SendDetectionResultToTCP (发送给客户端)
```

## 关键技术点

### 1. 点云数据转换
需要将 `SVzLaserLineData` 格式转换为 `std::vector<std::vector<SVzNL3DPosition>>` 格式

### 2. 调平参数应用
使用相机的平面校准参数对点云数据进行调平处理

### 3. 手眼标定
将相机坐标系下的检测结果转换到机械臂坐标系

### 4. JSON序列化
使用Qt的QJsonObject/QJsonDocument进行JSON格式转换

### 5. 多线程安全
- 使用 `m_detectionDataMutex` 保护检测数据缓存
- 使用 `m_algoDetectCondition` 进行线程同步

## 编译和部署

### Windows平台
1. 确保SDK库文件在正确路径
2. 使用Qt Creator打开项目
3. 选择Release或Debug配置
4. 构建项目

### ARM/Linux平台
1. 配置交叉编译环境
2. 确保ARM版本的SDK库文件存在
3. 使用qmake生成Makefile
4. 执行make编译

```bash
cd App/Workpiece/WorkpieceApp
qmake WorkpieceApp.pro
make
```

## 测试验证

### 1. 参数配置测试
- 打开算法参数配置对话框
- 修改lineLen参数
- 保存并验证config.xml文件

### 2. 算法检测测试
- 加载调试点云数据
- 触发检测
- 验证检测结果

### 3. TCP通信测试
- 启动TCP服务器
- 客户端连接并发送触发命令
- 验证JSON结果返回

### 4. UI显示测试
- 验证检测图像显示
- 验证角点结果列表
- 验证状态信息更新

## 注意事项

1. **SDK版本兼容性**：确保使用的SDK版本与项目兼容
2. **内存管理**：注意释放算法分配的内存资源
3. **坐标系转换**：确保手眼标定矩阵正确应用
4. **错误处理**：完善错误检测和日志输出
5. **性能优化**：大点云数据处理时注意性能
6. **线程安全**：确保多线程访问的数据安全

## 后续优化建议

1. 增加算法参数实时调整功能
2. 支持多种工件类型的角点检测
3. 增加检测结果的3D可视化
4. 优化大数据量下的处理速度
5. 增加离线调试和参数优化工具