946 lines
41 KiB
C++
946 lines
41 KiB
C++
#include "CalibDataWidget.h"
|
||
#include "../../SpinBoxPasteHelper.h"
|
||
|
||
#include <QVBoxLayout>
|
||
#include <QHBoxLayout>
|
||
#include <QSettings>
|
||
#include <QDateTime>
|
||
#include <QGridLayout>
|
||
#include <QHeaderView>
|
||
#include <QLabel>
|
||
#include <array>
|
||
|
||
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)
|
||
{
|
||
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 (默认)");
|
||
m_cbEulerOrder->addItem("XYZ");
|
||
m_cbEulerOrder->addItem("XZY");
|
||
m_cbEulerOrder->addItem("YXZ");
|
||
m_cbEulerOrder->addItem("YZX");
|
||
m_cbEulerOrder->addItem("ZXY");
|
||
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({
|
||
"标定板X(mm)", "标定板Y(mm)", "标定板Z(mm)", "标定板Rx(rad)", "标定板Ry(rad)", "标定板Rz(rad)",
|
||
"机械臂X(mm)", "机械臂Y(mm)", "机械臂Z(mm)", "机械臂Rx(°)", "机械臂Ry(°)", "机械臂Rz(°)"
|
||
});
|
||
m_tableHandEye->horizontalHeader()->setSectionResizeMode(QHeaderView::Stretch);
|
||
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);
|
||
|
||
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);
|
||
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(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 (弧度)
|
||
inputLayout->addWidget(new QLabel("标定板Rx(rad):", this), 1, 0);
|
||
m_inputCamRoll = createSpinBox();
|
||
inputLayout->addWidget(m_inputCamRoll, 1, 1);
|
||
inputLayout->addWidget(new QLabel("Ry(rad):", this), 1, 2);
|
||
m_inputCamPitch = createSpinBox();
|
||
inputLayout->addWidget(m_inputCamPitch, 1, 3);
|
||
inputLayout->addWidget(new QLabel("Rz(rad):", this), 1, 4);
|
||
m_inputCamYaw = createSpinBox();
|
||
inputLayout->addWidget(m_inputCamYaw, 1, 5);
|
||
|
||
// 第3行：机械臂 X/Y/Z
|
||
inputLayout->addWidget(new QLabel("机械臂X(mm):", this), 2, 0);
|
||
m_inputRobotX = createSpinBox();
|
||
inputLayout->addWidget(m_inputRobotX, 2, 1);
|
||
inputLayout->addWidget(new QLabel("Y(mm):", this), 2, 2);
|
||
m_inputRobotY = createSpinBox();
|
||
inputLayout->addWidget(m_inputRobotY, 2, 3);
|
||
inputLayout->addWidget(new QLabel("Z(mm):", this), 2, 4);
|
||
m_inputRobotZ = createSpinBox();
|
||
inputLayout->addWidget(m_inputRobotZ, 2, 5);
|
||
|
||
// 第4行：机械臂 Rx/Ry/Rz (角度)
|
||
inputLayout->addWidget(new QLabel("机械臂Rx(°):", this), 3, 0);
|
||
m_inputRobotRoll = createSpinBox();
|
||
inputLayout->addWidget(m_inputRobotRoll, 3, 1);
|
||
inputLayout->addWidget(new QLabel("Ry(°):", this), 3, 2);
|
||
m_inputRobotPitch = createSpinBox();
|
||
inputLayout->addWidget(m_inputRobotPitch, 3, 3);
|
||
inputLayout->addWidget(new QLabel("Rz(°):", this), 3, 4);
|
||
m_inputRobotYaw = createSpinBox();
|
||
inputLayout->addWidget(m_inputRobotYaw, 3, 5);
|
||
|
||
// 第5行：添加到表格按钮
|
||
m_btnHandEyeAddInput = new QPushButton("添加到表格", this);
|
||
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();
|
||
});
|
||
inputLayout->addWidget(m_btnHandEyeAddInput, 4, 0, 1, 6);
|
||
|
||
layout->addWidget(inputGroup);
|
||
|
||
return group;
|
||
}
|
||
|
||
void CalibDataWidget::onCalibTypeChanged(int index)
|
||
{
|
||
// 更新表格可见性
|
||
m_groupHandEye->setVisible(index == 0 || index == 1);
|
||
m_groupTCPCalib->setVisible(index == 2);
|
||
|
||
// 根据模式更新表格标题
|
||
if (index == 0) {
|
||
// Eye-To-Hand 模式
|
||
m_tableHandEye->setHorizontalHeaderLabels({
|
||
"Cam X", "Cam Y", "Cam Z", "Cam Roll", "Cam Pitch", "Cam Yaw",
|
||
"Robot X", "Robot Y", "Robot Z", "Robot Roll", "Robot Pitch", "Robot Yaw"
|
||
});
|
||
m_btnHandEyeCalib->setText("Eye-To-Hand 标定");
|
||
} else if (index == 1) {
|
||
// Eye-In-Hand 模式
|
||
m_tableHandEye->setHorizontalHeaderLabels({
|
||
"Robot X", "Robot Y", "Robot Z", "Robot Roll", "Robot Pitch", "Robot Yaw",
|
||
"Cam X", "Cam Y", "Cam Z", "Cam Roll", "Cam Pitch", "Cam Yaw"
|
||
});
|
||
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::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() : 0;
|
||
double endPitch = m_tableHandEye->item(row, 4) ?
|
||
m_tableHandEye->item(row, 4)->text().toDouble() : 0;
|
||
double endYaw = m_tableHandEye->item(row, 5) ?
|
||
m_tableHandEye->item(row, 5)->text().toDouble() : 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::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;
|
||
|
||
// 获取标定板在相机坐标系的姿态（Rodrigues旋转向量，单位：弧度）
|
||
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;
|
||
|
||
// 标定板姿态是Rodrigues旋转向量，需要转换为旋转矩阵
|
||
// 旋转向量的模长是旋转角度，方向是旋转轴
|
||
double theta = std::sqrt(boardRoll*boardRoll + boardPitch*boardPitch + boardYaw*boardYaw);
|
||
HECRotationMatrix R_board;
|
||
|
||
if (theta < 1e-6) {
|
||
// 旋转角度接近0，使用单位矩阵
|
||
R_board.at(0, 0) = 1; R_board.at(0, 1) = 0; R_board.at(0, 2) = 0;
|
||
R_board.at(1, 0) = 0; R_board.at(1, 1) = 1; R_board.at(1, 2) = 0;
|
||
R_board.at(2, 0) = 0; R_board.at(2, 1) = 0; R_board.at(2, 2) = 1;
|
||
} else {
|
||
// Rodrigues公式：R = I + sin(θ)/θ * K + (1-cos(θ))/θ² * K²
|
||
double rx = boardRoll / theta;
|
||
double ry = boardPitch / theta;
|
||
double rz = boardYaw / theta;
|
||
|
||
double c = std::cos(theta);
|
||
double s = std::sin(theta);
|
||
double c1 = 1.0 - c;
|
||
|
||
R_board.at(0, 0) = c + rx*rx*c1;
|
||
R_board.at(0, 1) = rx*ry*c1 - rz*s;
|
||
R_board.at(0, 2) = rx*rz*c1 + ry*s;
|
||
|
||
R_board.at(1, 0) = ry*rx*c1 + rz*s;
|
||
R_board.at(1, 1) = c + ry*ry*c1;
|
||
R_board.at(1, 2) = ry*rz*c1 - rx*s;
|
||
|
||
R_board.at(2, 0) = rz*rx*c1 - ry*s;
|
||
R_board.at(2, 1) = rz*ry*c1 + rx*s;
|
||
R_board.at(2, 2) = c + rz*rz*c1;
|
||
}
|
||
|
||
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);
|
||
}
|
||
|
||
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);
|
||
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;
|
||
};
|
||
|
||
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("添加到表格", 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);
|
||
|
||
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);
|
||
m_inputCamRoll->setValue(rx);
|
||
m_inputCamPitch->setValue(ry);
|
||
m_inputCamYaw->setValue(rz);
|
||
}
|
||
|
||
void CalibDataWidget::saveCalibData(const QString& filePath) const
|
||
{
|
||
QSettings ini(filePath, QSettings::IniFormat);
|
||
ini.setIniCodec("UTF-8");
|
||
|
||
int mode = m_cbCalibType->currentIndex();
|
||
|
||
ini.beginGroup("CommInfo");
|
||
ini.setValue("eCalibType", mode);
|
||
ini.setValue("sCalibTime", QDateTime::currentDateTime().toString("yyyy-MM-dd-HH-mm-ss"));
|
||
|
||
if (mode == 0) {
|
||
int count = m_tableHandEye->rowCount();
|
||
ini.setValue("nPointCount", 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();
|
||
}
|
||
} else if (mode == 1) {
|
||
int count = m_tableHandEye->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_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();
|
||
}
|
||
} 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();
|
||
|
||
// 读取欧拉角顺序
|
||
QString axisOrder = ini.value("eRobotPosAxisOrder", "RxRyRz").toString();
|
||
|
||
// 映射到UI的欧拉角顺序选择
|
||
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 (默认)
|
||
}
|
||
|
||
if (calibType == 0) {
|
||
int count = ini.value("nPointCount", 0).toInt();
|
||
int itemCount = ini.value("nCalibItemNum", 0).toInt();
|
||
ini.endGroup();
|
||
|
||
m_cbCalibType->setCurrentIndex(0);
|
||
|
||
if (itemCount > 0) {
|
||
m_tableHandEye->setRowCount(0);
|
||
m_boardPoseData.clear();
|
||
|
||
for (int i = 0; i < itemCount; ++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(QString::number(camRoll, 'f', 6)));
|
||
m_tableHandEye->setItem(row, 4, new QTableWidgetItem(QString::number(camPitch, 'f', 6)));
|
||
m_tableHandEye->setItem(row, 5, new QTableWidgetItem(QString::number(camYaw, 'f', 6)));
|
||
|
||
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 (count > 0) {
|
||
m_tableHandEye->setRowCount(0);
|
||
for (int i = 0; i < count; ++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) {
|
||
int count = ini.value("nPointCount", 0).toInt();
|
||
ini.endGroup();
|
||
|
||
m_cbCalibType->setCurrentIndex(1);
|
||
m_tableHandEye->setRowCount(0);
|
||
for (int i = 0; i < count; ++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);
|
||
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();
|
||
}
|
||
}
|