GrabBag/App/ScrewPosition/ScrewPositionApp/Presenter/Src/DetectPresenter.cpp

#include "DetectPresenter.h"
#include "AlgorithmParamConverter.h"
#include "CoordinateTransform.h"
#include "ScrewPositionTCPProtocol.h"
#include "rodAndBarDetection_Export.h"
#include <QColor>
#include <array>
#include <cmath>

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

namespace {

constexpr double kVectorEpsilon = 1e-8;
constexpr int kDirInvertNone = 0;
constexpr int kDirInvertXY = 1;
constexpr int kDirInvertXZ = 2;
constexpr int kDirInvertYZ = 3;

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

    constexpr double kAxisLineLength = 60.0;
    const QColor pointColor(0, 255, 0);
    const QColor lineColor(255, 0, 0);
    const QColor textColor(255, 255, 0);

    for (size_t i = 0; i < screwInfo.size(); ++i) {
        const auto& screw = screwInfo[i];
        canvas.drawPoint(screw.center.x, screw.center.y, pointColor, 8);
        canvas.drawText(screw.center.x, screw.center.y, QString::number(i + 1), textColor, 16, 10, -10);
        canvas.drawLine(screw.center.x - kAxisLineLength * screw.axialDir.x,
                        screw.center.y - kAxisLineLength * screw.axialDir.y,
                        screw.center.x + kAxisLineLength * screw.axialDir.x,
                        screw.center.y + kAxisLineLength * screw.axialDir.y,
                        lineColor, 2);
    }

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

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

    if (hasResult) {
        constexpr double kNormalLineLength = 60.0;
        const QColor centerColor(0, 255, 0);
        const QColor normalColor(255, 0, 0);
        const QColor xAxisColor(255, 0, 0);
        const QColor yAxisColor(0, 255, 0);
        const QColor textColor(255, 255, 0);

        // 绘制定位盘中心点
        canvas.drawPoint(poseInfo.center.x, poseInfo.center.y, centerColor, 10);
        canvas.drawText(poseInfo.center.x, poseInfo.center.y,
                        QStringLiteral("center"), textColor, 14, 10, -10);

        // 绘制法向量方向线
        canvas.drawLine(poseInfo.center.x,
                        poseInfo.center.y,
                        poseInfo.center.x + kNormalLineLength * poseInfo.normalDir.x,
                        poseInfo.center.y + kNormalLineLength * poseInfo.normalDir.y,
                        normalColor, 2);
        canvas.drawLine(poseInfo.center.x,
                        poseInfo.center.y,
                        poseInfo.center.x + kNormalLineLength * poseInfo.xDir.x,
                        poseInfo.center.y + kNormalLineLength * poseInfo.xDir.y,
                        xAxisColor, 2);
        canvas.drawLine(poseInfo.center.x,
                        poseInfo.center.y,
                        poseInfo.center.x + kNormalLineLength * poseInfo.yDir.x,
                        poseInfo.center.y + kNormalLineLength * poseInfo.yDir.y,
                        yAxisColor, 2);
    }

    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));
}

CTHomogeneousMatrix BuildHandEyeMatrix(const double clibMatrix[16])
{
    CTHomogeneousMatrix handEyeMatrix;
    for (int row = 0; row < 4; ++row) {
        for (int col = 0; col < 4; ++col) {
            handEyeMatrix.at(row, col) = clibMatrix[row * 4 + col];
        }
    }
    return handEyeMatrix;
}

CTRobotPoseConvention ResolveRobotPoseConvention(int poseOutputOrder)
{
    switch (poseOutputOrder) {
    case 1:
        return CTRobotPoseConvention::OrderedRxRzRy;
    case 2:
        return CTRobotPoseConvention::OrderedRyRxRz;
    case 3:
        return CTRobotPoseConvention::OrderedRyRzRx;
    case 4:
        return CTRobotPoseConvention::OrderedRzRxRy;
    case 5:
        return CTRobotPoseConvention::OrderedRzRyRx;
    case 0:
    default:
        return CTRobotPoseConvention::LegacyRxRyRz;
    }
}

