Files
VisionGuide/source/CoordTransformPanel.cpp
T
2026-10-26 10:56:56 +08:00

855 lines
36 KiB
C++
Raw Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
// 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;
}
}