// 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 #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include // ── 中文步骤名常量 ── #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(image.constBits()), image.bytesPerLine()); return gray.clone(); } if (image.format() == QImage::Format_RGB888) { cv::Mat rgb(image.height(), image.width(), CV_8UC3, const_cast(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(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(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::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(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(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(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(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(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(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(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(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 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( std::max(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 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( std::max(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(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(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(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(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(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)); }