double ClampUnit(double value)
{
    if (value > 1.0) {
        return 1.0;
    }
    if (value < -1.0) {
        return -1.0;
    }
    return value;
}

void RotationMatrixToConfiguredEulerDegrees(const CTRotationMatrix& rotation,
                                            CTEulerOrder order,
                                            double& rollDeg,
                                            double& pitchDeg,
                                            double& yawDeg)
{
    if (order == CTEulerOrder::sZYX) {
        // External Z-Y-X: R = Rz(yaw) * Ry(pitch) * Rx(roll)
        // Keep this decomposition aligned with the robot-side convention used in ScrewPosition.
        const double pitch = std::asin(ClampUnit(rotation.at(0, 2)));
        const double cosPitch = std::cos(pitch);
        constexpr double kSingularThreshold = 1e-6;

        double roll = 0.0;
        double yaw = 0.0;
        if (std::abs(cosPitch) > kSingularThreshold) {
            yaw = std::atan2(-rotation.at(0, 1) / cosPitch,
                             rotation.at(0, 0) / cosPitch);
            roll = std::atan2(rotation.at(1, 2) / cosPitch,
                              rotation.at(2, 2) / cosPitch);
        } else {
            // Near gimbal lock, keep yaw fixed and solve the residual X rotation.
            yaw = 0.0;
            roll = std::atan2(rotation.at(1, 0), rotation.at(1, 1));
        }

        rollDeg = roll * 180.0 / M_PI;
        pitchDeg = pitch * 180.0 / M_PI;
        yawDeg = yaw * 180.0 / M_PI;
        return;
    }

    const CTEulerAngles robotEuler = CCoordinateTransform::rotationMatrixToEuler(rotation, order);
    robotEuler.toDegrees(rollDeg, pitchDeg, yawDeg);
}

CTVec3D NormalizeVector(const CTVec3D& vector)
{
    const double length = vector.norm();
    if (length < kVectorEpsilon) {
        return CTVec3D();
    }
    return vector * (1.0 / length);
}

double DotProduct(const CTVec3D& a, const CTVec3D& b)
{
    return a.x * b.x + a.y * b.y + a.z * b.z;
}

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);
}

bool IsValidVector(const CTVec3D& vector)
{
    return vector.norm() >= kVectorEpsilon;
}

CTVec3D ToCTVec3D(const SVzNL3DPoint& point)
{
    return CTVec3D(point.x, point.y, point.z);
}

bool TryProjectedAxis(const CTVec3D& reference, const CTVec3D& primary, CTVec3D& axis)
{
    const CTVec3D candidate = NormalizeVector(reference - primary * DotProduct(reference, primary));
    if (!IsValidVector(candidate)) {
        return false;
    }
    axis = candidate;
    return true;
}

bool BuildFrameFromXAxis(const CTVec3D& primary, std::array<CTVec3D, 3>& axes)
{
    const CTVec3D xAxis = NormalizeVector(primary);
    if (!IsValidVector(xAxis)) {
        return false;
    }

    CTVec3D yAxis = NormalizeVector(CrossProduct(CTVec3D(0.0, 0.0, 1.0), xAxis));
    if (!IsValidVector(yAxis) &&
        !TryProjectedAxis(CTVec3D(0.0, 1.0, 0.0), xAxis, yAxis) &&
        !TryProjectedAxis(CTVec3D(1.0, 0.0, 0.0), xAxis, yAxis)) {
        return false;
    }

    const CTVec3D zAxis = NormalizeVector(CrossProduct(xAxis, yAxis));
    if (!IsValidVector(zAxis)) {
        return false;
    }

    axes = {xAxis, yAxis, zAxis};
    return true;
}

