473 lines
16 KiB
C++
473 lines
16 KiB
C++
#include "DetectPresenter.h"
|
||
#include "rodAndBarDetection_Export.h"
|
||
#include "AlgoParamConverter.h"
|
||
#include "IHandEyeCalib.h"
|
||
#include <cmath>
|
||
#include <fstream>
|
||
#include <memory>
|
||
#include <QBrush>
|
||
#include <QColor>
|
||
#include <QLineF>
|
||
#include <QPainter>
|
||
#include <QPolygonF>
|
||
|
||
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;
|
||
}
|
||
}
|
||
|
||
double NormalizeAngleDeg(double angle)
|
||
{
|
||
double normalized = std::fmod(angle + 180.0, 360.0);
|
||
if (normalized < 0.0) {
|
||
normalized += 360.0;
|
||
}
|
||
return normalized - 180.0;
|
||
}
|
||
|
||
bool IsFullAxialAngleRange(double rangeMin, double rangeMax)
|
||
{
|
||
if (!std::isfinite(rangeMin) || !std::isfinite(rangeMax)) {
|
||
return true;
|
||
}
|
||
return std::fabs(rangeMax - rangeMin) >= 359.999
|
||
|| (rangeMin <= -180.0 && rangeMax >= 180.0);
|
||
}
|
||
|
||
bool IsAngleInRange(double angle, double rangeMin, double rangeMax)
|
||
{
|
||
if (IsFullAxialAngleRange(rangeMin, rangeMax)) {
|
||
return true;
|
||
}
|
||
|
||
const double a = NormalizeAngleDeg(angle);
|
||
const double minAngle = NormalizeAngleDeg(rangeMin);
|
||
const double maxAngle = NormalizeAngleDeg(rangeMax);
|
||
constexpr double kEpsilon = 1e-9;
|
||
|
||
if (minAngle <= maxAngle) {
|
||
return a + kEpsilon >= minAngle && a - kEpsilon <= maxAngle;
|
||
}
|
||
return a + kEpsilon >= minAngle || a - kEpsilon <= maxAngle;
|
||
}
|
||
|
||
struct AxialDirectionAdjustment
|
||
{
|
||
HECPoint3D direction;
|
||
double inputAngle = 0.0;
|
||
double outputAngle = 0.0;
|
||
bool angleValid = false;
|
||
bool flipped = false;
|
||
};
|
||
|
||
AxialDirectionAdjustment AdjustAxialDirectionByRange(const HECPoint3D& axialDir,
|
||
double rangeMin,
|
||
double rangeMax)
|
||
{
|
||
AxialDirectionAdjustment result;
|
||
result.direction = axialDir;
|
||
|
||
const double xyNorm = std::hypot(axialDir.x, axialDir.y);
|
||
if (xyNorm < 1e-9 || IsFullAxialAngleRange(rangeMin, rangeMax)) {
|
||
return result;
|
||
}
|
||
|
||
result.angleValid = true;
|
||
result.inputAngle = NormalizeAngleDeg(std::atan2(axialDir.y, axialDir.x) * 180.0 / M_PI);
|
||
result.outputAngle = result.inputAngle;
|
||
|
||
if (IsAngleInRange(result.inputAngle, rangeMin, rangeMax)) {
|
||
return result;
|
||
}
|
||
|
||
const double flippedAngle = NormalizeAngleDeg(result.inputAngle + 180.0);
|
||
if (IsAngleInRange(flippedAngle, rangeMin, rangeMax)) {
|
||
result.direction = axialDir * -1.0;
|
||
result.outputAngle = flippedAngle;
|
||
result.flipped = true;
|
||
}
|
||
|
||
return result;
|
||
}
|
||
|
||
double DotProduct(const HECPoint3D& a, const HECPoint3D& b)
|
||
{
|
||
return a.x * b.x + a.y * b.y + a.z * b.z;
|
||
}
|
||
|
||
HECPoint3D CrossProduct(const HECPoint3D& a, const HECPoint3D& b)
|
||
{
|
||
return HECPoint3D(a.y * b.z - a.z * b.y,
|
||
a.z * b.x - a.x * b.z,
|
||
a.x * b.y - a.y * b.x);
|
||
}
|
||
|
||
bool BuildEyeAxes(const HECPoint3D& axialDir,
|
||
const HECPoint3D& normalDir,
|
||
HECPoint3D& xAxis,
|
||
HECPoint3D& yAxis,
|
||
HECPoint3D& zAxis)
|
||
{
|
||
xAxis = axialDir.normalized();
|
||
zAxis = normalDir.normalized();
|
||
if (xAxis.norm() < 1e-6 || zAxis.norm() < 1e-6) {
|
||
return false;
|
||
}
|
||
|
||
zAxis = (zAxis - xAxis * DotProduct(xAxis, zAxis)).normalized();
|
||
if (zAxis.norm() < 1e-6) {
|
||
return false;
|
||
}
|
||
|
||
yAxis = CrossProduct(zAxis, xAxis).normalized();
|
||
if (yAxis.norm() < 1e-6) {
|
||
return false;
|
||
}
|
||
|
||
zAxis = CrossProduct(xAxis, yAxis).normalized();
|
||
return zAxis.norm() >= 1e-6;
|
||
}
|
||
|
||
void ApplyToolRotationToEyeAxes(IHandEyeCalib& handEyeCalib,
|
||
HECEulerOrder eulerOrder,
|
||
double toolRotX,
|
||
double toolRotY,
|
||
double toolRotZ,
|
||
HECPoint3D& xAxis,
|
||
HECPoint3D& yAxis,
|
||
HECPoint3D& zAxis)
|
||
{
|
||
if (std::abs(toolRotX) < 1e-9 &&
|
||
std::abs(toolRotY) < 1e-9 &&
|
||
std::abs(toolRotZ) < 1e-9) {
|
||
return;
|
||
}
|
||
|
||
HECRotationMatrix toolRotation;
|
||
handEyeCalib.EulerToRotationMatrix(
|
||
HECEulerAngles::fromDegrees(toolRotX, toolRotY, toolRotZ),
|
||
eulerOrder,
|
||
toolRotation);
|
||
|
||
const HECPoint3D oldX = xAxis;
|
||
const HECPoint3D oldY = yAxis;
|
||
const HECPoint3D oldZ = zAxis;
|
||
|
||
xAxis = (oldX * toolRotation.at(0, 0)
|
||
+ oldY * toolRotation.at(1, 0)
|
||
+ oldZ * toolRotation.at(2, 0)).normalized();
|
||
yAxis = (oldX * toolRotation.at(0, 1)
|
||
+ oldY * toolRotation.at(1, 1)
|
||
+ oldZ * toolRotation.at(2, 1)).normalized();
|
||
zAxis = (oldX * toolRotation.at(0, 2)
|
||
+ oldY * toolRotation.at(1, 2)
|
||
+ oldZ * toolRotation.at(2, 2)).normalized();
|
||
}
|
||
|
||
void DrawCanvasArrow(PointCloudCanvas& canvas,
|
||
double startX,
|
||
double startY,
|
||
double endX,
|
||
double endY,
|
||
const QColor& color,
|
||
int penWidth)
|
||
{
|
||
if (!canvas.isValid()) {
|
||
return;
|
||
}
|
||
|
||
QLineF line(QPointF(canvas.project(startX, startY)),
|
||
QPointF(canvas.project(endX, endY)));
|
||
if (line.length() < 1.0) {
|
||
return;
|
||
}
|
||
|
||
QPainter painter(&canvas.image());
|
||
painter.setRenderHint(QPainter::Antialiasing);
|
||
painter.setPen(QPen(color, penWidth, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
|
||
painter.setBrush(QBrush(color));
|
||
painter.drawLine(line);
|
||
|
||
const double arrowSize = 14.0;
|
||
const double angle = std::atan2(line.dy(), line.dx());
|
||
const QPointF arrowP1 = line.p2() - QPointF(std::cos(angle - M_PI / 6.0) * arrowSize,
|
||
std::sin(angle - M_PI / 6.0) * arrowSize);
|
||
const QPointF arrowP2 = line.p2() - QPointF(std::cos(angle + M_PI / 6.0) * arrowSize,
|
||
std::sin(angle + M_PI / 6.0) * arrowSize);
|
||
|
||
QPolygonF arrowHead;
|
||
arrowHead << line.p2() << arrowP1 << arrowP2;
|
||
painter.drawPolygon(arrowHead);
|
||
}
|
||
|
||
} // 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::DetectRod(
|
||
int cameraIndex,
|
||
std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& laserLines,
|
||
const VrAlgorithmParams& algorithmParams,
|
||
const VrDebugParam& debugParam,
|
||
LaserDataLoader& dataLoader,
|
||
const double clibMatrix[16],
|
||
int handEyeEulerOrder,
|
||
double axialAngleMin,
|
||
double axialAngleMax,
|
||
double toolRotX,
|
||
double toolRotY,
|
||
double toolRotZ,
|
||
double toolOffsetX,
|
||
double toolOffsetY,
|
||
double toolOffsetZ,
|
||
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);
|
||
|
||
if(debugParam.enableDebug && debugParam.printDetailLog)
|
||
{
|
||
AlgoParamConverter::LogAlgoParams("[Algo Thread]", rodParam, cornerParam, filterParam, growParam, clibMatrix);
|
||
}
|
||
|
||
int errCode = 0;
|
||
CVrTimeUtils oTimeUtils;
|
||
|
||
LOG_DEBUG("before sx_rodPositioning \n");
|
||
|
||
// 调用棒材定位算法
|
||
std::vector<SSX_rodPositionInfo> rodInfo;
|
||
sx_rodPositioning(
|
||
xyzData,
|
||
poseCalibPara,
|
||
cornerParam,
|
||
filterParam,
|
||
growParam,
|
||
rodParam,
|
||
rodInfo,
|
||
&errCode);
|
||
|
||
LOG_DEBUG("after sx_rodPositioning \n");
|
||
|
||
LOG_INFO("sx_rodPositioning: detected %zu rods, err=%d runtime=%.3fms\n", rodInfo.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);
|
||
// Tool Euler order belongs to the hand-eye conversion chain, not the protocol A/B/C field order.
|
||
const HECEulerOrder hecEulerOrder = ToHandEyeEulerOrder(handEyeEulerOrder);
|
||
|
||
std::vector<AxialDirectionAdjustment> axialAdjustments;
|
||
axialAdjustments.reserve(rodInfo.size());
|
||
for (const auto& rod : rodInfo) {
|
||
axialAdjustments.push_back(AdjustAxialDirectionByRange(
|
||
HECPoint3D(rod.axialDir.x, rod.axialDir.y, rod.axialDir.z),
|
||
axialAngleMin,
|
||
axialAngleMax));
|
||
}
|
||
|
||
// 使用 PointCloudCanvas 生成点云图像（灰色底图 + 棒材中心/方向线标记）
|
||
{
|
||
PointCloudCanvas canvas = PointCloudCanvas::Create(xyzData);
|
||
|
||
for (size_t i = 0; i < rodInfo.size(); i++) {
|
||
const auto& rod = rodInfo[i];
|
||
const HECPoint3D& axialDir = axialAdjustments[i].direction;
|
||
|
||
// 绘制棒材中心点（红色）
|
||
canvas.drawPoint(rod.center.x, rod.center.y, QColor(255, 0, 0), 6);
|
||
|
||
// 绘制轴向箭头（黄色）
|
||
const double axialXYNorm = std::hypot(axialDir.x, axialDir.y);
|
||
if (axialXYNorm > 1e-9) {
|
||
const double axisUx = axialDir.x / axialXYNorm;
|
||
const double axisUy = axialDir.y / axialXYNorm;
|
||
const double axisHalfLen = std::max(60.0, algorithmParams.rodParam.rodLen * 0.25);
|
||
const double ax0 = rod.center.x - axisHalfLen * axisUx;
|
||
const double ay0 = rod.center.y - axisHalfLen * axisUy;
|
||
const double ax1 = rod.center.x + axisHalfLen * axisUx;
|
||
const double ay1 = rod.center.y + axisHalfLen * axisUy;
|
||
DrawCanvasArrow(canvas, ax0, ay0, ax1, ay1, QColor(255, 220, 0), 3);
|
||
}
|
||
|
||
// 绘制起点到终点线段（绿色）
|
||
canvas.drawLine(rod.startPt.x, rod.startPt.y, rod.endPt.x, rod.endPt.y, QColor(0, 255, 0), 2);
|
||
|
||
// 中心点白色编号
|
||
canvas.drawText(rod.center.x, rod.center.y, QString("%1").arg(i + 1), Qt::white, 14);
|
||
}
|
||
|
||
detectionResult.image = canvas.image();
|
||
}
|
||
|
||
// 转换检测结果为UI显示格式（使用机械臂坐标系数据）
|
||
for (size_t i = 0; i < rodInfo.size(); i++) {
|
||
const auto& rod = rodInfo[i];
|
||
const AxialDirectionAdjustment& axialAdjustment = axialAdjustments[i];
|
||
const HECPoint3D rawAxialDir(rod.axialDir.x, rod.axialDir.y, rod.axialDir.z);
|
||
const HECPoint3D adjustedAxialDir = axialAdjustment.direction;
|
||
const HECPoint3D normalDir(rod.normalDir.x, rod.normalDir.y, rod.normalDir.z);
|
||
|
||
HECPoint3D poseAxialDir = adjustedAxialDir;
|
||
HECPoint3D poseNormalDir = normalDir;
|
||
HECPoint3D poseYAxis;
|
||
if (BuildEyeAxes(adjustedAxialDir, normalDir, poseAxialDir, poseYAxis, poseNormalDir)) {
|
||
ApplyToolRotationToEyeAxes(
|
||
*handEyeCalib,
|
||
hecEulerOrder,
|
||
toolRotX,
|
||
toolRotY,
|
||
toolRotZ,
|
||
poseAxialDir,
|
||
poseYAxis,
|
||
poseNormalDir);
|
||
}
|
||
|
||
LOG_INFO("[Algo Thread] Rod %zu Eye Center: X=%.2f, Y=%.2f, Z=%.2f\n", i, rod.center.x, rod.center.y, rod.center.z);
|
||
LOG_INFO("[Algo Thread] Rod %zu Raw X seed: [%.6f, %.6f, %.6f]\n", i, rawAxialDir.x, rawAxialDir.y, rawAxialDir.z);
|
||
if (axialAdjustment.angleValid) {
|
||
LOG_INFO("[Algo Thread] Rod %zu Axial angle adjust: raw=%.3f, range=(%.3f, %.3f), output=%.3f, flipped=%d\n",
|
||
i,
|
||
axialAdjustment.inputAngle,
|
||
axialAngleMin,
|
||
axialAngleMax,
|
||
axialAdjustment.outputAngle,
|
||
axialAdjustment.flipped ? 1 : 0);
|
||
}
|
||
LOG_INFO("[Algo Thread] Rod %zu Input X seed: [%.6f, %.6f, %.6f]\n", i, poseAxialDir.x, poseAxialDir.y, poseAxialDir.z);
|
||
LOG_INFO("[Algo Thread] Rod %zu Input Z seed: [%.6f, %.6f, %.6f]\n", i, poseNormalDir.x, poseNormalDir.y, poseNormalDir.z);
|
||
|
||
HECPoseResult poseResult;
|
||
bool validPose = handEyeCalib->TransformPose(
|
||
calibResult,
|
||
HECPoint3D(rod.center.x, rod.center.y, rod.center.z),
|
||
poseAxialDir,
|
||
poseNormalDir,
|
||
0,
|
||
hecEulerOrder,
|
||
HECLongAxisDir::AxisX,
|
||
poseResult);
|
||
if (!validPose) {
|
||
LOG_WARNING("[Algo Thread] Rod %zu has invalid axial/normal direction, use zero pose\n", i);
|
||
}
|
||
|
||
poseResult.position.x += toolOffsetX;
|
||
poseResult.position.y += toolOffsetY;
|
||
poseResult.position.z += toolOffsetZ;
|
||
|
||
double rollDeg = 0.0, pitchDeg = 0.0, yawDeg = 0.0;
|
||
poseResult.angles.toDegrees(rollDeg, pitchDeg, yawDeg);
|
||
|
||
// 创建位置数据（使用转换后的机械臂坐标）
|
||
RodPosition 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);
|
||
|
||
// 保存棒材信息
|
||
RodInfo info;
|
||
info.centerX = poseResult.position.x;
|
||
info.centerY = poseResult.position.y;
|
||
info.centerZ = poseResult.position.z;
|
||
info.axialDirX = adjustedAxialDir.x;
|
||
info.axialDirY = adjustedAxialDir.y;
|
||
info.axialDirZ = adjustedAxialDir.z;
|
||
info.normalDirX = rod.normalDir.x;
|
||
info.normalDirY = rod.normalDir.y;
|
||
info.normalDirZ = rod.normalDir.z;
|
||
info.startPtX = rod.startPt.x;
|
||
info.startPtY = rod.startPt.y;
|
||
info.startPtZ = rod.startPt.z;
|
||
info.endPtX = rod.endPt.x;
|
||
info.endPtY = rod.endPt.y;
|
||
info.endPtZ = rod.endPt.z;
|
||
detectionResult.rodInfoList.push_back(info);
|
||
|
||
// Print key values for coordinate transform debugging
|
||
LOG_INFO("[Algo Thread] Rod %zu Robot Pose: X=%.2f, Y=%.2f, Z=%.2f, Roll=%.6f, Pitch=%.6f, Yaw=%.6f\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;
|
||
}
|