855 lines
36 KiB
C++
855 lines
36 KiB
C++
// 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;
|
||
}
|
||
}
|