first Commit

This commit is contained in:
lidg
2026-10-26 10:56:56 +08:00
commit c8cd533d9b
81 changed files with 128540 additions and 0 deletions
+854
View File
@@ -0,0 +1,854 @@
// CoordTransformPanel.cpp
#include "UserVisibleLog.h"
#include "CoordTransformPanel.h"
#include "ui_MainWindow.h"
#include "AppSettings.h"
#include <QFile>
#include <QTextStream>
#include <QFileInfo>
#include <QRegExp>
#include <QDoubleSpinBox>
#include <QSignalBlocker>
#include <algorithm>
#include <cmath>
#include <VisionGuideApi.h>
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_<double>(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_<double>(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_<double>(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<double>(0, 3) = xMm * 0.001;
result.at<double>(1, 3) = yMm * 0.001;
result.at<double>(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<double>(0, 3) * 1000.0;
yMm = matrix.at<double>(1, 3) * 1000.0;
zMm = matrix.at<double>(2, 3) * 1000.0;
double sinB = -matrix.at<double>(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<double>(2, 1), matrix.at<double>(2, 2));
c = std::atan2(matrix.at<double>(1, 0), matrix.at<double>(0, 0));
} else {
a = std::atan2(-matrix.at<double>(1, 2), matrix.at<double>(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<double>::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<double>(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<double>(row, column) += 2.0 * axis[row] * axis[column];
}
return rotation;
}
const cv::Mat skew = (cv::Mat_<double>(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<double>(0, 2),
referencePart.at<double>(1, 2),
referencePart.at<double>(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<double>(0, 3),
referencePart.at<double>(1, 3),
referencePart.at<double>(2, 3));
const cv::Vec3d graspPoint(referenceGrasp.at<double>(0, 3),
referenceGrasp.at<double>(1, 3),
referenceGrasp.at<double>(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_<double>(3, 1) << axis[0], axis[1], axis[2]);
const cv::Mat axisToolMat = graspRotation.t() * axisReferenceMat;
m_axisGuidanceAxisInTool = cv::Vec3d(
axisToolMat.at<double>(0), axisToolMat.at<double>(1), axisToolMat.at<double>(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<double>(row, c) = value;
}
if (!valuesOk) break;
++row;
}
f.close();
// 应用内生成的位姿与矩阵平移均以米为单位;兼容明确声明为毫米的外部矩阵。
if (valuesOk && row == 4 && detectedTranslationUnit == QStringLiteral("mm")) {
parsed.at<double>(0, 3) /= 1000.0;
parsed.at<double>(1, 3) /= 1000.0;
parsed.at<double>(2, 3) /= 1000.0;
} else if (!detectedTranslationUnit.isEmpty() &&
detectedTranslationUnit != QStringLiteral("m")) {
return false;
}
const bool affineLastRow = valuesOk && row == 4 &&
std::abs(parsed.at<double>(3, 0)) < 1e-6 &&
std::abs(parsed.at<double>(3, 1)) < 1e-6 &&
std::abs(parsed.at<double>(3, 2)) < 1e-6 &&
std::abs(parsed.at<double>(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<QWidget*>(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<double>(0, 2),
referencePart.at<double>(1, 2),
referencePart.at<double>(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_<double>(3, 1) <<
currentAxis[0], currentAxis[1], currentAxis[2]);
const cv::Mat targetAfterFixed = fixedRotation.t() * currentAxisMat;
const double uy = targetAfterFixed.at<double>(1);
const double uz = targetAfterFixed.at<double>(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<double>(0, 3) = referencePart.at<double>(0, 3) +
currentAxis[0] * axialOffsetMeters + m_axisGuidancePositionOffset[0];
transformed.at<double>(1, 3) = referencePart.at<double>(1, 3) +
currentAxis[1] * axialOffsetMeters + m_axisGuidancePositionOffset[1];
transformed.at<double>(2, 3) = referencePart.at<double>(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<double>(0, 3) += m_systemBiasMeters[0];
transformed.at<double>(1, 3) += m_systemBiasMeters[1];
transformed.at<double>(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;
}
}