319 lines
12 KiB
C++
319 lines
12 KiB
C++
|
#include "DetectPresenter.h"
|
|||
|
|
#include "BQ_workpieceCornerExtraction_Export.h"
|
|||
|
|
#include <fstream>
|
|||
|
|
|
|||
|
|
DetectPresenter::DetectPresenter(/* args */)
|
|||
|
|
{
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
DetectPresenter::~DetectPresenter()
|
|||
|
|
{
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
void vzReadLaserScanPointFromFile_XYZ_vector(const char* fileName, std::vector<std::vector< SVzNL3DPosition>>& scanData)
|
|||
|
|
{
|
|||
|
|
std::ifstream inputFile(fileName);
|
|||
|
|
std::string linedata;
|
|||
|
|
|
|||
|
|
if (inputFile.is_open() == false)
|
|||
|
|
return;
|
|||
|
|
|
|||
|
|
std::vector< SVzNL3DPosition> a_line;
|
|||
|
|
int ptIdx = 0;
|
|||
|
|
while (getline(inputFile, linedata))
|
|||
|
|
{
|
|||
|
|
if (0 == strncmp("Line_", linedata.c_str(), 5))
|
|||
|
|
{
|
|||
|
|
int ptSize = (int)a_line.size();
|
|||
|
|
if (ptSize > 0)
|
|||
|
|
{
|
|||
|
|
scanData.push_back(a_line);
|
|||
|
|
}
|
|||
|
|
a_line.clear();
|
|||
|
|
ptIdx = 0;
|
|||
|
|
}
|
|||
|
|
else if (0 == strncmp("{", linedata.c_str(), 1))
|
|||
|
|
{
|
|||
|
|
float X, Y, Z;
|
|||
|
|
int imageY = 0;
|
|||
|
|
float leftX, leftY;
|
|||
|
|
float rightX, rightY;
|
|||
|
|
sscanf_s(linedata.c_str(), "{%f,%f,%f}-{%f,%f}-{%f,%f}", &X, &Y, &Z, &leftX, &leftY, &rightX, &rightY);
|
|||
|
|
SVzNL3DPosition a_pt;
|
|||
|
|
a_pt.pt3D.x = X;
|
|||
|
|
a_pt.pt3D.y = Y;
|
|||
|
|
a_pt.pt3D.z = Z;
|
|||
|
|
a_pt.nPointIdx = ptIdx;
|
|||
|
|
ptIdx++;
|
|||
|
|
a_line.push_back(a_pt);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
//last line
|
|||
|
|
int ptSize = (int)a_line.size();
|
|||
|
|
if (ptSize > 0)
|
|||
|
|
{
|
|||
|
|
scanData.push_back(a_line);
|
|||
|
|
a_line.clear();
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
inputFile.close();
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
int DetectPresenter::DetectWorkpiece(
|
|||
|
|
int cameraIndex,
|
|||
|
|
std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& laserLines,
|
|||
|
|
const VrAlgorithmParams& algorithmParams,
|
|||
|
|
const VrDebugParam& debugParam,
|
|||
|
|
LaserDataLoader& dataLoader,
|
|||
|
|
const double clibMatrix[16],
|
|||
|
|
DetectionResult& detectionResult)
|
|||
|
|
{
|
|||
|
|
if (laserLines.empty()) {
|
|||
|
|
LOG_WARNING("No laser lines data available\n");
|
|||
|
|
return ERR_CODE(DEV_DATA_INVALID);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 获取当前相机的校准参数
|
|||
|
|
VrCameraPlaneCalibParam cameraCalibParamValue;
|
|||
|
|
const VrCameraPlaneCalibParam* cameraCalibParam = nullptr;
|
|||
|
|
if (algorithmParams.planeCalibParam.GetCameraCalibParam(cameraIndex, cameraCalibParamValue)) {
|
|||
|
|
cameraCalibParam = &cameraCalibParamValue;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 保存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);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
int nRet = SUCCESS;
|
|||
|
|
|
|||
|
|
// 转换为算法需要的XYZ格式
|
|||
|
|
std::vector<std::vector<SVzNL3DPosition>> xyzData;
|
|||
|
|
#if 0
|
|||
|
|
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);
|
|||
|
|
}
|
|||
|
|
#else
|
|||
|
|
vzReadLaserScanPointFromFile_XYZ_vector("C:\\project\\QT\\GrabBag\\TestData\\workpiece\\scanData_1_grid.txt", xyzData);
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
// 工件角点提取参数
|
|||
|
|
SSX_BQworkpiecePara bqWorkpieceParam;
|
|||
|
|
bqWorkpieceParam.lineLen = algorithmParams.workpieceParam.lineLen;
|
|||
|
|
|
|||
|
|
SSG_cornerParam cornerParam;
|
|||
|
|
cornerParam.minEndingGap = algorithmParams.cornerParam.minEndingGap;
|
|||
|
|
cornerParam.minEndingGap_z = algorithmParams.cornerParam.minEndingGap_z;
|
|||
|
|
cornerParam.scale = algorithmParams.cornerParam.scale;
|
|||
|
|
cornerParam.cornerTh = algorithmParams.cornerParam.cornerTh;
|
|||
|
|
cornerParam.jumpCornerTh_1 = algorithmParams.cornerParam.jumpCornerTh_1;
|
|||
|
|
cornerParam.jumpCornerTh_2 = algorithmParams.cornerParam.jumpCornerTh_2;
|
|||
|
|
|
|||
|
|
SSG_treeGrowParam growParam;
|
|||
|
|
growParam.yDeviation_max = algorithmParams.growParam.yDeviation_max;
|
|||
|
|
growParam.zDeviation_max = algorithmParams.growParam.zDeviation_max;
|
|||
|
|
growParam.maxLineSkipNum = algorithmParams.growParam.maxLineSkipNum;
|
|||
|
|
growParam.maxSkipDistance = algorithmParams.growParam.maxSkipDistance;
|
|||
|
|
growParam.minLTypeTreeLen = algorithmParams.growParam.minLTypeTreeLen;
|
|||
|
|
growParam.minVTypeTreeLen = algorithmParams.growParam.minVTypeTreeLen;
|
|||
|
|
|
|||
|
|
SSG_outlierFilterParam filterParam;
|
|||
|
|
filterParam.continuityTh = algorithmParams.filterParam.continuityTh;
|
|||
|
|
filterParam.outlierTh = algorithmParams.filterParam.outlierTh;
|
|||
|
|
|
|||
|
|
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] 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] BQ Workpiece: lineLen=%.1f\n", bqWorkpieceParam.lineLen);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 准备平面校准参数
|
|||
|
|
SSG_planeCalibPara groundCalibPara;
|
|||
|
|
if(cameraCalibParam){
|
|||
|
|
memcpy(groundCalibPara.planeCalib, cameraCalibParam->planeCalib, sizeof(double) * 9);
|
|||
|
|
groundCalibPara.planeHeight = cameraCalibParam->planeHeight;
|
|||
|
|
memcpy(groundCalibPara.invRMatrix, cameraCalibParam->invRMatrix, sizeof(double) * 9);
|
|||
|
|
} else {
|
|||
|
|
// 使用默认单位矩阵
|
|||
|
|
double identity[9] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
|
|||
|
|
memcpy(groundCalibPara.planeCalib, identity, sizeof(double) * 9);
|
|||
|
|
memcpy(groundCalibPara.invRMatrix, identity, sizeof(double) * 9);
|
|||
|
|
groundCalibPara.planeHeight = -1.0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(debugParam.enableDebug && debugParam.printDetailLog)
|
|||
|
|
{
|
|||
|
|
if(cameraCalibParam){
|
|||
|
|
LOG_INFO("Plane height: %.3f\n", cameraCalibParam->planeHeight);
|
|||
|
|
LOG_INFO(" Plane calibration matrix: [%f, %f, %f; %f, %f, %f; %f, %f, %f]\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]);
|
|||
|
|
|
|||
|
|
LOG_INFO(" Plane invRMatrix matrix: [%f, %f, %f; %f, %f, %f; %f, %f, %f]\n",
|
|||
|
|
cameraCalibParam->invRMatrix[0], cameraCalibParam->invRMatrix[1], cameraCalibParam->invRMatrix[2],
|
|||
|
|
cameraCalibParam->invRMatrix[3], cameraCalibParam->invRMatrix[4], cameraCalibParam->invRMatrix[5],
|
|||
|
|
cameraCalibParam->invRMatrix[6], cameraCalibParam->invRMatrix[7], cameraCalibParam->invRMatrix[8]);
|
|||
|
|
} else {
|
|||
|
|
LOG_WARNING("[Algo Thread] Camera calibration parameters not found for camera %d\n", cameraIndex);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 数据预处理：调平和去除地面（使用当前相机的调平参数）
|
|||
|
|
if(cameraCalibParam){
|
|||
|
|
LOG_DEBUG("Processing data with plane calibration\n");
|
|||
|
|
for(size_t i = 0; i < xyzData.size(); i++){
|
|||
|
|
sx_BQ_lineDataR(xyzData[i], cameraCalibParam->planeCalib, -1);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
int errCode = 0;
|
|||
|
|
CVrTimeUtils oTimeUtils;
|
|||
|
|
|
|||
|
|
LOG_DEBUG("before sx_BQ_getWorkpieceCorners \n");
|
|||
|
|
// 调用工件角点提取算法
|
|||
|
|
SSX_debugInfo debugContours[4];
|
|||
|
|
SSX_BQworkpieceResult bqResult = sx_BQ_getWorkpieceCorners(
|
|||
|
|
xyzData,
|
|||
|
|
cornerParam,
|
|||
|
|
filterParam,
|
|||
|
|
growParam,
|
|||
|
|
groundCalibPara,
|
|||
|
|
bqWorkpieceParam,
|
|||
|
|
#if _OUTPUT_DEBUG_DATA
|
|||
|
|
debugContours,
|
|||
|
|
#endif
|
|||
|
|
&errCode
|
|||
|
|
);
|
|||
|
|
LOG_DEBUG("after sx_BQ_getWorkpieceCorners \n");
|
|||
|
|
|
|||
|
|
LOG_INFO("sx_BQ_getWorkpieceCorners: workpieceType=%d err=%d runtime=%.3fms\n", bqResult.workpieceType, errCode, oTimeUtils.GetElapsedTimeInMilliSec());
|
|||
|
|
ERR_CODE_RETURN(errCode);
|
|||
|
|
|
|||
|
|
// 构建用于可视化的角点数组
|
|||
|
|
std::vector<std::vector<SVzNL3DPoint>> objOps;
|
|||
|
|
std::vector<SVzNL3DPoint> allCorners;
|
|||
|
|
|
|||
|
|
// 添加左侧角点
|
|||
|
|
for (int i = 0; i < 3; i++) {
|
|||
|
|
SVzNL3DPoint pt;
|
|||
|
|
pt.x = bqResult.corner_L[i].x;
|
|||
|
|
pt.y = bqResult.corner_L[i].y;
|
|||
|
|
pt.z = bqResult.corner_L[i].z;
|
|||
|
|
allCorners.push_back(pt);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 添加右侧角点
|
|||
|
|
for (int i = 0; i < 3; i++) {
|
|||
|
|
SVzNL3DPoint pt;
|
|||
|
|
pt.x = bqResult.corner_R[i].x;
|
|||
|
|
pt.y = bqResult.corner_R[i].y;
|
|||
|
|
pt.z = bqResult.corner_R[i].z;
|
|||
|
|
allCorners.push_back(pt);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 添加顶部角点
|
|||
|
|
for (int i = 0; i < 3; i++) {
|
|||
|
|
SVzNL3DPoint pt;
|
|||
|
|
pt.x = bqResult.corner_T[i].x;
|
|||
|
|
pt.y = bqResult.corner_T[i].y;
|
|||
|
|
pt.z = bqResult.corner_T[i].z;
|
|||
|
|
allCorners.push_back(pt);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 添加底部角点
|
|||
|
|
for (int i = 0; i < 3; i++) {
|
|||
|
|
SVzNL3DPoint pt;
|
|||
|
|
pt.x = bqResult.corner_B[i].x;
|
|||
|
|
pt.y = bqResult.corner_B[i].y;
|
|||
|
|
pt.z = bqResult.corner_B[i].z;
|
|||
|
|
allCorners.push_back(pt);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
objOps.push_back(allCorners);
|
|||
|
|
|
|||
|
|
// 从点云数据生成投影图像
|
|||
|
|
detectionResult.image = PointCloudImageUtils::GeneratePointCloudImage(xyzData, objOps);
|
|||
|
|
|
|||
|
|
// 转换检测结果为UI显示格式（使用机械臂坐标系数据）
|
|||
|
|
for (size_t i = 0; i < objOps.size(); i++) {
|
|||
|
|
|
|||
|
|
for(size_t j = 0; j < objOps[i].size(); j++){
|
|||
|
|
const SVzNL3DPoint& obj = objOps[i][j];
|
|||
|
|
|
|||
|
|
// 进行坐标转换：从算法坐标系转换到机械臂坐标系
|
|||
|
|
SVzNL3DPoint targetObj;
|
|||
|
|
targetObj.x = obj.x;
|
|||
|
|
targetObj.y = obj.y;
|
|||
|
|
targetObj.z = obj.z;
|
|||
|
|
|
|||
|
|
SVzNL3DPoint robotObj;
|
|||
|
|
CVrConvert::EyeToRobot(targetObj, robotObj, clibMatrix);
|
|||
|
|
|
|||
|
|
// 创建位置数据（使用转换后的机械臂坐标）
|
|||
|
|
WorkpiecePosition pos;
|
|||
|
|
pos.x = robotObj.x; // 机械臂坐标X
|
|||
|
|
pos.y = robotObj.y; // 机械臂坐标Y
|
|||
|
|
pos.z = robotObj.z; // 机械臂坐标Z
|
|||
|
|
pos.roll = 0.0; // 搭接焊缝检测不提供姿态角
|
|||
|
|
pos.pitch = 0.0; // 搭接焊缝检测不提供姿态角
|
|||
|
|
pos.yaw = 0.0; // 搭接焊缝检测不提供姿态角
|
|||
|
|
|
|||
|
|
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.x, obj.y, obj.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);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
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;
|
|||
|
|
}
|