234 lines
10 KiB
C++
234 lines
10 KiB
C++
#include "DetectPresenter.h"
|
|
#include "AlgorithmParamConverter.h"
|
|
#include "PoseAxesBuilder.h"
|
|
#include "TireHolePoseTCPProtocol.h"
|
|
#include "workpieceHolePositioning_Export.h"
|
|
#include <QColor>
|
|
|
|
namespace {
|
|
|
|
QImage BuildTireHolePointCloudImage(const std::vector<std::vector<SVzNL3DPosition>>& xyzData,
|
|
const WD_HolePositionInfo& holeInfo,
|
|
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(0, 180, 255); // 蓝色：法向
|
|
|
|
// 绘制孔中心点
|
|
canvas.drawPoint(holeInfo.center.x, holeInfo.center.y, centerColor, 10);
|
|
|
|
// 绘制法向量方向线
|
|
const double normalEndX = holeInfo.center.x + kNormalLineLength * holeInfo.normDir.x;
|
|
const double normalEndY = holeInfo.center.y + kNormalLineLength * holeInfo.normDir.y;
|
|
canvas.drawLine(holeInfo.center.x, holeInfo.center.y, normalEndX, normalEndY, normalColor, 2);
|
|
canvas.drawText(normalEndX, normalEndY, QStringLiteral("Z"), normalColor, 14, 6, -6);
|
|
}
|
|
|
|
return canvas.image().copy();
|
|
}
|
|
|
|
QImage BuildTireHolePointCloudImage(const std::vector<std::vector<SVzNL3DPosition>>& xyzData,
|
|
const std::vector<WD_HolePositionInfo>& holeInfos)
|
|
{
|
|
PointCloudCanvas canvas = PointCloudCanvas::Create(xyzData);
|
|
if (!canvas.isValid()) {
|
|
return QImage();
|
|
}
|
|
|
|
constexpr double kNormalLineLength = 60.0;
|
|
const QColor centerColor(0, 255, 0);
|
|
const QColor normalColor(0, 180, 255);
|
|
|
|
for (size_t i = 0; i < holeInfos.size(); ++i) {
|
|
const WD_HolePositionInfo& holeInfo = holeInfos[i];
|
|
canvas.drawPoint(holeInfo.center.x, holeInfo.center.y, centerColor, 10);
|
|
|
|
const double normalEndX = holeInfo.center.x + kNormalLineLength * holeInfo.normDir.x;
|
|
const double normalEndY = holeInfo.center.y + kNormalLineLength * holeInfo.normDir.y;
|
|
canvas.drawLine(holeInfo.center.x, holeInfo.center.y, normalEndX, normalEndY, normalColor, 2);
|
|
canvas.drawText(normalEndX, normalEndY, QString("T%1").arg(static_cast<int>(i) + 1), normalColor, 14, 6, -6);
|
|
}
|
|
|
|
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));
|
|
}
|
|
|
|
}
|
|
|
|
DetectPresenter::DetectPresenter(/* args */)
|
|
{
|
|
LOG_DEBUG("DetectPresenter Init algo ver: %s\n", wd_workpieceHolePositioningVersion());
|
|
}
|
|
|
|
DetectPresenter::~DetectPresenter()
|
|
{
|
|
}
|
|
|
|
QString DetectPresenter::GetAlgoVersion()
|
|
{
|
|
return QString(wd_workpieceHolePositioningVersion());
|
|
}
|
|
|
|
int DetectPresenter::DetectTireHole(
|
|
int cameraIndex,
|
|
std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& laserLines,
|
|
const VrAlgorithmParams& algorithmParams,
|
|
const SSG_planeCalibPara& groundCalibParam,
|
|
const VrDebugParam& debugParam,
|
|
LaserDataLoader& dataLoader,
|
|
const double clibMatrix[16],
|
|
const RobotPose6D& robotPose,
|
|
const HandEyeExtrinsic& extrinsic,
|
|
int poseOutputOrder,
|
|
DetectionResult& detectionResult)
|
|
{
|
|
if (laserLines.empty()) {
|
|
LOG_WARNING("No laser lines data available for tire hole 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 tire hole data to XYZ format or no XYZ data available\n");
|
|
return ERR_CODE(DEV_DATA_INVALID);
|
|
}
|
|
|
|
// 轮胎孔定位算法参数
|
|
const TireHoleAlgorithmParams algoParams = AlgorithmParamConverter::ToTireHoleAlgorithmParams(algorithmParams);
|
|
const SSG_cornerParam& cornerParam = algoParams.cornerParam;
|
|
const WD_tireParam& tireParam = algoParams.tireParam;
|
|
|
|
if (debugParam.enableDebug && debugParam.printDetailLog) {
|
|
LOG_INFO("[Algo Thread] TireHole 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] TireHole 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] TireHole Param: diameter=%.1f, thickness=%.1f, planeHeight=%.3f\n",
|
|
tireParam.diameter, tireParam.thickness, groundCalibParam.planeHeight);
|
|
LOG_INFO("[Algo Thread] TireHole Pose Config: eulerOrder=%d, poseOutputOrder=%d, rotX=%.3f, rotY=%.3f, rotZ=%.3f, outRotX=%.3f, outRotY=%.3f, outRotZ=%.3f\n",
|
|
extrinsic.eulerOrder, poseOutputOrder, extrinsic.rotX, extrinsic.rotY, extrinsic.rotZ,
|
|
extrinsic.outRotX, extrinsic.outRotY, extrinsic.outRotZ);
|
|
|
|
LOG_INFO("[Algo Thread] TireHole GroundCalib planeCalib:\n"
|
|
"\t[%.6f, %.6f, %.6f]\n"
|
|
"\t[%.6f, %.6f, %.6f]\n"
|
|
"\t[%.6f, %.6f, %.6f]\n",
|
|
groundCalibParam.planeCalib[0], groundCalibParam.planeCalib[1], groundCalibParam.planeCalib[2],
|
|
groundCalibParam.planeCalib[3], groundCalibParam.planeCalib[4], groundCalibParam.planeCalib[5],
|
|
groundCalibParam.planeCalib[6], groundCalibParam.planeCalib[7], groundCalibParam.planeCalib[8]);
|
|
LOG_INFO("[Algo Thread] TireHole GroundCalib planeHeight=%.6f\n", groundCalibParam.planeHeight);
|
|
LOG_INFO("[Algo Thread] TireHole GroundCalib invRMatrix:\n"
|
|
"\t[%.6f, %.6f, %.6f]\n"
|
|
"\t[%.6f, %.6f, %.6f]\n"
|
|
"\t[%.6f, %.6f, %.6f]\n",
|
|
groundCalibParam.invRMatrix[0], groundCalibParam.invRMatrix[1], groundCalibParam.invRMatrix[2],
|
|
groundCalibParam.invRMatrix[3], groundCalibParam.invRMatrix[4], groundCalibParam.invRMatrix[5],
|
|
groundCalibParam.invRMatrix[6], groundCalibParam.invRMatrix[7], groundCalibParam.invRMatrix[8]);
|
|
}
|
|
|
|
int errCode = 0;
|
|
CVrTimeUtils oTimeUtils;
|
|
|
|
LOG_DEBUG("before sx_getTireHolePose\n");
|
|
|
|
std::vector<WD_HolePositionInfo> tirePositions;
|
|
sx_getTireHolePose(xyzData, cornerParam, groundCalibParam, tireParam, tirePositions, &errCode);
|
|
|
|
LOG_DEBUG("after sx_getTireHolePose\n");
|
|
LOG_INFO("sx_getTireHolePose: err=%d count=%zu runtime=%.3fms\n",
|
|
errCode, tirePositions.size(), oTimeUtils.GetElapsedTimeInMilliSec());
|
|
ERR_CODE_RETURN(errCode);
|
|
|
|
detectionResult.success = true;
|
|
detectionResult.errorCode = 0;
|
|
detectionResult.message = QStringLiteral("轮胎检测成功");
|
|
|
|
for (size_t i = 0; i < tirePositions.size(); ++i) {
|
|
const WD_HolePositionInfo& holeInfo = tirePositions[i];
|
|
double outX = 0.0;
|
|
double outY = 0.0;
|
|
double outZ = 0.0;
|
|
double outRoll = 0.0;
|
|
double outPitch = 0.0;
|
|
double outYaw = 0.0;
|
|
|
|
if (!PoseAxesBuilder::NormalDirToPoseAngles(
|
|
holeInfo.center,
|
|
holeInfo.normDir,
|
|
clibMatrix,
|
|
extrinsic,
|
|
robotPose,
|
|
outX, outY, outZ,
|
|
outRoll, outPitch, outYaw,
|
|
poseOutputOrder)) {
|
|
LOG_WARNING("[Algo Thread] TireHole pose transform failed at index=%zu\n", i);
|
|
detectionResult.success = false;
|
|
detectionResult.errorCode = ERR_CODE(DEV_DATA_INVALID);
|
|
detectionResult.message = QStringLiteral("轮胎姿态计算失败");
|
|
return detectionResult.errorCode;
|
|
}
|
|
|
|
TireHolePosition pos;
|
|
pos.x = outX;
|
|
pos.y = outY;
|
|
pos.z = outZ;
|
|
pos.roll = outRoll;
|
|
pos.pitch = outPitch;
|
|
pos.yaw = outYaw;
|
|
detectionResult.positions.push_back(pos);
|
|
|
|
TireHoleInfo info;
|
|
info.centerX = holeInfo.center.x;
|
|
info.centerY = holeInfo.center.y;
|
|
info.centerZ = holeInfo.center.z;
|
|
info.normDirX = holeInfo.normDir.x;
|
|
info.normDirY = holeInfo.normDir.y;
|
|
info.normDirZ = holeInfo.normDir.z;
|
|
info.holeR = holeInfo.holeR;
|
|
detectionResult.tireHoleInfoList.push_back(info);
|
|
|
|
if (debugParam.enableDebug && debugParam.printDetailLog) {
|
|
LOG_INFO("[Algo Thread] TireHole[%zu] Eye Center: X=%.2f, Y=%.2f, Z=%.2f\n",
|
|
i, holeInfo.center.x, holeInfo.center.y, holeInfo.center.z);
|
|
LOG_INFO("[Algo Thread] TireHole[%zu] Eye NormDir: X=%.3f, Y=%.3f, Z=%.3f\n",
|
|
i, holeInfo.normDir.x, holeInfo.normDir.y, holeInfo.normDir.z);
|
|
LOG_INFO("[Algo Thread] TireHole[%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);
|
|
}
|
|
}
|
|
|
|
detectionResult.image = BuildTireHolePointCloudImage(xyzData, tirePositions);
|
|
|
|
SaveDebugImageIfNeeded(cameraIndex, debugParam, detectionResult.image, QStringLiteral("TireHole_Image"));
|
|
return SUCCESS;
|
|
}
|
|
|