414 lines
14 KiB
C++
414 lines
14 KiB
C++
#include "CalibDataWidget.h"
|
|
|
|
#include <QVBoxLayout>
|
|
#include <QHBoxLayout>
|
|
#include <QGridLayout>
|
|
#include <QHeaderView>
|
|
#include <QLabel>
|
|
|
|
CalibDataWidget::CalibDataWidget(QWidget* parent)
|
|
: QWidget(parent)
|
|
, m_cbCalibType(nullptr)
|
|
, m_tableEyeToHand(nullptr)
|
|
, m_groupEyeToHand(nullptr)
|
|
, m_tableEyeInHand(nullptr)
|
|
, m_groupEyeInHand(nullptr)
|
|
, m_sbTransformX(nullptr)
|
|
, m_sbTransformY(nullptr)
|
|
, m_sbTransformZ(nullptr)
|
|
, m_sbRoll(nullptr)
|
|
, m_sbPitch(nullptr)
|
|
, m_sbYaw(nullptr)
|
|
, m_cbEulerOrder(nullptr)
|
|
, m_btnAddRow(nullptr)
|
|
, m_btnDeleteRow(nullptr)
|
|
{
|
|
setupUI();
|
|
}
|
|
|
|
CalibDataWidget::~CalibDataWidget()
|
|
{
|
|
}
|
|
|
|
void CalibDataWidget::setupUI()
|
|
{
|
|
QVBoxLayout* mainLayout = new QVBoxLayout(this);
|
|
|
|
// 标定模式选择
|
|
QHBoxLayout* modeLayout = new QHBoxLayout();
|
|
QLabel* lblMode = new QLabel("标定模式:", this);
|
|
m_cbCalibType = new QComboBox(this);
|
|
m_cbCalibType->addItem("Eye-To-Hand (眼在手外)");
|
|
m_cbCalibType->addItem("Eye-In-Hand (眼在手上)");
|
|
connect(m_cbCalibType, QOverload<int>::of(&QComboBox::currentIndexChanged),
|
|
this, &CalibDataWidget::onCalibTypeChanged);
|
|
modeLayout->addWidget(lblMode);
|
|
modeLayout->addWidget(m_cbCalibType);
|
|
modeLayout->addStretch();
|
|
mainLayout->addLayout(modeLayout);
|
|
|
|
// Eye-To-Hand 数据组
|
|
m_groupEyeToHand = createEyeToHandGroup();
|
|
mainLayout->addWidget(m_groupEyeToHand);
|
|
|
|
// Eye-In-Hand 数据组
|
|
m_groupEyeInHand = createEyeInHandGroup();
|
|
mainLayout->addWidget(m_groupEyeInHand);
|
|
m_groupEyeInHand->setVisible(false);
|
|
|
|
// 按钮行
|
|
QHBoxLayout* btnLayout = new QHBoxLayout();
|
|
m_btnAddRow = new QPushButton("添加行", this);
|
|
m_btnDeleteRow = new QPushButton("删除行", this);
|
|
connect(m_btnAddRow, &QPushButton::clicked, this, &CalibDataWidget::onAddRow);
|
|
connect(m_btnDeleteRow, &QPushButton::clicked, this, &CalibDataWidget::onDeleteRow);
|
|
btnLayout->addWidget(m_btnAddRow);
|
|
btnLayout->addWidget(m_btnDeleteRow);
|
|
btnLayout->addStretch();
|
|
mainLayout->addLayout(btnLayout);
|
|
|
|
// 变换测试组
|
|
mainLayout->addWidget(createTransformGroup());
|
|
|
|
// 欧拉角测试组
|
|
mainLayout->addWidget(createEulerGroup());
|
|
|
|
mainLayout->addStretch();
|
|
}
|
|
|
|
QGroupBox* CalibDataWidget::createEyeToHandGroup()
|
|
{
|
|
QGroupBox* group = new QGroupBox("Eye-To-Hand 标定数据", this);
|
|
QVBoxLayout* layout = new QVBoxLayout(group);
|
|
|
|
m_tableEyeToHand = new QTableWidget(this);
|
|
m_tableEyeToHand->setColumnCount(6);
|
|
m_tableEyeToHand->setHorizontalHeaderLabels({
|
|
"Eye X", "Eye Y", "Eye Z", "Robot X", "Robot Y", "Robot Z"
|
|
});
|
|
m_tableEyeToHand->horizontalHeader()->setStretchLastSection(true);
|
|
m_tableEyeToHand->setSelectionBehavior(QAbstractItemView::SelectRows);
|
|
|
|
layout->addWidget(m_tableEyeToHand);
|
|
return group;
|
|
}
|
|
|
|
QGroupBox* CalibDataWidget::createEyeInHandGroup()
|
|
{
|
|
QGroupBox* group = new QGroupBox("Eye-In-Hand 标定数据", this);
|
|
QVBoxLayout* layout = new QVBoxLayout(group);
|
|
|
|
m_tableEyeInHand = new QTableWidget(this);
|
|
m_tableEyeInHand->setColumnCount(9);
|
|
m_tableEyeInHand->setHorizontalHeaderLabels({
|
|
"End X", "End Y", "End Z", "End Roll", "End Pitch", "End Yaw",
|
|
"Cam X", "Cam Y", "Cam Z"
|
|
});
|
|
m_tableEyeInHand->horizontalHeader()->setStretchLastSection(true);
|
|
m_tableEyeInHand->setSelectionBehavior(QAbstractItemView::SelectRows);
|
|
|
|
layout->addWidget(m_tableEyeInHand);
|
|
return group;
|
|
}
|
|
|
|
QGroupBox* CalibDataWidget::createTransformGroup()
|
|
{
|
|
QGroupBox* group = new QGroupBox("坐标变换测试", this);
|
|
QGridLayout* layout = new QGridLayout(group);
|
|
|
|
layout->addWidget(new QLabel("X:", this), 0, 0);
|
|
m_sbTransformX = new QDoubleSpinBox(this);
|
|
m_sbTransformX->setRange(-10000, 10000);
|
|
m_sbTransformX->setDecimals(3);
|
|
layout->addWidget(m_sbTransformX, 0, 1);
|
|
|
|
layout->addWidget(new QLabel("Y:", this), 0, 2);
|
|
m_sbTransformY = new QDoubleSpinBox(this);
|
|
m_sbTransformY->setRange(-10000, 10000);
|
|
m_sbTransformY->setDecimals(3);
|
|
layout->addWidget(m_sbTransformY, 0, 3);
|
|
|
|
layout->addWidget(new QLabel("Z:", this), 0, 4);
|
|
m_sbTransformZ = new QDoubleSpinBox(this);
|
|
m_sbTransformZ->setRange(-10000, 10000);
|
|
m_sbTransformZ->setDecimals(3);
|
|
layout->addWidget(m_sbTransformZ, 0, 5);
|
|
|
|
return group;
|
|
}
|
|
|
|
QGroupBox* CalibDataWidget::createEulerGroup()
|
|
{
|
|
QGroupBox* group = new QGroupBox("欧拉角转换测试", this);
|
|
QGridLayout* layout = new QGridLayout(group);
|
|
|
|
layout->addWidget(new QLabel("Roll (°):", this), 0, 0);
|
|
m_sbRoll = new QDoubleSpinBox(this);
|
|
m_sbRoll->setRange(-180, 180);
|
|
m_sbRoll->setDecimals(2);
|
|
layout->addWidget(m_sbRoll, 0, 1);
|
|
|
|
layout->addWidget(new QLabel("Pitch (°):", this), 0, 2);
|
|
m_sbPitch = new QDoubleSpinBox(this);
|
|
m_sbPitch->setRange(-180, 180);
|
|
m_sbPitch->setDecimals(2);
|
|
layout->addWidget(m_sbPitch, 0, 3);
|
|
|
|
layout->addWidget(new QLabel("Yaw (°):", this), 0, 4);
|
|
m_sbYaw = new QDoubleSpinBox(this);
|
|
m_sbYaw->setRange(-180, 180);
|
|
m_sbYaw->setDecimals(2);
|
|
layout->addWidget(m_sbYaw, 0, 5);
|
|
|
|
layout->addWidget(new QLabel("旋转顺序:", this), 1, 0);
|
|
m_cbEulerOrder = new QComboBox(this);
|
|
m_cbEulerOrder->addItem("XYZ", static_cast<int>(HECEulerOrder::XYZ));
|
|
m_cbEulerOrder->addItem("XZY", static_cast<int>(HECEulerOrder::XZY));
|
|
m_cbEulerOrder->addItem("YXZ", static_cast<int>(HECEulerOrder::YXZ));
|
|
m_cbEulerOrder->addItem("YZX", static_cast<int>(HECEulerOrder::YZX));
|
|
m_cbEulerOrder->addItem("ZXY", static_cast<int>(HECEulerOrder::ZXY));
|
|
m_cbEulerOrder->addItem("ZYX (常用)", static_cast<int>(HECEulerOrder::ZYX));
|
|
m_cbEulerOrder->setCurrentIndex(5); // 默认 ZYX
|
|
layout->addWidget(m_cbEulerOrder, 1, 1, 1, 2);
|
|
|
|
return group;
|
|
}
|
|
|
|
void CalibDataWidget::updateTableVisibility()
|
|
{
|
|
bool isEyeToHand = (m_cbCalibType->currentIndex() == 0);
|
|
m_groupEyeToHand->setVisible(isEyeToHand);
|
|
m_groupEyeInHand->setVisible(!isEyeToHand);
|
|
}
|
|
|
|
void CalibDataWidget::onAddRow()
|
|
{
|
|
if (m_cbCalibType->currentIndex() == 0) {
|
|
int row = m_tableEyeToHand->rowCount();
|
|
m_tableEyeToHand->insertRow(row);
|
|
for (int col = 0; col < 6; ++col) {
|
|
QTableWidgetItem* item = new QTableWidgetItem("0");
|
|
m_tableEyeToHand->setItem(row, col, item);
|
|
}
|
|
} else {
|
|
int row = m_tableEyeInHand->rowCount();
|
|
m_tableEyeInHand->insertRow(row);
|
|
for (int col = 0; col < 9; ++col) {
|
|
QTableWidgetItem* item = new QTableWidgetItem("0");
|
|
m_tableEyeInHand->setItem(row, col, item);
|
|
}
|
|
}
|
|
}
|
|
|
|
void CalibDataWidget::onDeleteRow()
|
|
{
|
|
if (m_cbCalibType->currentIndex() == 0) {
|
|
int row = m_tableEyeToHand->currentRow();
|
|
if (row >= 0) {
|
|
m_tableEyeToHand->removeRow(row);
|
|
}
|
|
} else {
|
|
int row = m_tableEyeInHand->currentRow();
|
|
if (row >= 0) {
|
|
m_tableEyeInHand->removeRow(row);
|
|
}
|
|
}
|
|
}
|
|
|
|
void CalibDataWidget::onCalibTypeChanged(int index)
|
|
{
|
|
updateTableVisibility();
|
|
emit calibTypeChanged(index == 0 ? HECCalibrationType::EyeToHand : HECCalibrationType::EyeInHand);
|
|
}
|
|
|
|
void CalibDataWidget::getEyeToHandData(std::vector<HECPoint3D>& eyePoints,
|
|
std::vector<HECPoint3D>& robotPoints) const
|
|
{
|
|
eyePoints.clear();
|
|
robotPoints.clear();
|
|
|
|
for (int row = 0; row < m_tableEyeToHand->rowCount(); ++row) {
|
|
HECPoint3D eyePt, robotPt;
|
|
|
|
QTableWidgetItem* item0 = m_tableEyeToHand->item(row, 0);
|
|
QTableWidgetItem* item1 = m_tableEyeToHand->item(row, 1);
|
|
QTableWidgetItem* item2 = m_tableEyeToHand->item(row, 2);
|
|
QTableWidgetItem* item3 = m_tableEyeToHand->item(row, 3);
|
|
QTableWidgetItem* item4 = m_tableEyeToHand->item(row, 4);
|
|
QTableWidgetItem* item5 = m_tableEyeToHand->item(row, 5);
|
|
|
|
if (item0 && item1 && item2 && item3 && item4 && item5) {
|
|
eyePt.x = item0->text().toDouble();
|
|
eyePt.y = item1->text().toDouble();
|
|
eyePt.z = item2->text().toDouble();
|
|
robotPt.x = item3->text().toDouble();
|
|
robotPt.y = item4->text().toDouble();
|
|
robotPt.z = item5->text().toDouble();
|
|
|
|
eyePoints.push_back(eyePt);
|
|
robotPoints.push_back(robotPt);
|
|
}
|
|
}
|
|
}
|
|
|
|
void CalibDataWidget::getEyeInHandData(std::vector<HECEyeInHandData>& calibData) const
|
|
{
|
|
calibData.clear();
|
|
|
|
const double deg2rad = M_PI / 180.0;
|
|
|
|
for (int row = 0; row < m_tableEyeInHand->rowCount(); ++row) {
|
|
HECEyeInHandData data;
|
|
|
|
// 获取末端位姿
|
|
double endX = m_tableEyeInHand->item(row, 0) ?
|
|
m_tableEyeInHand->item(row, 0)->text().toDouble() : 0;
|
|
double endY = m_tableEyeInHand->item(row, 1) ?
|
|
m_tableEyeInHand->item(row, 1)->text().toDouble() : 0;
|
|
double endZ = m_tableEyeInHand->item(row, 2) ?
|
|
m_tableEyeInHand->item(row, 2)->text().toDouble() : 0;
|
|
double endRoll = m_tableEyeInHand->item(row, 3) ?
|
|
m_tableEyeInHand->item(row, 3)->text().toDouble() * deg2rad : 0;
|
|
double endPitch = m_tableEyeInHand->item(row, 4) ?
|
|
m_tableEyeInHand->item(row, 4)->text().toDouble() * deg2rad : 0;
|
|
double endYaw = m_tableEyeInHand->item(row, 5) ?
|
|
m_tableEyeInHand->item(row, 5)->text().toDouble() * deg2rad : 0;
|
|
|
|
// 构建末端位姿矩阵
|
|
HECEulerAngles angles(endRoll, endPitch, endYaw);
|
|
HECRotationMatrix R;
|
|
|
|
// 使用 ZYX 顺序构建旋转矩阵
|
|
double cr = cos(endRoll), sr = sin(endRoll);
|
|
double cp = cos(endPitch), sp = sin(endPitch);
|
|
double cy = cos(endYaw), sy = sin(endYaw);
|
|
|
|
R.at(0, 0) = cy * cp;
|
|
R.at(0, 1) = cy * sp * sr - sy * cr;
|
|
R.at(0, 2) = cy * sp * cr + sy * sr;
|
|
R.at(1, 0) = sy * cp;
|
|
R.at(1, 1) = sy * sp * sr + cy * cr;
|
|
R.at(1, 2) = sy * sp * cr - cy * sr;
|
|
R.at(2, 0) = -sp;
|
|
R.at(2, 1) = cp * sr;
|
|
R.at(2, 2) = cp * cr;
|
|
|
|
HECTranslationVector T(endX, endY, endZ);
|
|
data.endPose = HECHomogeneousMatrix(R, T);
|
|
|
|
// 获取相机观测点
|
|
data.targetInCamera.x = m_tableEyeInHand->item(row, 6) ?
|
|
m_tableEyeInHand->item(row, 6)->text().toDouble() : 0;
|
|
data.targetInCamera.y = m_tableEyeInHand->item(row, 7) ?
|
|
m_tableEyeInHand->item(row, 7)->text().toDouble() : 0;
|
|
data.targetInCamera.z = m_tableEyeInHand->item(row, 8) ?
|
|
m_tableEyeInHand->item(row, 8)->text().toDouble() : 0;
|
|
|
|
calibData.push_back(data);
|
|
}
|
|
}
|
|
|
|
HECCalibrationType CalibDataWidget::getCalibType() const
|
|
{
|
|
return m_cbCalibType->currentIndex() == 0 ?
|
|
HECCalibrationType::EyeToHand : HECCalibrationType::EyeInHand;
|
|
}
|
|
|
|
HECEulerOrder CalibDataWidget::getEulerOrder() const
|
|
{
|
|
return static_cast<HECEulerOrder>(m_cbEulerOrder->currentData().toInt());
|
|
}
|
|
|
|
void CalibDataWidget::clearAll()
|
|
{
|
|
m_tableEyeToHand->setRowCount(0);
|
|
m_tableEyeInHand->setRowCount(0);
|
|
m_sbTransformX->setValue(0);
|
|
m_sbTransformY->setValue(0);
|
|
m_sbTransformZ->setValue(0);
|
|
m_sbRoll->setValue(0);
|
|
m_sbPitch->setValue(0);
|
|
m_sbYaw->setValue(0);
|
|
}
|
|
|
|
void CalibDataWidget::loadTestData()
|
|
{
|
|
// 清除现有数据
|
|
m_tableEyeToHand->setRowCount(0);
|
|
|
|
// 添加测试数据 (Eye-To-Hand)
|
|
// 模拟相机坐标系和机器人坐标系之间的对应点
|
|
struct TestPoint {
|
|
double ex, ey, ez; // 相机坐标
|
|
double rx, ry, rz; // 机器人坐标
|
|
};
|
|
|
|
TestPoint testData[] = {
|
|
{100.0, 50.0, 200.0, 500.0, 300.0, 100.0},
|
|
{150.0, 80.0, 210.0, 550.0, 330.0, 110.0},
|
|
{120.0, 60.0, 195.0, 520.0, 310.0, 95.0},
|
|
{180.0, 100.0, 220.0, 580.0, 350.0, 120.0},
|
|
{90.0, 40.0, 190.0, 490.0, 290.0, 90.0},
|
|
{200.0, 120.0, 230.0, 600.0, 370.0, 130.0},
|
|
{110.0, 55.0, 198.0, 510.0, 305.0, 98.0},
|
|
{160.0, 85.0, 215.0, 560.0, 335.0, 115.0},
|
|
{130.0, 65.0, 202.0, 530.0, 315.0, 102.0},
|
|
{170.0, 95.0, 218.0, 570.0, 345.0, 118.0}
|
|
};
|
|
|
|
for (const auto& pt : testData) {
|
|
int row = m_tableEyeToHand->rowCount();
|
|
m_tableEyeToHand->insertRow(row);
|
|
m_tableEyeToHand->setItem(row, 0, new QTableWidgetItem(QString::number(pt.ex)));
|
|
m_tableEyeToHand->setItem(row, 1, new QTableWidgetItem(QString::number(pt.ey)));
|
|
m_tableEyeToHand->setItem(row, 2, new QTableWidgetItem(QString::number(pt.ez)));
|
|
m_tableEyeToHand->setItem(row, 3, new QTableWidgetItem(QString::number(pt.rx)));
|
|
m_tableEyeToHand->setItem(row, 4, new QTableWidgetItem(QString::number(pt.ry)));
|
|
m_tableEyeToHand->setItem(row, 5, new QTableWidgetItem(QString::number(pt.rz)));
|
|
}
|
|
|
|
// 设置变换测试点
|
|
m_sbTransformX->setValue(140.0);
|
|
m_sbTransformY->setValue(70.0);
|
|
m_sbTransformZ->setValue(205.0);
|
|
|
|
// 设置欧拉角测试值
|
|
m_sbRoll->setValue(10.0);
|
|
m_sbPitch->setValue(20.0);
|
|
m_sbYaw->setValue(30.0);
|
|
}
|
|
|
|
void CalibDataWidget::setTransformPoint(const HECPoint3D& point)
|
|
{
|
|
m_sbTransformX->setValue(point.x);
|
|
m_sbTransformY->setValue(point.y);
|
|
m_sbTransformZ->setValue(point.z);
|
|
}
|
|
|
|
HECPoint3D CalibDataWidget::getTransformPoint() const
|
|
{
|
|
return HECPoint3D(
|
|
m_sbTransformX->value(),
|
|
m_sbTransformY->value(),
|
|
m_sbTransformZ->value()
|
|
);
|
|
}
|
|
|
|
void CalibDataWidget::setEulerAngles(const HECEulerAngles& angles)
|
|
{
|
|
double roll, pitch, yaw;
|
|
angles.toDegrees(roll, pitch, yaw);
|
|
m_sbRoll->setValue(roll);
|
|
m_sbPitch->setValue(pitch);
|
|
m_sbYaw->setValue(yaw);
|
|
}
|
|
|
|
HECEulerAngles CalibDataWidget::getEulerAngles() const
|
|
{
|
|
return HECEulerAngles::fromDegrees(
|
|
m_sbRoll->value(),
|
|
m_sbPitch->value(),
|
|
m_sbYaw->value()
|
|
);
|
|
}
|