GrabBag/App/JiuruiWorkpiecePose/JiuruiWorkpiecePoseApp/Presenter/Src/DetectPresenter.cpp

#include "DetectPresenter.h"
#include "AlgorithmParamConverter.h"
#include "PoseAxesBuilder.h"
#include "JiuruiWorkpiecePoseTCPProtocol.h"
#include "workpieceHolePositioning_Export.h"
#include <QColor>

namespace {

QImage BuildWorkpiecePointCloudImage(const std::vector<std::vector<SVzNL3DPosition>>& xyzData,
                                     const SSG_pointPose& pointPose,
                                     bool hasResult)
{
    PointCloudCanvas canvas = PointCloudCanvas::Create(xyzData);
    if (!canvas.isValid()) {
        return QImage();
    }

    if (hasResult) {
        constexpr double kAxisLineLength = 60.0;
        const QColor centerColor(0, 255, 0);        // 绿色：中心点
        const QColor colorX(255, 0, 0);              // 红色：X轴
        const QColor colorY(0, 255, 0);              // 绿色：Y轴
        const QColor colorZ(0, 0, 255);              // 蓝色：Z轴（法向）

        const SVzNL3DPoint& c = pointPose.point;
        const SVzNL3DPoint& xDir = pointPose.pose_x;
        const SVzNL3DPoint& yDir = pointPose.pose_y;
        const SVzNL3DPoint& zDir = pointPose.pose_z;

        // 绘制工件中心点
        canvas.drawPoint(c.x, c.y, centerColor, 10);

        // 绘制 X 轴方向线
        canvas.drawLine(c.x, c.y,
                        c.x + kAxisLineLength * xDir.x,
                        c.y + kAxisLineLength * xDir.y,
                        colorX, 2);
        canvas.drawText(static_cast<int>(c.x + kAxisLineLength * xDir.x),
                        static_cast<int>(c.y + kAxisLineLength * xDir.y),
                        QStringLiteral("X"), colorX, 14, 6, -6);

        // 绘制 Y 轴方向线
        canvas.drawLine(c.x, c.y,
                        c.x + kAxisLineLength * yDir.x,
                        c.y + kAxisLineLength * yDir.y,
                        colorY, 2);
        canvas.drawText(static_cast<int>(c.x + kAxisLineLength * yDir.x),
                        static_cast<int>(c.y + kAxisLineLength * yDir.y),
                        QStringLiteral("Y"), colorY, 14, 6, -6);

        // 绘制 Z 轴方向线
        canvas.drawLine(c.x, c.y,
                        c.x + kAxisLineLength * zDir.x,
                        c.y + kAxisLineLength * zDir.y,
                        colorZ, 2);
        canvas.drawText(static_cast<int>(c.x + kAxisLineLength * zDir.x),
                        static_cast<int>(c.y + kAxisLineLength * zDir.y),
                        QStringLiteral("Z"), colorZ, 14, 6, -6);
    }

    return canvas.image().copy();
}

QImage BuildWorkpiecePointCloudImage(const std::vector<std::vector<SVzNL3DPosition>>& xyzData,
                                     const std::vector<SSG_pointPose>& pointPoses)
{
    PointCloudCanvas canvas = PointCloudCanvas::Create(xyzData);
    if (!canvas.isValid()) {
        return QImage();
    }

    constexpr double kAxisLineLength = 60.0;
    const QColor centerColor(0, 255, 0);
    const QColor colorX(255, 0, 0);
    const QColor colorY(0, 255, 0);
    const QColor colorZ(0, 0, 255);

    for (size_t i = 0; i < pointPoses.size(); ++i) {
        const SSG_pointPose& pp = pointPoses[i];
        const SVzNL3DPoint& c = pp.point;
        const SVzNL3DPoint& xDir = pp.pose_x;
        const SVzNL3DPoint& yDir = pp.pose_y;
        const SVzNL3DPoint& zDir = pp.pose_z;

        canvas.drawPoint(c.x, c.y, centerColor, 10);

        canvas.drawLine(c.x, c.y,
                        c.x + kAxisLineLength * xDir.x,
                        c.y + kAxisLineLength * xDir.y,
                        colorX, 2);
        canvas.drawLine(c.x, c.y,
                        c.x + kAxisLineLength * yDir.x,
                        c.y + kAxisLineLength * yDir.y,
                        colorY, 2);
        canvas.drawLine(c.x, c.y,
                        c.x + kAxisLineLength * zDir.x,
                        c.y + kAxisLineLength * zDir.y,
                        colorZ, 2);

        canvas.drawText(static_cast<int>(c.x + kAxisLineLength * xDir.x),
                        static_cast<int>(c.y + kAxisLineLength * xDir.y),
                        QString("W%1").arg(static_cast<int>(i) + 1), colorX, 14, 6, -6);
    }

    return canvas.image().copy();
}

void SaveDebugImageIfNeeded(int cameraIndex,
                            const VrDebugParam& debugParam,
                            const QImage& image,
                            const QString& prefix)
{
    if (!debugParam.enableDebug || !debugParam.saveDebugImage || image.isNull()) {
        return;
    }

    const std::string timeStamp = CVrDateUtils::GetNowTime();
    const std::string fileName = debugParam.debugOutputPath + "/" +
                                 prefix.toStdString() + "_" + std::to_string(cameraIndex) + "_" + timeStamp + ".png";
    LOG_INFO("[Algo Thread] Debug image saved image : %s\n", fileName.c_str());
    image.save(QString::fromStdString(fileName));
}

} // namespace

