330 lines
11 KiB
C++
330 lines
11 KiB
C++
#include "CalibResultWidget.h"
|
|
|
|
#include <QVBoxLayout>
|
|
#include <QHBoxLayout>
|
|
#include <QGridLayout>
|
|
#include <QHeaderView>
|
|
#include <QScrollArea>
|
|
|
|
CalibResultWidget::CalibResultWidget(QWidget* parent)
|
|
: QWidget(parent)
|
|
, m_tableRotation(nullptr)
|
|
, m_lblTransX(nullptr)
|
|
, m_lblTransY(nullptr)
|
|
, m_lblTransZ(nullptr)
|
|
, m_lblRoll(nullptr)
|
|
, m_lblPitch(nullptr)
|
|
, m_lblYaw(nullptr)
|
|
, m_lblRollDeg(nullptr)
|
|
, m_lblPitchDeg(nullptr)
|
|
, m_lblYawDeg(nullptr)
|
|
, m_lblError(nullptr)
|
|
, m_lblCenterEye(nullptr)
|
|
, m_lblCenterRobot(nullptr)
|
|
, m_logEdit(nullptr)
|
|
, m_hasResult(false)
|
|
{
|
|
setupUI();
|
|
}
|
|
|
|
CalibResultWidget::~CalibResultWidget()
|
|
{
|
|
}
|
|
|
|
void CalibResultWidget::setupUI()
|
|
{
|
|
QVBoxLayout* mainLayout = new QVBoxLayout(this);
|
|
|
|
// 旋转矩阵
|
|
mainLayout->addWidget(createRotationGroup());
|
|
|
|
// 平移向量
|
|
mainLayout->addWidget(createTranslationGroup());
|
|
|
|
// 欧拉角
|
|
mainLayout->addWidget(createEulerGroup());
|
|
|
|
// 误差
|
|
mainLayout->addWidget(createErrorGroup());
|
|
|
|
// 日志
|
|
mainLayout->addWidget(createLogGroup(), 1);
|
|
}
|
|
|
|
QGroupBox* CalibResultWidget::createRotationGroup()
|
|
{
|
|
QGroupBox* group = new QGroupBox("旋转矩阵 R", this);
|
|
QVBoxLayout* layout = new QVBoxLayout(group);
|
|
|
|
m_tableRotation = new QTableWidget(3, 3, this);
|
|
m_tableRotation->setHorizontalHeaderLabels({"Col 0", "Col 1", "Col 2"});
|
|
m_tableRotation->setVerticalHeaderLabels({"Row 0", "Row 1", "Row 2"});
|
|
m_tableRotation->horizontalHeader()->setSectionResizeMode(QHeaderView::Stretch);
|
|
m_tableRotation->verticalHeader()->setSectionResizeMode(QHeaderView::Stretch);
|
|
m_tableRotation->setEditTriggers(QAbstractItemView::NoEditTriggers);
|
|
m_tableRotation->setMaximumHeight(120);
|
|
|
|
// 初始化为单位矩阵
|
|
for (int i = 0; i < 3; ++i) {
|
|
for (int j = 0; j < 3; ++j) {
|
|
QTableWidgetItem* item = new QTableWidgetItem(i == j ? "1.000000" : "0.000000");
|
|
item->setTextAlignment(Qt::AlignCenter);
|
|
m_tableRotation->setItem(i, j, item);
|
|
}
|
|
}
|
|
|
|
layout->addWidget(m_tableRotation);
|
|
return group;
|
|
}
|
|
|
|
QGroupBox* CalibResultWidget::createTranslationGroup()
|
|
{
|
|
QGroupBox* group = new QGroupBox("平移向量 T", this);
|
|
QGridLayout* layout = new QGridLayout(group);
|
|
|
|
layout->addWidget(new QLabel("X:", this), 0, 0);
|
|
m_lblTransX = new QLabel("0.000", this);
|
|
m_lblTransX->setStyleSheet("font-weight: bold;");
|
|
layout->addWidget(m_lblTransX, 0, 1);
|
|
|
|
layout->addWidget(new QLabel("Y:", this), 0, 2);
|
|
m_lblTransY = new QLabel("0.000", this);
|
|
m_lblTransY->setStyleSheet("font-weight: bold;");
|
|
layout->addWidget(m_lblTransY, 0, 3);
|
|
|
|
layout->addWidget(new QLabel("Z:", this), 0, 4);
|
|
m_lblTransZ = new QLabel("0.000", this);
|
|
m_lblTransZ->setStyleSheet("font-weight: bold;");
|
|
layout->addWidget(m_lblTransZ, 0, 5);
|
|
|
|
return group;
|
|
}
|
|
|
|
QGroupBox* CalibResultWidget::createEulerGroup()
|
|
{
|
|
QGroupBox* group = new QGroupBox("欧拉角 (ZYX顺序)", this);
|
|
QGridLayout* layout = new QGridLayout(group);
|
|
|
|
// 弧度
|
|
layout->addWidget(new QLabel("Roll (rad):", this), 0, 0);
|
|
m_lblRoll = new QLabel("0.000000", this);
|
|
layout->addWidget(m_lblRoll, 0, 1);
|
|
|
|
layout->addWidget(new QLabel("Pitch (rad):", this), 0, 2);
|
|
m_lblPitch = new QLabel("0.000000", this);
|
|
layout->addWidget(m_lblPitch, 0, 3);
|
|
|
|
layout->addWidget(new QLabel("Yaw (rad):", this), 0, 4);
|
|
m_lblYaw = new QLabel("0.000000", this);
|
|
layout->addWidget(m_lblYaw, 0, 5);
|
|
|
|
// 角度
|
|
layout->addWidget(new QLabel("Roll (°):", this), 1, 0);
|
|
m_lblRollDeg = new QLabel("0.00", this);
|
|
m_lblRollDeg->setStyleSheet("font-weight: bold; color: blue;");
|
|
layout->addWidget(m_lblRollDeg, 1, 1);
|
|
|
|
layout->addWidget(new QLabel("Pitch (°):", this), 1, 2);
|
|
m_lblPitchDeg = new QLabel("0.00", this);
|
|
m_lblPitchDeg->setStyleSheet("font-weight: bold; color: blue;");
|
|
layout->addWidget(m_lblPitchDeg, 1, 3);
|
|
|
|
layout->addWidget(new QLabel("Yaw (°):", this), 1, 4);
|
|
m_lblYawDeg = new QLabel("0.00", this);
|
|
m_lblYawDeg->setStyleSheet("font-weight: bold; color: blue;");
|
|
layout->addWidget(m_lblYawDeg, 1, 5);
|
|
|
|
return group;
|
|
}
|
|
|
|
QGroupBox* CalibResultWidget::createErrorGroup()
|
|
{
|
|
QGroupBox* group = new QGroupBox("标定信息", this);
|
|
QGridLayout* layout = new QGridLayout(group);
|
|
|
|
layout->addWidget(new QLabel("标定误差:", this), 0, 0);
|
|
m_lblError = new QLabel("0.0000 mm", this);
|
|
m_lblError->setStyleSheet("font-weight: bold; color: red;");
|
|
layout->addWidget(m_lblError, 0, 1);
|
|
|
|
layout->addWidget(new QLabel("眼坐标系质心:", this), 1, 0);
|
|
m_lblCenterEye = new QLabel("(0.000, 0.000, 0.000)", this);
|
|
layout->addWidget(m_lblCenterEye, 1, 1);
|
|
|
|
layout->addWidget(new QLabel("机器人坐标系质心:", this), 2, 0);
|
|
m_lblCenterRobot = new QLabel("(0.000, 0.000, 0.000)", this);
|
|
layout->addWidget(m_lblCenterRobot, 2, 1);
|
|
|
|
return group;
|
|
}
|
|
|
|
QGroupBox* CalibResultWidget::createLogGroup()
|
|
{
|
|
QGroupBox* group = new QGroupBox("日志", this);
|
|
QVBoxLayout* layout = new QVBoxLayout(group);
|
|
|
|
m_logEdit = new QTextEdit(this);
|
|
m_logEdit->setReadOnly(true);
|
|
m_logEdit->setFont(QFont("Consolas", 9));
|
|
|
|
layout->addWidget(m_logEdit);
|
|
return group;
|
|
}
|
|
|
|
void CalibResultWidget::updateRotationDisplay(const HECRotationMatrix& R)
|
|
{
|
|
for (int i = 0; i < 3; ++i) {
|
|
for (int j = 0; j < 3; ++j) {
|
|
QTableWidgetItem* item = m_tableRotation->item(i, j);
|
|
if (item) {
|
|
item->setText(QString::number(R.at(i, j), 'f', 6));
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void CalibResultWidget::updateTranslationDisplay(const HECTranslationVector& T)
|
|
{
|
|
m_lblTransX->setText(QString::number(T.at(0), 'f', 3));
|
|
m_lblTransY->setText(QString::number(T.at(1), 'f', 3));
|
|
m_lblTransZ->setText(QString::number(T.at(2), 'f', 3));
|
|
}
|
|
|
|
void CalibResultWidget::updateEulerDisplay(const HECEulerAngles& angles)
|
|
{
|
|
m_lblRoll->setText(QString::number(angles.roll, 'f', 6));
|
|
m_lblPitch->setText(QString::number(angles.pitch, 'f', 6));
|
|
m_lblYaw->setText(QString::number(angles.yaw, 'f', 6));
|
|
|
|
double rollDeg, pitchDeg, yawDeg;
|
|
angles.toDegrees(rollDeg, pitchDeg, yawDeg);
|
|
m_lblRollDeg->setText(QString::number(rollDeg, 'f', 2));
|
|
m_lblPitchDeg->setText(QString::number(pitchDeg, 'f', 2));
|
|
m_lblYawDeg->setText(QString::number(yawDeg, 'f', 2));
|
|
}
|
|
|
|
void CalibResultWidget::showCalibResult(const HECCalibResult& result)
|
|
{
|
|
m_currentResult = result;
|
|
m_hasResult = true;
|
|
|
|
// 更新旋转矩阵
|
|
updateRotationDisplay(result.R);
|
|
|
|
// 更新平移向量
|
|
updateTranslationDisplay(result.T);
|
|
|
|
// 计算并更新欧拉角 (使用 ZYX 顺序)
|
|
// 从旋转矩阵提取欧拉角
|
|
HECEulerAngles angles;
|
|
double sy = -result.R.at(2, 0);
|
|
if (std::abs(sy) < 0.99999) {
|
|
angles.pitch = std::asin(sy);
|
|
angles.roll = std::atan2(result.R.at(2, 1), result.R.at(2, 2));
|
|
angles.yaw = std::atan2(result.R.at(1, 0), result.R.at(0, 0));
|
|
} else {
|
|
// 万向锁情况
|
|
angles.pitch = sy > 0 ? M_PI / 2 : -M_PI / 2;
|
|
angles.roll = 0;
|
|
angles.yaw = std::atan2(-result.R.at(0, 1), result.R.at(1, 1));
|
|
}
|
|
updateEulerDisplay(angles);
|
|
|
|
// 更新误差
|
|
m_lblError->setText(QString::number(result.error, 'f', 4) + " mm");
|
|
|
|
// 更新质心
|
|
m_lblCenterEye->setText(QString("(%1, %2, %3)")
|
|
.arg(result.centerEye.x, 0, 'f', 3)
|
|
.arg(result.centerEye.y, 0, 'f', 3)
|
|
.arg(result.centerEye.z, 0, 'f', 3));
|
|
|
|
m_lblCenterRobot->setText(QString("(%1, %2, %3)")
|
|
.arg(result.centerRobot.x, 0, 'f', 3)
|
|
.arg(result.centerRobot.y, 0, 'f', 3)
|
|
.arg(result.centerRobot.z, 0, 'f', 3));
|
|
}
|
|
|
|
void CalibResultWidget::showTransformResult(const HECPoint3D& srcPoint,
|
|
const HECPoint3D& dstPoint)
|
|
{
|
|
appendLog(QString("坐标变换结果:"));
|
|
appendLog(QString(" 源点: (%1, %2, %3)")
|
|
.arg(srcPoint.x, 0, 'f', 3)
|
|
.arg(srcPoint.y, 0, 'f', 3)
|
|
.arg(srcPoint.z, 0, 'f', 3));
|
|
appendLog(QString(" 目标点: (%1, %2, %3)")
|
|
.arg(dstPoint.x, 0, 'f', 3)
|
|
.arg(dstPoint.y, 0, 'f', 3)
|
|
.arg(dstPoint.z, 0, 'f', 3));
|
|
}
|
|
|
|
void CalibResultWidget::showEulerResult(const HECEulerAngles& inputAngles,
|
|
const HECRotationMatrix& R,
|
|
const HECEulerAngles& outputAngles,
|
|
HECEulerOrder order)
|
|
{
|
|
// 更新旋转矩阵显示
|
|
updateRotationDisplay(R);
|
|
|
|
// 更新欧拉角显示
|
|
updateEulerDisplay(outputAngles);
|
|
|
|
// 获取顺序名称
|
|
QString orderName;
|
|
switch (order) {
|
|
case HECEulerOrder::XYZ: orderName = "XYZ"; break;
|
|
case HECEulerOrder::XZY: orderName = "XZY"; break;
|
|
case HECEulerOrder::YXZ: orderName = "YXZ"; break;
|
|
case HECEulerOrder::YZX: orderName = "YZX"; break;
|
|
case HECEulerOrder::ZXY: orderName = "ZXY"; break;
|
|
case HECEulerOrder::ZYX: orderName = "ZYX"; break;
|
|
}
|
|
|
|
appendLog(QString("欧拉角转换 (顺序: %1):").arg(orderName));
|
|
|
|
double inRoll, inPitch, inYaw;
|
|
inputAngles.toDegrees(inRoll, inPitch, inYaw);
|
|
appendLog(QString(" 输入: Roll=%1°, Pitch=%2°, Yaw=%3°")
|
|
.arg(inRoll, 0, 'f', 2)
|
|
.arg(inPitch, 0, 'f', 2)
|
|
.arg(inYaw, 0, 'f', 2));
|
|
|
|
double outRoll, outPitch, outYaw;
|
|
outputAngles.toDegrees(outRoll, outPitch, outYaw);
|
|
appendLog(QString(" 输出: Roll=%1°, Pitch=%2°, Yaw=%3°")
|
|
.arg(outRoll, 0, 'f', 2)
|
|
.arg(outPitch, 0, 'f', 2)
|
|
.arg(outYaw, 0, 'f', 2));
|
|
}
|
|
|
|
void CalibResultWidget::clearAll()
|
|
{
|
|
// 重置旋转矩阵为单位矩阵
|
|
HECRotationMatrix identity;
|
|
updateRotationDisplay(identity);
|
|
|
|
// 重置平移向量
|
|
HECTranslationVector zero;
|
|
updateTranslationDisplay(zero);
|
|
|
|
// 重置欧拉角
|
|
HECEulerAngles zeroAngles;
|
|
updateEulerDisplay(zeroAngles);
|
|
|
|
// 重置误差和质心
|
|
m_lblError->setText("0.0000 mm");
|
|
m_lblCenterEye->setText("(0.000, 0.000, 0.000)");
|
|
m_lblCenterRobot->setText("(0.000, 0.000, 0.000)");
|
|
|
|
// 清除日志
|
|
m_logEdit->clear();
|
|
|
|
m_hasResult = false;
|
|
}
|
|
|
|
void CalibResultWidget::appendLog(const QString& message)
|
|
{
|
|
m_logEdit->append(message);
|
|
}
|