329 lines
12 KiB
C++
329 lines
12 KiB
C++
#include "DetectPresenter.h"
|
||
#include "HoleDetection.h"
|
||
#include "HoleDetectionParams.h"
|
||
#include "AlgoParamConverter.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 (HoleDetection)\n");
|
||
}
|
||
|
||
DetectPresenter::~DetectPresenter()
|
||
{
|
||
}
|
||
|
||
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格式
|
||
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);
|
||
}
|
||
|
||
// 将 xyzData 转换为 SVzNLPointXYZ* 网格数据
|
||
int rows = static_cast<int>(xyzData.size());
|
||
int cols = 0;
|
||
for (const auto& row : xyzData) {
|
||
if (static_cast<int>(row.size()) > cols) {
|
||
cols = static_cast<int>(row.size());
|
||
}
|
||
}
|
||
|
||
if (rows <= 0 || cols <= 0) {
|
||
LOG_WARNING("Invalid grid dimensions: rows=%d, cols=%d\n", rows, cols);
|
||
return ERR_CODE(DEV_DATA_INVALID);
|
||
}
|
||
|
||
LOG_INFO("[Algo Thread] Grid dimensions: rows=%d, cols=%d\n", rows, cols);
|
||
|
||
// 分配网格数组（行优先）
|
||
SVzNLPointXYZ* points = new SVzNLPointXYZ[rows * cols];
|
||
memset(points, 0, sizeof(SVzNLPointXYZ) * rows * cols);
|
||
|
||
// 填充有效点，缺失点用 {0,0,0} 填充
|
||
for (int r = 0; r < rows; r++) {
|
||
int rowCols = static_cast<int>(xyzData[r].size());
|
||
for (int c = 0; c < cols; c++) {
|
||
int idx = r * cols + c;
|
||
if (c < rowCols) {
|
||
points[idx].x = static_cast<float>(xyzData[r][c].pt3D.x);
|
||
points[idx].y = static_cast<float>(xyzData[r][c].pt3D.y);
|
||
points[idx].z = static_cast<float>(xyzData[r][c].pt3D.z);
|
||
} else {
|
||
points[idx].x = 0.0f;
|
||
points[idx].y = 0.0f;
|
||
points[idx].z = 0.0f;
|
||
}
|
||
}
|
||
}
|
||
|
||
// 使用 AlgoParamConverter 进行参数转换
|
||
SHoleDetectionParams detectionParams = AlgoParamConverter::ToAlgoParam(algorithmParams.detectionParam);
|
||
SHoleFilterParams filterParams = AlgoParamConverter::ToAlgoParam(algorithmParams.filterParam);
|
||
|
||
if (debugParam.enableDebug && debugParam.printDetailLog) {
|
||
AlgoParamConverter::LogAlgoParams("[Algo Thread]", detectionParams, filterParams, clibMatrix);
|
||
}
|
||
|
||
CVrTimeUtils oTimeUtils;
|
||
|
||
LOG_DEBUG("before DetectMultipleHoles\n");
|
||
|
||
// 调用孔洞检测算法
|
||
SMultiHoleResult multiResult;
|
||
int errCode = DetectMultipleHoles(
|
||
points,
|
||
rows,
|
||
cols,
|
||
detectionParams,
|
||
filterParams,
|
||
&multiResult
|
||
);
|
||
|
||
LOG_DEBUG("after DetectMultipleHoles\n");
|
||
|
||
LOG_INFO("DetectMultipleHoles: found %d holes (candidates=%d, filtered=%d), err=%d runtime=%.3fms\n",
|
||
multiResult.holeCount, multiResult.totalCandidates, multiResult.filteredCount,
|
||
errCode, oTimeUtils.GetElapsedTimeInMilliSec());
|
||
|
||
// 记录统计信息到检测结果
|
||
detectionResult.totalCandidates = multiResult.totalCandidates;
|
||
detectionResult.filteredCount = multiResult.filteredCount;
|
||
|
||
if (errCode != HD_SUCCESS) {
|
||
LOG_ERROR("DetectMultipleHoles failed with error code: %d\n", errCode);
|
||
delete[] points;
|
||
FreeMultiHoleResult(&multiResult);
|
||
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 (int i = 0; i < multiResult.holeCount; i++) {
|
||
const SHoleResult& hole = multiResult.holes[i];
|
||
|
||
// 保存孔洞详细信息
|
||
HoleResultData holeData;
|
||
holeData.centerX = hole.center.x;
|
||
holeData.centerY = hole.center.y;
|
||
holeData.centerZ = hole.center.z;
|
||
holeData.normalX = hole.normal.x;
|
||
holeData.normalY = hole.normal.y;
|
||
holeData.normalZ = hole.normal.z;
|
||
holeData.radius = hole.radius;
|
||
holeData.depth = hole.depth;
|
||
holeData.eccentricity = hole.eccentricity;
|
||
holeData.radiusVariance = hole.radiusVariance;
|
||
holeData.angularSpan = hole.angularSpan;
|
||
holeData.qualityScore = hole.qualityScore;
|
||
detectionResult.holeDetails.push_back(holeData);
|
||
|
||
// 六轴转换：位置+姿态（转换到机械臂坐标系）
|
||
// 1. 位置转换：使用齐次变换矩阵进行坐标变换
|
||
CTVec3D ptEye(hole.center.x, hole.center.y, hole.center.z);
|
||
CTVec3D ptRobot = T_total.transformPoint(ptEye);
|
||
|
||
// 2. 姿态转换：从法向量构建旋转矩阵
|
||
// Z轴 = 法向量（归一化）
|
||
CTVec3D Z_axis(hole.normal.x, hole.normal.y, hole.normal.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
|
||
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);
|
||
|
||
// 保存可视化数据
|
||
holeVisData.push_back(std::make_pair(hole.center, hole.radius));
|
||
|
||
if (debugParam.enableDebug && debugParam.printDetailLog) {
|
||
LOG_INFO("[Algo Thread] Hole[%d] Normal (eye): (%.3f, %.3f, %.3f)\n",
|
||
i, hole.normal.x, hole.normal.y, hole.normal.z);
|
||
LOG_INFO("[Algo Thread] Hole[%d] 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[%d] Center Eye Coords: X=%.2f, Y=%.2f, Z=%.2f\n",
|
||
i, hole.center.x, hole.center.y, hole.center.z);
|
||
LOG_INFO("[Algo Thread] Hole[%d] 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);
|
||
LOG_INFO("[Algo Thread] Hole[%d] Radius=%.2f, Depth=%.2f, Quality=%.3f\n",
|
||
i, hole.radius, hole.depth, hole.qualityScore);
|
||
}
|
||
}
|
||
|
||
// 释放算法结果内存
|
||
FreeMultiHoleResult(&multiResult);
|
||
|
||
// 从点云数据生成投影图像（热力图着色 + 孔位标记）
|
||
{
|
||
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;
|
||
holes.push_back(h);
|
||
}
|
||
detectionResult.image = PointCloudImageUtils::GenerateHoleDetectionImage(xyzData, holes);
|
||
}
|
||
|
||
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");
|
||
}
|
||
}
|
||
|
||
// 释放网格数组
|
||
delete[] points;
|
||
|
||
return nRet;
|
||
}
|