GrabBag/App/HoleDetection/HoleDetectionApp/Presenter/Src/DetectPresenter.cpp

#include "DetectPresenter.h"
#include "HoleDetection.h"
#include "HoleDetectionParams.h"
#include "AlgoParamConverter.h"
#include <fstream>
#include "PointCloudImageUtils.h"
#include "CoordinateTransform.h"
#include "VrConvert.h"
#include "VrTimeUtils.h"
#include "VrDateUtils.h"

// 辅助函数：向量叉积
static CTVec3D crossProduct(const CTVec3D& a, const CTVec3D& b) {
    return CTVec3D(
        a.y * b.z - a.z * b.y,
        a.z * b.x - a.x * b.z,
        a.x * b.y - a.y * b.x
    );
}

DetectPresenter::DetectPresenter()
{
    LOG_DEBUG("DetectPresenter Init (HoleDetection), algo ver: %s\n", GetAlgorithmVersion());
}

DetectPresenter::~DetectPresenter()
{
}

QString DetectPresenter::GetAlgoVersion()
{
    // 关于窗口通过这里读取算法库版本号。
    const char* version = GetAlgorithmVersion();
    return version ? QString::fromUtf8(version) : QString();
}

int DetectPresenter::DetectHoles(
    int cameraIndex,
    std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& laserLines,
    const VrAlgorithmParams& algorithmParams,
    const VrDebugParam& debugParam,
    LaserDataLoader& dataLoader,
    const double clibMatrix[16],
    int eulerOrder,
    int dirVectorInvert,
    const RobotFlangePose& robotPose,
    HoleDetectionResult& detectionResult)
{
    if (laserLines.empty()) {
        LOG_WARNING("No laser lines data available\n");
        return ERR_CODE(DEV_DATA_INVALID);
    }

    // 保存debug数据
    // 调试模式下先保存原始激光线数据，便于后续离线复现问题。
    std::string timeStamp = CVrDateUtils::GetNowTime();
    if (debugParam.enableDebug && debugParam.savePointCloud) {
        LOG_INFO("[Algo Thread] Debug mode is enabled, saving point cloud data\n");

        std::string fileName = debugParam.debugOutputPath + "/Laserline_" + std::to_string(cameraIndex) + "_" + timeStamp + ".txt";
        dataLoader.SaveLaserScanData(fileName, laserLines, laserLines.size(), 0.0, 0, 0);
    }

    int nRet = SUCCESS;

    // 转换为算法需要的XYZ格式
    // 将激光线缓存转换为算法要求的 XYZ 网格格式。
    std::vector<std::vector<SVzNL3DPosition>> xyzData;
    int convertResult = dataLoader.ConvertToSVzNL3DPosition(laserLines, xyzData);
    if (convertResult != SUCCESS || xyzData.empty()) {
        LOG_WARNING("Failed to convert data to XYZ format or no XYZ data available\n");
        return ERR_CODE(DEV_DATA_INVALID);
    }

    // 将 xyzData 转换为 SVzNLPointXYZ* 网格数据
    int rows = static_cast<int>(xyzData.size());
    int cols = 0;
    for (const auto& row : xyzData) {
        if (static_cast<int>(row.size()) > cols) {
            cols = static_cast<int>(row.size());
        }
    }

    if (rows <= 0 || cols <= 0) {
        LOG_WARNING("Invalid grid dimensions: rows=%d, cols=%d\n", rows, cols);
        return ERR_CODE(DEV_DATA_INVALID);
    }

    LOG_INFO("[Algo Thread] Grid dimensions: rows=%d, cols=%d\n", rows, cols);

    // 分配网格数组（行优先）
    // 按行优先展开为一维数组，匹配 DetectMultipleHoles 的入参形式。
    SVzNLPointXYZ* points = new SVzNLPointXYZ[rows * cols];
    memset(points, 0, sizeof(SVzNLPointXYZ) * rows * cols);

    // 填充有效点，缺失点用 {0,0,0} 填充
    // 行长度不足时补 0，保证每行列数一致。
    for (int r = 0; r < rows; r++) {
        int rowCols = static_cast<int>(xyzData[r].size());
        for (int c = 0; c < cols; c++) {
            int idx = r * cols + c;
            if (c < rowCols) {
                points[idx].x = static_cast<float>(xyzData[r][c].pt3D.x);
                points[idx].y = static_cast<float>(xyzData[r][c].pt3D.y);
                points[idx].z = static_cast<float>(xyzData[r][c].pt3D.z);
            } else {
                points[idx].x = 0.0f;
                points[idx].y = 0.0f;
                points[idx].z = 0.0f;
            }
        }
    }

    // 使用 AlgoParamConverter 进行参数转换
    // 配置层参数在这里统一映射为算法库结构体。
    RansacPlaneSegmentationParams ransacParams = AlgoParamConverter::ToAlgoParam(algorithmParams.ransacParam);
    SHoleDetectionParams detectionParams = AlgoParamConverter::ToAlgoParam(algorithmParams.detectionParam);
    SHoleFilterParams filterParams = AlgoParamConverter::ToAlgoParam(algorithmParams.filterParam);

    if (debugParam.enableDebug && debugParam.printDetailLog) {
        AlgoParamConverter::LogAlgoParams("[Algo Thread]", ransacParams, detectionParams, filterParams, clibMatrix);
    }

    CVrTimeUtils oTimeUtils;

    LOG_DEBUG("before DetectMultipleHoles\n");

    // 调用孔洞检测算法
    // 新版接口同时需要 RANSAC、Detection、Filter 三组参数。
    SMultiHoleResult multiResult;
    int errCode = DetectMultipleHoles(
        points,
        rows,
        cols,
        ransacParams,
        detectionParams,
        filterParams,
        &multiResult
    );

    LOG_DEBUG("after DetectMultipleHoles\n");

    LOG_INFO("DetectMultipleHoles: found %d holes (candidates=%d, filtered=%d), err=%d runtime=%.3fms\n",
        multiResult.holeCount, multiResult.totalCandidates, multiResult.filteredCount,
        errCode, oTimeUtils.GetElapsedTimeInMilliSec());

    // 记录统计信息到检测结果
    // 这些统计值直接给界面展示使用。
    detectionResult.totalCandidates = multiResult.totalCandidates;
    detectionResult.filteredCount = multiResult.filteredCount;

    if (errCode != HD_SUCCESS) {
        LOG_ERROR("DetectMultipleHoles failed with error code: %d\n", errCode);
        delete[] points;
        FreeMultiHoleResult(&multiResult);
        return errCode;
    }

    // 构建手眼标定齐次变换矩阵 T_end_to_cam（从4x4 clibMatrix数组）
    // 先构造手眼矩阵 T_end_to_cam。
    CTHomogeneousMatrix handEyeHM;
    for (int i = 0; i < 4; i++) {
        for (int j = 0; j < 4; j++) {
            handEyeHM.at(i, j) = clibMatrix[i * 4 + j];
        }
    }

    // 构建机器人法兰位姿齐次变换矩阵 T_base_to_end
    CTRobotPose ctRobotPose = CTRobotPose::fromDegrees(
        robotPose.x, robotPose.y, robotPose.z,
        robotPose.roll, robotPose.pitch, robotPose.yaw);
    CTHomogeneousMatrix T_robot = ctRobotPose.toHomogeneousMatrix();

    // 眼在手上：T_total = T_base_to_end × T_end_to_cam
    // 眼在手上场景下，总变换为 T_base_to_end * T_end_to_cam。
    CTHomogeneousMatrix T_total = T_robot * handEyeHM;

    // 构建用于可视化的孔洞中心点列表
    // 保留一份孔中心和半径，后面用于可视化绘制。
    std::vector<std::pair<SVzNL3DPointF, float>> holeVisData;  // center + radius

    // 处理每个检测到的孔洞
    for (int i = 0; i < multiResult.holeCount; i++) {
        const SHoleResult& hole = multiResult.holes[i];

        // 保存孔洞详细信息
        HoleResultData holeData;
        holeData.centerX = hole.center.x;
        holeData.centerY = hole.center.y;
        holeData.centerZ = hole.center.z;
        holeData.normalX = hole.normal.x;
        holeData.normalY = hole.normal.y;
        holeData.normalZ = hole.normal.z;
        holeData.radius = hole.radius;
        holeData.depth = hole.depth;
        holeData.eccentricity = hole.eccentricity;
        holeData.radiusVariance = hole.radiusVariance;
        holeData.angularSpan = hole.angularSpan;
        holeData.qualityScore = hole.qualityScore;
        detectionResult.holeDetails.push_back(holeData);

        // 六轴转换：位置+姿态（转换到机械臂坐标系）
        // 1. 位置转换：使用齐次变换矩阵进行坐标变换
        CTVec3D ptEye(hole.center.x, hole.center.y, hole.center.z);
        CTVec3D ptRobot = T_total.transformPoint(ptEye);

        // 2. 姿态转换：从法向量构建旋转矩阵
        // Z轴 = 法向量（归一化）
        CTVec3D Z_axis(hole.normal.x, hole.normal.y, hole.normal.z);
        double normZ = Z_axis.norm();
        if (normZ > 1e-6) {
            Z_axis = Z_axis * (1.0 / normZ);
        }

        // 选择 X 轴：使用上方向 (0,1,0) 叉乘 Z_axis
        CTVec3D up(0.0, 1.0, 0.0);
        CTVec3D X_axis = crossProduct(up, Z_axis);
        double normX = X_axis.norm();

        // 如果 up 和 Z_axis 平行，使用 (1,0,0) 作为备选
        if (normX < 1e-6) {
            up = CTVec3D(1.0, 0.0, 0.0);
            X_axis = crossProduct(up, Z_axis);
            normX = X_axis.norm();
        }

        if (normX > 1e-6) {
            X_axis = X_axis * (1.0 / normX);
        }

        // Y轴 = Z × X
        CTVec3D Y_axis = crossProduct(Z_axis, X_axis);

        // 构建方向向量列表（eye坐标系）
        CTVec3D dirVectors_eye[3];
        dirVectors_eye[0] = X_axis;
        dirVectors_eye[1] = Y_axis;
        dirVectors_eye[2] = Z_axis;

        // 根据配置决定方向向量反向
        switch (dirVectorInvert) {
            case DIR_INVERT_NONE:
                break;
            case DIR_INVERT_XY:
                dirVectors_eye[0] = dirVectors_eye[0] * (-1.0);
                dirVectors_eye[1] = dirVectors_eye[1] * (-1.0);
                break;
            case DIR_INVERT_XZ:
                dirVectors_eye[0] = dirVectors_eye[0] * (-1.0);
                dirVectors_eye[2] = dirVectors_eye[2] * (-1.0);
                break;
            case DIR_INVERT_YZ:
            default:
                dirVectors_eye[1] = dirVectors_eye[1] * (-1.0);
                dirVectors_eye[2] = dirVectors_eye[2] * (-1.0);
                break;
        }

        // 使用齐次变换矩阵对方向向量进行旋转变换（仅旋转，不平移）
        CTVec3D dirVectors_robot[3];
        for (int j = 0; j < 3; j++) {
            dirVectors_robot[j] = T_total.transformVector(dirVectors_eye[j]);
        }

        // 构建旋转矩阵
        CTRotationMatrix R_pose;
        R_pose.at(0, 0) = dirVectors_robot[0].x;
        R_pose.at(0, 1) = dirVectors_robot[1].x;
        R_pose.at(0, 2) = dirVectors_robot[2].x;
        R_pose.at(1, 0) = dirVectors_robot[0].y;
        R_pose.at(1, 1) = dirVectors_robot[1].y;
        R_pose.at(1, 2) = dirVectors_robot[2].y;
        R_pose.at(2, 0) = dirVectors_robot[0].z;
        R_pose.at(2, 1) = dirVectors_robot[1].z;
        R_pose.at(2, 2) = dirVectors_robot[2].z;

        // 使用 CCoordinateTransform::rotationMatrixToEuler 转换为欧拉角（外旋ZYX，即RZ-RY-RX）
        CTEulerAngles robotRpyRad = CCoordinateTransform::rotationMatrixToEuler(R_pose, CTEulerOrder::sZYX);
        double roll_deg, pitch_deg, yaw_deg;
        robotRpyRad.toDegrees(roll_deg, pitch_deg, yaw_deg);

        // 将机器人坐标系下的位姿添加到positions列表
        HolePosition centerRobotPos;
        centerRobotPos.x = ptRobot.x;
        centerRobotPos.y = ptRobot.y;
        centerRobotPos.z = ptRobot.z;
        centerRobotPos.roll  = roll_deg;
        centerRobotPos.pitch = pitch_deg;
        centerRobotPos.yaw   = yaw_deg;

        detectionResult.positions.push_back(centerRobotPos);

        // 保存可视化数据
        holeVisData.push_back(std::make_pair(hole.center, hole.radius));

        if (debugParam.enableDebug && debugParam.printDetailLog) {
            LOG_INFO("[Algo Thread] Hole[%d] Normal (eye): (%.3f, %.3f, %.3f)\n",
                    i, hole.normal.x, hole.normal.y, hole.normal.z);
            LOG_INFO("[Algo Thread] Hole[%d] Direction vectors (robot): X=(%.3f,%.3f,%.3f), Y=(%.3f,%.3f,%.3f), Z=(%.3f,%.3f,%.3f)\n",
                    i,
                    dirVectors_robot[0].x, dirVectors_robot[0].y, dirVectors_robot[0].z,
                    dirVectors_robot[1].x, dirVectors_robot[1].y, dirVectors_robot[1].z,
                    dirVectors_robot[2].x, dirVectors_robot[2].y, dirVectors_robot[2].z);
            LOG_INFO("[Algo Thread] Hole[%d] Center Eye   Coords: X=%.2f, Y=%.2f, Z=%.2f\n",
                    i, hole.center.x, hole.center.y, hole.center.z);
            LOG_INFO("[Algo Thread] Hole[%d] Center Robot Coords: X=%.2f, Y=%.2f, Z=%.2f, R=%.4f, P=%.4f, Y=%.4f\n",
                    i, ptRobot.x, ptRobot.y, ptRobot.z,
                    centerRobotPos.roll, centerRobotPos.pitch, centerRobotPos.yaw);
            LOG_INFO("[Algo Thread] Hole[%d] Radius=%.2f, Depth=%.2f, Quality=%.3f\n",
                    i, hole.radius, hole.depth, hole.qualityScore);
        }
    }

    // 释放算法结果内存
    FreeMultiHoleResult(&multiResult);

    // 从点云数据生成投影图像（热力图着色 + 孔位标记）
    {
        std::vector<HoleMarkerInfo> holes;
        for (const auto& vis : holeVisData) {
            HoleMarkerInfo h;
            h.center.x = vis.first.x;
            h.center.y = vis.first.y;
            h.center.z = vis.first.z;
            h.radius = vis.second;
            holes.push_back(h);
        }
        detectionResult.image = PointCloudImageUtils::GenerateHoleDetectionImage(xyzData, holes);
    }

    if (debugParam.enableDebug && debugParam.saveDebugImage) {
        std::string fileName = debugParam.debugOutputPath + "/Image_" + std::to_string(cameraIndex) + "_" + timeStamp + ".png";
        LOG_INFO("[Algo Thread] Debug image saved image : %s\n", fileName.c_str());

        if (!detectionResult.image.isNull()) {
            QString qFileName = QString::fromStdString(fileName);
            detectionResult.image.save(qFileName);
        } else {
            LOG_WARNING("[Algo Thread] No valid image to save for debug\n");
        }
    }

    // 释放网格数组
    delete[] points;

    return nRet;
}