209 lines
10 KiB
C++
209 lines
10 KiB
C++
|
#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);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(debugParam.savePointCloud){
|
|||
|
|
LOG_INFO("[Algo Thread] Debug mode is enabled\n");
|
|||
|
|
|
|||
|
|
// 获取当前时间戳，格式为YYYYMMDDHHMMSS
|
|||
|
|
std::string timeStamp = CVrDateUtils::GetNowTime();
|
|||
|
|
|
|||
|
|
std::string fileName = debugParam.debugOutputPath + "/pointCloud_" + timeStamp + ".txt";
|
|||
|
|
dataLoader.SaveLaserScanData(fileName, detectionDataCache, detectionDataCache.size(), 0.0, 0, 0);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 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=%d\n", algoParam.filterParam.continuityTh, algoParam.filterParam.outlierTh);
|
|||
|
|
|
|||
|
|
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]);
|
|||
|
|
|
|||
|
|
// 2. 数据预处理：调平和去除地面（使用当前相机的调平参数）
|
|||
|
|
for (size_t i = 0; i < detectionDataCache.size(); i++) {
|
|||
|
|
sg_lineDataR(&detectionDataCache[i], cameraCalibParam.planeCalib, cameraCalibParam.planeHeight);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
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]);
|
|||
|
|
|
|||
|
|
// 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);
|
|||
|
|
|
|||
|
|
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);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(debugParam.savePointCloud){
|
|||
|
|
LOG_INFO("[Algo Thread] Debug mode is enabled\n");
|
|||
|
|
|
|||
|
|
// 获取当前时间戳，格式为YYYYMMDDHHMMSS
|
|||
|
|
std::string timeStamp = CVrDateUtils::GetNowTime();
|
|||
|
|
|
|||
|
|
std::string fileName = debugParam.debugOutputPath + "/pointCloud_" + timeStamp + ".txt";
|
|||
|
|
dataLoader.SaveLaserScanData(fileName, detectionDataCache, detectionDataCache.size(), 0.0, 0, 0);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 2. 数据预处理：调平和去除地面（使用当前相机的调平参数）
|
|||
|
|
for (size_t i = 0; i < detectionDataCache.size(); i++) {
|
|||
|
|
sg_lineDataR_RGBD(&detectionDataCache[i], cameraCalibParam.planeCalib, cameraCalibParam.planeHeight);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
#if 0
|
|||
|
|
|
|||
|
|
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]);
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
// 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);
|
|||
|
|
|
|||
|
|
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;
|
|||
|
|
}
|