bool BuildFrameFromYAxis(const CTVec3D& primary, std::array<CTVec3D, 3>& axes)
{
    const CTVec3D yAxis = NormalizeVector(primary);
    if (!IsValidVector(yAxis)) {
        return false;
    }

    CTVec3D xAxis = NormalizeVector(CrossProduct(yAxis, CTVec3D(0.0, 0.0, 1.0)));
    if (!IsValidVector(xAxis) &&
        !TryProjectedAxis(CTVec3D(1.0, 0.0, 0.0), yAxis, xAxis) &&
        !TryProjectedAxis(CTVec3D(0.0, 1.0, 0.0), yAxis, xAxis)) {
        return false;
    }

    const CTVec3D zAxis = NormalizeVector(CrossProduct(xAxis, yAxis));
    if (!IsValidVector(zAxis)) {
        return false;
    }

    axes = {xAxis, yAxis, zAxis};
    return true;
}

void ApplyLongAxisDir(std::array<CTVec3D, 3>& axes, int longAxisDir)
{
    if (longAxisDir != 1) {
        return;
    }

    const CTVec3D xAxis = axes[0];
    const CTVec3D yAxis = axes[1];
    axes[0] = yAxis * (-1.0);
    axes[1] = xAxis;
}

bool BuildFrameFromZAxis(const CTVec3D& primary, std::array<CTVec3D, 3>& axes)
{
    const CTVec3D zAxis = NormalizeVector(primary);
    if (!IsValidVector(zAxis)) {
        return false;
    }

    CTVec3D xAxis = NormalizeVector(CrossProduct(CTVec3D(0.0, 1.0, 0.0), zAxis));
    if (!IsValidVector(xAxis) &&
        !TryProjectedAxis(CTVec3D(1.0, 0.0, 0.0), zAxis, xAxis) &&
        !TryProjectedAxis(CTVec3D(0.0, 0.0, 1.0), zAxis, xAxis)) {
        return false;
    }

    const CTVec3D yAxis = NormalizeVector(CrossProduct(zAxis, xAxis));
    if (!IsValidVector(yAxis)) {
        return false;
    }

    axes = {xAxis, yAxis, zAxis};
    return true;
}

bool BuildFrameFromXYAxes(const CTVec3D& xSeed,
                          const CTVec3D& ySeed,
                          const CTVec3D& zReference,
                          std::array<CTVec3D, 3>& axes)
{
    const CTVec3D xAxis = NormalizeVector(xSeed);
    if (!IsValidVector(xAxis)) {
        return false;
    }

    CTVec3D yAxis = NormalizeVector(ySeed - xAxis * DotProduct(ySeed, xAxis));
    if (!IsValidVector(yAxis)) {
        return false;
    }

    CTVec3D zAxis = NormalizeVector(CrossProduct(xAxis, yAxis));
    if (!IsValidVector(zAxis)) {
        return false;
    }

    if (IsValidVector(zReference) && DotProduct(zAxis, NormalizeVector(zReference)) < 0.0) {
        yAxis = yAxis * (-1.0);
        zAxis = zAxis * (-1.0);
    }

    axes = {xAxis, yAxis, zAxis};
    return true;
}

void ApplyDirVectorInvert(std::array<CTVec3D, 3>& axes, int dirVectorInvert)
{
    switch (dirVectorInvert) {
    case kDirInvertXY:
        axes[0] = axes[0] * (-1.0);
        axes[1] = axes[1] * (-1.0);
        break;
    case kDirInvertXZ:
        axes[0] = axes[0] * (-1.0);
        axes[2] = axes[2] * (-1.0);
        break;
    case kDirInvertYZ:
        axes[1] = axes[1] * (-1.0);
        axes[2] = axes[2] * (-1.0);
        break;
    case kDirInvertNone:
    default:
        break;
    }
}

bool TransformAxes(const std::array<CTVec3D, 3>& eyeAxes,
                   const CTHomogeneousMatrix& transform,
                   std::array<CTVec3D, 3>& robotAxes)
{
    for (size_t i = 0; i < eyeAxes.size(); ++i) {
        robotAxes[i] = NormalizeVector(transform.transformVector(eyeAxes[i]));
        if (!IsValidVector(robotAxes[i])) {
            return false;
        }
    }
    return true;
}