DetectPresenter::DetectPresenter(/* args */)
{
    LOG_DEBUG("DetectPresenter Init algo ver: %s\n", wd_workpieceHolePositioningVersion());
}

DetectPresenter::~DetectPresenter()
{
}

QString DetectPresenter::GetAlgoVersion()
{
    return QString(wd_workpieceHolePositioningVersion());
}

int DetectPresenter::DetectWorkpiece(
    int cameraIndex,
    std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& laserLines,
    const VrAlgorithmParams& algorithmParams,
    const SSG_planeCalibPara& groundCalibParam,
    const VrDebugParam& debugParam,
    LaserDataLoader& dataLoader,
    const double clibMatrix[16],
    const RobotPose6D& robotPose,
    const HandEyeExtrinsic& extrinsic,
    int poseOutputOrder,
    DetectionResult& detectionResult)
{
    if (laserLines.empty()) {
        LOG_WARNING("No laser lines data available for workpiece detection\n");
        return ERR_CODE(DEV_DATA_INVALID);
    }

    std::vector<std::vector<SVzNL3DPosition>> xyzData;
    int convertResult = dataLoader.ConvertToSVzNL3DPosition(laserLines, xyzData);
    if (convertResult != SUCCESS || xyzData.empty()) {
        LOG_WARNING("Failed to convert workpiece data to XYZ format or no XYZ data available\n");
        return ERR_CODE(DEV_DATA_INVALID);
    }

    // 工件定位算法参数
    const JiuruiWorkpieceAlgorithmParams algoParams = AlgorithmParamConverter::ToJiuruiWorkpieceAlgorithmParams(algorithmParams);
    const SSG_cornerParam& cornerParam = algoParams.cornerParam;
    const WD_JiuruiWorkpieceParam& workpieceParam = algoParams.workpieceParam;

    if (debugParam.enableDebug && debugParam.printDetailLog) {
        LOG_INFO("[Algo Thread] JiuruiWorkpiece clibMatrix: \n\t[%.3f, %.3f, %.3f, %.3f] \n\t[%.3f, %.3f, %.3f, %.3f] \n\t[%.3f, %.3f, %.3f, %.3f] \n\t[%.3f, %.3f, %.3f, %.3f]\n",
            clibMatrix[0], clibMatrix[1], clibMatrix[2], clibMatrix[3],
            clibMatrix[4], clibMatrix[5], clibMatrix[6], clibMatrix[7],
            clibMatrix[8], clibMatrix[9], clibMatrix[10], clibMatrix[11],
            clibMatrix[12], clibMatrix[13], clibMatrix[14], clibMatrix[15]);

        LOG_INFO("[Algo Thread] JiuruiWorkpiece Corner: cornerTh=%.1f, scale=%.1f, minEndingGap=%.1f, minEndingGap_z=%.1f, jumpCornerTh_1=%.1f, jumpCornerTh_2=%.1f\n",
                 cornerParam.cornerTh, cornerParam.scale, cornerParam.minEndingGap,
                 cornerParam.minEndingGap_z, cornerParam.jumpCornerTh_1, cornerParam.jumpCornerTh_2);
        LOG_INFO("[Algo Thread] JiuruiWorkpiece Param: len=%.1f, width=%.1f, thickness=%.1f, planeHeight=%.3f\n",
                 workpieceParam.len, workpieceParam.width, workpieceParam.thickness, groundCalibParam.planeHeight);
        LOG_INFO("[Algo Thread] JiuruiWorkpiece Pose Config: eulerOrder=%d, poseOutputOrder=%d, rotX=%.3f, rotY=%.3f, rotZ=%.3f, outRotX=%.3f, outRotY=%.3f, outRotZ=%.3f\n",
                 extrinsic.eulerOrder, poseOutputOrder, extrinsic.rotX, extrinsic.rotY, extrinsic.rotZ,
                 extrinsic.outRotX, extrinsic.outRotY, extrinsic.outRotZ);

        LOG_INFO("[Algo Thread] JiuruiWorkpiece GroundCalib planeCalib:\n"
                 "\t[%.6f, %.6f, %.6f]\n"
                 "\t[%.6f, %.6f, %.6f]\n"
                 "\t[%.6f, %.6f, %.6f]\n",
                 groundCalibParam.planeCalib[0], groundCalibParam.planeCalib[1], groundCalibParam.planeCalib[2],
                 groundCalibParam.planeCalib[3], groundCalibParam.planeCalib[4], groundCalibParam.planeCalib[5],
                 groundCalibParam.planeCalib[6], groundCalibParam.planeCalib[7], groundCalibParam.planeCalib[8]);
        LOG_INFO("[Algo Thread] JiuruiWorkpiece GroundCalib planeHeight=%.6f\n", groundCalibParam.planeHeight);
        LOG_INFO("[Algo Thread] JiuruiWorkpiece GroundCalib invRMatrix:\n"
                 "\t[%.6f, %.6f, %.6f]\n"
                 "\t[%.6f, %.6f, %.6f]\n"
                 "\t[%.6f, %.6f, %.6f]\n",
                 groundCalibParam.invRMatrix[0], groundCalibParam.invRMatrix[1], groundCalibParam.invRMatrix[2],
                 groundCalibParam.invRMatrix[3], groundCalibParam.invRMatrix[4], groundCalibParam.invRMatrix[5],
                 groundCalibParam.invRMatrix[6], groundCalibParam.invRMatrix[7], groundCalibParam.invRMatrix[8]);
    }

    int errCode = 0;
    CVrTimeUtils oTimeUtils;

    LOG_DEBUG("before Jiurui_getWorkpiecePose\n");

    // 调用北京工件定位算法
    std::vector<SSG_pointPose> workpiecePositions;
    Jiurui_getWorkpiecePose(xyzData, cornerParam, groundCalibParam, workpieceParam, workpiecePositions, &errCode);

    LOG_DEBUG("after Jiurui_getWorkpiecePose\n");
    LOG_INFO("Jiurui_getWorkpiecePose: err=%d count=%zu runtime=%.3fms\n",
             errCode, workpiecePositions.size(), oTimeUtils.GetElapsedTimeInMilliSec());
    ERR_CODE_RETURN(errCode);

    detectionResult.success = true;
    detectionResult.errorCode = 0;
    detectionResult.message = QStringLiteral("工件检测成功");

    for (size_t i = 0; i < workpiecePositions.size(); ++i) {
        const SSG_pointPose& pointPose = workpiecePositions[i];
        double outX = 0.0;
        double outY = 0.0;
        double outZ = 0.0;
        double outRoll = 0.0;
        double outPitch = 0.0;
        double outYaw = 0.0;

        // 使用 PointPoseToEuler 从完整三轴姿态提取机器人位姿
        // （算法已返回完整的 pose_x / pose_y / pose_z 三轴坐标系，无需从法向量构造）
        if (!PoseAxesBuilder::PointPoseToEuler(
            pointPose,
            clibMatrix,
            extrinsic,
            robotPose,
            outX, outY, outZ,
            outRoll, outPitch, outYaw,
            poseOutputOrder)) {
            LOG_WARNING("[Algo Thread] JiuruiWorkpiece pose transform failed at index=%zu\n", i);
            detectionResult.success = false;
            detectionResult.errorCode = ERR_CODE(DEV_DATA_INVALID);
            detectionResult.message = QStringLiteral("工件姿态计算失败");
            return detectionResult.errorCode;
        }

        JiuruiWorkpiecePosition pos;
        pos.x = outX;
        pos.y = outY;
        pos.z = outZ;
        pos.roll = outRoll;
        pos.pitch = outPitch;
        pos.yaw = outYaw;
        detectionResult.positions.push_back(pos);

        // 填充工件原始信息（包含完整三轴姿态，用于调试显示）
        JiuruiWorkpieceInfo info;
        info.centerX = pointPose.point.x;
        info.centerY = pointPose.point.y;
        info.centerZ = pointPose.point.z;
        info.poseXx = pointPose.pose_x.x;
        info.poseXy = pointPose.pose_x.y;
        info.poseXz = pointPose.pose_x.z;
        info.poseYx = pointPose.pose_y.x;
        info.poseYy = pointPose.pose_y.y;
        info.poseYz = pointPose.pose_y.z;
        info.poseZx = pointPose.pose_z.x;
        info.poseZy = pointPose.pose_z.y;
        info.poseZz = pointPose.pose_z.z;
        detectionResult.workpieceInfoList.push_back(info);

        if (debugParam.enableDebug && debugParam.printDetailLog) {
            LOG_INFO("[Algo Thread] JiuruiWorkpiece[%zu] Eye Center: X=%.2f, Y=%.2f, Z=%.2f\n",
                     i, pointPose.point.x, pointPose.point.y, pointPose.point.z);
            LOG_INFO("[Algo Thread] JiuruiWorkpiece[%zu] Eye PoseX: (%.3f, %.3f, %.3f)\n",
                     i, pointPose.pose_x.x, pointPose.pose_x.y, pointPose.pose_x.z);
            LOG_INFO("[Algo Thread] JiuruiWorkpiece[%zu] Eye PoseY: (%.3f, %.3f, %.3f)\n",
                     i, pointPose.pose_y.x, pointPose.pose_y.y, pointPose.pose_y.z);
            LOG_INFO("[Algo Thread] JiuruiWorkpiece[%zu] Eye PoseZ: (%.3f, %.3f, %.3f)\n",
                     i, pointPose.pose_z.x, pointPose.pose_z.y, pointPose.pose_z.z);
            LOG_INFO("[Algo Thread] JiuruiWorkpiece[%zu] Robot Coords: X=%.2f, Y=%.2f, Z=%.2f, RPY=%.2f/%.2f/%.2f\n",
                     i, pos.x, pos.y, pos.z, pos.roll, pos.pitch, pos.yaw);
        }
    }

    detectionResult.image = BuildWorkpiecePointCloudImage(xyzData, workpiecePositions);

    SaveDebugImageIfNeeded(cameraIndex, debugParam, detectionResult.image, QStringLiteral("JiuruiWorkpiece_Image"));
    return SUCCESS;
}