GrabBag/GrabBagApp/Presenter/Src/DetectPresenter.cpp

#include "DetectPresenter.h"


DetectPresenter::DetectPresenter(/* args */)
{
}

DetectPresenter::~DetectPresenter()
{
}

int DetectPresenter::DetectBag(std::vector<SVzNL3DLaserLine>& detectionDataCache,
    const SG_bagPositionParam& algoParam, 
    const SSG_planeCalibPara& cameraCalibParam, 
    const VrDebugParam& debugParam,
    LaserDataLoader& dataLoader,
    const double clibMatrix[16],
    DetectionResult& detectionResult)
{
    if (detectionDataCache.empty()) {
        LOG_WARNING("No cached detection data available\n");
        return ERR_CODE(DEV_DATA_INVALID);
    }

    // 2. 数据预处理：调平和去除地面（使用当前相机的调平参数）
    for (size_t i = 0; i < detectionDataCache.size(); i++) {
        sg_lineDataR(&detectionDataCache[i], cameraCalibParam.planeCalib, cameraCalibParam.planeHeight);
    }

    // 3. 调用算法检测接口（使用当前相机的调平参数）
    std::vector<SSG_peakRgnInfo> objOps;
    sg_getBagPosition(static_cast<SVzNL3DLaserLine*>(detectionDataCache.data()), detectionDataCache.size(), algoParam, cameraCalibParam, objOps);

    // 从点云数据生成投影图像
    detectionResult.image = PointCloudImageUtils::GeneratePointCloudImage(static_cast<SVzNL3DLaserLine*>(detectionDataCache.data()),
                                                     detectionDataCache.size(), objOps);
    
    // 转换检测结果为UI显示格式（使用机械臂坐标系数据）
    for (size_t i = 0; i < objOps.size(); i++) {
        const SSG_peakRgnInfo& obj = objOps[i];
        
        // 进行坐标转换：从算法坐标系转换到机械臂坐标系
        SVzNL3DPoint targetObj;
        targetObj.x = obj.centerPos.x;
        targetObj.y = obj.centerPos.y;
        targetObj.z = obj.centerPos.z;
        
        SVzNL3DPoint robotObj;
        CVrConvert::EyeToRobot(targetObj, robotObj, clibMatrix);
        
        // 创建位置数据（使用转换后的机械臂坐标）
        GrabBagPosition pos;
        pos.x       = robotObj.x;           // 机械臂坐标X
        pos.y       = robotObj.y;           // 机械臂坐标Y
        pos.z       = robotObj.z;           // 机械臂坐标Z
        pos.roll    = obj.centerPos.x_roll; // 保持原有姿态角
        pos.pitch   = obj.centerPos.y_pitch; // 保持原有姿态角
        pos.yaw     = obj.centerPos.z_yaw;  // 保持原有偏航角
        
        detectionResult.positions.push_back(pos);

        if(debugParam.enableDebug && debugParam.printDetailLog){
            LOG_INFO("[Algo Thread] Object %zu Eye Coords: X=%.2f, Y=%.2f, Z=%.2f\n", 
                 i, obj.centerPos.x, obj.centerPos.y, obj.centerPos.z);
            LOG_INFO("[Algo Thread] Object %zu Robot Coords: X=%.2f, Y=%.2f, Z=%.2f, Roll=%.2f, Pitch=%.2f, Yaw=%.2f\n", 
                     i, pos.x, pos.y, pos.z, pos.roll, pos.pitch, pos.yaw);
        }
        
    }

    return 0;
}


