// CoordTransformPanel.cpp #include "UserVisibleLog.h" #include "CoordTransformPanel.h" #include "ui_MainWindow.h" #include "AppSettings.h" #include #include #include #include #include #include #include #include #include using namespace VisionGuideApi; namespace { constexpr double kPi = 3.14159265358979323846; cv::Mat estunTcpPoseToTransform(double xMm, double yMm, double zMm, double aDeg, double bDeg, double cDeg) { // ESTUN XYZABC: A/B/C are rotations about X/Y/Z respectively. // With active rotations and column vectors, intrinsic XYZ is written as // ^reference R_TCP = Rz(C) * Ry(B) * Rx(A) (equivalently extrinsic ZYX). const double a = aDeg * kPi / 180.0; const double b = bDeg * kPi / 180.0; const double c = cDeg * kPi / 180.0; const cv::Mat rz = (cv::Mat_(3, 3) << std::cos(c), -std::sin(c), 0.0, std::sin(c), std::cos(c), 0.0, 0.0, 0.0, 1.0); const cv::Mat ry = (cv::Mat_(3, 3) << std::cos(b), 0.0, std::sin(b), 0.0, 1.0, 0.0, -std::sin(b), 0.0, std::cos(b)); const cv::Mat rx = (cv::Mat_(3, 3) << 1.0, 0.0, 0.0, 0.0, std::cos(a), -std::sin(a), 0.0, std::sin(a), std::cos(a)); cv::Mat result = cv::Mat::eye(4, 4, CV_64F); const cv::Mat rotation = rz * ry * rx; rotation.copyTo(result(cv::Rect(0, 0, 3, 3))); result.at(0, 3) = xMm * 0.001; result.at(1, 3) = yMm * 0.001; result.at(2, 3) = zMm * 0.001; return result; } void transformToEstunPose(const cv::Mat& transform, double& xMm, double& yMm, double& zMm, double& aDeg, double& bDeg, double& cDeg) { cv::Mat matrix; transform.convertTo(matrix, CV_64F); xMm = matrix.at(0, 3) * 1000.0; yMm = matrix.at(1, 3) * 1000.0; zMm = matrix.at(2, 3) * 1000.0; double sinB = -matrix.at(2, 0); if (sinB < -1.0) sinB = -1.0; if (sinB > 1.0) sinB = 1.0; const double b = std::asin(sinB); const double cosB = std::cos(b); double a = 0.0; // X angle (roll) double c = 0.0; // Z angle (yaw) if (std::abs(cosB) > 1e-9) { a = std::atan2(matrix.at(2, 1), matrix.at(2, 2)); c = std::atan2(matrix.at(1, 0), matrix.at(0, 0)); } else { a = std::atan2(-matrix.at(1, 2), matrix.at(1, 1)); } aDeg = a * 180.0 / kPi; bDeg = b * 180.0 / kPi; cDeg = c * 180.0 / kPi; } QString referenceFrameLabel(const QString& referenceFrame) { if (referenceFrame.compare(QStringLiteral("world"), Qt::CaseInsensitive) == 0 || referenceFrame.compare(QStringLiteral("base"), Qt::CaseInsensitive) == 0) return QString::fromUtf8("世界坐标系"); if (referenceFrame.compare(QStringLiteral("user"), Qt::CaseInsensitive) == 0) return QString::fromUtf8("用户坐标系"); return QString::fromUtf8("参考坐标系"); } double normalizeAngleDeg(double angle) { while (angle > 180.0) angle -= 360.0; while (angle <= -180.0) angle += 360.0; return angle; } } CoordTransformPanel::CoordTransformPanel(Ui::MainWindowClass& u, QObject* parent) : QObject(parent), ui(u) { connect(ui.btnLoadHandEyeMatrix, &QPushButton::clicked, this, &CoordTransformPanel::onLoadMatrix); connect(ui.btnShowPartCenter, &QPushButton::clicked, this, &CoordTransformPanel::onShowPartCenter); connect(ui.btnShowGraspPoint, &QPushButton::clicked, this, &CoordTransformPanel::onShowGraspPoint); connect(ui.btnCoordTransform, &QPushButton::clicked, this, &CoordTransformPanel::onTransform); bool offsetOk = false; const double savedOffset = AppSettings::instance() .getRaw(AppSettings::K_GraspOffsetZMm, QStringLiteral("4.4")) .toDouble(&offsetOk); if (offsetOk && std::isfinite(savedOffset)) { const QSignalBlocker blocker(ui.spinGraspOffsetZMm); ui.spinGraspOffsetZMm->setValue(savedOffset); } connect(ui.spinGraspOffsetZMm, QOverload::of(&QDoubleSpinBox::valueChanged), this, &CoordTransformPanel::onGraspOffsetChanged); loadAxisGuidanceCalibration(); updateStates(); } void CoordTransformPanel::updateStates() { const bool poseReady = m_centerPose.valid; ui.btnShowPartCenter->setEnabled(poseReady); ui.btnShowGraspPoint->setEnabled(poseReady); ui.btnCoordTransform->setEnabled(m_loaded && poseReady && canTransform()); ui.btnExportPoseCsv->setEnabled(m_transformedPoseReady); } bool CoordTransformPanel::isEyeInHand() const { return m_matrixType.compare(QStringLiteral("eye_in_hand"), Qt::CaseInsensitive) == 0; } bool CoordTransformPanel::canTransform() const { // 眼在手上:^reference T_part = ^reference T_TCP(t) * ^TCP T_camera * ^camera T_part。 return !isEyeInHand() || m_currentTcpPoseValid; } void CoordTransformPanel::setCameraPose(const VisionPoseResult& p) { m_centerPose = p; m_activeCameraPose = p; m_activePointMode = CenterPoint; m_transformedPose = VisionPoseResult(); m_transformedPoseReady = false; if (p.valid) { if (m_loaded && isEyeInHand() && !m_currentTcpPoseValid) { ui.labelCoordTransformStatus->setText(QString::fromUtf8( "已接收零件中心位姿;尚未提供与当前图像配对的拍照时刻 TCP 位姿")); ui.labelCoordTransformStatus->setStyleSheet("color:#c07b00;"); } else { ui.labelCoordTransformStatus->setText( QString::fromUtf8("已接收零件中心位姿(相机坐标系)")); ui.labelCoordTransformStatus->setStyleSheet("color:#1e6f9f;"); } } else { ui.labelCoordTransformStatus->setText(QString::fromUtf8("状态: 等待有效位姿")); ui.labelCoordTransformStatus->setStyleSheet("color:gray; font-size:11px;"); } updateStates(); } bool CoordTransformPanel::setCurrentTcpPose(double xMm, double yMm, double zMm, double aDeg, double bDeg, double cDeg, const QString& referenceFrame) { const double values[] = {xMm, yMm, zMm, aDeg, bDeg, cDeg}; for (double value : values) { if (!std::isfinite(value)) return false; } m_tcpXmm = xMm; m_tcpYmm = yMm; m_tcpZmm = zMm; m_tcpADeg = aDeg; m_tcpBDeg = bDeg; m_tcpCDeg = cDeg; m_currentTcpReferenceFrame = referenceFrame.trimmed().toLower(); if (m_currentTcpReferenceFrame != QStringLiteral("world") && m_currentTcpReferenceFrame != QStringLiteral("user")) return false; m_currentTcpPoseValid = true; m_transformedPoseReady = false; m_transformedPose = VisionPoseResult(); ui.labelCoordTransformStatus->setText(QString::fromUtf8( "已设置测量时刻 TCP 位姿(%1),可计算抓取位姿") .arg(referenceFrameLabel(m_currentTcpReferenceFrame))); ui.labelCoordTransformStatus->setStyleSheet("color:#1e6f9f;"); updateStates(); return true; } void CoordTransformPanel::setAxisGuidanceEnabled(bool enabled, bool followAxisTilt) { Q_UNUSED(followAxisTilt); m_axisGuidanceEnabled = enabled; AppSettings::instance().set(QStringLiteral("axisGuide/enabled"), enabled ? QStringLiteral("1") : QStringLiteral("0")); m_transformedPoseReady = false; updateStates(); } void CoordTransformPanel::setSystemBiasCorrection(bool enabled, double xMm, double yMm, double zMm) { m_systemBiasEnabled = enabled; m_systemBiasMeters = cv::Vec3d(xMm, yMm, zMm) * 0.001; auto& settings = AppSettings::instance(); settings.set(QStringLiteral("systemBias/enabled"), enabled ? QStringLiteral("1") : QStringLiteral("0")); settings.set(QStringLiteral("systemBias/xMm"), QString::number(xMm, 'f', 3)); settings.set(QStringLiteral("systemBias/yMm"), QString::number(yMm, 'f', 3)); settings.set(QStringLiteral("systemBias/zMm"), QString::number(zMm, 'f', 3)); m_transformedPoseReady = false; updateStates(); } bool CoordTransformPanel::cameraPoseToTransform(const VisionPoseResult& cameraPose, cv::Mat& transform) const { if (!cameraPose.valid) return false; try { HTuple pose; pose[0] = cameraPose.tx; pose[1] = cameraPose.ty; pose[2] = cameraPose.tz; pose[3] = cameraPose.rx; pose[4] = cameraPose.ry; pose[5] = cameraPose.rz; pose[6] = cameraPose.poseType; HTuple homogeneous; PoseToHomMat3d(pose, &homogeneous); transform = cv::Mat::eye(4, 4, CV_64F); for (int row = 0; row < 3; ++row) { for (int column = 0; column < 4; ++column) transform.at(row, column) = homogeneous[row * 4 + column].D(); } return cv::checkRange(transform); } catch (...) { transform.release(); return false; } } bool CoordTransformPanel::currentReferencePartTransform(cv::Mat& transform, QString* errorMessage) const { if (!m_loaded) { if (errorMessage) *errorMessage = QString::fromUtf8("尚未加载手眼标定矩阵"); return false; } if (!m_centerPose.valid) { if (errorMessage) *errorMessage = QString::fromUtf8("尚未获得有效零件位姿"); return false; } if (isEyeInHand() && !m_currentTcpPoseValid) { if (errorMessage) *errorMessage = QString::fromUtf8("尚未输入拍照时刻 TCP 位姿"); return false; } cv::Mat cameraPart; if (!cameraPoseToTransform(m_centerPose, cameraPart)) { if (errorMessage) *errorMessage = QString::fromUtf8("相机零件位姿无法转换为刚体矩阵"); return false; } if (isEyeInHand()) { const cv::Mat referenceTcp = estunTcpPoseToTransform( m_tcpXmm, m_tcpYmm, m_tcpZmm, m_tcpADeg, m_tcpBDeg, m_tcpCDeg); // 眼在手上完整链:^reference T_part = ^reference T_TCP * ^TCP T_camera * ^camera T_part。 transform = referenceTcp * m_matrix * cameraPart; } else { transform = m_matrix * cameraPart; } return cv::checkRange(transform); } cv::Mat CoordTransformPanel::rotationAligningVectors(const cv::Vec3d& from, const cv::Vec3d& to) { cv::Vec3d first = from; cv::Vec3d second = to; const double firstNorm = cv::norm(first); const double secondNorm = cv::norm(second); if (firstNorm < 1e-12 || secondNorm < 1e-12) return cv::Mat::eye(3, 3, CV_64F); first *= 1.0 / firstNorm; second *= 1.0 / secondNorm; double cosine = first.dot(second); if (cosine > 1.0) cosine = 1.0; if (cosine < -1.0) cosine = -1.0; const cv::Vec3d cross = first.cross(second); const double sine = cv::norm(cross); if (sine < 1e-10) { if (cosine > 0.0) return cv::Mat::eye(3, 3, CV_64F); cv::Vec3d helper = std::abs(first[0]) < 0.8 ? cv::Vec3d(1.0, 0.0, 0.0) : cv::Vec3d(0.0, 1.0, 0.0); cv::Vec3d axis = first.cross(helper); axis *= 1.0 / cv::norm(axis); cv::Mat rotation = -cv::Mat::eye(3, 3, CV_64F); for (int row = 0; row < 3; ++row) { for (int column = 0; column < 3; ++column) rotation.at(row, column) += 2.0 * axis[row] * axis[column]; } return rotation; } const cv::Mat skew = (cv::Mat_(3, 3) << 0.0, -cross[2], cross[1], cross[2], 0.0, -cross[0], -cross[1], cross[0], 0.0); return cv::Mat::eye(3, 3, CV_64F) + skew + skew * skew * ((1.0 - cosine) / (sine * sine)); } bool CoordTransformPanel::calibrateAxisGuidance(double graspXmm, double graspYmm, double graspZmm, double graspADeg, double graspBDeg, double graspCDeg, const QString& referenceFrame, QString* errorMessage) { const QString normalizedFrame = referenceFrame.trimmed().toLower(); if (normalizedFrame != QStringLiteral("world") && normalizedFrame != QStringLiteral("user")) { if (errorMessage) *errorMessage = QString::fromUtf8("抓取真值的参考坐标系无效"); return false; } if (isEyeInHand() && normalizedFrame != m_currentTcpReferenceFrame) { if (errorMessage) *errorMessage = QString::fromUtf8( "抓取真值与拍照时刻 TCP 必须属于同一参考坐标系"); return false; } cv::Mat referencePart; if (!currentReferencePartTransform(referencePart, errorMessage)) return false; const cv::Mat referenceGrasp = estunTcpPoseToTransform( graspXmm, graspYmm, graspZmm, graspADeg, graspBDeg, graspCDeg); cv::Vec3d axis(referencePart.at(0, 2), referencePart.at(1, 2), referencePart.at(2, 2)); const double axisNorm = cv::norm(axis); if (axisNorm < 1e-9) { if (errorMessage) *errorMessage = QString::fromUtf8("视觉中心轴向量无效"); return false; } axis *= 1.0 / axisNorm; cv::Vec3d partCenter(referencePart.at(0, 3), referencePart.at(1, 3), referencePart.at(2, 3)); const cv::Vec3d graspPoint(referenceGrasp.at(0, 3), referenceGrasp.at(1, 3), referenceGrasp.at(2, 3)); const double axialOffsetMeters = ui.spinGraspOffsetZMm->value() * 0.001; const cv::Vec3d nominalGraspPoint = partCenter + axis * axialOffsetMeters; m_axisGuidanceReferenceAxis = axis; const cv::Mat graspRotation = referenceGrasp(cv::Rect(0, 0, 3, 3)); cv::Mat axisReferenceMat = (cv::Mat_(3, 1) << axis[0], axis[1], axis[2]); const cv::Mat axisToolMat = graspRotation.t() * axisReferenceMat; m_axisGuidanceAxisInTool = cv::Vec3d( axisToolMat.at(0), axisToolMat.at(1), axisToolMat.at(2)); const double controllableNorm = std::hypot( m_axisGuidanceAxisInTool[1], m_axisGuidanceAxisInTool[2]); if (controllableNorm < 0.1) { if (errorMessage) *errorMessage = QString::fromUtf8( "示教关系中中心轴接近TCP X轴;固定B/C时改变A无法确定抓取姿态"); return false; } // 这里只学习手眼/视觉链的固定平移偏差,不再把该向量误当作最终XYZ。 m_axisGuidancePositionOffset = graspPoint - nominalGraspPoint; m_axisGuidanceFixedBDeg = graspBDeg; m_axisGuidanceFixedCDeg = graspCDeg; m_axisGuidanceReferenceFrame = normalizedFrame; m_axisGuidanceValid = true; saveAxisGuidanceCalibration(); emit logMessage(QString::fromUtf8( "回转体垂直抓取已标定(%1):A由完整中心轴向量求解,B=%2°,C=%3°;位置偏差 dX=%4 dY=%5 dZ=%6 mm;视觉绕中心轴角已忽略") .arg(referenceFrameLabel(normalizedFrame)) .arg(m_axisGuidanceFixedBDeg, 0, 'f', 3) .arg(m_axisGuidanceFixedCDeg, 0, 'f', 3) .arg(m_axisGuidancePositionOffset[0] * 1000.0, 0, 'f', 3) .arg(m_axisGuidancePositionOffset[1] * 1000.0, 0, 'f', 3) .arg(m_axisGuidancePositionOffset[2] * 1000.0, 0, 'f', 3)); return true; } void CoordTransformPanel::loadAxisGuidanceCalibration() { auto& settings = AppSettings::instance(); m_axisGuidanceEnabled = settings.getRaw( QStringLiteral("axisGuide/enabled"), QStringLiteral("0")) == QStringLiteral("1"); const bool currentSchema = settings.getRaw( QStringLiteral("axisGuide/schemaVersion"), QStringLiteral("0")) == QStringLiteral("4"); m_axisGuidanceValid = currentSchema && settings.getRaw( QStringLiteral("axisGuide/valid"), QStringLiteral("0")) == QStringLiteral("1"); if (!m_axisGuidanceValid) return; auto value = [&settings](const QString& key) { return settings.getRaw(key, QStringLiteral("0")).toDouble(); }; m_axisGuidanceReferenceAxis = cv::Vec3d( value(QStringLiteral("axisGuide/axisX")), value(QStringLiteral("axisGuide/axisY")), value(QStringLiteral("axisGuide/axisZ"))); m_axisGuidanceAxisInTool = cv::Vec3d( value(QStringLiteral("axisGuide/toolAxisX")), value(QStringLiteral("axisGuide/toolAxisY")), value(QStringLiteral("axisGuide/toolAxisZ"))); m_axisGuidancePositionOffset = cv::Vec3d( value(QStringLiteral("axisGuide/offsetX")), value(QStringLiteral("axisGuide/offsetY")), value(QStringLiteral("axisGuide/offsetZ"))); m_axisGuidanceFixedBDeg = value(QStringLiteral("axisGuide/fixedBDeg")); m_axisGuidanceFixedCDeg = value(QStringLiteral("axisGuide/fixedCDeg")); m_axisGuidanceReferenceFrame = settings.getRaw( QStringLiteral("axisGuide/referenceFrame"), QStringLiteral("world")).toLower(); if (cv::norm(m_axisGuidanceReferenceAxis) < 1e-9 || cv::norm(m_axisGuidanceAxisInTool) < 1e-9) m_axisGuidanceValid = false; } void CoordTransformPanel::saveAxisGuidanceCalibration() { if (!m_axisGuidanceValid) return; auto& settings = AppSettings::instance(); settings.set(QStringLiteral("axisGuide/schemaVersion"), QStringLiteral("4")); settings.set(QStringLiteral("axisGuide/valid"), QStringLiteral("1")); settings.set(QStringLiteral("axisGuide/referenceFrame"), m_axisGuidanceReferenceFrame); settings.set(QStringLiteral("axisGuide/axisX"), QString::number(m_axisGuidanceReferenceAxis[0], 'g', 17)); settings.set(QStringLiteral("axisGuide/axisY"), QString::number(m_axisGuidanceReferenceAxis[1], 'g', 17)); settings.set(QStringLiteral("axisGuide/axisZ"), QString::number(m_axisGuidanceReferenceAxis[2], 'g', 17)); settings.set(QStringLiteral("axisGuide/toolAxisX"), QString::number(m_axisGuidanceAxisInTool[0], 'g', 17)); settings.set(QStringLiteral("axisGuide/toolAxisY"), QString::number(m_axisGuidanceAxisInTool[1], 'g', 17)); settings.set(QStringLiteral("axisGuide/toolAxisZ"), QString::number(m_axisGuidanceAxisInTool[2], 'g', 17)); settings.set(QStringLiteral("axisGuide/offsetX"), QString::number(m_axisGuidancePositionOffset[0], 'g', 17)); settings.set(QStringLiteral("axisGuide/offsetY"), QString::number(m_axisGuidancePositionOffset[1], 'g', 17)); settings.set(QStringLiteral("axisGuide/offsetZ"), QString::number(m_axisGuidancePositionOffset[2], 'g', 17)); settings.set(QStringLiteral("axisGuide/fixedBDeg"), QString::number(m_axisGuidanceFixedBDeg, 'g', 17)); settings.set(QStringLiteral("axisGuide/fixedCDeg"), QString::number(m_axisGuidanceFixedCDeg, 'g', 17)); } bool CoordTransformPanel::loadFromFile(const QString& path) { QFile f(path); if (!f.open(QIODevice::ReadOnly | QIODevice::Text)) return false; QTextStream in(&f); cv::Mat parsed = cv::Mat::eye(4, 4, CV_64F); QString detectedType; QString detectedTranslationUnit; QString detectedReferenceFrame; int row = 0; bool valuesOk = true; while (!in.atEnd() && row < 4) { QString line = in.readLine().trimmed(); if (line.isEmpty()) continue; if (line.startsWith('#')) { const int colon = line.indexOf(':'); if (colon > 1) { const QString key = line.mid(1, colon - 1).trimmed(); const QString value = line.mid(colon + 1).trimmed().toLower(); if (key.compare(QStringLiteral("Type"), Qt::CaseInsensitive) == 0) detectedType = value; else if (key.compare(QStringLiteral("Translation Unit"), Qt::CaseInsensitive) == 0) detectedTranslationUnit = value; else if (key.compare(QStringLiteral("Reference Frame"), Qt::CaseInsensitive) == 0) detectedReferenceFrame = value; } continue; } QStringList parts = line.split(QRegExp("[,;\\s]+"), QString::SkipEmptyParts); if (parts.size() < 4) { valuesOk = false; break; } for (int c = 0; c < 4; ++c) { bool ok = false; const double value = parts[c].toDouble(&ok); if (!ok || !std::isfinite(value)) { valuesOk = false; break; } parsed.at(row, c) = value; } if (!valuesOk) break; ++row; } f.close(); // 应用内生成的位姿与矩阵平移均以米为单位;兼容明确声明为毫米的外部矩阵。 if (valuesOk && row == 4 && detectedTranslationUnit == QStringLiteral("mm")) { parsed.at(0, 3) /= 1000.0; parsed.at(1, 3) /= 1000.0; parsed.at(2, 3) /= 1000.0; } else if (!detectedTranslationUnit.isEmpty() && detectedTranslationUnit != QStringLiteral("m")) { return false; } const bool affineLastRow = valuesOk && row == 4 && std::abs(parsed.at(3, 0)) < 1e-6 && std::abs(parsed.at(3, 1)) < 1e-6 && std::abs(parsed.at(3, 2)) < 1e-6 && std::abs(parsed.at(3, 3) - 1.0) < 1e-6; cv::Mat rotation = parsed(cv::Rect(0, 0, 3, 3)).clone(); const double determinant = cv::determinant(rotation); const double orthogonalError = cv::norm(rotation.t() * rotation - cv::Mat::eye(3, 3, CV_64F), cv::NORM_INF); const bool rigidRotation = std::isfinite(determinant) && std::isfinite(orthogonalError) && std::abs(determinant - 1.0) < 0.02 && orthogonalError < 0.02; if (affineLastRow && rigidRotation) { m_matrix = parsed; m_loaded = true; m_file = path; m_matrixType = detectedType; m_matrixReferenceFrame = detectedReferenceFrame; const QString typeLabel = detectedType.isEmpty() ? QString::fromUtf8("未声明类型,按相机到基座矩阵使用") : detectedType; ui.labelHandEyeFile->setText( QStringLiteral("%1 (%2)").arg(QFileInfo(path).fileName(), typeLabel)); AppSettings::instance().set(AppSettings::K_HandEyeMatrix, path); if (!isEyeInHand()) { ui.labelCoordTransformStatus->setText(QString::fromUtf8( "手眼矩阵已加载;获得有效相机位姿后可转换到机械臂基座坐标系")); ui.labelCoordTransformStatus->setStyleSheet("color:#1e6f9f;"); emit logMessage(QString::fromUtf8("手眼矩阵已加载: %1(%2)") .arg(QFileInfo(path).fileName(), typeLabel)); if (detectedTranslationUnit == QStringLiteral("mm")) emit logMessage(QString::fromUtf8("手眼矩阵平移单位已从 mm 自动换算为 m")); } else { ui.labelCoordTransformStatus->setText(QString::fromUtf8( "已加载眼在手上矩阵;尚未提供与当前图像配对的拍照时刻 TCP 位姿")); ui.labelCoordTransformStatus->setStyleSheet("color:#c07b00;"); emit logMessage(QString::fromUtf8( "[提示] 眼在手上转换需要测量时刻的 TCP 位姿;输出参考坐标系与 TCP 输入一致")); } updateStates(); return true; } return false; } void CoordTransformPanel::onLoadMatrix() { QString p = AppSettings::instance().openFile( qobject_cast(parent()), AppSettings::K_HandEyeMatrix, QString::fromUtf8("导入手眼标定矩阵"), QString::fromUtf8("Matrix (*.txt *.mat *.dat);;All (*)")); if (p.isEmpty()) return; if (!loadFromFile(p)) emit logMessage(QString::fromUtf8( "[错误] 手眼矩阵解析失败(需要有效的4×4刚体变换矩阵,最后一行为0 0 0 1)")); } bool CoordTransformPanel::makeGraspPose(const VisionPoseResult& centerPose, VisionPoseResult& result) const { if (!centerPose.valid) return false; try { HTuple pose; pose[0] = centerPose.tx; pose[1] = centerPose.ty; pose[2] = centerPose.tz; pose[3] = centerPose.rx; pose[4] = centerPose.ry; pose[5] = centerPose.rz; pose[6] = centerPose.poseType; HTuple transform; PoseToHomMat3d(pose, &transform); const double offsetMeters = ui.spinGraspOffsetZMm->value() / 1000.0; result = centerPose; // 4.4 mm 是工件局部 +Z,不是相机坐标 Z;第三旋转列是局部 Z 在相机系的方向。 result.tx += transform[2].D() * offsetMeters; result.ty += transform[6].D() * offsetMeters; result.tz += transform[10].D() * offsetMeters; result.valid = true; return true; } catch (HException&) { return false; } } void CoordTransformPanel::onShowPartCenter() { if (!m_centerPose.valid) return; m_activeCameraPose = m_centerPose; m_activePointMode = CenterPoint; m_transformedPoseReady = false; m_transformedPose = VisionPoseResult(); ui.labelCoordTransformStatus->setText( QString::fromUtf8("当前显示: 零件中心(相机坐标系)")); ui.labelCoordTransformStatus->setStyleSheet("color:#1e6f9f;"); emit poseDisplayRequested(m_activeCameraPose, QStringLiteral("center")); emit logMessage(QString::fromUtf8("已还原零件中心位姿(相机坐标系)")); updateStates(); } void CoordTransformPanel::onShowGraspPoint() { VisionPoseResult graspPose; if (!makeGraspPose(m_centerPose, graspPose)) { emit logMessage(QString::fromUtf8("[抓取点转换错误] 当前没有有效的零件中心位姿")); return; } m_activeCameraPose = graspPose; m_activePointMode = GraspPoint; m_transformedPoseReady = false; m_transformedPose = VisionPoseResult(); ui.labelCoordTransformStatus->setText( QString::fromUtf8("当前显示: 实际抓取点(工件 +Z %1 mm,相机坐标系)") .arg(ui.spinGraspOffsetZMm->value(), 0, 'f', 3)); ui.labelCoordTransformStatus->setStyleSheet("color:#0b8f72;"); emit poseDisplayRequested(m_activeCameraPose, QStringLiteral("grasp")); emit logMessage(QString::fromUtf8( "实际抓取点转换完成:沿工件局部 +Z 偏移 %1 mm,姿态保持不变") .arg(ui.spinGraspOffsetZMm->value(), 0, 'f', 3)); updateStates(); } void CoordTransformPanel::onGraspOffsetChanged(double valueMm) { AppSettings::instance().set(AppSettings::K_GraspOffsetZMm, QString::number(valueMm, 'f', 3)); if (m_centerPose.valid && m_activePointMode == GraspPoint) onShowGraspPoint(); } void CoordTransformPanel::onTransform() { transformGraspPose(); } bool CoordTransformPanel::transformGraspPose() { if (m_axisGuidanceEnabled) { if (!m_axisGuidanceValid) { emit logMessage(QString::fromUtf8( "[回转体引导错误] 尚未标定中心轴抓取关系,请先输入实际抓取真值并执行标定")); return false; } if (!m_centerPose.valid) { emit logMessage(QString::fromUtf8("[坐标转换错误] 当前没有有效的零件中心位姿")); return false; } m_activeCameraPose = m_centerPose; m_activePointMode = CenterPoint; return performTransform(m_centerPose); } VisionPoseResult graspPose; if (!makeGraspPose(m_centerPose, graspPose)) { emit logMessage(QString::fromUtf8("[坐标转换错误] 当前没有有效的零件中心位姿")); return false; } m_activeCameraPose = graspPose; m_activePointMode = GraspPoint; return performTransform(graspPose); } bool CoordTransformPanel::transformActivePose() { return performTransform(m_activeCameraPose); } bool CoordTransformPanel::performTransform(const VisionPoseResult& cameraPose) { if (!m_loaded || !cameraPose.valid) return false; if (isEyeInHand() && !m_currentTcpPoseValid) { emit logMessage(QString::fromUtf8( "[坐标转换提示] 尚未提供与当前图像配对的拍照时刻 TCP XYZABC")); return false; } try { cv::Mat Tcam; if (!cameraPoseToTransform(cameraPose, Tcam)) { emit logMessage(QString::fromUtf8("[坐标转换错误] 相机位姿矩阵无效")); return false; } cv::Mat transformed; cv::Mat referencePart; if (isEyeInHand()) { const cv::Mat TreferenceTcp = estunTcpPoseToTransform( m_tcpXmm, m_tcpYmm, m_tcpZmm, m_tcpADeg, m_tcpBDeg, m_tcpCDeg); referencePart = TreferenceTcp * m_matrix * Tcam; transformed = referencePart; m_outputReferenceFrame = m_currentTcpReferenceFrame; } else { referencePart = m_matrix * Tcam; transformed = referencePart; m_outputReferenceFrame = m_matrixReferenceFrame.isEmpty() ? QStringLiteral("world") : m_matrixReferenceFrame; } if (m_axisGuidanceEnabled) { if (!m_axisGuidanceValid) { emit logMessage(QString::fromUtf8("[回转体引导错误] 中心轴抓取关系尚未标定")); return false; } if (m_outputReferenceFrame != m_axisGuidanceReferenceFrame) { emit logMessage(QString::fromUtf8( "[回转体引导错误] 当前 TCP 使用%1,但轴向抓取关系是在%2下标定;请切换一致或重新标定") .arg(referenceFrameLabel(m_outputReferenceFrame)) .arg(referenceFrameLabel(m_axisGuidanceReferenceFrame))); return false; } cv::Vec3d currentAxis(referencePart.at(0, 2), referencePart.at(1, 2), referencePart.at(2, 2)); const double currentAxisNorm = cv::norm(currentAxis); if (currentAxisNorm < 1e-9) { emit logMessage(QString::fromUtf8("[回转体引导错误] 当前中心轴向量无效")); return false; } currentAxis *= 1.0 / currentAxisNorm; // 回转体的轴是无向直线;选择与标定轴同向的一支,避免正负号跳变。 if (currentAxis.dot(m_axisGuidanceReferenceAxis) < 0.0) currentAxis *= -1.0; // 固定 B/C 后,R(A)=Rz(C)*Ry(B)*Rx(A)。把当前中心轴转到 // Q=Rz(C)*Ry(B) 后的坐标中,解析求出使 Rx(A)*v_tool 最接近它的 A。 const cv::Mat fixedPose = estunTcpPoseToTransform( 0.0, 0.0, 0.0, 0.0, m_axisGuidanceFixedBDeg, m_axisGuidanceFixedCDeg); const cv::Mat fixedRotation = fixedPose(cv::Rect(0, 0, 3, 3)); const cv::Mat currentAxisMat = (cv::Mat_(3, 1) << currentAxis[0], currentAxis[1], currentAxis[2]); const cv::Mat targetAfterFixed = fixedRotation.t() * currentAxisMat; const double uy = targetAfterFixed.at(1); const double uz = targetAfterFixed.at(2); const double vy = m_axisGuidanceAxisInTool[1]; const double vz = m_axisGuidanceAxisInTool[2]; const double cosineTerm = uy * vy + uz * vz; const double sineTerm = -uy * vz + uz * vy; if (std::hypot(cosineTerm, sineTerm) < 1e-6) { emit logMessage(QString::fromUtf8( "[回转体引导错误] 当前中心轴无法在固定B/C约束下可靠求解A角")); return false; } const double graspADeg = normalizeAngleDeg( std::atan2(sineTerm, cosineTerm) * 180.0 / kPi); const cv::Mat guidedPose = estunTcpPoseToTransform( 0.0, 0.0, 0.0, graspADeg, m_axisGuidanceFixedBDeg, m_axisGuidanceFixedCDeg); transformed = cv::Mat::eye(4, 4, CV_64F); guidedPose(cv::Rect(0, 0, 3, 3)).copyTo(transformed(cv::Rect(0, 0, 3, 3))); const double axialOffsetMeters = ui.spinGraspOffsetZMm->value() * 0.001; transformed.at(0, 3) = referencePart.at(0, 3) + currentAxis[0] * axialOffsetMeters + m_axisGuidancePositionOffset[0]; transformed.at(1, 3) = referencePart.at(1, 3) + currentAxis[1] * axialOffsetMeters + m_axisGuidancePositionOffset[1]; transformed.at(2, 3) = referencePart.at(2, 3) + currentAxis[2] * axialOffsetMeters + m_axisGuidancePositionOffset[2]; m_lastAxisVectorText = QStringLiteral("(%1, %2, %3)") .arg(currentAxis[0], 0, 'f', 6) .arg(currentAxis[1], 0, 'f', 6) .arg(currentAxis[2], 0, 'f', 6); } else { m_lastAxisVectorText.clear(); } // 现场残余系统偏差在最终参考坐标系下修正。轴向抓取标定已经学习基础偏差, // 此项仅用于多次验证后的少量精调,避免修改手眼矩阵。 if (m_systemBiasEnabled) { transformed.at(0, 3) += m_systemBiasMeters[0]; transformed.at(1, 3) += m_systemBiasMeters[1]; transformed.at(2, 3) += m_systemBiasMeters[2]; } double xMm = 0.0, yMm = 0.0, zMm = 0.0; double aDeg = 0.0, bDeg = 0.0, cDeg = 0.0; transformToEstunPose(transformed, xMm, yMm, zMm, aDeg, bDeg, cDeg); m_transformedPose = cameraPose; // 保留匹配分数等元数据 m_transformedPose.tx = xMm * 0.001; m_transformedPose.ty = yMm * 0.001; m_transformedPose.tz = zMm * 0.001; m_transformedPose.rx = aDeg; m_transformedPose.ry = bDeg; m_transformedPose.rz = cDeg; m_transformedPose.poseType = 0; m_transformedPose.valid = true; m_transformedPoseReady = true; const QString sourceName = m_axisGuidanceEnabled ? QString::fromUtf8("回转体垂直抓取(A由中心轴向量求解,B/C固定,忽略视觉绕轴角)") : (m_activePointMode == GraspPoint ? QString::fromUtf8("实际抓取点") : QString::fromUtf8("零件中心")); ui.labelCoordTransformStatus->setText( QString::fromUtf8("抓取位姿 XYZABC(%1)X=%2 Y=%3 Z=%4 A=%5 B=%6 C=%7") .arg(referenceFrameLabel(m_outputReferenceFrame)) .arg(m_transformedPose.tx*1000,0,'f',3) .arg(m_transformedPose.ty*1000,0,'f',3) .arg(m_transformedPose.tz*1000,0,'f',3) .arg(m_transformedPose.rx,0,'f',3) .arg(m_transformedPose.ry,0,'f',3) .arg(m_transformedPose.rz,0,'f',3)); ui.labelCoordTransformStatus->setStyleSheet("color:#00cc66;"); emit poseDisplayRequested(m_transformedPose, QStringLiteral("robot")); emit transformedPoseUpdated(m_transformedPose, m_outputReferenceFrame); emit logMessage(QString::fromUtf8( "抓取位姿转换完成(%1,来源: %2)X=%3 Y=%4 Z=%5 mm A=%6 B=%7 C=%8 deg") .arg(referenceFrameLabel(m_outputReferenceFrame)) .arg(sourceName) .arg(m_transformedPose.tx*1000,0,'f',3) .arg(m_transformedPose.ty*1000,0,'f',3) .arg(m_transformedPose.tz*1000,0,'f',3) .arg(m_transformedPose.rx,0,'f',3) .arg(m_transformedPose.ry,0,'f',3) .arg(m_transformedPose.rz,0,'f',3)); if (m_axisGuidanceEnabled) { emit logMessage(QString::fromUtf8( "回转体中心轴(%1)=%2;视觉绕轴 Rz 未参与抓取姿态") .arg(referenceFrameLabel(m_outputReferenceFrame), m_lastAxisVectorText)); } if (m_systemBiasEnabled) { emit logMessage(QString::fromUtf8( "已应用系统偏差修正(%1)ΔX=%2 ΔY=%3 ΔZ=%4 mm") .arg(referenceFrameLabel(m_outputReferenceFrame)) .arg(m_systemBiasMeters[0] * 1000.0, 0, 'f', 3) .arg(m_systemBiasMeters[1] * 1000.0, 0, 'f', 3) .arg(m_systemBiasMeters[2] * 1000.0, 0, 'f', 3)); } updateStates(); return true; } catch (HException& e) { emit logMessage(QString::fromUtf8("[坐标转换错误] ") + sanitizeUserVisibleLog(QString::fromLocal8Bit(e.ErrorMessage().TextA()))); return false; } catch (...) { emit logMessage(QString::fromUtf8("[坐标转换错误] 矩阵计算失败")); return false; } }