336 lines
14 KiB
C++
336 lines
14 KiB
C++
#include "DetectPresenter.h"
|
||
#include "workpieceHolePositioning_Export.h"
|
||
#include "SG_baseDataType.h"
|
||
#include <fstream>
|
||
#include "PointCloudImageUtils.h"
|
||
#include "CoordinateTransform.h"
|
||
#include "VrConvert.h"
|
||
#include "VrTimeUtils.h"
|
||
#include "VrDateUtils.h"
|
||
|
||
// 辅助函数：向量叉积
|
||
static CTVec3D crossProduct(const CTVec3D& a, const CTVec3D& b) {
|
||
return CTVec3D(
|
||
a.y * b.z - a.z * b.y,
|
||
a.z * b.x - a.x * b.z,
|
||
a.x * b.y - a.y * b.x
|
||
);
|
||
}
|
||
|
||
DetectPresenter::DetectPresenter()
|
||
{
|
||
LOG_DEBUG("DetectPresenter Init (HolePitPosition)\n");
|
||
}
|
||
|
||
DetectPresenter::~DetectPresenter()
|
||
{
|
||
}
|
||
|
||
std::string DetectPresenter::GetAlgoVersion() const
|
||
{
|
||
const char* ver = wd_workpieceHolePositioningVersion();
|
||
return ver ? std::string(ver) : std::string("unknown");
|
||
}
|
||
|
||
int DetectPresenter::DetectHoles(
|
||
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,
|
||
const RobotFlangePose& robotPose,
|
||
HoleDetectionResult& detectionResult)
|
||
{
|
||
if (laserLines.empty()) {
|
||
LOG_WARNING("No laser lines data available\n");
|
||
return ERR_CODE(DEV_DATA_INVALID);
|
||
}
|
||
|
||
// 保存debug数据
|
||
std::string timeStamp = CVrDateUtils::GetNowTime();
|
||
if (debugParam.enableDebug && debugParam.savePointCloud) {
|
||
LOG_INFO("[Algo Thread] Debug mode is enabled, saving point cloud data\n");
|
||
|
||
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格式（vector<vector<SVzNL3DPosition>>）
|
||
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);
|
||
}
|
||
|
||
LOG_INFO("[Algo Thread] Converted %zu scan lines for wd_HolePositioning\n", xyzData.size());
|
||
|
||
// ========== 参数转换：VrAlgorithmParams -> 算法结构体 ==========
|
||
|
||
// 线段参数
|
||
SSG_lineSegParam lineSegPara;
|
||
lineSegPara.distScale = algorithmParams.lineSegParam.distScale;
|
||
lineSegPara.segGapTh_y = algorithmParams.lineSegParam.segGapTh_y;
|
||
lineSegPara.segGapTh_z = algorithmParams.lineSegParam.segGapTh_z;
|
||
|
||
// 角点参数
|
||
SSG_cornerParam cornerParam;
|
||
cornerParam.cornerTh = algorithmParams.cornerParam.cornerTh;
|
||
cornerParam.scale = algorithmParams.cornerParam.scale;
|
||
cornerParam.minEndingGap = algorithmParams.cornerParam.minEndingGap;
|
||
cornerParam.minEndingGap_z = algorithmParams.cornerParam.minEndingGap_z;
|
||
cornerParam.jumpCornerTh_1 = algorithmParams.cornerParam.jumpCornerTh_1;
|
||
cornerParam.jumpCornerTh_2 = algorithmParams.cornerParam.jumpCornerTh_2;
|
||
|
||
// 噪声滤除参数
|
||
SSG_outlierFilterParam filterParam;
|
||
filterParam.continuityTh = algorithmParams.outlierFilterParam.continuityTh;
|
||
filterParam.outlierTh = algorithmParams.outlierFilterParam.outlierTh;
|
||
|
||
// 树生长参数
|
||
SSG_treeGrowParam growParam;
|
||
growParam.maxLineSkipNum = algorithmParams.treeGrowParam.maxLineSkipNum;
|
||
growParam.yDeviation_max = algorithmParams.treeGrowParam.yDeviation_max;
|
||
growParam.maxSkipDistance = algorithmParams.treeGrowParam.maxSkipDistance;
|
||
growParam.zDeviation_max = algorithmParams.treeGrowParam.zDeviation_max;
|
||
growParam.minLTypeTreeLen = algorithmParams.treeGrowParam.minLTypeTreeLen;
|
||
growParam.minVTypeTreeLen = algorithmParams.treeGrowParam.minVTypeTreeLen;
|
||
|
||
if (debugParam.enableDebug && debugParam.printDetailLog) {
|
||
LOG_INFO("[Algo Thread] lineSegPara: distScale=%.2f, segGapTh_y=%.2f, segGapTh_z=%.2f\n",
|
||
lineSegPara.distScale, lineSegPara.segGapTh_y, lineSegPara.segGapTh_z);
|
||
LOG_INFO("[Algo Thread] cornerParam: cornerTh=%.2f, scale=%.2f, minEndingGap=%.2f, minEndingGap_z=%.2f, jumpCornerTh_1=%.2f, jumpCornerTh_2=%.2f\n",
|
||
cornerParam.cornerTh, cornerParam.scale, cornerParam.minEndingGap, cornerParam.minEndingGap_z,
|
||
cornerParam.jumpCornerTh_1, cornerParam.jumpCornerTh_2);
|
||
LOG_INFO("[Algo Thread] filterParam: continuityTh=%.2f, outlierTh=%.2f\n",
|
||
filterParam.continuityTh, filterParam.outlierTh);
|
||
LOG_INFO("[Algo Thread] growParam: maxLineSkipNum=%d, yDeviation_max=%.2f, maxSkipDistance=%.2f, zDeviation_max=%.2f, minLTypeTreeLen=%.2f, minVTypeTreeLen=%.2f\n",
|
||
growParam.maxLineSkipNum, growParam.yDeviation_max, growParam.maxSkipDistance,
|
||
growParam.zDeviation_max, growParam.minLTypeTreeLen, growParam.minVTypeTreeLen);
|
||
|
||
// 打印手眼标定矩阵
|
||
LOG_INFO("[Algo Thread] Hand-Eye Calibration Matrix:\n");
|
||
for (int r = 0; r < 4; ++r) {
|
||
LOG_INFO(" [%.4f, %.4f, %.4f, %.4f]\n",
|
||
clibMatrix[r * 4 + 0], clibMatrix[r * 4 + 1], clibMatrix[r * 4 + 2], clibMatrix[r * 4 + 3]);
|
||
}
|
||
}
|
||
|
||
CVrTimeUtils oTimeUtils;
|
||
|
||
LOG_DEBUG("before wd_HolePositioning\n");
|
||
|
||
// ========== 调用 wd_HolePositioning 算法 ==========
|
||
std::vector<WD_HolePositionInfo> holePositioning;
|
||
int errCode = 0;
|
||
wd_HolePositioning(xyzData, lineSegPara, cornerParam, filterParam, growParam, holePositioning, &errCode);
|
||
|
||
LOG_DEBUG("after wd_HolePositioning\n");
|
||
|
||
LOG_INFO("wd_HolePositioning: found %zu holes, err=%d runtime=%.3fms\n",
|
||
holePositioning.size(), errCode, oTimeUtils.GetElapsedTimeInMilliSec());
|
||
|
||
// 记录统计信息到检测结果
|
||
detectionResult.totalCandidates = static_cast<int>(holePositioning.size());
|
||
detectionResult.filteredCount = 0;
|
||
|
||
if (errCode != 0) {
|
||
LOG_ERROR("wd_HolePositioning failed with error code: %d\n", errCode);
|
||
return errCode;
|
||
}
|
||
|
||
// ========== 构建手眼标定齐次变换矩阵 ==========
|
||
|
||
// T_end_to_cam（从4x4 clibMatrix数组）
|
||
CTHomogeneousMatrix handEyeHM;
|
||
for (int i = 0; i < 4; i++) {
|
||
for (int j = 0; j < 4; j++) {
|
||
handEyeHM.at(i, j) = clibMatrix[i * 4 + j];
|
||
}
|
||
}
|
||
|
||
// 构建机器人法兰位姿齐次变换矩阵 T_base_to_end
|
||
CTRobotPose ctRobotPose = CTRobotPose::fromDegrees(
|
||
robotPose.x, robotPose.y, robotPose.z,
|
||
robotPose.roll, robotPose.pitch, robotPose.yaw);
|
||
CTHomogeneousMatrix T_robot = ctRobotPose.toHomogeneousMatrix();
|
||
|
||
// 眼在手上：T_total = T_base_to_end * T_end_to_cam
|
||
CTHomogeneousMatrix T_total = T_robot * handEyeHM;
|
||
|
||
// 构建用于可视化的孔洞中心点列表
|
||
std::vector<std::pair<SVzNL3DPointF, float>> holeVisData; // center + radius
|
||
|
||
// ========== 处理每个检测到的孔洞 ==========
|
||
for (size_t i = 0; i < holePositioning.size(); i++) {
|
||
const WD_HolePositionInfo& holeInfo = holePositioning[i];
|
||
|
||
// 保存孔洞详细信息（center和normDir来自算法结果）
|
||
HoleResultData holeData;
|
||
holeData.centerX = static_cast<float>(holeInfo.center.x);
|
||
holeData.centerY = static_cast<float>(holeInfo.center.y);
|
||
holeData.centerZ = static_cast<float>(holeInfo.center.z);
|
||
holeData.normalX = static_cast<float>(holeInfo.normDir.x);
|
||
holeData.normalY = static_cast<float>(holeInfo.normDir.y);
|
||
holeData.normalZ = static_cast<float>(holeInfo.normDir.z);
|
||
// wd_HolePositioning 不输出 radius/depth/eccentricity 等，设为0
|
||
holeData.radius = 0.0f;
|
||
holeData.depth = 0.0f;
|
||
holeData.eccentricity = 0.0f;
|
||
holeData.radiusVariance = 0.0f;
|
||
holeData.angularSpan = 0.0f;
|
||
holeData.qualityScore = 0.0f;
|
||
detectionResult.holeDetails.push_back(holeData);
|
||
|
||
// ========== 六轴转换：位置+姿态（转换到机械臂坐标系） ==========
|
||
|
||
// 1. 位置转换：使用齐次变换矩阵进行坐标变换
|
||
CTVec3D ptEye(holeInfo.center.x, holeInfo.center.y, holeInfo.center.z);
|
||
CTVec3D ptRobot = T_total.transformPoint(ptEye);
|
||
|
||
// 2. 姿态转换：从法向量构建旋转矩阵
|
||
// Z轴 = 法向量（归一化）
|
||
CTVec3D Z_axis(holeInfo.normDir.x, holeInfo.normDir.y, holeInfo.normDir.z);
|
||
double normZ = Z_axis.norm();
|
||
if (normZ > 1e-6) {
|
||
Z_axis = Z_axis * (1.0 / normZ);
|
||
}
|
||
|
||
// 选择 X 轴：使用上方向 (0,1,0) 叉乘 Z_axis
|
||
CTVec3D up(0.0, 1.0, 0.0);
|
||
CTVec3D X_axis = crossProduct(up, Z_axis);
|
||
double normX = X_axis.norm();
|
||
|
||
// 如果 up 和 Z_axis 平行，使用 (1,0,0) 作为备选
|
||
if (normX < 1e-6) {
|
||
up = CTVec3D(1.0, 0.0, 0.0);
|
||
X_axis = crossProduct(up, Z_axis);
|
||
normX = X_axis.norm();
|
||
}
|
||
|
||
if (normX > 1e-6) {
|
||
X_axis = X_axis * (1.0 / normX);
|
||
}
|
||
|
||
// Y轴 = Z x X
|
||
CTVec3D Y_axis = crossProduct(Z_axis, X_axis);
|
||
|
||
// 构建方向向量列表（eye坐标系）
|
||
CTVec3D dirVectors_eye[3];
|
||
dirVectors_eye[0] = X_axis;
|
||
dirVectors_eye[1] = Y_axis;
|
||
dirVectors_eye[2] = Z_axis;
|
||
|
||
// 根据配置决定方向向量反向
|
||
switch (dirVectorInvert) {
|
||
case DIR_INVERT_NONE:
|
||
break;
|
||
case DIR_INVERT_XY:
|
||
dirVectors_eye[0] = dirVectors_eye[0] * (-1.0);
|
||
dirVectors_eye[1] = dirVectors_eye[1] * (-1.0);
|
||
break;
|
||
case DIR_INVERT_XZ:
|
||
dirVectors_eye[0] = dirVectors_eye[0] * (-1.0);
|
||
dirVectors_eye[2] = dirVectors_eye[2] * (-1.0);
|
||
break;
|
||
case DIR_INVERT_YZ:
|
||
default:
|
||
dirVectors_eye[1] = dirVectors_eye[1] * (-1.0);
|
||
dirVectors_eye[2] = dirVectors_eye[2] * (-1.0);
|
||
break;
|
||
}
|
||
|
||
// 使用齐次变换矩阵对方向向量进行旋转变换（仅旋转，不平移）
|
||
CTVec3D dirVectors_robot[3];
|
||
for (int j = 0; j < 3; j++) {
|
||
dirVectors_robot[j] = T_total.transformVector(dirVectors_eye[j]);
|
||
}
|
||
|
||
// 构建旋转矩阵
|
||
CTRotationMatrix R_pose;
|
||
R_pose.at(0, 0) = dirVectors_robot[0].x;
|
||
R_pose.at(0, 1) = dirVectors_robot[1].x;
|
||
R_pose.at(0, 2) = dirVectors_robot[2].x;
|
||
R_pose.at(1, 0) = dirVectors_robot[0].y;
|
||
R_pose.at(1, 1) = dirVectors_robot[1].y;
|
||
R_pose.at(1, 2) = dirVectors_robot[2].y;
|
||
R_pose.at(2, 0) = dirVectors_robot[0].z;
|
||
R_pose.at(2, 1) = dirVectors_robot[1].z;
|
||
R_pose.at(2, 2) = dirVectors_robot[2].z;
|
||
|
||
// 使用 CCoordinateTransform::rotationMatrixToEuler 转换为欧拉角（外旋ZYX，即RZ-RY-RX）
|
||
CTEulerAngles robotRpyRad = CCoordinateTransform::rotationMatrixToEuler(R_pose, CTEulerOrder::sZYX);
|
||
double roll_deg, pitch_deg, yaw_deg;
|
||
robotRpyRad.toDegrees(roll_deg, pitch_deg, yaw_deg);
|
||
|
||
// 将机器人坐标系下的位姿添加到positions列表
|
||
HolePosition centerRobotPos;
|
||
centerRobotPos.x = ptRobot.x;
|
||
centerRobotPos.y = ptRobot.y;
|
||
centerRobotPos.z = ptRobot.z;
|
||
centerRobotPos.roll = roll_deg;
|
||
centerRobotPos.pitch = pitch_deg;
|
||
centerRobotPos.yaw = yaw_deg;
|
||
|
||
detectionResult.positions.push_back(centerRobotPos);
|
||
|
||
// 保存可视化数据（半径设为0，因为 wd_HolePositioning 不输出半径）
|
||
SVzNL3DPointF visCenter;
|
||
visCenter.x = static_cast<float>(holeInfo.center.x);
|
||
visCenter.y = static_cast<float>(holeInfo.center.y);
|
||
visCenter.z = static_cast<float>(holeInfo.center.z);
|
||
holeVisData.push_back(std::make_pair(visCenter, 0.0f));
|
||
|
||
if (debugParam.enableDebug && debugParam.printDetailLog) {
|
||
LOG_INFO("[Algo Thread] Hole[%zu] Normal (eye): (%.3f, %.3f, %.3f)\n",
|
||
i, holeInfo.normDir.x, holeInfo.normDir.y, holeInfo.normDir.z);
|
||
LOG_INFO("[Algo Thread] Hole[%zu] Direction vectors (robot): X=(%.3f,%.3f,%.3f), Y=(%.3f,%.3f,%.3f), Z=(%.3f,%.3f,%.3f)\n",
|
||
i,
|
||
dirVectors_robot[0].x, dirVectors_robot[0].y, dirVectors_robot[0].z,
|
||
dirVectors_robot[1].x, dirVectors_robot[1].y, dirVectors_robot[1].z,
|
||
dirVectors_robot[2].x, dirVectors_robot[2].y, dirVectors_robot[2].z);
|
||
LOG_INFO("[Algo Thread] Hole[%zu] Center Eye Coords: X=%.2f, Y=%.2f, Z=%.2f\n",
|
||
i, holeInfo.center.x, holeInfo.center.y, holeInfo.center.z);
|
||
LOG_INFO("[Algo Thread] Hole[%zu] Center Robot Coords: X=%.2f, Y=%.2f, Z=%.2f, R=%.4f, P=%.4f, Y=%.4f\n",
|
||
i, ptRobot.x, ptRobot.y, ptRobot.z,
|
||
centerRobotPos.roll, centerRobotPos.pitch, centerRobotPos.yaw);
|
||
}
|
||
}
|
||
|
||
// ========== 生成可视化图像 ==========
|
||
{
|
||
std::vector<HoleMarkerInfo> holes;
|
||
for (const auto& vis : holeVisData) {
|
||
HoleMarkerInfo h;
|
||
h.center.x = vis.first.x;
|
||
h.center.y = vis.first.y;
|
||
h.center.z = vis.first.z;
|
||
h.radius = vis.second; // 半径为0
|
||
holes.push_back(h);
|
||
}
|
||
detectionResult.image = PointCloudImageUtils::GenerateHoleDetectionImage(
|
||
xyzData, holes,
|
||
0.0, // 绕X轴旋转角度（度），可按需调整视图
|
||
0.0); // 绕Y轴旋转角度（度），可按需调整视图
|
||
}
|
||
|
||
if (debugParam.enableDebug && debugParam.saveDebugImage) {
|
||
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;
|
||
}
|