int DetectPresenter::DetectBag(std::vector<SVzNLXYZRGBDLaserLine>& detectionDataCache,
    const SG_bagPositionParam& algoParam, 
    const SSG_planeCalibPara& cameraCalibParam, 
    const VrDebugParam& debugParam,
    LaserDataLoader& dataLoader,
    const double clibMatrix[16],
    const SSG_hsvCmpParam& hsvCmpParam,
    const RGB& rgbColorPattern,
    const double frontColorTemplate[RGN_HIST_SIZE],
    const double backColorTemplate[RGN_HIST_SIZE],
    DetectionResult& detectionResult)
{

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

    // 2. 数据预处理：调平和去除地面（使用当前相机的调平参数）
    for (size_t i = 0; i < detectionDataCache.size(); i++) {
        sg_lineDataR_RGBD(&detectionDataCache[i], cameraCalibParam.planeCalib, cameraCalibParam.planeHeight);
    }
    
    
    // 3. 调用算法检测接口（使用当前相机的调平参数）
    std::vector<SSG_peakOrienRgnInfo> objOps;
    int nRet = 0;
    sg_getBagPositionAndOrientation(static_cast<SVzNLXYZRGBDLaserLine*>(detectionDataCache.data()), detectionDataCache.size(), 
                                    algoParam, cameraCalibParam, hsvCmpParam, rgbColorPattern,
                                    frontColorTemplate, backColorTemplate, objOps, &nRet);
    if(nRet != 0){
        LOG_ERROR("sg_getBagPositionAndOrientation failed, error code: %d\n", nRet);
        return nRet;
    }

    // 从点云数据生成投影图像
    detectionResult.image = PointCloudImageUtils::GeneratePointCloudImage(
                                            static_cast<SVzNLXYZRGBDLaserLine*>(detectionDataCache.data()),
                                            detectionDataCache.size(), objOps);
    
    // 转换检测结果为UI显示格式（使用机械臂坐标系数据）
    for (size_t i = 0; i < objOps.size(); i++) {
        const SSG_peakOrienRgnInfo& obj = objOps[i];
        
        // 进行坐标转换：从算法坐标系转换到机械臂坐标系
        SVzNL3DPoint targetObj;
        targetObj.x = obj.centerPos.x;
        targetObj.y = obj.centerPos.y;
        targetObj.z = obj.centerPos.z;
        
        SVzNL3DPoint robotObj;
        CVrConvert::EyeToRobot(targetObj, robotObj, clibMatrix);
        
        // 创建位置数据（使用转换后的机械臂坐标）
        GrabBagPosition pos;
        pos.x       = robotObj.x;           // 机械臂坐标X
        pos.y       = robotObj.y;           // 机械臂坐标Y
        pos.z       = robotObj.z;           // 机械臂坐标Z
        pos.roll    = obj.centerPos.x_roll; // 保持原有姿态角
        pos.pitch   = obj.centerPos.y_pitch; // 保持原有姿态角
        pos.yaw     = obj.centerPos.z_yaw;  // 保持原有偏航角
        
        detectionResult.positions.push_back(pos);

        if(debugParam.enableDebug && debugParam.printDetailLog){
            LOG_INFO("[Algo Thread] Object %zu Eye Coords: X=%.2f, Y=%.2f, Z=%.2f\n", 
                 i, obj.centerPos.x, obj.centerPos.y, obj.centerPos.z);
            LOG_INFO("[Algo Thread] Object %zu Robot Coords: X=%.2f, Y=%.2f, Z=%.2f, Roll=%.2f, Pitch=%.2f, Yaw=%.2f\n", 
                     i, pos.x, pos.y, pos.z, pos.roll, pos.pitch, pos.yaw);
        }
        
    }

    return 0;
}