CTRotationMatrix BuildRotationMatrix(const std::array<CTVec3D, 3>& axes)
{
    CTRotationMatrix rotation;
    rotation.at(0, 0) = axes[0].x;
    rotation.at(0, 1) = axes[1].x;
    rotation.at(0, 2) = axes[2].x;
    rotation.at(1, 0) = axes[0].y;
    rotation.at(1, 1) = axes[1].y;
    rotation.at(1, 2) = axes[2].y;
    rotation.at(2, 0) = axes[0].z;
    rotation.at(2, 1) = axes[1].z;
    rotation.at(2, 2) = axes[2].z;
    return rotation;
}

void VectorToPitchYaw(const CTVec3D& direction, double& pitch, double& yaw)
{
    const double xyLen = std::sqrt(direction.x * direction.x + direction.y * direction.y);
    pitch = std::atan2(-direction.z, xyLen) * 180.0 / M_PI;
    yaw = std::atan2(direction.y, direction.x) * 180.0 / M_PI;
}

}

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

DetectPresenter::~DetectPresenter()
{
}

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

int DetectPresenter::DetectScrew(
    int cameraIndex,
    std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& laserLines,
    const VrAlgorithmParams& algorithmParams,
    const VrDebugParam& debugParam,
    LaserDataLoader& dataLoader,
    const double clibMatrix[16],
    const RobotPose6D& robotPose,
    int eulerOrder,
    int poseOutputOrder,
    int dirVectorInvert,
    int longAxisDir,
    DetectionResult& detectionResult)
{
    if (laserLines.empty()) {
        LOG_WARNING("No laser lines data available\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 data to XYZ format or no XYZ data available\n");
        return ERR_CODE(DEV_DATA_INVALID);
    }

    const ScrewDetectAlgorithmParams algoParams = AlgorithmParamConverter::ToScrewDetectAlgorithmParams(algorithmParams);
    const double rodDiameter = algoParams.rodDiameter;
    const bool isHorizonScan = algoParams.isHorizonScan;
    const SSG_cornerParam& cornerParam = algoParams.cornerParam;
    const SSG_treeGrowParam& growParam = algoParams.growParam;
    const SSG_outlierFilterParam& filterParam = algoParams.filterParam;

    if (debugParam.enableDebug && debugParam.printDetailLog) {
        LOG_INFO("[Algo Thread] 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] Screw: rodDiameter=%.1f, isHorizonScan=%s\n", rodDiameter, isHorizonScan ? "true" : "false");
        LOG_INFO("[Algo Thread] 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] Tree Grow: yDeviation_max=%.1f, zDeviation_max=%.1f, maxLineSkipNum=%d, maxSkipDistance=%.1f, minLTypeTreeLen=%.1f, minVTypeTreeLen=%.1f\n",
                 growParam.yDeviation_max, growParam.zDeviation_max, growParam.maxLineSkipNum,
                 growParam.maxSkipDistance, growParam.minLTypeTreeLen, growParam.minVTypeTreeLen);
        LOG_INFO("[Algo Thread] Filter: continuityTh=%.1f, outlierTh=%.1f\n", filterParam.continuityTh, filterParam.outlierTh);
        LOG_INFO("[Algo Thread] Pose Config: eulerOrder=%d, poseOutputOrder=%d, dirVectorInvert=%d, longAxisDir=%d\n",
                 eulerOrder, poseOutputOrder, dirVectorInvert, longAxisDir);
    }

    int errCode = 0;
    CVrTimeUtils oTimeUtils;

    LOG_DEBUG("before sx_hexHeadScrewMeasure \n");

    std::vector<SSX_rodPoseInfo> screwInfo;
    sx_hexHeadScrewMeasure(
        xyzData,
        isHorizonScan,
        cornerParam,
        filterParam,
        growParam,
        rodDiameter,
        screwInfo,
        &errCode);

    LOG_DEBUG("after sx_hexHeadScrewMeasure \n");
    LOG_INFO("sx_hexHeadScrewMeasure: detected %zu screws, err=%d runtime=%.3fms\n",
             screwInfo.size(), errCode, oTimeUtils.GetElapsedTimeInMilliSec());
    ERR_CODE_RETURN(errCode);

    detectionResult.success = true;
    detectionResult.errorCode = 0;
    detectionResult.message = QStringLiteral("\u68c0\u6d4b\u6210\u529f");
    detectionResult.image = BuildScrewPointCloudImage(xyzData, screwInfo);
    const CTHomogeneousMatrix handEyeMatrix = BuildHandEyeMatrix(clibMatrix);
    const CTEulerOrder order = static_cast<CTEulerOrder>(eulerOrder);
    const CTRobotPose flangePose = CTRobotPose::fromDegrees(robotPose.x, robotPose.y, robotPose.z, robotPose.rx, robotPose.ry, robotPose.rz);
    const CTRobotPoseConvention poseConvention = ResolveRobotPoseConvention(poseOutputOrder);
    const CTHomogeneousMatrix eyeInHandTransform = CCoordinateTransform::sixAxisEyeInHandBuildTransform(
            flangePose, order, handEyeMatrix, poseConvention);

    if (debugParam.enableDebug && debugParam.printDetailLog) {
        LOG_INFO("[Algo Thread] Robot flange pose fields: X=%.3f, Y=%.3f, Z=%.3f, A1=%.3f, A2=%.3f, A3=%.3f\n",
                 robotPose.x, robotPose.y, robotPose.z,
                 robotPose.rx, robotPose.ry, robotPose.rz);
    }

    for (size_t i = 0; i < screwInfo.size(); ++i) {
        const auto& screw = screwInfo[i];

        const CTVec3D robotCenter = eyeInHandTransform.transformPoint(ToCTVec3D(screw.center));
        const CTVec3D eyeAxialDir = NormalizeVector(ToCTVec3D(screw.axialDir));
        const CTVec3D robotAxialDir = NormalizeVector(eyeInHandTransform.transformVector(eyeAxialDir));

        ScrewPosition pos;
        pos.x = robotCenter.x;
        pos.y = robotCenter.y;
        pos.z = robotCenter.z;
        // 从轴向量计算欧拉角
        const bool useConfiguredPose = (dirVectorInvert != kDirInvertNone || longAxisDir != 0);
        bool frameReady = false;
        if (useConfiguredPose) {
            std::array<CTVec3D, 3> eyeAxes;
            frameReady = BuildFrameFromXAxis(eyeAxialDir, eyeAxes);

            if (frameReady) {
                ApplyLongAxisDir(eyeAxes, longAxisDir);
                ApplyDirVectorInvert(eyeAxes, dirVectorInvert);

                std::array<CTVec3D, 3> robotAxes;
                frameReady = TransformAxes(eyeAxes, eyeInHandTransform, robotAxes);
                if (frameReady) {
                    const CTRotationMatrix rotation = BuildRotationMatrix(robotAxes);
                    RotationMatrixToConfiguredEulerDegrees(rotation, order, pos.roll, pos.pitch, pos.yaw);
                }
            }
        }

        if (!frameReady && useConfiguredPose) {
            LOG_WARNING("[Algo Thread] Screw %zu failed to build configured pose frame, fallback to pitch/yaw only\n", i);
        }
        if (!frameReady) {
            pos.roll = 0.0;
            VectorToPitchYaw(robotAxialDir, pos.pitch, pos.yaw);
        }
        detectionResult.positions.push_back(pos);

        ScrewInfo info;
        info.centerX = robotCenter.x;
        info.centerY = robotCenter.y;
        info.centerZ = robotCenter.z;
        info.axialDirX = robotAxialDir.x;
        info.axialDirY = robotAxialDir.y;
        info.axialDirZ = robotAxialDir.z;
        info.rotateAngle = pos.roll;
        detectionResult.screwInfoList.push_back(info);

        if (debugParam.enableDebug && debugParam.printDetailLog) {
            LOG_INFO("[Algo Thread] Screw %zu Eye Coords: X=%.2f, Y=%.2f, Z=%.2f\n", i, screw.center.x, screw.center.y, screw.center.z);
            LOG_INFO("[Algo Thread] Screw %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);
            LOG_INFO("[Algo Thread] Screw %zu Axial Dir Eye: X=%.3f, Y=%.3f, Z=%.3f\n", i, screw.axialDir.x, screw.axialDir.y, screw.axialDir.z);
            LOG_INFO("[Algo Thread] Screw %zu Axial Dir Robot: X=%.3f, Y=%.3f, Z=%.3f\n", i, robotAxialDir.x, robotAxialDir.y, robotAxialDir.z);
        }
    }

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

int DetectPresenter::DetectToolDisk(
    int cameraIndex,
    std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& laserLines,
    const VrAlgorithmParams& algorithmParams,
    const VrDebugParam& debugParam,
    LaserDataLoader& dataLoader,
    const double clibMatrix[16],
    const RobotPose6D& robotPose,
    int eulerOrder,
    int poseOutputOrder,
    int dirVectorInvert,
    int longAxisDir,
    DetectionResult& detectionResult)
{
    if (laserLines.empty()) {
        LOG_WARNING("No laser lines data available for tool disk 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 tool disk data to XYZ format or no XYZ data available\n");
        return ERR_CODE(DEV_DATA_INVALID);
    }

    // 构造算法参数（与螺杆检测共享 cornerParam）
    const ScrewDetectAlgorithmParams algoParams = AlgorithmParamConverter::ToScrewDetectAlgorithmParams(algorithmParams);
    const SSG_cornerParam& cornerParam = algoParams.cornerParam;

    if (debugParam.enableDebug && debugParam.printDetailLog) {
        LOG_INFO("[Algo Thread] ToolDisk 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] ToolDisk Corner: cornerTh=%.1f, scale=%.1f, minEndingGap=%.1f, minEndingGap_z=%.1f\n",
                 cornerParam.cornerTh, cornerParam.scale, cornerParam.minEndingGap, cornerParam.minEndingGap_z);
        LOG_INFO("[Algo Thread] ToolDisk Pose Config: eulerOrder=%d, poseOutputOrder=%d, dirVectorInvert=%d, longAxisDir=%d\n",
                 eulerOrder, poseOutputOrder, dirVectorInvert, longAxisDir);
    }

    int errCode = 0;
    CVrTimeUtils oTimeUtils;

    LOG_DEBUG("before sx_getLocationPlatePose\n");

    SSX_pointPoseInfo poseInfo = sx_getLocationPlatePose(xyzData, cornerParam, &errCode);

    LOG_DEBUG("after sx_getLocationPlatePose\n");
    LOG_INFO("sx_getLocationPlatePose: err=%d runtime=%.3fms\n", errCode, oTimeUtils.GetElapsedTimeInMilliSec());
    ERR_CODE_RETURN(errCode);

    detectionResult.success = true;
    detectionResult.errorCode = 0;
    detectionResult.message = QStringLiteral("\u5de5\u5177\u76d8\u68c0\u6d4b\u6210\u529f");
    detectionResult.image = BuildToolDiskPointCloudImage(xyzData, poseInfo, true);
    const CTHomogeneousMatrix handEyeMatrix = BuildHandEyeMatrix(clibMatrix);
    const CTEulerOrder order = static_cast<CTEulerOrder>(eulerOrder);
    const CTRobotPose flangePose = CTRobotPose::fromDegrees(
        robotPose.x, robotPose.y, robotPose.z,
        robotPose.rx, robotPose.ry, robotPose.rz);
    const CTRobotPoseConvention poseConvention = ResolveRobotPoseConvention(poseOutputOrder);
    const CTHomogeneousMatrix eyeInHandTransform = CCoordinateTransform::sixAxisEyeInHandBuildTransform(
            flangePose, order, handEyeMatrix, poseConvention);

    // 将定位盘中心点通过手眼标定转换为机器人坐标
    const CTVec3D robotCenter = eyeInHandTransform.transformPoint(ToCTVec3D(poseInfo.center));
    const CTVec3D eyeXAxis = NormalizeVector(ToCTVec3D(poseInfo.xDir));
    const CTVec3D eyeYAxis = NormalizeVector(ToCTVec3D(poseInfo.yDir));
    const CTVec3D eyeNormalDir = NormalizeVector(ToCTVec3D(poseInfo.normalDir));
    const CTVec3D robotNormalDir = NormalizeVector(eyeInHandTransform.transformVector(eyeNormalDir));

    ScrewPosition pos;
    pos.x = robotCenter.x;
    pos.y = robotCenter.y;
    pos.z = robotCenter.z;
    double eyeRoll = 0.0;
    double eyePitch = 0.0;
    double eyeYaw = 0.0;
    bool eyeEulerReady = false;
    // 法向量转欧拉角：roll=0, pitch=atan2(-nz, sqrt(nx^2+ny^2)), yaw=atan2(ny, nx)
    const bool useConfiguredPose = true;
    bool frameReady = false;
    {
        std::array<CTVec3D, 3> eyeAxes;
        frameReady = BuildFrameFromXYAxes(eyeXAxis, eyeYAxis, eyeNormalDir, eyeAxes);
        if (!frameReady) {
            frameReady = BuildFrameFromZAxis(eyeNormalDir, eyeAxes);
        }
        if (frameReady) {
            ApplyLongAxisDir(eyeAxes, longAxisDir);
            ApplyDirVectorInvert(eyeAxes, dirVectorInvert);
            const CTRotationMatrix eyeRotation = BuildRotationMatrix(eyeAxes);
            RotationMatrixToConfiguredEulerDegrees(eyeRotation, order, eyeRoll, eyePitch, eyeYaw);
            eyeEulerReady = true;

            std::array<CTVec3D, 3> robotAxes;
            frameReady = TransformAxes(eyeAxes, eyeInHandTransform, robotAxes);
            if (frameReady) {
                const CTRotationMatrix rotation = BuildRotationMatrix(robotAxes);
                RotationMatrixToConfiguredEulerDegrees(rotation, order, pos.roll, pos.pitch, pos.yaw);
            }
        }
    }

    if (!frameReady) {
        LOG_WARNING("[Algo Thread] ToolDisk failed to build pose frame from algorithm axes, fallback to pitch/yaw only\n");
    }
    if (!frameReady) {
        pos.roll = 0.0;
        VectorToPitchYaw(robotNormalDir, pos.pitch, pos.yaw);
    }
    detectionResult.positions.push_back(pos);

    if (debugParam.enableDebug && debugParam.printDetailLog) {
        LOG_INFO("[Algo Thread] ToolDisk Eye Coords: X=%.2f, Y=%.2f, Z=%.2f\n", poseInfo.center.x, poseInfo.center.y, poseInfo.center.z);
        LOG_INFO("[Algo Thread] ToolDisk XDir Eye: X=%.3f, Y=%.3f, Z=%.3f\n", poseInfo.xDir.x, poseInfo.xDir.y, poseInfo.xDir.z);
        LOG_INFO("[Algo Thread] ToolDisk YDir Eye: X=%.3f, Y=%.3f, Z=%.3f\n", poseInfo.yDir.x, poseInfo.yDir.y, poseInfo.yDir.z);
        LOG_INFO("[Algo Thread] ToolDisk ZDir Eye: X=%.3f, Y=%.3f, Z=%.3f\n", poseInfo.normalDir.x, poseInfo.normalDir.y, poseInfo.normalDir.z);
        if (eyeEulerReady) {
            LOG_INFO("[Algo Thread] ToolDisk Euler Eye: Roll=%.2f, Pitch=%.2f, Yaw=%.2f\n",
                     eyeRoll, eyePitch, eyeYaw);
        } else {
            LOG_INFO("[Algo Thread] ToolDisk Euler Eye: unavailable\n");
        }
        LOG_INFO("[Algo Thread] ToolDisk Robot Coords: X=%.2f, Y=%.2f, Z=%.2f\n",  pos.x, pos.y, pos.z);
        LOG_INFO("[Algo Thread] ToolDisk Euler: Roll=%.2f, Pitch=%.2f, Yaw=%.2f\n", pos.roll, pos.pitch, pos.yaw);
    }

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