451 lines
15 KiB
C++
451 lines
15 KiB
C++
#include "dialogalgoarg.h"
|
|
#include "ui_dialogalgoarg.h"
|
|
#include "RodAndBarPositionPresenter.h"
|
|
#include "StyledMessageBox.h"
|
|
#include "HandEyeCalibWidget.h"
|
|
#include "NetworkConfigWidget.h"
|
|
#include "ToolExtrinsicWidget.h"
|
|
#include "PathManager.h"
|
|
#include "VrLog.h"
|
|
#include <algorithm>
|
|
#include <cstring>
|
|
|
|
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;
|
|
}
|
|
}
|
|
|
|
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;
|
|
}
|
|
}
|
|
|
|
} // namespace
|
|
|
|
DialogAlgoArg::DialogAlgoArg(QWidget *parent)
|
|
: QDialog(parent)
|
|
, ui(new Ui::DialogAlgoArg)
|
|
, m_presenter(nullptr)
|
|
, m_handEyeCalibWidget(nullptr)
|
|
, m_networkConfigWidget(nullptr)
|
|
, m_toolExtrinsicWidget(nullptr)
|
|
{
|
|
ui->setupUi(this);
|
|
setWindowTitle("算法参数设置");
|
|
}
|
|
|
|
DialogAlgoArg::~DialogAlgoArg()
|
|
{
|
|
delete ui;
|
|
}
|
|
|
|
void DialogAlgoArg::SetPresenter(RodAndBarPositionPresenter* presenter)
|
|
{
|
|
m_presenter = presenter;
|
|
loadParams();
|
|
|
|
// 初始化手眼标定 tab
|
|
InitHandEyeCalibTab();
|
|
|
|
// 初始化工具参数 tab
|
|
InitToolExtrinsicTab();
|
|
|
|
// 初始化网络配置 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();
|
|
|
|
int toolEulerOrder = systemConfig.configResult.eulerOrder;
|
|
double toolRotX = 0.0;
|
|
double toolRotY = 0.0;
|
|
double toolRotZ = 0.0;
|
|
double toolOffsetX = 0.0;
|
|
double toolOffsetY = 0.0;
|
|
double toolOffsetZ = 0.0;
|
|
if (!systemConfig.configResult.handEyeCalibMatrixList.empty()) {
|
|
const auto& toolConfig = systemConfig.configResult.handEyeCalibMatrixList.front();
|
|
toolEulerOrder = toolConfig.eulerOrder;
|
|
toolRotX = toolConfig.rotX;
|
|
toolRotY = toolConfig.rotY;
|
|
toolRotZ = toolConfig.rotZ;
|
|
toolOffsetX = toolConfig.offsetX;
|
|
toolOffsetY = toolConfig.offsetY;
|
|
toolOffsetZ = toolConfig.offsetZ;
|
|
}
|
|
if (m_toolExtrinsicWidget) {
|
|
m_toolExtrinsicWidget->getData(toolEulerOrder,
|
|
toolRotX, toolRotY, toolRotZ,
|
|
toolOffsetX, toolOffsetY, toolOffsetZ);
|
|
}
|
|
systemConfig.configResult.eulerOrder = toolEulerOrder;
|
|
|
|
// 网络配置保存通信字节序和姿态输入/输出顺序；方向向量/长轴不再提供配置。
|
|
if (m_networkConfigWidget) {
|
|
NetworkConfigData netConfig = m_networkConfigWidget->getConfig();
|
|
toolEulerOrder = PoseOutputOrderToEulerOrder(netConfig.poseOutputOrder);
|
|
systemConfig.configResult.eulerOrder = toolEulerOrder;
|
|
systemConfig.configResult.byteOrder = netConfig.byteOrder;
|
|
}
|
|
|
|
// 保存手眼标定矩阵
|
|
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 = toolEulerOrder;
|
|
calibMatrix.rotX = toolRotX;
|
|
calibMatrix.rotY = toolRotY;
|
|
calibMatrix.rotZ = toolRotZ;
|
|
calibMatrix.offsetX = toolOffsetX;
|
|
calibMatrix.offsetY = toolOffsetY;
|
|
calibMatrix.offsetZ = toolOffsetZ;
|
|
for (const auto& old : oldMatrixList) {
|
|
if (old.cameraIndex == camIdx) {
|
|
calibMatrix = old;
|
|
break;
|
|
}
|
|
}
|
|
calibMatrix.eulerOrder = toolEulerOrder;
|
|
calibMatrix.rotX = toolRotX;
|
|
calibMatrix.rotY = toolRotY;
|
|
calibMatrix.rotZ = toolRotZ;
|
|
calibMatrix.offsetX = toolOffsetX;
|
|
calibMatrix.offsetY = toolOffsetY;
|
|
calibMatrix.offsetZ = toolOffsetZ;
|
|
|
|
// 如果控件中有更新的数据则覆盖
|
|
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;
|
|
}
|
|
|
|
// 统一更新并保存到文件
|
|
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::InitToolExtrinsicTab()
|
|
{
|
|
if (!ui || !m_presenter) return;
|
|
|
|
m_toolExtrinsicWidget = new ToolExtrinsicWidget(this);
|
|
m_toolExtrinsicWidget->setTargetOffsetVisible(true);
|
|
ui->verticalLayout_toolExtrinsicHost->addWidget(m_toolExtrinsicWidget);
|
|
|
|
loadToolExtrinsicConfig();
|
|
}
|
|
|
|
void DialogAlgoArg::loadToolExtrinsicConfig()
|
|
{
|
|
if (!m_presenter || !m_toolExtrinsicWidget) return;
|
|
|
|
ConfigManager* configManager = m_presenter->GetConfigManager();
|
|
if (!configManager) return;
|
|
|
|
ConfigResult configResult = configManager->GetConfigResult();
|
|
int eulerOrder = configResult.eulerOrder;
|
|
double rotX = 0.0;
|
|
double rotY = 0.0;
|
|
double rotZ = 0.0;
|
|
double offsetX = 0.0;
|
|
double offsetY = 0.0;
|
|
double offsetZ = 0.0;
|
|
|
|
if (!configResult.handEyeCalibMatrixList.empty()) {
|
|
const auto& toolConfig = configResult.handEyeCalibMatrixList.front();
|
|
eulerOrder = toolConfig.eulerOrder;
|
|
rotX = toolConfig.rotX;
|
|
rotY = toolConfig.rotY;
|
|
rotZ = toolConfig.rotZ;
|
|
offsetX = toolConfig.offsetX;
|
|
offsetY = toolConfig.offsetY;
|
|
offsetZ = toolConfig.offsetZ;
|
|
}
|
|
|
|
m_toolExtrinsicWidget->setData(eulerOrder, rotX, rotY, rotZ, offsetX, offsetY, offsetZ);
|
|
}
|
|
|
|
// ========== 网络配置相关实现 ==========
|
|
|
|
void DialogAlgoArg::InitNetworkConfigTab()
|
|
{
|
|
if (!ui || !m_presenter) return;
|
|
|
|
m_networkConfigWidget = new NetworkConfigWidget(false, false, this);
|
|
ui->verticalLayout_networkConfigHost->addWidget(m_networkConfigWidget);
|
|
m_networkConfigWidget->setExtrinsicControlsVisible(false);
|
|
m_networkConfigWidget->setPoseOutputOrderVisible(true);
|
|
|
|
loadNetworkConfig();
|
|
}
|
|
|
|
void DialogAlgoArg::loadNetworkConfig()
|
|
{
|
|
if (!m_presenter || !m_networkConfigWidget) return;
|
|
|
|
ConfigManager* configManager = m_presenter->GetConfigManager();
|
|
if (!configManager) return;
|
|
|
|
ConfigResult configResult = configManager->GetConfigResult();
|
|
|
|
NetworkConfigData netConfig;
|
|
netConfig.eulerOrder = configResult.eulerOrder;
|
|
if (!configResult.handEyeCalibMatrixList.empty()) {
|
|
netConfig.eulerOrder = configResult.handEyeCalibMatrixList.front().eulerOrder;
|
|
}
|
|
netConfig.poseOutputOrder = EulerOrderToPoseOutputOrder(netConfig.eulerOrder);
|
|
netConfig.byteOrder = configResult.byteOrder;
|
|
|
|
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();
|
|
SystemConfig systemConfig = configManager->GetConfig();
|
|
const int eulerOrder = PoseOutputOrderToEulerOrder(netConfig.poseOutputOrder);
|
|
systemConfig.configResult.eulerOrder = eulerOrder;
|
|
systemConfig.configResult.byteOrder = netConfig.byteOrder;
|
|
for (auto& calibMatrix : systemConfig.configResult.handEyeCalibMatrixList) {
|
|
calibMatrix.eulerOrder = eulerOrder;
|
|
}
|
|
|
|
// 更新并保存
|
|
configManager->UpdateFullConfig(systemConfig);
|
|
|
|
QString configPath = PathManager::GetInstance().GetConfigFilePath();
|
|
configManager->SaveConfigToFile(configPath.toStdString());
|
|
}
|