// 新增：处理统一数据格式和项目类型的DetectBag函数
int DetectPresenter::DetectBag(
    int cameraIndex,
    std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& laserLines,
    ProjectType projectType,
    const SG_bagPositionParam& algoParam, 
    const SSG_planeCalibPara& cameraCalibParam, 
    const VrDebugParam& debugParam,
    LaserDataLoader& dataLoader,
    const double clibMatrix[16],
    const SSG_hsvCmpParam& hsvCmpParam,
    const RGB& rgbColorPattern,
    const double frontColorTemplate[RGN_HIST_SIZE],
    const double backColorTemplate[RGN_HIST_SIZE],
    DetectionResult& 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");
        
        // 获取当前时间戳，格式为YYYYMMDDHHMMSS
        std::string fileName = debugParam.debugOutputPath + "/Laserline_" + std::to_string(cameraIndex) + "_" + timeStamp + ".txt";
        
        // 直接使用统一格式保存数据
        dataLoader.SaveLaserScanData(fileName, laserLines, laserLines.size(), 0.0, 0, 0);
    }


    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]);
    
        // 1. 使用成员变量算法参数（已在初始化时从XML读取）
        LOG_INFO("[Algo Thread] Using algorithm parameters from XML configuration\n");
        LOG_INFO("  Bag: L=%.1f, W=%.1f, H=%.1f\n",algoParam.bagParam.bagL, algoParam.bagParam.bagW, algoParam.bagParam.bagH);
        LOG_INFO("  Filter: continuityTh=%.1f, outlierTh=%.1f\n", algoParam.filterParam.continuityTh, algoParam.filterParam.outlierTh);
    
        LOG_INFO("  Corner: minEndingGap=%.1f, minEndingGap_z=%.1f, scale=%.1f, cornerTh=%.1f, jumpCornerTh_1=%.1f, jumpCornerTh_2=%.1f\n",
                algoParam.cornerParam.minEndingGap, algoParam.cornerParam.minEndingGap_z, algoParam.cornerParam.scale, algoParam.cornerParam.cornerTh,
                algoParam.cornerParam.jumpCornerTh_1, algoParam.cornerParam.jumpCornerTh_2);
    
        LOG_INFO("  Grow: minVTypeTreeLen=%.1f, minLTypeTreeLen=%.1f yDeviation_max=%.1f, zDeviation_max=%.1f, maxLineSkipNum=%d, maxSkipDistance=%.1f\n", 
                algoParam.growParam.minVTypeTreeLen, algoParam.growParam.minLTypeTreeLen, 
                algoParam.growParam.yDeviation_max, algoParam.growParam.zDeviation_max,
                algoParam.growParam.maxLineSkipNum, algoParam.growParam.maxSkipDistance);
    
        LOG_INFO("  Plane height: %.3f\n", cameraCalibParam.planeHeight);
        LOG_INFO("  Plane calibration matrix: [%.3f, %.3f, %.3f; %.3f, %.3f, %.3f; %.3f, %.3f, %.3f]\n",
                cameraCalibParam.planeCalib[0], cameraCalibParam.planeCalib[1], cameraCalibParam.planeCalib[2],
                cameraCalibParam.planeCalib[3], cameraCalibParam.planeCalib[4], cameraCalibParam.planeCalib[5],
                cameraCalibParam.planeCalib[6], cameraCalibParam.planeCalib[7], cameraCalibParam.planeCalib[8]);
    }

    int nRet = SUCCESS;
    // 根据项目类型转换数据并进行算法调用
    if (projectType == ProjectType::DirectBag) {
        // RGBD项目：转换为RGBD格式进行算法调用
        std::vector<SVzNLXYZRGBDLaserLine> rgbdData;
        int convertResult = dataLoader.ConvertToSVzNLXYZRGBDLaserLine(laserLines, rgbdData);
        if (convertResult == SUCCESS && !rgbdData.empty()) {
            nRet = DetectBag(rgbdData, algoParam, cameraCalibParam, debugParam, dataLoader, clibMatrix, hsvCmpParam, rgbColorPattern, frontColorTemplate, backColorTemplate, detectionResult);
            
            // 清理RGBD内存
            dataLoader.FreeConvertedData(rgbdData);
            
        } else {
            LOG_WARNING("Failed to convert data to RGBD format or no RGBD data available\n");
            return ERR_CODE(DEV_DATA_INVALID);
        }
        
    } else {
        // 普通项目：转换为XYZ格式进行算法调用
        std::vector<SVzNL3DLaserLine> xyzData;
        int convertResult = dataLoader.ConvertToSVzNL3DLaserLine(laserLines, xyzData);
        if (convertResult == SUCCESS && !xyzData.empty()) {
            nRet = DetectBag(xyzData, algoParam, cameraCalibParam, debugParam, dataLoader, clibMatrix, detectionResult);
            // 清理XYZ内存
            dataLoader.FreeConvertedData(xyzData);
            
        } else {
            LOG_WARNING("Failed to convert data to XYZ format or no XYZ data available\n");
            return ERR_CODE(DEV_DATA_INVALID);
        }
    }

    if(debugParam.enableDebug && debugParam.saveDebugImage){
        // 获取当前时间戳，格式为YYYYMMDDHHMMSS
        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");
        }
    }

    return nRet;
}