YangOne/GrabBag
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
GrabBag/Tools/CalibView/Src/CalibDataWidget.cpp

864 lines
35 KiB
C++
Raw Normal View History

手眼标定完善
2026-02-22 00:10:46 +08:00
#include "CalibDataWidget.h"
#include "../../SpinBoxPasteHelper.h"
#include <QVBoxLayout>
#include <QHBoxLayout>
#include <QSettings>
#include <QDateTime>
#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();
SpinBoxPasteHelper::install(this);
}
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);
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);
// 第3行：添加到表格按钮
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, 2, 0, 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);
inputLayout->setColumnStretch(0, 0);
inputLayout->setColumnStretch(1, 1);
inputLayout->setColumnStretch(2, 0);
inputLayout->setColumnStretch(3, 1);
inputLayout->setColumnStretch(4, 0);
inputLayout->setColumnStretch(5, 1);
auto createSpinBox = [this]() {
QDoubleSpinBox* sb = new QDoubleSpinBox(this);
sb->setRange(-10000, 10000);
sb->setDecimals(3);
return sb;
};
// 第1行：End X/Y/Z
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);
// 第2行：End Roll/Pitch/Yaw
inputLayout->addWidget(new QLabel("Roll\302\260:", this), 1, 0);
m_inputEndRoll = createSpinBox();
inputLayout->addWidget(m_inputEndRoll, 1, 1);
inputLayout->addWidget(new QLabel("Pitch\302\260:", this), 1, 2);
m_inputEndPitch = createSpinBox();
inputLayout->addWidget(m_inputEndPitch, 1, 3);
inputLayout->addWidget(new QLabel("Yaw\302\260:", this), 1, 4);
m_inputEndYaw = createSpinBox();
inputLayout->addWidget(m_inputEndYaw, 1, 5);
// 第3行：Cam X/Y/Z
inputLayout->addWidget(new QLabel("Cam X:", this), 2, 0);
m_inputCamX = createSpinBox();
inputLayout->addWidget(m_inputCamX, 2, 1);
inputLayout->addWidget(new QLabel("Y:", this), 2, 2);
m_inputCamY = createSpinBox();
inputLayout->addWidget(m_inputCamY, 2, 3);
inputLayout->addWidget(new QLabel("Z:", this), 2, 4);
m_inputCamZ = createSpinBox();
inputLayout->addWidget(m_inputCamZ, 2, 5);
// 第4行：添加到表格按钮
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, 3, 0, 1, 6);
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;
}
int CalibDataWidget::getCalibTypeIndex() const
{
return m_cbCalibType->currentIndex();
}
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);
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);
// 第3行：添加到表格按钮
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, 2, 0, 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 模式不需要相机输入
}
void CalibDataWidget::saveCalibData(const QString& filePath) const
{
QSettings ini(filePath, QSettings::IniFormat);
ini.setIniCodec("UTF-8");
int mode = m_cbCalibType->currentIndex();
// [CommInfo] 通用信息
ini.beginGroup("CommInfo");
ini.setValue("eCalibType", mode); // 0=EyeToHand, 1=EyeInHand, 2=TCP
ini.setValue("sCalibTime", QDateTime::currentDateTime().toString("yyyy-MM-dd-HH-mm-ss"));
if (mode == 0) {
int count = m_tableEyeToHand->rowCount();
ini.setValue("nPointCount", count);
ini.endGroup();
for (int row = 0; row < count; ++row) {
ini.beginGroup(QString("CalibPointInfo_%1").arg(row));
ini.setValue("dEyeX", m_tableEyeToHand->item(row, 0) ? m_tableEyeToHand->item(row, 0)->text() : "0");
ini.setValue("dEyeY", m_tableEyeToHand->item(row, 1) ? m_tableEyeToHand->item(row, 1)->text() : "0");
ini.setValue("dEyeZ", m_tableEyeToHand->item(row, 2) ? m_tableEyeToHand->item(row, 2)->text() : "0");
ini.setValue("dRobotX", m_tableEyeToHand->item(row, 3) ? m_tableEyeToHand->item(row, 3)->text() : "0");
ini.setValue("dRobotY", m_tableEyeToHand->item(row, 4) ? m_tableEyeToHand->item(row, 4)->text() : "0");
ini.setValue("dRobotZ", m_tableEyeToHand->item(row, 5) ? m_tableEyeToHand->item(row, 5)->text() : "0");
ini.endGroup();
}
} else if (mode == 1) {
int count = m_tableEyeInHand->rowCount();
ini.setValue("nPointCount", count);
ini.endGroup();
for (int row = 0; row < count; ++row) {
ini.beginGroup(QString("CalibPointInfo_%1").arg(row));
ini.setValue("dEndX", m_tableEyeInHand->item(row, 0) ? m_tableEyeInHand->item(row, 0)->text() : "0");
ini.setValue("dEndY", m_tableEyeInHand->item(row, 1) ? m_tableEyeInHand->item(row, 1)->text() : "0");
ini.setValue("dEndZ", m_tableEyeInHand->item(row, 2) ? m_tableEyeInHand->item(row, 2)->text() : "0");
ini.setValue("dEndRoll", m_tableEyeInHand->item(row, 3) ? m_tableEyeInHand->item(row, 3)->text() : "0");
ini.setValue("dEndPitch", m_tableEyeInHand->item(row, 4) ? m_tableEyeInHand->item(row, 4)->text() : "0");
ini.setValue("dEndYaw", m_tableEyeInHand->item(row, 5) ? m_tableEyeInHand->item(row, 5)->text() : "0");
ini.setValue("dCamX", m_tableEyeInHand->item(row, 6) ? m_tableEyeInHand->item(row, 6)->text() : "0");
ini.setValue("dCamY", m_tableEyeInHand->item(row, 7) ? m_tableEyeInHand->item(row, 7)->text() : "0");
ini.setValue("dCamZ", m_tableEyeInHand->item(row, 8) ? m_tableEyeInHand->item(row, 8)->text() : "0");
ini.endGroup();
}
} else {
int count = m_tableTCP->rowCount();
ini.setValue("nPoseCount", count);
ini.setValue("eCalibMode", m_tcpModeCombo->currentIndex());
ini.setValue("eEulerOrder", m_tcpEulerOrderCombo->currentIndex());
ini.setValue("nRefPoseIndex", m_tcpRefPoseIndex->value());
ini.setValue("dWorldRx", m_tcpWorldRx->value());
ini.setValue("dWorldRy", m_tcpWorldRy->value());
ini.setValue("dWorldRz", m_tcpWorldRz->value());
ini.endGroup();
for (int row = 0; row < count; ++row) {
ini.beginGroup(QString("CalibPoseInfo_%1").arg(row));
ini.setValue("dX", m_tableTCP->item(row, 0) ? m_tableTCP->item(row, 0)->text() : "0");
ini.setValue("dY", m_tableTCP->item(row, 1) ? m_tableTCP->item(row, 1)->text() : "0");
ini.setValue("dZ", m_tableTCP->item(row, 2) ? m_tableTCP->item(row, 2)->text() : "0");
ini.setValue("dRx", m_tableTCP->item(row, 3) ? m_tableTCP->item(row, 3)->text() : "0");
ini.setValue("dRy", m_tableTCP->item(row, 4) ? m_tableTCP->item(row, 4)->text() : "0");
ini.setValue("dRz", m_tableTCP->item(row, 5) ? m_tableTCP->item(row, 5)->text() : "0");
ini.endGroup();
}
}
}
void CalibDataWidget::loadCalibData(const QString& filePath)
{
QSettings ini(filePath, QSettings::IniFormat);
ini.setIniCodec("UTF-8");
ini.beginGroup("CommInfo");
int calibType = ini.value("eCalibType", -1).toInt();
if (calibType == 0) {
// Eye-To-Hand
int count = ini.value("nPointCount", 0).toInt();
ini.endGroup();
m_cbCalibType->setCurrentIndex(0);
m_tableEyeToHand->setRowCount(0);
for (int i = 0; i < count; ++i) {
ini.beginGroup(QString("CalibPointInfo_%1").arg(i));
int row = m_tableEyeToHand->rowCount();
m_tableEyeToHand->insertRow(row);
m_tableEyeToHand->setItem(row, 0, new QTableWidgetItem(ini.value("dEyeX", "0").toString()));
m_tableEyeToHand->setItem(row, 1, new QTableWidgetItem(ini.value("dEyeY", "0").toString()));
m_tableEyeToHand->setItem(row, 2, new QTableWidgetItem(ini.value("dEyeZ", "0").toString()));
m_tableEyeToHand->setItem(row, 3, new QTableWidgetItem(ini.value("dRobotX", "0").toString()));
m_tableEyeToHand->setItem(row, 4, new QTableWidgetItem(ini.value("dRobotY", "0").toString()));
m_tableEyeToHand->setItem(row, 5, new QTableWidgetItem(ini.value("dRobotZ", "0").toString()));
ini.endGroup();
}
} else if (calibType == 1) {
// Eye-In-Hand
int count = ini.value("nPointCount", 0).toInt();
ini.endGroup();
m_cbCalibType->setCurrentIndex(1);
m_tableEyeInHand->setRowCount(0);
for (int i = 0; i < count; ++i) {
ini.beginGroup(QString("CalibPointInfo_%1").arg(i));
int row = m_tableEyeInHand->rowCount();
m_tableEyeInHand->insertRow(row);
m_tableEyeInHand->setItem(row, 0, new QTableWidgetItem(ini.value("dEndX", "0").toString()));
m_tableEyeInHand->setItem(row, 1, new QTableWidgetItem(ini.value("dEndY", "0").toString()));
m_tableEyeInHand->setItem(row, 2, new QTableWidgetItem(ini.value("dEndZ", "0").toString()));
m_tableEyeInHand->setItem(row, 3, new QTableWidgetItem(ini.value("dEndRoll", "0").toString()));
m_tableEyeInHand->setItem(row, 4, new QTableWidgetItem(ini.value("dEndPitch", "0").toString()));
m_tableEyeInHand->setItem(row, 5, new QTableWidgetItem(ini.value("dEndYaw", "0").toString()));
m_tableEyeInHand->setItem(row, 6, new QTableWidgetItem(ini.value("dCamX", "0").toString()));
m_tableEyeInHand->setItem(row, 7, new QTableWidgetItem(ini.value("dCamY", "0").toString()));
m_tableEyeInHand->setItem(row, 8, new QTableWidgetItem(ini.value("dCamZ", "0").toString()));
ini.endGroup();
}
} else if (calibType == 2) {
// TCP
int count = ini.value("nPoseCount", 0).toInt();
m_tcpModeCombo->setCurrentIndex(ini.value("eCalibMode", 0).toInt());
m_tcpEulerOrderCombo->setCurrentIndex(ini.value("eEulerOrder", 5).toInt());
m_tcpRefPoseIndex->setValue(ini.value("nRefPoseIndex", 0).toInt());
m_tcpWorldRx->setValue(ini.value("dWorldRx", 0).toDouble());
m_tcpWorldRy->setValue(ini.value("dWorldRy", 0).toDouble());
m_tcpWorldRz->setValue(ini.value("dWorldRz", 0).toDouble());
ini.endGroup();
m_cbCalibType->setCurrentIndex(2);
m_tableTCP->setRowCount(0);
for (int i = 0; i < count; ++i) {
ini.beginGroup(QString("CalibPoseInfo_%1").arg(i));
int row = m_tableTCP->rowCount();
m_tableTCP->insertRow(row);
m_tableTCP->setItem(row, 0, new QTableWidgetItem(ini.value("dX", "0").toString()));
m_tableTCP->setItem(row, 1, new QTableWidgetItem(ini.value("dY", "0").toString()));
m_tableTCP->setItem(row, 2, new QTableWidgetItem(ini.value("dZ", "0").toString()));
m_tableTCP->setItem(row, 3, new QTableWidgetItem(ini.value("dRx", "0").toString()));
m_tableTCP->setItem(row, 4, new QTableWidgetItem(ini.value("dRy", "0").toString()));
m_tableTCP->setItem(row, 5, new QTableWidgetItem(ini.value("dRz", "0").toString()));
ini.endGroup();
}
} else {
ini.endGroup();
}
}
Reference in New Issue Copy Permalink