386 lines
16 KiB
C++
386 lines
16 KiB
C++
#include "DetectPresenter.h"
|
||
#include "motorStatorPosition_Export.h"
|
||
#include "SG_baseAlgo_Export.h"
|
||
#include <fstream>
|
||
#include <QPainter>
|
||
#include <QPen>
|
||
#include <QColor>
|
||
#include <opencv2/opencv.hpp>
|
||
#include "CoordinateTransform.h"
|
||
#include "VrConvert.h"
|
||
|
||
DetectPresenter::DetectPresenter(/* args */)
|
||
{
|
||
LOG_DEBUG("DetectPresenter Init algo ver: %s\n", wd_particleSegVersion());
|
||
}
|
||
|
||
DetectPresenter::~DetectPresenter()
|
||
{
|
||
}
|
||
|
||
|
||
int DetectPresenter::DetectStatorPosition(
|
||
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,
|
||
StatorPositionDetectionResult& 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);
|
||
}
|
||
|
||
// 定子物理参数
|
||
SWD_statorParam statorParam;
|
||
statorParam.statorOuterD = algorithmParams.statorParam.statorOuterD;
|
||
statorParam.statorInnerD = algorithmParams.statorParam.statorInnerD;
|
||
statorParam.statorHeight = algorithmParams.statorParam.statorHeight;
|
||
statorParam.plateThickness = algorithmParams.statorParam.plateThickness;
|
||
statorParam.plateW = algorithmParams.statorParam.plateW;
|
||
statorParam.plateH = algorithmParams.statorParam.plateH;
|
||
|
||
// 构造算法参数（使用 cornerParam 而非 lineSegParam）
|
||
SWD_statorPositonParam algoParam;
|
||
|
||
// 滤波参数
|
||
algoParam.filterParam.continuityTh = algorithmParams.filterParam.continuityTh;
|
||
algoParam.filterParam.outlierTh = algorithmParams.filterParam.outlierTh;
|
||
|
||
// 角点检测参数
|
||
algoParam.cornerParam.minEndingGap = algorithmParams.cornerParam.minEndingGap;
|
||
algoParam.cornerParam.minEndingGap_z = algorithmParams.cornerParam.minEndingGap_z;
|
||
algoParam.cornerParam.scale = algorithmParams.cornerParam.scale;
|
||
algoParam.cornerParam.cornerTh = algorithmParams.cornerParam.cornerTh;
|
||
algoParam.cornerParam.jumpCornerTh_1 = algorithmParams.cornerParam.jumpCornerTh_1;
|
||
algoParam.cornerParam.jumpCornerTh_2 = algorithmParams.cornerParam.jumpCornerTh_2;
|
||
|
||
// 树生长参数
|
||
algoParam.growParam.yDeviation_max = algorithmParams.growParam.yDeviation_max;
|
||
algoParam.growParam.zDeviation_max = algorithmParams.growParam.zDeviation_max;
|
||
algoParam.growParam.maxLineSkipNum = algorithmParams.growParam.maxLineSkipNum;
|
||
algoParam.growParam.maxSkipDistance = algorithmParams.growParam.maxSkipDistance;
|
||
algoParam.growParam.minLTypeTreeLen = algorithmParams.growParam.minLTypeTreeLen;
|
||
algoParam.growParam.minVTypeTreeLen = algorithmParams.growParam.minVTypeTreeLen;
|
||
|
||
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] StatorParam: outerD=%.1f, innerD=%.1f, height=%.1f, plateThickness=%.1f, plateW=%.1f, plateH=%.1f\n",
|
||
statorParam.statorOuterD, statorParam.statorInnerD, statorParam.statorHeight,
|
||
statorParam.plateThickness, statorParam.plateW, statorParam.plateH);
|
||
|
||
// 打印角点检测参数
|
||
LOG_INFO("[Algo Thread] CornerParam: minEndingGap=%.1f, minEndingGap_z=%.1f, scale=%.1f, cornerTh=%.1f, jumpCornerTh_1=%.1f, jumpCornerTh_2=%.1f\n",
|
||
algoParam.cornerParam.minEndingGap, algoParam.cornerParam.minEndingGap_z,
|
||
algoParam.cornerParam.scale, algoParam.cornerParam.cornerTh,
|
||
algoParam.cornerParam.jumpCornerTh_1, algoParam.cornerParam.jumpCornerTh_2);
|
||
|
||
// 打印树生长参数
|
||
LOG_INFO("[Algo Thread] Tree Grow: yDeviation_max=%.1f, zDeviation_max=%.1f, maxLineSkipNum=%d, maxSkipDistance=%.1f, minLTypeTreeLen=%.1f, minVTypeTreeLen=%.1f\n",
|
||
algoParam.growParam.yDeviation_max, algoParam.growParam.zDeviation_max, algoParam.growParam.maxLineSkipNum,
|
||
algoParam.growParam.maxSkipDistance, algoParam.growParam.minLTypeTreeLen, algoParam.growParam.minVTypeTreeLen);
|
||
|
||
// 打印滤波参数
|
||
LOG_INFO("[Algo Thread] Filter: continuityTh=%.1f, outlierTh=%.1f\n", algoParam.filterParam.continuityTh, algoParam.filterParam.outlierTh);
|
||
}
|
||
|
||
// 准备平面校准参数
|
||
SSG_planeCalibPara groundCalibPara;
|
||
if(cameraCalibParam){
|
||
memcpy(groundCalibPara.planeCalib, cameraCalibParam->planeCalib, sizeof(double) * 9);
|
||
memcpy(groundCalibPara.invRMatrix, cameraCalibParam->invRMatrix, sizeof(double) * 9);
|
||
groundCalibPara.planeHeight = cameraCalibParam->planeHeight;
|
||
} else {
|
||
// 使用默认单位矩阵
|
||
double identity[9] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
|
||
memcpy(groundCalibPara.planeCalib, identity, sizeof(double) * 9);
|
||
memcpy(groundCalibPara.invRMatrix, identity, sizeof(double) * 9);
|
||
groundCalibPara.planeHeight = -1.0;
|
||
}
|
||
|
||
if(debugParam.enableDebug && debugParam.printDetailLog)
|
||
{
|
||
LOG_INFO("Plane height: %.3f\n", groundCalibPara.planeHeight);
|
||
LOG_INFO(" Plane calibration matrix: [%f, %f, %f; %f, %f, %f; %f, %f, %f]\n",
|
||
groundCalibPara.planeCalib[0], groundCalibPara.planeCalib[1], groundCalibPara.planeCalib[2],
|
||
groundCalibPara.planeCalib[3], groundCalibPara.planeCalib[4], groundCalibPara.planeCalib[5],
|
||
groundCalibPara.planeCalib[6], groundCalibPara.planeCalib[7], groundCalibPara.planeCalib[8]);
|
||
|
||
LOG_INFO(" Plane invRMatrix matrix: [%f, %f, %f; %f, %f, %f; %f, %f, %f]\n",
|
||
groundCalibPara.invRMatrix[0], groundCalibPara.invRMatrix[1], groundCalibPara.invRMatrix[2],
|
||
groundCalibPara.invRMatrix[3], groundCalibPara.invRMatrix[4], groundCalibPara.invRMatrix[5],
|
||
groundCalibPara.invRMatrix[6], groundCalibPara.invRMatrix[7], groundCalibPara.invRMatrix[8]);
|
||
}
|
||
|
||
// 数据预处理：调平和去除地面（使用当前相机的调平参数）
|
||
if(cameraCalibParam){
|
||
LOG_DEBUG("Processing data with plane calibration\n");
|
||
double groundH = -1;
|
||
for(size_t i = 0; i < xyzData.size(); i++){
|
||
wd_lineDataR(xyzData[i], cameraCalibParam->planeCalib, groundH);
|
||
}
|
||
}
|
||
|
||
int errCode = 0;
|
||
CVrTimeUtils oTimeUtils;
|
||
|
||
LOG_DEBUG("before wd_motorStatorPosition \n");
|
||
|
||
// 状态机（每次检测新建）
|
||
SWD_statorGriperState stateMachine;
|
||
memset(&stateMachine, 0, sizeof(SWD_statorGriperState));
|
||
|
||
// 调用电机定子定位算法
|
||
std::vector<SWD_statorInnerGrasper> resultObjPositions;
|
||
wd_motorStatorPosition(
|
||
xyzData,
|
||
statorParam,
|
||
groundCalibPara,
|
||
algoParam,
|
||
&stateMachine,
|
||
&errCode,
|
||
resultObjPositions
|
||
);
|
||
|
||
LOG_DEBUG("after wd_motorStatorPosition \n");
|
||
|
||
LOG_INFO("wd_motorStatorPosition: found %zu stators, err=%d runtime=%.3fms\n", resultObjPositions.size(), errCode, oTimeUtils.GetElapsedTimeInMilliSec());
|
||
ERR_CODE_RETURN(errCode);
|
||
|
||
// 从4x4齐次变换矩阵中提取旋转矩阵R(3x3)和平移向量T(3x1)
|
||
// clibMatrix 是行优先存储的4x4矩阵
|
||
cv::Mat R = cv::Mat::zeros(3, 3, CV_64F);
|
||
cv::Mat T = cv::Mat::zeros(3, 1, CV_64F);
|
||
for (int i = 0; i < 3; i++) {
|
||
for (int j = 0; j < 3; j++) {
|
||
R.at<double>(i, j) = clibMatrix[i * 4 + j];
|
||
}
|
||
T.at<double>(i, 0) = clibMatrix[i * 4 + 3];
|
||
}
|
||
|
||
// 构建用于可视化的点数组
|
||
std::vector<SVzNL3DPoint> objCenterPoints;
|
||
|
||
// 处理每个定子的检测结果
|
||
for (size_t i = 0; i < resultObjPositions.size(); i++) {
|
||
const SWD_statorInnerGrasper& obj = resultObjPositions[i];
|
||
|
||
// SSG_6DOF opCenter 包含了相机坐标系下的 x, y, z, x_roll, y_pitch, z_yaw
|
||
// 1. 位置转换：使用 pointRT_2 进行坐标变换
|
||
cv::Point3d ptEye(obj.opCenter.x, obj.opCenter.y, obj.opCenter.z);
|
||
cv::Point3d ptRobot;
|
||
pointRT_2(R, T, ptEye, ptRobot);
|
||
|
||
// 2. 姿态转换：motorStatorPosition 的 SSG_6DOF 直接给出了欧拉角
|
||
// 构建相机坐标系下的旋转矩阵，然后用手眼标定矩阵旋转
|
||
double roll_rad = obj.opCenter.x_roll * CV_PI / 180.0;
|
||
double pitch_rad = obj.opCenter.y_pitch * CV_PI / 180.0;
|
||
double yaw_rad = obj.opCenter.z_yaw * CV_PI / 180.0;
|
||
|
||
// ZYX欧拉角转旋转矩阵
|
||
cv::Mat Rz = (cv::Mat_<double>(3, 3) <<
|
||
cos(yaw_rad), -sin(yaw_rad), 0,
|
||
sin(yaw_rad), cos(yaw_rad), 0,
|
||
0, 0, 1);
|
||
cv::Mat Ry = (cv::Mat_<double>(3, 3) <<
|
||
cos(pitch_rad), 0, sin(pitch_rad),
|
||
0, 1, 0,
|
||
-sin(pitch_rad), 0, cos(pitch_rad));
|
||
cv::Mat Rx = (cv::Mat_<double>(3, 3) <<
|
||
1, 0, 0,
|
||
0, cos(roll_rad), -sin(roll_rad),
|
||
0, sin(roll_rad), cos(roll_rad));
|
||
|
||
cv::Mat R_eye = Rz * Ry * Rx;
|
||
|
||
// 将相机坐标系下的旋转矩阵变换到机器人坐标系
|
||
cv::Mat R_robot = R * R_eye;
|
||
|
||
// 转为3x3 double数组
|
||
double R_pose[3][3];
|
||
for (int ri = 0; ri < 3; ri++) {
|
||
for (int rj = 0; rj < 3; rj++) {
|
||
R_pose[ri][rj] = R_robot.at<double>(ri, rj);
|
||
}
|
||
}
|
||
|
||
// 使用 rotationMatrixToEulerZYX 转换为欧拉角
|
||
SSG_EulerAngles robotRpy = rotationMatrixToEulerZYX(R_pose);
|
||
|
||
// 将机器人坐标系下的位姿添加到positions列表
|
||
StatorGraspPosition pos;
|
||
pos.objID = obj.objID;
|
||
pos.x = ptRobot.x;
|
||
pos.y = ptRobot.y;
|
||
pos.z = ptRobot.z;
|
||
pos.roll = robotRpy.roll;
|
||
pos.pitch = robotRpy.pitch;
|
||
pos.yaw = robotRpy.yaw;
|
||
|
||
detectionResult.positions.push_back(pos);
|
||
|
||
// 添加中心点到可视化列表
|
||
SVzNL3DPoint centerPt;
|
||
centerPt.x = obj.opCenter.x;
|
||
centerPt.y = obj.opCenter.y;
|
||
centerPt.z = obj.opCenter.z;
|
||
objCenterPoints.push_back(centerPt);
|
||
|
||
if(debugParam.enableDebug && debugParam.printDetailLog){
|
||
LOG_INFO("[Algo Thread] Stator[%zu] objID=%d Eye Coords: X=%.2f, Y=%.2f, Z=%.2f, Roll=%.2f, Pitch=%.2f, Yaw=%.2f\n",
|
||
i, obj.objID,
|
||
obj.opCenter.x, obj.opCenter.y, obj.opCenter.z,
|
||
obj.opCenter.x_roll, obj.opCenter.y_pitch, obj.opCenter.z_yaw);
|
||
LOG_INFO("[Algo Thread] Stator[%zu] Robot Coords: X=%.2f, Y=%.2f, Z=%.2f, R=%.4f, P=%.4f, Y=%.4f\n",
|
||
i, ptRobot.x, ptRobot.y, ptRobot.z,
|
||
pos.roll, pos.pitch, pos.yaw);
|
||
}
|
||
}
|
||
|
||
// 从点云数据生成投影图像（单色灰色点云 + 红色抓取点标记）
|
||
{
|
||
// 固定图像尺寸
|
||
int imgRows = 992;
|
||
int imgCols = 1056;
|
||
int x_skip = 50;
|
||
int y_skip = 50;
|
||
|
||
// 计算点云范围
|
||
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);
|
||
}
|
||
}
|
||
|
||
// 计算投影比例
|
||
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;
|
||
if (x_scale < y_scale)
|
||
x_scale = y_scale;
|
||
else
|
||
y_scale = x_scale;
|
||
|
||
// 创建图像
|
||
QImage image(imgCols, imgRows, QImage::Format_RGB888);
|
||
image.fill(Qt::black);
|
||
QPainter painter(&image);
|
||
painter.setRenderHint(QPainter::Antialiasing);
|
||
|
||
// 绘制点云数据 - 使用单色灰色
|
||
QColor grayColor(150, 150, 150);
|
||
for (const auto& scanLine : xyzData) {
|
||
for (const auto& point : scanLine) {
|
||
if (point.pt3D.z < 1e-4) continue;
|
||
|
||
int px = (int)((point.pt3D.x - xMin) / x_scale + x_skip);
|
||
int py = (int)((point.pt3D.y - yMin) / y_scale + y_skip);
|
||
|
||
if (px >= 0 && px < imgCols && py >= 0 && py < imgRows) {
|
||
painter.setPen(QPen(grayColor, 1));
|
||
painter.drawPoint(px, py);
|
||
}
|
||
}
|
||
}
|
||
|
||
// 绘制抓取点标记 - 使用红色
|
||
if (!objCenterPoints.empty()) {
|
||
QColor markerColor(255, 0, 0); // 红色
|
||
painter.setPen(QPen(markerColor, 1));
|
||
painter.setBrush(QBrush(markerColor));
|
||
|
||
for (size_t i = 0; i < objCenterPoints.size(); i++) {
|
||
const SVzNL3DPoint& pt = objCenterPoints[i];
|
||
|
||
// 跳过全0的点
|
||
if (fabs(pt.x) < 0.0001 && fabs(pt.y) < 0.0001 && fabs(pt.z) < 0.0001) {
|
||
continue;
|
||
}
|
||
|
||
int px = (int)((pt.x - xMin) / x_scale + x_skip);
|
||
int py = (int)((pt.y - yMin) / y_scale + y_skip);
|
||
|
||
if (px >= 0 && px < imgCols && py >= 0 && py < imgRows) {
|
||
// 绘制圆点标记
|
||
int circleSize = 6;
|
||
painter.drawEllipse(px - circleSize/2, py - circleSize/2, circleSize, circleSize);
|
||
|
||
// 绘制编号
|
||
painter.setPen(QPen(Qt::white, 1));
|
||
QFont font("Arial", 14, QFont::Bold);
|
||
painter.setFont(font);
|
||
painter.drawText(px + 8, py + 6, QString("%1").arg(i + 1));
|
||
|
||
// 恢复画笔
|
||
painter.setPen(QPen(markerColor, 1));
|
||
painter.setBrush(QBrush(markerColor));
|
||
}
|
||
}
|
||
}
|
||
|
||
detectionResult.image = image;
|
||
}
|
||
|
||
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;
|
||
}
|