425 lines
15 KiB
C++
425 lines
15 KiB
C++
#include "dialogalgoarg.h"
|
||
#include "ui_dialogalgoarg.h"
|
||
#include "RodAndBarPositionPresenter.h"
|
||
#include "StyledMessageBox.h"
|
||
#include "HandEyeCalibWidget.h"
|
||
#include "NetworkConfigWidget.h"
|
||
#include "PathManager.h"
|
||
#include "VrLog.h"
|
||
#include <cstring>
|
||
#include <QFormLayout>
|
||
#include <QGroupBox>
|
||
|
||
namespace
|
||
{
|
||
|
||
/// 姿态输入/输出顺序 → 欧拉角旋转顺序 映射
|
||
int PoseOutputOrderToEulerOrder(int poseOrder)
|
||
{
|
||
switch (poseOrder) {
|
||
case 0: return 10; // RX-RY-RZ → XYZ
|
||
case 1: return 15; // RX-RZ-RY → XZY
|
||
case 2: return 13; // RY-RX-RZ → YXZ
|
||
case 3: return 14; // RY-RZ-RX → YZX
|
||
case 4: return 12; // RZ-RX-RY → ZXY
|
||
case 5: return 11; // RZ-RY-RX → ZYX
|
||
default: return 11; // 默认 ZYX
|
||
}
|
||
}
|
||
|
||
/// 欧拉角旋转顺序 → 姿态输入/输出顺序 映射（UI 显示用）
|
||
int EulerOrderToPoseOutputOrder(int eulerOrder)
|
||
{
|
||
switch (eulerOrder) {
|
||
case 10: return 0; // XYZ → RX-RY-RZ
|
||
case 15: return 1; // XZY → RX-RZ-RY
|
||
case 13: return 2; // YXZ → RY-RX-RZ
|
||
case 14: return 3; // YZX → RY-RZ-RX
|
||
case 12: return 4; // ZXY → RZ-RX-RY
|
||
case 11: return 5; // ZYX → RZ-RY-RX
|
||
default: return 5; // 默认 RZ-RY-RX
|
||
}
|
||
}
|
||
|
||
} // namespace
|
||
|
||
DialogAlgoArg::DialogAlgoArg(QWidget *parent)
|
||
: QDialog(parent)
|
||
, ui(new Ui::DialogAlgoArg)
|
||
, m_presenter(nullptr)
|
||
, m_handEyeCalibWidget(nullptr)
|
||
, m_networkConfigWidget(nullptr)
|
||
{
|
||
ui->setupUi(this);
|
||
setWindowTitle("算法参数设置");
|
||
}
|
||
|
||
DialogAlgoArg::~DialogAlgoArg()
|
||
{
|
||
delete ui;
|
||
}
|
||
|
||
void DialogAlgoArg::SetPresenter(RodAndBarPositionPresenter* presenter)
|
||
{
|
||
m_presenter = presenter;
|
||
loadParams();
|
||
|
||
// 初始化手眼标定 tab
|
||
InitHandEyeCalibTab();
|
||
|
||
// 初始化网络配置 tab
|
||
InitNetworkConfigTab();
|
||
}
|
||
|
||
void DialogAlgoArg::loadParams()
|
||
{
|
||
if (!m_presenter) {
|
||
return;
|
||
}
|
||
|
||
auto params = m_presenter->GetAlgoParams();
|
||
|
||
// 棒材参数
|
||
ui->spinRodDiameter->setValue(params.rodParam.rodDiameter);
|
||
ui->spinRodLen->setValue(params.rodParam.rodLen);
|
||
|
||
// 角点检测参数
|
||
ui->spinCornerTh->setValue(params.cornerParam.cornerTh);
|
||
ui->spinScale->setValue(params.cornerParam.scale);
|
||
ui->spinMinEndingGap->setValue(params.cornerParam.minEndingGap);
|
||
ui->spinMinEndingGapZ->setValue(params.cornerParam.minEndingGap_z);
|
||
ui->spinJumpCornerTh1->setValue(params.cornerParam.jumpCornerTh_1);
|
||
ui->spinJumpCornerTh2->setValue(params.cornerParam.jumpCornerTh_2);
|
||
|
||
// 滤波参数
|
||
ui->spinContinuityTh->setValue(params.filterParam.continuityTh);
|
||
ui->spinOutlierTh->setValue(params.filterParam.outlierTh);
|
||
|
||
// 树生长参数
|
||
ui->spinMaxLineSkipNum->setValue(params.growParam.maxLineSkipNum);
|
||
ui->spinYDeviationMax->setValue(params.growParam.yDeviation_max);
|
||
ui->spinMaxSkipDistance->setValue(params.growParam.maxSkipDistance);
|
||
ui->spinZDeviationMax->setValue(params.growParam.zDeviation_max);
|
||
ui->spinMinLTypeTreeLen->setValue(params.growParam.minLTypeTreeLen);
|
||
ui->spinMinVTypeTreeLen->setValue(params.growParam.minVTypeTreeLen);
|
||
}
|
||
|
||
void DialogAlgoArg::saveParams()
|
||
{
|
||
if (!m_presenter) {
|
||
return;
|
||
}
|
||
|
||
auto params = m_presenter->GetAlgoParams();
|
||
|
||
// 棒材参数
|
||
params.rodParam.rodDiameter = ui->spinRodDiameter->value();
|
||
params.rodParam.rodLen = ui->spinRodLen->value();
|
||
|
||
// 角点检测参数
|
||
params.cornerParam.cornerTh = ui->spinCornerTh->value();
|
||
params.cornerParam.scale = ui->spinScale->value();
|
||
params.cornerParam.minEndingGap = ui->spinMinEndingGap->value();
|
||
params.cornerParam.minEndingGap_z = ui->spinMinEndingGapZ->value();
|
||
params.cornerParam.jumpCornerTh_1 = ui->spinJumpCornerTh1->value();
|
||
params.cornerParam.jumpCornerTh_2 = ui->spinJumpCornerTh2->value();
|
||
|
||
// 滤波参数
|
||
params.filterParam.continuityTh = ui->spinContinuityTh->value();
|
||
params.filterParam.outlierTh = ui->spinOutlierTh->value();
|
||
|
||
// 树生长参数
|
||
params.growParam.maxLineSkipNum = ui->spinMaxLineSkipNum->value();
|
||
params.growParam.yDeviation_max = ui->spinYDeviationMax->value();
|
||
params.growParam.maxSkipDistance = ui->spinMaxSkipDistance->value();
|
||
params.growParam.zDeviation_max = ui->spinZDeviationMax->value();
|
||
params.growParam.minLTypeTreeLen = ui->spinMinLTypeTreeLen->value();
|
||
params.growParam.minVTypeTreeLen = ui->spinMinVTypeTreeLen->value();
|
||
|
||
m_presenter->SetAlgoParams(params);
|
||
|
||
// 保存手眼标定矩阵 + 网络配置到配置文件（统一获取一次配置，避免互相覆盖）
|
||
if (m_presenter->GetConfigManager()) {
|
||
ConfigManager* configManager = m_presenter->GetConfigManager();
|
||
SystemConfig systemConfig = configManager->GetConfig();
|
||
NetworkConfigData netConfig;
|
||
bool hasNetworkConfig = false;
|
||
|
||
if (m_networkConfigWidget) {
|
||
netConfig = m_networkConfigWidget->getConfig();
|
||
// 欧拉角旋转顺序由姿态输入/输出顺序决定
|
||
netConfig.eulerOrder = PoseOutputOrderToEulerOrder(netConfig.poseOutputOrder);
|
||
hasNetworkConfig = true;
|
||
systemConfig.configResult.eulerOrder = netConfig.eulerOrder;
|
||
systemConfig.configResult.dirVectorInvert = netConfig.dirVectorInvert;
|
||
systemConfig.configResult.byteOrder = netConfig.byteOrder;
|
||
systemConfig.configResult.longAxisDir = netConfig.longAxisDir;
|
||
}
|
||
if (hasNetworkConfig) {
|
||
for (auto& calibMatrix : systemConfig.configResult.handEyeCalibMatrixList) {
|
||
calibMatrix.eulerOrder = netConfig.eulerOrder;
|
||
}
|
||
}
|
||
|
||
// 保存手眼标定矩阵
|
||
if (m_handEyeCalibWidget) {
|
||
const auto& oldMatrixList = systemConfig.configResult.handEyeCalibMatrixList;
|
||
std::vector<VrHandEyeCalibMatrix> newMatrixList;
|
||
const auto& cameraList = m_presenter->GetCameraList();
|
||
int cameraCount = std::max(1, static_cast<int>(cameraList.size()));
|
||
|
||
for (int i = 0; i < cameraCount; ++i) {
|
||
int camIdx = i + 1;
|
||
|
||
// 先从已有配置中获取该相机的矩阵作为基础
|
||
VrHandEyeCalibMatrix calibMatrix;
|
||
calibMatrix.cameraIndex = camIdx;
|
||
calibMatrix.eulerOrder = systemConfig.configResult.eulerOrder;
|
||
for (const auto& old : oldMatrixList) {
|
||
if (old.cameraIndex == camIdx) {
|
||
calibMatrix = old;
|
||
break;
|
||
}
|
||
}
|
||
calibMatrix.eulerOrder = systemConfig.configResult.eulerOrder;
|
||
|
||
// 如果控件中有更新的数据则覆盖
|
||
bool isCalibrated = false;
|
||
double matrix[16];
|
||
if (m_handEyeCalibWidget->getCalibData(camIdx, matrix, isCalibrated) && isCalibrated) {
|
||
memcpy(calibMatrix.matrix, matrix, sizeof(double) * 16);
|
||
}
|
||
|
||
newMatrixList.push_back(calibMatrix);
|
||
}
|
||
systemConfig.configResult.handEyeCalibMatrixList = newMatrixList;
|
||
}
|
||
|
||
// 保存网络配置
|
||
if (m_networkConfigWidget) {
|
||
NetworkConfigData netConfig = m_networkConfigWidget->getConfig();
|
||
// 欧拉角旋转顺序由姿态输入/输出顺序决定
|
||
netConfig.eulerOrder = PoseOutputOrderToEulerOrder(netConfig.poseOutputOrder);
|
||
systemConfig.configResult.eulerOrder = netConfig.eulerOrder;
|
||
systemConfig.configResult.dirVectorInvert = netConfig.dirVectorInvert;
|
||
systemConfig.configResult.byteOrder = netConfig.byteOrder;
|
||
}
|
||
|
||
// 统一更新并保存到文件
|
||
configManager->UpdateFullConfig(systemConfig);
|
||
QString configPath = PathManager::GetInstance().GetConfigFilePath();
|
||
configManager->SaveConfigToFile(configPath.toStdString());
|
||
|
||
// 通知 Presenter 重新加载手眼标定矩阵等参数
|
||
m_presenter->OnConfigChanged(systemConfig.configResult);
|
||
}
|
||
}
|
||
|
||
void DialogAlgoArg::resetParams()
|
||
{
|
||
ui->spinRodDiameter->setValue(52.0);
|
||
ui->spinRodLen->setValue(290.0);
|
||
|
||
ui->spinCornerTh->setValue(60.0);
|
||
ui->spinScale->setValue(13.0);
|
||
ui->spinMinEndingGap->setValue(20.0);
|
||
ui->spinMinEndingGapZ->setValue(5.0);
|
||
ui->spinJumpCornerTh1->setValue(15.0);
|
||
ui->spinJumpCornerTh2->setValue(60.0);
|
||
|
||
ui->spinContinuityTh->setValue(5.0);
|
||
ui->spinOutlierTh->setValue(5.0);
|
||
|
||
ui->spinMaxLineSkipNum->setValue(5);
|
||
ui->spinYDeviationMax->setValue(10.0);
|
||
ui->spinMaxSkipDistance->setValue(10.0);
|
||
ui->spinZDeviationMax->setValue(10.0);
|
||
ui->spinMinLTypeTreeLen->setValue(100.0);
|
||
ui->spinMinVTypeTreeLen->setValue(100.0);
|
||
}
|
||
|
||
void DialogAlgoArg::on_btnOK_clicked()
|
||
{
|
||
saveParams();
|
||
accept();
|
||
}
|
||
|
||
void DialogAlgoArg::on_btnCancel_clicked()
|
||
{
|
||
reject();
|
||
}
|
||
|
||
void DialogAlgoArg::on_btnApply_clicked()
|
||
{
|
||
saveParams();
|
||
StyledMessageBox::information(this, "提示", "参数已应用");
|
||
}
|
||
|
||
void DialogAlgoArg::on_btnReset_clicked()
|
||
{
|
||
auto ret = StyledMessageBox::question(this, "确认重置",
|
||
"确定要重置为默认参数吗？");
|
||
if (ret == QMessageBox::Yes) {
|
||
resetParams();
|
||
}
|
||
}
|
||
|
||
// ========== 手眼标定相关实现 ==========
|
||
|
||
void DialogAlgoArg::InitHandEyeCalibTab()
|
||
{
|
||
if (!ui || !m_presenter) return;
|
||
|
||
m_handEyeCalibWidget = new HandEyeCalibWidget(this);
|
||
m_handEyeCalibWidget->setMatrixEditable(true);
|
||
ui->verticalLayout_handEyeCalibHost->addWidget(m_handEyeCalibWidget);
|
||
|
||
m_handEyeCalibWidget->setDefaultFilePath(
|
||
PathManager::GetInstance().GetAppConfigDirectory());
|
||
|
||
const auto& cameraList = m_presenter->GetCameraList();
|
||
|
||
QVector<HandEyeCalibCameraInfo> calibCameraList;
|
||
if (cameraList.empty()) {
|
||
HandEyeCalibCameraInfo defaultCam;
|
||
defaultCam.cameraIndex = 1;
|
||
defaultCam.displayName = QString::fromUtf8("相机 1");
|
||
calibCameraList.append(defaultCam);
|
||
} else {
|
||
for (size_t i = 0; i < cameraList.size(); ++i) {
|
||
HandEyeCalibCameraInfo info;
|
||
info.cameraIndex = static_cast<int>(i + 1);
|
||
info.displayName = QString::fromStdString(cameraList[i].first);
|
||
calibCameraList.append(info);
|
||
}
|
||
}
|
||
m_handEyeCalibWidget->setCameraList(calibCameraList);
|
||
|
||
for (const auto& camInfo : calibCameraList) {
|
||
CalibMatrix calibMatrix = m_presenter->GetClibMatrix(camInfo.cameraIndex - 1);
|
||
bool hasCalibData = false;
|
||
for (int i = 0; i < 16; ++i) {
|
||
double identity = (i / 4 == i % 4) ? 1.0 : 0.0;
|
||
if (qAbs(calibMatrix.clibMatrix[i] - identity) > 1e-9) {
|
||
hasCalibData = true;
|
||
break;
|
||
}
|
||
}
|
||
m_handEyeCalibWidget->setCalibData(camInfo.cameraIndex, calibMatrix.clibMatrix, hasCalibData);
|
||
}
|
||
|
||
connect(m_handEyeCalibWidget, &HandEyeCalibWidget::calibMatrixLoaded,
|
||
this, &DialogAlgoArg::onCalibMatrixLoaded);
|
||
connect(m_handEyeCalibWidget, &HandEyeCalibWidget::saveCalibRequested,
|
||
this, &DialogAlgoArg::onSaveCalibRequested);
|
||
}
|
||
|
||
void DialogAlgoArg::onCalibMatrixLoaded(int cameraIndex, const double* matrix)
|
||
{
|
||
Q_UNUSED(cameraIndex);
|
||
Q_UNUSED(matrix);
|
||
StyledMessageBox::information(this, "提示", "已从文件导入标定矩阵");
|
||
}
|
||
|
||
void DialogAlgoArg::onSaveCalibRequested(int cameraIndex, const double* matrix)
|
||
{
|
||
Q_UNUSED(cameraIndex);
|
||
Q_UNUSED(matrix);
|
||
|
||
saveParams();
|
||
StyledMessageBox::information(this, "成功", "手眼标定参数已保存！");
|
||
}
|
||
|
||
// ========== 网络配置相关实现 ==========
|
||
|
||
void DialogAlgoArg::InitNetworkConfigTab()
|
||
{
|
||
if (!ui || !m_presenter) return;
|
||
|
||
// 创建网络配置控件：不显示PLC配置，不显示TCP配置
|
||
m_networkConfigWidget = new NetworkConfigWidget(false, false, this);
|
||
ui->verticalLayout_networkConfigHost->addWidget(m_networkConfigWidget);
|
||
|
||
// 隐藏"欧拉角旋转顺序"和"输出欧拉角顺序"控件（本项目由姿态输入/输出顺序统一控制）
|
||
{
|
||
QList<QGroupBox*> groups = m_networkConfigWidget->findChildren<QGroupBox*>();
|
||
for (QGroupBox* group : groups) {
|
||
if (group->title() == QStringLiteral("通信参数")) {
|
||
QFormLayout* form = qobject_cast<QFormLayout*>(group->layout());
|
||
if (form) {
|
||
for (int i = 0; i < form->rowCount(); ++i) {
|
||
QLayoutItem* labelItem = form->itemAt(i, QFormLayout::LabelRole);
|
||
if (labelItem && labelItem->widget()) {
|
||
QLabel* label = qobject_cast<QLabel*>(labelItem->widget());
|
||
if (label && (label->text().contains(QStringLiteral("欧拉角旋转顺序"))
|
||
|| label->text().contains(QStringLiteral("输出欧拉角顺序")))) {
|
||
label->hide();
|
||
QLayoutItem* fieldItem = form->itemAt(i, QFormLayout::FieldRole);
|
||
if (fieldItem && fieldItem->widget()) {
|
||
fieldItem->widget()->hide();
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
|
||
// 加载当前配置
|
||
loadNetworkConfig();
|
||
}
|
||
|
||
void DialogAlgoArg::loadNetworkConfig()
|
||
{
|
||
if (!m_presenter || !m_networkConfigWidget) return;
|
||
|
||
ConfigManager* configManager = m_presenter->GetConfigManager();
|
||
if (!configManager) return;
|
||
|
||
ConfigResult configResult = configManager->GetConfigResult();
|
||
|
||
NetworkConfigData netConfig;
|
||
// 欧拉角旋转顺序由姿态输入/输出顺序统一控制
|
||
// 从存储的 eulerOrder 反推 poseOutputOrder 用于UI显示
|
||
netConfig.eulerOrder = configResult.eulerOrder;
|
||
if (!configResult.handEyeCalibMatrixList.empty()) {
|
||
netConfig.eulerOrder = configResult.handEyeCalibMatrixList[0].eulerOrder;
|
||
}
|
||
netConfig.poseOutputOrder = EulerOrderToPoseOutputOrder(netConfig.eulerOrder);
|
||
netConfig.dirVectorInvert = configResult.dirVectorInvert;
|
||
netConfig.byteOrder = configResult.byteOrder;
|
||
netConfig.longAxisDir = configResult.longAxisDir;
|
||
|
||
m_networkConfigWidget->setConfig(netConfig);
|
||
}
|
||
|
||
void DialogAlgoArg::saveNetworkConfig()
|
||
{
|
||
if (!m_presenter || !m_networkConfigWidget) return;
|
||
|
||
ConfigManager* configManager = m_presenter->GetConfigManager();
|
||
if (!configManager) return;
|
||
|
||
NetworkConfigData netConfig = m_networkConfigWidget->getConfig();
|
||
// 欧拉角旋转顺序由姿态输入/输出顺序决定
|
||
netConfig.eulerOrder = PoseOutputOrderToEulerOrder(netConfig.poseOutputOrder);
|
||
|
||
// 获取当前完整配置并更新网络参数
|
||
SystemConfig systemConfig = configManager->GetConfig();
|
||
systemConfig.configResult.eulerOrder = netConfig.eulerOrder;
|
||
systemConfig.configResult.dirVectorInvert = netConfig.dirVectorInvert;
|
||
systemConfig.configResult.byteOrder = netConfig.byteOrder;
|
||
systemConfig.configResult.longAxisDir = netConfig.longAxisDir;
|
||
for (auto& calibMatrix : systemConfig.configResult.handEyeCalibMatrixList) {
|
||
calibMatrix.eulerOrder = netConfig.eulerOrder;
|
||
}
|
||
|
||
// 更新并保存
|
||
configManager->UpdateFullConfig(systemConfig);
|
||
|
||
QString configPath = PathManager::GetInstance().GetConfigFilePath();
|
||
configManager->SaveConfigToFile(configPath.toStdString());
|
||
}
|