1937 lines
87 KiB
C++
1937 lines
87 KiB
C++
// MeasurePanel.cpp
|
||
#include "MeasurePanel.h"
|
||
#include "ui_MainWindow.h"
|
||
#include "CameraPanel.h"
|
||
#include "LightPanel.h"
|
||
#include "YoloPanel.h"
|
||
#include "VisionGuidePanel.h"
|
||
#include "CoordTransformPanel.h"
|
||
#include "CameraManager.h"
|
||
#include "AppSettings.h"
|
||
|
||
#include <QCoreApplication>
|
||
#include <QElapsedTimer>
|
||
#include <QTimer>
|
||
#include <QFileInfo>
|
||
#include <QFile>
|
||
#include <QTextStream>
|
||
#include <QDateTime>
|
||
#include <QDir>
|
||
#include <QMenu>
|
||
#include <QAction>
|
||
#include <QFileDialog>
|
||
#include <QVBoxLayout>
|
||
#include <QHBoxLayout>
|
||
#include <QLabel>
|
||
#include <QPushButton>
|
||
#include <QMetaObject>
|
||
#include <QThread>
|
||
#include <QPointer>
|
||
#include <QEvent>
|
||
#include <QMouseEvent>
|
||
#include <QWheelEvent>
|
||
#include <QPainter>
|
||
#include <QFontMetrics>
|
||
#include <QRegExp>
|
||
#include <QResizeEvent>
|
||
#include <opencv2/opencv.hpp>
|
||
#include <algorithm>
|
||
#include <cmath>
|
||
|
||
// ── 中文步骤名常量 ──
|
||
#define S_CAPTURE "捕获图像"
|
||
#define S_LEFT_CAPTURE "左图原图"
|
||
#define S_RIGHT_CAPTURE "右图原图"
|
||
#define S_ROI "选择ROI"
|
||
#define S_LEFT_ROI "左图ROI"
|
||
#define S_RIGHT_ROI "右图ROI"
|
||
#define S_LEFT_ROI_POS "左图ROI位置"
|
||
#define S_RIGHT_ROI_POS "右图ROI位置"
|
||
#define S_YOLO "YOLO检测"
|
||
#define S_SEG "自动分割"
|
||
#define S_LEFT_SEG "左图自动分割"
|
||
#define S_RIGHT_SEG "右图自动分割"
|
||
#define S_POSE "位姿估计"
|
||
#define S_LEFT_POSE "左图位姿"
|
||
#define S_RIGHT_POSE "右图位姿"
|
||
#define S_FUSION "双目融合诊断"
|
||
#define S_GRASP_POSE "实际抓取点"
|
||
#define S_DEBUG "参数调试"
|
||
|
||
// ── 动态创建的右模板 UI 控件 ──
|
||
static QLabel* s_labelRightContourLibFile = nullptr;
|
||
static QPushButton* s_btnLoadRightContourLib = nullptr;
|
||
static QLabel* s_labelRightContourLibStatus = nullptr;
|
||
|
||
// “开始测量”的 10 s 是从按下按钮开始计算的全流程截止时间。
|
||
// 若左路已成功且剩余时间不足,不再启动可能耗时数秒的右路匹配。
|
||
static constexpr qint64 kMeasureDeadlineMs = 10000;
|
||
static constexpr qint64 kRightMatchReserveMs = 1500;
|
||
static constexpr qint64 kFastMeasureDeadlineMs = 5000;
|
||
static constexpr double kFastSlowLeftMs = 3000.0;
|
||
|
||
static cv::Mat qImageToGrayFast(const QImage& image)
|
||
{
|
||
if (image.isNull()) return cv::Mat();
|
||
if (image.format() == QImage::Format_Grayscale8) {
|
||
cv::Mat gray(image.height(), image.width(), CV_8UC1,
|
||
const_cast<uchar*>(image.constBits()), image.bytesPerLine());
|
||
return gray.clone();
|
||
}
|
||
if (image.format() == QImage::Format_RGB888) {
|
||
cv::Mat rgb(image.height(), image.width(), CV_8UC3,
|
||
const_cast<uchar*>(image.constBits()), image.bytesPerLine());
|
||
cv::Mat gray;
|
||
cv::cvtColor(rgb, gray, cv::COLOR_RGB2GRAY);
|
||
return gray;
|
||
}
|
||
const QImage rgbImage = image.convertToFormat(QImage::Format_RGB888);
|
||
cv::Mat rgb(rgbImage.height(), rgbImage.width(), CV_8UC3,
|
||
const_cast<uchar*>(rgbImage.constBits()), rgbImage.bytesPerLine());
|
||
cv::Mat gray;
|
||
cv::cvtColor(rgb, gray, cv::COLOR_RGB2GRAY);
|
||
return gray;
|
||
}
|
||
|
||
static QString poseLogLine(const QString& tag, const VisionPoseResult& p)
|
||
{
|
||
if (!p.valid)
|
||
return QStringLiteral(" %1 invalid: %2").arg(tag, p.errorMsg);
|
||
|
||
QString s = QStringLiteral(" %1 X=%2 Y=%3 Z=%4 mm Rx=%5 Ry=%6 Rz=%7 deg Score=%8 Time=%9ms")
|
||
.arg(tag)
|
||
.arg(p.tx * 1000.0, 0, 'f', 3)
|
||
.arg(p.ty * 1000.0, 0, 'f', 3)
|
||
.arg(p.tz * 1000.0, 0, 'f', 3)
|
||
.arg(p.rx, 0, 'f', 3)
|
||
.arg(p.ry, 0, 'f', 3)
|
||
.arg(p.rz, 0, 'f', 3)
|
||
.arg(p.score, 0, 'f', 4)
|
||
.arg(p.matchTimeMs, 0, 'f', 0);
|
||
s += QStringLiteral(" StdT=(%1,%2,%3)mm")
|
||
.arg(p.stdTx, 0, 'f', 3)
|
||
.arg(p.stdTy, 0, 'f', 3)
|
||
.arg(p.stdTz, 0, 'f', 3);
|
||
return s;
|
||
}
|
||
|
||
static QString poseShortLine(const QString& tag, const VisionPoseResult& p)
|
||
{
|
||
if (!p.valid)
|
||
return QStringLiteral("%1: invalid %2").arg(tag, p.errorMsg);
|
||
|
||
return QStringLiteral("%1: X=%2 Y=%3 Z=%4 mm Rx=%5 Ry=%6 Rz=%7 Score=%8")
|
||
.arg(tag)
|
||
.arg(p.tx * 1000.0, 0, 'f', 3)
|
||
.arg(p.ty * 1000.0, 0, 'f', 3)
|
||
.arg(p.tz * 1000.0, 0, 'f', 3)
|
||
.arg(p.rx, 0, 'f', 2)
|
||
.arg(p.ry, 0, 'f', 2)
|
||
.arg(p.rz, 0, 'f', 2)
|
||
.arg(p.score, 0, 'f', 4);
|
||
}
|
||
|
||
static QString zh(const wchar_t* text)
|
||
{
|
||
return QString::fromWCharArray(text);
|
||
}
|
||
|
||
static QFont reportFont(int pointSize, bool bold = false)
|
||
{
|
||
QFont f(QStringLiteral("Microsoft YaHei"));
|
||
f.setPointSize(pointSize);
|
||
f.setBold(bold);
|
||
return f;
|
||
}
|
||
|
||
static void drawTextBox(QPainter& p, const QRect& rect, const QStringList& lines,
|
||
const QColor& border, const QColor& titleColor)
|
||
{
|
||
p.save();
|
||
p.setRenderHint(QPainter::Antialiasing, true);
|
||
p.setPen(Qt::NoPen);
|
||
p.setBrush(QColor(255, 255, 255, 238));
|
||
p.drawRoundedRect(rect, 8, 8);
|
||
p.setPen(QPen(border, 2));
|
||
p.setBrush(Qt::NoBrush);
|
||
p.drawRoundedRect(rect, 8, 8);
|
||
|
||
QFont f = reportFont(14);
|
||
p.setFont(f);
|
||
QFontMetrics fm(f);
|
||
int y = rect.top() + 28;
|
||
for (int i = 0; i < lines.size(); ++i) {
|
||
p.setPen(i == 0 ? titleColor : QColor(20, 29, 43));
|
||
p.drawText(rect.left() + 18, y, lines[i]);
|
||
y += fm.height() + 6;
|
||
}
|
||
p.restore();
|
||
}
|
||
|
||
static QImage drawRoiOverlayImage(const QImage& src, const QRect& roi, const QString& label)
|
||
{
|
||
if (src.isNull())
|
||
return QImage();
|
||
|
||
QImage out = src.convertToFormat(QImage::Format_ARGB32);
|
||
QPainter p(&out);
|
||
p.setRenderHint(QPainter::Antialiasing, true);
|
||
const int penW = qMax(3, out.width() / 1500);
|
||
p.setPen(QPen(QColor(255, 204, 0), penW));
|
||
p.setBrush(Qt::NoBrush);
|
||
p.drawRect(roi.adjusted(0, 0, -1, -1));
|
||
|
||
QFont f = p.font();
|
||
f.setPointSize(qMax(18, out.width() / 170));
|
||
f.setBold(true);
|
||
p.setFont(f);
|
||
const QFontMetrics fm(f);
|
||
QRect textRect = fm.boundingRect(label).adjusted(-14, -10, 14, 10);
|
||
textRect.moveTopLeft(QPoint(roi.left(), qMax(0, roi.top() - textRect.height() - 8)));
|
||
p.fillRect(textRect, QColor(10, 18, 28, 185));
|
||
p.setPen(QColor(255, 230, 120));
|
||
p.drawText(textRect, Qt::AlignCenter, label);
|
||
return out;
|
||
}
|
||
|
||
static QImage drawPoseOverlayImage(VisionGuideMatcher* matcher, const QImage& src, const VisionPoseResult& pose)
|
||
{
|
||
if (!matcher || src.isNull() || !pose.valid)
|
||
return QImage();
|
||
cv::Mat vis = VisionGuideMatcher::qImageToMat(src);
|
||
if (vis.empty())
|
||
return QImage();
|
||
matcher->drawPoseResult(vis, pose);
|
||
return VisionGuideMatcher::matToQImage(vis);
|
||
}
|
||
|
||
static QImage makeFusionDiagnosticImage(const VisionPoseResult& leftPose,
|
||
const VisionPoseResult& rightPose,
|
||
const StereoPoseFusion::FusionResult& fusion,
|
||
const VisionPoseResult& finalPose,
|
||
qint64 totalMs)
|
||
{
|
||
QImage img(1600, 900, QImage::Format_ARGB32);
|
||
img.fill(QColor(248, 250, 252));
|
||
|
||
QPainter p(&img);
|
||
p.setRenderHint(QPainter::Antialiasing, true);
|
||
|
||
QFont titleFont = reportFont(30, true);
|
||
p.setFont(titleFont);
|
||
p.setPen(QColor(18, 28, 42));
|
||
p.drawText(QRect(56, 36, 1488, 58), Qt::AlignLeft | Qt::AlignVCenter,
|
||
zh(L"\x53cc\x76ee\x878d\x5408\x8bca\x65ad"));
|
||
|
||
QFont subFont = reportFont(15);
|
||
p.setFont(subFont);
|
||
p.setPen(QColor(86, 98, 115));
|
||
p.drawText(QRect(58, 92, 1200, 34), Qt::AlignLeft | Qt::AlignVCenter,
|
||
QDateTime::currentDateTime().toString(QStringLiteral("yyyy-MM-dd HH:mm:ss.zzz")));
|
||
|
||
drawTextBox(p, QRect(60, 150, 700, 210),
|
||
QStringList()
|
||
<< zh(L"\x5de6\x76f8\x673a\x539f\x59cb\x4f4d\x59ff")
|
||
<< poseShortLine(QStringLiteral("LEFT"), leftPose)
|
||
<< QStringLiteral("StdT=(%1,%2,%3) mm")
|
||
.arg(leftPose.stdTx, 0, 'f', 3)
|
||
.arg(leftPose.stdTy, 0, 'f', 3)
|
||
.arg(leftPose.stdTz, 0, 'f', 3)
|
||
<< (zh(L"\x8017\x65f6") + QStringLiteral("=%1 ms").arg(leftPose.matchTimeMs, 0, 'f', 0)),
|
||
QColor(60, 132, 246), QColor(37, 99, 235));
|
||
|
||
drawTextBox(p, QRect(840, 150, 700, 210),
|
||
QStringList()
|
||
<< zh(L"\x53f3\x76f8\x673a\x539f\x59cb\x4f4d\x59ff")
|
||
<< poseShortLine(QStringLiteral("RIGHT"), rightPose)
|
||
<< QStringLiteral("StdT=(%1,%2,%3) mm")
|
||
.arg(rightPose.stdTx, 0, 'f', 3)
|
||
.arg(rightPose.stdTy, 0, 'f', 3)
|
||
.arg(rightPose.stdTz, 0, 'f', 3)
|
||
<< (zh(L"\x8017\x65f6") + QStringLiteral("=%1 ms").arg(rightPose.matchTimeMs, 0, 'f', 0)),
|
||
QColor(16, 185, 129), QColor(5, 150, 105));
|
||
|
||
const QColor statusColor = fusion.consistent ? QColor(22, 163, 74) : QColor(220, 38, 38);
|
||
drawTextBox(p, QRect(60, 410, 700, 260),
|
||
QStringList()
|
||
<< zh(L"\x53f3\x76f8\x673a\x8f6c\x6362\x5230\x5de6\x76f8\x673a\x5750\x6807\x7cfb")
|
||
<< poseShortLine(QStringLiteral("RIGHT_IN_LEFT"), fusion.rightInLeft)
|
||
<< (zh(L"\x5de6\x53f3\x4f4d\x7f6e\x5dee") + QStringLiteral(" dt=%1 mm").arg(fusion.positionDisparityMm, 0, 'f', 3))
|
||
<< (zh(L"\x5de6\x53f3\x89d2\x5ea6\x5dee") + QStringLiteral(" daxis=%1 deg").arg(fusion.orientationDisparityDeg, 0, 'f', 3))
|
||
<< (zh(L"\x4e00\x81f4\x6027") + QStringLiteral("=%1").arg(fusion.consistent ? zh(L"\x901a\x8fc7") : zh(L"\x672a\x901a\x8fc7"))),
|
||
statusColor, statusColor);
|
||
|
||
drawTextBox(p, QRect(840, 410, 700, 260),
|
||
QStringList()
|
||
<< zh(L"\x6700\x7ec8\x8f93\x51fa")
|
||
<< poseShortLine(QStringLiteral("FINAL"), finalPose)
|
||
<< (zh(L"\x878d\x5408\x6743\x91cd") + QStringLiteral(" L=%1 R=%2")
|
||
.arg(fusion.weightLeft, 0, 'f', 3)
|
||
.arg(fusion.weightRight, 0, 'f', 3))
|
||
<< (zh(L"\x7f6e\x4fe1\x5ea6") + QStringLiteral("=%1 ").arg(fusion.confidence, 0, 'f', 3) +
|
||
zh(L"\x603b\x8017\x65f6") + QStringLiteral("=%1 ms")
|
||
.arg(totalMs)),
|
||
QColor(109, 40, 217), QColor(91, 33, 182));
|
||
|
||
QRect bar(100, 740, 1400, 34);
|
||
p.setPen(Qt::NoPen);
|
||
p.setBrush(QColor(226, 232, 240));
|
||
p.drawRoundedRect(bar, 17, 17);
|
||
const double ratio = qBound(0.0, fusion.positionDisparityMm / 5.0, 1.0);
|
||
QRect fill = bar;
|
||
fill.setWidth(qMax(18, int(bar.width() * ratio)));
|
||
p.setBrush(statusColor);
|
||
p.drawRoundedRect(fill, 17, 17);
|
||
p.setPen(QColor(18, 28, 42));
|
||
QFont labelFont = reportFont(17, true);
|
||
p.setFont(labelFont);
|
||
p.drawText(QRect(100, 790, 1400, 40), Qt::AlignCenter,
|
||
zh(L"\x878d\x5408\x95e8\x69db\x003a\x0020\x0064\x0074\x0020\x003c\x003d\x0020\x0035\x002e\x0030\x0020\x006d\x006d\x002c\x0020\x0064\x0061\x0078\x0069\x0073\x0020\x003c\x003d\x0020\x0031\x0030\x002e\x0030\x0020\x0064\x0065\x0067\x3002\x5f53\x524d\x0020\x0064\x0074\x003d") +
|
||
QString::number(fusion.positionDisparityMm, 'f', 3) +
|
||
zh(L"\x0020\x006d\x006d\x002c\x0020\x0064\x0061\x0078\x0069\x0073\x003d") +
|
||
QString::number(fusion.orientationDisparityDeg, 'f', 3) +
|
||
zh(L"\x0020\x0064\x0065\x0067\xff0c") +
|
||
(fusion.consistent ? zh(L"\x5141\x8bb8\x878d\x5408") : zh(L"\x62d2\x7edd\x878d\x5408\xff0c\x91c7\x7528\x53ef\x4fe1\x5355\x4fa7\x7ed3\x679c")));
|
||
return img;
|
||
}
|
||
|
||
MeasurePanel::MeasurePanel(Ui::MainWindowClass& u,
|
||
CameraPanel* cam, LightPanel* light, YoloPanel* yolo,
|
||
VisionGuidePanel* VisionGuide, CoordTransformPanel* coord,
|
||
VisionGuideMatcher* halM, StereoPoseFusion* fusion, QObject* parent)
|
||
: QObject(parent), ui(u),
|
||
m_cam(cam), m_light(light), m_yolo(yolo), m_visionGuide(VisionGuide),
|
||
m_coord(coord), m_visionMatcher(halM), m_fusion(fusion)
|
||
{
|
||
// ── 数据管理 ──
|
||
connect(ui.btnLoadLeftCalFile, &QPushButton::clicked, this, &MeasurePanel::onLoadLeftCal);
|
||
connect(ui.btnLoadRightCalFile, &QPushButton::clicked, this, &MeasurePanel::onLoadRightCal);
|
||
connect(ui.btnLoadStereoCal, &QPushButton::clicked, this, &MeasurePanel::onLoadStereoCal);
|
||
|
||
// 左模板 — 使用原有按钮
|
||
ui.btnLoadContourLib->setText(QString::fromUtf8("加载左模板..."));
|
||
ui.labelContourLibFile->setText(QString::fromUtf8("左: 未加载"));
|
||
connect(ui.btnLoadContourLib, &QPushButton::clicked, this, &MeasurePanel::onLoadShapeModelLeft);
|
||
|
||
// 右模板 — 动态添加控件到 groupContourLib 的 layout
|
||
{
|
||
auto* layout = qobject_cast<QVBoxLayout*>(ui.groupContourLib->layout());
|
||
if (layout) {
|
||
auto* rightRow = new QHBoxLayout;
|
||
s_labelRightContourLibFile = new QLabel(QString::fromUtf8("右: 未加载"));
|
||
s_btnLoadRightContourLib = new QPushButton(QString::fromUtf8("加载右模板..."));
|
||
rightRow->addWidget(s_labelRightContourLibFile);
|
||
rightRow->addWidget(s_btnLoadRightContourLib);
|
||
layout->insertLayout(layout->count() - 1, rightRow);
|
||
|
||
s_labelRightContourLibStatus = new QLabel(QString::fromUtf8("状态: 未加载"));
|
||
s_labelRightContourLibStatus->setStyleSheet("color: gray; font-size: 11px;");
|
||
layout->insertWidget(layout->count() - 1, s_labelRightContourLibStatus);
|
||
|
||
connect(s_btnLoadRightContourLib, &QPushButton::clicked,
|
||
this, &MeasurePanel::onLoadShapeModelRight);
|
||
}
|
||
}
|
||
|
||
ui.groupContourLib->setTitle(QString::fromUtf8("3D形状模板 (.sm3) — 左右分开"));
|
||
|
||
connect(ui.btnCapture, &QPushButton::clicked, this, &MeasurePanel::onCapture);
|
||
connect(ui.btnStartMeasure, &QPushButton::clicked, this, &MeasurePanel::onStartMeasure);
|
||
connect(m_coord, &CoordTransformPanel::poseDisplayRequested,
|
||
this, &MeasurePanel::onPoseDisplayRequested);
|
||
connect(ui.comboResultView, QOverload<int>::of(&QComboBox::currentIndexChanged),
|
||
this, &MeasurePanel::onResultViewChanged);
|
||
connect(ui.btnBrowseExportDir, &QPushButton::clicked, this, &MeasurePanel::onBrowseExportDir);
|
||
connect(ui.btnExportPoseTxt, &QPushButton::clicked, this, &MeasurePanel::onExportPoseCamera);
|
||
connect(ui.btnExportPoseCsv, &QPushButton::clicked, this, &MeasurePanel::onExportPoseRobot);
|
||
{
|
||
m_btnExportResultImages = new QPushButton(QString::fromUtf8("导出结果图(JPG)"), ui.groupExportPose);
|
||
m_btnExportResultImages->setEnabled(false);
|
||
if (auto* exportLayout = qobject_cast<QVBoxLayout*>(ui.groupExportPose->layout())) {
|
||
exportLayout->insertWidget(1, m_btnExportResultImages);
|
||
}
|
||
connect(m_btnExportResultImages, &QPushButton::clicked,
|
||
this, &MeasurePanel::onExportResultImages);
|
||
}
|
||
ui.labelROIResult->installEventFilter(this);
|
||
ui.labelROIResult->setMouseTracking(true);
|
||
ui.labelROIResult->setContextMenuPolicy(Qt::CustomContextMenu);
|
||
connect(ui.labelROIResult, &QWidget::customContextMenuRequested,
|
||
this, &MeasurePanel::onROIContextMenu);
|
||
|
||
// YOLO ROI 变化 → 同步到 VisionGuidePanel
|
||
connect(m_yolo, &YoloPanel::roiChanged, this, [this](const QRect& r){
|
||
m_visionGuide->setROI(r);
|
||
m_visionGuide->updateStepButtons();
|
||
if (r.isValid() && !m_capturedFrame.isNull()) {
|
||
storeResultImage(S_ROI, m_capturedFrame.copy(r));
|
||
ui.comboResultView->setCurrentText(S_ROI);
|
||
showCurrentResultImage();
|
||
}
|
||
});
|
||
connect(m_visionGuide, &VisionGuidePanel::resultImage, this, [this](const QString& n, const QImage& img){
|
||
storeResultImage(n, img);
|
||
ui.comboResultView->setCurrentText(n);
|
||
showCurrentResultImage();
|
||
});
|
||
connect(m_visionGuide, &VisionGuidePanel::poseReady, this, [this](const VisionPoseResult& p){
|
||
m_lastPose = p;
|
||
m_displayPose = p;
|
||
rememberPoseOverlaySource(m_capturedFrame, p, m_capturedSide);
|
||
displayPoseResult(p);
|
||
m_coord->setCameraPose(p);
|
||
});
|
||
|
||
// 初始化
|
||
ui.comboResultView->clear();
|
||
ui.comboResultView->addItem(S_CAPTURE);
|
||
ui.comboResultView->addItem(S_ROI);
|
||
|
||
m_exportDir = AppSettings::instance().get(AppSettings::K_ExportDir,
|
||
QCoreApplication::applicationDirPath() + "/export");
|
||
ui.editExportDir->setText(m_exportDir);
|
||
|
||
tryLoadDefaultStereoCalib();
|
||
|
||
// ★ 延迟加载轮廓库 (窗口先显示)
|
||
QTimer::singleShot(0, this, &MeasurePanel::autoLoadShapeModels);
|
||
QTimer::singleShot(1, this, [this]{ updateActionButtons(); });
|
||
updateActionButtons();
|
||
}
|
||
|
||
MeasurePanel::~MeasurePanel()
|
||
{
|
||
if (m_measureThread) {
|
||
m_measureThread->quit();
|
||
m_measureThread->wait(3000);
|
||
}
|
||
}
|
||
|
||
// ── 自动加载轮廓库 ──────────────────────────────────────────
|
||
void MeasurePanel::updateActionButtons()
|
||
{
|
||
const bool leftConnected = m_cam && m_cam->leftConnected();
|
||
const bool rightConnected = m_cam && m_cam->rightConnected();
|
||
const bool leftReady = leftConnected && m_visionMatcher && m_visionMatcher->isReady(CAM_LEFT);
|
||
const bool rightReady = rightConnected && m_visionMatcher && m_visionMatcher->isReady(CAM_RIGHT);
|
||
|
||
ui.btnCapture->setEnabled(!m_measureBusy && (leftConnected || rightConnected));
|
||
ui.btnStartMeasure->setEnabled(!m_measureBusy && (leftReady || rightReady));
|
||
if (m_btnExportResultImages)
|
||
m_btnExportResultImages->setEnabled(!m_measureBusy && m_resultImagesReadyForExport && !m_resultImages.isEmpty());
|
||
}
|
||
|
||
bool MeasurePanel::beginExternalMeasurement(const MeasurementCaptureSnapshot& snapshot)
|
||
{
|
||
if (m_measureBusy) return false;
|
||
++m_measureRunId; // Invalidate timers belonging to completed legacy measurements.
|
||
m_measureTimedOut = false;
|
||
m_measurePartialLeft = VisionPoseResult();
|
||
m_measurePartialRight = VisionPoseResult();
|
||
m_measureBusy = true;
|
||
m_lastPose = VisionPoseResult();
|
||
m_captureSnapshot = snapshot;
|
||
m_captureSnapshot.valid = false;
|
||
m_lastFusion = StereoPoseFusion::FusionResult();
|
||
m_poseOverlayBaseImage = QImage();
|
||
m_poseOverlayCenterPose = VisionPoseResult();
|
||
m_coord->setCameraPose(VisionPoseResult());
|
||
m_resultImages.clear();
|
||
m_resultImagesReadyForExport = false;
|
||
updateActionButtons();
|
||
emit measurementCaptureStarted();
|
||
return true;
|
||
}
|
||
|
||
void MeasurePanel::acceptExternalMeasurement(const MeasurementCaptureSnapshot& snapshot)
|
||
{
|
||
m_captureSnapshot = snapshot;
|
||
m_lastPose = snapshot.finalPose;
|
||
m_lastFusion = snapshot.fusion;
|
||
// The new tab computes robot targets with its verified calibration rule.
|
||
// Do not populate the legacy robot conversion with a differently-defined ABC.
|
||
if (snapshot.valid) displayPoseResult(snapshot.finalPose);
|
||
}
|
||
|
||
void MeasurePanel::endExternalMeasurement()
|
||
{
|
||
m_measureBusy = false;
|
||
updateActionButtons();
|
||
}
|
||
|
||
|
||
void MeasurePanel::autoLoadShapeModels()
|
||
{
|
||
auto& s = AppSettings::instance();
|
||
|
||
// 左模板
|
||
QString sm3Left = s.get(AppSettings::K_ShapeModelLeft, "");
|
||
if (sm3Left.isEmpty()) sm3Left = s.get(AppSettings::K_ShapeModel, ""); // 兼容旧版
|
||
if (!sm3Left.isEmpty() && QFile::exists(sm3Left)) {
|
||
if (m_visionMatcher->loadShapeModel(sm3Left, CAM_LEFT)) {
|
||
m_shapeModelFileLeft = sm3Left;
|
||
ui.labelContourLibFile->setText(QString::fromUtf8("左: %1").arg(QFileInfo(sm3Left).fileName()));
|
||
ui.labelContourLibStatus->setText(QString::fromUtf8("左模板已加载"));
|
||
ui.labelContourLibStatus->setStyleSheet("color:#00cc66;");
|
||
}
|
||
}
|
||
|
||
// 右模板
|
||
QString sm3Right = s.get(AppSettings::K_ShapeModelRight, "");
|
||
if (!sm3Right.isEmpty() && QFile::exists(sm3Right)) {
|
||
if (m_visionMatcher->loadShapeModel(sm3Right, CAM_RIGHT)) {
|
||
m_shapeModelFileRight = sm3Right;
|
||
if (s_labelRightContourLibFile)
|
||
s_labelRightContourLibFile->setText(QString::fromUtf8("右: %1").arg(QFileInfo(sm3Right).fileName()));
|
||
if (s_labelRightContourLibStatus) {
|
||
s_labelRightContourLibStatus->setText(QString::fromUtf8("右模板已加载: %1").arg(QFileInfo(sm3Right).fileName()));
|
||
s_labelRightContourLibStatus->setStyleSheet("color:#00cc66;");
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
// ── 默认路径 ─────────────────────────────────────────────────
|
||
QString MeasurePanel::defaultStereoCalibDir() {
|
||
QString d = QCoreApplication::applicationDirPath() + "/StereoCalib";
|
||
return QDir(d).exists() ? d : "D:/Qt/firstdemo/firstdemo/StereoCalib";
|
||
}
|
||
QString MeasurePanel::defaultLeftCalFile() { return defaultStereoCalibDir() + "/cam_left.cal"; }
|
||
QString MeasurePanel::defaultRightCalFile() { return defaultStereoCalibDir() + "/cam_right.cal"; }
|
||
QString MeasurePanel::defaultRelPoseFile() { return defaultStereoCalibDir() + "/pos_right2left.dat"; }
|
||
|
||
void MeasurePanel::tryLoadDefaultStereoCalib()
|
||
{
|
||
auto& s = AppSettings::instance();
|
||
QString lc = s.get(AppSettings::K_LeftCal, defaultLeftCalFile());
|
||
QString rc = s.get(AppSettings::K_RightCal, defaultRightCalFile());
|
||
QString rp = s.get(AppSettings::K_StereoCalDir, defaultRelPoseFile());
|
||
applyStereoCalibFiles(lc, rc, rp);
|
||
}
|
||
|
||
void MeasurePanel::applyStereoCalibFiles(const QString& l, const QString& r, const QString& rel)
|
||
{
|
||
auto& s = AppSettings::instance();
|
||
if (!l.isEmpty() && QFile::exists(l)) {
|
||
if (m_visionMatcher->loadCameraParams(l, CAM_LEFT)) {
|
||
m_leftCalFile = l;
|
||
ui.labelLeftCalFile->setText(QFileInfo(l).fileName());
|
||
s.set(AppSettings::K_LeftCal, l);
|
||
}
|
||
}
|
||
if (!r.isEmpty() && QFile::exists(r)) {
|
||
if (m_visionMatcher->loadCameraParams(r, CAM_RIGHT)) {
|
||
m_rightCalFile = r;
|
||
ui.labelRightCalFile->setText(QFileInfo(r).fileName());
|
||
s.set(AppSettings::K_RightCal, r);
|
||
}
|
||
}
|
||
if (!rel.isEmpty() && QFile::exists(rel)) {
|
||
m_stereoCalFile = rel;
|
||
if (ui.labelStereoCalFile)
|
||
ui.labelStereoCalFile->setText(QFileInfo(rel).fileName());
|
||
if (m_fusion) m_fusion->loadRightToLeftPose(rel);
|
||
s.set(AppSettings::K_StereoCalDir, rel);
|
||
}
|
||
}
|
||
|
||
// ── 数据管理 slots ───────────────────────────────────────────
|
||
void MeasurePanel::onLoadLeftCal() {
|
||
QString p = AppSettings::instance().openFile(
|
||
qobject_cast<QWidget*>(parent()), AppSettings::K_LeftCal,
|
||
QString::fromUtf8("左相机标定文件"), "VisionGuide Cal (*.cal);;All (*)",
|
||
defaultLeftCalFile());
|
||
if (p.isEmpty()) return;
|
||
m_leftCalFile = p;
|
||
ui.labelLeftCalFile->setText(QFileInfo(p).fileName());
|
||
if (m_visionMatcher->loadCameraParams(p, CAM_LEFT))
|
||
emit logMessage(QString::fromUtf8("左相机标定参数加载: ") + p);
|
||
m_visionGuide->updateStepButtons();
|
||
updateActionButtons();
|
||
}
|
||
|
||
void MeasurePanel::onLoadRightCal() {
|
||
QString p = AppSettings::instance().openFile(
|
||
qobject_cast<QWidget*>(parent()), AppSettings::K_RightCal,
|
||
QString::fromUtf8("右相机标定文件"), "VisionGuide Cal (*.cal);;All (*)",
|
||
defaultRightCalFile());
|
||
if (p.isEmpty()) return;
|
||
m_rightCalFile = p;
|
||
ui.labelRightCalFile->setText(QFileInfo(p).fileName());
|
||
if (m_visionMatcher->loadCameraParams(p, CAM_RIGHT))
|
||
emit logMessage(QString::fromUtf8("右相机标定参数加载: ") + p);
|
||
m_visionGuide->updateStepButtons();
|
||
updateActionButtons();
|
||
}
|
||
|
||
void MeasurePanel::onLoadStereoCal() {
|
||
QString p = AppSettings::instance().openFile(
|
||
qobject_cast<QWidget*>(parent()), AppSettings::K_StereoCalDir,
|
||
QString::fromUtf8("导入双目标定结果"),
|
||
"VisionGuide Cal/Pose (*.cal *.dat);;All (*)", defaultRelPoseFile());
|
||
if (p.isEmpty()) return;
|
||
QString dir = QFileInfo(p).absolutePath();
|
||
applyStereoCalibFiles(dir+"/cam_left.cal", dir+"/cam_right.cal", dir+"/pos_right2left.dat");
|
||
emit logMessage(QString::fromUtf8("双目标定结果已导入: ") + dir);
|
||
m_visionGuide->updateStepButtons();
|
||
updateActionButtons();
|
||
}
|
||
|
||
void MeasurePanel::onLoadShapeModelLeft() {
|
||
QString p = AppSettings::instance().openFile(
|
||
qobject_cast<QWidget*>(parent()), AppSettings::K_ShapeModelLeft,
|
||
QString::fromUtf8("加载左相机3D形状模板"), "VisionGuide SM3 (*.sm3);;All (*)");
|
||
if (p.isEmpty()) return;
|
||
m_shapeModelFileLeft = p;
|
||
if (m_visionMatcher->loadShapeModel(p, CAM_LEFT)) {
|
||
ui.labelContourLibFile->setText(QString::fromUtf8("左: %1").arg(QFileInfo(p).fileName()));
|
||
ui.labelContourLibStatus->setText(QString::fromUtf8("左模板已加载: %1").arg(QFileInfo(p).fileName()));
|
||
ui.labelContourLibStatus->setStyleSheet("color:#00cc66;");
|
||
} else {
|
||
ui.labelContourLibStatus->setText(QString::fromUtf8("左模板加载失败"));
|
||
ui.labelContourLibStatus->setStyleSheet("color:red;");
|
||
}
|
||
m_visionGuide->updateStepButtons();
|
||
updateActionButtons();
|
||
}
|
||
|
||
void MeasurePanel::onLoadShapeModelRight() {
|
||
QString p = AppSettings::instance().openFile(
|
||
qobject_cast<QWidget*>(parent()), AppSettings::K_ShapeModelRight,
|
||
QString::fromUtf8("加载右相机3D形状模板"), "VisionGuide SM3 (*.sm3);;All (*)");
|
||
if (p.isEmpty()) return;
|
||
m_shapeModelFileRight = p;
|
||
if (m_visionMatcher->loadShapeModel(p, CAM_RIGHT)) {
|
||
if (s_labelRightContourLibFile)
|
||
s_labelRightContourLibFile->setText(QString::fromUtf8("右: %1").arg(QFileInfo(p).fileName()));
|
||
if (s_labelRightContourLibStatus) {
|
||
s_labelRightContourLibStatus->setText(QString::fromUtf8("右模板已加载: %1").arg(QFileInfo(p).fileName()));
|
||
s_labelRightContourLibStatus->setStyleSheet("color:#00cc66;");
|
||
}
|
||
} else {
|
||
if (s_labelRightContourLibStatus) {
|
||
s_labelRightContourLibStatus->setText(QString::fromUtf8("右模板加载失败"));
|
||
s_labelRightContourLibStatus->setStyleSheet("color:red;");
|
||
}
|
||
}
|
||
m_visionGuide->updateStepButtons();
|
||
updateActionButtons();
|
||
}
|
||
|
||
// ── 采集 ─────────────────────────────────────────────────────
|
||
void MeasurePanel::onCapture()
|
||
{
|
||
emit logMessage(QString::fromUtf8("正在捕获图像..."));
|
||
CameraSide captureSide = CAM_LEFT;
|
||
m_capturedFrame = m_cam->captureLeft();
|
||
if (m_capturedFrame.isNull()) {
|
||
m_capturedFrame = m_cam->captureRight();
|
||
captureSide = CAM_RIGHT;
|
||
}
|
||
if (m_capturedFrame.isNull()) { emit logMessage(QString::fromUtf8("捕获失败")); return; }
|
||
m_capturedSide = captureSide;
|
||
|
||
emit logMessage(QString::fromUtf8("捕获成功: %1x%2")
|
||
.arg(m_capturedFrame.width()).arg(m_capturedFrame.height()));
|
||
storeResultImage(S_CAPTURE, m_capturedFrame);
|
||
ui.comboResultView->setCurrentIndex(0);
|
||
showCurrentResultImage();
|
||
|
||
m_yolo->setCapturedFrame(m_capturedFrame);
|
||
m_yolo->clearState();
|
||
m_visionGuide->setFrame(m_capturedFrame, captureSide);
|
||
m_visionGuide->setROI(QRect());
|
||
m_visionGuide->updateStepButtons();
|
||
updateActionButtons();
|
||
|
||
m_lastPose = VisionPoseResult();
|
||
m_displayPose = VisionPoseResult();
|
||
m_lastFusion = StereoPoseFusion::FusionResult();
|
||
m_poseOverlayBaseImage = QImage();
|
||
m_poseOverlayCenterPose = VisionPoseResult();
|
||
m_coord->setCameraPose(VisionPoseResult());
|
||
m_resultImagesReadyForExport = false;
|
||
m_resultImages.remove(S_YOLO); m_resultImages.remove(S_ROI);
|
||
m_resultImages.remove(S_LEFT_CAPTURE); m_resultImages.remove(S_RIGHT_CAPTURE);
|
||
m_resultImages.remove(S_LEFT_ROI); m_resultImages.remove(S_RIGHT_ROI);
|
||
m_resultImages.remove(S_LEFT_ROI_POS); m_resultImages.remove(S_RIGHT_ROI_POS);
|
||
m_resultImages.remove(S_LEFT_SEG); m_resultImages.remove(S_RIGHT_SEG);
|
||
m_resultImages.remove(S_LEFT_POSE); m_resultImages.remove(S_RIGHT_POSE);
|
||
m_resultImages.remove(S_FUSION);
|
||
m_resultImages.remove(S_SEG); m_resultImages.remove(S_POSE);
|
||
m_resultImages.remove(S_GRASP_POSE);
|
||
m_resultImages.remove(S_DEBUG);
|
||
ui.editResultX->clear(); ui.editResultY->clear(); ui.editResultZ->clear();
|
||
ui.editResultAxis->clear(); ui.editResultClass->clear(); ui.editResultTime->clear();
|
||
ui.groupResult->setTitle(QString::fromUtf8("位姿估计结果"));
|
||
ui.textAlgoResult->clear(); ui.progressAlgoStep->setValue(0);
|
||
}
|
||
|
||
// ── 全流程测量 ───────────────────────────────────────────────
|
||
void MeasurePanel::onStartMeasure()
|
||
{
|
||
{
|
||
if (m_measureBusy) {
|
||
emit logMessage(QStringLiteral("[Measure] Previous measurement is still running; request ignored."));
|
||
emit measureCompleted(false);
|
||
return;
|
||
}
|
||
|
||
const bool leftReady = m_cam && m_cam->leftConnected() && m_visionMatcher && m_visionMatcher->isReady(CAM_LEFT);
|
||
m_captureSnapshot.valid = false;
|
||
const bool rightReady = m_cam && m_cam->rightConnected() && m_visionMatcher && m_visionMatcher->isReady(CAM_RIGHT);
|
||
const bool dualMode = leftReady && rightReady && m_fusion && m_fusion->isReady();
|
||
|
||
// “开始测量”前再次读取“位姿估计”页参数,保证自动分割开关即时生效。
|
||
if (m_visionGuide)
|
||
m_visionGuide->applyCurrentParameters();
|
||
|
||
emit logMessage(dualMode
|
||
? QStringLiteral("=== Start stereo measurement ===")
|
||
: QStringLiteral("=== Start single-camera measurement ==="));
|
||
emit logMessage(m_visionMatcher && m_visionMatcher->params.useAutoSeg
|
||
? QString::fromUtf8("[Measure] 自动分割=启用:YOLO ROI → 自动分割 → 3D形状匹配。")
|
||
: QString::fromUtf8("[Measure] 自动分割=关闭:YOLO ROI → 直接3D形状匹配。"));
|
||
|
||
if (!leftReady && !rightReady) {
|
||
updateActionButtons();
|
||
emit logMessage(QStringLiteral("[Measure] No ready camera path. Check camera connection, calibration and 3D model."));
|
||
emit measureCompleted(false);
|
||
return;
|
||
}
|
||
|
||
m_measureBusy = true;
|
||
m_measureTimedOut = false;
|
||
const quint64 runId = ++m_measureRunId;
|
||
m_measurePartialLeft = VisionPoseResult();
|
||
m_measurePartialRight = VisionPoseResult();
|
||
m_resultImagesReadyForExport = false;
|
||
updateActionButtons();
|
||
ui.progressAlgo->setValue(0);
|
||
m_lastPose = VisionPoseResult();
|
||
m_displayPose = VisionPoseResult();
|
||
m_lastFusion = StereoPoseFusion::FusionResult();
|
||
m_poseOverlayBaseImage = QImage();
|
||
m_poseOverlayCenterPose = VisionPoseResult();
|
||
m_coord->setCameraPose(VisionPoseResult());
|
||
m_resultImages.remove(S_LEFT_CAPTURE); m_resultImages.remove(S_RIGHT_CAPTURE);
|
||
m_resultImages.remove(S_LEFT_ROI); m_resultImages.remove(S_RIGHT_ROI);
|
||
m_resultImages.remove(S_LEFT_ROI_POS); m_resultImages.remove(S_RIGHT_ROI_POS);
|
||
m_resultImages.remove(S_LEFT_SEG); m_resultImages.remove(S_RIGHT_SEG);
|
||
m_resultImages.remove(S_LEFT_POSE); m_resultImages.remove(S_RIGHT_POSE);
|
||
m_resultImages.remove(S_FUSION);
|
||
m_resultImages.remove(S_POSE);
|
||
m_resultImages.remove(S_GRASP_POSE);
|
||
|
||
const qint64 startMs = QDateTime::currentMSecsSinceEpoch();
|
||
m_captureSnapshot = MeasurementCaptureSnapshot();
|
||
m_captureSnapshot.runId = runId;
|
||
m_captureSnapshot.captureBeginMs = startMs;
|
||
m_captureSnapshot.parameters = m_visionMatcher->params;
|
||
m_captureSnapshot.leftCal = m_visionMatcher->cameraParamFile(CAM_LEFT);
|
||
m_captureSnapshot.rightCal = m_visionMatcher->cameraParamFile(CAM_RIGHT);
|
||
m_captureSnapshot.leftModel = m_visionMatcher->shapeModelFile(CAM_LEFT);
|
||
m_captureSnapshot.rightModel = m_visionMatcher->shapeModelFile(CAM_RIGHT);
|
||
m_captureSnapshot.stereoPoseFile = m_stereoCalFile;
|
||
emit measurementCaptureStarted();
|
||
|
||
QImage leftImg;
|
||
QImage rightImg;
|
||
if (leftReady) {
|
||
ui.progressAlgo->setValue(10);
|
||
leftImg = m_cam->captureLeft();
|
||
if (leftImg.isNull())
|
||
emit logMessage(QStringLiteral(" LEFT capture failed."));
|
||
}
|
||
|
||
if (rightReady && (dualMode || !leftReady)) {
|
||
ui.progressAlgo->setValue(20);
|
||
rightImg = m_cam->captureRight();
|
||
if (rightImg.isNull())
|
||
emit logMessage(QStringLiteral(" RIGHT capture failed."));
|
||
}
|
||
|
||
if (leftImg.isNull() && rightImg.isNull()) {
|
||
emit logMessage(QStringLiteral("[Measure] No image captured."));
|
||
appendFusionCsv(VisionPoseResult(), VisionPoseResult(),
|
||
StereoPoseFusion::FusionResult(), VisionPoseResult(),
|
||
QDateTime::currentMSecsSinceEpoch() - startMs);
|
||
ui.progressAlgo->setValue(0);
|
||
m_measureBusy = false;
|
||
updateActionButtons();
|
||
emit measureCompleted(false);
|
||
return;
|
||
}
|
||
|
||
m_capturedFrame = leftImg.isNull() ? rightImg : leftImg;
|
||
m_captureSnapshot.leftImage = leftImg;
|
||
m_captureSnapshot.rightImage = rightImg;
|
||
m_captureSnapshot.captureEndMs = QDateTime::currentMSecsSinceEpoch();
|
||
storeResultImage(S_CAPTURE, m_capturedFrame);
|
||
if (!leftImg.isNull())
|
||
storeResultImage(S_LEFT_CAPTURE, leftImg);
|
||
if (!rightImg.isNull())
|
||
storeResultImage(S_RIGHT_CAPTURE, rightImg);
|
||
m_visionGuide->setFrame(m_capturedFrame, leftImg.isNull() ? CAM_RIGHT : CAM_LEFT);
|
||
m_visionGuide->setROI(QRect());
|
||
|
||
QRect roiL;
|
||
QRect roiR;
|
||
cv::Mat leftGray;
|
||
cv::Mat rightGray;
|
||
|
||
if (!leftImg.isNull() && leftReady) {
|
||
ui.progressAlgo->setValue(32);
|
||
roiL = m_yolo->runYoloAndCrop(leftImg, -1, {}, QStringLiteral("LEFT"));
|
||
if (roiL.isValid()) {
|
||
m_visionGuide->setFrame(leftImg, CAM_LEFT);
|
||
m_visionGuide->setROI(roiL);
|
||
storeResultImage(S_LEFT_ROI, leftImg.copy(roiL));
|
||
storeResultImage(S_LEFT_ROI_POS,
|
||
drawRoiOverlayImage(leftImg, roiL, QStringLiteral("LEFT ROI")));
|
||
QImage rgb = leftImg.convertToFormat(QImage::Format_RGB888);
|
||
cv::Mat rgbMat(rgb.height(), rgb.width(), CV_8UC3,
|
||
const_cast<uchar*>(rgb.constBits()), rgb.bytesPerLine());
|
||
cv::cvtColor(rgbMat, leftGray, cv::COLOR_RGB2GRAY);
|
||
} else {
|
||
emit logMessage(QStringLiteral(" LEFT ROI invalid; skip LEFT VisionGuide match."));
|
||
}
|
||
}
|
||
|
||
if (!rightImg.isNull() && rightReady) {
|
||
ui.progressAlgo->setValue(45);
|
||
roiR = m_yolo->runYoloAndCrop(rightImg, -1, roiL, QStringLiteral("RIGHT"));
|
||
if (roiR.isValid()) {
|
||
if (leftGray.empty()) {
|
||
m_visionGuide->setFrame(rightImg, CAM_RIGHT);
|
||
m_visionGuide->setROI(roiR);
|
||
}
|
||
storeResultImage(S_RIGHT_ROI, rightImg.copy(roiR));
|
||
storeResultImage(S_RIGHT_ROI_POS,
|
||
drawRoiOverlayImage(rightImg, roiR, QStringLiteral("RIGHT ROI")));
|
||
QImage rgb = rightImg.convertToFormat(QImage::Format_RGB888);
|
||
cv::Mat rgbMat(rgb.height(), rgb.width(), CV_8UC3,
|
||
const_cast<uchar*>(rgb.constBits()), rgb.bytesPerLine());
|
||
cv::cvtColor(rgbMat, rightGray, cv::COLOR_RGB2GRAY);
|
||
} else {
|
||
emit logMessage(QStringLiteral(" RIGHT ROI invalid; skip RIGHT VisionGuide match."));
|
||
}
|
||
}
|
||
|
||
if (leftGray.empty() && rightGray.empty()) {
|
||
emit logMessage(QStringLiteral("[Measure] No valid ROI for VisionGuide matching."));
|
||
appendFusionCsv(VisionPoseResult(), VisionPoseResult(),
|
||
StereoPoseFusion::FusionResult(), VisionPoseResult(),
|
||
QDateTime::currentMSecsSinceEpoch() - startMs);
|
||
ui.progressAlgo->setValue(0);
|
||
m_measureBusy = false;
|
||
updateActionButtons();
|
||
emit measureCompleted(false);
|
||
return;
|
||
}
|
||
|
||
ui.progressAlgo->setValue(60);
|
||
m_captureSnapshot.leftRoi = roiL;
|
||
m_captureSnapshot.rightRoi = roiR;
|
||
emit logMessage(QStringLiteral("[Measure] VisionGuide matching is running in background..."));
|
||
|
||
QPointer<MeasurePanel> self(this);
|
||
VisionGuideMatcher* matcher = m_visionMatcher;
|
||
StereoPoseFusion* fusionEngine = m_fusion;
|
||
QThread* thread = new QThread(this);
|
||
QObject* worker = new QObject;
|
||
worker->moveToThread(thread);
|
||
connect(thread, &QThread::finished, worker, &QObject::deleteLater);
|
||
connect(thread, &QThread::finished, thread, &QObject::deleteLater);
|
||
connect(thread, &QThread::finished, this, [this, thread, runId]() {
|
||
if (m_measureThread == thread)
|
||
m_measureThread = nullptr;
|
||
// 视觉引导 的同步匹配无法安全强制终止。超时结果已经返回后,
|
||
// 等后台调用自然退出再允许下一轮,避免两个测量并发访问同一 matcher。
|
||
if (m_measureRunId == runId && m_measureTimedOut) {
|
||
m_measureBusy = false;
|
||
updateActionButtons();
|
||
emit logMessage(QString::fromUtf8(
|
||
"[Measure] 超时测量的后台任务已结束,现在可以开始下一次测量。"));
|
||
}
|
||
});
|
||
connect(thread, &QThread::started, worker,
|
||
[self, matcher, fusionEngine, leftImg, rightImg, leftGray, rightGray, roiL, roiR,
|
||
leftReady, rightReady, dualMode, startMs, runId]() mutable {
|
||
VisionPoseResult poseL;
|
||
VisionPoseResult poseR;
|
||
StereoPoseFusion::FusionResult fusion;
|
||
VisionPoseResult finalPose;
|
||
bool rightSkippedForDeadline = false;
|
||
|
||
try {
|
||
if (leftReady && !leftGray.empty() && roiL.isValid()) {
|
||
poseL = matcher->match(leftGray, roiL, CAM_LEFT);
|
||
if (self) {
|
||
QMetaObject::invokeMethod(self.data(), [self, runId, poseL]() {
|
||
if (self && self->m_measureRunId == runId && !self->m_measureTimedOut)
|
||
self->m_measurePartialLeft = poseL;
|
||
}, Qt::BlockingQueuedConnection);
|
||
}
|
||
}
|
||
|
||
if (rightReady && !rightGray.empty() && roiR.isValid()) {
|
||
const qint64 elapsedMs = QDateTime::currentMSecsSinceEpoch() - startMs;
|
||
if (poseL.valid && elapsedMs >= kMeasureDeadlineMs - kRightMatchReserveMs) {
|
||
rightSkippedForDeadline = true;
|
||
} else {
|
||
poseR = matcher->match(rightGray, roiR, CAM_RIGHT);
|
||
if (self) {
|
||
QMetaObject::invokeMethod(self.data(), [self, runId, poseR]() {
|
||
if (self && self->m_measureRunId == runId && !self->m_measureTimedOut)
|
||
self->m_measurePartialRight = poseR;
|
||
}, Qt::BlockingQueuedConnection);
|
||
}
|
||
}
|
||
}
|
||
|
||
if (dualMode && !rightSkippedForDeadline) {
|
||
fusion = fusionEngine->fuse(poseL, poseR, StereoPoseFusion::FM_InverseVariance,1.0,10.0,matcher->cameraParamFile(CAM_LEFT),matcher->cameraParamFile(CAM_RIGHT));
|
||
finalPose = fusion.fused;
|
||
} else {
|
||
finalPose = poseL.valid ? poseL : poseR;
|
||
}
|
||
} catch (...) {
|
||
finalPose.valid = false;
|
||
finalPose.errorMsg = QStringLiteral("Unhandled exception during background measurement.");
|
||
}
|
||
|
||
const qint64 totalMs = QDateTime::currentMSecsSinceEpoch() - startMs;
|
||
|
||
if (!self) {
|
||
QThread::currentThread()->quit();
|
||
return;
|
||
}
|
||
QMetaObject::invokeMethod(self.data(), [self, poseL, poseR, fusion, finalPose,
|
||
leftImg, rightImg, dualMode, totalMs,
|
||
rightSkippedForDeadline, runId]() mutable {
|
||
if (!self) return;
|
||
if (self->m_measureRunId != runId) {
|
||
emit self->logMessage(QStringLiteral("[Measure] Stale worker result discarded."));
|
||
return;
|
||
}
|
||
if (self->m_measureTimedOut) {
|
||
emit self->logMessage(QStringLiteral("[Measure] Worker finished after the 10 s timeout; result discarded."));
|
||
return;
|
||
}
|
||
self->m_lastFusion = fusion;
|
||
self->m_lastPose = finalPose;
|
||
self->m_captureSnapshot.leftPose = poseL;
|
||
self->m_captureSnapshot.rightPose = poseR;
|
||
self->m_captureSnapshot.finalPose = finalPose;
|
||
self->m_captureSnapshot.fusion = fusion;
|
||
self->m_captureSnapshot.finalSide = (dualMode && !rightSkippedForDeadline) || poseL.valid ? CAM_LEFT : CAM_RIGHT;
|
||
self->m_captureSnapshot.valid = finalPose.valid;
|
||
self->m_displayPose = finalPose;
|
||
|
||
emit self->logMessage(poseLogLine(QStringLiteral("LEFT_RAW"), poseL));
|
||
emit self->logMessage(poseLogLine(QStringLiteral("RIGHT_RAW"), poseR));
|
||
|
||
if (rightSkippedForDeadline) {
|
||
emit self->logMessage(QString::fromUtf8(
|
||
"[Measure] 左路已成功且接近 10 s 截止时间,跳过右路匹配并返回左路结果。"));
|
||
}
|
||
|
||
if (dualMode && !rightSkippedForDeadline) {
|
||
emit self->logMessage(QStringLiteral(" ") + fusion.message);
|
||
emit self->logMessage(poseLogLine(QStringLiteral("LEFT_IN_LEFT"), fusion.leftInLeft));
|
||
emit self->logMessage(poseLogLine(QStringLiteral("RIGHT_IN_LEFT"), fusion.rightInLeft));
|
||
emit self->logMessage(QStringLiteral(" FUSION_METRIC dt=%1mm daxis=%2deg consistent=%3 wL=%4 wR=%5 confidence=%6")
|
||
.arg(fusion.positionDisparityMm, 0, 'f', 3)
|
||
.arg(fusion.orientationDisparityDeg, 0, 'f', 3)
|
||
.arg(fusion.consistent ? QStringLiteral("true") : QStringLiteral("false"))
|
||
.arg(fusion.weightLeft, 0, 'f', 3)
|
||
.arg(fusion.weightRight, 0, 'f', 3)
|
||
.arg(fusion.confidence, 0, 'f', 3));
|
||
emit self->logMessage(poseLogLine(QStringLiteral("FUSED_LEFT_FRAME"), fusion.fused));
|
||
}
|
||
|
||
if (!poseL.segmentationDebugImage.empty()) {
|
||
self->storeResultImage(S_LEFT_SEG,
|
||
VisionGuideMatcher::matToQImage(poseL.segmentationDebugImage));
|
||
}
|
||
if (!poseR.segmentationDebugImage.empty()) {
|
||
self->storeResultImage(S_RIGHT_SEG,
|
||
VisionGuideMatcher::matToQImage(poseR.segmentationDebugImage));
|
||
}
|
||
if (poseL.valid && !leftImg.isNull()) {
|
||
self->storeResultImage(S_LEFT_POSE,
|
||
drawPoseOverlayImage(self->m_visionMatcher, leftImg, poseL));
|
||
}
|
||
if (poseR.valid && !rightImg.isNull()) {
|
||
self->storeResultImage(S_RIGHT_POSE,
|
||
drawPoseOverlayImage(self->m_visionMatcher, rightImg, poseR));
|
||
}
|
||
self->storeResultImage(S_FUSION,
|
||
makeFusionDiagnosticImage(poseL, poseR, fusion, finalPose, totalMs));
|
||
|
||
if (finalPose.valid) {
|
||
emit self->logMessage(poseLogLine(QStringLiteral("FINAL_OUTPUT"), finalPose));
|
||
self->displayPoseResult(finalPose);
|
||
VisionPoseResult poseForDraw = finalPose;
|
||
bool drawRightRaw = false;
|
||
if (dualMode && poseR.valid && !rightImg.isNull()) {
|
||
drawRightRaw = !poseL.valid ||
|
||
(!fusion.consistent && fusion.weightRight > fusion.weightLeft);
|
||
}
|
||
QImage poseFrame = drawRightRaw ? rightImg
|
||
: ((dualMode || poseL.valid) ? leftImg : rightImg);
|
||
if (drawRightRaw) {
|
||
poseForDraw = poseR;
|
||
emit self->logMessage(QStringLiteral("[Measure] result view uses RIGHT image because stereo fusion selected right pose."));
|
||
}
|
||
if (poseFrame.isNull())
|
||
poseFrame = self->m_capturedFrame;
|
||
const bool poseUsesRightCamera = drawRightRaw ||
|
||
(leftImg.isNull() && !rightImg.isNull());
|
||
const CameraSide poseSide = poseUsesRightCamera ? CAM_RIGHT : CAM_LEFT;
|
||
self->m_visionMatcher->updatePoseProjection(poseForDraw, poseSide, true);
|
||
self->rememberPoseOverlaySource(poseFrame, poseForDraw, poseSide);
|
||
cv::Mat poseVis = VisionGuideMatcher::qImageToMat(poseFrame);
|
||
if (!poseVis.empty()) {
|
||
self->m_visionMatcher->drawPoseResult(poseVis, poseForDraw);
|
||
QImage finalImage = VisionGuideMatcher::matToQImage(poseVis);
|
||
self->storeResultImage(S_POSE, finalImage);
|
||
self->showExternalResultImage(S_POSE, finalImage);
|
||
self->ui.tabWidget->setCurrentWidget(self->ui.tabVisionAlgo);
|
||
}
|
||
self->ui.editResultTime->setText(QString::number(totalMs));
|
||
self->m_coord->setCameraPose(finalPose);
|
||
self->appendFusionCsv(poseL, poseR, fusion, finalPose, totalMs);
|
||
self->ui.progressAlgo->setValue(100);
|
||
emit self->logMessage(QStringLiteral("=== Measurement completed Score=%1 Total=%2ms ===")
|
||
.arg(finalPose.score, 0, 'f', 4)
|
||
.arg(totalMs));
|
||
} else {
|
||
emit self->logMessage(QStringLiteral("[Measure] Pose measurement failed: %1").arg(finalPose.errorMsg));
|
||
self->appendFusionCsv(poseL, poseR, fusion, finalPose, totalMs);
|
||
self->ui.progressAlgo->setValue(0);
|
||
}
|
||
|
||
self->m_resultImagesReadyForExport = true;
|
||
self->m_measureBusy = false;
|
||
self->updateActionButtons();
|
||
emit self->measureCompleted(finalPose.valid);
|
||
}, Qt::QueuedConnection);
|
||
QThread::currentThread()->quit();
|
||
}, Qt::QueuedConnection);
|
||
m_measureThread = thread;
|
||
thread->start();
|
||
|
||
// 旧实现的 singleShot 只检查共享 bool:上一轮的定时器可能在下一轮
|
||
// 测量期间触发,造成“只运行了 3 s 却报 10 s 超时”。runId 将定时器
|
||
// 与本轮测量绑定;remainingMs 则让 10 s 覆盖采图、YOLO 和匹配全流程。
|
||
const qint64 elapsedBeforeWorkerMs = QDateTime::currentMSecsSinceEpoch() - startMs;
|
||
const int remainingMs = static_cast<int>(
|
||
std::max<qint64>(0, kMeasureDeadlineMs - elapsedBeforeWorkerMs));
|
||
QTimer::singleShot(remainingMs, this,
|
||
[this, runId, startMs, leftImg, rightImg]() {
|
||
if (m_measureRunId != runId || !m_measureBusy || m_measureTimedOut)
|
||
return;
|
||
|
||
m_measureTimedOut = true;
|
||
const qint64 totalMs = QDateTime::currentMSecsSinceEpoch() - startMs;
|
||
const VisionPoseResult poseL = m_measurePartialLeft;
|
||
const VisionPoseResult poseR = m_measurePartialRight;
|
||
const bool useLeft = poseL.valid && (!poseR.valid || poseL.score >= poseR.score);
|
||
const VisionPoseResult fallbackPose = useLeft ? poseL : poseR;
|
||
|
||
if (fallbackPose.valid) {
|
||
m_lastFusion = StereoPoseFusion::FusionResult();
|
||
m_lastPose = fallbackPose;
|
||
m_captureSnapshot.leftPose = poseL;
|
||
m_captureSnapshot.rightPose = poseR;
|
||
m_captureSnapshot.finalPose = fallbackPose;
|
||
m_captureSnapshot.finalSide = useLeft ? CAM_LEFT : CAM_RIGHT;
|
||
m_captureSnapshot.valid = true;
|
||
m_displayPose = fallbackPose;
|
||
emit logMessage(QString::fromUtf8(
|
||
"[Measure][TIMEOUT] 全流程达到 %1 ms;%2路已经匹配成功,立即返回该路结果,不再等待另一相机。")
|
||
.arg(totalMs)
|
||
.arg(useLeft ? QString::fromUtf8("左") : QString::fromUtf8("右")));
|
||
emit logMessage(poseLogLine(QStringLiteral("LEFT_RAW_AT_TIMEOUT"), poseL));
|
||
emit logMessage(poseLogLine(QStringLiteral("RIGHT_RAW_AT_TIMEOUT"), poseR));
|
||
emit logMessage(poseLogLine(QStringLiteral("FINAL_OUTPUT_TIMEOUT_FALLBACK"), fallbackPose));
|
||
|
||
displayPoseResult(fallbackPose);
|
||
QImage poseFrame = useLeft ? leftImg : rightImg;
|
||
if (poseFrame.isNull())
|
||
poseFrame = m_capturedFrame;
|
||
VisionPoseResult poseForDraw = fallbackPose;
|
||
const CameraSide poseSide = useLeft ? CAM_LEFT : CAM_RIGHT;
|
||
m_visionMatcher->updatePoseProjection(poseForDraw, poseSide, true);
|
||
rememberPoseOverlaySource(poseFrame, poseForDraw, poseSide);
|
||
cv::Mat poseVis = VisionGuideMatcher::qImageToMat(poseFrame);
|
||
if (!poseVis.empty()) {
|
||
m_visionMatcher->drawPoseResult(poseVis, poseForDraw);
|
||
const QImage finalImage = VisionGuideMatcher::matToQImage(poseVis);
|
||
storeResultImage(S_POSE, finalImage);
|
||
showExternalResultImage(S_POSE, finalImage);
|
||
ui.tabWidget->setCurrentWidget(ui.tabVisionAlgo);
|
||
}
|
||
ui.editResultTime->setText(QString::number(totalMs));
|
||
m_coord->setCameraPose(fallbackPose);
|
||
appendFusionCsv(poseL, poseR, StereoPoseFusion::FusionResult(),
|
||
fallbackPose, totalMs);
|
||
ui.progressAlgo->setValue(100);
|
||
m_resultImagesReadyForExport = true;
|
||
emit logMessage(QString::fromUtf8(
|
||
"=== Measurement completed by single-camera timeout fallback Score=%1 Total=%2ms ===")
|
||
.arg(fallbackPose.score, 0, 'f', 4)
|
||
.arg(totalMs));
|
||
emit measureCompleted(true);
|
||
} else {
|
||
ui.progressAlgo->setValue(0);
|
||
emit logMessage(QString::fromUtf8(
|
||
"[Measure][ERROR] 全流程达到 %1 ms,左右路均无可用结果,已返回失败。")
|
||
.arg(totalMs));
|
||
emit measureCompleted(false);
|
||
}
|
||
|
||
// 不强杀正在执行的 视觉引导 调用,也不允许下一轮与其并发。
|
||
// 数值结果已经按时返回;后台退出后 finished 回调会重新启用按钮。
|
||
updateActionButtons();
|
||
});
|
||
return;
|
||
}
|
||
}
|
||
|
||
double MeasurePanel::currentGraspOffsetMm() const
|
||
{
|
||
return ui.spinGraspOffsetZMm->value();
|
||
}
|
||
VisionGuideMatcher::Params MeasurePanel::currentVisionParameters(){
|
||
if(m_visionGuide)m_visionGuide->applyCurrentParameters();
|
||
return m_visionMatcher?m_visionMatcher->params:VisionGuideMatcher::Params();
|
||
}
|
||
|
||
// ── 独立快速测量(5 s)────────────────────────────────────────
|
||
void MeasurePanel::onFastMeasure()
|
||
{
|
||
if (m_measureBusy) {
|
||
emit logMessage(QString::fromUtf8("[快速测量] 当前仍有测量任务运行,请等待其结束。"));
|
||
return;
|
||
}
|
||
|
||
const bool leftReady = m_cam && m_cam->leftConnected() &&
|
||
m_visionMatcher && m_visionMatcher->isReady(CAM_LEFT);
|
||
const bool rightReady = m_cam && m_cam->rightConnected() &&
|
||
m_visionMatcher && m_visionMatcher->isReady(CAM_RIGHT);
|
||
const bool stereoReady = rightReady && m_fusion && m_fusion->isReady();
|
||
if (!leftReady) {
|
||
emit logMessage(QString::fromUtf8("[快速测量][ERROR] 左相机、左标定或左3D模板未就绪。"));
|
||
emit measureCompleted(false);
|
||
return;
|
||
}
|
||
|
||
if (m_visionGuide)
|
||
m_visionGuide->applyCurrentParameters();
|
||
|
||
m_measureBusy = true;
|
||
m_measureTimedOut = false;
|
||
const quint64 runId = ++m_measureRunId;
|
||
m_measurePartialLeft = VisionPoseResult();
|
||
m_measurePartialRight = VisionPoseResult();
|
||
m_resultImagesReadyForExport = false;
|
||
m_lastPose = VisionPoseResult();
|
||
m_lastFusion = StereoPoseFusion::FusionResult();
|
||
ui.progressAlgo->setValue(0);
|
||
ui.editResultX->clear(); ui.editResultY->clear(); ui.editResultZ->clear();
|
||
ui.editResultAxis->clear(); ui.editResultClass->clear(); ui.editResultTime->clear();
|
||
|
||
// 快速流程的图像结果严格只保留“原图”和“位姿估计”。
|
||
const QStringList measurementImages = {
|
||
S_CAPTURE, S_LEFT_CAPTURE, S_RIGHT_CAPTURE, S_ROI, S_LEFT_ROI, S_RIGHT_ROI,
|
||
S_LEFT_ROI_POS, S_RIGHT_ROI_POS, S_YOLO, S_SEG, S_LEFT_SEG, S_RIGHT_SEG,
|
||
S_LEFT_POSE, S_RIGHT_POSE, S_FUSION, S_DEBUG, S_POSE
|
||
};
|
||
for (const QString& name : measurementImages) {
|
||
m_resultImages.remove(name);
|
||
const int comboIndex = ui.comboResultView->findText(name);
|
||
if (comboIndex >= 0) ui.comboResultView->removeItem(comboIndex);
|
||
}
|
||
updateActionButtons();
|
||
|
||
const qint64 startMs = QDateTime::currentMSecsSinceEpoch();
|
||
emit logMessage(QString::fromUtf8(
|
||
"=== 快速测量开始(截止5秒;左图YOLO;右图使用双目标定预测ROI;跳过自动分割) ==="));
|
||
|
||
QElapsedTimer stage;
|
||
stage.start();
|
||
ui.progressAlgo->setValue(10);
|
||
const QImage leftImg = m_cam->captureLeft();
|
||
const qint64 captureLeftMs = stage.restart();
|
||
QImage rightImg;
|
||
qint64 captureRightMs = 0;
|
||
if (stereoReady) {
|
||
rightImg = m_cam->captureRight();
|
||
captureRightMs = stage.restart();
|
||
}
|
||
if (leftImg.isNull()) {
|
||
m_measureBusy = false;
|
||
updateActionButtons();
|
||
emit logMessage(QString::fromUtf8("[快速测量][ERROR] 左图采集失败。"));
|
||
emit measureCompleted(false);
|
||
return;
|
||
}
|
||
|
||
m_capturedFrame = leftImg;
|
||
storeResultImage(S_CAPTURE, leftImg);
|
||
ui.progressAlgo->setValue(25);
|
||
|
||
stage.restart();
|
||
const QRect roiL = m_yolo->runYoloAndCrop(leftImg, -1, {}, QStringLiteral("FAST_LEFT"));
|
||
const qint64 yoloLeftMs = stage.elapsed();
|
||
if (!roiL.isValid()) {
|
||
m_measureBusy = false;
|
||
ui.progressAlgo->setValue(0);
|
||
updateActionButtons();
|
||
emit logMessage(QString::fromUtf8("[快速测量][ERROR] 左图YOLO未得到有效ROI。"));
|
||
emit measureCompleted(false);
|
||
return;
|
||
}
|
||
|
||
stage.restart();
|
||
const cv::Mat leftGray = qImageToGrayFast(leftImg);
|
||
const qint64 grayLeftMs = stage.elapsed();
|
||
if (leftGray.empty()) {
|
||
m_measureBusy = false;
|
||
ui.progressAlgo->setValue(0);
|
||
updateActionButtons();
|
||
emit logMessage(QString::fromUtf8("[快速测量][ERROR] 左图灰度转换失败。"));
|
||
emit measureCompleted(false);
|
||
return;
|
||
}
|
||
|
||
if (QDateTime::currentMSecsSinceEpoch() - startMs >= kFastMeasureDeadlineMs) {
|
||
m_measureBusy = false;
|
||
ui.progressAlgo->setValue(0);
|
||
updateActionButtons();
|
||
emit logMessage(QString::fromUtf8("[快速测量][ERROR] 采图和左图预处理已达到5秒,返回失败。"));
|
||
emit measureCompleted(false);
|
||
return;
|
||
}
|
||
|
||
ui.progressAlgo->setValue(45);
|
||
QPointer<MeasurePanel> self(this);
|
||
VisionGuideMatcher* matcher = m_visionMatcher;
|
||
StereoPoseFusion* fusionEngine = m_fusion;
|
||
const QString relPoseFile = stereoReady ? m_fusion->relPoseFile() : QString();
|
||
QThread* thread = new QThread(this);
|
||
QObject* worker = new QObject;
|
||
worker->moveToThread(thread);
|
||
connect(thread, &QThread::finished, worker, &QObject::deleteLater);
|
||
connect(thread, &QThread::finished, thread, &QObject::deleteLater);
|
||
connect(thread, &QThread::finished, this, [this, thread, runId]() {
|
||
if (m_measureThread == thread) m_measureThread = nullptr;
|
||
if (m_measureRunId == runId && m_measureTimedOut) {
|
||
m_measureBusy = false;
|
||
updateActionButtons();
|
||
emit logMessage(QString::fromUtf8(
|
||
"[快速测量] 超时后台任务已结束,现在可以再次测量。"));
|
||
}
|
||
});
|
||
|
||
connect(thread, &QThread::started, worker,
|
||
[self, matcher, fusionEngine, leftImg, rightImg, leftGray, roiL,
|
||
stereoReady, relPoseFile, startMs, runId,
|
||
captureLeftMs, captureRightMs, yoloLeftMs, grayLeftMs]() mutable {
|
||
VisionPoseResult poseL;
|
||
VisionPoseResult poseR;
|
||
StereoPoseFusion::FusionResult fusion;
|
||
VisionPoseResult finalPose;
|
||
QRect roiR;
|
||
bool rightAttempted = false;
|
||
bool rightSkippedByLeftRule = false;
|
||
QString decision;
|
||
qint64 leftMatchWallMs = 0;
|
||
qint64 rightRoiPredictMs = 0;
|
||
qint64 grayRightMs = 0;
|
||
qint64 rightMatchWallMs = 0;
|
||
qint64 fusionMs = 0;
|
||
|
||
try {
|
||
QElapsedTimer timer;
|
||
timer.start();
|
||
poseL = matcher->match(leftGray, roiL, CAM_LEFT, true);
|
||
leftMatchWallMs = timer.elapsed();
|
||
|
||
const bool slowAccepted = poseL.valid && poseL.matchTimeMs >= kFastSlowLeftMs &&
|
||
poseL.score > 0.80;
|
||
const bool fastAccepted = poseL.valid && poseL.matchTimeMs < kFastSlowLeftMs &&
|
||
poseL.score > 0.85;
|
||
rightSkippedByLeftRule = slowAccepted || fastAccepted;
|
||
|
||
if (rightSkippedByLeftRule) {
|
||
finalPose = poseL;
|
||
decision = slowAccepted
|
||
? QString::fromUtf8("左路匹配时间≥3秒且Score>0.80,跳过右路")
|
||
: QString::fromUtf8("左路匹配时间<3秒且Score>0.85,跳过右路");
|
||
} else if (stereoReady && !rightImg.isNull() &&
|
||
QDateTime::currentMSecsSinceEpoch() - startMs < kFastMeasureDeadlineMs) {
|
||
timer.restart();
|
||
const double estimatedDepth = poseL.valid && poseL.tz > 0.03 ? poseL.tz : 0.30;
|
||
const bool predicted = matcher->predictRightRoi(
|
||
roiL, relPoseFile, rightImg.size(), roiR, estimatedDepth, 0.25);
|
||
rightRoiPredictMs = timer.elapsed();
|
||
if (predicted) {
|
||
rightAttempted = true;
|
||
timer.restart();
|
||
const cv::Mat rightGray = qImageToGrayFast(rightImg);
|
||
grayRightMs = timer.elapsed();
|
||
if (!rightGray.empty()) {
|
||
timer.restart();
|
||
poseR = matcher->match(rightGray, roiR, CAM_RIGHT, true);
|
||
rightMatchWallMs = timer.elapsed();
|
||
} else {
|
||
poseR.errorMsg = QString::fromUtf8("右图灰度转换失败");
|
||
}
|
||
decision = QString::fromUtf8("左路未达到快速接受阈值,已使用双目标定预测右ROI并计算右路");
|
||
} else {
|
||
decision = QString::fromUtf8("右ROI双目标定投影失败,未执行右图完整YOLO");
|
||
finalPose = stereoReady ? VisionPoseResult() : poseL;
|
||
if (!finalPose.valid)
|
||
finalPose.errorMsg = QString::fromUtf8("右ROI预测失败");
|
||
}
|
||
} else {
|
||
finalPose = poseL;
|
||
decision = stereoReady
|
||
? QString::fromUtf8("剩余时间不足,未启动右路")
|
||
: QString::fromUtf8("右路未就绪,使用左路");
|
||
}
|
||
|
||
if (rightAttempted) {
|
||
timer.restart();
|
||
fusion = fusionEngine->fuse(poseL, poseR, StereoPoseFusion::FM_InverseVariance,1.0,10.0,matcher->cameraParamFile(CAM_LEFT),matcher->cameraParamFile(CAM_RIGHT));
|
||
fusionMs = timer.elapsed();
|
||
finalPose = fusion.fused;
|
||
} else if (!finalPose.valid && poseL.valid && !stereoReady) {
|
||
finalPose = poseL;
|
||
}
|
||
} catch (...) {
|
||
finalPose = VisionPoseResult();
|
||
finalPose.errorMsg = QString::fromUtf8("快速测量后台发生未处理异常");
|
||
}
|
||
|
||
const qint64 totalMs = QDateTime::currentMSecsSinceEpoch() - startMs;
|
||
if (!self) {
|
||
QThread::currentThread()->quit();
|
||
return;
|
||
}
|
||
QMetaObject::invokeMethod(self.data(),
|
||
[self, runId, leftImg, rightImg, poseL, poseR, fusion, finalPose,
|
||
rightAttempted, rightSkippedByLeftRule, roiR, decision, totalMs,
|
||
captureLeftMs, captureRightMs, yoloLeftMs, grayLeftMs,
|
||
leftMatchWallMs, rightRoiPredictMs, grayRightMs,
|
||
rightMatchWallMs, fusionMs, startMs]() mutable {
|
||
if (!self || self->m_measureRunId != runId) return;
|
||
if (self->m_measureTimedOut) {
|
||
emit self->logMessage(QString::fromUtf8(
|
||
"[快速测量] 后台在5秒截止后结束,结果已丢弃。"));
|
||
return;
|
||
}
|
||
if (totalMs > kFastMeasureDeadlineMs) {
|
||
self->m_measureBusy = false;
|
||
self->ui.progressAlgo->setValue(0);
|
||
self->updateActionButtons();
|
||
emit self->logMessage(QString::fromUtf8(
|
||
"[快速测量][ERROR] 总耗时=%1ms,超过5秒,返回失败。").arg(totalMs));
|
||
emit self->measureCompleted(false);
|
||
return;
|
||
}
|
||
|
||
if (finalPose.valid) {
|
||
bool drawRight = rightAttempted && poseR.valid &&
|
||
(!poseL.valid || (!fusion.consistent && fusion.weightRight > fusion.weightLeft));
|
||
QImage poseFrame = drawRight ? rightImg : leftImg;
|
||
VisionPoseResult poseForDraw = drawRight ? poseR : finalPose;
|
||
if (poseFrame.isNull()) poseFrame = leftImg;
|
||
QImage poseImage;
|
||
cv::Mat poseVis = VisionGuideMatcher::qImageToMat(poseFrame);
|
||
if (!poseVis.empty()) {
|
||
self->m_visionMatcher->drawPoseResult(poseVis, poseForDraw);
|
||
poseImage = VisionGuideMatcher::matToQImage(poseVis);
|
||
}
|
||
|
||
// 将最终位姿图生成也纳入5秒全流程截止判断;日志和CSV在判定后写入。
|
||
const qint64 completionMs = QDateTime::currentMSecsSinceEpoch() - startMs;
|
||
if (completionMs > kFastMeasureDeadlineMs || poseImage.isNull()) {
|
||
self->m_measureBusy = false;
|
||
self->ui.progressAlgo->setValue(0);
|
||
self->updateActionButtons();
|
||
emit self->logMessage(QString::fromUtf8(
|
||
"[快速测量][ERROR] 位姿图生成后总耗时=%1ms,超过5秒或图像生成失败,返回失败。")
|
||
.arg(completionMs));
|
||
emit self->measureCompleted(false);
|
||
return;
|
||
}
|
||
|
||
emit self->logMessage(QString::fromUtf8(
|
||
"[快速测量][耗时] 左采图=%1ms 右采图=%2ms 左YOLO=%3ms 左灰度=%4ms "
|
||
"左匹配墙钟=%5ms/核心=%6ms 右ROI投影=%7ms 右灰度=%8ms "
|
||
"右匹配墙钟=%9ms/核心=%10ms 融合=%11ms 结果图后总计=%12ms")
|
||
.arg(captureLeftMs).arg(captureRightMs).arg(yoloLeftMs).arg(grayLeftMs)
|
||
.arg(leftMatchWallMs).arg(poseL.matchTimeMs, 0, 'f', 0)
|
||
.arg(rightRoiPredictMs).arg(grayRightMs)
|
||
.arg(rightMatchWallMs).arg(poseR.matchTimeMs, 0, 'f', 0)
|
||
.arg(fusionMs).arg(completionMs));
|
||
emit self->logMessage(QString::fromUtf8("[快速测量][决策] %1;右路执行=%2;右ROI=%3")
|
||
.arg(decision)
|
||
.arg(rightAttempted ? QString::fromUtf8("是") : QString::fromUtf8("否"))
|
||
.arg(roiR.isValid()
|
||
? QStringLiteral("(%1,%2,%3,%4)").arg(roiR.x()).arg(roiR.y()).arg(roiR.width()).arg(roiR.height())
|
||
: QStringLiteral("--")));
|
||
emit self->logMessage(poseLogLine(QStringLiteral("FAST_FINAL_OUTPUT"), finalPose));
|
||
|
||
self->m_lastFusion = fusion;
|
||
self->m_lastPose = finalPose;
|
||
self->displayPoseResult(finalPose);
|
||
self->storeResultImage(S_CAPTURE, poseFrame);
|
||
self->storeResultImage(S_POSE, poseImage);
|
||
self->showExternalResultImage(S_POSE, poseImage);
|
||
self->ui.tabWidget->setCurrentWidget(self->ui.tabVisionAlgo);
|
||
self->ui.editResultTime->setText(QString::number(completionMs));
|
||
self->m_coord->setCameraPose(finalPose);
|
||
self->appendFusionCsv(poseL, poseR, fusion, finalPose, completionMs);
|
||
self->ui.progressAlgo->setValue(100);
|
||
self->m_resultImagesReadyForExport = true;
|
||
emit self->logMessage(QString::fromUtf8(
|
||
"=== 快速测量完成 Score=%1 Total=%2ms ===")
|
||
.arg(finalPose.score, 0, 'f', 4).arg(completionMs));
|
||
} else {
|
||
self->ui.progressAlgo->setValue(0);
|
||
emit self->logMessage(QString::fromUtf8("[快速测量][ERROR] 位姿无效:%1")
|
||
.arg(finalPose.errorMsg));
|
||
}
|
||
self->m_measureBusy = false;
|
||
self->updateActionButtons();
|
||
emit self->measureCompleted(finalPose.valid);
|
||
}, Qt::QueuedConnection);
|
||
QThread::currentThread()->quit();
|
||
}, Qt::QueuedConnection);
|
||
|
||
m_measureThread = thread;
|
||
thread->start();
|
||
const qint64 elapsedBeforeWorkerMs = QDateTime::currentMSecsSinceEpoch() - startMs;
|
||
const int remainingMs = static_cast<int>(
|
||
std::max<qint64>(0, kFastMeasureDeadlineMs - elapsedBeforeWorkerMs));
|
||
QTimer::singleShot(remainingMs, this, [this, runId, startMs]() {
|
||
if (m_measureRunId != runId || !m_measureBusy || m_measureTimedOut) return;
|
||
m_measureTimedOut = true;
|
||
ui.progressAlgo->setValue(0);
|
||
ui.editResultTime->setText(QString::number(
|
||
QDateTime::currentMSecsSinceEpoch() - startMs));
|
||
emit logMessage(QString::fromUtf8(
|
||
"[快速测量][ERROR] 全流程达到5秒截止时间,按规则返回失败。"));
|
||
emit measureCompleted(false);
|
||
// 视觉引导同步调用不能安全强杀;后台退出前保持按钮禁用,防止并发访问matcher。
|
||
updateActionButtons();
|
||
});
|
||
}
|
||
|
||
// ── 结果图管理 ───────────────────────────────────────────────
|
||
void MeasurePanel::storeResultImage(const QString& name, const QImage& img, bool saveToExport) {
|
||
if (img.isNull())
|
||
return;
|
||
const bool replacingCurrent = (ui.comboResultView->currentText() == name);
|
||
m_resultImages[name] = img;
|
||
if (ui.comboResultView->findText(name) < 0)
|
||
ui.comboResultView->addItem(name);
|
||
if (saveToExport && ensureExportDir()) {
|
||
QString fn = name;
|
||
fn.replace(QRegExp("[^a-zA-Z0-9\\x4e00-\\x9fa5]"), "_");
|
||
const QString ts = QDateTime::currentDateTime().toString(QStringLiteral("yyyyMMdd_HHmmss_zzz"));
|
||
const QString fp = m_exportDir + "/" + fn + "_" + ts + ".png";
|
||
if (img.save(fp))
|
||
emit logMessage(QStringLiteral("[Measure] result image saved: %1").arg(fp));
|
||
else
|
||
emit logMessage(QStringLiteral("[Measure] result image save failed: %1").arg(fp));
|
||
}
|
||
if (replacingCurrent)
|
||
resetResultViewTransform();
|
||
}
|
||
|
||
void MeasurePanel::showExternalResultImage(const QString& name, const QImage& img)
|
||
{
|
||
if (img.isNull())
|
||
return;
|
||
storeResultImage(name, img);
|
||
resetResultViewTransform();
|
||
ui.comboResultView->setCurrentText(name);
|
||
showCurrentResultImage();
|
||
}
|
||
|
||
void MeasurePanel::showCurrentResultImage() {
|
||
QString name = ui.comboResultView->currentText();
|
||
if (m_resultImages.contains(name)) {
|
||
renderCurrentResultImage();
|
||
} else {
|
||
m_resultPanning = false;
|
||
ui.labelROIResult->clear();
|
||
ui.labelROIResult->setText("ROI / Result");
|
||
}
|
||
}
|
||
|
||
void MeasurePanel::onResultViewChanged(int)
|
||
{
|
||
resetResultViewTransform();
|
||
showCurrentResultImage();
|
||
}
|
||
|
||
void MeasurePanel::resetResultViewTransform()
|
||
{
|
||
m_resultZoom = 1.0;
|
||
m_resultPan = QPointF(0.0, 0.0);
|
||
m_resultPanning = false;
|
||
}
|
||
|
||
QRectF MeasurePanel::currentResultDrawRect() const
|
||
{
|
||
const QString name = ui.comboResultView->currentText();
|
||
const QImage img = m_resultImages.value(name);
|
||
const QSize view = ui.labelROIResult->size();
|
||
if (img.isNull() || img.width() <= 0 || img.height() <= 0 ||
|
||
view.width() <= 0 || view.height() <= 0)
|
||
return QRectF();
|
||
|
||
const double fit = (std::min)(double(view.width()) / double(img.width()),
|
||
double(view.height()) / double(img.height()));
|
||
const double scale = fit * m_resultZoom;
|
||
const QSizeF drawSize(double(img.width()) * scale, double(img.height()) * scale);
|
||
const QPointF topLeft((double(view.width()) - drawSize.width()) * 0.5 + m_resultPan.x(),
|
||
(double(view.height()) - drawSize.height()) * 0.5 + m_resultPan.y());
|
||
return QRectF(topLeft, drawSize);
|
||
}
|
||
|
||
void MeasurePanel::clampResultPan()
|
||
{
|
||
const QString name = ui.comboResultView->currentText();
|
||
const QImage img = m_resultImages.value(name);
|
||
const QSize view = ui.labelROIResult->size();
|
||
if (img.isNull() || img.width() <= 0 || img.height() <= 0 ||
|
||
view.width() <= 0 || view.height() <= 0) {
|
||
m_resultPan = QPointF(0.0, 0.0);
|
||
return;
|
||
}
|
||
|
||
if (m_resultZoom <= 1.0) {
|
||
m_resultZoom = 1.0;
|
||
m_resultPan = QPointF(0.0, 0.0);
|
||
return;
|
||
}
|
||
|
||
const double fit = (std::min)(double(view.width()) / double(img.width()),
|
||
double(view.height()) / double(img.height()));
|
||
const double drawW = double(img.width()) * fit * m_resultZoom;
|
||
const double drawH = double(img.height()) * fit * m_resultZoom;
|
||
const double maxX = (std::max)(0.0, (drawW - double(view.width())) * 0.5);
|
||
const double maxY = (std::max)(0.0, (drawH - double(view.height())) * 0.5);
|
||
m_resultPan.setX(qBound(-maxX, m_resultPan.x(), maxX));
|
||
m_resultPan.setY(qBound(-maxY, m_resultPan.y(), maxY));
|
||
}
|
||
|
||
void MeasurePanel::renderCurrentResultImage()
|
||
{
|
||
const QString name = ui.comboResultView->currentText();
|
||
const QImage img = m_resultImages.value(name);
|
||
const QSize view = ui.labelROIResult->size();
|
||
if (img.isNull() || view.width() <= 0 || view.height() <= 0) {
|
||
ui.labelROIResult->clear();
|
||
ui.labelROIResult->setText("ROI / Result");
|
||
return;
|
||
}
|
||
|
||
clampResultPan();
|
||
QPixmap canvas(view);
|
||
canvas.fill(QColor(28, 31, 36));
|
||
QPainter painter(&canvas);
|
||
painter.setRenderHint(QPainter::SmoothPixmapTransform, true);
|
||
painter.drawImage(currentResultDrawRect(), img);
|
||
painter.end();
|
||
ui.labelROIResult->setPixmap(canvas);
|
||
}
|
||
|
||
bool MeasurePanel::eventFilter(QObject* obj, QEvent* event)
|
||
{
|
||
if (obj != ui.labelROIResult)
|
||
return QObject::eventFilter(obj, event);
|
||
|
||
const QString name = ui.comboResultView->currentText();
|
||
if (!m_resultImages.contains(name) || m_resultImages.value(name).isNull())
|
||
return QObject::eventFilter(obj, event);
|
||
|
||
if (event->type() == QEvent::Wheel) {
|
||
auto* we = static_cast<QWheelEvent*>(event);
|
||
const QPointF cursor = we->pos();
|
||
const QRectF before = currentResultDrawRect();
|
||
if (!before.isValid())
|
||
return true;
|
||
|
||
const double nx = (cursor.x() - before.left()) / before.width();
|
||
const double ny = (cursor.y() - before.top()) / before.height();
|
||
const int delta = we->angleDelta().y() != 0 ? we->angleDelta().y() : we->angleDelta().x();
|
||
if (delta == 0)
|
||
return true;
|
||
|
||
const double factor = std::pow(1.15, double(delta) / 120.0);
|
||
m_resultZoom = qBound(1.0, m_resultZoom * factor, 20.0);
|
||
|
||
const QSize view = ui.labelROIResult->size();
|
||
const QImage img = m_resultImages.value(name);
|
||
const double fit = (std::min)(double(view.width()) / double(img.width()),
|
||
double(view.height()) / double(img.height()));
|
||
const QSizeF drawSize(double(img.width()) * fit * m_resultZoom,
|
||
double(img.height()) * fit * m_resultZoom);
|
||
m_resultPan = QPointF(cursor.x() - nx * drawSize.width() - (double(view.width()) - drawSize.width()) * 0.5,
|
||
cursor.y() - ny * drawSize.height() - (double(view.height()) - drawSize.height()) * 0.5);
|
||
renderCurrentResultImage();
|
||
event->accept();
|
||
return true;
|
||
}
|
||
|
||
if (event->type() == QEvent::MouseButtonDblClick) {
|
||
resetResultViewTransform();
|
||
renderCurrentResultImage();
|
||
event->accept();
|
||
return true;
|
||
}
|
||
|
||
if (event->type() == QEvent::MouseButtonPress) {
|
||
auto* me = static_cast<QMouseEvent*>(event);
|
||
if (me->button() == Qt::LeftButton && m_resultZoom > 1.0) {
|
||
m_resultPanning = true;
|
||
m_resultPanStart = me->pos();
|
||
m_resultPanStartOffset = m_resultPan;
|
||
ui.labelROIResult->setCursor(Qt::ClosedHandCursor);
|
||
event->accept();
|
||
return true;
|
||
}
|
||
}
|
||
|
||
if (event->type() == QEvent::MouseMove && m_resultPanning) {
|
||
auto* me = static_cast<QMouseEvent*>(event);
|
||
m_resultPan = m_resultPanStartOffset + QPointF(me->pos() - m_resultPanStart);
|
||
renderCurrentResultImage();
|
||
event->accept();
|
||
return true;
|
||
}
|
||
|
||
if (event->type() == QEvent::MouseButtonRelease && m_resultPanning) {
|
||
auto* me = static_cast<QMouseEvent*>(event);
|
||
if (me->button() == Qt::LeftButton) {
|
||
m_resultPanning = false;
|
||
ui.labelROIResult->unsetCursor();
|
||
event->accept();
|
||
return true;
|
||
}
|
||
}
|
||
|
||
if (event->type() == QEvent::Resize) {
|
||
renderCurrentResultImage();
|
||
return QObject::eventFilter(obj, event);
|
||
}
|
||
|
||
return QObject::eventFilter(obj, event);
|
||
}
|
||
|
||
void MeasurePanel::rememberPoseOverlaySource(const QImage& image,
|
||
const VisionPoseResult& pose,
|
||
CameraSide side)
|
||
{
|
||
if (image.isNull() || !pose.valid)
|
||
return;
|
||
m_poseOverlayBaseImage = image;
|
||
m_poseOverlayCenterPose = pose;
|
||
m_poseOverlaySide = side;
|
||
ui.groupResult->setTitle(QString::fromUtf8(
|
||
"位姿估计结果 - 零件中心(相机坐标系)"));
|
||
}
|
||
|
||
bool MeasurePanel::showGraspPointOverlay(const VisionPoseResult& pose)
|
||
{
|
||
if (!pose.valid || m_poseOverlayBaseImage.isNull() ||
|
||
!m_poseOverlayCenterPose.valid || !m_visionMatcher)
|
||
return false;
|
||
|
||
VisionPoseResult overlayPose;
|
||
if (!m_coord->makeGraspPose(m_poseOverlayCenterPose, overlayPose))
|
||
return false;
|
||
|
||
// 保持实物轮廓与原图对齐,只把坐标原点和三轴移动到实际抓取点。
|
||
// updateContours=false 会保留零件中心位姿生成的原始投影轮廓。
|
||
if (!m_visionMatcher->updatePoseProjection(overlayPose, m_poseOverlaySide, false))
|
||
return false;
|
||
|
||
cv::Mat vis = VisionGuideMatcher::qImageToMat(m_poseOverlayBaseImage);
|
||
if (vis.empty())
|
||
return false;
|
||
m_visionMatcher->drawPoseResult(vis, overlayPose);
|
||
cv::putText(vis,
|
||
cv::format("GRASP: local +Z %.3f mm", ui.spinGraspOffsetZMm->value()),
|
||
cv::Point(15, 104), cv::FONT_HERSHEY_SIMPLEX, 0.60,
|
||
cv::Scalar(0, 200, 255), 2, cv::LINE_AA);
|
||
|
||
const QImage image = VisionGuideMatcher::matToQImage(vis);
|
||
if (image.isNull())
|
||
return false;
|
||
storeResultImage(S_GRASP_POSE, image);
|
||
ui.comboResultView->setCurrentText(S_GRASP_POSE);
|
||
showCurrentResultImage();
|
||
m_resultImagesReadyForExport = true;
|
||
updateActionButtons();
|
||
return true;
|
||
}
|
||
|
||
void MeasurePanel::onPoseDisplayRequested(const VisionPoseResult& pose,
|
||
const QString& mode)
|
||
{
|
||
if (!pose.valid)
|
||
return;
|
||
|
||
m_displayPose = pose;
|
||
displayPoseResult(pose, mode == QStringLiteral("robot"));
|
||
|
||
if (mode == QStringLiteral("center")) {
|
||
ui.groupResult->setTitle(QString::fromUtf8(
|
||
"位姿估计结果 - 零件中心(相机坐标系)"));
|
||
if (m_resultImages.contains(S_POSE)) {
|
||
ui.comboResultView->setCurrentText(S_POSE);
|
||
showCurrentResultImage();
|
||
}
|
||
} else if (mode == QStringLiteral("grasp")) {
|
||
ui.groupResult->setTitle(QString::fromUtf8(
|
||
"位姿估计结果 - 实际抓取点(相机坐标系)"));
|
||
if (!showGraspPointOverlay(pose))
|
||
emit logMessage(QString::fromUtf8(
|
||
"[抓取点显示警告] XYZ 已转换,但当前缺少可重新投影的原图或相机参数,未更新结果图。"));
|
||
} else if (mode == QStringLiteral("robot")) {
|
||
const QString frameLabel = m_coord->outputReferenceFrame() == QStringLiteral("user")
|
||
? QString::fromUtf8("用户坐标系") : QString::fromUtf8("世界坐标系");
|
||
ui.groupResult->setTitle(QString::fromUtf8(
|
||
"抓取位姿 XYZABC - %1").arg(frameLabel));
|
||
}
|
||
}
|
||
|
||
void MeasurePanel::displayPoseResult(const VisionPoseResult& p, bool estunAbc) {
|
||
if (!p.valid) {
|
||
ui.editResultX->clear();
|
||
ui.editResultY->clear();
|
||
ui.editResultZ->clear();
|
||
ui.editResultAxis->clear();
|
||
return;
|
||
}
|
||
ui.editResultX->setText(QString::number(p.tx*1000,'f',3));
|
||
ui.editResultY->setText(QString::number(p.ty*1000,'f',3));
|
||
ui.editResultZ->setText(QString::number(p.tz*1000,'f',3));
|
||
ui.editResultAxis->setText(estunAbc
|
||
? QString("A=%1 B=%2 C=%3")
|
||
.arg(p.rx,0,'f',3).arg(p.ry,0,'f',3).arg(p.rz,0,'f',3)
|
||
: QString("Rx=%1 Ry=%2 Rz=%3")
|
||
.arg(p.rx,0,'f',2).arg(p.ry,0,'f',2).arg(p.rz,0,'f',2));
|
||
}
|
||
|
||
// ── 导出 ─────────────────────────────────────────────────────
|
||
static QString poseCsvFields(const VisionPoseResult& p)
|
||
{
|
||
return QStringLiteral("%1,%2,%3,%4,%5,%6,%7,%8,%9,%10,%11,%12")
|
||
.arg(p.valid ? 1 : 0)
|
||
.arg(p.tx * 1000.0, 0, 'f', 6)
|
||
.arg(p.ty * 1000.0, 0, 'f', 6)
|
||
.arg(p.tz * 1000.0, 0, 'f', 6)
|
||
.arg(p.rx, 0, 'f', 6)
|
||
.arg(p.ry, 0, 'f', 6)
|
||
.arg(p.rz, 0, 'f', 6)
|
||
.arg(p.score, 0, 'f', 6)
|
||
.arg(p.stdTx, 0, 'f', 6)
|
||
.arg(p.stdTy, 0, 'f', 6)
|
||
.arg(p.stdTz, 0, 'f', 6)
|
||
.arg(p.matchTimeMs, 0, 'f', 0);
|
||
}
|
||
|
||
void MeasurePanel::appendFusionCsv(const VisionPoseResult& leftPose,
|
||
const VisionPoseResult& rightPose,
|
||
const StereoPoseFusion::FusionResult& fusion,
|
||
const VisionPoseResult& finalPose,
|
||
qint64 totalMs)
|
||
{
|
||
if (!ensureExportDir()) {
|
||
emit logMessage(QStringLiteral("[Measure] Fusion CSV write skipped: export directory unavailable."));
|
||
return;
|
||
}
|
||
|
||
auto headerFor = [](const QString& prefix) {
|
||
return QStringLiteral("%1_valid,%1_x_mm,%1_y_mm,%1_z_mm,%1_rx_deg,%1_ry_deg,%1_rz_deg,%1_score,%1_std_tx_mm,%1_std_ty_mm,%1_std_tz_mm,%1_match_ms")
|
||
.arg(prefix);
|
||
};
|
||
|
||
const QString header =
|
||
QStringLiteral("timestamp,total_ms,") +
|
||
headerFor(QStringLiteral("left_raw")) + QStringLiteral(",") +
|
||
headerFor(QStringLiteral("right_raw")) + QStringLiteral(",") +
|
||
headerFor(QStringLiteral("right_in_left")) + QStringLiteral(",") +
|
||
headerFor(QStringLiteral("fused")) + QStringLiteral(",") +
|
||
QStringLiteral("dt_mm,daxis_deg,consistent,w_left,w_right,confidence,final_valid\n");
|
||
|
||
const QString row =
|
||
QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss.zzz") + QStringLiteral(",") +
|
||
QString::number(totalMs) + QStringLiteral(",") +
|
||
poseCsvFields(leftPose) + QStringLiteral(",") +
|
||
poseCsvFields(rightPose) + QStringLiteral(",") +
|
||
poseCsvFields(fusion.rightInLeft) + QStringLiteral(",") +
|
||
poseCsvFields(finalPose) + QStringLiteral(",") +
|
||
QString::number(fusion.positionDisparityMm, 'f', 6) + QStringLiteral(",") +
|
||
QString::number(fusion.orientationDisparityDeg, 'f', 6) + QStringLiteral(",") +
|
||
QString::number(fusion.consistent ? 1 : 0) + QStringLiteral(",") +
|
||
QString::number(fusion.weightLeft, 'f', 6) + QStringLiteral(",") +
|
||
QString::number(fusion.weightRight, 'f', 6) + QStringLiteral(",") +
|
||
QString::number(fusion.confidence, 'f', 6) + QStringLiteral(",") +
|
||
QString::number(finalPose.valid ? 1 : 0) + QStringLiteral("\n");
|
||
|
||
auto tryWriteCsv = [&](const QString& path, QString* errorText) -> bool {
|
||
const bool writeHeader = !QFileInfo::exists(path) || QFileInfo(path).size() == 0;
|
||
QFile f(path);
|
||
if (!f.open(QIODevice::WriteOnly | QIODevice::Append | QIODevice::Text)) {
|
||
if (errorText)
|
||
*errorText = f.errorString();
|
||
return false;
|
||
}
|
||
|
||
QTextStream out(&f);
|
||
if (writeHeader)
|
||
out << header;
|
||
out << row;
|
||
out.flush();
|
||
const bool flushed = f.flush();
|
||
if (!flushed && errorText)
|
||
*errorText = f.errorString();
|
||
f.close();
|
||
return flushed;
|
||
};
|
||
|
||
const QString fp = m_exportDir + "/fusion_measure_log.csv";
|
||
QString errorText;
|
||
if (tryWriteCsv(fp, &errorText)) {
|
||
emit logMessage(QStringLiteral("[Measure] Fusion CSV appended: %1").arg(fp));
|
||
return;
|
||
}
|
||
|
||
emit logMessage(QStringLiteral("[Measure] Fusion CSV open/write failed: %1 (%2)")
|
||
.arg(fp, errorText));
|
||
|
||
const QString backup = m_exportDir + "/fusion_measure_log_" +
|
||
QDateTime::currentDateTime().toString("yyyyMMdd_HHmmss_zzz") + ".csv";
|
||
errorText.clear();
|
||
if (tryWriteCsv(backup, &errorText)) {
|
||
emit logMessage(QStringLiteral("[Measure] Fusion CSV appended to fallback file: %1").arg(backup));
|
||
} else {
|
||
emit logMessage(QStringLiteral("[Measure] Fusion CSV fallback failed: %1 (%2)")
|
||
.arg(backup, errorText));
|
||
}
|
||
}
|
||
|
||
bool MeasurePanel::ensureExportDir() {
|
||
if (m_exportDir.isEmpty())
|
||
m_exportDir = QCoreApplication::applicationDirPath() + "/export";
|
||
QDir d(m_exportDir);
|
||
return d.exists() || d.mkpath(".");
|
||
}
|
||
|
||
void MeasurePanel::onBrowseExportDir() {
|
||
QString d = AppSettings::instance().openDir(
|
||
qobject_cast<QWidget*>(parent()), AppSettings::K_ExportDir,
|
||
QString::fromUtf8("选择导出目录"), m_exportDir);
|
||
if (!d.isEmpty()) { m_exportDir = d; ui.editExportDir->setText(d); }
|
||
}
|
||
|
||
void MeasurePanel::onExportPoseCamera() {
|
||
if (!ensureExportDir()) return;
|
||
VisionPoseResult pose = m_coord ? m_coord->activeCameraPose() : m_lastPose;
|
||
if (!pose.valid) pose = m_lastPose;
|
||
if (!pose.valid) return;
|
||
const bool graspPoint = m_coord && m_coord->activeCameraPoseIsGraspPoint();
|
||
QString ts = QDateTime::currentDateTime().toString("yyyyMMdd_HHmmss");
|
||
QString fp = m_exportDir + (graspPoint ? "/pose_camera_grasp_" : "/pose_camera_center_") + ts + ".txt";
|
||
QFile f(fp);
|
||
if (!f.open(QIODevice::WriteOnly | QIODevice::Text)) return;
|
||
QTextStream out(&f);
|
||
out << "PointType: " << (graspPoint ? "grasp" : "part_center") << "\n"
|
||
<< "CoordinateFrame: camera\n"
|
||
<< "X: " << QString::number(pose.tx*1000,'f',3) << " mm\n"
|
||
<< "Y: " << QString::number(pose.ty*1000,'f',3) << " mm\n"
|
||
<< "Z: " << QString::number(pose.tz*1000,'f',3) << " mm\n"
|
||
<< "Rx: " << QString::number(pose.rx,'f',4) << " deg\n"
|
||
<< "Ry: " << QString::number(pose.ry,'f',4) << " deg\n"
|
||
<< "Rz: " << QString::number(pose.rz,'f',4) << " deg\n"
|
||
<< "Score: " << QString::number(pose.score,'f',6) << "\n";
|
||
f.close();
|
||
emit logMessage(QString::fromUtf8("位姿已导出: ") + fp);
|
||
}
|
||
|
||
void MeasurePanel::onExportPoseRobot() {
|
||
if (!ensureExportDir() || !m_coord->robotPoseReady()) return;
|
||
auto rp = m_coord->robotPose();
|
||
const QString referenceFrame = m_coord->outputReferenceFrame();
|
||
const QString frameLabel = referenceFrame == QStringLiteral("user")
|
||
? QString::fromUtf8("用户坐标系") : QString::fromUtf8("世界坐标系");
|
||
QString ts = QDateTime::currentDateTime().toString("yyyyMMdd_HHmmss");
|
||
QString fp = m_exportDir + "/pose_grasp_" + referenceFrame + "_" + ts + ".csv";
|
||
QFile f(fp);
|
||
if (!f.open(QIODevice::WriteOnly | QIODevice::Text)) return;
|
||
QTextStream out(&f);
|
||
out << "时间,参考坐标系,X_mm,Y_mm,Z_mm,A_deg,B_deg,C_deg\n"
|
||
<< QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss") << ","
|
||
<< frameLabel << ","
|
||
<< QString::number(rp.tx*1000,'f',3) << "," << QString::number(rp.ty*1000,'f',3) << ","
|
||
<< QString::number(rp.tz*1000,'f',3) << "," << QString::number(rp.rx,'f',4) << ","
|
||
<< QString::number(rp.ry,'f',4) << "," << QString::number(rp.rz,'f',4) << "\n";
|
||
f.close();
|
||
emit logMessage(QString::fromUtf8("抓取位姿 XYZABC 已导出: ") + fp);
|
||
}
|
||
|
||
void MeasurePanel::onExportResultImages()
|
||
{
|
||
if (!m_resultImagesReadyForExport || m_resultImages.isEmpty()) {
|
||
emit logMessage(QString::fromUtf8("[Measure] 暂无可导出的全流程结果图。"));
|
||
return;
|
||
}
|
||
if (!ensureExportDir())
|
||
return;
|
||
|
||
const QString ts = QDateTime::currentDateTime().toString(QStringLiteral("yyyyMMdd_HHmmss"));
|
||
int saved = 0;
|
||
for (auto it = m_resultImages.constBegin(); it != m_resultImages.constEnd(); ++it) {
|
||
if (it.value().isNull())
|
||
continue;
|
||
QString fn = it.key();
|
||
fn.replace(QRegExp("[^a-zA-Z0-9\\x4e00-\\x9fa5]"), "_");
|
||
const QString fp = m_exportDir + "/" + fn + "_" + ts + ".jpg";
|
||
if (it.value().save(fp, "JPG", 92))
|
||
++saved;
|
||
else
|
||
emit logMessage(QString::fromUtf8("[Measure] 结果图导出失败: ") + fp);
|
||
}
|
||
emit logMessage(QString::fromUtf8("[Measure] 结果图已导出 JPG: %1 张 -> %2")
|
||
.arg(saved)
|
||
.arg(m_exportDir));
|
||
}
|
||
|
||
void MeasurePanel::onROIContextMenu(const QPoint& pos) {
|
||
QString cur = ui.comboResultView->currentText();
|
||
if (!m_resultImages.contains(cur) || m_resultImages[cur].isNull()) return;
|
||
QMenu menu;
|
||
QAction* act = menu.addAction(QString::fromUtf8("导出「%1」为 PNG").arg(cur));
|
||
connect(act, &QAction::triggered, [this, cur]{
|
||
if (!ensureExportDir()) return;
|
||
QString ts = QDateTime::currentDateTime().toString("yyyyMMdd_HHmmss");
|
||
QString fn = cur; fn.replace(QRegExp("[^a-zA-Z0-9\\x4e00-\\x9fa5]"), "_");
|
||
m_resultImages[cur].save(m_exportDir + "/" + fn + "_" + ts + ".png");
|
||
emit logMessage(QString::fromUtf8("图片已导出"));
|
||
});
|
||
menu.exec(ui.labelROIResult->mapToGlobal(pos));
|
||
}
|