846 lines
30 KiB
C++
846 lines
30 KiB
C++
#include "CalibViewMainWindow.h"
|
||
#include "CalibDataWidget.h"
|
||
#include "CalibResultWidget.h"
|
||
#include "BatchVerifyDialog.h"
|
||
#include "MainWindow.h"
|
||
#include "VrEyeViewWidget.h"
|
||
#include "../../SpinBoxPasteHelper.h"
|
||
#include "IChessboardDetector.h"
|
||
|
||
#include <QMenuBar>
|
||
#include <QAction>
|
||
#include <QSplitter>
|
||
#include <QVBoxLayout>
|
||
#include <QHBoxLayout>
|
||
#include <QGridLayout>
|
||
#include <QGroupBox>
|
||
#include <QMessageBox>
|
||
#include <QFileDialog>
|
||
#include <QFile>
|
||
#include <QTextStream>
|
||
#include <QLabel>
|
||
#include <QScrollArea>
|
||
#include <QSettings>
|
||
#include <QDateTime>
|
||
|
||
CalibViewMainWindow::CalibViewMainWindow(QWidget* parent)
|
||
: QMainWindow(parent)
|
||
, m_calib(nullptr)
|
||
, m_dataWidget(nullptr)
|
||
, m_resultWidget(nullptr)
|
||
, m_sbTransformX(nullptr)
|
||
, m_sbTransformY(nullptr)
|
||
, m_sbTransformZ(nullptr)
|
||
, m_sbRoll(nullptr)
|
||
, m_sbPitch(nullptr)
|
||
, m_sbYaw(nullptr)
|
||
, m_cbEulerOrder(nullptr)
|
||
, m_btnTransform(nullptr)
|
||
, m_logEdit(nullptr)
|
||
, m_hasResult(false)
|
||
, m_robotView(nullptr)
|
||
, m_vrEyeView(nullptr)
|
||
{
|
||
// 创建标定实例
|
||
m_calib = CreateHandEyeCalibInstance();
|
||
|
||
setupUI();
|
||
createMenuBar();
|
||
SpinBoxPasteHelper::install(this);
|
||
|
||
setWindowTitle("CalibView - 手眼标定工具 - 1.0.0");
|
||
resize(1400, 700);
|
||
|
||
updateStatusBar("就绪");
|
||
}
|
||
|
||
CalibViewMainWindow::~CalibViewMainWindow()
|
||
{
|
||
if (m_calib) {
|
||
DestroyHandEyeCalibInstance(m_calib);
|
||
m_calib = nullptr;
|
||
}
|
||
}
|
||
|
||
QWidget* CalibViewMainWindow::createRightPanel()
|
||
{
|
||
QWidget* rightPanel = new QWidget(this);
|
||
QVBoxLayout* rightLayout = new QVBoxLayout(rightPanel);
|
||
|
||
// 坐标变换测试组
|
||
QGroupBox* transformGroup = new QGroupBox("坐标变换测试", this);
|
||
QGridLayout* transformLayout = new QGridLayout(transformGroup);
|
||
|
||
transformLayout->addWidget(new QLabel("X:", this), 0, 0);
|
||
m_sbTransformX = new QDoubleSpinBox(this);
|
||
m_sbTransformX->setRange(-10000, 10000);
|
||
m_sbTransformX->setDecimals(3);
|
||
transformLayout->addWidget(m_sbTransformX, 0, 1);
|
||
|
||
transformLayout->addWidget(new QLabel("Y:", this), 0, 2);
|
||
m_sbTransformY = new QDoubleSpinBox(this);
|
||
m_sbTransformY->setRange(-10000, 10000);
|
||
m_sbTransformY->setDecimals(3);
|
||
transformLayout->addWidget(m_sbTransformY, 0, 3);
|
||
|
||
transformLayout->addWidget(new QLabel("Z:", this), 0, 4);
|
||
m_sbTransformZ = new QDoubleSpinBox(this);
|
||
m_sbTransformZ->setRange(-10000, 10000);
|
||
m_sbTransformZ->setDecimals(3);
|
||
transformLayout->addWidget(m_sbTransformZ, 0, 5);
|
||
|
||
// 第2行：姿态 Roll/Pitch/Yaw
|
||
transformLayout->addWidget(new QLabel("Roll:", this), 1, 0);
|
||
m_sbRoll = new QDoubleSpinBox(this);
|
||
m_sbRoll->setRange(-180, 180);
|
||
m_sbRoll->setDecimals(6);
|
||
transformLayout->addWidget(m_sbRoll, 1, 1);
|
||
|
||
transformLayout->addWidget(new QLabel("Pitch:", this), 1, 2);
|
||
m_sbPitch = new QDoubleSpinBox(this);
|
||
m_sbPitch->setRange(-180, 180);
|
||
m_sbPitch->setDecimals(6);
|
||
transformLayout->addWidget(m_sbPitch, 1, 3);
|
||
|
||
transformLayout->addWidget(new QLabel("Yaw:", this), 1, 4);
|
||
m_sbYaw = new QDoubleSpinBox(this);
|
||
m_sbYaw->setRange(-180, 180);
|
||
m_sbYaw->setDecimals(6);
|
||
transformLayout->addWidget(m_sbYaw, 1, 5);
|
||
|
||
// 第3行：角度单位选择
|
||
transformLayout->addWidget(new QLabel("角度单位:", this), 2, 0);
|
||
m_cbAngleUnit = new QComboBox(this);
|
||
m_cbAngleUnit->addItem("度(°)", 0);
|
||
m_cbAngleUnit->addItem("弧度(rad)", 1);
|
||
m_cbAngleUnit->setCurrentIndex(0); // 默认度
|
||
transformLayout->addWidget(m_cbAngleUnit, 2, 1);
|
||
|
||
// 第4行：欧拉角顺序
|
||
transformLayout->addWidget(new QLabel("欧拉角顺序:", this), 3, 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
|
||
transformLayout->addWidget(m_cbEulerOrder, 3, 1, 1, 2);
|
||
|
||
// 第5行：变换按钮
|
||
m_btnTransform = new QPushButton("变换", this);
|
||
connect(m_btnTransform, &QPushButton::clicked, this, &CalibViewMainWindow::onTransformTest);
|
||
transformLayout->addWidget(m_btnTransform, 4, 0, 1, 6);
|
||
|
||
rightLayout->addWidget(transformGroup);
|
||
|
||
// 日志组
|
||
QGroupBox* logGroup = new QGroupBox("日志", this);
|
||
QVBoxLayout* logLayout = new QVBoxLayout(logGroup);
|
||
|
||
m_logEdit = new QTextEdit(this);
|
||
m_logEdit->setReadOnly(true);
|
||
m_logEdit->setFont(QFont("Consolas", 9));
|
||
|
||
logLayout->addWidget(m_logEdit);
|
||
|
||
// 清除日志按钮
|
||
QPushButton* btnClearLog = new QPushButton("清除日志", this);
|
||
connect(btnClearLog, &QPushButton::clicked, m_logEdit, &QTextEdit::clear);
|
||
logLayout->addWidget(btnClearLog);
|
||
|
||
rightLayout->addWidget(logGroup, 1); // stretch=1 让日志区占据剩余空间
|
||
|
||
return rightPanel;
|
||
}
|
||
|
||
void CalibViewMainWindow::setupUI()
|
||
{
|
||
// 创建中央控件
|
||
QWidget* centralWidget = new QWidget(this);
|
||
QHBoxLayout* mainLayout = new QHBoxLayout(centralWidget);
|
||
|
||
// 创建分割器
|
||
QSplitter* splitter = new QSplitter(Qt::Horizontal, this);
|
||
|
||
// 左侧面板：数据输入 + 标定结果
|
||
QWidget* leftPanel = new QWidget(this);
|
||
QVBoxLayout* leftLayout = new QVBoxLayout(leftPanel);
|
||
|
||
// 数据输入（可滚动）
|
||
QScrollArea* dataScroll = new QScrollArea(this);
|
||
m_dataWidget = new CalibDataWidget(this);
|
||
dataScroll->setWidget(m_dataWidget);
|
||
dataScroll->setWidgetResizable(true);
|
||
leftLayout->addWidget(dataScroll, 1);
|
||
|
||
// 标定结果
|
||
m_resultWidget = new CalibResultWidget(this);
|
||
leftLayout->addWidget(m_resultWidget);
|
||
|
||
splitter->addWidget(leftPanel);
|
||
|
||
// 右侧面板：测试工具 + 日志
|
||
QWidget* rightPanel = createRightPanel();
|
||
splitter->addWidget(rightPanel);
|
||
|
||
// 设置分割比例（左侧标定部分更宽，右侧测试栏更窄）
|
||
splitter->setStretchFactor(0, 5);
|
||
splitter->setStretchFactor(1, 1);
|
||
|
||
// 设置初始大小
|
||
splitter->setSizes(QList<int>() << 1300 << 150);
|
||
|
||
mainLayout->addWidget(splitter);
|
||
setCentralWidget(centralWidget);
|
||
|
||
// 创建状态栏
|
||
statusBar()->showMessage("就绪");
|
||
|
||
// 连接 CalibDataWidget 的信号
|
||
connect(m_dataWidget, &CalibDataWidget::requestEyeToHandCalib, this, &CalibViewMainWindow::onEyeToHandCalib);
|
||
connect(m_dataWidget, &CalibDataWidget::requestEyeInHandCalib, this, &CalibViewMainWindow::onEyeInHandCalib);
|
||
connect(m_dataWidget, &CalibDataWidget::requestTCPCalib, this, &CalibViewMainWindow::onTCPCalib);
|
||
}
|
||
|
||
void CalibViewMainWindow::createMenuBar()
|
||
{
|
||
// 文件菜单
|
||
QMenu* fileMenu = menuBar()->addMenu("文件(&F)");
|
||
|
||
QAction* actSave = fileMenu->addAction("保存结果(&S)");
|
||
actSave->setShortcut(QKeySequence::Save);
|
||
connect(actSave, &QAction::triggered, this, &CalibViewMainWindow::onSaveResult);
|
||
|
||
QAction* actLoad = fileMenu->addAction("加载结果(&L)");
|
||
actLoad->setShortcut(QKeySequence::Open);
|
||
connect(actLoad, &QAction::triggered, this, &CalibViewMainWindow::onLoadResult);
|
||
|
||
fileMenu->addSeparator();
|
||
|
||
QAction* actSaveData = fileMenu->addAction("保存标定数据(&D)");
|
||
actSaveData->setShortcut(QKeySequence(Qt::CTRL | Qt::SHIFT | Qt::Key_S));
|
||
connect(actSaveData, &QAction::triggered, this, &CalibViewMainWindow::onSaveCalibData);
|
||
|
||
QAction* actLoadData = fileMenu->addAction("加载标定数据(&A)");
|
||
actLoadData->setShortcut(QKeySequence(Qt::CTRL | Qt::SHIFT | Qt::Key_O));
|
||
connect(actLoadData, &QAction::triggered, this, &CalibViewMainWindow::onLoadCalibData);
|
||
|
||
fileMenu->addSeparator();
|
||
|
||
QAction* actExit = fileMenu->addAction("退出(&X)");
|
||
actExit->setShortcut(QKeySequence::Quit);
|
||
connect(actExit, &QAction::triggered, this, &QMainWindow::close);
|
||
|
||
// 标定菜单
|
||
QMenu* calibMenu = menuBar()->addMenu("标定(&C)");
|
||
|
||
QAction* actEyeToHand = calibMenu->addAction("Eye-To-Hand 标定(&E)");
|
||
connect(actEyeToHand, &QAction::triggered, this, &CalibViewMainWindow::onEyeToHandCalib);
|
||
|
||
QAction* actEyeInHand = calibMenu->addAction("Eye-In-Hand 标定(&I)");
|
||
connect(actEyeInHand, &QAction::triggered, this, &CalibViewMainWindow::onEyeInHandCalib);
|
||
|
||
QAction* actTCPCalib = calibMenu->addAction("TCP 标定(&P)");
|
||
connect(actTCPCalib, &QAction::triggered, this, &CalibViewMainWindow::onTCPCalib);
|
||
|
||
calibMenu->addSeparator();
|
||
|
||
QAction* actTransform = calibMenu->addAction("坐标变换测试(&T)");
|
||
connect(actTransform, &QAction::triggered, this, &CalibViewMainWindow::onTransformTest);
|
||
|
||
calibMenu->addSeparator();
|
||
|
||
QAction* actClear = calibMenu->addAction("清除所有(&C)");
|
||
connect(actClear, &QAction::triggered, this, &CalibViewMainWindow::onClearAll);
|
||
|
||
// 工具菜单
|
||
QMenu* toolMenu = menuBar()->addMenu("工具(&T)");
|
||
QAction* actRobotView = toolMenu->addAction("机器人控制(&R)");
|
||
connect(actRobotView, &QAction::triggered, this, &CalibViewMainWindow::onOpenRobotView);
|
||
|
||
QAction* actVrEyeView = toolMenu->addAction("相机标定板检测(&V)");
|
||
connect(actVrEyeView, &QAction::triggered, this, &CalibViewMainWindow::onOpenVrEyeView);
|
||
|
||
toolMenu->addSeparator();
|
||
|
||
QAction* actBatchVerify = toolMenu->addAction("批量验证(&B)");
|
||
actBatchVerify->setShortcut(QKeySequence(Qt::CTRL | Qt::Key_B));
|
||
connect(actBatchVerify, &QAction::triggered, this, &CalibViewMainWindow::onOpenBatchVerify);
|
||
|
||
// 帮助菜单
|
||
QMenu* helpMenu = menuBar()->addMenu("帮助(&H)");
|
||
|
||
QAction* actAbout = helpMenu->addAction("关于(&A)");
|
||
connect(actAbout, &QAction::triggered, this, [this]() {
|
||
QMessageBox::about(this, "关于 CalibView",
|
||
"CalibView - 手眼标定工具\n\n"
|
||
"用于测试 HandEyeCalib 模块的各项功能：\n"
|
||
"- Eye-To-Hand 标定\n"
|
||
"- Eye-In-Hand 标定\n"
|
||
"- TCP 标定\n"
|
||
"- 坐标变换\n"
|
||
"- 欧拉角转换\n\n"
|
||
"基于 Eigen 库实现的 SVD 分解算法");
|
||
});
|
||
}
|
||
|
||
void CalibViewMainWindow::updateStatusBar(const QString& message)
|
||
{
|
||
statusBar()->showMessage(message);
|
||
}
|
||
|
||
void CalibViewMainWindow::appendLog(const QString& message)
|
||
{
|
||
m_logEdit->append(message);
|
||
}
|
||
|
||
void CalibViewMainWindow::onEyeToHandCalib()
|
||
{
|
||
if (!m_calib) {
|
||
QMessageBox::critical(this, "错误", "标定实例未初始化");
|
||
return;
|
||
}
|
||
|
||
QMessageBox msgBox(this);
|
||
msgBox.setWindowTitle("选择标定方法");
|
||
msgBox.setText("请选择 Eye-To-Hand 标定方法：");
|
||
QPushButton* simpleBtn = msgBox.addButton("简单方法 (仅位置)", QMessageBox::ActionRole);
|
||
QPushButton* poseBtn = msgBox.addButton("完整位姿方法 (Park算法)", QMessageBox::ActionRole);
|
||
QPushButton* cancelBtn = msgBox.addButton("取消", QMessageBox::RejectRole);
|
||
|
||
msgBox.exec();
|
||
|
||
if (msgBox.clickedButton() == cancelBtn) {
|
||
return;
|
||
}
|
||
|
||
if (msgBox.clickedButton() == simpleBtn) {
|
||
onEyeToHandCalibSimple();
|
||
} else if (msgBox.clickedButton() == poseBtn) {
|
||
onEyeToHandCalibWithPose();
|
||
}
|
||
}
|
||
|
||
void CalibViewMainWindow::onEyeToHandCalibSimple()
|
||
{
|
||
appendLog("开始 Eye-To-Hand 标定（简单方法 - 仅位置）...");
|
||
|
||
std::vector<HECPoint3D> eyePoints;
|
||
std::vector<HECPoint3D> robotPoints;
|
||
m_dataWidget->getEyeToHandData(eyePoints, robotPoints);
|
||
|
||
if (eyePoints.size() < 3) {
|
||
QMessageBox::warning(this, "警告", "至少需要3组数据进行标定");
|
||
return;
|
||
}
|
||
|
||
appendLog(QString("输入数据组数: %1").arg(eyePoints.size()));
|
||
appendLog("使用 SVD 分解方法，仅利用位置信息");
|
||
|
||
int ret = m_calib->CalculateRT(eyePoints, robotPoints, m_currentResult);
|
||
|
||
if (ret == 0) {
|
||
m_hasResult = true;
|
||
m_resultWidget->showCalibResult(m_currentResult);
|
||
appendLog(QString("标定成功，误差: %1 mm")
|
||
.arg(m_currentResult.error, 0, 'f', 4));
|
||
updateStatusBar("Eye-To-Hand 标定完成（简单方法）");
|
||
emit calibrationCompleted(m_currentResult);
|
||
} else {
|
||
appendLog(QString("标定失败，错误码: %1").arg(ret));
|
||
QMessageBox::critical(this, "错误", QString("标定失败，错误码: %1").arg(ret));
|
||
}
|
||
}
|
||
|
||
void CalibViewMainWindow::onEyeToHandCalibWithPose()
|
||
{
|
||
appendLog("开始 Eye-To-Hand 标定（完整位姿方法 - Park 算法）...");
|
||
|
||
std::vector<HECEyeToHandData> calibData;
|
||
m_dataWidget->getEyeToHandPoseData(calibData);
|
||
|
||
if (calibData.size() < 3) {
|
||
QMessageBox::warning(this, "警告",
|
||
"完整位姿法需要至少3组数据，且数据必须包含标定板的完整位姿信息。\n"
|
||
"请确保数据已从包含完整位姿的 INI 文件加载。");
|
||
return;
|
||
}
|
||
|
||
HECEulerOrder eulerOrder = m_dataWidget->getEulerOrder();
|
||
QString eulerOrderStr;
|
||
switch (eulerOrder) {
|
||
case HECEulerOrder::ZYX: eulerOrderStr = "ZYX"; break;
|
||
case HECEulerOrder::XYZ: eulerOrderStr = "XYZ"; break;
|
||
case HECEulerOrder::XZY: eulerOrderStr = "XZY"; break;
|
||
case HECEulerOrder::YXZ: eulerOrderStr = "YXZ"; break;
|
||
case HECEulerOrder::YZX: eulerOrderStr = "YZX"; break;
|
||
case HECEulerOrder::ZXY: eulerOrderStr = "ZXY"; break;
|
||
}
|
||
|
||
appendLog(QString("输入数据组数: %1").arg(calibData.size()));
|
||
appendLog(QString("欧拉角顺序: %1").arg(eulerOrderStr));
|
||
appendLog("使用 Park/Tsai-Lenz 算法，利用完整位姿信息（位置+姿态）");
|
||
|
||
int ret = m_calib->CalculateEyeToHandWithPose(calibData, m_currentResult);
|
||
|
||
if (ret == 0) {
|
||
m_hasResult = true;
|
||
m_resultWidget->showCalibResult(m_currentResult);
|
||
appendLog(QString("标定成功，误差: %1 mm")
|
||
.arg(m_currentResult.error, 0, 'f', 4));
|
||
updateStatusBar("Eye-To-Hand 标定完成（Park 方法）");
|
||
emit calibrationCompleted(m_currentResult);
|
||
} else {
|
||
appendLog(QString("标定失败，错误码: %1").arg(ret));
|
||
QMessageBox::critical(this, "错误", QString("标定失败，错误码: %1").arg(ret));
|
||
}
|
||
}
|
||
|
||
void CalibViewMainWindow::onEyeInHandCalib()
|
||
{
|
||
if (!m_calib) {
|
||
QMessageBox::critical(this, "错误", "标定实例未初始化");
|
||
return;
|
||
}
|
||
|
||
std::vector<HECEyeInHandData> calibData;
|
||
m_dataWidget->getEyeInHandData(calibData);
|
||
|
||
if (calibData.size() < 3) {
|
||
QMessageBox::warning(this, "警告", "至少需要3组数据进行标定");
|
||
return;
|
||
}
|
||
|
||
appendLog("开始 Eye-In-Hand 标定...");
|
||
appendLog(QString("输入数据组数: %1").arg(calibData.size()));
|
||
|
||
int ret = m_calib->CalculateEyeInHand(calibData, m_currentResult);
|
||
|
||
if (ret == 0) {
|
||
m_hasResult = true;
|
||
m_resultWidget->showCalibResult(m_currentResult);
|
||
appendLog(QString("标定成功，误差: %1 mm")
|
||
.arg(m_currentResult.error, 0, 'f', 4));
|
||
updateStatusBar("Eye-In-Hand 标定完成");
|
||
emit calibrationCompleted(m_currentResult);
|
||
} else {
|
||
appendLog(QString("标定失败，错误码: %1").arg(ret));
|
||
QMessageBox::critical(this, "错误", QString("标定失败，错误码: %1").arg(ret));
|
||
}
|
||
}
|
||
|
||
void CalibViewMainWindow::onTCPCalib()
|
||
{
|
||
if (!m_calib) {
|
||
QMessageBox::critical(this, "错误", "标定实例未初始化");
|
||
return;
|
||
}
|
||
|
||
HECTCPCalibData tcpData = m_dataWidget->getTCPCalibData();
|
||
|
||
if (tcpData.poses.size() < 3) {
|
||
QMessageBox::warning(this, "警告", "至少需要3组法兰位姿进行TCP标定");
|
||
return;
|
||
}
|
||
|
||
if (tcpData.mode == HECTCPCalibMode::Full6DOF) {
|
||
if (tcpData.referencePoseIndex < 0 ||
|
||
tcpData.referencePoseIndex >= static_cast<int>(tcpData.poses.size())) {
|
||
QMessageBox::warning(this, "警告",
|
||
QString("参考位姿索引 %1 越界，有效范围: 0-%2")
|
||
.arg(tcpData.referencePoseIndex)
|
||
.arg(tcpData.poses.size() - 1));
|
||
return;
|
||
}
|
||
}
|
||
|
||
QString modeName = (tcpData.mode == HECTCPCalibMode::PositionOnly) ?
|
||
"3-DOF 位置标定" : "6-DOF 完整标定";
|
||
appendLog(QString("开始 TCP 标定 (%1)...").arg(modeName));
|
||
appendLog(QString("输入位姿数: %1").arg(tcpData.poses.size()));
|
||
|
||
HECTCPCalibResult tcpResult = m_calib->CalculateTCP(tcpData);
|
||
|
||
if (tcpResult.success) {
|
||
m_resultWidget->showTCPCalibResult(tcpResult);
|
||
|
||
appendLog("=== TCP 标定结果 ===");
|
||
appendLog(QString("TCP 位置偏移: tx=%1, ty=%2, tz=%3")
|
||
.arg(tcpResult.tx, 0, 'f', 3)
|
||
.arg(tcpResult.ty, 0, 'f', 3)
|
||
.arg(tcpResult.tz, 0, 'f', 3));
|
||
if (tcpResult.rx != 0 || tcpResult.ry != 0 || tcpResult.rz != 0) {
|
||
appendLog(QString("TCP 姿态偏移: rx=%1\302\260, ry=%2\302\260, rz=%3\302\260")
|
||
.arg(tcpResult.rx, 0, 'f', 2)
|
||
.arg(tcpResult.ry, 0, 'f', 2)
|
||
.arg(tcpResult.rz, 0, 'f', 2));
|
||
}
|
||
appendLog(QString("残差误差: %1 mm").arg(tcpResult.residualError, 0, 'f', 4));
|
||
appendLog("TCP 标定成功");
|
||
updateStatusBar("TCP 标定完成");
|
||
} else {
|
||
appendLog(QString("TCP 标定失败: %1")
|
||
.arg(QString::fromStdString(tcpResult.errorMessage)));
|
||
QMessageBox::critical(this, "错误",
|
||
QString("TCP 标定失败: %1").arg(QString::fromStdString(tcpResult.errorMessage)));
|
||
}
|
||
}
|
||
|
||
void CalibViewMainWindow::onTransformTest()
|
||
{
|
||
if (!m_calib) {
|
||
QMessageBox::critical(this, "错误", "标定实例未初始化");
|
||
return;
|
||
}
|
||
|
||
if (!m_hasResult) {
|
||
QMessageBox::warning(this, "警告", "请先执行标定或加载标定结果");
|
||
return;
|
||
}
|
||
|
||
// 源位置
|
||
HECPoint3D srcPoint(
|
||
m_sbTransformX->value(),
|
||
m_sbTransformY->value(),
|
||
m_sbTransformZ->value()
|
||
);
|
||
|
||
// 获取角度单位（0=度，1=弧度）
|
||
bool isRadian = (m_cbAngleUnit->currentData().toInt() == 1);
|
||
|
||
// 源姿态
|
||
HECEulerAngles srcAngles;
|
||
if (isRadian) {
|
||
// 弧度直接使用
|
||
srcAngles = HECEulerAngles(
|
||
m_sbRoll->value(),
|
||
m_sbPitch->value(),
|
||
m_sbYaw->value()
|
||
);
|
||
} else {
|
||
// 角度转弧度
|
||
srcAngles = HECEulerAngles::fromDegrees(
|
||
m_sbRoll->value(),
|
||
m_sbPitch->value(),
|
||
m_sbYaw->value()
|
||
);
|
||
}
|
||
HECEulerOrder order = static_cast<HECEulerOrder>(m_cbEulerOrder->currentData().toInt());
|
||
|
||
// 欧拉角 -> 旋转矩阵
|
||
HECRotationMatrix R_src;
|
||
m_calib->EulerToRotationMatrix(srcAngles, order, R_src);
|
||
|
||
// 变换位置
|
||
HECPoint3D dstPoint;
|
||
m_calib->TransformPoint(m_currentResult.R, m_currentResult.T, srcPoint, dstPoint);
|
||
|
||
// 变换姿态：R_dst = R_calib * R_src
|
||
HECRotationMatrix R_dst = m_currentResult.R * R_src;
|
||
|
||
// 旋转矩阵 -> 欧拉角
|
||
HECEulerAngles dstAngles;
|
||
m_calib->RotationMatrixToEuler(R_dst, order, dstAngles);
|
||
|
||
// 输出日志
|
||
QString unitStr = isRadian ? "rad" : "°";
|
||
appendLog(QString("坐标变换结果:"));
|
||
appendLog(QString(" 源位置: (%1, %2, %3)")
|
||
.arg(srcPoint.x, 0, 'f', 3)
|
||
.arg(srcPoint.y, 0, 'f', 3)
|
||
.arg(srcPoint.z, 0, 'f', 3));
|
||
|
||
if (isRadian) {
|
||
appendLog(QString(" 源姿态: Roll=%1rad, Pitch=%2rad, Yaw=%3rad")
|
||
.arg(m_sbRoll->value(), 0, 'f', 6)
|
||
.arg(m_sbPitch->value(), 0, 'f', 6)
|
||
.arg(m_sbYaw->value(), 0, 'f', 6));
|
||
} else {
|
||
appendLog(QString(" 源姿态: Roll=%1°, Pitch=%2°, Yaw=%3°")
|
||
.arg(m_sbRoll->value(), 0, 'f', 2)
|
||
.arg(m_sbPitch->value(), 0, 'f', 2)
|
||
.arg(m_sbYaw->value(), 0, 'f', 2));
|
||
}
|
||
|
||
appendLog(QString(" 目标位置: (%1, %2, %3)")
|
||
.arg(dstPoint.x, 0, 'f', 3)
|
||
.arg(dstPoint.y, 0, 'f', 3)
|
||
.arg(dstPoint.z, 0, 'f', 3));
|
||
|
||
// 目标姿态：同时输出弧度和角度
|
||
double dstRoll, dstPitch, dstYaw;
|
||
dstAngles.toDegrees(dstRoll, dstPitch, dstYaw);
|
||
appendLog(QString(" 目标姿态(弧度): Roll=%1rad, Pitch=%2rad, Yaw=%3rad")
|
||
.arg(dstAngles.roll, 0, 'f', 6)
|
||
.arg(dstAngles.pitch, 0, 'f', 6)
|
||
.arg(dstAngles.yaw, 0, 'f', 6));
|
||
appendLog(QString(" 目标姿态(角度): Roll=%1°, Pitch=%2°, Yaw=%3°")
|
||
.arg(dstRoll, 0, 'f', 2)
|
||
.arg(dstPitch, 0, 'f', 2)
|
||
.arg(dstYaw, 0, 'f', 2));
|
||
|
||
updateStatusBar("坐标变换完成");
|
||
}
|
||
|
||
|
||
void CalibViewMainWindow::onClearAll()
|
||
{
|
||
m_dataWidget->clearAll();
|
||
m_resultWidget->clearAll();
|
||
m_logEdit->clear();
|
||
m_sbTransformX->setValue(0);
|
||
m_sbTransformY->setValue(0);
|
||
m_sbTransformZ->setValue(0);
|
||
m_sbRoll->setValue(0);
|
||
m_sbPitch->setValue(0);
|
||
m_sbYaw->setValue(0);
|
||
m_hasResult = false;
|
||
updateStatusBar("已清除所有数据");
|
||
}
|
||
|
||
void CalibViewMainWindow::onSaveResult()
|
||
{
|
||
if (!m_hasResult) {
|
||
QMessageBox::warning(this, "警告", "没有可保存的标定结果");
|
||
return;
|
||
}
|
||
|
||
QString defaultName = QString("CalibResult_%1.ini")
|
||
.arg(QDateTime::currentDateTime().toString("yyyyMMdd_HHmmss"));
|
||
|
||
QString fileName = QFileDialog::getSaveFileName(this,
|
||
"保存标定结果", defaultName, "INI文件 (*.ini)");
|
||
|
||
if (fileName.isEmpty()) {
|
||
return;
|
||
}
|
||
|
||
QSettings ini(fileName, QSettings::IniFormat);
|
||
ini.setIniCodec("UTF-8");
|
||
|
||
// [CommInfo]
|
||
ini.beginGroup("CommInfo");
|
||
ini.setValue("nMaxMaxMatrixNum", 8);
|
||
ini.setValue("nExistMatrixNum", 1);
|
||
ini.endGroup();
|
||
|
||
// [CalibMatrixInfo_0] - 4x4 齐次矩阵
|
||
ini.beginGroup("CalibMatrixInfo_0");
|
||
ini.setValue("sCalibTime", QDateTime::currentDateTime().toString("yyyy-MM-dd-HH-mm-ss"));
|
||
ini.setValue("nCalibPosIdx", 0);
|
||
ini.setValue("nCalibType", 0);
|
||
ini.setValue("nCalibMode", 0);
|
||
ini.setValue("sCalibPosName", 0);
|
||
ini.setValue("dError", m_currentResult.error);
|
||
// 第0行: R[0,0] R[0,1] R[0,2] Tx
|
||
ini.setValue("dCalibMatrix_0", m_currentResult.R.data[0]);
|
||
ini.setValue("dCalibMatrix_1", m_currentResult.R.data[1]);
|
||
ini.setValue("dCalibMatrix_2", m_currentResult.R.data[2]);
|
||
ini.setValue("dCalibMatrix_3", m_currentResult.T.data[0]);
|
||
// 第1行: R[1,0] R[1,1] R[1,2] Ty
|
||
ini.setValue("dCalibMatrix_4", m_currentResult.R.data[3]);
|
||
ini.setValue("dCalibMatrix_5", m_currentResult.R.data[4]);
|
||
ini.setValue("dCalibMatrix_6", m_currentResult.R.data[5]);
|
||
ini.setValue("dCalibMatrix_7", m_currentResult.T.data[1]);
|
||
// 第2行: R[2,0] R[2,1] R[2,2] Tz
|
||
ini.setValue("dCalibMatrix_8", m_currentResult.R.data[6]);
|
||
ini.setValue("dCalibMatrix_9", m_currentResult.R.data[7]);
|
||
ini.setValue("dCalibMatrix_10", m_currentResult.R.data[8]);
|
||
ini.setValue("dCalibMatrix_11", m_currentResult.T.data[2]);
|
||
// 第3行: 0 0 0 1
|
||
ini.setValue("dCalibMatrix_12", 0.0);
|
||
ini.setValue("dCalibMatrix_13", 0.0);
|
||
ini.setValue("dCalibMatrix_14", 0.0);
|
||
ini.setValue("dCalibMatrix_15", 1.0);
|
||
ini.endGroup();
|
||
|
||
appendLog(QString("结果已保存到: %1").arg(fileName));
|
||
updateStatusBar("结果已保存");
|
||
}
|
||
|
||
void CalibViewMainWindow::onLoadResult()
|
||
{
|
||
QString fileName = QFileDialog::getOpenFileName(this,
|
||
"加载标定结果", "", "INI文件 (*.ini)");
|
||
|
||
if (fileName.isEmpty()) {
|
||
return;
|
||
}
|
||
|
||
QSettings ini(fileName, QSettings::IniFormat);
|
||
ini.setIniCodec("UTF-8");
|
||
|
||
// 校验是否包含标定矩阵
|
||
ini.beginGroup("CalibMatrixInfo_0");
|
||
bool hasMatrix = ini.contains("dCalibMatrix_0");
|
||
if (!hasMatrix) {
|
||
ini.endGroup();
|
||
QMessageBox::warning(this, "警告", "该文件不包含有效的标定结果");
|
||
return;
|
||
}
|
||
|
||
// 加载 4x4 齐次矩阵 → 拆分为 R[3x3] + T[3x1]
|
||
// 第0行
|
||
m_currentResult.R.data[0] = ini.value("dCalibMatrix_0", 0).toDouble();
|
||
m_currentResult.R.data[1] = ini.value("dCalibMatrix_1", 0).toDouble();
|
||
m_currentResult.R.data[2] = ini.value("dCalibMatrix_2", 0).toDouble();
|
||
m_currentResult.T.data[0] = ini.value("dCalibMatrix_3", 0).toDouble();
|
||
// 第1行
|
||
m_currentResult.R.data[3] = ini.value("dCalibMatrix_4", 0).toDouble();
|
||
m_currentResult.R.data[4] = ini.value("dCalibMatrix_5", 0).toDouble();
|
||
m_currentResult.R.data[5] = ini.value("dCalibMatrix_6", 0).toDouble();
|
||
m_currentResult.T.data[1] = ini.value("dCalibMatrix_7", 0).toDouble();
|
||
// 第2行
|
||
m_currentResult.R.data[6] = ini.value("dCalibMatrix_8", 0).toDouble();
|
||
m_currentResult.R.data[7] = ini.value("dCalibMatrix_9", 0).toDouble();
|
||
m_currentResult.R.data[8] = ini.value("dCalibMatrix_10", 0).toDouble();
|
||
m_currentResult.T.data[2] = ini.value("dCalibMatrix_11", 0).toDouble();
|
||
ini.endGroup();
|
||
|
||
// 加载误差
|
||
ini.beginGroup("CommInfo");
|
||
m_currentResult.error = ini.value("dError", 0).toDouble();
|
||
ini.endGroup();
|
||
|
||
// 加载质心
|
||
ini.beginGroup("CenterInfo");
|
||
m_currentResult.centerEye.x = ini.value("dCenterEyeX", 0).toDouble();
|
||
m_currentResult.centerEye.y = ini.value("dCenterEyeY", 0).toDouble();
|
||
m_currentResult.centerEye.z = ini.value("dCenterEyeZ", 0).toDouble();
|
||
m_currentResult.centerRobot.x = ini.value("dCenterRobotX", 0).toDouble();
|
||
m_currentResult.centerRobot.y = ini.value("dCenterRobotY", 0).toDouble();
|
||
m_currentResult.centerRobot.z = ini.value("dCenterRobotZ", 0).toDouble();
|
||
ini.endGroup();
|
||
|
||
m_hasResult = true;
|
||
m_resultWidget->showCalibResult(m_currentResult);
|
||
appendLog(QString("已加载标定结果: %1").arg(fileName));
|
||
updateStatusBar("标定结果已加载");
|
||
}
|
||
|
||
void CalibViewMainWindow::onOpenRobotView()
|
||
{
|
||
if (!m_robotView) {
|
||
m_robotView = new MainWindow(this);
|
||
connect(m_robotView, &MainWindow::tcpPoseUpdated,
|
||
this, &CalibViewMainWindow::onRobotTcpPoseReceived);
|
||
}
|
||
m_robotView->show();
|
||
m_robotView->raise();
|
||
m_robotView->activateWindow();
|
||
}
|
||
|
||
void CalibViewMainWindow::onRobotTcpPoseReceived(
|
||
double x, double y, double z, double rx, double ry, double rz)
|
||
{
|
||
m_dataWidget->setRobotInput(x, y, z, rx, ry, rz);
|
||
appendLog(QString("收到机器人数据: (%1, %2, %3, %4, %5, %6)")
|
||
.arg(x, 0, 'f', 2).arg(y, 0, 'f', 2).arg(z, 0, 'f', 2)
|
||
.arg(rx, 0, 'f', 2).arg(ry, 0, 'f', 2).arg(rz, 0, 'f', 2));
|
||
}
|
||
|
||
void CalibViewMainWindow::onOpenVrEyeView()
|
||
{
|
||
if (!m_vrEyeView) {
|
||
m_vrEyeView = new VrEyeViewWidget(); // 不设置父窗口，独立窗口
|
||
|
||
// 连接信号槽
|
||
connect(m_vrEyeView, &VrEyeViewWidget::chessboardDetected,
|
||
this, [this](const ChessboardDetectionData& data) {
|
||
if (data.detected) {
|
||
onChessboardDetected(data.x, data.y, data.z, data.rx, data.ry, data.rz);
|
||
}
|
||
});
|
||
|
||
// 设置为独立窗口
|
||
m_vrEyeView->setWindowFlags(Qt::Window);
|
||
m_vrEyeView->resize(900, 700);
|
||
m_vrEyeView->setWindowTitle("相机标定板检测 - VrEyeView");
|
||
}
|
||
|
||
m_vrEyeView->show();
|
||
m_vrEyeView->raise();
|
||
m_vrEyeView->activateWindow();
|
||
}
|
||
|
||
void CalibViewMainWindow::onChessboardDetected(
|
||
double x, double y, double z, double rx, double ry, double rz)
|
||
{
|
||
// 将标定板检测结果作为相机坐标输入到数据控件
|
||
m_dataWidget->setCameraInput(x, y, z, rx, ry, rz);
|
||
appendLog(QString("收到标定板检测数据: 位置(%1, %2, %3) 姿态(%4°, %5°, %6°)")
|
||
.arg(x, 0, 'f', 2).arg(y, 0, 'f', 2).arg(z, 0, 'f', 2)
|
||
.arg(rx, 0, 'f', 2).arg(ry, 0, 'f', 2).arg(rz, 0, 'f', 2));
|
||
}
|
||
|
||
void CalibViewMainWindow::onSaveCalibData()
|
||
{
|
||
// 根据当前模式生成默认文件名
|
||
static const char* typeNames[] = { "EyeToHand", "EyeInHand", "TCP" };
|
||
int typeIndex = m_dataWidget->getCalibTypeIndex();
|
||
QString typeName = typeNames[typeIndex];
|
||
|
||
QString defaultName = QString("CalibData_%1_%2.ini")
|
||
.arg(typeName)
|
||
.arg(QDateTime::currentDateTime().toString("yyyyMMdd_HHmmss"));
|
||
|
||
QString fileName = QFileDialog::getSaveFileName(this,
|
||
"保存标定数据", defaultName, "INI文件 (*.ini)");
|
||
|
||
if (fileName.isEmpty()) {
|
||
return;
|
||
}
|
||
|
||
m_dataWidget->saveCalibData(fileName);
|
||
appendLog(QString("标定数据已保存到: %1").arg(fileName));
|
||
updateStatusBar("标定数据已保存");
|
||
}
|
||
|
||
void CalibViewMainWindow::onLoadCalibData()
|
||
{
|
||
QString fileName = QFileDialog::getOpenFileName(this,
|
||
"加载标定数据", "", "INI文件 (*.ini)");
|
||
|
||
if (fileName.isEmpty()) {
|
||
return;
|
||
}
|
||
|
||
// 校验文件是否包含标定数据
|
||
QSettings check(fileName, QSettings::IniFormat);
|
||
check.beginGroup("CommInfo");
|
||
int calibType = check.value("eCalibType", -1).toInt();
|
||
QString saveTime = check.value("sCalibTime").toString();
|
||
check.endGroup();
|
||
|
||
if (calibType < 0 || calibType > 2) {
|
||
QMessageBox::warning(this, "警告", "该文件不包含有效的标定数据");
|
||
return;
|
||
}
|
||
|
||
m_dataWidget->loadCalibData(fileName);
|
||
|
||
static const char* typeNames[] = { "EyeToHand", "EyeInHand", "TCP" };
|
||
appendLog(QString("已加载标定数据: %1 (保存时间: %2, 模式: %3)")
|
||
.arg(fileName).arg(saveTime).arg(typeNames[calibType]));
|
||
updateStatusBar("标定数据已加载");
|
||
}
|
||
|
||
void CalibViewMainWindow::onOpenBatchVerify()
|
||
{
|
||
// 创建标定板检测器实例
|
||
IChessboardDetector* detector = CreateChessboardDetectorInstance();
|
||
if (!detector) {
|
||
QMessageBox::critical(this, "错误", "无法创建标定板检测器实例");
|
||
return;
|
||
}
|
||
|
||
// 创建批量验证对话框
|
||
BatchVerifyDialog* dialog = new BatchVerifyDialog(detector, m_calib, this);
|
||
dialog->setAttribute(Qt::WA_DeleteOnClose);
|
||
dialog->show();
|
||
|
||
appendLog("打开批量验证工具");
|
||
}
|