347 lines
14 KiB
C++
347 lines
14 KiB
C++
#include "DetectPresenter.h"
|
||
#include "rodAndBarDetection_Export.h"
|
||
#include <fstream>
|
||
#define _USE_MATH_DEFINES
|
||
#include <cmath>
|
||
#include <QPainter>
|
||
#include <QPen>
|
||
#include <QColor>
|
||
|
||
DetectPresenter::DetectPresenter(/* args */)
|
||
{
|
||
LOG_DEBUG("DetectPresenter Init algo ver: %s\n", wd_rodAndBarDetectionVersion());
|
||
}
|
||
|
||
DetectPresenter::~DetectPresenter()
|
||
{
|
||
}
|
||
|
||
|
||
int DetectPresenter::DetectRod(
|
||
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;
|
||
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);
|
||
}
|
||
|
||
// 棒材定位算法参数
|
||
SSX_rodParam rodParam;
|
||
rodParam.diameter = algorithmParams.rodParam.rodDiameter;
|
||
rodParam.len = algorithmParams.rodParam.rodLen;
|
||
|
||
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;
|
||
|
||
// 构建平面标定参数
|
||
SSG_planeCalibPara poseCalibPara;
|
||
double identityMatrix[9] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
|
||
for (int i = 0; i < 9; i++) {
|
||
poseCalibPara.planeCalib[i] = identityMatrix[i];
|
||
poseCalibPara.invRMatrix[i] = identityMatrix[i];
|
||
}
|
||
poseCalibPara.planeHeight = -1.0;
|
||
|
||
if (cameraCalibParam && cameraCalibParam->isCalibrated) {
|
||
for (int i = 0; i < 9; i++) {
|
||
poseCalibPara.planeCalib[i] = cameraCalibParam->planeCalib[i];
|
||
poseCalibPara.invRMatrix[i] = cameraCalibParam->invRMatrix[i];
|
||
}
|
||
poseCalibPara.planeHeight = cameraCalibParam->planeHeight;
|
||
}
|
||
|
||
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] Rod: diameter=%.1f, len=%.1f\n",
|
||
rodParam.diameter, rodParam.len);
|
||
|
||
// 打印拐角参数
|
||
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);
|
||
}
|
||
|
||
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);
|
||
|
||
// 构建检测结果：生成点云图像
|
||
// 1. 获取所有棒材的中心点用于可视化
|
||
std::vector<std::vector<SVzNL3DPoint>> objOps;
|
||
std::vector<SVzNL3DPoint> rodCenters;
|
||
|
||
for (const auto& rod : rodInfo) {
|
||
SVzNL3DPoint pt;
|
||
pt.x = rod.center.x;
|
||
pt.y = rod.center.y;
|
||
pt.z = rod.center.z;
|
||
rodCenters.push_back(pt);
|
||
}
|
||
if (!rodCenters.empty()) {
|
||
objOps.push_back(rodCenters);
|
||
}
|
||
|
||
// 从点云数据生成投影图像（10cm边界）
|
||
detectionResult.image = PointCloudImageUtils::GeneratePointCloudRetPointImage(xyzData, objOps, 100.0);
|
||
|
||
// 在图像上绘制棒材的轴向方向线
|
||
if (!detectionResult.image.isNull() && !rodInfo.empty()) {
|
||
QPainter painter(&detectionResult.image);
|
||
painter.setRenderHint(QPainter::Antialiasing);
|
||
|
||
// 计算点云范围（与PointCloudImageUtils相同的方式）
|
||
double xMin = 1e10, xMax = -1e10, yMin = 1e10, yMax = -1e10;
|
||
for (const auto& line : xyzData) {
|
||
for (const auto& pt : line) {
|
||
if (pt.pt3D.z < 1e-4) continue;
|
||
xMin = std::min(xMin, (double)pt.pt3D.x);
|
||
xMax = std::max(xMax, (double)pt.pt3D.x);
|
||
yMin = std::min(yMin, (double)pt.pt3D.y);
|
||
yMax = std::max(yMax, (double)pt.pt3D.y);
|
||
}
|
||
}
|
||
|
||
// 扩展边界（与GeneratePointCloudRetPointImage相同）
|
||
double margin = 100.0; // 10cm = 100mm
|
||
xMin -= margin;
|
||
xMax += margin;
|
||
yMin -= margin;
|
||
yMax += margin;
|
||
|
||
// 使用与GeneratePointCloudRetPointImage相同的参数
|
||
int imgRows = detectionResult.image.height();
|
||
int imgCols = detectionResult.image.width();
|
||
int x_skip = 50;
|
||
int y_skip = 50;
|
||
|
||
// 计算投影比例（与PointCloudImageUtils相同的方式）
|
||
double y_rows = (double)(imgRows - y_skip * 2);
|
||
double x_cols = (double)(imgCols - x_skip * 2);
|
||
double x_scale = (xMax - xMin) / x_cols;
|
||
double y_scale = (yMax - yMin) / y_rows;
|
||
|
||
// 使用统一的比例尺
|
||
double scale = std::max(x_scale, y_scale);
|
||
x_scale = scale;
|
||
y_scale = scale;
|
||
|
||
// 计算点云在图像中居中的偏移量（与PointCloudImageUtils一致）
|
||
double cloudWidth = (xMax - xMin) / scale;
|
||
double cloudHeight = (yMax - yMin) / scale;
|
||
int x_offset = x_skip + (int)((x_cols - cloudWidth) / 2);
|
||
int y_offset = y_skip + (int)((y_rows - cloudHeight) / 2);
|
||
|
||
// 转换3D坐标到图像坐标的lambda函数（使用居中偏移）
|
||
auto toImageCoord = [&](const SVzNL3DPoint& pt) -> QPointF {
|
||
int px = (int)((pt.x - xMin) / x_scale + x_offset);
|
||
int py = (int)((pt.y - yMin) / y_scale + y_offset);
|
||
return QPointF(px, py);
|
||
};
|
||
|
||
// 绘制棒材的轴向方向线
|
||
for (const auto& rod : rodInfo) {
|
||
// 绘制轴向方向线（红色）
|
||
double len = 60;
|
||
QPen axisPen(QColor(255, 0, 0), 2);
|
||
painter.setPen(axisPen);
|
||
|
||
SVzNL3DPoint pt0 = { rod.center.x - len * rod.axialDir.x,
|
||
rod.center.y - len * rod.axialDir.y,
|
||
rod.center.z - len * rod.axialDir.z };
|
||
SVzNL3DPoint pt1 = { rod.center.x + len * rod.axialDir.x,
|
||
rod.center.y + len * rod.axialDir.y,
|
||
rod.center.z + len * rod.axialDir.z };
|
||
|
||
QPointF imgPt0 = toImageCoord(pt0);
|
||
QPointF imgPt1 = toImageCoord(pt1);
|
||
painter.drawLine(imgPt0, imgPt1);
|
||
|
||
// 绘制起点到终点线段（绿色）
|
||
QPen segPen(QColor(0, 255, 0), 2);
|
||
painter.setPen(segPen);
|
||
|
||
SVzNL3DPoint startPt = { rod.startPt.x, rod.startPt.y, rod.startPt.z };
|
||
SVzNL3DPoint endPt = { rod.endPt.x, rod.endPt.y, rod.endPt.z };
|
||
QPointF imgStart = toImageCoord(startPt);
|
||
QPointF imgEnd = toImageCoord(endPt);
|
||
painter.drawLine(imgStart, imgEnd);
|
||
}
|
||
}
|
||
|
||
// 转换检测结果为UI显示格式（使用机械臂坐标系数据）
|
||
for (size_t i = 0; i < rodInfo.size(); i++) {
|
||
const auto& rod = rodInfo[i];
|
||
|
||
// 进行坐标转换：从算法坐标系转换到机械臂坐标系
|
||
SVzNL3DPoint targetObj;
|
||
targetObj.x = rod.center.x;
|
||
targetObj.y = rod.center.y;
|
||
targetObj.z = rod.center.z;
|
||
|
||
SVzNL3DPoint robotObj;
|
||
CVrConvert::EyeToRobot(targetObj, robotObj, clibMatrix);
|
||
|
||
// 创建位置数据（使用转换后的机械臂坐标）
|
||
RodPosition pos;
|
||
pos.x = robotObj.x; // 机械臂坐标X
|
||
pos.y = robotObj.y; // 机械臂坐标Y
|
||
pos.z = robotObj.z; // 机械臂坐标Z
|
||
// 从轴向方向和法向方向构建旋转矩阵，提取欧拉角 (ZYX外旋)
|
||
// X轴 = axialDir, Z轴 = normalDir, Y轴 = cross(Z, X)
|
||
double ax = rod.axialDir.x, ay = rod.axialDir.y, az = rod.axialDir.z;
|
||
double nx = rod.normalDir.x, ny = rod.normalDir.y, nz = rod.normalDir.z;
|
||
// Y = cross(normal, axial)
|
||
double yx = ny * az - nz * ay;
|
||
double yy = nz * ax - nx * az;
|
||
double yz = nx * ay - ny * ax;
|
||
// 旋转矩阵 R = [axialDir | Y | normalDir] (列向量)
|
||
// R[0][0]=ax, R[1][0]=ay, R[2][0]=az (第一列: X轴=axialDir)
|
||
// R[0][1]=yx, R[1][1]=yy, R[2][1]=yz (第二列: Y轴)
|
||
// R[0][2]=nx, R[1][2]=ny, R[2][2]=nz (第三列: Z轴=normalDir)
|
||
// ZYX外旋欧拉角提取:
|
||
// pitch = -asin(R[2][0])
|
||
// roll = atan2(R[2][1], R[2][2])
|
||
// yaw = atan2(R[1][0], R[0][0])
|
||
double sinPitch = -az; // R[2][0] = az
|
||
if (sinPitch > 1.0) sinPitch = 1.0;
|
||
if (sinPitch < -1.0) sinPitch = -1.0;
|
||
pos.pitch = asin(sinPitch) * 180.0 / M_PI;
|
||
pos.roll = atan2(yz, nz) * 180.0 / M_PI; // atan2(R[2][1], R[2][2])
|
||
pos.yaw = atan2(ay, ax) * 180.0 / M_PI; // atan2(R[1][0], R[0][0])
|
||
|
||
detectionResult.positions.push_back(pos);
|
||
|
||
// 保存棒材信息
|
||
RodInfo info;
|
||
info.centerX = robotObj.x;
|
||
info.centerY = robotObj.y;
|
||
info.centerZ = robotObj.z;
|
||
info.axialDirX = rod.axialDir.x;
|
||
info.axialDirY = rod.axialDir.y;
|
||
info.axialDirZ = rod.axialDir.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);
|
||
|
||
if(debugParam.enableDebug && debugParam.printDetailLog){
|
||
LOG_INFO("[Algo Thread] Rod %zu Eye Coords: X=%.2f, Y=%.2f, Z=%.2f\n",
|
||
i, rod.center.x, rod.center.y, rod.center.z);
|
||
LOG_INFO("[Algo Thread] Rod %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);
|
||
LOG_INFO("[Algo Thread] Rod %zu Axial Dir: X=%.3f, Y=%.3f, Z=%.3f\n",
|
||
i, rod.axialDir.x, rod.axialDir.y, rod.axialDir.z);
|
||
LOG_INFO("[Algo Thread] Rod %zu Normal Dir: X=%.3f, Y=%.3f, Z=%.3f\n",
|
||
i, rod.normalDir.x, rod.normalDir.y, rod.normalDir.z);
|
||
}
|
||
}
|
||
|
||
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;
|
||
}
|