437 lines
14 KiB
C++
437 lines
14 KiB
C++
#include "CalibViewMainWindow.h"
|
|
#include "CalibDataWidget.h"
|
|
#include "CalibResultWidget.h"
|
|
|
|
#include <QMenuBar>
|
|
#include <QToolBar>
|
|
#include <QAction>
|
|
#include <QSplitter>
|
|
#include <QVBoxLayout>
|
|
#include <QHBoxLayout>
|
|
#include <QMessageBox>
|
|
#include <QFileDialog>
|
|
#include <QFile>
|
|
#include <QTextStream>
|
|
#include <QJsonDocument>
|
|
#include <QJsonObject>
|
|
#include <QJsonArray>
|
|
|
|
CalibViewMainWindow::CalibViewMainWindow(QWidget* parent)
|
|
: QMainWindow(parent)
|
|
, m_calib(nullptr)
|
|
, m_tabWidget(nullptr)
|
|
, m_dataWidget(nullptr)
|
|
, m_resultWidget(nullptr)
|
|
, m_hasResult(false)
|
|
{
|
|
// 创建标定实例
|
|
m_calib = CreateHandEyeCalibInstance();
|
|
|
|
setupUI();
|
|
createMenuBar();
|
|
createToolBar();
|
|
|
|
setWindowTitle("CalibView - 手眼标定测试工具");
|
|
resize(1200, 800);
|
|
|
|
updateStatusBar("就绪");
|
|
}
|
|
|
|
CalibViewMainWindow::~CalibViewMainWindow()
|
|
{
|
|
if (m_calib) {
|
|
DestroyHandEyeCalibInstance(m_calib);
|
|
m_calib = nullptr;
|
|
}
|
|
}
|
|
|
|
void CalibViewMainWindow::setupUI()
|
|
{
|
|
// 创建中央控件
|
|
QWidget* centralWidget = new QWidget(this);
|
|
QHBoxLayout* mainLayout = new QHBoxLayout(centralWidget);
|
|
|
|
// 创建分割器
|
|
QSplitter* splitter = new QSplitter(Qt::Horizontal, this);
|
|
|
|
// 左侧：数据输入
|
|
m_dataWidget = new CalibDataWidget(this);
|
|
splitter->addWidget(m_dataWidget);
|
|
|
|
// 右侧：结果显示
|
|
m_resultWidget = new CalibResultWidget(this);
|
|
splitter->addWidget(m_resultWidget);
|
|
|
|
// 设置分割比例
|
|
splitter->setStretchFactor(0, 1);
|
|
splitter->setStretchFactor(1, 1);
|
|
|
|
mainLayout->addWidget(splitter);
|
|
setCentralWidget(centralWidget);
|
|
|
|
// 创建状态栏
|
|
statusBar()->showMessage("就绪");
|
|
}
|
|
|
|
void CalibViewMainWindow::createMenuBar()
|
|
{
|
|
// 文件菜单
|
|
QMenu* fileMenu = menuBar()->addMenu("文件(&F)");
|
|
|
|
QAction* actLoadTest = fileMenu->addAction("加载测试数据(&T)");
|
|
connect(actLoadTest, &QAction::triggered, this, &CalibViewMainWindow::onLoadTestData);
|
|
|
|
fileMenu->addSeparator();
|
|
|
|
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* 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);
|
|
|
|
calibMenu->addSeparator();
|
|
|
|
QAction* actTransform = calibMenu->addAction("坐标变换测试(&T)");
|
|
connect(actTransform, &QAction::triggered, this, &CalibViewMainWindow::onTransformTest);
|
|
|
|
QAction* actEuler = calibMenu->addAction("欧拉角转换测试(&U)");
|
|
connect(actEuler, &QAction::triggered, this, &CalibViewMainWindow::onEulerTest);
|
|
|
|
calibMenu->addSeparator();
|
|
|
|
QAction* actClear = calibMenu->addAction("清除所有(&C)");
|
|
connect(actClear, &QAction::triggered, this, &CalibViewMainWindow::onClearAll);
|
|
|
|
// 帮助菜单
|
|
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"
|
|
"- 坐标变换\n"
|
|
"- 欧拉角转换\n\n"
|
|
"基于 Eigen 库实现的 SVD 分解算法");
|
|
});
|
|
}
|
|
|
|
void CalibViewMainWindow::createToolBar()
|
|
{
|
|
QToolBar* toolBar = addToolBar("工具栏");
|
|
toolBar->setMovable(false);
|
|
|
|
QAction* actLoadTest = toolBar->addAction("测试数据");
|
|
connect(actLoadTest, &QAction::triggered, this, &CalibViewMainWindow::onLoadTestData);
|
|
|
|
toolBar->addSeparator();
|
|
|
|
QAction* actEyeToHand = toolBar->addAction("Eye-To-Hand");
|
|
connect(actEyeToHand, &QAction::triggered, this, &CalibViewMainWindow::onEyeToHandCalib);
|
|
|
|
QAction* actEyeInHand = toolBar->addAction("Eye-In-Hand");
|
|
connect(actEyeInHand, &QAction::triggered, this, &CalibViewMainWindow::onEyeInHandCalib);
|
|
|
|
toolBar->addSeparator();
|
|
|
|
QAction* actTransform = toolBar->addAction("变换测试");
|
|
connect(actTransform, &QAction::triggered, this, &CalibViewMainWindow::onTransformTest);
|
|
|
|
QAction* actEuler = toolBar->addAction("欧拉角测试");
|
|
connect(actEuler, &QAction::triggered, this, &CalibViewMainWindow::onEulerTest);
|
|
|
|
toolBar->addSeparator();
|
|
|
|
QAction* actClear = toolBar->addAction("清除");
|
|
connect(actClear, &QAction::triggered, this, &CalibViewMainWindow::onClearAll);
|
|
}
|
|
|
|
void CalibViewMainWindow::updateStatusBar(const QString& message)
|
|
{
|
|
statusBar()->showMessage(message);
|
|
}
|
|
|
|
void CalibViewMainWindow::onEyeToHandCalib()
|
|
{
|
|
if (!m_calib) {
|
|
QMessageBox::critical(this, "错误", "标定实例未初始化");
|
|
return;
|
|
}
|
|
|
|
std::vector<HECPoint3D> eyePoints;
|
|
std::vector<HECPoint3D> robotPoints;
|
|
m_dataWidget->getEyeToHandData(eyePoints, robotPoints);
|
|
|
|
if (eyePoints.size() < 3) {
|
|
QMessageBox::warning(this, "警告", "至少需要3组对应点进行标定");
|
|
return;
|
|
}
|
|
|
|
m_resultWidget->appendLog("开始 Eye-To-Hand 标定...");
|
|
m_resultWidget->appendLog(QString("输入点数: %1").arg(eyePoints.size()));
|
|
|
|
int ret = m_calib->CalculateRT(eyePoints, robotPoints, m_currentResult);
|
|
|
|
if (ret == 0) {
|
|
m_hasResult = true;
|
|
m_resultWidget->showCalibResult(m_currentResult);
|
|
m_resultWidget->appendLog(QString("标定成功，误差: %1 mm")
|
|
.arg(m_currentResult.error, 0, 'f', 4));
|
|
updateStatusBar("Eye-To-Hand 标定完成");
|
|
emit calibrationCompleted(m_currentResult);
|
|
} else {
|
|
m_resultWidget->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;
|
|
}
|
|
|
|
m_resultWidget->appendLog("开始 Eye-In-Hand 标定...");
|
|
m_resultWidget->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);
|
|
m_resultWidget->appendLog(QString("标定成功，误差: %1 mm")
|
|
.arg(m_currentResult.error, 0, 'f', 4));
|
|
updateStatusBar("Eye-In-Hand 标定完成");
|
|
emit calibrationCompleted(m_currentResult);
|
|
} else {
|
|
m_resultWidget->appendLog(QString("标定失败，错误码: %1").arg(ret));
|
|
QMessageBox::critical(this, "错误", QString("标定失败，错误码: %1").arg(ret));
|
|
}
|
|
}
|
|
|
|
void CalibViewMainWindow::onTransformTest()
|
|
{
|
|
if (!m_calib) {
|
|
QMessageBox::critical(this, "错误", "标定实例未初始化");
|
|
return;
|
|
}
|
|
|
|
if (!m_hasResult) {
|
|
QMessageBox::warning(this, "警告", "请先执行标定或加载标定结果");
|
|
return;
|
|
}
|
|
|
|
HECPoint3D srcPoint = m_dataWidget->getTransformPoint();
|
|
HECPoint3D dstPoint;
|
|
|
|
m_calib->TransformPoint(m_currentResult.R, m_currentResult.T, srcPoint, dstPoint);
|
|
|
|
m_resultWidget->showTransformResult(srcPoint, dstPoint);
|
|
m_resultWidget->appendLog(QString("坐标变换: (%1, %2, %3) -> (%4, %5, %6)")
|
|
.arg(srcPoint.x, 0, 'f', 3)
|
|
.arg(srcPoint.y, 0, 'f', 3)
|
|
.arg(srcPoint.z, 0, 'f', 3)
|
|
.arg(dstPoint.x, 0, 'f', 3)
|
|
.arg(dstPoint.y, 0, 'f', 3)
|
|
.arg(dstPoint.z, 0, 'f', 3));
|
|
|
|
updateStatusBar("坐标变换完成");
|
|
}
|
|
|
|
void CalibViewMainWindow::onEulerTest()
|
|
{
|
|
if (!m_calib) {
|
|
QMessageBox::critical(this, "错误", "标定实例未初始化");
|
|
return;
|
|
}
|
|
|
|
HECEulerAngles inputAngles = m_dataWidget->getEulerAngles();
|
|
HECEulerOrder order = m_dataWidget->getEulerOrder();
|
|
|
|
// 欧拉角 -> 旋转矩阵
|
|
HECRotationMatrix R;
|
|
m_calib->EulerToRotationMatrix(inputAngles, order, R);
|
|
|
|
// 旋转矩阵 -> 欧拉角
|
|
HECEulerAngles outputAngles;
|
|
m_calib->RotationMatrixToEuler(R, order, outputAngles);
|
|
|
|
m_resultWidget->showEulerResult(inputAngles, R, outputAngles, order);
|
|
|
|
double inRoll, inPitch, inYaw;
|
|
inputAngles.toDegrees(inRoll, inPitch, inYaw);
|
|
double outRoll, outPitch, outYaw;
|
|
outputAngles.toDegrees(outRoll, outPitch, outYaw);
|
|
|
|
m_resultWidget->appendLog(QString("欧拉角转换测试:"));
|
|
m_resultWidget->appendLog(QString(" 输入: Roll=%1°, Pitch=%2°, Yaw=%3°")
|
|
.arg(inRoll, 0, 'f', 2).arg(inPitch, 0, 'f', 2).arg(inYaw, 0, 'f', 2));
|
|
m_resultWidget->appendLog(QString(" 输出: Roll=%1°, Pitch=%2°, Yaw=%3°")
|
|
.arg(outRoll, 0, 'f', 2).arg(outPitch, 0, 'f', 2).arg(outYaw, 0, 'f', 2));
|
|
|
|
updateStatusBar("欧拉角转换完成");
|
|
}
|
|
|
|
void CalibViewMainWindow::onClearAll()
|
|
{
|
|
m_dataWidget->clearAll();
|
|
m_resultWidget->clearAll();
|
|
m_hasResult = false;
|
|
updateStatusBar("已清除所有数据");
|
|
}
|
|
|
|
void CalibViewMainWindow::onLoadTestData()
|
|
{
|
|
m_dataWidget->loadTestData();
|
|
m_resultWidget->appendLog("已加载测试数据");
|
|
updateStatusBar("已加载测试数据");
|
|
}
|
|
|
|
void CalibViewMainWindow::onSaveResult()
|
|
{
|
|
if (!m_hasResult) {
|
|
QMessageBox::warning(this, "警告", "没有可保存的标定结果");
|
|
return;
|
|
}
|
|
|
|
QString fileName = QFileDialog::getSaveFileName(this,
|
|
"保存标定结果", "", "JSON文件 (*.json)");
|
|
|
|
if (fileName.isEmpty()) {
|
|
return;
|
|
}
|
|
|
|
QJsonObject root;
|
|
|
|
// 保存旋转矩阵
|
|
QJsonArray rotArray;
|
|
for (int i = 0; i < 9; ++i) {
|
|
rotArray.append(m_currentResult.R.data[i]);
|
|
}
|
|
root["rotation"] = rotArray;
|
|
|
|
// 保存平移向量
|
|
QJsonArray transArray;
|
|
for (int i = 0; i < 3; ++i) {
|
|
transArray.append(m_currentResult.T.data[i]);
|
|
}
|
|
root["translation"] = transArray;
|
|
|
|
// 保存质心
|
|
QJsonObject centerEye;
|
|
centerEye["x"] = m_currentResult.centerEye.x;
|
|
centerEye["y"] = m_currentResult.centerEye.y;
|
|
centerEye["z"] = m_currentResult.centerEye.z;
|
|
root["centerEye"] = centerEye;
|
|
|
|
QJsonObject centerRobot;
|
|
centerRobot["x"] = m_currentResult.centerRobot.x;
|
|
centerRobot["y"] = m_currentResult.centerRobot.y;
|
|
centerRobot["z"] = m_currentResult.centerRobot.z;
|
|
root["centerRobot"] = centerRobot;
|
|
|
|
// 保存误差
|
|
root["error"] = m_currentResult.error;
|
|
|
|
QFile file(fileName);
|
|
if (file.open(QIODevice::WriteOnly)) {
|
|
QJsonDocument doc(root);
|
|
file.write(doc.toJson(QJsonDocument::Indented));
|
|
file.close();
|
|
m_resultWidget->appendLog(QString("结果已保存到: %1").arg(fileName));
|
|
updateStatusBar("结果已保存");
|
|
} else {
|
|
QMessageBox::critical(this, "错误", "无法保存文件");
|
|
}
|
|
}
|
|
|
|
void CalibViewMainWindow::onLoadResult()
|
|
{
|
|
QString fileName = QFileDialog::getOpenFileName(this,
|
|
"加载标定结果", "", "JSON文件 (*.json)");
|
|
|
|
if (fileName.isEmpty()) {
|
|
return;
|
|
}
|
|
|
|
QFile file(fileName);
|
|
if (!file.open(QIODevice::ReadOnly)) {
|
|
QMessageBox::critical(this, "错误", "无法打开文件");
|
|
return;
|
|
}
|
|
|
|
QByteArray data = file.readAll();
|
|
file.close();
|
|
|
|
QJsonDocument doc = QJsonDocument::fromJson(data);
|
|
if (doc.isNull()) {
|
|
QMessageBox::critical(this, "错误", "无效的JSON文件");
|
|
return;
|
|
}
|
|
|
|
QJsonObject root = doc.object();
|
|
|
|
// 加载旋转矩阵
|
|
QJsonArray rotArray = root["rotation"].toArray();
|
|
if (rotArray.size() == 9) {
|
|
for (int i = 0; i < 9; ++i) {
|
|
m_currentResult.R.data[i] = rotArray[i].toDouble();
|
|
}
|
|
}
|
|
|
|
// 加载平移向量
|
|
QJsonArray transArray = root["translation"].toArray();
|
|
if (transArray.size() == 3) {
|
|
for (int i = 0; i < 3; ++i) {
|
|
m_currentResult.T.data[i] = transArray[i].toDouble();
|
|
}
|
|
}
|
|
|
|
// 加载质心
|
|
QJsonObject centerEye = root["centerEye"].toObject();
|
|
m_currentResult.centerEye.x = centerEye["x"].toDouble();
|
|
m_currentResult.centerEye.y = centerEye["y"].toDouble();
|
|
m_currentResult.centerEye.z = centerEye["z"].toDouble();
|
|
|
|
QJsonObject centerRobot = root["centerRobot"].toObject();
|
|
m_currentResult.centerRobot.x = centerRobot["x"].toDouble();
|
|
m_currentResult.centerRobot.y = centerRobot["y"].toDouble();
|
|
m_currentResult.centerRobot.z = centerRobot["z"].toDouble();
|
|
|
|
// 加载误差
|
|
m_currentResult.error = root["error"].toDouble();
|
|
|
|
m_hasResult = true;
|
|
m_resultWidget->showCalibResult(m_currentResult);
|
|
m_resultWidget->appendLog(QString("已加载标定结果: %1").arg(fileName));
|
|
updateStatusBar("标定结果已加载");
|
|
}
|