1375 lines
60 KiB
C++
1375 lines
60 KiB
C++
#include "CalibDataWidget.h"
|
||
#include "../../SpinBoxPasteHelper.h"
|
||
|
||
#include <QVBoxLayout>
|
||
#include <QHBoxLayout>
|
||
#include <QSettings>
|
||
#include <QDateTime>
|
||
#include <QGridLayout>
|
||
#include <QHeaderView>
|
||
#include <QLabel>
|
||
#include <array>
|
||
|
||
namespace {
|
||
|
||
int parseCalibType(const QVariant& value)
|
||
{
|
||
bool ok = false;
|
||
const QString text = value.toString().trimmed();
|
||
const int numericType = text.toInt(&ok);
|
||
if (ok && numericType >= 0 && numericType <= 2) {
|
||
return numericType;
|
||
}
|
||
|
||
QString normalized = text.toLower();
|
||
normalized.remove('-');
|
||
normalized.remove('_');
|
||
normalized.remove(' ');
|
||
|
||
if (normalized == "eyetohand") {
|
||
return 0;
|
||
}
|
||
if (normalized == "eyeinhand") {
|
||
return 1;
|
||
}
|
||
if (normalized == "tcp") {
|
||
return 2;
|
||
}
|
||
|
||
return -1;
|
||
}
|
||
|
||
QString calibTypeToString(int mode)
|
||
{
|
||
switch (mode) {
|
||
case 0: return "EyeToHand";
|
||
case 1: return "EyeInHand";
|
||
case 2: return "TCP";
|
||
default: return QString::number(mode);
|
||
}
|
||
}
|
||
|
||
QString tableText(const QTableWidget* table, int row, int col)
|
||
{
|
||
const QTableWidgetItem* item = table->item(row, col);
|
||
return item ? item->text() : "0";
|
||
}
|
||
|
||
double cameraAngleToRadians(const QTableWidget* table, int row, int col, int camAngleUnit)
|
||
{
|
||
double angle = tableText(table, row, col).toDouble();
|
||
if (camAngleUnit == 1) {
|
||
angle *= M_PI / 180.0;
|
||
}
|
||
return angle;
|
||
}
|
||
|
||
QString rawAngleToDisplayText(double radians, int camAngleUnit)
|
||
{
|
||
const double angle = (camAngleUnit == 1) ? (radians * 180.0 / M_PI) : radians;
|
||
return QString::number(angle, 'f', 6);
|
||
}
|
||
|
||
}
|
||
|
||
CalibDataWidget::CalibDataWidget(QWidget* parent)
|
||
: QWidget(parent)
|
||
, m_cbCalibType(nullptr)
|
||
, m_cbEulerOrder(nullptr)
|
||
, m_tableHandEye(nullptr)
|
||
, m_groupHandEye(nullptr)
|
||
, m_btnHandEyeAddRow(nullptr)
|
||
, m_btnHandEyeDeleteRow(nullptr)
|
||
, m_btnHandEyeCalib(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_inputCamX(nullptr)
|
||
, m_inputCamY(nullptr)
|
||
, m_inputCamZ(nullptr)
|
||
, m_inputCamRoll(nullptr)
|
||
, m_inputCamPitch(nullptr)
|
||
, m_inputCamYaw(nullptr)
|
||
, m_inputRobotX(nullptr)
|
||
, m_inputRobotY(nullptr)
|
||
, m_inputRobotZ(nullptr)
|
||
, m_inputRobotRoll(nullptr)
|
||
, m_inputRobotPitch(nullptr)
|
||
, m_inputRobotYaw(nullptr)
|
||
, m_btnHandEyeAddInput(nullptr)
|
||
, m_inputTcpX(nullptr)
|
||
, m_inputTcpY(nullptr)
|
||
, m_inputTcpZ(nullptr)
|
||
, m_inputTcpRx(nullptr)
|
||
, m_inputTcpRy(nullptr)
|
||
, m_inputTcpRz(nullptr)
|
||
, m_btnTcpAddInput(nullptr)
|
||
, m_cbCamAngleUnit(nullptr)
|
||
, m_lblCamRx(nullptr)
|
||
, m_lblCamRy(nullptr)
|
||
, m_lblCamRz(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->addSpacing(20);
|
||
QLabel* lblEuler = new QLabel("欧拉角顺序:", this);
|
||
m_cbEulerOrder = new QComboBox(this);
|
||
m_cbEulerOrder->addItem("ZYX (Rz-Ry-Rx)");
|
||
m_cbEulerOrder->addItem("XYZ (Rx-Ry-Rz)");
|
||
m_cbEulerOrder->addItem("XZY (Rx-Rz-Ry)");
|
||
m_cbEulerOrder->addItem("YXZ (Ry-Rx-Rz)");
|
||
m_cbEulerOrder->addItem("YZX (Ry-Rz-Rx)");
|
||
m_cbEulerOrder->addItem("ZXY (Rz-Rx-Ry)");
|
||
modeLayout->addWidget(lblEuler);
|
||
modeLayout->addWidget(m_cbEulerOrder);
|
||
|
||
modeLayout->addStretch();
|
||
mainLayout->addLayout(modeLayout);
|
||
|
||
// 手眼标定数据组（Eye-To-Hand 和 Eye-In-Hand 共用）
|
||
m_groupHandEye = createHandEyeGroup();
|
||
mainLayout->addWidget(m_groupHandEye, 1);
|
||
|
||
// TCP 标定数据组
|
||
m_groupTCPCalib = createTCPCalibGroup();
|
||
mainLayout->addWidget(m_groupTCPCalib, 1);
|
||
m_groupTCPCalib->setVisible(false);
|
||
|
||
// 初始化按钮启用状态
|
||
onCalibTypeChanged(0);
|
||
}
|
||
|
||
QWidget* CalibDataWidget::createHandEyeGroup()
|
||
{
|
||
QWidget* group = new QWidget(this);
|
||
QVBoxLayout* layout = new QVBoxLayout(group);
|
||
layout->setContentsMargins(0, 0, 0, 0);
|
||
|
||
m_tableHandEye = new QTableWidget(this);
|
||
m_tableHandEye->setColumnCount(12);
|
||
m_tableHandEye->setHorizontalHeaderLabels({
|
||
"Cam\nX", "Cam\nY", "Cam\nZ",
|
||
"Cam\nRoll(rad)", "Cam\nPitch(rad)", "Cam\nYaw(rad)",
|
||
"Robot\nX", "Robot\nY", "Robot\nZ",
|
||
"Robot\nRoll(°)", "Robot\nPitch(°)", "Robot\nYaw(°)"
|
||
});
|
||
m_tableHandEye->horizontalHeader()->setSectionResizeMode(QHeaderView::Stretch);
|
||
m_tableHandEye->horizontalHeader()->setDefaultAlignment(Qt::AlignCenter);
|
||
m_tableHandEye->setSelectionBehavior(QAbstractItemView::SelectRows);
|
||
|
||
layout->addWidget(m_tableHandEye, 1);
|
||
|
||
// 内联按钮行
|
||
QHBoxLayout* btnLayout = new QHBoxLayout();
|
||
m_btnHandEyeAddRow = new QPushButton("添加行", this);
|
||
m_btnHandEyeDeleteRow = new QPushButton("删除行", this);
|
||
m_btnHandEyeCalib = new QPushButton("手眼标定", this);
|
||
m_btnHandEyeCalib->setStyleSheet(
|
||
"QPushButton { background-color: #4CAF50; color: white; font-weight: bold; padding: 4px 12px; }"
|
||
"QPushButton:hover { background-color: #45a049; }"
|
||
"QPushButton:pressed { background-color: #3d8b40; }"
|
||
);
|
||
|
||
connect(m_btnHandEyeAddRow, &QPushButton::clicked, this, [this]() {
|
||
int row = m_tableHandEye->rowCount();
|
||
m_tableHandEye->insertRow(row);
|
||
for (int col = 0; col < 12; ++col) {
|
||
m_tableHandEye->setItem(row, col, new QTableWidgetItem("0"));
|
||
}
|
||
m_tableHandEye->scrollToBottom();
|
||
});
|
||
connect(m_btnHandEyeDeleteRow, &QPushButton::clicked, this, [this]() {
|
||
int row = m_tableHandEye->currentRow();
|
||
if (row >= 0) {
|
||
m_tableHandEye->removeRow(row);
|
||
}
|
||
});
|
||
connect(m_btnHandEyeCalib, &QPushButton::clicked, this, [this]() {
|
||
int mode = m_cbCalibType->currentIndex();
|
||
if (mode == 0) {
|
||
emit requestEyeToHandCalib();
|
||
} else if (mode == 1) {
|
||
emit requestEyeInHandCalib();
|
||
}
|
||
});
|
||
|
||
btnLayout->addWidget(m_btnHandEyeAddRow);
|
||
btnLayout->addWidget(m_btnHandEyeDeleteRow);
|
||
btnLayout->addWidget(m_btnHandEyeCalib);
|
||
btnLayout->addStretch();
|
||
layout->addLayout(btnLayout);
|
||
|
||
// 数据输入区（水平布局：左侧输入框 + 右侧添加按钮）
|
||
QGroupBox* inputGroup = new QGroupBox("数据输入", group);
|
||
QHBoxLayout* inputMainLayout = new QHBoxLayout(inputGroup);
|
||
inputMainLayout->setContentsMargins(4, 4, 4, 4);
|
||
inputMainLayout->setSpacing(8);
|
||
|
||
// 左侧：输入框网格
|
||
QWidget* inputFields = new QWidget(this);
|
||
QGridLayout* inputLayout = new QGridLayout(inputFields);
|
||
inputLayout->setHorizontalSpacing(2);
|
||
inputLayout->setVerticalSpacing(4);
|
||
inputLayout->setContentsMargins(0, 0, 0, 0);
|
||
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(mm):", this), 0, 0);
|
||
m_inputCamX = createSpinBox();
|
||
inputLayout->addWidget(m_inputCamX, 0, 1);
|
||
inputLayout->addWidget(new QLabel("Y(mm):", this), 0, 2);
|
||
m_inputCamY = createSpinBox();
|
||
inputLayout->addWidget(m_inputCamY, 0, 3);
|
||
inputLayout->addWidget(new QLabel("Z(mm):", this), 0, 4);
|
||
m_inputCamZ = createSpinBox();
|
||
inputLayout->addWidget(m_inputCamZ, 0, 5);
|
||
|
||
// 第2行：标定板 Rx/Ry/Rz (支持度/弧度切换)
|
||
m_lblCamRx = new QLabel("标定板Rx(rad):", this);
|
||
inputLayout->addWidget(m_lblCamRx, 1, 0);
|
||
m_inputCamRoll = createSpinBox();
|
||
inputLayout->addWidget(m_inputCamRoll, 1, 1);
|
||
m_lblCamRy = new QLabel("Ry(rad):", this);
|
||
inputLayout->addWidget(m_lblCamRy, 1, 2);
|
||
m_inputCamPitch = createSpinBox();
|
||
inputLayout->addWidget(m_inputCamPitch, 1, 3);
|
||
m_lblCamRz = new QLabel("Rz(rad):", this);
|
||
inputLayout->addWidget(m_lblCamRz, 1, 4);
|
||
m_inputCamYaw = createSpinBox();
|
||
inputLayout->addWidget(m_inputCamYaw, 1, 5);
|
||
|
||
// 第2.5行：角度单位选择
|
||
QLabel* lblAngleUnit = new QLabel("姿态输入单位:", this);
|
||
inputLayout->addWidget(lblAngleUnit, 2, 0);
|
||
m_cbCamAngleUnit = new QComboBox(this);
|
||
m_cbCamAngleUnit->addItem("弧度 (rad)");
|
||
m_cbCamAngleUnit->addItem("角度 (°)");
|
||
connect(m_cbCamAngleUnit, QOverload<int>::of(&QComboBox::currentIndexChanged),
|
||
this, [this](int index) {
|
||
QString unitSuffix = (index == 0) ? "rad" : "°";
|
||
m_lblCamRx->setText(QString("标定板Rx(%1):").arg(unitSuffix));
|
||
m_lblCamRy->setText(QString("Ry(%1):").arg(unitSuffix));
|
||
m_lblCamRz->setText(QString("Rz(%1):").arg(unitSuffix));
|
||
// 同步更新表格列头
|
||
updateTableHeaders();
|
||
});
|
||
inputLayout->addWidget(m_cbCamAngleUnit, 2, 1);
|
||
|
||
// 第3行：机械臂 X/Y/Z
|
||
inputLayout->addWidget(new QLabel("机械臂X(mm):", this), 3, 0);
|
||
m_inputRobotX = createSpinBox();
|
||
inputLayout->addWidget(m_inputRobotX, 3, 1);
|
||
inputLayout->addWidget(new QLabel("Y(mm):", this), 3, 2);
|
||
m_inputRobotY = createSpinBox();
|
||
inputLayout->addWidget(m_inputRobotY, 3, 3);
|
||
inputLayout->addWidget(new QLabel("Z(mm):", this), 3, 4);
|
||
m_inputRobotZ = createSpinBox();
|
||
inputLayout->addWidget(m_inputRobotZ, 3, 5);
|
||
|
||
// 第4行：机械臂 Rx/Ry/Rz (角度)
|
||
inputLayout->addWidget(new QLabel("机械臂Rx(°):", this), 4, 0);
|
||
m_inputRobotRoll = createSpinBox();
|
||
inputLayout->addWidget(m_inputRobotRoll, 4, 1);
|
||
inputLayout->addWidget(new QLabel("Ry(°):", this), 4, 2);
|
||
m_inputRobotPitch = createSpinBox();
|
||
inputLayout->addWidget(m_inputRobotPitch, 4, 3);
|
||
inputLayout->addWidget(new QLabel("Rz(°):", this), 4, 4);
|
||
m_inputRobotYaw = createSpinBox();
|
||
inputLayout->addWidget(m_inputRobotYaw, 4, 5);
|
||
|
||
// 第5行不再放按钮
|
||
m_btnHandEyeAddInput = new QPushButton("添加到\n表格", this);
|
||
m_btnHandEyeAddInput->setMinimumHeight(60);
|
||
connect(m_btnHandEyeAddInput, &QPushButton::clicked, this, [this]() {
|
||
int row = m_tableHandEye->rowCount();
|
||
m_tableHandEye->insertRow(row);
|
||
m_tableHandEye->setItem(row, 0, new QTableWidgetItem(QString::number(m_inputCamX->value(), 'f', 3)));
|
||
m_tableHandEye->setItem(row, 1, new QTableWidgetItem(QString::number(m_inputCamY->value(), 'f', 3)));
|
||
m_tableHandEye->setItem(row, 2, new QTableWidgetItem(QString::number(m_inputCamZ->value(), 'f', 3)));
|
||
|
||
// 标定板姿态：直接存储用户输入的原始值（度存度、弧度存弧度）
|
||
m_tableHandEye->setItem(row, 3, new QTableWidgetItem(QString::number(m_inputCamRoll->value(), 'f', 3)));
|
||
m_tableHandEye->setItem(row, 4, new QTableWidgetItem(QString::number(m_inputCamPitch->value(), 'f', 3)));
|
||
m_tableHandEye->setItem(row, 5, new QTableWidgetItem(QString::number(m_inputCamYaw->value(), 'f', 3)));
|
||
|
||
m_tableHandEye->setItem(row, 6, new QTableWidgetItem(QString::number(m_inputRobotX->value(), 'f', 3)));
|
||
m_tableHandEye->setItem(row, 7, new QTableWidgetItem(QString::number(m_inputRobotY->value(), 'f', 3)));
|
||
m_tableHandEye->setItem(row, 8, new QTableWidgetItem(QString::number(m_inputRobotZ->value(), 'f', 3)));
|
||
m_tableHandEye->setItem(row, 9, new QTableWidgetItem(QString::number(m_inputRobotRoll->value(), 'f', 3)));
|
||
m_tableHandEye->setItem(row, 10, new QTableWidgetItem(QString::number(m_inputRobotPitch->value(), 'f', 3)));
|
||
m_tableHandEye->setItem(row, 11, new QTableWidgetItem(QString::number(m_inputRobotYaw->value(), 'f', 3)));
|
||
m_tableHandEye->scrollToBottom();
|
||
});
|
||
|
||
// 组装：左侧输入框 + 右侧按钮
|
||
inputMainLayout->addWidget(inputFields, 1);
|
||
inputMainLayout->addWidget(m_btnHandEyeAddInput);
|
||
|
||
layout->addWidget(inputGroup);
|
||
|
||
return group;
|
||
}
|
||
|
||
void CalibDataWidget::onCalibTypeChanged(int index)
|
||
{
|
||
// 更新表格可见性
|
||
m_groupHandEye->setVisible(index == 0 || index == 1);
|
||
m_groupTCPCalib->setVisible(index == 2);
|
||
|
||
// 根据模式更新表格标题
|
||
updateTableHeaders();
|
||
|
||
if (index == 0) {
|
||
m_btnHandEyeCalib->setText("Eye-To-Hand 标定");
|
||
} else if (index == 1) {
|
||
m_btnHandEyeCalib->setText("Eye-In-Hand 标定");
|
||
}
|
||
|
||
// TCP 按钮
|
||
m_tcpAddRowBtn->setEnabled(index == 2);
|
||
m_tcpRemoveRowBtn->setEnabled(index == 2);
|
||
m_btnTCPCalib->setEnabled(index == 2);
|
||
|
||
// 发送标定类型改变信号
|
||
if (index == 0) {
|
||
emit calibTypeChanged(HECCalibrationType::EyeToHand);
|
||
} else if (index == 1) {
|
||
emit calibTypeChanged(HECCalibrationType::EyeInHand);
|
||
}
|
||
}
|
||
|
||
void CalibDataWidget::updateTableHeaders()
|
||
{
|
||
int mode = m_cbCalibType->currentIndex();
|
||
QString camAngleUnit = (m_cbCamAngleUnit->currentIndex() == 0) ? "rad" : "°";
|
||
|
||
if (mode == 0) {
|
||
// Eye-To-Hand: 标定板在前，机械臂在后
|
||
m_tableHandEye->setHorizontalHeaderLabels({
|
||
"Cam\nX", "Cam\nY", "Cam\nZ",
|
||
QString("Cam\nRoll(%1)").arg(camAngleUnit),
|
||
QString("Cam\nPitch(%1)").arg(camAngleUnit),
|
||
QString("Cam\nYaw(%1)").arg(camAngleUnit),
|
||
"Robot\nX", "Robot\nY", "Robot\nZ",
|
||
"Robot\nRoll(°)", "Robot\nPitch(°)", "Robot\nYaw(°)"
|
||
});
|
||
} else if (mode == 1) {
|
||
// Eye-In-Hand: 机械臂在前，标定板在后
|
||
m_tableHandEye->setHorizontalHeaderLabels({
|
||
"Robot\nX", "Robot\nY", "Robot\nZ",
|
||
"Robot\nRoll(°)", "Robot\nPitch(°)", "Robot\nYaw(°)",
|
||
"Cam\nX", "Cam\nY", "Cam\nZ",
|
||
QString("Cam\nRoll(%1)").arg(camAngleUnit),
|
||
QString("Cam\nPitch(%1)").arg(camAngleUnit),
|
||
QString("Cam\nYaw(%1)").arg(camAngleUnit)
|
||
});
|
||
}
|
||
}
|
||
|
||
void CalibDataWidget::getEyeToHandData(std::vector<HECPoint3D>& eyePoints,
|
||
std::vector<HECPoint3D>& robotPoints) const
|
||
{
|
||
eyePoints.clear();
|
||
robotPoints.clear();
|
||
|
||
int rowCount = m_tableHandEye->rowCount();
|
||
|
||
for (int row = 0; row < rowCount; ++row) {
|
||
HECPoint3D eyePoint;
|
||
eyePoint.x = m_tableHandEye->item(row, 0) ?
|
||
m_tableHandEye->item(row, 0)->text().toDouble() : 0;
|
||
eyePoint.y = m_tableHandEye->item(row, 1) ?
|
||
m_tableHandEye->item(row, 1)->text().toDouble() : 0;
|
||
eyePoint.z = m_tableHandEye->item(row, 2) ?
|
||
m_tableHandEye->item(row, 2)->text().toDouble() : 0;
|
||
|
||
HECPoint3D robotPoint;
|
||
robotPoint.x = m_tableHandEye->item(row, 6) ?
|
||
m_tableHandEye->item(row, 6)->text().toDouble() : 0;
|
||
robotPoint.y = m_tableHandEye->item(row, 7) ?
|
||
m_tableHandEye->item(row, 7)->text().toDouble() : 0;
|
||
robotPoint.z = m_tableHandEye->item(row, 8) ?
|
||
m_tableHandEye->item(row, 8)->text().toDouble() : 0;
|
||
|
||
eyePoints.push_back(eyePoint);
|
||
robotPoints.push_back(robotPoint);
|
||
}
|
||
}
|
||
|
||
void CalibDataWidget::getEyeInHandData(std::vector<HECEyeInHandData>& calibData) const
|
||
{
|
||
calibData.clear();
|
||
|
||
// 创建标定实例用于欧拉角转换
|
||
IHandEyeCalib* calib = CreateHandEyeCalibInstance();
|
||
if (!calib) {
|
||
return;
|
||
}
|
||
|
||
// 获取用户选择的欧拉角顺序
|
||
HECEulerOrder eulerOrder = getEulerOrder();
|
||
|
||
int rowCount = m_tableHandEye->rowCount();
|
||
|
||
for (int row = 0; row < rowCount; ++row) {
|
||
HECEyeInHandData data;
|
||
|
||
// 获取末端位姿（Eye-In-Hand模式下，Robot数据在前6列）
|
||
double endX = m_tableHandEye->item(row, 0) ?
|
||
m_tableHandEye->item(row, 0)->text().toDouble() : 0;
|
||
double endY = m_tableHandEye->item(row, 1) ?
|
||
m_tableHandEye->item(row, 1)->text().toDouble() : 0;
|
||
double endZ = m_tableHandEye->item(row, 2) ?
|
||
m_tableHandEye->item(row, 2)->text().toDouble() : 0;
|
||
// 机器人姿态是角度，需要转换为弧度
|
||
double endRoll = m_tableHandEye->item(row, 3) ?
|
||
m_tableHandEye->item(row, 3)->text().toDouble() * M_PI / 180.0 : 0;
|
||
double endPitch = m_tableHandEye->item(row, 4) ?
|
||
m_tableHandEye->item(row, 4)->text().toDouble() * M_PI / 180.0 : 0;
|
||
double endYaw = m_tableHandEye->item(row, 5) ?
|
||
m_tableHandEye->item(row, 5)->text().toDouble() * M_PI / 180.0 : 0;
|
||
|
||
// 使用标定接口统一转换欧拉角到旋转矩阵
|
||
HECEulerAngles endAngles(endRoll, endPitch, endYaw);
|
||
HECRotationMatrix R_end;
|
||
calib->EulerToRotationMatrix(endAngles, eulerOrder, R_end);
|
||
HECTranslationVector T_end(endX, endY, endZ);
|
||
data.endPose = HECHomogeneousMatrix(R_end, T_end);
|
||
|
||
// 获取相机观测点（Eye-In-Hand模式下，Cam数据在后6列）
|
||
data.targetInCamera.x = m_tableHandEye->item(row, 6) ?
|
||
m_tableHandEye->item(row, 6)->text().toDouble() : 0;
|
||
data.targetInCamera.y = m_tableHandEye->item(row, 7) ?
|
||
m_tableHandEye->item(row, 7)->text().toDouble() : 0;
|
||
data.targetInCamera.z = m_tableHandEye->item(row, 8) ?
|
||
m_tableHandEye->item(row, 8)->text().toDouble() : 0;
|
||
|
||
calibData.push_back(data);
|
||
}
|
||
|
||
DestroyHandEyeCalibInstance(calib);
|
||
}
|
||
|
||
void CalibDataWidget::getEyeInHandPoseData(std::vector<HECEyeInHandPoseData>& calibData) const
|
||
{
|
||
calibData.clear();
|
||
|
||
IHandEyeCalib* calib = CreateHandEyeCalibInstance();
|
||
if (!calib) {
|
||
return;
|
||
}
|
||
|
||
const HECEulerOrder eulerOrder = getEulerOrder();
|
||
const int rowCount = m_tableHandEye->rowCount();
|
||
|
||
for (int row = 0; row < rowCount; ++row) {
|
||
HECEyeInHandPoseData data;
|
||
|
||
const double endX = m_tableHandEye->item(row, 0) ?
|
||
m_tableHandEye->item(row, 0)->text().toDouble() : 0;
|
||
const double endY = m_tableHandEye->item(row, 1) ?
|
||
m_tableHandEye->item(row, 1)->text().toDouble() : 0;
|
||
const double endZ = m_tableHandEye->item(row, 2) ?
|
||
m_tableHandEye->item(row, 2)->text().toDouble() : 0;
|
||
const double endRoll = m_tableHandEye->item(row, 3) ?
|
||
m_tableHandEye->item(row, 3)->text().toDouble() * M_PI / 180.0 : 0;
|
||
const double endPitch = m_tableHandEye->item(row, 4) ?
|
||
m_tableHandEye->item(row, 4)->text().toDouble() * M_PI / 180.0 : 0;
|
||
const double endYaw = m_tableHandEye->item(row, 5) ?
|
||
m_tableHandEye->item(row, 5)->text().toDouble() * M_PI / 180.0 : 0;
|
||
|
||
HECEulerAngles endAngles(endRoll, endPitch, endYaw);
|
||
HECRotationMatrix R_end;
|
||
calib->EulerToRotationMatrix(endAngles, eulerOrder, R_end);
|
||
data.endPose = HECHomogeneousMatrix(R_end, HECTranslationVector(endX, endY, endZ));
|
||
|
||
const double boardX = m_tableHandEye->item(row, 6) ?
|
||
m_tableHandEye->item(row, 6)->text().toDouble() : 0;
|
||
const double boardY = m_tableHandEye->item(row, 7) ?
|
||
m_tableHandEye->item(row, 7)->text().toDouble() : 0;
|
||
const double boardZ = m_tableHandEye->item(row, 8) ?
|
||
m_tableHandEye->item(row, 8)->text().toDouble() : 0;
|
||
|
||
double boardRoll = m_tableHandEye->item(row, 9) ?
|
||
m_tableHandEye->item(row, 9)->text().toDouble() : 0;
|
||
double boardPitch = m_tableHandEye->item(row, 10) ?
|
||
m_tableHandEye->item(row, 10)->text().toDouble() : 0;
|
||
double boardYaw = m_tableHandEye->item(row, 11) ?
|
||
m_tableHandEye->item(row, 11)->text().toDouble() : 0;
|
||
|
||
if (m_cbCamAngleUnit->currentIndex() == 1) {
|
||
boardRoll *= M_PI / 180.0;
|
||
boardPitch *= M_PI / 180.0;
|
||
boardYaw *= M_PI / 180.0;
|
||
}
|
||
|
||
// ChessboardDetector::DetectChessboardWithPose 固定按 ZYX 顺序输出相机观测姿态。
|
||
// 这里不能复用机器人姿态的欧拉角顺序，否则会把旧文件中的 dExtrinsic_3..5 解释错。
|
||
HECEulerAngles boardAngles(boardRoll, boardPitch, boardYaw);
|
||
HECEulerAngles boardAnglesFixed(boardRoll, boardPitch, boardYaw);
|
||
HECRotationMatrix R_board;
|
||
calib->EulerToRotationMatrix(boardAnglesFixed, HECEulerOrder::XYZ, R_board);
|
||
data.boardInCamera = HECHomogeneousMatrix(R_board, HECTranslationVector(boardX, boardY, boardZ));
|
||
|
||
calibData.push_back(data);
|
||
}
|
||
|
||
DestroyHandEyeCalibInstance(calib);
|
||
}
|
||
|
||
void CalibDataWidget::getEyeInHandPoseData(std::vector<HECEyeInHandPoseData>& calibData,
|
||
HECEulerOrder robotEulerOrder) const
|
||
{
|
||
calibData.clear();
|
||
|
||
IHandEyeCalib* calib = CreateHandEyeCalibInstance();
|
||
if (!calib) {
|
||
return;
|
||
}
|
||
|
||
const int rowCount = m_tableHandEye->rowCount();
|
||
|
||
for (int row = 0; row < rowCount; ++row) {
|
||
HECEyeInHandPoseData data;
|
||
|
||
const double endX = m_tableHandEye->item(row, 0) ?
|
||
m_tableHandEye->item(row, 0)->text().toDouble() : 0;
|
||
const double endY = m_tableHandEye->item(row, 1) ?
|
||
m_tableHandEye->item(row, 1)->text().toDouble() : 0;
|
||
const double endZ = m_tableHandEye->item(row, 2) ?
|
||
m_tableHandEye->item(row, 2)->text().toDouble() : 0;
|
||
const double endRoll = m_tableHandEye->item(row, 3) ?
|
||
m_tableHandEye->item(row, 3)->text().toDouble() * M_PI / 180.0 : 0;
|
||
const double endPitch = m_tableHandEye->item(row, 4) ?
|
||
m_tableHandEye->item(row, 4)->text().toDouble() * M_PI / 180.0 : 0;
|
||
const double endYaw = m_tableHandEye->item(row, 5) ?
|
||
m_tableHandEye->item(row, 5)->text().toDouble() * M_PI / 180.0 : 0;
|
||
|
||
HECEulerAngles endAngles(endRoll, endPitch, endYaw);
|
||
HECRotationMatrix R_end;
|
||
calib->EulerToRotationMatrix(endAngles, robotEulerOrder, R_end);
|
||
data.endPose = HECHomogeneousMatrix(R_end, HECTranslationVector(endX, endY, endZ));
|
||
|
||
const double boardX = m_tableHandEye->item(row, 6) ?
|
||
m_tableHandEye->item(row, 6)->text().toDouble() : 0;
|
||
const double boardY = m_tableHandEye->item(row, 7) ?
|
||
m_tableHandEye->item(row, 7)->text().toDouble() : 0;
|
||
const double boardZ = m_tableHandEye->item(row, 8) ?
|
||
m_tableHandEye->item(row, 8)->text().toDouble() : 0;
|
||
|
||
double boardRoll = m_tableHandEye->item(row, 9) ?
|
||
m_tableHandEye->item(row, 9)->text().toDouble() : 0;
|
||
double boardPitch = m_tableHandEye->item(row, 10) ?
|
||
m_tableHandEye->item(row, 10)->text().toDouble() : 0;
|
||
double boardYaw = m_tableHandEye->item(row, 11) ?
|
||
m_tableHandEye->item(row, 11)->text().toDouble() : 0;
|
||
|
||
if (m_cbCamAngleUnit->currentIndex() == 1) {
|
||
boardRoll *= M_PI / 180.0;
|
||
boardPitch *= M_PI / 180.0;
|
||
boardYaw *= M_PI / 180.0;
|
||
}
|
||
|
||
HECEulerAngles boardAngles(boardRoll, boardPitch, boardYaw);
|
||
HECRotationMatrix R_board;
|
||
calib->EulerToRotationMatrix(boardAngles, HECEulerOrder::XYZ, R_board);
|
||
data.boardInCamera = HECHomogeneousMatrix(R_board, HECTranslationVector(boardX, boardY, boardZ));
|
||
|
||
calibData.push_back(data);
|
||
}
|
||
|
||
DestroyHandEyeCalibInstance(calib);
|
||
}
|
||
|
||
void CalibDataWidget::getEyeInHandPoseData(std::vector<HECEyeInHandPoseData>& calibData,
|
||
HECEulerOrder robotEulerOrder,
|
||
HECEulerOrder boardEulerOrder,
|
||
bool invertBoardPose) const
|
||
{
|
||
calibData.clear();
|
||
|
||
IHandEyeCalib* calib = CreateHandEyeCalibInstance();
|
||
if (!calib) {
|
||
return;
|
||
}
|
||
|
||
const int rowCount = m_tableHandEye->rowCount();
|
||
|
||
for (int row = 0; row < rowCount; ++row) {
|
||
HECEyeInHandPoseData data;
|
||
|
||
const double endX = m_tableHandEye->item(row, 0) ?
|
||
m_tableHandEye->item(row, 0)->text().toDouble() : 0;
|
||
const double endY = m_tableHandEye->item(row, 1) ?
|
||
m_tableHandEye->item(row, 1)->text().toDouble() : 0;
|
||
const double endZ = m_tableHandEye->item(row, 2) ?
|
||
m_tableHandEye->item(row, 2)->text().toDouble() : 0;
|
||
const double endRoll = m_tableHandEye->item(row, 3) ?
|
||
m_tableHandEye->item(row, 3)->text().toDouble() * M_PI / 180.0 : 0;
|
||
const double endPitch = m_tableHandEye->item(row, 4) ?
|
||
m_tableHandEye->item(row, 4)->text().toDouble() * M_PI / 180.0 : 0;
|
||
const double endYaw = m_tableHandEye->item(row, 5) ?
|
||
m_tableHandEye->item(row, 5)->text().toDouble() * M_PI / 180.0 : 0;
|
||
|
||
HECEulerAngles endAngles(endRoll, endPitch, endYaw);
|
||
HECRotationMatrix R_end;
|
||
calib->EulerToRotationMatrix(endAngles, robotEulerOrder, R_end);
|
||
data.endPose = HECHomogeneousMatrix(R_end, HECTranslationVector(endX, endY, endZ));
|
||
|
||
const double boardX = m_tableHandEye->item(row, 6) ?
|
||
m_tableHandEye->item(row, 6)->text().toDouble() : 0;
|
||
const double boardY = m_tableHandEye->item(row, 7) ?
|
||
m_tableHandEye->item(row, 7)->text().toDouble() : 0;
|
||
const double boardZ = m_tableHandEye->item(row, 8) ?
|
||
m_tableHandEye->item(row, 8)->text().toDouble() : 0;
|
||
|
||
double boardRoll = m_tableHandEye->item(row, 9) ?
|
||
m_tableHandEye->item(row, 9)->text().toDouble() : 0;
|
||
double boardPitch = m_tableHandEye->item(row, 10) ?
|
||
m_tableHandEye->item(row, 10)->text().toDouble() : 0;
|
||
double boardYaw = m_tableHandEye->item(row, 11) ?
|
||
m_tableHandEye->item(row, 11)->text().toDouble() : 0;
|
||
|
||
if (m_cbCamAngleUnit->currentIndex() == 1) {
|
||
boardRoll *= M_PI / 180.0;
|
||
boardPitch *= M_PI / 180.0;
|
||
boardYaw *= M_PI / 180.0;
|
||
}
|
||
|
||
HECEulerAngles boardAngles(boardRoll, boardPitch, boardYaw);
|
||
HECRotationMatrix R_board;
|
||
calib->EulerToRotationMatrix(boardAngles, boardEulerOrder, R_board);
|
||
data.boardInCamera = HECHomogeneousMatrix(R_board, HECTranslationVector(boardX, boardY, boardZ));
|
||
if (invertBoardPose) {
|
||
data.boardInCamera = data.boardInCamera.inverse();
|
||
}
|
||
|
||
calibData.push_back(data);
|
||
}
|
||
|
||
DestroyHandEyeCalibInstance(calib);
|
||
}
|
||
|
||
void CalibDataWidget::getEyeToHandPoseData(std::vector<HECEyeToHandData>& calibData) const
|
||
{
|
||
calibData.clear();
|
||
|
||
// 创建标定实例用于欧拉角转换
|
||
IHandEyeCalib* calib = CreateHandEyeCalibInstance();
|
||
if (!calib) {
|
||
return;
|
||
}
|
||
|
||
// 获取用户选择的欧拉角顺序
|
||
HECEulerOrder eulerOrder = getEulerOrder();
|
||
|
||
int rowCount = m_tableHandEye->rowCount();
|
||
|
||
for (int row = 0; row < rowCount; ++row) {
|
||
HECEyeToHandData data;
|
||
|
||
// 获取末端位姿（Eye-To-Hand模式下，Robot数据在后6列）
|
||
double endX = m_tableHandEye->item(row, 6) ?
|
||
m_tableHandEye->item(row, 6)->text().toDouble() : 0;
|
||
double endY = m_tableHandEye->item(row, 7) ?
|
||
m_tableHandEye->item(row, 7)->text().toDouble() : 0;
|
||
double endZ = m_tableHandEye->item(row, 8) ?
|
||
m_tableHandEye->item(row, 8)->text().toDouble() : 0;
|
||
|
||
// 机器人姿态是角度，需要转换为弧度
|
||
double endRoll = m_tableHandEye->item(row, 9) ?
|
||
m_tableHandEye->item(row, 9)->text().toDouble() * M_PI / 180.0 : 0;
|
||
double endPitch = m_tableHandEye->item(row, 10) ?
|
||
m_tableHandEye->item(row, 10)->text().toDouble() * M_PI / 180.0 : 0;
|
||
double endYaw = m_tableHandEye->item(row, 11) ?
|
||
m_tableHandEye->item(row, 11)->text().toDouble() * M_PI / 180.0 : 0;
|
||
|
||
// 末端位姿和标定板姿态统一使用相同的欧拉角顺序
|
||
HECEulerAngles endAngles(endRoll, endPitch, endYaw);
|
||
HECRotationMatrix R_end;
|
||
calib->EulerToRotationMatrix(endAngles, eulerOrder, R_end);
|
||
HECTranslationVector T_end(endX, endY, endZ);
|
||
data.endPose = HECHomogeneousMatrix(R_end, T_end);
|
||
|
||
// 获取标定板在相机坐标系的位置（Eye-To-Hand模式下，Cam数据在前6列）
|
||
double boardX = m_tableHandEye->item(row, 0) ?
|
||
m_tableHandEye->item(row, 0)->text().toDouble() : 0;
|
||
double boardY = m_tableHandEye->item(row, 1) ?
|
||
m_tableHandEye->item(row, 1)->text().toDouble() : 0;
|
||
double boardZ = m_tableHandEye->item(row, 2) ?
|
||
m_tableHandEye->item(row, 2)->text().toDouble() : 0;
|
||
|
||
// 获取标定板在相机坐标系的姿态
|
||
// 表格中存储的是欧拉角，当前单位可能是弧度或角度
|
||
double boardRoll = m_tableHandEye->item(row, 3) ?
|
||
m_tableHandEye->item(row, 3)->text().toDouble() : 0;
|
||
double boardPitch = m_tableHandEye->item(row, 4) ?
|
||
m_tableHandEye->item(row, 4)->text().toDouble() : 0;
|
||
double boardYaw = m_tableHandEye->item(row, 5) ?
|
||
m_tableHandEye->item(row, 5)->text().toDouble() : 0;
|
||
|
||
// 如果当前单位是角度，转换为弧度
|
||
if (m_cbCamAngleUnit->currentIndex() == 1) {
|
||
boardRoll = boardRoll * M_PI / 180.0;
|
||
boardPitch = boardPitch * M_PI / 180.0;
|
||
boardYaw = boardYaw * M_PI / 180.0;
|
||
}
|
||
|
||
// 标定板姿态与机器人姿态统一使用当前选中的欧拉角顺序
|
||
// ChessboardDetector::DetectChessboardWithPose 固定按 ZYX 顺序输出相机观测姿态。
|
||
HECEulerAngles boardAngles(boardRoll, boardPitch, boardYaw);
|
||
HECEulerAngles boardAnglesFixed(boardRoll, boardPitch, boardYaw);
|
||
HECRotationMatrix R_board;
|
||
calib->EulerToRotationMatrix(boardAnglesFixed, HECEulerOrder::XYZ, R_board);
|
||
|
||
HECTranslationVector T_board(boardX, boardY, boardZ);
|
||
data.boardInCamera = HECHomogeneousMatrix(R_board, T_board);
|
||
|
||
calibData.push_back(data);
|
||
}
|
||
|
||
DestroyHandEyeCalibInstance(calib);
|
||
}
|
||
|
||
HECCalibrationType CalibDataWidget::getCalibType() const
|
||
{
|
||
return m_cbCalibType->currentIndex() == 0 ?
|
||
HECCalibrationType::EyeToHand : HECCalibrationType::EyeInHand;
|
||
}
|
||
|
||
int CalibDataWidget::getCalibTypeIndex() const
|
||
{
|
||
return m_cbCalibType->currentIndex();
|
||
}
|
||
|
||
HECEulerOrder CalibDataWidget::getEulerOrder() const
|
||
{
|
||
switch (m_cbEulerOrder->currentIndex()) {
|
||
case 0: return HECEulerOrder::ZYX;
|
||
case 1: return HECEulerOrder::XYZ;
|
||
case 2: return HECEulerOrder::XZY;
|
||
case 3: return HECEulerOrder::YXZ;
|
||
case 4: return HECEulerOrder::YZX;
|
||
case 5: return HECEulerOrder::ZXY;
|
||
default: return HECEulerOrder::ZYX;
|
||
}
|
||
}
|
||
|
||
void CalibDataWidget::clearAll()
|
||
{
|
||
m_tableHandEye->setRowCount(0);
|
||
m_tableTCP->setRowCount(0);
|
||
m_boardPoseData.clear();
|
||
}
|
||
|
||
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);
|
||
|
||
QHBoxLayout* btnLayout = new QHBoxLayout();
|
||
m_tcpAddRowBtn = new QPushButton("添加行", this);
|
||
m_tcpRemoveRowBtn = new QPushButton("删除行", this);
|
||
m_btnTCPCalib = new QPushButton("TCP 标定", this);
|
||
m_btnTCPCalib->setStyleSheet(
|
||
"QPushButton { background-color: #4CAF50; color: white; font-weight: bold; padding: 4px 12px; }"
|
||
"QPushButton:hover { background-color: #45a049; }"
|
||
"QPushButton:pressed { background-color: #3d8b40; }"
|
||
);
|
||
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);
|
||
QHBoxLayout* tcpInputMainLayout = new QHBoxLayout(inputGroup);
|
||
tcpInputMainLayout->setContentsMargins(4, 4, 4, 4);
|
||
tcpInputMainLayout->setSpacing(8);
|
||
|
||
QWidget* tcpInputFields = new QWidget(this);
|
||
QGridLayout* inputLayout = new QGridLayout(tcpInputFields);
|
||
inputLayout->setHorizontalSpacing(2);
|
||
inputLayout->setVerticalSpacing(4);
|
||
inputLayout->setContentsMargins(0, 0, 0, 0);
|
||
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;
|
||
};
|
||
|
||
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);
|
||
|
||
inputLayout->addWidget(new QLabel("Roll(°):", this), 1, 0);
|
||
m_inputTcpRx = createSpinBox();
|
||
inputLayout->addWidget(m_inputTcpRx, 1, 1);
|
||
inputLayout->addWidget(new QLabel("Pitch(°):", this), 1, 2);
|
||
m_inputTcpRy = createSpinBox();
|
||
inputLayout->addWidget(m_inputTcpRy, 1, 3);
|
||
inputLayout->addWidget(new QLabel("Yaw(°):", this), 1, 4);
|
||
m_inputTcpRz = createSpinBox();
|
||
inputLayout->addWidget(m_inputTcpRz, 1, 5);
|
||
|
||
m_btnTcpAddInput = new QPushButton("添加到\n表格", this);
|
||
m_btnTcpAddInput->setMinimumHeight(40);
|
||
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();
|
||
});
|
||
|
||
// 组装：左侧输入框 + 右侧按钮
|
||
tcpInputMainLayout->addWidget(tcpInputFields, 1);
|
||
tcpInputMainLayout->addWidget(m_btnTcpAddInput);
|
||
|
||
layout->addWidget(inputGroup);
|
||
|
||
m_tcpOrientationGroup = new QGroupBox("6-DOF 参数", group);
|
||
QGridLayout* oriLayout = new QGridLayout(m_tcpOrientationGroup);
|
||
|
||
oriLayout->addWidget(new QLabel("参考位姿索引:", this), 0, 0);
|
||
m_tcpRefPoseIndex = new QSpinBox(this);
|
||
m_tcpRefPoseIndex->setRange(0, 999);
|
||
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) {
|
||
m_tableTCP->setItem(row, col, new QTableWidgetItem("0"));
|
||
}
|
||
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);
|
||
}
|
||
|
||
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) {
|
||
m_inputRobotX->setValue(x);
|
||
m_inputRobotY->setValue(y);
|
||
m_inputRobotZ->setValue(z);
|
||
m_inputRobotRoll->setValue(rx);
|
||
m_inputRobotPitch->setValue(ry);
|
||
m_inputRobotYaw->setValue(rz);
|
||
} else if (mode == 1) {
|
||
m_inputRobotX->setValue(x);
|
||
m_inputRobotY->setValue(y);
|
||
m_inputRobotZ->setValue(z);
|
||
m_inputRobotRoll->setValue(rx);
|
||
m_inputRobotPitch->setValue(ry);
|
||
m_inputRobotYaw->setValue(rz);
|
||
} else if (mode == 2) {
|
||
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)
|
||
{
|
||
m_inputCamX->setValue(x);
|
||
m_inputCamY->setValue(y);
|
||
m_inputCamZ->setValue(z);
|
||
|
||
// VrEyeView 回填的是欧拉角“度”，UI 根据当前输入单位决定如何显示
|
||
if (m_cbCamAngleUnit->currentIndex() == 1) {
|
||
m_inputCamRoll->setValue(rx);
|
||
m_inputCamPitch->setValue(ry);
|
||
m_inputCamYaw->setValue(rz);
|
||
} else {
|
||
m_inputCamRoll->setValue(rx * M_PI / 180.0);
|
||
m_inputCamPitch->setValue(ry * M_PI / 180.0);
|
||
m_inputCamYaw->setValue(rz * M_PI / 180.0);
|
||
}
|
||
}
|
||
|
||
void CalibDataWidget::saveCalibData(const QString& filePath) const
|
||
{
|
||
QSettings ini(filePath, QSettings::IniFormat);
|
||
ini.setIniCodec("UTF-8");
|
||
ini.clear();
|
||
|
||
int mode = m_cbCalibType->currentIndex();
|
||
const int eulerOrderIndex = m_cbEulerOrder->currentIndex();
|
||
// 兼容旧格式：轴序字符串直接表示姿态角的应用顺序。
|
||
// 例如 RxRyRz 表示 RX=roll, RY=pitch, RZ=yaw。
|
||
QString axisOrder;
|
||
switch (eulerOrderIndex) {
|
||
case 0: axisOrder = "RzRyRx"; break;
|
||
case 1: axisOrder = "RxRyRz"; break;
|
||
case 2: axisOrder = "RxRzRy"; break;
|
||
case 3: axisOrder = "RyRxRz"; break;
|
||
case 4: axisOrder = "RyRzRx"; break;
|
||
case 5: axisOrder = "RzRxRy"; break;
|
||
default: axisOrder = "RzRyRx"; break;
|
||
}
|
||
|
||
ini.beginGroup("CommInfo");
|
||
ini.setValue("eCalibType", calibTypeToString(mode));
|
||
ini.setValue("eCalibTypeIndex", mode);
|
||
ini.setValue("sCalibTime", QDateTime::currentDateTime().toString("yyyy-MM-dd-HH-mm-ss"));
|
||
ini.setValue("eCamAngleUnit", m_cbCamAngleUnit->currentIndex()); // 0=弧度, 1=角度
|
||
ini.setValue("eEulerOrder", eulerOrderIndex);
|
||
ini.setValue("eRobotPosAxisOrder", axisOrder);
|
||
|
||
if (mode == 0) {
|
||
int count = m_tableHandEye->rowCount();
|
||
ini.setValue("eCalibMethod", "PARK");
|
||
ini.setValue("nPointCount", count);
|
||
ini.setValue("nCalibItemNum", count);
|
||
ini.setValue("nCurCalibItemIdx", count);
|
||
ini.endGroup();
|
||
|
||
for (int row = 0; row < count; ++row) {
|
||
ini.beginGroup(QString("CalibPointInfo_%1").arg(row));
|
||
ini.setValue("dCamX", m_tableHandEye->item(row, 0) ? m_tableHandEye->item(row, 0)->text() : "0");
|
||
ini.setValue("dCamY", m_tableHandEye->item(row, 1) ? m_tableHandEye->item(row, 1)->text() : "0");
|
||
ini.setValue("dCamZ", m_tableHandEye->item(row, 2) ? m_tableHandEye->item(row, 2)->text() : "0");
|
||
ini.setValue("dCamRoll", m_tableHandEye->item(row, 3) ? m_tableHandEye->item(row, 3)->text() : "0");
|
||
ini.setValue("dCamPitch", m_tableHandEye->item(row, 4) ? m_tableHandEye->item(row, 4)->text() : "0");
|
||
ini.setValue("dCamYaw", m_tableHandEye->item(row, 5) ? m_tableHandEye->item(row, 5)->text() : "0");
|
||
ini.setValue("dRobotX", m_tableHandEye->item(row, 6) ? m_tableHandEye->item(row, 6)->text() : "0");
|
||
ini.setValue("dRobotY", m_tableHandEye->item(row, 7) ? m_tableHandEye->item(row, 7)->text() : "0");
|
||
ini.setValue("dRobotZ", m_tableHandEye->item(row, 8) ? m_tableHandEye->item(row, 8)->text() : "0");
|
||
ini.setValue("dRobotRoll", m_tableHandEye->item(row, 9) ? m_tableHandEye->item(row, 9)->text() : "0");
|
||
ini.setValue("dRobotPitch", m_tableHandEye->item(row, 10) ? m_tableHandEye->item(row, 10)->text() : "0");
|
||
ini.setValue("dRobotYaw", m_tableHandEye->item(row, 11) ? m_tableHandEye->item(row, 11)->text() : "0");
|
||
ini.endGroup();
|
||
|
||
ini.beginGroup(QString("pCalibItemList_%1").arg(row));
|
||
ini.setValue("sRobotPos_x", tableText(m_tableHandEye, row, 6));
|
||
ini.setValue("sRobotPos_y", tableText(m_tableHandEye, row, 7));
|
||
ini.setValue("sRobotPos_z", tableText(m_tableHandEye, row, 8));
|
||
ini.setValue("sRobotPos_roll", tableText(m_tableHandEye, row, 9));
|
||
ini.setValue("sRobotPos_pitch", tableText(m_tableHandEye, row, 10));
|
||
ini.setValue("sRobotPos_yaw", tableText(m_tableHandEye, row, 11));
|
||
ini.setValue("dExtrinsic_0", tableText(m_tableHandEye, row, 0).toDouble());
|
||
ini.setValue("dExtrinsic_1", tableText(m_tableHandEye, row, 1).toDouble());
|
||
ini.setValue("dExtrinsic_2", tableText(m_tableHandEye, row, 2).toDouble());
|
||
ini.setValue("dExtrinsic_3", cameraAngleToRadians(m_tableHandEye, row, 3, m_cbCamAngleUnit->currentIndex()));
|
||
ini.setValue("dExtrinsic_4", cameraAngleToRadians(m_tableHandEye, row, 4, m_cbCamAngleUnit->currentIndex()));
|
||
ini.setValue("dExtrinsic_5", cameraAngleToRadians(m_tableHandEye, row, 5, m_cbCamAngleUnit->currentIndex()));
|
||
ini.endGroup();
|
||
}
|
||
} else if (mode == 1) {
|
||
int count = m_tableHandEye->rowCount();
|
||
ini.setValue("eCalibMethod", "PARK");
|
||
ini.setValue("nPointCount", count);
|
||
ini.setValue("nCalibItemNum", count);
|
||
ini.setValue("nCurCalibItemIdx", count);
|
||
ini.endGroup();
|
||
|
||
for (int row = 0; row < count; ++row) {
|
||
ini.beginGroup(QString("CalibPointInfo_%1").arg(row));
|
||
ini.setValue("dEndX", m_tableHandEye->item(row, 0) ? m_tableHandEye->item(row, 0)->text() : "0");
|
||
ini.setValue("dEndY", m_tableHandEye->item(row, 1) ? m_tableHandEye->item(row, 1)->text() : "0");
|
||
ini.setValue("dEndZ", m_tableHandEye->item(row, 2) ? m_tableHandEye->item(row, 2)->text() : "0");
|
||
ini.setValue("dEndRoll", m_tableHandEye->item(row, 3) ? m_tableHandEye->item(row, 3)->text() : "0");
|
||
ini.setValue("dEndPitch", m_tableHandEye->item(row, 4) ? m_tableHandEye->item(row, 4)->text() : "0");
|
||
ini.setValue("dEndYaw", m_tableHandEye->item(row, 5) ? m_tableHandEye->item(row, 5)->text() : "0");
|
||
ini.setValue("dCamX", m_tableHandEye->item(row, 6) ? m_tableHandEye->item(row, 6)->text() : "0");
|
||
ini.setValue("dCamY", m_tableHandEye->item(row, 7) ? m_tableHandEye->item(row, 7)->text() : "0");
|
||
ini.setValue("dCamZ", m_tableHandEye->item(row, 8) ? m_tableHandEye->item(row, 8)->text() : "0");
|
||
ini.setValue("dCamRoll", m_tableHandEye->item(row, 9) ? m_tableHandEye->item(row, 9)->text() : "0");
|
||
ini.setValue("dCamPitch", m_tableHandEye->item(row, 10) ? m_tableHandEye->item(row, 10)->text() : "0");
|
||
ini.setValue("dCamYaw", m_tableHandEye->item(row, 11) ? m_tableHandEye->item(row, 11)->text() : "0");
|
||
ini.endGroup();
|
||
|
||
ini.beginGroup(QString("pCalibItemList_%1").arg(row));
|
||
ini.setValue("sRobotPos_x", tableText(m_tableHandEye, row, 0));
|
||
ini.setValue("sRobotPos_y", tableText(m_tableHandEye, row, 1));
|
||
ini.setValue("sRobotPos_z", tableText(m_tableHandEye, row, 2));
|
||
ini.setValue("sRobotPos_roll", tableText(m_tableHandEye, row, 3));
|
||
ini.setValue("sRobotPos_pitch", tableText(m_tableHandEye, row, 4));
|
||
ini.setValue("sRobotPos_yaw", tableText(m_tableHandEye, row, 5));
|
||
ini.setValue("dExtrinsic_0", tableText(m_tableHandEye, row, 6).toDouble());
|
||
ini.setValue("dExtrinsic_1", tableText(m_tableHandEye, row, 7).toDouble());
|
||
ini.setValue("dExtrinsic_2", tableText(m_tableHandEye, row, 8).toDouble());
|
||
ini.setValue("dExtrinsic_3", cameraAngleToRadians(m_tableHandEye, row, 9, m_cbCamAngleUnit->currentIndex()));
|
||
ini.setValue("dExtrinsic_4", cameraAngleToRadians(m_tableHandEye, row, 10, m_cbCamAngleUnit->currentIndex()));
|
||
ini.setValue("dExtrinsic_5", cameraAngleToRadians(m_tableHandEye, row, 11, m_cbCamAngleUnit->currentIndex()));
|
||
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 = parseCalibType(ini.value("eCalibType"));
|
||
if (calibType < 0) {
|
||
calibType = ini.value("eCalibTypeIndex", -1).toInt();
|
||
}
|
||
|
||
// 读取标定板角度单位 (0=弧度, 1=角度，默认0兼容旧文件)
|
||
int camAngleUnit = ini.value("eCamAngleUnit", 0).toInt();
|
||
m_cbCamAngleUnit->setCurrentIndex(camAngleUnit);
|
||
|
||
// 读取欧拉角顺序：优先使用当前工具保存的 eEulerOrder，兼容旧文件的 eRobotPosAxisOrder
|
||
int savedEulerOrder = ini.value("eEulerOrder", -1).toInt();
|
||
if (savedEulerOrder >= 0 && savedEulerOrder <= 5) {
|
||
m_cbEulerOrder->setCurrentIndex(savedEulerOrder);
|
||
} else {
|
||
QString axisOrder = ini.value("eRobotPosAxisOrder", "RzRyRx").toString();
|
||
|
||
// 旧文件里保存的是轴顺序标签，不是矩阵乘法顺序。
|
||
// RxRyRz 表示 RX-roll / RY-pitch / RZ-yaw。
|
||
if (axisOrder == "RzRyRx") {
|
||
m_cbEulerOrder->setCurrentIndex(0); // ZYX
|
||
} else if (axisOrder == "RxRyRz") {
|
||
m_cbEulerOrder->setCurrentIndex(1); // XYZ
|
||
} else if (axisOrder == "RxRzRy") {
|
||
m_cbEulerOrder->setCurrentIndex(2); // XZY
|
||
} else if (axisOrder == "RyRxRz") {
|
||
m_cbEulerOrder->setCurrentIndex(3); // YXZ
|
||
} else if (axisOrder == "RyRzRx") {
|
||
m_cbEulerOrder->setCurrentIndex(4); // YZX
|
||
} else if (axisOrder == "RzRxRy") {
|
||
m_cbEulerOrder->setCurrentIndex(5); // ZXY
|
||
} else {
|
||
m_cbEulerOrder->setCurrentIndex(0); // 默认 ZYX / RzRyRx
|
||
}
|
||
}
|
||
|
||
const int handEyePointCount = ini.value("nPointCount", 0).toInt();
|
||
const int handEyeItemCount = ini.value("nCalibItemNum", 0).toInt();
|
||
|
||
if (calibType == 0) {
|
||
ini.endGroup();
|
||
|
||
m_cbCalibType->setCurrentIndex(0);
|
||
m_tableHandEye->setRowCount(0);
|
||
m_boardPoseData.clear();
|
||
|
||
if (handEyeItemCount > 0) {
|
||
for (int i = 0; i < handEyeItemCount; ++i) {
|
||
ini.beginGroup(QString("pCalibItemList_%1").arg(i));
|
||
int row = m_tableHandEye->rowCount();
|
||
m_tableHandEye->insertRow(row);
|
||
|
||
m_tableHandEye->setItem(row, 0, new QTableWidgetItem(ini.value("dExtrinsic_0", "0").toString()));
|
||
m_tableHandEye->setItem(row, 1, new QTableWidgetItem(ini.value("dExtrinsic_1", "0").toString()));
|
||
m_tableHandEye->setItem(row, 2, new QTableWidgetItem(ini.value("dExtrinsic_2", "0").toString()));
|
||
|
||
double camRoll = ini.value("dExtrinsic_3", 0).toDouble();
|
||
double camPitch = ini.value("dExtrinsic_4", 0).toDouble();
|
||
double camYaw = ini.value("dExtrinsic_5", 0).toDouble();
|
||
m_tableHandEye->setItem(row, 3, new QTableWidgetItem(rawAngleToDisplayText(camRoll, camAngleUnit)));
|
||
m_tableHandEye->setItem(row, 4, new QTableWidgetItem(rawAngleToDisplayText(camPitch, camAngleUnit)));
|
||
m_tableHandEye->setItem(row, 5, new QTableWidgetItem(rawAngleToDisplayText(camYaw, camAngleUnit)));
|
||
|
||
m_tableHandEye->setItem(row, 6, new QTableWidgetItem(ini.value("sRobotPos_x", "0").toString()));
|
||
m_tableHandEye->setItem(row, 7, new QTableWidgetItem(ini.value("sRobotPos_y", "0").toString()));
|
||
m_tableHandEye->setItem(row, 8, new QTableWidgetItem(ini.value("sRobotPos_z", "0").toString()));
|
||
|
||
// 机器人姿态保持角度（不转换）
|
||
double robotRoll = ini.value("sRobotPos_roll", 0).toDouble();
|
||
double robotPitch = ini.value("sRobotPos_pitch", 0).toDouble();
|
||
double robotYaw = ini.value("sRobotPos_yaw", 0).toDouble();
|
||
m_tableHandEye->setItem(row, 9, new QTableWidgetItem(QString::number(robotRoll, 'f', 6)));
|
||
m_tableHandEye->setItem(row, 10, new QTableWidgetItem(QString::number(robotPitch, 'f', 6)));
|
||
m_tableHandEye->setItem(row, 11, new QTableWidgetItem(QString::number(robotYaw, 'f', 6)));
|
||
|
||
m_boardPoseData.push_back({camRoll, camPitch, camYaw});
|
||
|
||
ini.endGroup();
|
||
}
|
||
} else if (handEyePointCount > 0) {
|
||
for (int i = 0; i < handEyePointCount; ++i) {
|
||
ini.beginGroup(QString("CalibPointInfo_%1").arg(i));
|
||
int row = m_tableHandEye->rowCount();
|
||
m_tableHandEye->insertRow(row);
|
||
m_tableHandEye->setItem(row, 0, new QTableWidgetItem(ini.value("dCamX", "0").toString()));
|
||
m_tableHandEye->setItem(row, 1, new QTableWidgetItem(ini.value("dCamY", "0").toString()));
|
||
m_tableHandEye->setItem(row, 2, new QTableWidgetItem(ini.value("dCamZ", "0").toString()));
|
||
m_tableHandEye->setItem(row, 3, new QTableWidgetItem(ini.value("dCamRoll", "0").toString()));
|
||
m_tableHandEye->setItem(row, 4, new QTableWidgetItem(ini.value("dCamPitch", "0").toString()));
|
||
m_tableHandEye->setItem(row, 5, new QTableWidgetItem(ini.value("dCamYaw", "0").toString()));
|
||
m_tableHandEye->setItem(row, 6, new QTableWidgetItem(ini.value("dRobotX", "0").toString()));
|
||
m_tableHandEye->setItem(row, 7, new QTableWidgetItem(ini.value("dRobotY", "0").toString()));
|
||
m_tableHandEye->setItem(row, 8, new QTableWidgetItem(ini.value("dRobotZ", "0").toString()));
|
||
m_tableHandEye->setItem(row, 9, new QTableWidgetItem(ini.value("dRobotRoll", "0").toString()));
|
||
m_tableHandEye->setItem(row, 10, new QTableWidgetItem(ini.value("dRobotPitch", "0").toString()));
|
||
m_tableHandEye->setItem(row, 11, new QTableWidgetItem(ini.value("dRobotYaw", "0").toString()));
|
||
ini.endGroup();
|
||
}
|
||
}
|
||
} else if (calibType == 1) {
|
||
ini.endGroup();
|
||
|
||
m_cbCalibType->setCurrentIndex(1);
|
||
m_tableHandEye->setRowCount(0);
|
||
m_boardPoseData.clear();
|
||
if (handEyeItemCount > 0) {
|
||
for (int i = 0; i < handEyeItemCount; ++i) {
|
||
ini.beginGroup(QString("pCalibItemList_%1").arg(i));
|
||
int row = m_tableHandEye->rowCount();
|
||
m_tableHandEye->insertRow(row);
|
||
m_tableHandEye->setItem(row, 0, new QTableWidgetItem(ini.value("sRobotPos_x", "0").toString()));
|
||
m_tableHandEye->setItem(row, 1, new QTableWidgetItem(ini.value("sRobotPos_y", "0").toString()));
|
||
m_tableHandEye->setItem(row, 2, new QTableWidgetItem(ini.value("sRobotPos_z", "0").toString()));
|
||
m_tableHandEye->setItem(row, 3, new QTableWidgetItem(ini.value("sRobotPos_roll", "0").toString()));
|
||
m_tableHandEye->setItem(row, 4, new QTableWidgetItem(ini.value("sRobotPos_pitch", "0").toString()));
|
||
m_tableHandEye->setItem(row, 5, new QTableWidgetItem(ini.value("sRobotPos_yaw", "0").toString()));
|
||
m_tableHandEye->setItem(row, 6, new QTableWidgetItem(ini.value("dExtrinsic_0", "0").toString()));
|
||
m_tableHandEye->setItem(row, 7, new QTableWidgetItem(ini.value("dExtrinsic_1", "0").toString()));
|
||
m_tableHandEye->setItem(row, 8, new QTableWidgetItem(ini.value("dExtrinsic_2", "0").toString()));
|
||
|
||
double camRoll = ini.value("dExtrinsic_3", 0).toDouble();
|
||
double camPitch = ini.value("dExtrinsic_4", 0).toDouble();
|
||
double camYaw = ini.value("dExtrinsic_5", 0).toDouble();
|
||
m_tableHandEye->setItem(row, 9, new QTableWidgetItem(rawAngleToDisplayText(camRoll, camAngleUnit)));
|
||
m_tableHandEye->setItem(row, 10, new QTableWidgetItem(rawAngleToDisplayText(camPitch, camAngleUnit)));
|
||
m_tableHandEye->setItem(row, 11, new QTableWidgetItem(rawAngleToDisplayText(camYaw, camAngleUnit)));
|
||
|
||
m_boardPoseData.push_back({camRoll, camPitch, camYaw});
|
||
ini.endGroup();
|
||
}
|
||
} else {
|
||
for (int i = 0; i < handEyePointCount; ++i) {
|
||
ini.beginGroup(QString("CalibPointInfo_%1").arg(i));
|
||
int row = m_tableHandEye->rowCount();
|
||
m_tableHandEye->insertRow(row);
|
||
m_tableHandEye->setItem(row, 0, new QTableWidgetItem(ini.value("dEndX", "0").toString()));
|
||
m_tableHandEye->setItem(row, 1, new QTableWidgetItem(ini.value("dEndY", "0").toString()));
|
||
m_tableHandEye->setItem(row, 2, new QTableWidgetItem(ini.value("dEndZ", "0").toString()));
|
||
m_tableHandEye->setItem(row, 3, new QTableWidgetItem(ini.value("dEndRoll", "0").toString()));
|
||
m_tableHandEye->setItem(row, 4, new QTableWidgetItem(ini.value("dEndPitch", "0").toString()));
|
||
m_tableHandEye->setItem(row, 5, new QTableWidgetItem(ini.value("dEndYaw", "0").toString()));
|
||
m_tableHandEye->setItem(row, 6, new QTableWidgetItem(ini.value("dCamX", "0").toString()));
|
||
m_tableHandEye->setItem(row, 7, new QTableWidgetItem(ini.value("dCamY", "0").toString()));
|
||
m_tableHandEye->setItem(row, 8, new QTableWidgetItem(ini.value("dCamZ", "0").toString()));
|
||
m_tableHandEye->setItem(row, 9, new QTableWidgetItem(ini.value("dCamRoll", "0").toString()));
|
||
m_tableHandEye->setItem(row, 10, new QTableWidgetItem(ini.value("dCamPitch", "0").toString()));
|
||
m_tableHandEye->setItem(row, 11, new QTableWidgetItem(ini.value("dCamYaw", "0").toString()));
|
||
ini.endGroup();
|
||
}
|
||
}
|
||
} else if (calibType == 2) {
|
||
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);
|
||
m_boardPoseData.clear();
|
||
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();
|
||
}
|
||
}
|