259 lines
9.9 KiB
C++
259 lines
9.9 KiB
C++
#include "DetectPresenter.h"
|
||
#include "rodAndBarDetection_Export.h"
|
||
#include "AlgoParamConverter.h"
|
||
#include "IHandEyeCalib.h"
|
||
#include <fstream>
|
||
#include <memory>
|
||
#include <QColor>
|
||
|
||
namespace
|
||
{
|
||
|
||
HECEulerOrder ToHandEyeEulerOrder(int eulerOrder)
|
||
{
|
||
switch (eulerOrder) {
|
||
case 10: return HECEulerOrder::XYZ;
|
||
case 11: return HECEulerOrder::ZYX;
|
||
case 12: return HECEulerOrder::ZXY;
|
||
case 13: return HECEulerOrder::YXZ;
|
||
case 14: return HECEulerOrder::YZX;
|
||
case 15: return HECEulerOrder::XZY;
|
||
default:
|
||
LOG_WARNING("Unsupported euler order %d, fallback to 11(sZYX)\n", eulerOrder);
|
||
return HECEulerOrder::ZYX;
|
||
}
|
||
}
|
||
|
||
} // namespace
|
||
|
||
DetectPresenter::DetectPresenter(/* args */)
|
||
{
|
||
LOG_DEBUG("DetectPresenter Init algo ver: %s\n", wd_rodAndBarDetectionVersion());
|
||
}
|
||
|
||
DetectPresenter::~DetectPresenter()
|
||
{
|
||
}
|
||
|
||
QString DetectPresenter::GetAlgoVersion()
|
||
{
|
||
return QString(wd_rodAndBarDetectionVersion());
|
||
}
|
||
|
||
int DetectPresenter::DetectWeldSeam(
|
||
int cameraIndex,
|
||
std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& laserLines,
|
||
const VrAlgorithmParams& algorithmParams,
|
||
const VrDebugParam& debugParam,
|
||
LaserDataLoader& dataLoader,
|
||
const double clibMatrix[16],
|
||
int eulerOrder,
|
||
int dirVectorInvert,
|
||
int longAxisDir,
|
||
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保存点云已由 BasePresenter::DetectTask() 统一处理，此处不再重复保存
|
||
std::string timeStamp = CVrDateUtils::GetNowTime();
|
||
|
||
int nRet = SUCCESS;
|
||
|
||
// 转换为算法需要的XYZ格式
|
||
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);
|
||
}
|
||
|
||
// 使用 AlgoParamConverter 进行参数转换
|
||
SSX_rodParam rodParam = AlgoParamConverter::ToAlgoParam(algorithmParams.rodParam);
|
||
SSG_cornerParam cornerParam = AlgoParamConverter::ToAlgoParam(algorithmParams.cornerParam);
|
||
SSG_treeGrowParam growParam = AlgoParamConverter::ToAlgoParam(algorithmParams.growParam);
|
||
SSG_outlierFilterParam filterParam = AlgoParamConverter::ToAlgoParam(algorithmParams.filterParam);
|
||
|
||
// 构建平面标定参数
|
||
SSG_planeCalibPara poseCalibPara = AlgoParamConverter::ToAlgoPlaneCalibParam(cameraCalibParam);
|
||
|
||
SVzNLRangeD svznlRangeD; // = algorithmParams.weldSeamParam.weldSeanRange;
|
||
|
||
if(debugParam.enableDebug && debugParam.printDetailLog)
|
||
{
|
||
AlgoParamConverter::LogAlgoParams("[Algo Thread]", rodParam, cornerParam, filterParam, growParam, clibMatrix, algorithmParams.weldSeamParam.weldSeanRange);
|
||
}
|
||
|
||
int errCode = 0;
|
||
CVrTimeUtils oTimeUtils;
|
||
|
||
LOG_DEBUG("before sx_rebarWeldSeamPositioning \n");
|
||
|
||
// 调用钢筋焊缝定位算法
|
||
std::vector<SSX_weldSeamInfo> weldSeamInfo;
|
||
sx_rebarWeldSeamPositioning(
|
||
xyzData,
|
||
poseCalibPara,
|
||
cornerParam,
|
||
filterParam,
|
||
growParam,
|
||
rodParam,
|
||
svznlRangeD,
|
||
KeWD_WELD_CATEGORY_UNKNOWN,
|
||
weldSeamInfo,
|
||
&errCode);
|
||
|
||
LOG_DEBUG("after sx_rebarWeldSeamPositioning \n");
|
||
|
||
LOG_INFO("sx_rebarWeldSeamPositioning: detected %zu weld seams, err=%d runtime=%.3fms\n", weldSeamInfo.size(), errCode, oTimeUtils.GetElapsedTimeInMilliSec());
|
||
ERR_CODE_RETURN(errCode);
|
||
std::unique_ptr<IHandEyeCalib, decltype(&DestroyHandEyeCalibInstance)> handEyeCalib(
|
||
CreateHandEyeCalibInstance(),
|
||
DestroyHandEyeCalibInstance);
|
||
if (!handEyeCalib) {
|
||
LOG_ERROR("Failed to create HandEyeCalib instance\n");
|
||
return ERR_CODE(DEV_NOT_FIND);
|
||
}
|
||
|
||
const HECCalibResult calibResult = HECCalibResult::fromHomogeneousArray(clibMatrix);
|
||
const HECEulerOrder hecEulerOrder = ToHandEyeEulerOrder(eulerOrder);
|
||
|
||
// 使用 PointCloudCanvas 生成点云图像（灰色底图 + 焊缝中心/方向线标记）
|
||
{
|
||
PointCloudCanvas canvas = PointCloudCanvas::Create(xyzData);
|
||
|
||
for (size_t i = 0; i < weldSeamInfo.size(); i++) {
|
||
const auto& seam = weldSeamInfo[i];
|
||
|
||
// 根据焊缝类型选择不同颜色
|
||
QColor pointColor(255, 128, 0); // 默认橙色
|
||
switch (seam.weldType) {
|
||
case KeWD_WELD_POINT: pointColor = QColor(255, 255, 0); break; // 黄色=点焊
|
||
case KeWD_WELD_SEAM: pointColor = QColor(0, 255, 255); break; // 青色=焊缝
|
||
case KeWD_WELD_SEAM_GAP: pointColor = QColor(255, 0, 255); break; // 紫色=焊缝(间隙)
|
||
default: break;
|
||
}
|
||
|
||
// 绘制焊缝中心点
|
||
canvas.drawPoint(seam.center.x, seam.center.y, pointColor, 6);
|
||
|
||
// 绘制轴向方向线
|
||
double len = 60;
|
||
double ax0 = seam.center.x - len * seam.axialDir.x;
|
||
double ay0 = seam.center.y - len * seam.axialDir.y;
|
||
double ax1 = seam.center.x + len * seam.axialDir.x;
|
||
double ay1 = seam.center.y + len * seam.axialDir.y;
|
||
canvas.drawLine(ax0, ay0, ax1, ay1, pointColor, 2);
|
||
|
||
// 绘制起点到终点线段（绿色）
|
||
canvas.drawLine(seam.startPt.x, seam.startPt.y, seam.endPt.x, seam.endPt.y, QColor(0, 255, 0), 2);
|
||
|
||
// 中心点白色编号
|
||
canvas.drawText(seam.center.x, seam.center.y, QString("%1").arg(i + 1), Qt::white, 14);
|
||
|
||
// 绘制焊缝中心线采样点（白色小点）
|
||
// for (const auto& cp : seam.centerPts) {
|
||
// canvas.drawPoint(cp.x, cp.y, QColor(255, 255, 255), 2);
|
||
// }
|
||
}
|
||
|
||
detectionResult.image = canvas.image();
|
||
}
|
||
|
||
// 转换检测结果为UI显示格式（使用机械臂坐标系数据）
|
||
for (size_t i = 0; i < weldSeamInfo.size(); i++) {
|
||
const auto& seam = weldSeamInfo[i];
|
||
|
||
LOG_INFO("[Algo Thread] WeldSeam %zu Eye Center: X=%.2f, Y=%.2f, Z=%.2f\n", i, seam.center.x, seam.center.y, seam.center.z);
|
||
LOG_INFO("[Algo Thread] WeldSeam %zu Input X seed: [%.6f, %.6f, %.6f]\n", i, seam.axialDir.x, seam.axialDir.y, seam.axialDir.z);
|
||
LOG_INFO("[Algo Thread] WeldSeam %zu Input Z seed: [%.6f, %.6f, %.6f]\n", i, seam.normalDir.x, seam.normalDir.y, seam.normalDir.z);
|
||
|
||
HECPoseResult poseResult;
|
||
bool validPose = handEyeCalib->TransformPose(
|
||
calibResult,
|
||
HECPoint3D(seam.center.x, seam.center.y, seam.center.z),
|
||
HECPoint3D(seam.axialDir.x, seam.axialDir.y, seam.axialDir.z),
|
||
HECPoint3D(seam.normalDir.x, seam.normalDir.y, seam.normalDir.z),
|
||
dirVectorInvert,
|
||
hecEulerOrder,
|
||
longAxisDir == 1 ? HECLongAxisDir::AxisY : HECLongAxisDir::AxisX,
|
||
poseResult);
|
||
if (!validPose) {
|
||
LOG_WARNING("[Algo Thread] WeldSeam %zu has invalid axial/normal direction, use zero pose\n", i);
|
||
}
|
||
|
||
double rollDeg = 0.0, pitchDeg = 0.0, yawDeg = 0.0;
|
||
poseResult.angles.toDegrees(rollDeg, pitchDeg, yawDeg);
|
||
|
||
// 创建位置数据（使用转换后的机械臂坐标）
|
||
WeldSeamPosition pos;
|
||
pos.roll = rollDeg;
|
||
pos.pitch = pitchDeg;
|
||
pos.yaw = yawDeg;
|
||
pos.x = poseResult.position.x;
|
||
pos.y = poseResult.position.y;
|
||
pos.z = poseResult.position.z;
|
||
|
||
detectionResult.positions.push_back(pos);
|
||
|
||
// 保存焊缝信息
|
||
WeldSeamInfo info;
|
||
info.weldType = static_cast<int>(seam.weldType);
|
||
info.centerX = poseResult.position.x;
|
||
info.centerY = poseResult.position.y;
|
||
info.centerZ = poseResult.position.z;
|
||
info.axialDirX = seam.axialDir.x;
|
||
info.axialDirY = seam.axialDir.y;
|
||
info.axialDirZ = seam.axialDir.z;
|
||
info.normalDirX = seam.normalDir.x;
|
||
info.normalDirY = seam.normalDir.y;
|
||
info.normalDirZ = seam.normalDir.z;
|
||
info.startPtX = seam.startPt.x;
|
||
info.startPtY = seam.startPt.y;
|
||
info.startPtZ = seam.startPt.z;
|
||
info.endPtX = seam.endPt.x;
|
||
info.endPtY = seam.endPt.y;
|
||
info.endPtZ = seam.endPt.z;
|
||
|
||
// 复制焊缝中心线点和边缘点
|
||
for (const auto& cp : seam.centerPts) {
|
||
info.centerPts.push_back({cp.x, cp.y, cp.z});
|
||
}
|
||
for (const auto& ep : seam.edgePts_0) {
|
||
info.edgePts_0.push_back({ep.x, ep.y, ep.z});
|
||
}
|
||
for (const auto& ep : seam.edgePts_1) {
|
||
info.edgePts_1.push_back({ep.x, ep.y, ep.z});
|
||
}
|
||
|
||
detectionResult.weldSeamInfoList.push_back(info);
|
||
|
||
LOG_INFO("[Algo Thread] WeldSeam %zu Robot Pose: X=%.2f, Y=%.2f, Z=%.2f, Roll=%.6f, Pitch=%.6f, Yaw=%.6f (type=%d)\n", i, pos.x, pos.y, pos.z, pos.roll, pos.pitch, pos.yaw, info.weldType);
|
||
}
|
||
|
||
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;
|
||
}
|