700 lines
27 KiB
C++
700 lines
27 KiB
C++
#include "CalibDataWidget.h"
|
||
|
||
#include <QVBoxLayout>
|
||
#include <QHBoxLayout>
|
||
#include <QGridLayout>
|
||
#include <QHeaderView>
|
||
#include <QLabel>
|
||
|
||
CalibDataWidget::CalibDataWidget(QWidget* parent)
|
||
: QWidget(parent)
|
||
, m_cbCalibType(nullptr)
|
||
, m_tableEyeToHand(nullptr)
|
||
, m_groupEyeToHand(nullptr)
|
||
, m_tableEyeInHand(nullptr)
|
||
, m_groupEyeInHand(nullptr)
|
||
, m_btnEyeToHandAddRow(nullptr)
|
||
, m_btnEyeToHandDeleteRow(nullptr)
|
||
, m_btnEyeToHandCalib(nullptr)
|
||
, m_btnEyeInHandAddRow(nullptr)
|
||
, m_btnEyeInHandDeleteRow(nullptr)
|
||
, m_btnEyeInHandCalib(nullptr)
|
||
, m_groupTCPCalib(nullptr)
|
||
, m_tableTCP(nullptr)
|
||
, m_tcpModeCombo(nullptr)
|
||
, m_tcpEulerOrderCombo(nullptr)
|
||
, m_tcpRefPoseIndex(nullptr)
|
||
, m_tcpWorldRx(nullptr)
|
||
, m_tcpWorldRy(nullptr)
|
||
, m_tcpWorldRz(nullptr)
|
||
, m_tcpOrientationGroup(nullptr)
|
||
, m_tcpAddRowBtn(nullptr)
|
||
, m_tcpRemoveRowBtn(nullptr)
|
||
, m_btnTCPCalib(nullptr)
|
||
, m_inputEyeX(nullptr)
|
||
, m_inputEyeY(nullptr)
|
||
, m_inputEyeZ(nullptr)
|
||
, m_inputRobotX(nullptr)
|
||
, m_inputRobotY(nullptr)
|
||
, m_inputRobotZ(nullptr)
|
||
, m_btnEyeToHandAddInput(nullptr)
|
||
, m_inputEndX(nullptr)
|
||
, m_inputEndY(nullptr)
|
||
, m_inputEndZ(nullptr)
|
||
, m_inputEndRoll(nullptr)
|
||
, m_inputEndPitch(nullptr)
|
||
, m_inputEndYaw(nullptr)
|
||
, m_inputCamX(nullptr)
|
||
, m_inputCamY(nullptr)
|
||
, m_inputCamZ(nullptr)
|
||
, m_btnEyeInHandAddInput(nullptr)
|
||
, m_inputTcpX(nullptr)
|
||
, m_inputTcpY(nullptr)
|
||
, m_inputTcpZ(nullptr)
|
||
, m_inputTcpRx(nullptr)
|
||
, m_inputTcpRy(nullptr)
|
||
, m_inputTcpRz(nullptr)
|
||
, m_btnTcpAddInput(nullptr)
|
||
{
|
||
setupUI();
|
||
}
|
||
|
||
CalibDataWidget::~CalibDataWidget()
|
||
{
|
||
}
|
||
|
||
void CalibDataWidget::setupUI()
|
||
{
|
||
QVBoxLayout* mainLayout = new QVBoxLayout(this);
|
||
|
||
// 标定模式选择
|
||
QHBoxLayout* modeLayout = new QHBoxLayout();
|
||
QLabel* lblMode = new QLabel("标定模式:", this);
|
||
m_cbCalibType = new QComboBox(this);
|
||
m_cbCalibType->addItem("Eye-To-Hand (眼在手外)");
|
||
m_cbCalibType->addItem("Eye-In-Hand (眼在手上)");
|
||
m_cbCalibType->addItem("TCP 标定");
|
||
connect(m_cbCalibType, QOverload<int>::of(&QComboBox::currentIndexChanged),
|
||
this, &CalibDataWidget::onCalibTypeChanged);
|
||
modeLayout->addWidget(lblMode);
|
||
modeLayout->addWidget(m_cbCalibType);
|
||
modeLayout->addStretch();
|
||
mainLayout->addLayout(modeLayout);
|
||
|
||
// Eye-To-Hand 数据组
|
||
m_groupEyeToHand = createEyeToHandGroup();
|
||
mainLayout->addWidget(m_groupEyeToHand, 1);
|
||
|
||
// Eye-In-Hand 数据组
|
||
m_groupEyeInHand = createEyeInHandGroup();
|
||
mainLayout->addWidget(m_groupEyeInHand, 1);
|
||
m_groupEyeInHand->setVisible(false);
|
||
|
||
// TCP 标定数据组
|
||
m_groupTCPCalib = createTCPCalibGroup();
|
||
mainLayout->addWidget(m_groupTCPCalib, 1);
|
||
m_groupTCPCalib->setVisible(false);
|
||
|
||
// 初始化按钮启用状态
|
||
onCalibTypeChanged(0);
|
||
}
|
||
|
||
QWidget* CalibDataWidget::createEyeToHandGroup()
|
||
{
|
||
QWidget* group = new QWidget(this);
|
||
QVBoxLayout* layout = new QVBoxLayout(group);
|
||
layout->setContentsMargins(0, 0, 0, 0);
|
||
|
||
m_tableEyeToHand = new QTableWidget(this);
|
||
m_tableEyeToHand->setColumnCount(6);
|
||
m_tableEyeToHand->setHorizontalHeaderLabels({
|
||
"Eye X", "Eye Y", "Eye Z", "Robot X", "Robot Y", "Robot Z"
|
||
});
|
||
m_tableEyeToHand->horizontalHeader()->setSectionResizeMode(QHeaderView::Stretch);
|
||
m_tableEyeToHand->setSelectionBehavior(QAbstractItemView::SelectRows);
|
||
|
||
layout->addWidget(m_tableEyeToHand, 1);
|
||
|
||
// 内联按钮行
|
||
QHBoxLayout* btnLayout = new QHBoxLayout();
|
||
m_btnEyeToHandAddRow = new QPushButton("添加行", this);
|
||
m_btnEyeToHandDeleteRow = new QPushButton("删除行", this);
|
||
m_btnEyeToHandCalib = new QPushButton("Eye-To-Hand 标定", this);
|
||
|
||
connect(m_btnEyeToHandAddRow, &QPushButton::clicked, this, [this]() {
|
||
int row = m_tableEyeToHand->rowCount();
|
||
m_tableEyeToHand->insertRow(row);
|
||
for (int col = 0; col < 6; ++col) {
|
||
m_tableEyeToHand->setItem(row, col, new QTableWidgetItem("0"));
|
||
}
|
||
m_tableEyeToHand->scrollToBottom();
|
||
});
|
||
connect(m_btnEyeToHandDeleteRow, &QPushButton::clicked, this, [this]() {
|
||
int row = m_tableEyeToHand->currentRow();
|
||
if (row >= 0) {
|
||
m_tableEyeToHand->removeRow(row);
|
||
}
|
||
});
|
||
connect(m_btnEyeToHandCalib, &QPushButton::clicked, this, &CalibDataWidget::requestEyeToHandCalib);
|
||
|
||
btnLayout->addWidget(m_btnEyeToHandAddRow);
|
||
btnLayout->addWidget(m_btnEyeToHandDeleteRow);
|
||
btnLayout->addWidget(m_btnEyeToHandCalib);
|
||
btnLayout->addStretch();
|
||
layout->addLayout(btnLayout);
|
||
|
||
// 数据输入区
|
||
QGroupBox* inputGroup = new QGroupBox("数据输入", group);
|
||
QGridLayout* inputLayout = new QGridLayout(inputGroup);
|
||
inputLayout->setHorizontalSpacing(2);
|
||
inputLayout->setVerticalSpacing(4);
|
||
inputLayout->setContentsMargins(4, 4, 4, 4);
|
||
inputLayout->setColumnStretch(0, 0);
|
||
inputLayout->setColumnStretch(1, 1);
|
||
inputLayout->setColumnStretch(2, 0);
|
||
inputLayout->setColumnStretch(3, 1);
|
||
inputLayout->setColumnStretch(4, 0);
|
||
inputLayout->setColumnStretch(5, 1);
|
||
inputLayout->setColumnStretch(6, 0);
|
||
|
||
auto createSpinBox = [this]() {
|
||
QDoubleSpinBox* sb = new QDoubleSpinBox(this);
|
||
sb->setRange(-10000, 10000);
|
||
sb->setDecimals(3);
|
||
return sb;
|
||
};
|
||
|
||
// 第1行：Eye X/Y/Z
|
||
inputLayout->addWidget(new QLabel("Eye X:", this), 0, 0);
|
||
m_inputEyeX = createSpinBox();
|
||
inputLayout->addWidget(m_inputEyeX, 0, 1);
|
||
inputLayout->addWidget(new QLabel("Y:", this), 0, 2);
|
||
m_inputEyeY = createSpinBox();
|
||
inputLayout->addWidget(m_inputEyeY, 0, 3);
|
||
inputLayout->addWidget(new QLabel("Z:", this), 0, 4);
|
||
m_inputEyeZ = createSpinBox();
|
||
inputLayout->addWidget(m_inputEyeZ, 0, 5);
|
||
|
||
// 第2行：Robot X/Y/Z + 添加按钮
|
||
inputLayout->addWidget(new QLabel("Robot X:", this), 1, 0);
|
||
m_inputRobotX = createSpinBox();
|
||
inputLayout->addWidget(m_inputRobotX, 1, 1);
|
||
inputLayout->addWidget(new QLabel("Y:", this), 1, 2);
|
||
m_inputRobotY = createSpinBox();
|
||
inputLayout->addWidget(m_inputRobotY, 1, 3);
|
||
inputLayout->addWidget(new QLabel("Z:", this), 1, 4);
|
||
m_inputRobotZ = createSpinBox();
|
||
inputLayout->addWidget(m_inputRobotZ, 1, 5);
|
||
|
||
m_btnEyeToHandAddInput = new QPushButton("添加到表格", this);
|
||
connect(m_btnEyeToHandAddInput, &QPushButton::clicked, this, [this]() {
|
||
int row = m_tableEyeToHand->rowCount();
|
||
m_tableEyeToHand->insertRow(row);
|
||
m_tableEyeToHand->setItem(row, 0, new QTableWidgetItem(QString::number(m_inputEyeX->value(), 'f', 3)));
|
||
m_tableEyeToHand->setItem(row, 1, new QTableWidgetItem(QString::number(m_inputEyeY->value(), 'f', 3)));
|
||
m_tableEyeToHand->setItem(row, 2, new QTableWidgetItem(QString::number(m_inputEyeZ->value(), 'f', 3)));
|
||
m_tableEyeToHand->setItem(row, 3, new QTableWidgetItem(QString::number(m_inputRobotX->value(), 'f', 3)));
|
||
m_tableEyeToHand->setItem(row, 4, new QTableWidgetItem(QString::number(m_inputRobotY->value(), 'f', 3)));
|
||
m_tableEyeToHand->setItem(row, 5, new QTableWidgetItem(QString::number(m_inputRobotZ->value(), 'f', 3)));
|
||
m_tableEyeToHand->scrollToBottom();
|
||
});
|
||
inputLayout->addWidget(m_btnEyeToHandAddInput, 1, 6);
|
||
|
||
layout->addWidget(inputGroup);
|
||
|
||
return group;
|
||
}
|
||
|
||
QWidget* CalibDataWidget::createEyeInHandGroup()
|
||
{
|
||
QWidget* group = new QWidget(this);
|
||
QVBoxLayout* layout = new QVBoxLayout(group);
|
||
layout->setContentsMargins(0, 0, 0, 0);
|
||
|
||
m_tableEyeInHand = new QTableWidget(this);
|
||
m_tableEyeInHand->setColumnCount(9);
|
||
m_tableEyeInHand->setHorizontalHeaderLabels({
|
||
"End X", "End Y", "End Z", "End Roll", "End Pitch", "End Yaw",
|
||
"Cam X", "Cam Y", "Cam Z"
|
||
});
|
||
m_tableEyeInHand->horizontalHeader()->setSectionResizeMode(QHeaderView::Stretch);
|
||
m_tableEyeInHand->setSelectionBehavior(QAbstractItemView::SelectRows);
|
||
|
||
layout->addWidget(m_tableEyeInHand, 1);
|
||
|
||
// 内联按钮行
|
||
QHBoxLayout* btnLayout = new QHBoxLayout();
|
||
m_btnEyeInHandAddRow = new QPushButton("添加行", this);
|
||
m_btnEyeInHandDeleteRow = new QPushButton("删除行", this);
|
||
m_btnEyeInHandCalib = new QPushButton("Eye-In-Hand 标定", this);
|
||
|
||
connect(m_btnEyeInHandAddRow, &QPushButton::clicked, this, [this]() {
|
||
int row = m_tableEyeInHand->rowCount();
|
||
m_tableEyeInHand->insertRow(row);
|
||
for (int col = 0; col < 9; ++col) {
|
||
m_tableEyeInHand->setItem(row, col, new QTableWidgetItem("0"));
|
||
}
|
||
m_tableEyeInHand->scrollToBottom();
|
||
});
|
||
connect(m_btnEyeInHandDeleteRow, &QPushButton::clicked, this, [this]() {
|
||
int row = m_tableEyeInHand->currentRow();
|
||
if (row >= 0) {
|
||
m_tableEyeInHand->removeRow(row);
|
||
}
|
||
});
|
||
connect(m_btnEyeInHandCalib, &QPushButton::clicked, this, &CalibDataWidget::requestEyeInHandCalib);
|
||
|
||
btnLayout->addWidget(m_btnEyeInHandAddRow);
|
||
btnLayout->addWidget(m_btnEyeInHandDeleteRow);
|
||
btnLayout->addWidget(m_btnEyeInHandCalib);
|
||
btnLayout->addStretch();
|
||
layout->addLayout(btnLayout);
|
||
|
||
// 数据输入区
|
||
QGroupBox* inputGroup = new QGroupBox("数据输入", group);
|
||
QGridLayout* inputLayout = new QGridLayout(inputGroup);
|
||
inputLayout->setHorizontalSpacing(2);
|
||
inputLayout->setVerticalSpacing(4);
|
||
inputLayout->setContentsMargins(4, 4, 4, 4);
|
||
for (int c = 0; c <= 11; ++c)
|
||
inputLayout->setColumnStretch(c, (c % 2 == 1) ? 1 : 0);
|
||
|
||
auto createSpinBox = [this]() {
|
||
QDoubleSpinBox* sb = new QDoubleSpinBox(this);
|
||
sb->setRange(-10000, 10000);
|
||
sb->setDecimals(3);
|
||
return sb;
|
||
};
|
||
|
||
// 第1行：末端位姿
|
||
inputLayout->addWidget(new QLabel("End X:", this), 0, 0);
|
||
m_inputEndX = createSpinBox();
|
||
inputLayout->addWidget(m_inputEndX, 0, 1);
|
||
inputLayout->addWidget(new QLabel("Y:", this), 0, 2);
|
||
m_inputEndY = createSpinBox();
|
||
inputLayout->addWidget(m_inputEndY, 0, 3);
|
||
inputLayout->addWidget(new QLabel("Z:", this), 0, 4);
|
||
m_inputEndZ = createSpinBox();
|
||
inputLayout->addWidget(m_inputEndZ, 0, 5);
|
||
inputLayout->addWidget(new QLabel("Roll\302\260:", this), 0, 6);
|
||
m_inputEndRoll = createSpinBox();
|
||
inputLayout->addWidget(m_inputEndRoll, 0, 7);
|
||
inputLayout->addWidget(new QLabel("Pitch\302\260:", this), 0, 8);
|
||
m_inputEndPitch = createSpinBox();
|
||
inputLayout->addWidget(m_inputEndPitch, 0, 9);
|
||
inputLayout->addWidget(new QLabel("Yaw\302\260:", this), 0, 10);
|
||
m_inputEndYaw = createSpinBox();
|
||
inputLayout->addWidget(m_inputEndYaw, 0, 11);
|
||
|
||
// 第2行：相机观测点 + 添加按钮
|
||
inputLayout->addWidget(new QLabel("Cam X:", this), 1, 0);
|
||
m_inputCamX = createSpinBox();
|
||
inputLayout->addWidget(m_inputCamX, 1, 1);
|
||
inputLayout->addWidget(new QLabel("Y:", this), 1, 2);
|
||
m_inputCamY = createSpinBox();
|
||
inputLayout->addWidget(m_inputCamY, 1, 3);
|
||
inputLayout->addWidget(new QLabel("Z:", this), 1, 4);
|
||
m_inputCamZ = createSpinBox();
|
||
inputLayout->addWidget(m_inputCamZ, 1, 5);
|
||
|
||
m_btnEyeInHandAddInput = new QPushButton("添加到表格", this);
|
||
connect(m_btnEyeInHandAddInput, &QPushButton::clicked, this, [this]() {
|
||
int row = m_tableEyeInHand->rowCount();
|
||
m_tableEyeInHand->insertRow(row);
|
||
m_tableEyeInHand->setItem(row, 0, new QTableWidgetItem(QString::number(m_inputEndX->value(), 'f', 3)));
|
||
m_tableEyeInHand->setItem(row, 1, new QTableWidgetItem(QString::number(m_inputEndY->value(), 'f', 3)));
|
||
m_tableEyeInHand->setItem(row, 2, new QTableWidgetItem(QString::number(m_inputEndZ->value(), 'f', 3)));
|
||
m_tableEyeInHand->setItem(row, 3, new QTableWidgetItem(QString::number(m_inputEndRoll->value(), 'f', 3)));
|
||
m_tableEyeInHand->setItem(row, 4, new QTableWidgetItem(QString::number(m_inputEndPitch->value(), 'f', 3)));
|
||
m_tableEyeInHand->setItem(row, 5, new QTableWidgetItem(QString::number(m_inputEndYaw->value(), 'f', 3)));
|
||
m_tableEyeInHand->setItem(row, 6, new QTableWidgetItem(QString::number(m_inputCamX->value(), 'f', 3)));
|
||
m_tableEyeInHand->setItem(row, 7, new QTableWidgetItem(QString::number(m_inputCamY->value(), 'f', 3)));
|
||
m_tableEyeInHand->setItem(row, 8, new QTableWidgetItem(QString::number(m_inputCamZ->value(), 'f', 3)));
|
||
m_tableEyeInHand->scrollToBottom();
|
||
});
|
||
inputLayout->addWidget(m_btnEyeInHandAddInput, 1, 10, 1, 2);
|
||
|
||
layout->addWidget(inputGroup);
|
||
|
||
return group;
|
||
}
|
||
|
||
void CalibDataWidget::updateTableVisibility()
|
||
{
|
||
int index = m_cbCalibType->currentIndex();
|
||
m_groupEyeToHand->setVisible(index == 0);
|
||
m_groupEyeInHand->setVisible(index == 1);
|
||
m_groupTCPCalib->setVisible(index == 2);
|
||
}
|
||
|
||
void CalibDataWidget::onCalibTypeChanged(int index)
|
||
{
|
||
updateTableVisibility();
|
||
|
||
// Eye-To-Hand 按钮
|
||
m_btnEyeToHandCalib->setEnabled(index == 0);
|
||
m_btnEyeToHandAddRow->setEnabled(index == 0);
|
||
m_btnEyeToHandDeleteRow->setEnabled(index == 0);
|
||
|
||
// Eye-In-Hand 按钮
|
||
m_btnEyeInHandCalib->setEnabled(index == 1);
|
||
m_btnEyeInHandAddRow->setEnabled(index == 1);
|
||
m_btnEyeInHandDeleteRow->setEnabled(index == 1);
|
||
|
||
// TCP 按钮
|
||
m_tcpAddRowBtn->setEnabled(index == 2);
|
||
m_tcpRemoveRowBtn->setEnabled(index == 2);
|
||
m_btnTCPCalib->setEnabled(index == 2);
|
||
|
||
if (index <= 1) {
|
||
emit calibTypeChanged(index == 0 ? HECCalibrationType::EyeToHand : HECCalibrationType::EyeInHand);
|
||
}
|
||
}
|
||
|
||
void CalibDataWidget::getEyeToHandData(std::vector<HECPoint3D>& eyePoints,
|
||
std::vector<HECPoint3D>& robotPoints) const
|
||
{
|
||
eyePoints.clear();
|
||
robotPoints.clear();
|
||
|
||
for (int row = 0; row < m_tableEyeToHand->rowCount(); ++row) {
|
||
HECPoint3D eyePt, robotPt;
|
||
|
||
QTableWidgetItem* item0 = m_tableEyeToHand->item(row, 0);
|
||
QTableWidgetItem* item1 = m_tableEyeToHand->item(row, 1);
|
||
QTableWidgetItem* item2 = m_tableEyeToHand->item(row, 2);
|
||
QTableWidgetItem* item3 = m_tableEyeToHand->item(row, 3);
|
||
QTableWidgetItem* item4 = m_tableEyeToHand->item(row, 4);
|
||
QTableWidgetItem* item5 = m_tableEyeToHand->item(row, 5);
|
||
|
||
if (item0 && item1 && item2 && item3 && item4 && item5) {
|
||
eyePt.x = item0->text().toDouble();
|
||
eyePt.y = item1->text().toDouble();
|
||
eyePt.z = item2->text().toDouble();
|
||
robotPt.x = item3->text().toDouble();
|
||
robotPt.y = item4->text().toDouble();
|
||
robotPt.z = item5->text().toDouble();
|
||
|
||
eyePoints.push_back(eyePt);
|
||
robotPoints.push_back(robotPt);
|
||
}
|
||
}
|
||
}
|
||
|
||
void CalibDataWidget::getEyeInHandData(std::vector<HECEyeInHandData>& calibData) const
|
||
{
|
||
calibData.clear();
|
||
|
||
const double deg2rad = M_PI / 180.0;
|
||
|
||
for (int row = 0; row < m_tableEyeInHand->rowCount(); ++row) {
|
||
HECEyeInHandData data;
|
||
|
||
// 获取末端位姿
|
||
double endX = m_tableEyeInHand->item(row, 0) ?
|
||
m_tableEyeInHand->item(row, 0)->text().toDouble() : 0;
|
||
double endY = m_tableEyeInHand->item(row, 1) ?
|
||
m_tableEyeInHand->item(row, 1)->text().toDouble() : 0;
|
||
double endZ = m_tableEyeInHand->item(row, 2) ?
|
||
m_tableEyeInHand->item(row, 2)->text().toDouble() : 0;
|
||
double endRoll = m_tableEyeInHand->item(row, 3) ?
|
||
m_tableEyeInHand->item(row, 3)->text().toDouble() * deg2rad : 0;
|
||
double endPitch = m_tableEyeInHand->item(row, 4) ?
|
||
m_tableEyeInHand->item(row, 4)->text().toDouble() * deg2rad : 0;
|
||
double endYaw = m_tableEyeInHand->item(row, 5) ?
|
||
m_tableEyeInHand->item(row, 5)->text().toDouble() * deg2rad : 0;
|
||
|
||
// 构建末端位姿矩阵
|
||
// 外旋 ZYX: R = Rx(roll) * Ry(pitch) * Rz(yaw)
|
||
HECRotationMatrix R;
|
||
double cr = cos(endRoll), sr = sin(endRoll);
|
||
double cp = cos(endPitch), sp = sin(endPitch);
|
||
double cy = cos(endYaw), sy = sin(endYaw);
|
||
|
||
R.at(0, 0) = cp * cy;
|
||
R.at(0, 1) = -cp * sy;
|
||
R.at(0, 2) = sp;
|
||
R.at(1, 0) = sr * sp * cy + cr * sy;
|
||
R.at(1, 1) = -sr * sp * sy + cr * cy;
|
||
R.at(1, 2) = -sr * cp;
|
||
R.at(2, 0) = -cr * sp * cy + sr * sy;
|
||
R.at(2, 1) = cr * sp * sy + sr * cy;
|
||
R.at(2, 2) = cr * cp;
|
||
|
||
HECTranslationVector T(endX, endY, endZ);
|
||
data.endPose = HECHomogeneousMatrix(R, T);
|
||
|
||
// 获取相机观测点
|
||
data.targetInCamera.x = m_tableEyeInHand->item(row, 6) ?
|
||
m_tableEyeInHand->item(row, 6)->text().toDouble() : 0;
|
||
data.targetInCamera.y = m_tableEyeInHand->item(row, 7) ?
|
||
m_tableEyeInHand->item(row, 7)->text().toDouble() : 0;
|
||
data.targetInCamera.z = m_tableEyeInHand->item(row, 8) ?
|
||
m_tableEyeInHand->item(row, 8)->text().toDouble() : 0;
|
||
|
||
calibData.push_back(data);
|
||
}
|
||
}
|
||
|
||
HECCalibrationType CalibDataWidget::getCalibType() const
|
||
{
|
||
return m_cbCalibType->currentIndex() == 0 ?
|
||
HECCalibrationType::EyeToHand : HECCalibrationType::EyeInHand;
|
||
}
|
||
|
||
void CalibDataWidget::clearAll()
|
||
{
|
||
m_tableEyeToHand->setRowCount(0);
|
||
m_tableEyeInHand->setRowCount(0);
|
||
m_tableTCP->setRowCount(0);
|
||
}
|
||
|
||
QWidget* CalibDataWidget::createTCPCalibGroup()
|
||
{
|
||
QWidget* group = new QWidget(this);
|
||
QVBoxLayout* layout = new QVBoxLayout(group);
|
||
layout->setContentsMargins(0, 0, 0, 0);
|
||
|
||
// 模式选择行
|
||
QHBoxLayout* modeLayout = new QHBoxLayout();
|
||
modeLayout->addWidget(new QLabel("标定模式:", this));
|
||
m_tcpModeCombo = new QComboBox(this);
|
||
m_tcpModeCombo->addItem("位置标定 (3-DOF)");
|
||
m_tcpModeCombo->addItem("完整标定 (6-DOF)");
|
||
connect(m_tcpModeCombo, QOverload<int>::of(&QComboBox::currentIndexChanged),
|
||
this, &CalibDataWidget::onTCPModeChanged);
|
||
modeLayout->addWidget(m_tcpModeCombo);
|
||
|
||
modeLayout->addWidget(new QLabel("欧拉角顺序:", this));
|
||
m_tcpEulerOrderCombo = new QComboBox(this);
|
||
m_tcpEulerOrderCombo->addItem("XYZ", static_cast<int>(HECEulerOrder::XYZ));
|
||
m_tcpEulerOrderCombo->addItem("XZY", static_cast<int>(HECEulerOrder::XZY));
|
||
m_tcpEulerOrderCombo->addItem("YXZ", static_cast<int>(HECEulerOrder::YXZ));
|
||
m_tcpEulerOrderCombo->addItem("YZX", static_cast<int>(HECEulerOrder::YZX));
|
||
m_tcpEulerOrderCombo->addItem("ZXY", static_cast<int>(HECEulerOrder::ZXY));
|
||
m_tcpEulerOrderCombo->addItem("ZYX (常用)", static_cast<int>(HECEulerOrder::ZYX));
|
||
m_tcpEulerOrderCombo->setCurrentIndex(5);
|
||
modeLayout->addWidget(m_tcpEulerOrderCombo);
|
||
modeLayout->addStretch();
|
||
layout->addLayout(modeLayout);
|
||
|
||
// 法兰位姿表格
|
||
m_tableTCP = new QTableWidget(this);
|
||
m_tableTCP->setColumnCount(6);
|
||
m_tableTCP->setHorizontalHeaderLabels({"X", "Y", "Z", "Roll(°)", "Pitch(°)", "Yaw(°)"});
|
||
m_tableTCP->horizontalHeader()->setSectionResizeMode(QHeaderView::Stretch);
|
||
m_tableTCP->setSelectionBehavior(QAbstractItemView::SelectRows);
|
||
layout->addWidget(m_tableTCP, 1);
|
||
|
||
// 按钮行（含 TCP 标定按钮）
|
||
QHBoxLayout* btnLayout = new QHBoxLayout();
|
||
m_tcpAddRowBtn = new QPushButton("添加行", this);
|
||
m_tcpRemoveRowBtn = new QPushButton("删除行", this);
|
||
m_btnTCPCalib = new QPushButton("TCP 标定", this);
|
||
connect(m_tcpAddRowBtn, &QPushButton::clicked, this, &CalibDataWidget::onTCPAddRow);
|
||
connect(m_tcpRemoveRowBtn, &QPushButton::clicked, this, &CalibDataWidget::onTCPRemoveRow);
|
||
connect(m_btnTCPCalib, &QPushButton::clicked, this, &CalibDataWidget::requestTCPCalib);
|
||
btnLayout->addWidget(m_tcpAddRowBtn);
|
||
btnLayout->addWidget(m_tcpRemoveRowBtn);
|
||
btnLayout->addWidget(m_btnTCPCalib);
|
||
btnLayout->addStretch();
|
||
layout->addLayout(btnLayout);
|
||
|
||
// 数据输入区
|
||
QGroupBox* inputGroup = new QGroupBox("数据输入", group);
|
||
QGridLayout* inputLayout = new QGridLayout(inputGroup);
|
||
inputLayout->setHorizontalSpacing(2);
|
||
inputLayout->setVerticalSpacing(4);
|
||
inputLayout->setContentsMargins(4, 4, 4, 4);
|
||
inputLayout->setColumnStretch(0, 0);
|
||
inputLayout->setColumnStretch(1, 1);
|
||
inputLayout->setColumnStretch(2, 0);
|
||
inputLayout->setColumnStretch(3, 1);
|
||
inputLayout->setColumnStretch(4, 0);
|
||
inputLayout->setColumnStretch(5, 1);
|
||
inputLayout->setColumnStretch(6, 0);
|
||
|
||
auto createSpinBox = [this]() {
|
||
QDoubleSpinBox* sb = new QDoubleSpinBox(this);
|
||
sb->setRange(-10000, 10000);
|
||
sb->setDecimals(3);
|
||
return sb;
|
||
};
|
||
|
||
// 第1行：X/Y/Z
|
||
inputLayout->addWidget(new QLabel("X:", this), 0, 0);
|
||
m_inputTcpX = createSpinBox();
|
||
inputLayout->addWidget(m_inputTcpX, 0, 1);
|
||
inputLayout->addWidget(new QLabel("Y:", this), 0, 2);
|
||
m_inputTcpY = createSpinBox();
|
||
inputLayout->addWidget(m_inputTcpY, 0, 3);
|
||
inputLayout->addWidget(new QLabel("Z:", this), 0, 4);
|
||
m_inputTcpZ = createSpinBox();
|
||
inputLayout->addWidget(m_inputTcpZ, 0, 5);
|
||
|
||
// 第2行：Roll/Pitch/Yaw + 添加按钮
|
||
inputLayout->addWidget(new QLabel("Roll\302\260:", this), 1, 0);
|
||
m_inputTcpRx = createSpinBox();
|
||
inputLayout->addWidget(m_inputTcpRx, 1, 1);
|
||
inputLayout->addWidget(new QLabel("Pitch\302\260:", this), 1, 2);
|
||
m_inputTcpRy = createSpinBox();
|
||
inputLayout->addWidget(m_inputTcpRy, 1, 3);
|
||
inputLayout->addWidget(new QLabel("Yaw\302\260:", this), 1, 4);
|
||
m_inputTcpRz = createSpinBox();
|
||
inputLayout->addWidget(m_inputTcpRz, 1, 5);
|
||
|
||
m_btnTcpAddInput = new QPushButton("添加到表格", this);
|
||
connect(m_btnTcpAddInput, &QPushButton::clicked, this, [this]() {
|
||
int row = m_tableTCP->rowCount();
|
||
m_tableTCP->insertRow(row);
|
||
m_tableTCP->setItem(row, 0, new QTableWidgetItem(QString::number(m_inputTcpX->value(), 'f', 3)));
|
||
m_tableTCP->setItem(row, 1, new QTableWidgetItem(QString::number(m_inputTcpY->value(), 'f', 3)));
|
||
m_tableTCP->setItem(row, 2, new QTableWidgetItem(QString::number(m_inputTcpZ->value(), 'f', 3)));
|
||
m_tableTCP->setItem(row, 3, new QTableWidgetItem(QString::number(m_inputTcpRx->value(), 'f', 3)));
|
||
m_tableTCP->setItem(row, 4, new QTableWidgetItem(QString::number(m_inputTcpRy->value(), 'f', 3)));
|
||
m_tableTCP->setItem(row, 5, new QTableWidgetItem(QString::number(m_inputTcpRz->value(), 'f', 3)));
|
||
m_tableTCP->scrollToBottom();
|
||
});
|
||
inputLayout->addWidget(m_btnTcpAddInput, 1, 6);
|
||
|
||
layout->addWidget(inputGroup);
|
||
|
||
// 6-DOF 姿态参数组（默认隐藏）
|
||
m_tcpOrientationGroup = new QGroupBox("6-DOF 姿态参数", this);
|
||
QGridLayout* oriLayout = new QGridLayout(m_tcpOrientationGroup);
|
||
|
||
oriLayout->addWidget(new QLabel("参考位姿索引:", this), 0, 0);
|
||
m_tcpRefPoseIndex = new QSpinBox(this);
|
||
m_tcpRefPoseIndex->setRange(0, 999);
|
||
m_tcpRefPoseIndex->setValue(0);
|
||
oriLayout->addWidget(m_tcpRefPoseIndex, 0, 1);
|
||
|
||
oriLayout->addWidget(new QLabel("期望世界朝向 Rx(°):", this), 1, 0);
|
||
m_tcpWorldRx = new QDoubleSpinBox(this);
|
||
m_tcpWorldRx->setRange(-360, 360);
|
||
m_tcpWorldRx->setDecimals(2);
|
||
oriLayout->addWidget(m_tcpWorldRx, 1, 1);
|
||
|
||
oriLayout->addWidget(new QLabel("Ry(°):", this), 1, 2);
|
||
m_tcpWorldRy = new QDoubleSpinBox(this);
|
||
m_tcpWorldRy->setRange(-360, 360);
|
||
m_tcpWorldRy->setDecimals(2);
|
||
oriLayout->addWidget(m_tcpWorldRy, 1, 3);
|
||
|
||
oriLayout->addWidget(new QLabel("Rz(°):", this), 1, 4);
|
||
m_tcpWorldRz = new QDoubleSpinBox(this);
|
||
m_tcpWorldRz->setRange(-360, 360);
|
||
m_tcpWorldRz->setDecimals(2);
|
||
oriLayout->addWidget(m_tcpWorldRz, 1, 5);
|
||
|
||
m_tcpOrientationGroup->setVisible(false);
|
||
layout->addWidget(m_tcpOrientationGroup);
|
||
|
||
return group;
|
||
}
|
||
|
||
void CalibDataWidget::onTCPModeChanged(int index)
|
||
{
|
||
m_tcpOrientationGroup->setVisible(index == 1);
|
||
}
|
||
|
||
void CalibDataWidget::onTCPAddRow()
|
||
{
|
||
int row = m_tableTCP->rowCount();
|
||
m_tableTCP->insertRow(row);
|
||
for (int col = 0; col < 6; ++col) {
|
||
QTableWidgetItem* item = new QTableWidgetItem("0");
|
||
m_tableTCP->setItem(row, col, item);
|
||
}
|
||
m_tableTCP->scrollToBottom();
|
||
}
|
||
|
||
void CalibDataWidget::onTCPRemoveRow()
|
||
{
|
||
int row = m_tableTCP->currentRow();
|
||
if (row >= 0) {
|
||
m_tableTCP->removeRow(row);
|
||
}
|
||
}
|
||
|
||
HECTCPCalibData CalibDataWidget::getTCPCalibData() const
|
||
{
|
||
HECTCPCalibData data;
|
||
|
||
// 标定模式
|
||
data.mode = (m_tcpModeCombo->currentIndex() == 0) ?
|
||
HECTCPCalibMode::PositionOnly : HECTCPCalibMode::Full6DOF;
|
||
|
||
// 欧拉角顺序
|
||
HECEulerOrder eulerOrder = static_cast<HECEulerOrder>(
|
||
m_tcpEulerOrderCombo->currentData().toInt());
|
||
|
||
// 读取表格中的法兰位姿
|
||
for (int row = 0; row < m_tableTCP->rowCount(); ++row) {
|
||
HECTCPCalibPose pose;
|
||
pose.x = m_tableTCP->item(row, 0) ? m_tableTCP->item(row, 0)->text().toDouble() : 0;
|
||
pose.y = m_tableTCP->item(row, 1) ? m_tableTCP->item(row, 1)->text().toDouble() : 0;
|
||
pose.z = m_tableTCP->item(row, 2) ? m_tableTCP->item(row, 2)->text().toDouble() : 0;
|
||
pose.rx = m_tableTCP->item(row, 3) ? m_tableTCP->item(row, 3)->text().toDouble() : 0;
|
||
pose.ry = m_tableTCP->item(row, 4) ? m_tableTCP->item(row, 4)->text().toDouble() : 0;
|
||
pose.rz = m_tableTCP->item(row, 5) ? m_tableTCP->item(row, 5)->text().toDouble() : 0;
|
||
pose.eulerOrder = eulerOrder;
|
||
data.poses.push_back(pose);
|
||
}
|
||
|
||
// 6-DOF 参数
|
||
data.referencePoseIndex = m_tcpRefPoseIndex->value();
|
||
data.worldRx = m_tcpWorldRx->value();
|
||
data.worldRy = m_tcpWorldRy->value();
|
||
data.worldRz = m_tcpWorldRz->value();
|
||
data.worldEulerOrder = eulerOrder;
|
||
|
||
return data;
|
||
}
|
||
|
||
void CalibDataWidget::setRobotInput(double x, double y, double z,
|
||
double rx, double ry, double rz)
|
||
{
|
||
int mode = m_cbCalibType->currentIndex();
|
||
if (mode == 0) {
|
||
// Eye-To-Hand: 仅填充 Robot X/Y/Z
|
||
m_inputRobotX->setValue(x);
|
||
m_inputRobotY->setValue(y);
|
||
m_inputRobotZ->setValue(z);
|
||
} else if (mode == 1) {
|
||
// Eye-In-Hand: 填充末端位姿
|
||
m_inputEndX->setValue(x);
|
||
m_inputEndY->setValue(y);
|
||
m_inputEndZ->setValue(z);
|
||
m_inputEndRoll->setValue(rx);
|
||
m_inputEndPitch->setValue(ry);
|
||
m_inputEndYaw->setValue(rz);
|
||
} else if (mode == 2) {
|
||
// TCP: 填充位姿
|
||
m_inputTcpX->setValue(x);
|
||
m_inputTcpY->setValue(y);
|
||
m_inputTcpZ->setValue(z);
|
||
m_inputTcpRx->setValue(rx);
|
||
m_inputTcpRy->setValue(ry);
|
||
m_inputTcpRz->setValue(rz);
|
||
}
|
||
}
|
||
|
||
void CalibDataWidget::setCameraInput(double x, double y, double z,
|
||
double rx, double ry, double rz)
|
||
{
|
||
int mode = m_cbCalibType->currentIndex();
|
||
if (mode == 0) {
|
||
// Eye-To-Hand: 填充相机坐标 Eye X/Y/Z
|
||
m_inputEyeX->setValue(x);
|
||
m_inputEyeY->setValue(y);
|
||
m_inputEyeZ->setValue(z);
|
||
} else if (mode == 1) {
|
||
// Eye-In-Hand: 填充相机观测点
|
||
m_inputCamX->setValue(x);
|
||
m_inputCamY->setValue(y);
|
||
m_inputCamZ->setValue(z);
|
||
}
|
||
// TCP 模式不需要相机输入
|
||
}
|